1/* 2 * Copyright 2014, General Dynamics C4 Systems 3 * 4 * This software may be distributed and modified according to the terms of 5 * the GNU General Public License version 2. Note that NO WARRANTY is provided. 6 * See "LICENSE_GPLv2.txt" for details. 7 * 8 * @TAG(GD_GPL) 9 */ 10 11#include <config.h> 12#include <machine/io.h> 13 14#ifdef CONFIG_PRINTING 15 16#include <stdarg.h> 17 18void 19putchar(char c) 20{ 21 putConsoleChar(c); 22 if (c == '\n') { 23 putConsoleChar('\r'); 24 } 25} 26 27static unsigned int 28print_spaces(int n) 29{ 30 for (int i = 0; i < n; i++) { 31 kernel_putchar(' '); 32 } 33 34 return n; 35} 36 37static unsigned int 38print_string(const char *s) 39{ 40 unsigned int n; 41 42 for (n = 0; *s; s++, n++) { 43 kernel_putchar(*s); 44 } 45 46 return n; 47} 48 49static unsigned long 50xdiv(unsigned long x, unsigned int denom) 51{ 52 switch (denom) { 53 case 16: 54 return x / 16; 55 case 10: 56 return x / 10; 57 default: 58 return 0; 59 } 60} 61 62static unsigned long 63xmod(unsigned long x, unsigned int denom) 64{ 65 switch (denom) { 66 case 16: 67 return x % 16; 68 case 10: 69 return x % 10; 70 default: 71 return 0; 72 } 73} 74 75word_t 76print_unsigned_long(unsigned long x, word_t ui_base) 77{ 78 char out[sizeof(unsigned long) * 2 + 3]; 79 word_t i, j; 80 unsigned int d; 81 82 /* 83 * Only base 10 and 16 supported for now. We want to avoid invoking the 84 * compiler's support libraries through doing arbitrary divisions. 85 */ 86 if (ui_base != 10 && ui_base != 16) { 87 return 0; 88 } 89 90 if (x == 0) { 91 kernel_putchar('0'); 92 return 1; 93 } 94 95 for (i = 0; x; x = xdiv(x, ui_base), i++) { 96 d = xmod(x, ui_base); 97 98 if (d >= 10) { 99 out[i] = 'a' + d - 10; 100 } else { 101 out[i] = '0' + d; 102 } 103 } 104 105 for (j = i; j > 0; j--) { 106 kernel_putchar(out[j - 1]); 107 } 108 109 return i; 110} 111 112/* The print_unsigned_long_long function assumes that an unsinged int 113 is half the size of an unsigned long long */ 114compile_assert(print_unsigned_long_long_sizes, sizeof(unsigned int) * 2 == sizeof(unsigned long long)) 115 116static unsigned int 117print_unsigned_long_long(unsigned long long x, unsigned int ui_base) 118{ 119 unsigned int upper, lower; 120 unsigned int n = 0; 121 unsigned int mask = 0xF0000000u; 122 unsigned int shifts = 0; 123 124 /* only implemented for hex, decimal is harder without 64 bit division */ 125 if (ui_base != 16) { 126 return 0; 127 } 128 129 /* we can't do 64 bit division so break it up into two hex numbers */ 130 upper = (unsigned int) (x >> 32llu); 131 lower = (unsigned int) x & 0xffffffff; 132 133 /* print first 32 bits if they exist */ 134 if (upper > 0) { 135 n += print_unsigned_long(upper, ui_base); 136 /* print leading 0s */ 137 while (!(mask & lower)) { 138 kernel_putchar('0'); 139 n++; 140 mask = mask >> 4; 141 shifts++; 142 if (shifts == 8) { 143 break; 144 } 145 } 146 } 147 /* print last 32 bits */ 148 n += print_unsigned_long(lower, ui_base); 149 150 return n; 151} 152 153static inline bool_t 154isdigit(char c) 155{ 156 return c >= '0' && 157 c <= '9'; 158} 159 160static inline int 161atoi(char c) 162{ 163 return c - '0'; 164} 165 166static int 167vprintf(const char *format, va_list ap) 168{ 169 unsigned int n; 170 unsigned int formatting; 171 int nspaces = 0; 172 173 if (!format) { 174 return 0; 175 } 176 177 n = 0; 178 formatting = 0; 179 while (*format) { 180 if (formatting) { 181 while (isdigit(*format)) { 182 nspaces = nspaces * 10 + atoi(*format); 183 format++; 184 if (format == NULL) { 185 break; 186 } 187 } 188 switch (*format) { 189 case '%': 190 kernel_putchar('%'); 191 n++; 192 format++; 193 break; 194 195 case 'd': { 196 int x = va_arg(ap, int); 197 198 if (x < 0) { 199 kernel_putchar('-'); 200 n++; 201 x = -x; 202 } 203 204 n += print_unsigned_long(x, 10); 205 format++; 206 break; 207 } 208 209 case 'u': 210 n += print_unsigned_long(va_arg(ap, unsigned int), 10); 211 format++; 212 break; 213 214 case 'x': 215 n += print_unsigned_long(va_arg(ap, unsigned int), 16); 216 format++; 217 break; 218 219 case 'p': { 220 unsigned long p = va_arg(ap, unsigned long); 221 if (p == 0) { 222 n += print_string("(nil)"); 223 } else { 224 n += print_string("0x"); 225 n += print_unsigned_long(p, 16); 226 } 227 format++; 228 break; 229 } 230 231 case 's': 232 n += print_string(va_arg(ap, char *)); 233 format++; 234 break; 235 236 case 'l': 237 format++; 238 switch (*format) { 239 case 'd': { 240 long x = va_arg(ap, long); 241 242 if (x < 0) { 243 kernel_putchar('-'); 244 n++; 245 x = -x; 246 } 247 248 n += print_unsigned_long((unsigned long)x, 10); 249 format++; 250 } 251 break; 252 case 'l': 253 if (*(format + 1) == 'x') { 254 n += print_unsigned_long_long(va_arg(ap, unsigned long long), 16); 255 } 256 format += 2; 257 break; 258 case 'u': 259 n += print_unsigned_long(va_arg(ap, unsigned long), 10); 260 format++; 261 break; 262 case 'x': 263 n += print_unsigned_long(va_arg(ap, unsigned long), 16); 264 format++; 265 break; 266 267 default: 268 /* format not supported */ 269 return -1; 270 } 271 break; 272 default: 273 /* format not supported */ 274 return -1; 275 } 276 277 n += print_spaces(nspaces - n); 278 nspaces = 0; 279 formatting = 0; 280 } else { 281 switch (*format) { 282 case '%': 283 formatting = 1; 284 format++; 285 break; 286 287 default: 288 kernel_putchar(*format); 289 n++; 290 format++; 291 break; 292 } 293 } 294 } 295 296 /* This updateTimestamp() call here will enable us to do printf() 297 * calls in the realtime kernel without the printf() calls contributing 298 * silently to the elapsed time in the kernel. 299 * 300 * As long as your printfs occur *BEFORE* the kernel calls setDeadline(), 301 * they should be factored in correctly. 302 * 303 * The caveat is that the current thread gets billed for all of the printf()s. 304 */ 305// if (NODE_STATE(ksCurThread)) { 306// updateTimestamp(true); 307// } 308 return n; 309} 310 311word_t puts(const char *s) 312{ 313 for (; *s; s++) { 314 kernel_putchar(*s); 315 } 316 kernel_putchar('\n'); 317 return 0; 318} 319 320word_t 321kprintf(const char *format, ...) 322{ 323 va_list args; 324 word_t i; 325 326 va_start(args, format); 327 i = vprintf(format, args); 328 va_end(args); 329 return i; 330} 331 332#endif /* CONFIG_PRINTING */ 333