libk.c (6017B)
1 #include "libk.h" 2 #include "io.h" 3 #include "vga.h" 4 5 #define ZEROPAD 0x01 6 #define SIGN 0x02 7 #define PLUS 0x04 8 #define SPACE 0x08 9 #define LEFT 0x10 10 #define SMALL 0x20 11 #define SPECIAL 0x40 12 13 static int calc_width(const char **); 14 static int div(long *, int); 15 static char *itoa(char *, long, int, int, int, int); 16 17 static int 18 calc_width(const char **s) 19 { 20 int i = 0; 21 22 while (isdigit(**s)) 23 i = i * 10 + *((*s)++) - '0'; 24 return (i); 25 } 26 27 static int 28 div(long *n, int base) 29 { 30 int res; 31 32 res = (u_long)*n % (u_int)base; 33 *n = (u_long)*n / (u_int)base; 34 return (res); 35 } 36 37 static char * 38 itoa(char *str, long num, int base, int size, int precision, int flags) 39 { 40 static const char digits[16] = "0123456789ABCDEF"; 41 char tmp[66]; 42 int i; 43 char c, locase, sign; 44 45 locase = (flags & SMALL); 46 if (flags & LEFT) 47 flags &= ~ZEROPAD; 48 if (base < 2 || base > 16) 49 return (NULL); 50 c = (flags & ZEROPAD) ? '0' : ' '; 51 sign = 0; 52 if (flags & SIGN) { 53 if (num < 0) { 54 sign = '-'; 55 num = -num; 56 size--; 57 } else if (flags & PLUS) { 58 sign = '+'; 59 size--; 60 } else if (flags & SPACE) { 61 sign = ' '; 62 size--; 63 } 64 } 65 /* FIXME: what? */ 66 /*if (flags & SPECIAL) {*/ 67 /*if (base == 8)*/ 68 /*size--;*/ 69 /*else if (base == 16)*/ 70 /*size -= 2;*/ 71 /*}*/ 72 i = 0; 73 if (!num) 74 tmp[i++] = '0'; 75 else 76 while (num) 77 tmp[i++] = (digits[div(&num, base)] | locase); 78 if (i > precision) 79 precision = i; 80 size -= precision; 81 if (!(flags & (ZEROPAD | LEFT))) /* XXX: + instead of | ? */ 82 while (size-- > 0) 83 *str++ = ' '; 84 if (sign) 85 *str++ = sign; 86 if (flags & SPECIAL) { 87 if (base == 8) 88 *str++ = '0'; 89 else if (base == 16) { 90 *str++ = '0'; 91 *str++ = ('X' | locase); 92 } 93 } 94 if (!(flags & LEFT)) 95 while (size-- > 0) 96 *str++ = c; 97 while (i < precision--) 98 *str++ = '0'; 99 while (i-- > 0) 100 *str++ = tmp[i]; 101 while (size-- > 0) 102 *str++ = ' '; 103 104 return (str); 105 } 106 107 void * 108 memset(void *dst, int v, size_t len) 109 { 110 u_char *dst0; 111 112 dst0 = dst; 113 while (len--) 114 *dst0++ = v; 115 116 return (dst); 117 } 118 119 void * 120 memcpy(void *dst, const void *src, size_t len) 121 { 122 const u_char *src0; 123 u_char *dst0; 124 125 src0 = src; 126 dst0 = dst; 127 128 while (len--) 129 *dst0++ = *src0++; 130 131 return (dst); 132 } 133 134 size_t 135 strlen(const char *str) 136 { 137 const char *s; 138 139 for (s = str; *s != '\0'; s++) 140 ; /* nothing */ 141 142 return (s - str); 143 } 144 145 size_t 146 strnlen(const char *str, size_t maxlen) 147 { 148 const char *s = str; 149 150 while (*s && maxlen) { 151 s++; 152 maxlen--; 153 } 154 return (s - str); 155 } 156 157 size_t 158 strlcpy(char *dst, const char *src, size_t dsize) 159 { 160 const char *osrc = src; 161 size_t nleft = dsize; 162 163 if (nleft != 0) { 164 while (--nleft != 0) { 165 if ((*dst++ = *src++) == '\0') 166 break; 167 } 168 } 169 170 if (nleft == 0) { 171 if (dsize != 0) 172 *dst = '\0'; 173 while (*src++) 174 ; 175 } 176 177 return (src - osrc - 1); 178 } 179 180 int 181 strcmp(const char *s1, const char *s2) 182 { 183 while (*s1 == *s2++) 184 if (*s1++ == '\0') 185 return (0); 186 187 return (*(const u_char *)s1 - *(const u_char *)(s2 - 1)); 188 } 189 190 int 191 isdigit(char c) 192 { 193 return (c >= '0' && c <= '9'); 194 } 195 196 int 197 vsprintf(char *buf, const char *fmt, va_list ap) 198 { 199 char *str, *s; 200 int base, i, n, flags, qual, size, precision; 201 202 for (str = buf; *fmt != '\0'; fmt++) { 203 if (*fmt != '%') { 204 *str++ = *fmt; 205 continue; 206 } 207 flags = 0; 208 repeat: 209 fmt++; 210 switch (*fmt) { 211 case '-': 212 flags |= LEFT; 213 goto repeat; 214 case '+': 215 flags |= PLUS; 216 goto repeat; 217 case ' ': 218 flags |= SPACE; 219 goto repeat; 220 case '#': 221 flags |= SPECIAL; 222 goto repeat; 223 case '0': 224 flags |= ZEROPAD; 225 goto repeat; 226 } 227 228 size = -1; 229 if (isdigit(*fmt)) 230 size = calc_width(&fmt); 231 else if (*fmt == '*') { 232 fmt++; 233 if ((size = va_arg(ap, int)) < 0) { 234 size -= size; 235 flags |= LEFT; 236 } 237 } 238 239 precision = -1; 240 if (*fmt == '.') { 241 fmt++; 242 if (isdigit(*fmt)) 243 precision = calc_width(&fmt); 244 else if (*fmt == '*') { 245 fmt++; 246 precision = va_arg(ap, int); 247 } 248 if (precision < 0) 249 precision = 0; 250 } 251 252 qual = -1; 253 if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L') 254 qual = *fmt++; 255 256 base = 10; 257 switch (*fmt) { 258 case 'c': 259 if (!(flags & LEFT)) 260 while (--size > 0) 261 *str++ = ' '; 262 *str++ = (u_char)va_arg(ap, int); 263 while (--size > 0) 264 *str++ = ' '; 265 continue; 266 case 's': 267 s = va_arg(ap, char *); 268 n = strnlen(s, precision); 269 if (!(flags & LEFT)) 270 while (n < size--) 271 *str++ = ' '; 272 for (i = 0; i < n; i++) 273 *str++ = *s++; 274 while (n < size--) 275 *str++ = ' '; 276 continue; 277 case 'p': 278 if (size == -1) { 279 size = sizeof(void *) << 1; 280 flags |= ZEROPAD; 281 } 282 str = itoa(str, 283 (u_long)va_arg(ap, void *), 16, 284 size, precision, flags); 285 continue; 286 case 'n': 287 if (qual == 'l') { 288 long *ip = va_arg(ap, long *); 289 *ip = (str - buf); 290 } else { 291 int *ip = va_arg(ap, int *); 292 *ip = (str - buf); 293 } 294 continue; 295 case '%': 296 *str++ = '%'; 297 continue; 298 case 'd': /* FALLTHROUGH */ 299 case 'i': 300 flags |= SIGN; 301 break; 302 case 'u': 303 break; 304 case 'o': 305 base = 8; 306 break; 307 case 'x': /* FALLTHROUGH */ 308 flags |= SMALL; 309 case 'X': 310 base = 16; 311 break; 312 default: 313 *str++ = '%'; 314 if (*fmt) 315 *str++ = *fmt; 316 else 317 fmt--; 318 continue; 319 } 320 321 if (qual == 'l') 322 n = va_arg(ap, u_long); 323 else if (qual == 'h') { 324 n = (u_short)va_arg(ap, int); 325 if (flags & SIGN) 326 n = (short)n; 327 } else if (flags & SIGN) 328 n = va_arg(ap, int); 329 else 330 n = va_arg(ap, u_int); 331 str = itoa(str, n, base, size, precision, flags); 332 } 333 *str = '\0'; 334 335 return (str - buf); 336 } 337 338 int 339 sprintf(char *buf, const char *fmt, ...) 340 { 341 va_list ap; 342 int n; 343 344 va_start(ap, fmt); 345 n = vsprintf(buf, fmt, ap); 346 va_end(ap); 347 348 return (n); 349 } 350 351 int 352 printf(const char *fmt, ...) 353 { 354 char buf[BUFSIZ]; 355 va_list ap; 356 int n; 357 358 va_start(ap, fmt); 359 n = vsprintf(buf, fmt, ap); 360 va_end(ap); 361 vga_puts(buf); 362 363 return (n); 364 } 365 366 void 367 panic(const char *fmt, ...) 368 { 369 char buf[BUFSIZ]; 370 va_list ap; 371 int n; 372 373 cli(); 374 vga_set_color(VGA_RED, VGA_WHITE); 375 printf("panic: "); 376 va_start(ap, fmt); 377 n = vsprintf(buf, fmt, ap); 378 va_end(ap); 379 vga_puts(buf); 380 vga_set_color(VGA_BLACK, VGA_WHITE); 381 hlt(); 382 }