diff options
| -rw-r--r-- | firmware/SOURCES | 3 | ||||
| -rw-r--r-- | firmware/common/ap_int.c | 266 | ||||
| -rw-r--r-- | firmware/common/vuprintf.c | 475 | ||||
| -rw-r--r-- | firmware/include/ap_int.h | 59 |
4 files changed, 793 insertions, 10 deletions
diff --git a/firmware/SOURCES b/firmware/SOURCES index 16ae4cc350..89f4f52895 100644 --- a/firmware/SOURCES +++ b/firmware/SOURCES @@ -199,6 +199,9 @@ libc/gmtime.c #endif /* CONFIG_PLATFORM || HAVE_ROCKBOX_C_LIBRARY */ /* Common */ +#ifndef BOOTLOADER +common/ap_int.c +#endif common/version.c common/config.c common/crc32.c diff --git a/firmware/common/ap_int.c b/firmware/common/ap_int.c new file mode 100644 index 0000000000..12214534dd --- /dev/null +++ b/firmware/common/ap_int.c @@ -0,0 +1,266 @@ +/*************************************************************************** + * __________ __ ___. + * Open \______ \ ____ ____ | | _\_ |__ _______ ___ + * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / + * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < + * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ + * \/ \/ \/ \/ \/ + * $Id$ + * + * Copyright (C) 2018 by Michael A. Sevakis + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY + * KIND, either express or implied. + * + ****************************************************************************/ +#include "ap_int.h" +#include "fixedpoint.h" + +/* Miscellaneous large-sized integer functions */ + +/* round string, base 10 */ +bool round_number_string10(char *p_rdig, long len) +{ + /* + * * p should point to the digit that determines if rounding should occur + * * buffer is updated in reverse + * * an additional '1' may be added to the beginning: eg. 9.9 => 10.0 + */ +#if 1 /* nearest */ + bool round = p_rdig[0] >= '5'; +#else /* even */ + bool round = p_rdig[0] >= '5' && (p_rdig[-1] & 1); +#endif + + while (round && len-- > 0) { + int d = *--p_rdig; + round = ++d > '9'; + *p_rdig = round ? '0' : d; + } + + if (round) { + /* carry to the next place */ + *--p_rdig = '1'; + } + + return round; +} + +/* format arbitrary-precision base 10 integer */ +char * format_ap_int10(struct ap_int *a, + char *p_end) +{ + /* + * * chunks are in least-to-most-significant order + * * chunk array is used for intermediate division results + * * digit string buffer is written high-to-low address order + */ + long numchunks = a->numchunks; + char *p = p_end; + + if (numchunks == 0) { + /* fast formatting */ + uint64_t val = a->val; + + do { + *--p = val % 10 + '0'; + val /= 10; + } while (val); + + a->len = p_end - p; + return p; + } + + uint32_t *chunks = a->chunks; + long topchunk = numchunks - 1; + + /* if top chunk(s) are zero, ignore */ + while (topchunk >= 0 && chunks[topchunk] == 0) { + topchunk--; + } + + /* optimized to divide number by the biggest 10^x a uint32_t can hold + so that r_part holds the remainder (x % 1000000000) at the end of + the division */ + do { + uint64_t r_part = 0; + + for (long i = topchunk; i >= 0; i--) { + /* + * Testing showed 29 bits as a sweet spot: + * * Is a 32-bit constant (good for 32-bit hardware) + * * No more normalization is required than with 30 and 31 + * (32 bits requires the least but also a large constant) + * * Doesn't need to be reduced before hand by subtracting the + * divisor in order to keep it 32-bits which obviates the need + * to correct with another term of the remainder after + * multiplying + * + * 2305843009 = floor(ldexp(1, 29) / 1000000000.0 * ldexp(1, 32)) + */ + static const unsigned long c = 2305843009; /* .213693952 */ + uint64_t q_part = r_part*c >> 29; + r_part = (r_part << 32) | chunks[i]; + r_part -= q_part*1000000000; + + /* if remainder is still out of modular range, normalize it + and carry over into quotient */ + while (r_part >= 1000000000) { + r_part -= 1000000000; + q_part++; + } + + chunks[i] = q_part; + } + + /* if top chunk(s) became zero, ignore from now on */ + while (topchunk >= 0 && chunks[topchunk] == 0) { + topchunk--; + } + + /* format each digit chunk, padded to width 9 if not the leading one */ + uint32_t val = r_part; + int len = 8*(topchunk >= 0); + + while (len-- >= 0 || val) { + *--p = (val % 10) + '0'; + val /= 10; + } + } while (topchunk >= 0); + + a->len = p_end - p; + return p; +} + +/* format arbitrary-precision base 10 fraction */ +char * format_ap_frac10(struct ap_int *a, + char *p_start, + long precision) +{ + /* + * * chunks are in least-to-most-significant order + * * chunk array is used for intermediate multiplication results + * * digit string buffer is written low-to-high address order + * * high bit of fraction must be left-justified to a chunk + * boundary + */ + long numchunks = a->numchunks; + bool trimlz = precision < 0; + char *p = p_start; + + if (trimlz) { + /* trim leading zeros and provide <precision> digits; a->len + will end up greater than the specified precision unless the + value is zero */ + precision = -precision; + } + + a->len = precision; + + if (numchunks == 0) { + /* fast formatting; shift must be <= 60 as top four bits are used + for digit carryout */ + if (trimlz && !a->val) { + /* value is zero */ + trimlz = false; + } + + uint64_t val = a->val << (60 - a->shift); + + while (precision > 0) { + val *= 10; + uint32_t c_part = val >> 60; + + if (trimlz) { + if (!c_part) { + a->len++; + continue; + } + + trimlz = false; + } + + *p++ = c_part + '0'; + val ^= (uint64_t)c_part << 60; + precision--; + } + + return p; + } + + uint32_t *chunks = a->chunks; + long bottomchunk = 0, topchunk = numchunks; + + while (topchunk > 0 && chunks[topchunk - 1] == 0) { + topchunk--; + } + + /* optimized to multiply number by the biggest 10^x a uint32_t can hold + so that c_part holds the carryover into the integer part at the end + of the multiplication */ + while (precision > 0) { + /* if bottom chunk(s) are or became zero, skip them */ + while (bottomchunk < numchunks && chunks[bottomchunk] == 0) { + bottomchunk++; + } + + uint32_t c_part = 0; + + for (long i = bottomchunk; i < topchunk; i++) { + uint64_t p_part = chunks[i]; + + p_part = p_part * 1000000000 + c_part; + c_part = p_part >> 32; + + chunks[i] = p_part; + } + + if (topchunk < numchunks && c_part) { + chunks[topchunk++] = c_part; + c_part = 0; + } + + int len = 9; + + if (trimlz && bottomchunk < numchunks) { + if (!c_part) { + a->len += 9; + continue; + } + + /* first non-zero chunk has leading zeros? */ + for (uint32_t val = c_part; val < 100000000; val *= 10) { + len--; + } + + a->len += 9 - len; + trimlz = false; + } + + /* format each digit chunk, padded to width 9 if not exceeding + precision */ + precision -= len; + + if (precision < 0) { + /* remove extra digits */ + c_part /= ipow(10, -precision); + len += precision; + } + + p += len; + + char *p2 = p; + + while (len-- > 0) { + *--p2 = (c_part % 10) + '0'; + c_part /= 10; + } + } + + return p; +} diff --git a/firmware/common/vuprintf.c b/firmware/common/vuprintf.c index 7dd9449e56..8bb9451662 100644 --- a/firmware/common/vuprintf.c +++ b/firmware/common/vuprintf.c @@ -23,8 +23,10 @@ #include <limits.h> #include <string.h> #include <stddef.h> +#include <stdlib.h> #include "system.h" #include "vuprintf.h" +#include "ap_int.h" #ifndef BOOTLOADER /* turn everything on */ @@ -58,7 +60,11 @@ #define FMT_LENMOD_ll 0x010 /* signed/unsigned long long (%ll<radix>) */ #define FMT_LENMOD_t 0x020 /* signed/unsigned ptrdiff_t (%t<radix>) */ #define FMT_LENMOD_z 0x040 /* size_t/ssize_t (%z<radix>) */ +#if 0 #define FMT_LENMOD_L 0x080 /* long double (instead of double) */ +#else +#define FMT_LENMOD_L 0x000 +#endif /* compulsory radixes: c, d, i, u, s */ #define FMT_RADIX_c 0x001 /* single character (%c) */ @@ -75,6 +81,18 @@ #define FMT_RADIX_g 0x800 /* floating point exponent or decimal depending upon value and precision */ +/* TODO: 'a' 'A' */ +#define FMT_RADIX_floats (FMT_RADIX_e|FMT_RADIX_f|FMT_RADIX_g) + +#if (FMT_RADIX & FMT_RADIX_floats) +/* Assumes IEEE 754 double-precision, native-endian; replace to parse and init + for some other format */ +#define parse_double parse_ieee754_double +#define init_double_chunks init_ieee754_double_chunks +#define format_double_int10 format_ap_int10 +#define format_double_frac10 format_ap_frac10 +#endif + /* avoid defining redundant functions if two or more types can use the same * something not getting a macro means it gets assigned its own value and * formatter */ @@ -374,8 +392,24 @@ struct fmt_buf { or prefix (numeric) */ char buf[24]; /* work buffer */ char bufend[1]; /* buffer end marker and guard '0' */ +#if (FMT_RADIX & FMT_RADIX_floats) + int lenmod; + int radixchar; + int signchar; + int alignchar; + int width; + int precision; + char *p; +#endif }; +#define PUSHCHAR(ch) \ + ({ int __rc = push(userp, (ch)); \ + count += __rc >= 0; \ + if (__rc <= 0) { \ + goto done; \ + } }) + /* %d %i */ static inline const char * format_d(int val, struct fmt_buf *fmt_buf, @@ -530,6 +564,400 @@ static inline const char * format_p(const void *p, } #endif /* FMT_RADIX_p */ +#if (FMT_RADIX & FMT_RADIX_floats) +/* find out how many uint32_t chunks need to be allocated, if any + * if none are needed, finish the init for the number here */ +static long parse_ieee754_double(double f, + struct ap_int *ia, + struct ap_int *fa, + struct fmt_buf *fmt_buf) +{ + long rc = 0; + + union { + double f; + uint64_t f64; + } u = { .f = f }; + + int e = ((int)(u.f64 >> 52) & 0x7ff) - 1023; /* -1023..1024 */ + uint64_t mantissa = u.f64 & 0x000fffffffffffffull; + + if (u.f64 >> 63) { + fmt_buf->signchar = '-'; + } + + if (LIKELY(e >= -8 && e <= 63)) { /* -8 to +63 */ + /* integer, fraction and manipulations fit in uint64_t */ + mantissa |= 0x0010000000000000ull; + ia->numchunks = 0; + ia->shift = 0; + fa->numchunks = 0; + + if (e < 0) { /* -8 to -1 - fraction */ + long fracbits = 52 - e; + /* int - none */ + ia->len = 0; + ia->val = 0; + /* frac */ + fa->len = fracbits - __builtin_ctzll(mantissa); + fa->shift = fracbits; + fa->val = mantissa; + } + else if (e <= 51) { /* 0 to +51 - integer|fraction */ + long fracbits = 52 - e; + /* int */ + ia->len = base10exp(e) + 2; /* go up + possibly 1 longer */ + ia->val = mantissa >> fracbits; + /* frac */ + fa->shift = fracbits; + fa->val = mantissa ^ (ia->val << fracbits); + fa->len = fa->val ? fracbits - __builtin_ctzll(mantissa) : 0; + } + else { /* +52 to +63 - integer */ + /* int */ + ia->len = base10exp(e) + 2; + ia->val = mantissa << (e - 52); + /* frac - none */ + fa->len = 0; + fa->shift = 0; + fa->val = 0; + } + } + else if (e < 0) { /* -1023 to -9 - fraction */ + /* int - none */ + ia->numchunks = 0; + ia->len = 0; + ia->shift = 0; + ia->val = 0; + /* frac - left-justify on bit 31 of the chunk of the MSb */ + if (e >= -1022) { /* normal */ + mantissa |= 0x0010000000000000ull; + } + else { /* subnormal (including zero) */ + e = -1022; + } + + if (mantissa) { + long fracbits = 52 - e; + fa->len = fracbits - __builtin_ctzll(mantissa); + fa->shift = 31 - ((51 - e) % 32); + fa->val = mantissa; + fa->basechunk = (fa->shift + 52) / 32; + fa->numchunks = (51 - e + fa->shift) / 32 + 1; + rc = fa->numchunks; + } + else { /* zero */ + fa->numchunks = 0; + fa->len = 0; + fa->shift = 0; + fa->val = 0; + } + } + else if (e <= 1023) { /* +64 to +1023 - integer */ + /* int - right-justify on bit 0 of the first chunk */ + ia->val = mantissa | 0x0010000000000000ull; + ia->len = base10exp(e) + 2; + ia->shift = (e - 52) % 32; + ia->basechunk = e / 32; + ia->numchunks = ia->basechunk + 1; + rc = ia->numchunks; + /* frac - none */ + fa->numchunks = 0; + fa->len = 0; + fa->shift = 0; + fa->val = 0; + } + else { /* +1024: INF, NAN */ + rc = -1 - !!mantissa; + } + + return rc; +} + +/* construct the arbitrary-precision value in the provided allocation */ +static void init_ieee754_double_chunks(struct ap_int *a, + uint32_t *a_chunks) +{ + long basechunk = a->basechunk; + long shift = a->shift; + uint64_t val = a->val; + + a->chunks = a_chunks; + + memset(a_chunks, 0, a->numchunks*sizeof (uint32_t)); + + if (shift < 12) { + a_chunks[basechunk - 1] = val << shift; + a_chunks[basechunk - 0] = val >> (32 - shift); + } + else { + a_chunks[basechunk - 2] = val << shift; + a_chunks[basechunk - 1] = val >> (32 - shift); + a_chunks[basechunk - 0] = val >> (64 - shift); + } +} + +/* format inf, nan strings */ +static void format_inf_nan(struct fmt_buf *fmt_buf, long type) +{ + /* certain special values */ + static const char text[2][2][3] = + { + { { 'I', 'N', 'F' }, { 'i', 'n', 'f' } }, + { { 'N', 'A', 'N' }, { 'n', 'a', 'n' } }, + }; + + char *p = fmt_buf->buf; + fmt_buf->p = p; + fmt_buf->length = 3; + + /* they also have a sign */ + if (fmt_buf->signchar) { + *p++ = fmt_buf->signchar; + fmt_buf->length++; + } + + memcpy(p, &text[type][(fmt_buf->radixchar >> 5) & 0x1], 3); +} + +/* %e %E %f %F %g %G */ +static int format_double_radix(double f, + struct fmt_buf *fmt_buf, + vuprintf_push_cb push, + void *userp) +{ + struct ap_int ia, fa; + long rc = parse_double(f, &ia, &fa, fmt_buf); + + if (UNLIKELY(rc < 0)) { + format_inf_nan(fmt_buf, -rc - 1); + return 0; + } + + int count = 0; + + /* default precision is 6 for all formats */ + int prec_rem = fmt_buf->precision < 0 ? 6 : fmt_buf->precision; + + int exp = exp; + int explen = 0; + + switch (fmt_buf->radixchar & 0x3) + { + case 3: /* %g, %G */ + fmt_buf->precision = prec_rem; + if (prec_rem) { + prec_rem--; + } + case 1: /* %e, %E */ + explen = 2; + break; + default: + break; + } + + if (rc > 0 && ia.numchunks > 0) { + /* large integer required */ + init_double_chunks(&ia, alloca(rc*sizeof(*ia.chunks))); + rc = 0; + } + + const int bufoffs = 6; /* log rollover + round rollover + leading zeros (%g) */ + long f_prec = MIN(fa.len, prec_rem + 1); + char buf[bufoffs + ia.len + f_prec + 1]; + char *p_last = &buf[bufoffs + ia.len]; + char *p_dec = p_last; + char *p_first = format_double_int10(&ia, p_last); + + if (explen) { + if (!ia.val && fa.len) { + p_first = p_last = &buf[bufoffs]; + f_prec = -f_prec - 1; /* no lead zeros */ + } + else { /* handles 0e+0 too */ + exp = ia.len - 1; + + if (exp) { + prec_rem -= exp; + + if (prec_rem < 0) { + p_last += prec_rem + 1; + f_prec = 0; + } + else { + f_prec = MIN(fa.len, prec_rem + 1); + } + } + } + + p_dec = p_first + 1; + } + + if (f_prec) { + if (rc > 0) { + /* large integer required */ + init_double_chunks(&fa, alloca(rc*sizeof(*fa.chunks))); + } + + p_last = format_double_frac10(&fa, p_last, f_prec); + + if (f_prec < 0) { + f_prec = -f_prec - 1; + exp = f_prec - fa.len; + } + + prec_rem -= f_prec; + } + + if (prec_rem < 0) { + prec_rem = 0; + p_last--; + + if (round_number_string10(p_last, p_last - p_first)) { + /* carried left */ + p_first--; + + if (explen) { + /* slide everything left by 1 */ + exp++; + p_dec--; + p_last--; + } + } + } + + if (explen) { + if ((fmt_buf->radixchar & 0x3) == 0x3) { /* g, G */ + /* 'g' is some weird crap */ + /* now that the final exponent is known and everything rounded, + it is possible to decide whether to format similarly to + 'e' or 'f' */ + if (fmt_buf->precision > exp && exp >= -4) { /* P > X >= -4 */ + if (exp >= 0) { + /* integer digits will be in the buffer */ + p_dec = p_first + exp + 1; + } + else { + /* we didn't keep leading zeros and need to regenerate + them; space was reserved just in case */ + p_first = memset(p_dec + exp - 1, '0', -exp); + p_dec = p_first + 1; + } + + /* suppress exponent */ + explen = 0; + } + + if (!fmt_buf->length) { + /* strip any trailing zeros from the fraction */ + while (p_last > p_dec && p_last[-1] == '0') { + p_last--; + } + + /* suppress trailing precision fill */ + prec_rem = 0; + } + } + + if (explen) { + /* build exponent string: 'eħdd' */ + char *p = fmt_buf->bufend; + int signchar = '+'; + + if (exp < 0) { + signchar = '-'; + exp = -exp; + } + + while (exp || explen < 4) { + *--p = exp % 10 + '0'; + exp /= 10; + explen++; + } + + *--p = signchar; + *--p = fmt_buf->radixchar & ~0x2; + } + } + + int width = fmt_buf->width; + int point = p_last > p_dec || prec_rem || fmt_buf->length; + int length = p_last - p_first + !!fmt_buf->signchar + point + explen; + + if (width) { + if (width - length <= prec_rem) { + width = 0; + } + else { + width -= length + prec_rem; + } + } + + rc = -1; + + /* left padding */ + if (fmt_buf->alignchar > '0') { + /* space-padded width -- before sign */ + while (width > 0) { + PUSHCHAR(' '); + width--; + } + } + + if (fmt_buf->signchar) { + PUSHCHAR(fmt_buf->signchar); + } + + if (fmt_buf->alignchar == '0') { + /* zero-padded width -- after sign */ + while (width > 0) { + PUSHCHAR('0'); + width--; + } + } + + /* integer part */ + while (p_first < p_dec) { + PUSHCHAR(*p_first++); + } + + /* decimal point */ + if (point) { + PUSHCHAR('.'); + } + + /* fractional part */ + while (p_first < p_last) { + PUSHCHAR(*p_first++); + } + + /* precision 0-padding */ + while (prec_rem > 0) { + PUSHCHAR('0'); + prec_rem--; + } + + /* exponent */ + if (explen > 0) { + char *p = fmt_buf->bufend; + while (explen > 0) { + PUSHCHAR(p[-explen--]); + } + } + + /* right padding */ + while (width > 0) { + PUSHCHAR(' '); + width--; + } + + rc = 1; +done: + fmt_buf->length = count; + return rc; +} +#endif /* FMT_RADIX_floats */ + /* parse fixed width or precision field */ static const char * parse_number_spec(const char *fmt, int ch, @@ -558,16 +986,6 @@ int vuprintf(vuprintf_push_cb push, /* call 'push()' for each output letter */ const char *fmt, va_list ap) { - #define PUSHCHAR(ch) \ - do { \ - int __ch = (ch); \ - int __rc = push(userp, __ch); \ - count += __rc >= 0; \ - if (__rc <= 0) { \ - goto done; \ - } \ - } while (0) - int count = 0; int ch; @@ -706,6 +1124,9 @@ int vuprintf(vuprintf_push_cb push, /* call 'push()' for each output letter */ #if (FMT_LENMOD & FMT_LENMOD_z) case 'z': #endif + #if (FMT_LENMOD & FMT_LENMOD_L) + case 'L': + #endif lenmod = ch; ch = *fmt++; #if (FMT_LENMOD & (FMT_LENMOD_hh | FMT_LENMOD_ll)) @@ -747,6 +1168,40 @@ int vuprintf(vuprintf_push_cb push, /* call 'push()' for each output letter */ break; #endif + #if (FMT_RADIX & FMT_RADIX_floats) + /* any floats gets all of them (except with 'L' and %a, %A for now) */ + case 'e': + case 'E': + case 'f': + case 'F': + case 'g': + case 'G': + /* LENMOD_L isn't supported for now and will be rejected automatically */ + + /* floating point has very different spec interpretations to other + formats and requires special handling */ + fmt_buf.length = pfxlen; + fmt_buf.lenmod = lenmod; + fmt_buf.radixchar = ch; + fmt_buf.signchar = signchar; + fmt_buf.alignchar = alignchar; + fmt_buf.width = width; + fmt_buf.precision = precision; + + ch = format_double_radix(va_arg(ap, double), &fmt_buf, push, userp); + if (ch) { + count += fmt_buf.length; + if (ch > 0) { + continue; + } + + goto done; + } + + buf = fmt_buf.p; + break; + #endif + /** signed integer **/ case 'd': case 'i': diff --git a/firmware/include/ap_int.h b/firmware/include/ap_int.h new file mode 100644 index 0000000000..68cbb2fb71 --- /dev/null +++ b/firmware/include/ap_int.h @@ -0,0 +1,59 @@ +/*************************************************************************** + * __________ __ ___. + * Open \______ \ ____ ____ | | _\_ |__ _______ ___ + * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / + * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < + * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ + * \/ \/ \/ \/ \/ + * $Id$ + * + * Copyright (C) 2018 by Michael A. Sevakis + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY + * KIND, either express or implied. + * + ****************************************************************************/ +#ifndef AP_INT_H +#define AP_INT_H + +/* Miscellaneous large-sized integer functions */ + +#include <stdbool.h> +#include <stdint.h> + +/* return floor(log(2)*base2exp) - assists in estimating buffer sizes + * when converting to decimal */ +static inline int base10exp(int base2exp) +{ + /* 1292913986 = floor(2^32*log(2)) */ + static const long log10of2 = 1292913986L; + return log10of2 * (int64_t)base2exp >> 32; +} + +struct ap_int +{ + long numchunks; /* number of uint32_t chunks or zero */ + long basechunk; /* chunk of start of value bits */ + uint32_t *chunks; /* pointer to chunk array (caller alloced) */ + long len; /* length of output */ + long shift; /* number of fractional bits */ + uint64_t val; /* value, if it fits and numchunks is zero */ +}; + +bool round_number_string10(char *p_rdig, long len); + +/* format arbitrary-precision base 10 integer */ +char * format_ap_int10(struct ap_int *a, + char *p_end); + +/* format arbitrary-precision base 10 fraction */ +char * format_ap_frac10(struct ap_int *a, + char *p_start, + long precision); + +#endif /* AP_INT_H */ |