Remove test code from bwputstr
[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     while (*str) {
68         if (bwputc(channel, *str) < 0)
69             return -1;
70         str++;
71     }
72     return 0;
73 }
74
75 void bwputw(int channel, int n, char fc, char *bf) {
76     char ch;
77     char *p = bf;
78
79     while (*p++ && n > 0)
80         n--;
81     while (n-- > 0)
82         bwputc(channel, fc);
83     while ((ch = *bf++))
84         bwputc(channel, ch);
85 }
86
87 int bwgetc(int channel) {
88     volatile int *flags, *data;
89     unsigned char c;
90
91     switch (channel) {
92     case COM1:
93         flags = (int *)(UART1_PHYS_BASE + UART_LSR_OFFSET);
94         data = (int *)(UART1_PHYS_BASE + UART_RBR_OFFSET);
95         break;
96     case COM2:
97         flags = (int *)(UART2_PHYS_BASE + UART_LSR_OFFSET);
98         data = (int *)(UART2_PHYS_BASE + UART_RBR_OFFSET);
99         break;
100     default:
101         return -1;
102         break;
103     }
104     while (!(*flags & UART_DRS_MASK)) ;
105     c = *data;
106     return c;
107 }
108
109 int bwa2d(char ch) {
110     if (ch >= '0' && ch <= '9')
111         return ch - '0';
112     if (ch >= 'a' && ch <= 'f')
113         return ch - 'a' + 10;
114     if (ch >= 'A' && ch <= 'F')
115         return ch - 'A' + 10;
116     return -1;
117 }
118
119 char bwa2i(char ch, char **src, int base, int *nump) {
120     int num, digit;
121     char *p;
122
123     p = *src;
124     num = 0;
125     while ((digit = bwa2d(ch)) >= 0) {
126         if (digit > base)
127             break;
128         num = num * base + digit;
129         ch = *p++;
130     }
131     *src = p;
132     *nump = num;
133     return ch;
134 }
135
136 void bwui2a(unsigned int num, unsigned int base, char *bf) {
137     int n = 0;
138     int dgt;
139     unsigned int d = 1;
140
141     while ((num / d) >= base)
142         d *= base;
143     while (d != 0) {
144         dgt = num / d;
145         num %= d;
146         d /= base;
147         if (n || dgt > 0 || d == 0) {
148             *bf++ = dgt + (dgt < 10 ? '0' : 'a' - 10);
149             ++n;
150         }
151     }
152     *bf = 0;
153 }
154
155 void bwi2a(int num, char *bf) {
156     if (num < 0) {
157         num = -num;
158         *bf++ = '-';
159     }
160     bwui2a(num, 10, bf);
161 }
162
163 void bwformat(int channel, char *fmt, va_list va) {
164     char bf[12];
165     char ch, lz;
166     int w;
167
168     while ((ch = *(fmt++))) {
169         if (ch != '%')
170             bwputc(channel, ch);
171         else {
172             lz = 0;
173             w = 0;
174             ch = *(fmt++);
175             switch (ch) {
176             case '0':
177                 lz = 1;
178                 ch = *(fmt++);
179                 break;
180             case '1':
181             case '2':
182             case '3':
183             case '4':
184             case '5':
185             case '6':
186             case '7':
187             case '8':
188             case '9':
189                 ch = bwa2i(ch, &fmt, 10, &w);
190                 break;
191             }
192             switch (ch) {
193             case 0:
194                 return;
195             case 'c':
196                 bwputc(channel, va_arg(va, int));
197                 break;
198             case 's':
199                 bwputw(channel, w, 0, va_arg(va, char *));
200                 break;
201             case 'u':
202                 bwui2a(va_arg(va, unsigned int), 10, bf);
203                 bwputw(channel, w, lz, bf);
204                 break;
205             case 'd':
206                 bwi2a(va_arg(va, int), bf);
207                 bwputw(channel, w, lz, bf);
208                 break;
209             case 'x':
210                 bwui2a(va_arg(va, unsigned int), 16, bf);
211                 bwputw(channel, w, lz, bf);
212                 break;
213             case '%':
214                 bwputc(channel, ch);
215                 break;
216             }
217         }
218     }
219 }
220
221 void bwprintf(int channel, char *fmt, ...) {
222     va_list va;
223
224     va_start(va, fmt);
225     bwformat(channel, fmt, va);
226     va_end(va);
227 }