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