Embed ISRs at known address
[viridis.git] / kernel / string.c
1 /* vim: set ts=4 sw=4 sts=4 et : */
2
3 #include <kernel.h>
4 #include <vua/vua.h>    // printing native objects
5
6 #include <stdarg.h>
7
8
9 void memcpy(void *dest, void *src, u32 size)
10 {
11     int i;
12     for (i = 0; i < size; i++)
13         ((char *)dest)[i] = ((char *)src)[i];
14 }
15
16 void memset(void *dest, char pattern, u32 size)
17 {
18     int i;
19     for (i = 0; i < size; i++)
20         ((char *) dest)[i] = pattern;
21 }
22
23 char *strncpy(char *dest, char *src, u32 size)
24 {
25     int i;
26     for (i = 0; i < size && src[i]; i++)
27         dest[i] = src[i];
28     if (i < size)
29         src[i] = 0;
30     return dest;
31 }
32
33 int strncmp(char *a, char *b, int n)
34 {
35     int i;
36     for (i = 0; i < n; i++) {
37         if (a[i] != b[i])
38             return -1;
39         if (a[i] == 0)
40             break;
41     }
42
43     return 0;
44 }
45
46 int strlen(char *src)
47 {
48     int i = 0;
49     while (src[i])
50         i++;
51     return i;
52 }
53
54 /* Convert a single value into ASCII */
55
56 static int convert(char *buf, char fill, u32 rem, s32 fieldwidth,
57                    unsigned long long val, u32 base)
58 {
59     char conv[] = "0123456789abcdef";
60     char tmp[20];
61     int digits = 0;
62     int i = 0;
63
64     /* Write the digits into tmp, backwards because it's easiest to calculate
65      * the smallest digits first */
66
67     do {
68         tmp[digits] = conv[val % base];
69         val /= base;
70         digits++;
71     } while (val > 0);
72
73     /* Pad out tmp to fieldwidth */
74
75     i = 0;
76     if ((fieldwidth > digits)&&(rem >= fieldwidth)) {
77         for (i = 0; i < (fieldwidth - digits); i++)
78             buf[i] = fill;
79     }
80
81     /* Then reverse the digits into buf */
82
83     while (digits && rem) {
84         buf[i++] = tmp[digits - 1];
85         digits--;
86         rem--;
87     }
88
89     return i;
90 }
91
92 void vsnprintf(char *buf, u32 size, char *format, va_list ap)
93 {
94     char *str = buf;
95     char qualifier[2];
96     char padding[16];
97     char fill;
98     char *s;
99     char c;
100     vua_str_t *vs;
101
102     unsigned long long val;
103     s32 fieldwidth = -1;
104     s32 p_len = 0;
105     u32 escaped = 0;
106     u32 tmp;
107     u32 i;
108
109     /* size - 1, so we have room for the null byte regardless */
110
111     for (i = 0; i < (size - 1) && format[i]; i++) {
112         if (format[i] == '%') {
113             if (escaped) {
114                 *str++ = '%';
115                 escaped = 0;
116             } else
117                 escaped = 1;
118             continue;
119         }
120
121         if (escaped) {
122             switch (format[i]) {
123             case 'c':
124                 c = (char)va_arg(ap, int);
125                 *str++ = c;
126                 escaped = 0;
127                 break;
128             case 's':
129                 s = va_arg(ap, char *);
130
131                 while ((size--) && (*s))
132                     *str++ = *s++;
133
134                 escaped = 0;
135                 size++;
136                 break;
137             case 'S':
138                 vs = tv_str(va_arg(ap, vua_tv_t));
139
140                 if (vs) {
141                     for (tmp = 0; tmp < min(vs->len, size); tmp++) {
142                         *str++ = vs->data[tmp];
143                         size--;
144                     }
145                 }
146
147                 escaped = 0;
148                 size++;
149                 break;
150             case 'l':
151                 if (qualifier[0] == 'l')
152                     qualifier[1] = 'l';
153                 else {
154                     qualifier[0] = 'l';
155                     qualifier[1] = 0;
156                 }
157
158                 size++;
159                 break;
160             case '0':
161             case '1':
162             case '2':
163             case '3':
164             case '4':
165             case '5':
166             case '6':
167             case '7':
168             case '8':
169             case '9':
170                 if (p_len < 16)
171                     padding[p_len++] = format[i] - '0';
172                 size++;
173                 break;
174             case 'd':
175             case 'x':
176                 if (qualifier[0] == 'l') {
177                     if (qualifier[1] == 'l')
178                         val = va_arg(ap, unsigned long long);
179                     else
180                         val = va_arg(ap, unsigned long);
181                 } else
182                     val = va_arg(ap, unsigned int);
183
184                 if (p_len) {
185                     if (padding[0] == 0)
186                         fill = '0';
187                     else
188                         fill = ' ';
189
190                     tmp = p_len;
191
192                     fieldwidth = 0;
193                     while (p_len) {
194                         fieldwidth *= 10;
195                         fieldwidth += padding[tmp - p_len];
196                         p_len--;
197                     }
198                 }
199
200                 if (format[i] == 'x')
201                     tmp = convert(str, fill, (size - i), fieldwidth, val, 16);
202                 else
203                     tmp = convert(str, fill, (size - i), fieldwidth, val, 10);
204                 str += tmp;
205
206                 /* tmp were printed, but the format string shouldn't
207                  * count against size */
208
209                 size -= (tmp - 1);
210
211                 escaped = 0;
212                 qualifier[0] = 0;
213                 p_len = 0;
214                 fieldwidth = -1;
215                 break;
216             }
217         } else
218             *str++ = format[i];
219     }
220     *str = 0;
221 }