vtxprintf.c
1 /* SPDX-License-Identifier: GPL-2.0-only */ 2 3 /* 4 * vtxprintf.c, originally from linux/lib/vsprintf.c 5 */ 6 7 #include <console/vtxprintf.h> 8 #include <ctype.h> 9 #include <string.h> 10 #include <types.h> 11 12 #define call_tx(x) tx_byte(x, data) 13 14 #define ZEROPAD 1 /* pad with zero */ 15 #define SIGN 2 /* unsigned/signed long */ 16 #define PLUS 4 /* show plus */ 17 #define SPACE 8 /* space if plus */ 18 #define LEFT 16 /* left justified */ 19 #define SPECIAL 32 /* 0x */ 20 #define LARGE 64 /* use 'ABCDEF' instead of 'abcdef' */ 21 22 static int number(void (*tx_byte)(unsigned char byte, void *data), unsigned long long inum, 23 int base, int size, int precision, int type, void *data) 24 { 25 char c, sign, tmp[66]; 26 const char *digits = "0123456789abcdef"; 27 int i; 28 int count = 0; 29 unsigned long long num = inum; 30 long long snum = num; 31 32 if (type & LARGE) 33 digits = "0123456789ABCDEF"; 34 if (type & LEFT) 35 type &= ~ZEROPAD; 36 c = (type & ZEROPAD) ? '0' : ' '; 37 sign = 0; 38 if (type & SIGN) { 39 if (snum < 0) { 40 sign = '-'; 41 num = -snum; 42 size--; 43 } else if (type & PLUS) { 44 sign = '+'; 45 size--; 46 } else if (type & SPACE) { 47 sign = ' '; 48 size--; 49 } 50 } 51 if (type & SPECIAL) { 52 if (base == 16) 53 size -= 2; 54 else if (base == 8) 55 size--; 56 } 57 i = 0; 58 if (num == 0) { 59 tmp[i++] = '0'; 60 } else { 61 while (num != 0) { 62 tmp[i++] = digits[num % base]; 63 num /= base; 64 } 65 } 66 if (i > precision) { 67 precision = i; 68 } 69 size -= precision; 70 if (!(type & (ZEROPAD | LEFT))) { 71 while (size-- > 0) 72 call_tx(' '), count++; 73 } 74 if (sign) { 75 call_tx(sign), count++; 76 } 77 if (type & SPECIAL) { 78 if (base == 8) 79 call_tx('0'), count++; 80 else if (base == 16) { 81 call_tx('0'), count++; 82 if (type & LARGE) 83 call_tx('X'), count++; 84 else 85 call_tx('x'), count++; 86 } 87 } 88 if (!(type & LEFT)) { 89 while (size-- > 0) 90 call_tx(c), count++; 91 } 92 while (i < precision--) 93 call_tx('0'), count++; 94 while (i-- > 0) 95 call_tx(tmp[i]), count++; 96 while (size-- > 0) 97 call_tx(' '), count++; 98 return count; 99 } 100 101 int vtxprintf(void (*tx_byte)(unsigned char byte, void *data), const char *fmt, va_list args, 102 void *data) 103 { 104 int len; 105 unsigned long long num; 106 int i, base; 107 const char *s; 108 109 int flags; /* flags to number() */ 110 111 int field_width; /* width of output field */ 112 int precision; /* min. # of digits for integers; max 113 number of chars for from string */ 114 int qualifier; /* 'h', 'H', 'l', 'L', 'z', or 'j' for integer fields */ 115 116 int count; 117 118 for (count = 0; *fmt; ++fmt) { 119 if (*fmt != '%') { 120 call_tx(*fmt), count++; 121 continue; 122 } 123 124 /* process flags */ 125 flags = 0; 126 repeat: 127 ++fmt; /* this also skips first '%' */ 128 switch (*fmt) { 129 case '-': flags |= LEFT; goto repeat; 130 case '+': flags |= PLUS; goto repeat; 131 case ' ': flags |= SPACE; goto repeat; 132 case '#': flags |= SPECIAL; goto repeat; 133 case '0': flags |= ZEROPAD; goto repeat; 134 } 135 136 /* get field width */ 137 field_width = -1; 138 if (isdigit(*fmt)) { 139 field_width = skip_atoi((char **)&fmt); 140 } else if (*fmt == '*') { 141 ++fmt; 142 /* it's the next argument */ 143 field_width = va_arg(args, int); 144 if (field_width < 0) { 145 field_width = -field_width; 146 flags |= LEFT; 147 } 148 } 149 150 /* get the precision */ 151 precision = -1; 152 if (*fmt == '.') { 153 ++fmt; 154 if (isdigit(*fmt)) { 155 precision = skip_atoi((char **)&fmt); 156 } else if (*fmt == '*') { 157 ++fmt; 158 /* it's the next argument */ 159 precision = va_arg(args, int); 160 } 161 if (precision < 0) { 162 precision = 0; 163 } 164 } 165 166 /* get the conversion qualifier */ 167 qualifier = -1; 168 if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L' || *fmt == 'z' || *fmt == 'j') { 169 qualifier = *fmt; 170 ++fmt; 171 if (*fmt == 'l') { 172 qualifier = 'L'; 173 ++fmt; 174 } 175 if (*fmt == 'h') { 176 qualifier = 'H'; 177 ++fmt; 178 } 179 } 180 181 /* default base */ 182 base = 10; 183 184 switch (*fmt) { 185 case 'c': 186 if (!(flags & LEFT)) 187 while (--field_width > 0) 188 call_tx(' '), count++; 189 call_tx((unsigned char)va_arg(args, int)), count++; 190 while (--field_width > 0) 191 call_tx(' '), count++; 192 continue; 193 194 case 's': 195 s = va_arg(args, char *); 196 if (!s) 197 s = "<NULL>"; 198 199 len = strnlen(s, (size_t)precision); 200 201 if (!(flags & LEFT)) { 202 while (len < field_width--) 203 call_tx(' '), count++; 204 } 205 for (i = 0; i < len; ++i) 206 call_tx(*s++), count++; 207 while (len < field_width--) 208 call_tx(' '), count++; 209 continue; 210 211 case 'p': 212 /* even on 64-bit systems, coreboot only resides in the 213 low 4GB so pad pointers to 32-bit for readability. */ 214 if (field_width == -1 && precision == -1) 215 precision = 2 * sizeof(uint32_t); 216 flags |= SPECIAL; 217 count += number(tx_byte, (unsigned long)va_arg(args, void *), 16, 218 field_width, precision, flags, data); 219 continue; 220 221 case 'n': 222 if (qualifier == 'L') { 223 long long *ip = va_arg(args, long long *); 224 *ip = count; 225 } else if (qualifier == 'l') { 226 long *ip = va_arg(args, long *); 227 *ip = count; 228 } else { 229 int *ip = va_arg(args, int *); 230 *ip = count; 231 } 232 continue; 233 234 case '%': 235 call_tx('%'), count++; 236 continue; 237 238 /* integer number formats - set up the flags and "break" */ 239 case 'o': 240 base = 8; 241 break; 242 243 case 'X': 244 flags |= LARGE; 245 __fallthrough; 246 case 'x': 247 base = 16; 248 break; 249 250 case 'd': 251 case 'i': 252 flags |= SIGN; 253 __fallthrough; 254 case 'u': 255 break; 256 257 default: 258 call_tx('%'), count++; 259 if (*fmt) 260 call_tx(*fmt), count++; 261 else 262 --fmt; 263 continue; 264 } 265 if (qualifier == 'L') { 266 num = va_arg(args, unsigned long long); 267 } else if (qualifier == 'l') { 268 num = va_arg(args, unsigned long); 269 } else if (qualifier == 'z') { 270 num = va_arg(args, size_t); 271 } else if (qualifier == 'j') { 272 num = va_arg(args, uintmax_t); 273 } else if (qualifier == 'h') { 274 num = (unsigned short)va_arg(args, int); 275 if (flags & SIGN) 276 num = (short)num; 277 } else if (qualifier == 'H') { 278 num = (unsigned char)va_arg(args, int); 279 if (flags & SIGN) 280 num = (signed char)num; 281 } else if (flags & SIGN) { 282 num = va_arg(args, int); 283 } else { 284 num = va_arg(args, unsigned int); 285 } 286 count += number(tx_byte, num, base, field_width, precision, flags, data); 287 } 288 return count; 289 }