printk.c (3134B)
1 #include <limits.h> 2 #include <stdbool.h> 3 #include <stdarg.h> 4 #include <dat.h> 5 #include <fns.h> 6 7 static char *utoa_hex(unsigned int num, char *buf, size_t bufsize) { 8 char *p = buf + bufsize - 1; 9 *p = '\0'; 10 do { 11 int digit = num & 0xF; 12 *--p = (digit < 10) ? ('0' + digit) : ('a' + digit - 10); 13 num >>= 4; 14 } while (num != 0 && p > buf); 15 return p; 16 } 17 18 static char *itoa_dec(int num, char *buf, size_t bufsize) { 19 char *p = buf + bufsize - 1; 20 *p = '\0'; 21 unsigned int un; 22 int sign = 0; 23 24 if (num < 0) { 25 sign = 1; 26 un = (unsigned int)(-(long long)num); 27 } else { 28 un = (unsigned int)num; 29 } 30 31 do { 32 *--p = '0' + (un % 10); 33 un /= 10; 34 } while (un != 0 && p > buf); 35 36 if (sign) { 37 if (p > buf) *--p = '-'; 38 } 39 return p; 40 } 41 42 static bool print(const char* data, size_t length) { 43 if (vfs_write(stdout, (void*) data, length) == (size_t)EOF) 44 return false; 45 return true; 46 } 47 48 int printk(const char* restrict format, ...) { 49 va_list args; 50 va_start(args, format); 51 52 int count = 0; 53 const char *p = format; 54 55 while (*p) { 56 if (*p != '%') { 57 /* char */ 58 if (!print(p, 1)) { 59 va_end(args); 60 return -1; 61 } 62 count++; 63 p++; 64 } else { 65 p++; /* skip '%' */ 66 if (!*p) break; 67 68 char spec = *p++; 69 switch (spec) { 70 case '%': 71 { 72 if (!print("%", 1)) { 73 va_end(args); 74 return -1; 75 } 76 count++; 77 break; 78 } 79 case 'c': 80 { 81 char c = (char)va_arg(args, int); 82 if (!print(&c, 1)) { 83 va_end(args); 84 return -1; 85 } 86 count++; 87 break; 88 } 89 case 's': 90 { 91 char *str = va_arg(args, char*); 92 if (str == NULL) str = "(null)"; 93 size_t len = strlen(str); 94 if (!print(str, len)) { 95 va_end(args); 96 return -1; 97 } 98 count += len; 99 break; 100 } 101 case 'x': 102 { 103 unsigned int num = va_arg(args, unsigned int); 104 char buf[32]; 105 char *hex = utoa_hex(num, buf, sizeof(buf)); 106 size_t len = strlen(hex); 107 if (!print(hex, len)) { 108 va_end(args); 109 return -1; 110 } 111 count += len; 112 break; 113 } 114 case 'd': 115 { 116 int num = va_arg(args, int); 117 char buf[32]; 118 char *dec = itoa_dec(num, buf, sizeof(buf)); 119 size_t len = strlen(dec); 120 if (!print(dec, len)) { 121 va_end(args); 122 return -1; 123 } 124 count += len; 125 break; 126 } 127 default: 128 { 129 if (!print("%", 1)) { 130 va_end(args); 131 return -1; 132 } 133 count++; 134 if (!print(&spec, 1)) { 135 va_end(args); 136 return -1; 137 } 138 count++; 139 break; 140 } 141 } 142 } 143 } 144 145 va_end(args); 146 return count; 147 }