LXR uboot/lib/tiny-printf.c

   1// SPDX-License-Identifier: LGPL-2.1+
   2/*
   3 * Tiny printf version for SPL
   4 *
   5 * Copied from:
   6 * http://www.sparetimelabs.com/printfrevisited/printfrevisited.php
   7 *
   8 * Copyright (C) 2004,2008  Kustaa Nyholm
   9 */
  10
  11#include <common.h>
  12#include <stdarg.h>
  13#include <serial.h>
  14#include <linux/ctype.h>
  15
  16struct printf_info {
  17        char *bf;       /* Digit buffer */
  18        char zs;        /* non-zero if a digit has been written */
  19        char *outstr;   /* Next output position for sprintf() */
  20
  21        /* Output a character */
  22        void (*putc)(struct printf_info *info, char ch);
  23};
  24
  25static void out(struct printf_info *info, char c)
  26{
  27        *info->bf++ = c;
  28}
  29
  30static void out_dgt(struct printf_info *info, char dgt)
  31{
  32        out(info, dgt + (dgt < 10 ? '0' : 'a' - 10));
  33        info->zs = 1;
  34}
  35
  36static void div_out(struct printf_info *info, unsigned long *num,
  37                    unsigned long div)
  38{
  39        unsigned char dgt = 0;
  40
  41        while (*num >= div) {
  42                *num -= div;
  43                dgt++;
  44        }
  45
  46        if (info->zs || dgt > 0)
  47                out_dgt(info, dgt);
  48}
  49
  50#ifdef CONFIG_SPL_NET_SUPPORT
  51static void string(struct printf_info *info, char *s)
  52{
  53        char ch;
  54
  55        while ((ch = *s++))
  56                out(info, ch);
  57}
  58
  59static const char hex_asc[] = "0123456789abcdef";
  60#define hex_asc_lo(x)   hex_asc[((x) & 0x0f)]
  61#define hex_asc_hi(x)   hex_asc[((x) & 0xf0) >> 4]
  62
  63static inline char *pack_hex_byte(char *buf, u8 byte)
  64{
  65        *buf++ = hex_asc_hi(byte);
  66        *buf++ = hex_asc_lo(byte);
  67        return buf;
  68}
  69
  70static void mac_address_string(struct printf_info *info, u8 *addr,
  71                                bool separator)
  72{
  73        /* (6 * 2 hex digits), 5 colons and trailing zero */
  74        char mac_addr[6 * 3];
  75        char *p = mac_addr;
  76        int i;
  77
  78        for (i = 0; i < 6; i++) {
  79                p = pack_hex_byte(p, addr[i]);
  80                if (separator && i != 5)
  81                        *p++ = ':';
  82        }
  83        *p = '\0';
  84
  85        string(info, mac_addr);
  86}
  87
  88static char *put_dec_trunc(char *buf, unsigned int q)
  89{
  90        unsigned int d3, d2, d1, d0;
  91        d1 = (q >> 4) & 0xf;
  92        d2 = (q >> 8) & 0xf;
  93        d3 = (q >> 12);
  94
  95        d0 = 6 * (d3 + d2 + d1) + (q & 0xf);
  96        q = (d0 * 0xcd) >> 11;
  97        d0 = d0 - 10 * q;
  98        *buf++ = d0 + '0'; /* least significant digit */
  99        d1 = q + 9 * d3 + 5 * d2 + d1;
 100        if (d1 != 0) {
 101                q = (d1 * 0xcd) >> 11;
 102                d1 = d1 - 10 * q;
 103                *buf++ = d1 + '0'; /* next digit */
 104
 105                d2 = q + 2 * d2;
 106                if ((d2 != 0) || (d3 != 0)) {
 107                        q = (d2 * 0xd) >> 7;
 108                        d2 = d2 - 10 * q;
 109                        *buf++ = d2 + '0'; /* next digit */
 110
 111                        d3 = q + 4 * d3;
 112                        if (d3 != 0) {
 113                                q = (d3 * 0xcd) >> 11;
 114                                d3 = d3 - 10 * q;
 115                                *buf++ = d3 + '0';  /* next digit */
 116                                if (q != 0)
 117                                        *buf++ = q + '0'; /* most sign. digit */
 118                        }
 119                }
 120        }
 121        return buf;
 122}
 123
 124static void ip4_addr_string(struct printf_info *info, u8 *addr)
 125{
 126        /* (4 * 3 decimal digits), 3 dots and trailing zero */
 127        char ip4_addr[4 * 4];
 128        char temp[3];   /* hold each IP quad in reverse order */
 129        char *p = ip4_addr;
 130        int i, digits;
 131
 132        for (i = 0; i < 4; i++) {
 133                digits = put_dec_trunc(temp, addr[i]) - temp;
 134                /* reverse the digits in the quad */
 135                while (digits--)
 136                        *p++ = temp[digits];
 137                if (i != 3)
 138                        *p++ = '.';
 139        }
 140        *p = '\0';
 141
 142        string(info, ip4_addr);
 143}
 144#endif
 145
 146/*
 147 * Show a '%p' thing.  A kernel extension is that the '%p' is followed
 148 * by an extra set of characters that are extended format
 149 * specifiers.
 150 *
 151 * Right now we handle:
 152 *
 153 * - 'M' For a 6-byte MAC address, it prints the address in the
 154 *       usual colon-separated hex notation.
 155 * - 'm' Same as above except there is no colon-separator.
 156 * - 'I4'for IPv4 addresses printed in the usual way (dot-separated
 157 *       decimal).
 158 */
 159
 160static void pointer(struct printf_info *info, const char *fmt, void *ptr)
 161{
 162#ifdef DEBUG
 163        unsigned long num = (uintptr_t)ptr;
 164        unsigned long div;
 165#endif
 166
 167        switch (*fmt) {
 168#ifdef DEBUG
 169        case 'a':
 170
 171                switch (fmt[1]) {
 172                case 'p':
 173                default:
 174                        num = *(phys_addr_t *)ptr;
 175                        break;
 176                }
 177                break;
 178#endif
 179#ifdef CONFIG_SPL_NET_SUPPORT
 180        case 'm':
 181                return mac_address_string(info, ptr, false);
 182        case 'M':
 183                return mac_address_string(info, ptr, true);
 184        case 'I':
 185                if (fmt[1] == '4')
 186                        return ip4_addr_string(info, ptr);
 187#endif
 188        default:
 189                break;
 190        }
 191#ifdef DEBUG
 192        div = 1UL << (sizeof(long) * 8 - 4);
 193        for (; div; div /= 0x10)
 194                div_out(info, &num, div);
 195#endif
 196}
 197
 198static int _vprintf(struct printf_info *info, const char *fmt, va_list va)
 199{
 200        char ch;
 201        char *p;
 202        unsigned long num;
 203        char buf[12];
 204        unsigned long div;
 205
 206        while ((ch = *(fmt++))) {
 207                if (ch != '%') {
 208                        info->putc(info, ch);
 209                } else {
 210                        bool lz = false;
 211                        int width = 0;
 212                        bool islong = false;
 213
 214                        ch = *(fmt++);
 215                        if (ch == '-')
 216                                ch = *(fmt++);
 217
 218                        if (ch == '0') {
 219                                ch = *(fmt++);
 220                                lz = 1;
 221                        }
 222
 223                        if (ch >= '0' && ch <= '9') {
 224                                width = 0;
 225                                while (ch >= '0' && ch <= '9') {
 226                                        width = (width * 10) + ch - '0';
 227                                        ch = *fmt++;
 228                                }
 229                        }
 230                        if (ch == 'l') {
 231                                ch = *(fmt++);
 232                                islong = true;
 233                        }
 234
 235                        info->bf = buf;
 236                        p = info->bf;
 237                        info->zs = 0;
 238
 239                        switch (ch) {
 240                        case '\0':
 241                                goto abort;
 242                        case 'u':
 243                        case 'd':
 244                                div = 1000000000;
 245                                if (islong) {
 246                                        num = va_arg(va, unsigned long);
 247                                        if (sizeof(long) > 4)
 248                                                div *= div * 10;
 249                                } else {
 250                                        num = va_arg(va, unsigned int);
 251                                }
 252
 253                                if (ch == 'd') {
 254                                        if (islong && (long)num < 0) {
 255                                                num = -(long)num;
 256                                                out(info, '-');
 257                                        } else if (!islong && (int)num < 0) {
 258                                                num = -(int)num;
 259                                                out(info, '-');
 260                                        }
 261                                }
 262                                if (!num) {
 263                                        out_dgt(info, 0);
 264                                } else {
 265                                        for (; div; div /= 10)
 266                                                div_out(info, &num, div);
 267                                }
 268                                break;
 269                        case 'x':
 270                                if (islong) {
 271                                        num = va_arg(va, unsigned long);
 272                                        div = 1UL << (sizeof(long) * 8 - 4);
 273                                } else {
 274                                        num = va_arg(va, unsigned int);
 275                                        div = 0x10000000;
 276                                }
 277                                if (!num) {
 278                                        out_dgt(info, 0);
 279                                } else {
 280                                        for (; div; div /= 0x10)
 281                                                div_out(info, &num, div);
 282                                }
 283                                break;
 284                        case 'c':
 285                                out(info, (char)(va_arg(va, int)));
 286                                break;
 287                        case 's':
 288                                p = va_arg(va, char*);
 289                                break;
 290                        case 'p':
 291                                pointer(info, fmt, va_arg(va, void *));
 292                                while (isalnum(fmt[0]))
 293                                        fmt++;
 294                                break;
 295                        case '%':
 296                                out(info, '%');
 297                        default:
 298                                break;
 299                        }
 300
 301                        *info->bf = 0;
 302                        info->bf = p;
 303                        while (*info->bf++ && width > 0)
 304                                width--;
 305                        while (width-- > 0)
 306                                info->putc(info, lz ? '0' : ' ');
 307                        if (p) {
 308                                while ((ch = *p++))
 309                                        info->putc(info, ch);
 310                        }
 311                }
 312        }
 313
 314abort:
 315        return 0;
 316}
 317
 318#if CONFIG_IS_ENABLED(PRINTF)
 319static void putc_normal(struct printf_info *info, char ch)
 320{
 321        putc(ch);
 322}
 323
 324int vprintf(const char *fmt, va_list va)
 325{
 326        struct printf_info info;
 327
 328        info.putc = putc_normal;
 329        return _vprintf(&info, fmt, va);
 330}
 331
 332int printf(const char *fmt, ...)
 333{
 334        struct printf_info info;
 335
 336        va_list va;
 337        int ret;
 338
 339        info.putc = putc_normal;
 340        va_start(va, fmt);
 341        ret = _vprintf(&info, fmt, va);
 342        va_end(va);
 343
 344        return ret;
 345}
 346#endif
 347
 348static void putc_outstr(struct printf_info *info, char ch)
 349{
 350        *info->outstr++ = ch;
 351}
 352
 353int sprintf(char *buf, const char *fmt, ...)
 354{
 355        struct printf_info info;
 356        va_list va;
 357        int ret;
 358
 359        va_start(va, fmt);
 360        info.outstr = buf;
 361        info.putc = putc_outstr;
 362        ret = _vprintf(&info, fmt, va);
 363        va_end(va);
 364        *info.outstr = '\0';
 365
 366        return ret;
 367}
 368
 369/* Note that size is ignored */
 370int snprintf(char *buf, size_t size, const char *fmt, ...)
 371{
 372        struct printf_info info;
 373        va_list va;
 374        int ret;
 375
 376        va_start(va, fmt);
 377        info.outstr = buf;
 378        info.putc = putc_outstr;
 379        ret = _vprintf(&info, fmt, va);
 380        va_end(va);
 381        *info.outstr = '\0';
 382
 383        return ret;
 384}
 385