00001
00044 #include <assert.h>
00045 #include <console/core.h>
00046 #include <console/driver.h>
00047 #include <interrupt.h>
00048 #include <stdarg.h>
00049 #include <string.h>
00050 #include <util.h>
00051
00059 enum conversion_state {
00061 STATE_NORMAL,
00063 STATE_FLAG,
00065 STATE_WIDTH,
00067 STATE_PERIOD,
00069 STATE_PRECISION,
00071 STATE_LENGTH,
00073 STATE_CONVSPEC,
00074 };
00075
00079 struct printf_conversion {
00081 int width;
00083 int precision;
00085 char length;
00087 char spec;
00089 char pad_char;
00091 union {
00093 long d;
00095 unsigned long u;
00097 double f;
00099 const char *s;
00101 void *p;
00106 int *n;
00107 } arg;
00108 };
00109
00116 static void console_drv_putchar(struct console_driver *drv, char c)
00117 {
00118 unsigned long iflags;
00119
00120 if (c == '\n')
00121 console_drv_putchar(drv, '\r');
00122
00123 iflags = cpu_irq_save();
00124 while (console_buf_unused(&drv->tx_buf) < 1) {
00125 cpu_irq_restore(iflags);
00126 drv->tx_make_room(drv, 1);
00127 iflags = cpu_irq_save();
00128 }
00129
00130 console_buf_insert_char(&drv->tx_buf, c);
00131 cpu_irq_restore(iflags);
00132 }
00133
00141 static void console_drv_write(struct console_driver *drv,
00142 const char *data, size_t len)
00143 {
00144 size_t partial;
00145 unsigned long head;
00146 unsigned long iflags;
00147
00148 while (len) {
00149 if (console_buf_unused(&drv->tx_buf) < len)
00150 drv->tx_make_room(drv, len);
00151
00152 iflags = cpu_irq_save();
00153 head = ring_get_head(&drv->tx_buf.ring, CONSOLE_BUF_SIZE);
00154 partial = min(len, console_buf_unused_before_end(&drv->tx_buf));
00155 memcpy(drv->tx_buf.data + head, data, partial);
00156 ring_insert_entries(&drv->tx_buf.ring, partial);
00157 cpu_irq_restore(iflags);
00158
00159 data += partial;
00160 len -= partial;
00161 }
00162 }
00163
00170 static int console_drv_putstr(struct console_driver *drv, const char *str)
00171 {
00172 int len;
00173
00174 len = strlen(str);
00175 console_drv_write(drv, str, len);
00176
00177 return len;
00178 }
00179
00189 static void console_drv_commit(struct console_driver *drv)
00190 {
00191 drv->tx_commit(drv);
00192 }
00193
00202 int console_putstr(struct console *con, const char *str)
00203 {
00204 int len = console_drv_putstr(con->drv, str);
00205 console_drv_commit(con->drv);
00206 return len;
00207 }
00208
00217 int console_putchar(struct console *con, int c)
00218 {
00219 struct console_driver *drv = con->drv;
00220 char ch = c;
00221
00222 console_drv_putchar(drv, ch);
00223 console_drv_commit(drv);
00224
00225 return ch;
00226 }
00227
00237 static int console_drv_print_signed(struct console_driver *drv,
00238 struct printf_conversion *conv)
00239 {
00240 char buf[32];
00241 long number = conv->arg.d;
00242 int negative = 0;
00243 int i = sizeof(buf);
00244 int len;
00245 char c;
00246
00247 if (number == 0) {
00248 console_drv_putchar(drv, '0');
00249 return 1;
00250 }
00251
00252 if (number < 0) {
00253 negative = 1;
00254 number = -number;
00255 }
00256
00257 while (number) {
00258 c = '0' + number % 10;
00259 number /= 10;
00260 buf[--i] = c;
00261 }
00262
00263 if (negative)
00264 buf[--i] = '-';
00265
00266 if (conv->width > sizeof(buf))
00267 conv->width = sizeof(buf);
00268
00269 while ((sizeof(buf) - i) < conv->width)
00270 buf[--i] = conv->pad_char;
00271
00272 len = sizeof(buf) - i;
00273 console_drv_write(drv, buf + i, len);
00274
00275 return len;
00276 }
00277
00287 static int console_drv_print_unsigned(struct console_driver *drv,
00288 struct printf_conversion *conv)
00289 {
00290 char buf[32];
00291 unsigned long number = conv->arg.u;
00292 int i = sizeof(buf);
00293 int len;
00294 char c;
00295
00296 if (number == 0)
00297 buf[--i] = '0';
00298
00299 switch (conv->spec) {
00300 case 'o':
00301 while (number) {
00302 c = '0' + (number & 7);
00303 number >>= 3;
00304 buf[--i] = c;
00305 }
00306 break;
00307 case 'u':
00308 while (number) {
00309 c = '0' + (number % 10);
00310 number /= 10;
00311 buf[--i] = c;
00312 }
00313 break;
00314 case 'x':
00315 while (number) {
00316 if ((number & 15) > 9)
00317 c = 'a' - 10 + (number & 15);
00318 else
00319 c = '0' + (number & 15);
00320 number >>= 4;
00321 buf[--i] = c;
00322 }
00323 break;
00324 case 'X':
00325 while (number) {
00326 if ((number & 15) > 9)
00327 c = 'A' - 10 + (number & 15);
00328 else
00329 c = '0' + (number & 15);
00330 number >>= 4;
00331 buf[--i] = c;
00332 }
00333 break;
00334 }
00335
00336 if (conv->width > sizeof(buf))
00337 conv->width = sizeof(buf);
00338
00339 while ((sizeof(buf) - i) < conv->width)
00340 buf[--i] = conv->pad_char;
00341
00342 len = sizeof(buf) - i;
00343 console_drv_write(drv, buf + i, len);
00344
00345 return len;
00346 }
00347
00361 int console_vprintf(struct console *con, const char *format, va_list ap)
00362 {
00363 int state = STATE_NORMAL;
00364 struct printf_conversion conv;
00365 struct console_driver *drv = con->drv;
00366 int n = 0;
00367 char c;
00368
00369 while ((c = *format++)) {
00370 switch (state) {
00371 case STATE_NORMAL:
00372 if (c == '%') {
00373 state = STATE_FLAG;
00374 conv.width = 0;
00375 conv.precision = 0;
00376 conv.length = 0;
00377 conv.pad_char = ' ';
00378 } else {
00379 console_drv_putchar(drv, c);
00380 n++;
00381 }
00382 break;
00383
00384 case STATE_FLAG:
00385 state = STATE_WIDTH;
00386
00387
00388 switch (c) {
00389 case '0':
00390 conv.pad_char = '0';
00391 break;
00392 case '#':
00393 case '-':
00394 case ' ':
00395 case '+':
00396 break;
00397
00398 case '%':
00399
00400 console_drv_putchar(drv, c);
00401 n++;
00402 state = STATE_NORMAL;
00403 break;
00404
00405 default:
00406 goto state_width;
00407 }
00408 break;
00409
00410 state_width:
00411 case STATE_WIDTH:
00412 if (isdigit(c) && (c != '0' || conv.width != 0)) {
00413 conv.width *= 10;
00414 conv.width += c - '0';
00415 break;
00416 }
00417
00418 state = STATE_PERIOD;
00419
00420
00421 case STATE_PERIOD:
00422 if (c != '.') {
00423 state = STATE_LENGTH;
00424 goto state_length;
00425 }
00426 state = STATE_PRECISION;
00427 break;
00428
00429 case STATE_PRECISION:
00430
00431 if (isdigit(c))
00432 break;
00433
00434 state = STATE_LENGTH;
00435
00436
00437 state_length:
00438 case STATE_LENGTH:
00439
00440 if (c == 'h' || c == 'l' || c == 'L') {
00441 conv.length = c;
00442 break;
00443 } else if (c == 'z') {
00444 if (sizeof(size_t) == sizeof(long))
00445 conv.length = 'l';
00446 break;
00447 }
00448
00449 state = STATE_CONVSPEC;
00450
00451
00452 case STATE_CONVSPEC:
00453 conv.spec = c;
00454
00455 switch (c) {
00456 case 'd':
00457 case 'i':
00458 if (conv.length == 'l')
00459 conv.arg.d = va_arg(ap, long);
00460 else
00461 conv.arg.d = va_arg(ap, int);
00462 n += console_drv_print_signed(drv, &conv);
00463 break;
00464 case 'o':
00465 case 'u':
00466 case 'x':
00467 case 'X':
00468 if (conv.length == 'l')
00469 conv.arg.u = va_arg(ap, unsigned long);
00470 else
00471 conv.arg.u = va_arg(ap, unsigned int);
00472 n += console_drv_print_unsigned(drv, &conv);
00473 break;
00474 case 'c':
00475 conv.arg.d = va_arg(ap, int);
00476 console_drv_putchar(drv, conv.arg.d);
00477 n++;
00478 break;
00479
00480
00481
00482 case 's':
00483 conv.arg.s = va_arg(ap, const char *);
00484 n += console_drv_putstr(drv, conv.arg.s);
00485 break;
00486 case 'p':
00487 conv.arg.p = va_arg(ap, void *);
00488 console_drv_write(drv, "0x", 2);
00489 n += 2;
00490 conv.spec = 'x';
00491 n += console_drv_print_unsigned(drv, &conv);
00492 break;
00493 case 'n':
00494 conv.arg.n = va_arg(ap, int *);
00495 *conv.arg.n = n;
00496 break;
00497 }
00498
00499 state = STATE_NORMAL;
00500 break;
00501 }
00502 }
00503
00504 console_drv_commit(drv);
00505
00506 return n;
00507 }
00508
00515 int console_printf(struct console *con, const char *format, ...)
00516 {
00517 int n;
00518 va_list ap;
00519
00520 va_start(ap, format);
00521 n = console_vprintf(con, format, ap);
00522 va_end(ap);
00523
00524 return n;
00525 }
