Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) Paul Mackerras 1997. |
| 3 | * |
| 4 | * This program is free software; you can redistribute it and/or |
| 5 | * modify it under the terms of the GNU General Public License |
| 6 | * as published by the Free Software Foundation; either version |
| 7 | * 2 of the License, or (at your option) any later version. |
| 8 | */ |
| 9 | #include <stdarg.h> |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 10 | #include <stddef.h> |
| 11 | #include "string.h" |
| 12 | #include "stdio.h" |
| 13 | #include "prom.h" |
Olaf Hering | 7840e5e | 2005-06-08 15:12:00 +1000 | [diff] [blame] | 14 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 15 | int (*prom)(void *); |
| 16 | |
| 17 | void *chosen_handle; |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 18 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 19 | void *stdin; |
| 20 | void *stdout; |
| 21 | void *stderr; |
| 22 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 23 | |
| 24 | int |
| 25 | write(void *handle, void *ptr, int nb) |
| 26 | { |
| 27 | struct prom_args { |
| 28 | char *service; |
| 29 | int nargs; |
| 30 | int nret; |
| 31 | void *ihandle; |
| 32 | void *addr; |
| 33 | int len; |
| 34 | int actual; |
| 35 | } args; |
| 36 | |
| 37 | args.service = "write"; |
| 38 | args.nargs = 3; |
| 39 | args.nret = 1; |
| 40 | args.ihandle = handle; |
| 41 | args.addr = ptr; |
| 42 | args.len = nb; |
| 43 | args.actual = -1; |
| 44 | (*prom)(&args); |
| 45 | return args.actual; |
| 46 | } |
| 47 | |
| 48 | int |
| 49 | read(void *handle, void *ptr, int nb) |
| 50 | { |
| 51 | struct prom_args { |
| 52 | char *service; |
| 53 | int nargs; |
| 54 | int nret; |
| 55 | void *ihandle; |
| 56 | void *addr; |
| 57 | int len; |
| 58 | int actual; |
| 59 | } args; |
| 60 | |
| 61 | args.service = "read"; |
| 62 | args.nargs = 3; |
| 63 | args.nret = 1; |
| 64 | args.ihandle = handle; |
| 65 | args.addr = ptr; |
| 66 | args.len = nb; |
| 67 | args.actual = -1; |
| 68 | (*prom)(&args); |
| 69 | return args.actual; |
| 70 | } |
| 71 | |
| 72 | void |
| 73 | exit() |
| 74 | { |
| 75 | struct prom_args { |
| 76 | char *service; |
| 77 | } args; |
| 78 | |
| 79 | for (;;) { |
| 80 | args.service = "exit"; |
| 81 | (*prom)(&args); |
| 82 | } |
| 83 | } |
| 84 | |
| 85 | void |
| 86 | pause(void) |
| 87 | { |
| 88 | struct prom_args { |
| 89 | char *service; |
| 90 | } args; |
| 91 | |
| 92 | args.service = "enter"; |
| 93 | (*prom)(&args); |
| 94 | } |
| 95 | |
| 96 | void * |
| 97 | finddevice(const char *name) |
| 98 | { |
| 99 | struct prom_args { |
| 100 | char *service; |
| 101 | int nargs; |
| 102 | int nret; |
| 103 | const char *devspec; |
| 104 | void *phandle; |
| 105 | } args; |
| 106 | |
| 107 | args.service = "finddevice"; |
| 108 | args.nargs = 1; |
| 109 | args.nret = 1; |
| 110 | args.devspec = name; |
| 111 | args.phandle = (void *) -1; |
| 112 | (*prom)(&args); |
| 113 | return args.phandle; |
| 114 | } |
| 115 | |
| 116 | void * |
| 117 | claim(unsigned long virt, unsigned long size, unsigned long align) |
| 118 | { |
| 119 | struct prom_args { |
| 120 | char *service; |
| 121 | int nargs; |
| 122 | int nret; |
| 123 | unsigned int virt; |
| 124 | unsigned int size; |
| 125 | unsigned int align; |
| 126 | void *ret; |
| 127 | } args; |
| 128 | |
| 129 | args.service = "claim"; |
| 130 | args.nargs = 3; |
| 131 | args.nret = 1; |
| 132 | args.virt = virt; |
| 133 | args.size = size; |
| 134 | args.align = align; |
| 135 | (*prom)(&args); |
| 136 | return args.ret; |
| 137 | } |
| 138 | |
| 139 | int |
| 140 | getprop(void *phandle, const char *name, void *buf, int buflen) |
| 141 | { |
| 142 | struct prom_args { |
| 143 | char *service; |
| 144 | int nargs; |
| 145 | int nret; |
| 146 | void *phandle; |
| 147 | const char *name; |
| 148 | void *buf; |
| 149 | int buflen; |
| 150 | int size; |
| 151 | } args; |
| 152 | |
| 153 | args.service = "getprop"; |
| 154 | args.nargs = 4; |
| 155 | args.nret = 1; |
| 156 | args.phandle = phandle; |
| 157 | args.name = name; |
| 158 | args.buf = buf; |
| 159 | args.buflen = buflen; |
| 160 | args.size = -1; |
| 161 | (*prom)(&args); |
| 162 | return args.size; |
| 163 | } |
| 164 | |
| 165 | int |
| 166 | putc(int c, void *f) |
| 167 | { |
| 168 | char ch = c; |
| 169 | |
| 170 | if (c == '\n') |
| 171 | putc('\r', f); |
| 172 | return write(f, &ch, 1) == 1? c: -1; |
| 173 | } |
| 174 | |
| 175 | int |
| 176 | putchar(int c) |
| 177 | { |
| 178 | return putc(c, stdout); |
| 179 | } |
| 180 | |
| 181 | int |
| 182 | fputs(char *str, void *f) |
| 183 | { |
| 184 | int n = strlen(str); |
| 185 | |
| 186 | return write(f, str, n) == n? 0: -1; |
| 187 | } |
| 188 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 189 | size_t strnlen(const char * s, size_t count) |
| 190 | { |
| 191 | const char *sc; |
| 192 | |
| 193 | for (sc = s; count-- && *sc != '\0'; ++sc) |
| 194 | /* nothing */; |
| 195 | return sc - s; |
| 196 | } |
| 197 | |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 198 | extern unsigned int __div64_32(unsigned long long *dividend, |
| 199 | unsigned int divisor); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 200 | |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 201 | /* The unnecessary pointer compare is there |
| 202 | * to check for type safety (n must be 64bit) |
| 203 | */ |
| 204 | # define do_div(n,base) ({ \ |
| 205 | unsigned int __base = (base); \ |
| 206 | unsigned int __rem; \ |
| 207 | (void)(((typeof((n)) *)0) == ((unsigned long long *)0)); \ |
| 208 | if (((n) >> 32) == 0) { \ |
| 209 | __rem = (unsigned int)(n) % __base; \ |
| 210 | (n) = (unsigned int)(n) / __base; \ |
| 211 | } else \ |
| 212 | __rem = __div64_32(&(n), __base); \ |
| 213 | __rem; \ |
| 214 | }) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 215 | |
| 216 | static int skip_atoi(const char **s) |
| 217 | { |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 218 | int i, c; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 219 | |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 220 | for (i = 0; '0' <= (c = **s) && c <= '9'; ++*s) |
| 221 | i = i*10 + c - '0'; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 222 | return i; |
| 223 | } |
| 224 | |
| 225 | #define ZEROPAD 1 /* pad with zero */ |
| 226 | #define SIGN 2 /* unsigned/signed long */ |
| 227 | #define PLUS 4 /* show plus */ |
| 228 | #define SPACE 8 /* space if plus */ |
| 229 | #define LEFT 16 /* left justified */ |
| 230 | #define SPECIAL 32 /* 0x */ |
| 231 | #define LARGE 64 /* use 'ABCDEF' instead of 'abcdef' */ |
| 232 | |
Olaf Hering | 7840e5e | 2005-06-08 15:12:00 +1000 | [diff] [blame] | 233 | static char * number(char * str, unsigned long long num, int base, int size, int precision, int type) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 234 | { |
| 235 | char c,sign,tmp[66]; |
| 236 | const char *digits="0123456789abcdefghijklmnopqrstuvwxyz"; |
| 237 | int i; |
| 238 | |
| 239 | if (type & LARGE) |
| 240 | digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"; |
| 241 | if (type & LEFT) |
| 242 | type &= ~ZEROPAD; |
| 243 | if (base < 2 || base > 36) |
| 244 | return 0; |
| 245 | c = (type & ZEROPAD) ? '0' : ' '; |
| 246 | sign = 0; |
| 247 | if (type & SIGN) { |
Olaf Hering | 7840e5e | 2005-06-08 15:12:00 +1000 | [diff] [blame] | 248 | if ((signed long long)num < 0) { |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 249 | sign = '-'; |
Olaf Hering | 7840e5e | 2005-06-08 15:12:00 +1000 | [diff] [blame] | 250 | num = - (signed long long)num; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 251 | size--; |
| 252 | } else if (type & PLUS) { |
| 253 | sign = '+'; |
| 254 | size--; |
| 255 | } else if (type & SPACE) { |
| 256 | sign = ' '; |
| 257 | size--; |
| 258 | } |
| 259 | } |
| 260 | if (type & SPECIAL) { |
| 261 | if (base == 16) |
| 262 | size -= 2; |
| 263 | else if (base == 8) |
| 264 | size--; |
| 265 | } |
| 266 | i = 0; |
| 267 | if (num == 0) |
| 268 | tmp[i++]='0'; |
| 269 | else while (num != 0) { |
Olaf Hering | 7840e5e | 2005-06-08 15:12:00 +1000 | [diff] [blame] | 270 | tmp[i++] = digits[do_div(num, base)]; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 271 | } |
| 272 | if (i > precision) |
| 273 | precision = i; |
| 274 | size -= precision; |
| 275 | if (!(type&(ZEROPAD+LEFT))) |
| 276 | while(size-->0) |
| 277 | *str++ = ' '; |
| 278 | if (sign) |
| 279 | *str++ = sign; |
| 280 | if (type & SPECIAL) { |
| 281 | if (base==8) |
| 282 | *str++ = '0'; |
| 283 | else if (base==16) { |
| 284 | *str++ = '0'; |
| 285 | *str++ = digits[33]; |
| 286 | } |
| 287 | } |
| 288 | if (!(type & LEFT)) |
| 289 | while (size-- > 0) |
| 290 | *str++ = c; |
| 291 | while (i < precision--) |
| 292 | *str++ = '0'; |
| 293 | while (i-- > 0) |
| 294 | *str++ = tmp[i]; |
| 295 | while (size-- > 0) |
| 296 | *str++ = ' '; |
| 297 | return str; |
| 298 | } |
| 299 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 300 | int vsprintf(char *buf, const char *fmt, va_list args) |
| 301 | { |
| 302 | int len; |
Olaf Hering | 7840e5e | 2005-06-08 15:12:00 +1000 | [diff] [blame] | 303 | unsigned long long num; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 304 | int i, base; |
| 305 | char * str; |
| 306 | const char *s; |
| 307 | |
| 308 | int flags; /* flags to number() */ |
| 309 | |
| 310 | int field_width; /* width of output field */ |
| 311 | int precision; /* min. # of digits for integers; max |
| 312 | number of chars for from string */ |
| 313 | int qualifier; /* 'h', 'l', or 'L' for integer fields */ |
| 314 | /* 'z' support added 23/7/1999 S.H. */ |
| 315 | /* 'z' changed to 'Z' --davidm 1/25/99 */ |
| 316 | |
| 317 | |
| 318 | for (str=buf ; *fmt ; ++fmt) { |
| 319 | if (*fmt != '%') { |
| 320 | *str++ = *fmt; |
| 321 | continue; |
| 322 | } |
| 323 | |
| 324 | /* process flags */ |
| 325 | flags = 0; |
| 326 | repeat: |
| 327 | ++fmt; /* this also skips first '%' */ |
| 328 | switch (*fmt) { |
| 329 | case '-': flags |= LEFT; goto repeat; |
| 330 | case '+': flags |= PLUS; goto repeat; |
| 331 | case ' ': flags |= SPACE; goto repeat; |
| 332 | case '#': flags |= SPECIAL; goto repeat; |
| 333 | case '0': flags |= ZEROPAD; goto repeat; |
| 334 | } |
| 335 | |
| 336 | /* get field width */ |
| 337 | field_width = -1; |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 338 | if ('0' <= *fmt && *fmt <= '9') |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 339 | field_width = skip_atoi(&fmt); |
| 340 | else if (*fmt == '*') { |
| 341 | ++fmt; |
| 342 | /* it's the next argument */ |
| 343 | field_width = va_arg(args, int); |
| 344 | if (field_width < 0) { |
| 345 | field_width = -field_width; |
| 346 | flags |= LEFT; |
| 347 | } |
| 348 | } |
| 349 | |
| 350 | /* get the precision */ |
| 351 | precision = -1; |
| 352 | if (*fmt == '.') { |
| 353 | ++fmt; |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 354 | if ('0' <= *fmt && *fmt <= '9') |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 355 | precision = skip_atoi(&fmt); |
| 356 | else if (*fmt == '*') { |
| 357 | ++fmt; |
| 358 | /* it's the next argument */ |
| 359 | precision = va_arg(args, int); |
| 360 | } |
| 361 | if (precision < 0) |
| 362 | precision = 0; |
| 363 | } |
| 364 | |
| 365 | /* get the conversion qualifier */ |
| 366 | qualifier = -1; |
| 367 | if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L' || *fmt =='Z') { |
| 368 | qualifier = *fmt; |
| 369 | ++fmt; |
| 370 | } |
| 371 | |
| 372 | /* default base */ |
| 373 | base = 10; |
| 374 | |
| 375 | switch (*fmt) { |
| 376 | case 'c': |
| 377 | if (!(flags & LEFT)) |
| 378 | while (--field_width > 0) |
| 379 | *str++ = ' '; |
| 380 | *str++ = (unsigned char) va_arg(args, int); |
| 381 | while (--field_width > 0) |
| 382 | *str++ = ' '; |
| 383 | continue; |
| 384 | |
| 385 | case 's': |
| 386 | s = va_arg(args, char *); |
| 387 | if (!s) |
| 388 | s = "<NULL>"; |
| 389 | |
| 390 | len = strnlen(s, precision); |
| 391 | |
| 392 | if (!(flags & LEFT)) |
| 393 | while (len < field_width--) |
| 394 | *str++ = ' '; |
| 395 | for (i = 0; i < len; ++i) |
| 396 | *str++ = *s++; |
| 397 | while (len < field_width--) |
| 398 | *str++ = ' '; |
| 399 | continue; |
| 400 | |
| 401 | case 'p': |
| 402 | if (field_width == -1) { |
| 403 | field_width = 2*sizeof(void *); |
| 404 | flags |= ZEROPAD; |
| 405 | } |
| 406 | str = number(str, |
| 407 | (unsigned long) va_arg(args, void *), 16, |
| 408 | field_width, precision, flags); |
| 409 | continue; |
| 410 | |
| 411 | |
| 412 | case 'n': |
| 413 | if (qualifier == 'l') { |
| 414 | long * ip = va_arg(args, long *); |
| 415 | *ip = (str - buf); |
| 416 | } else if (qualifier == 'Z') { |
| 417 | size_t * ip = va_arg(args, size_t *); |
| 418 | *ip = (str - buf); |
| 419 | } else { |
| 420 | int * ip = va_arg(args, int *); |
| 421 | *ip = (str - buf); |
| 422 | } |
| 423 | continue; |
| 424 | |
| 425 | case '%': |
| 426 | *str++ = '%'; |
| 427 | continue; |
| 428 | |
| 429 | /* integer number formats - set up the flags and "break" */ |
| 430 | case 'o': |
| 431 | base = 8; |
| 432 | break; |
| 433 | |
| 434 | case 'X': |
| 435 | flags |= LARGE; |
| 436 | case 'x': |
| 437 | base = 16; |
| 438 | break; |
| 439 | |
| 440 | case 'd': |
| 441 | case 'i': |
| 442 | flags |= SIGN; |
| 443 | case 'u': |
| 444 | break; |
| 445 | |
| 446 | default: |
| 447 | *str++ = '%'; |
| 448 | if (*fmt) |
| 449 | *str++ = *fmt; |
| 450 | else |
| 451 | --fmt; |
| 452 | continue; |
| 453 | } |
| 454 | if (qualifier == 'l') { |
| 455 | num = va_arg(args, unsigned long); |
| 456 | if (flags & SIGN) |
| 457 | num = (signed long) num; |
| 458 | } else if (qualifier == 'Z') { |
| 459 | num = va_arg(args, size_t); |
| 460 | } else if (qualifier == 'h') { |
| 461 | num = (unsigned short) va_arg(args, int); |
| 462 | if (flags & SIGN) |
| 463 | num = (signed short) num; |
| 464 | } else { |
| 465 | num = va_arg(args, unsigned int); |
| 466 | if (flags & SIGN) |
| 467 | num = (signed int) num; |
| 468 | } |
| 469 | str = number(str, num, base, field_width, precision, flags); |
| 470 | } |
| 471 | *str = '\0'; |
| 472 | return str-buf; |
| 473 | } |
| 474 | |
| 475 | int sprintf(char * buf, const char *fmt, ...) |
| 476 | { |
| 477 | va_list args; |
| 478 | int i; |
| 479 | |
| 480 | va_start(args, fmt); |
| 481 | i=vsprintf(buf,fmt,args); |
| 482 | va_end(args); |
| 483 | return i; |
| 484 | } |
| 485 | |
| 486 | static char sprint_buf[1024]; |
| 487 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 488 | int |
Olaf Hering | decd300 | 2005-08-08 13:24:38 +1000 | [diff] [blame] | 489 | printf(const char *fmt, ...) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 490 | { |
| 491 | va_list args; |
| 492 | int n; |
| 493 | |
| 494 | va_start(args, fmt); |
| 495 | n = vsprintf(sprint_buf, fmt, args); |
| 496 | va_end(args); |
| 497 | write(stdout, sprint_buf, n); |
| 498 | return n; |
| 499 | } |