Inital commit of stm32f4xx framework.

1 files changed, 277 insertions, 0 deletions

diff --git a/stm/printf.c b/stm/printf.c
new file mode 100644
index 0000000000..1cdb6a48f6
--- /dev/null
+++ b/stm/printf.c
@@ -0,0 +1,277 @@
+#include <stdarg.h>
+#include "std.h"
+
+#define PF_FLAG_LEFT_ADJUST (0x01)
+#define PF_FLAG_SHOW_SIGN   (0x02)
+#define PF_FLAG_SPACE_SIGN  (0x04)
+#define PF_FLAG_NO_TRAILZ   (0x08)
+#define PF_FLAG_ZERO_PAD    (0x10)
+
+// tricky; we compute pad string by: pad_chars + (flags & PF_FLAG_ZERO_PAD)
+#define PF_PAD_SIZE PF_FLAG_ZERO_PAD
+static const char *pad_chars = "                0000000000000000";
+
+typedef struct _pfenv_t {
+    void *data;
+    void (*print_strn)(void *, const char *str, unsigned int len);
+} pfenv_t;
+
+static void print_str_dummy(void *data, const char *str, unsigned int len) {
+}
+
+pfenv_t pfenv_dummy = {0, print_str_dummy};
+
+static int pfenv_print_strn(pfenv_t *pfenv, const char *str, unsigned int len, int flags, int width) {
+    int pad = width - len;
+    if (pad > 0 && (flags & PF_FLAG_LEFT_ADJUST) == 0) {
+        while (pad > 0) {
+            int p = pad;
+            if (p > PF_PAD_SIZE)
+                p = PF_PAD_SIZE;
+            pfenv->print_strn(pfenv->data, pad_chars + (flags & PF_FLAG_ZERO_PAD), p);
+            pad -= p;
+        }
+    }
+    pfenv->print_strn(pfenv->data, str, len);
+    while (pad > 0) {
+        int p = pad;
+        if (p > PF_PAD_SIZE)
+            p = PF_PAD_SIZE;
+        pfenv->print_strn(pfenv->data, pad_chars, p);
+        pad -= p;
+    }
+    return len;
+}
+
+// enough room for 32 signed number
+#define INT_BUF_SIZE (12)
+
+static int pfenv_print_int(pfenv_t *pfenv, unsigned int x, int sgn, int base, int base_char, int flags, int width) {
+    char sign = 0;
+    if (sgn) {
+        if ((int)x < 0) {
+            sign = '-';
+            x = -x;
+        } else if (flags & PF_FLAG_SHOW_SIGN) {
+            sign = '+';
+        } else if (flags & PF_FLAG_SPACE_SIGN) {
+            sign = ' ';
+        }
+    }
+
+    char buf[INT_BUF_SIZE];
+    char *b = buf + INT_BUF_SIZE;
+
+    if (x == 0) {
+        *(--b) = '0';
+    } else {
+        do {
+            int c = x % base;
+            x /= base;
+            if (c >= 10) {
+                c += base_char - 10;
+            } else {
+                c += '0';
+            }
+            *(--b) = c;
+        } while (b > buf && x != 0);
+    }
+
+    if (b > buf && sign != 0) {
+        *(--b) = sign;
+    }
+
+    return pfenv_print_strn(pfenv, b, buf + INT_BUF_SIZE - b, flags, width);
+}
+
+void pfenv_prints(pfenv_t *pfenv, const char *str) {
+    pfenv->print_strn(pfenv->data, str, strlen(str));
+}
+
+int pfenv_printf(pfenv_t *pfenv, const char *fmt, va_list args) {
+    int chrs = 0;
+    for (;;) {
+        {
+            const char *f = fmt;
+            while (*f != '\0' && *f != '%') {
+                ++f; // XXX UTF8 advance char
+            }
+            if (f > fmt) {
+                pfenv->print_strn(pfenv->data, fmt, f - fmt);
+                chrs += f - fmt;
+                fmt = f;
+            }
+        }
+
+        if (*fmt == '\0') {
+            break;
+        }
+
+        // move past % character
+        ++fmt;
+
+        // parse flags, if they exist
+        int flags = 0;
+        while (*fmt != '\0') {
+            if (*fmt == '-') flags |= PF_FLAG_LEFT_ADJUST;
+            else if (*fmt == '+') flags |= PF_FLAG_SHOW_SIGN;
+            else if (*fmt == ' ') flags |= PF_FLAG_SPACE_SIGN;
+            else if (*fmt == '!') flags |= PF_FLAG_NO_TRAILZ;
+            else if (*fmt == '0') flags |= PF_FLAG_ZERO_PAD;
+            else break;
+            ++fmt;
+        }
+
+        // parse width, if it exists
+        int width = 0;
+        for (; '0' <= *fmt && *fmt <= '9'; ++fmt) {
+            width = width * 10 + *fmt - '0';
+        }
+
+        // parse precision, if it exists
+        int prec = -1;
+        if (*fmt == '.') {
+            ++fmt;
+            if (*fmt == '*') {
+                ++fmt;
+                prec = va_arg(args, int);
+            } else {
+                prec = 0;
+                for (; '0' <= *fmt && *fmt <= '9'; ++fmt) {
+                    prec = prec * 10 + *fmt - '0';
+                }
+            }
+            if (prec < 0) {
+                prec = 0;
+            }
+        }
+
+        // parse long specifiers (current not used)
+        //bool long_arg = false;
+        if (*fmt == 'l') {
+            ++fmt;
+            //long_arg = true;
+        }
+
+        if (*fmt == '\0') {
+            break;
+        }
+
+        switch (*fmt) {
+            case 'b':
+                if (va_arg(args, int)) {
+                    chrs += pfenv_print_strn(pfenv, "true", 4, flags, width);
+                } else {
+                    chrs += pfenv_print_strn(pfenv, "false", 5, flags, width);
+                }
+                break;
+            case 'c':
+            {
+                char str = va_arg(args, int);
+                chrs += pfenv_print_strn(pfenv, &str, 1, flags, width);
+                break;
+            }
+            case 's':
+            {
+                const char *str = va_arg(args, const char*);
+                if (str) {
+                    if (prec < 0) {
+                        prec = strlen(str);
+                    }
+                    chrs += pfenv_print_strn(pfenv, str, prec, flags, width);
+                } else {
+                    chrs += pfenv_print_strn(pfenv, "(null)", 6, flags, width);
+                }
+                break;
+            }
+            case 'u':
+                chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 10, 'a', flags, width);
+                break;
+            case 'd':
+                chrs += pfenv_print_int(pfenv, va_arg(args, int), 1, 10, 'a', flags, width);
+                break;
+            case 'x':
+            case 'p': // ?
+                chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 16, 'a', flags, width);
+                break;
+            case 'X':
+            case 'P': // ?
+                chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 16, 'A', flags, width);
+                break;
+            default:
+                pfenv->print_strn(pfenv->data, fmt, 1);
+                chrs += 1;
+                break;
+        }
+        ++fmt;
+    }
+    return chrs;
+}
+
+void lcd_print_strn(const char *str, unsigned int len);
+
+void xxx(void *data, const char *str, unsigned int len) {
+    lcd_print_strn(str, len);
+}
+
+pfenv_t pfenv_stdout = {0, xxx};
+
+int printf(const char *fmt, ...) {
+    //pfenv_t pfenv;
+    //pfenv.data = 0;
+    //pfenv.print_strn = xxx;
+    va_list args;
+    va_start(args, fmt);
+    return pfenv_printf(&pfenv_stdout, fmt, args);
+}
+
+// need this because gcc optimises printf("%c", c) -> putchar(c), and printf("a") -> putchar('a')
+int putchar(int c) {
+    char chr = c;
+    xxx(0, &chr, 1);
+    return chr;
+}
+
+// need this because gcc optimises printf("string\n") -> puts("string")
+int puts(const char *s) {
+    xxx(0, s, strlen(s));
+    char chr = '\n';
+    xxx(0, &chr, 1);
+    return 1;
+}
+
+typedef struct _strn_pfenv_t {
+    char *cur;
+    size_t remain;
+} strn_pfenv_t;
+
+void strn_print_strn(void *data, const char *str, unsigned int len) {
+    strn_pfenv_t *strn_pfenv = data;
+    if (len > strn_pfenv->remain) {
+        len = strn_pfenv->remain;
+    }
+    memcpy(strn_pfenv->cur, str, len);
+    strn_pfenv->cur += len;
+    strn_pfenv->remain -= len;
+}
+
+int snprintf(char *str, size_t size, const char *fmt, ...) {
+    strn_pfenv_t strn_pfenv;
+    strn_pfenv.cur = str;
+    strn_pfenv.remain = size;
+    pfenv_t pfenv;
+    pfenv.data = &strn_pfenv;
+    pfenv.print_strn = strn_print_strn;
+    va_list args;
+    va_start(args, fmt);
+    int len = pfenv_printf(&pfenv, fmt, args);
+    // add terminating null byte
+    if (size > 0) {
+        if (strn_pfenv.remain == 0) {
+            strn_pfenv.cur[-1] = 0;
+        } else {
+            strn_pfenv.cur[0] = 0;
+        }
+    }
+    return len;
+}