Update bwio for TS-7800
[arm/overo-bwio.git] / bwio.c
1 /*
2  * bwio.c - busy-wait I/O routines for diagnosis
3  *
4  * Specific to the TS-7800 ARM evaluation board
5  *
6  */
7
8 #include <stdarg.h>
9 #include <ts7800.h>
10 #include <bwio.h>
11
12 void raise() {}
13
14 /*
15  * UART 0 is initialized by TS-BOOTROM to the following state:
16  *      115,200 bps
17  *      8 bits
18  *      no parity
19  *      fifos enabled
20  */
21
22 int bwputc(int channel, char c) {
23     volatile int *flags, *data;
24     switch (channel) {
25     case COM1:
26         flags = (int *)(UART1_PHYS_BASE + UART_LSR_OFFSET);
27         data = (int *)(UART1_PHYS_BASE + UART_THR_OFFSET);
28         break;
29     case COM2:
30         flags = (int *)(UART2_PHYS_BASE + UART_LSR_OFFSET);
31         data = (int *)(UART2_PHYS_BASE + UART_THR_OFFSET);
32         break;
33     default:
34         return -1;
35         break;
36     }
37     while (!(*flags & UART_THRE_MASK));
38     *data = c;
39     return 0;
40 }
41
42 char c2x(char ch) {
43     if ((ch <= 9))
44         return '0' + ch;
45     return 'a' + ch - 10;
46 }
47
48 int bwputx(int channel, char c) {
49     char chh, chl;
50
51     chh = c2x(c / 16);
52     chl = c2x(c % 16);
53     bwputc(channel, chh);
54     return bwputc(channel, chl);
55 }
56
57 int bwputr(int channel, unsigned int reg) {
58     int byte;
59     char *ch = (char *)&reg;
60
61     for (byte = 3; byte >= 0; byte--)
62         bwputx(channel, ch[byte]);
63     return bwputc(channel, ' ');
64 }
65
66 int bwputstr(int channel, char *str) {
67   bwputc(COM1, '!');
68
69     while (*str) {
70         if (bwputc(channel, *str) < 0)
71             return -1;
72         str++;
73     }
74     return 0;
75 }
76
77 void bwputw(int channel, int n, char fc, char *bf) {
78     char ch;
79     char *p = bf;
80
81     while (*p++ && n > 0)
82         n--;
83     while (n-- > 0)
84         bwputc(channel, fc);
85     while ((ch = *bf++))
86         bwputc(channel, ch);
87 }
88
89 int bwgetc(int channel) {
90     volatile int *flags, *data;
91     unsigned char c;
92
93     switch (channel) {
94     case COM1:
95         flags = (int *)(UART1_PHYS_BASE + UART_LSR_OFFSET);
96         data = (int *)(UART1_PHYS_BASE + UART_RBR_OFFSET);
97         break;
98     case COM2:
99         flags = (int *)(UART2_PHYS_BASE + UART_LSR_OFFSET);
100         data = (int *)(UART2_PHYS_BASE + UART_RBR_OFFSET);
101         break;
102     default:
103         return -1;
104         break;
105     }
106     while (!(*flags & UART_DRS_MASK)) ;
107     c = *data;
108     return c;
109 }
110
111 int bwa2d(char ch) {
112     if (ch >= '0' && ch <= '9')
113         return ch - '0';
114     if (ch >= 'a' && ch <= 'f')
115         return ch - 'a' + 10;
116     if (ch >= 'A' && ch <= 'F')
117         return ch - 'A' + 10;
118     return -1;
119 }
120
121 char bwa2i(char ch, char **src, int base, int *nump) {
122     int num, digit;
123     char *p;
124
125     p = *src;
126     num = 0;
127     while ((digit = bwa2d(ch)) >= 0) {
128         if (digit > base)
129             break;
130         num = num * base + digit;
131         ch = *p++;
132     }
133     *src = p;
134     *nump = num;
135     return ch;
136 }
137
138 void bwui2a(unsigned int num, unsigned int base, char *bf) {
139     int n = 0;
140     int dgt;
141     unsigned int d = 1;
142
143     while ((num / d) >= base)
144         d *= base;
145     while (d != 0) {
146         dgt = num / d;
147         num %= d;
148         d /= base;
149         if (n || dgt > 0 || d == 0) {
150             *bf++ = dgt + (dgt < 10 ? '0' : 'a' - 10);
151             ++n;
152         }
153     }
154     *bf = 0;
155 }
156
157 void bwi2a(int num, char *bf) {
158     if (num < 0) {
159         num = -num;
160         *bf++ = '-';
161     }
162     bwui2a(num, 10, bf);
163 }
164
165 void bwformat(int channel, char *fmt, va_list va) {
166     char bf[12];
167     char ch, lz;
168     int w;
169
170     while ((ch = *(fmt++))) {
171         if (ch != '%')
172             bwputc(channel, ch);
173         else {
174             lz = 0;
175             w = 0;
176             ch = *(fmt++);
177             switch (ch) {
178             case '0':
179                 lz = 1;
180                 ch = *(fmt++);
181                 break;
182             case '1':
183             case '2':
184             case '3':
185             case '4':
186             case '5':
187             case '6':
188             case '7':
189             case '8':
190             case '9':
191                 ch = bwa2i(ch, &fmt, 10, &w);
192                 break;
193             }
194             switch (ch) {
195             case 0:
196                 return;
197             case 'c':
198                 bwputc(channel, va_arg(va, int));
199                 break;
200             case 's':
201                 bwputw(channel, w, 0, va_arg(va, char *));
202                 break;
203             case 'u':
204                 bwui2a(va_arg(va, unsigned int), 10, bf);
205                 bwputw(channel, w, lz, bf);
206                 break;
207             case 'd':
208                 bwi2a(va_arg(va, int), bf);
209                 bwputw(channel, w, lz, bf);
210                 break;
211             case 'x':
212                 bwui2a(va_arg(va, unsigned int), 16, bf);
213                 bwputw(channel, w, lz, bf);
214                 break;
215             case '%':
216                 bwputc(channel, ch);
217                 break;
218             }
219         }
220     }
221 }
222
223 void bwprintf(int channel, char *fmt, ...) {
224     va_list va;
225
226     va_start(va, fmt);
227     bwformat(channel, fmt, va);
228     va_end(va);
229 }