Add proper float formatting to vuprintf

Wanted to see how gnarly it is to do.

Big number handling could be done with better algorithms
since it can get a bit slow with large integers or tiny
fractions with many lead zeros when only a few digits are
needed.

Anyway, it supports %e, %E, %f, %F, %g and %G. No %a or long
double support seems warranted at the moment.

Assumes IEEE 754 double format but it's laid out to be able to
replace a function to handle others if needed.

Tested in a driver program that has a duplicate vuprintf and
the content was pasted in once it looked sound enough to put
up a patch.

Change-Id: I6dae8624d3208e644c88e36e6a17d8fc9144f988

1 files changed, 465 insertions, 10 deletions

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':