Formatting, buttons, printf from musl

Makefile

@@ -3,13 +3,15 @@ OOCD_INTERFACE  ?= stlink-v2
 OOCD_TARGET     ?= stm32f4x
 BMP_PORT        ?= /dev/ttyBmpGdb
 
-CFLAGS=-g -O2 -std=c99 -pedantic -Wall
+COMFLAGS=-g -Os -pedantic -Wall
-CXXFLAGS=-g -O2 -std=c++17 -pedantic -Wall
 
-OBJS = usb.o adc.o ringbuffer.o uart.o
+CFLAGS=$(COMFLAGS) -std=c99 -D_POSIX_C_SOURCE=200809L
+CXXFLAGS=$(COMFLAGS) -std=c++17
+
+OBJS = usb.o adc.o ringbuffer.o uart.o buttons.o printf.o
 BINARY ?= main
 DEVICE=STM32F405RG
-#DEVICE=STM32F411CE
+
+printf.o: CFLAGS:=$(CFLAGS) -Wno-parentheses -Wno-char-subscripts -Wno-sign-compare -Wno-implicit-fallthrough
 
 include ../rules.mk
-

buttons.c

@@ -0,0 +1,54 @@
+#include <libopencm3/stm32/rcc.h>
+#include <libopencm3/stm32/gpio.h>
+#include <stdint.h>
+#include <stdio.h>
+#include "buttons.h"
+
+#define BUTTON_RCC RCC_GPIOA
+#define BUTTON_PORT GPIOA
+#define OPEN_BUTTON GPIO6
+#define CLOSE_BUTTON GPIO7
+
+void buttons_setup() {
+  rcc_periph_clock_enable(BUTTON_RCC);
+  gpio_mode_setup(BUTTON_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, OPEN_BUTTON | CLOSE_BUTTON);
+  gpio_set_output_options(BUTTON_PORT, GPIO_OTYPE_OD, GPIO_OSPEED_2MHZ, OPEN_BUTTON | CLOSE_BUTTON);
+}
+
+static uint32_t open_ticks = 0;
+static uint32_t close_ticks = 0;
+
+void buttons_open(uint32_t ticks) {
+  gpio_set(BUTTON_PORT, OPEN_BUTTON);
+  printf("Open %lu", ticks);
+  open_ticks = ticks;
+}
+
+void buttons_close(uint32_t ticks) {
+  gpio_set(BUTTON_PORT, CLOSE_BUTTON);
+  printf("Close %lu", ticks);
+  close_ticks = ticks;
+}
+
+void buttons_release(void) {
+  open_ticks = close_ticks = 0;
+  printf("Release");
+  gpio_clear(BUTTON_PORT, OPEN_BUTTON | CLOSE_BUTTON);
+}
+
+void buttons_tick(void) {
+  if (open_ticks) {
+    open_ticks--;
+    if (!open_ticks) {
+      gpio_clear(BUTTON_PORT, OPEN_BUTTON);
+      printf("Release open");
+    }
+  }
+  if (close_ticks) {
+    close_ticks--;
+    if (!close_ticks) {
+      printf("Release close");
+      gpio_clear(BUTTON_PORT, CLOSE_BUTTON);
+    }
+  }
+}

buttons.h

@@ -0,0 +1,8 @@
+#ifndef BUTTONS_H
+#define BUTTONS_H
+void buttons_setup(void);
+void buttons_open(uint32_t ticks);
+void buttons_close(uint32_t ticks);
+void buttons_release(void);
+void buttons_tick(void);
+#endif

main.c

@@ -5,6 +5,7 @@
 #include <stdio.h>
 
 #include "adc.h"
+#include "buttons.h"
 #include "usb.h"
 #include "ringbuffer.h"
 #include "uart.h"
@@ -44,20 +45,31 @@ int main(void) {
   adc_setup();
   uart_setup();
   sys_tick_setup();
+  buttons_setup();
 
   nvic_enable_irq(NVIC_USART3_IRQ);
 
+  uint32_t last_tick = tick;
   while (1) {
     /* Handle control messages through the comms CDC */
     char buf[64];
     unsigned buf_len = 0;
+
+    while (tick != last_tick) {
+      buttons_tick();
+      last_tick++;
+    }
+
     for (char c; ringbuffer_get(comm_in_buf, (void *)&c, 1);) {
       switch (c) {
         case 'B':
-        buf_len =
+          printf("%lu", (unsigned long)adc_bat_voltage());
-            snprintf(buf, sizeof(buf), "%lu\r\n",
+          break;
-                     (unsigned long)adc_bat_voltage());
+        case 'O':
-        ringbuffer_add(comm_out_buf, buf, buf_len);
+          buttons_open(15);
+          break;
+        case 'C':
+          buttons_close(15);
           break;
       }
     }

printf.c

@@ -0,0 +1,400 @@
+#include "stdio_impl.h"
+#include "ringbuffer.h"
+#include <errno.h>
+#include <ctype.h>
+#include <limits.h>
+#include <string.h>
+#include <stdarg.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <inttypes.h>
+#include <math.h>
+
+/* Some useful macros */
+
+#define MAX(a,b) ((a)>(b) ? (a) : (b))
+#define MIN(a,b) ((a)<(b) ? (a) : (b))
+
+/* Convenient bit representation for modifier flags, which all fall
+ * within 31 codepoints of the space character. */
+
+#define ALT_FORM   (1U<<'#'-' ')
+#define ZERO_PAD   (1U<<'0'-' ')
+#define LEFT_ADJ   (1U<<'-'-' ')
+#define PAD_POS    (1U<<' '-' ')
+#define MARK_POS   (1U<<'+'-' ')
+#define GROUPED    (1U<<'\''-' ')
+
+#define FLAGMASK (ALT_FORM|ZERO_PAD|LEFT_ADJ|PAD_POS|MARK_POS|GROUPED)
+
+/* State machine to accept length modifiers + conversion specifiers.
+ * Result is 0 on failure, or an argument type to pop on success. */
+
+enum {
+  BARE, LPRE, LLPRE, HPRE, HHPRE, BIGLPRE,
+  ZTPRE, JPRE,
+  STOP,
+  PTR, INT, UINT, ULLONG,
+  LONG, ULONG,
+  SHORT, USHORT, CHAR, UCHAR,
+  LLONG, SIZET, IMAX, UMAX, PDIFF, UIPTR,
+  NOARG,
+  MAXSTATE
+};
+
+#define S(x) [(x)-'A']
+
+extern ringbuffer *comm_out_buf;
+
+static const unsigned char states[]['z'-'A'+1] = {
+  { /* 0: bare types */
+    S('d') = INT, S('i') = INT,
+    S('o') = UINT, S('u') = UINT, S('x') = UINT, S('X') = UINT,
+    S('c') = CHAR, S('C') = INT,
+    S('s') = PTR, S('S') = PTR, S('p') = UIPTR, S('n') = PTR,
+    S('m') = NOARG,
+    S('l') = LPRE, S('h') = HPRE, S('L') = BIGLPRE,
+    S('z') = ZTPRE, S('j') = JPRE, S('t') = ZTPRE,
+  }, { /* 1: l-prefixed */
+    S('d') = LONG, S('i') = LONG,
+    S('o') = ULONG, S('u') = ULONG, S('x') = ULONG, S('X') = ULONG,
+    S('c') = INT, S('s') = PTR, S('n') = PTR,
+    S('l') = LLPRE,
+  }, { /* 2: ll-prefixed */
+    S('d') = LLONG, S('i') = LLONG,
+    S('o') = ULLONG, S('u') = ULLONG,
+    S('x') = ULLONG, S('X') = ULLONG,
+    S('n') = PTR,
+  }, { /* 3: h-prefixed */
+    S('d') = SHORT, S('i') = SHORT,
+    S('o') = USHORT, S('u') = USHORT,
+    S('x') = USHORT, S('X') = USHORT,
+    S('n') = PTR,
+    S('h') = HHPRE,
+  }, { /* 4: hh-prefixed */
+    S('d') = CHAR, S('i') = CHAR,
+    S('o') = UCHAR, S('u') = UCHAR,
+    S('x') = UCHAR, S('X') = UCHAR,
+    S('n') = PTR,
+  }, { /* 5: L-prefixed */
+    S('n') = PTR,
+  }, { /* 6: z- or t-prefixed (assumed to be same size) */
+    S('d') = PDIFF, S('i') = PDIFF,
+    S('o') = SIZET, S('u') = SIZET,
+    S('x') = SIZET, S('X') = SIZET,
+    S('n') = PTR,
+  }, { /* 7: j-prefixed */
+    S('d') = IMAX, S('i') = IMAX,
+    S('o') = UMAX, S('u') = UMAX,
+    S('x') = UMAX, S('X') = UMAX,
+    S('n') = PTR,
+  }
+};
+
+#define OOB(x) ((unsigned)(x)-'A' > 'z'-'A')
+
+union arg
+{
+  uintmax_t i;
+  long double f;
+  void *p;
+};
+
+static void pop_arg(union arg *arg, int type, va_list *ap)
+{
+  switch (type) {
+         case PTR:	arg->p = va_arg(*ap, void *);
+  break; case INT:	arg->i = va_arg(*ap, int);
+  break; case UINT:	arg->i = va_arg(*ap, unsigned int);
+  break; case LONG:	arg->i = va_arg(*ap, long);
+  break; case ULONG:	arg->i = va_arg(*ap, unsigned long);
+  break; case ULLONG:	arg->i = va_arg(*ap, unsigned long long);
+  break; case SHORT:	arg->i = (short)va_arg(*ap, int);
+  break; case USHORT:	arg->i = (unsigned short)va_arg(*ap, int);
+  break; case CHAR:	arg->i = (signed char)va_arg(*ap, int);
+  break; case UCHAR:	arg->i = (unsigned char)va_arg(*ap, int);
+  break; case LLONG:	arg->i = va_arg(*ap, long long);
+  break; case SIZET:	arg->i = va_arg(*ap, size_t);
+  break; case IMAX:	arg->i = va_arg(*ap, intmax_t);
+  break; case UMAX:	arg->i = va_arg(*ap, uintmax_t);
+  break; case PDIFF:	arg->i = va_arg(*ap, ptrdiff_t);
+  break; case UIPTR:	arg->i = (uintptr_t)va_arg(*ap, void *);
+  }
+}
+
+static void out(FILE *f, const char *s, size_t l)
+{
+  f->f(f->arg, s, l);
+}
+
+static void pad(FILE *f, char c, int w, int l, int fl)
+{
+  char pad[256];
+  if (fl & (LEFT_ADJ | ZERO_PAD) || l >= w) return;
+  l = w - l;
+  memset(pad, c, l>sizeof pad ? sizeof pad : l);
+  for (; l >= sizeof pad; l -= sizeof pad)
+    out(f, pad, sizeof pad);
+  out(f, pad, l);
+}
+
+static const char xdigits[16] = {
+  "0123456789ABCDEF"
+};
+
+static char *fmt_x(uintmax_t x, char *s, int lower)
+{
+  for (; x; x>>=4) *--s = xdigits[(x&15)]|lower;
+  return s;
+}
+
+static char *fmt_o(uintmax_t x, char *s)
+{
+  for (; x; x>>=3) *--s = '0' + (x&7);
+  return s;
+}
+
+static char *fmt_u(uintmax_t x, char *s)
+{
+  unsigned long y;
+  for (   ; x>ULONG_MAX; x/=10) *--s = '0' + x%10;
+  for (y=x;           y; y/=10) *--s = '0' + y%10;
+  return s;
+}
+
+static int getint(char **s) {
+  int i;
+  for (i=0; isdigit(**s); (*s)++) {
+    if (i > INT_MAX/10U || **s-'0' > INT_MAX-10*i) i = -1;
+    else i = 10*i + (**s-'0');
+  }
+  return i;
+}
+
+static int printf_core(FILE *f, const char *fmt, va_list *ap, union arg *nl_arg, int *nl_type)
+{
+  char *a, *z, *s=(char *)fmt;
+  unsigned l10n=0, fl;
+  int w, p, xp;
+  union arg arg;
+  int argpos;
+  unsigned st, ps;
+  int cnt=0, l=0;
+  size_t i;
+  const char *prefix;
+  int t, pl;
+
+  for (;;) {
+    /* This error is only specified for snprintf, but since it's
+     * unspecified for other forms, do the same. Stop immediately
+     * on overflow; otherwise %n could produce wrong results. */
+    if (l > INT_MAX - cnt) goto overflow;
+
+    /* Update output count, end loop when fmt is exhausted */
+    cnt += l;
+    if (!*s) break;
+
+    /* Handle literal text and %% format specifiers */
+    for (a=s; *s && *s!='%'; s++);
+    for (z=s; s[0]=='%' && s[1]=='%'; z++, s+=2);
+    if (z-a > INT_MAX-cnt) goto overflow;
+    l = z-a;
+    if (f) out(f, a, l);
+    if (l) continue;
+
+    if (isdigit(s[1]) && s[2]=='$') {
+      l10n=1;
+      argpos = s[1]-'0';
+      s+=3;
+    } else {
+      argpos = -1;
+      s++;
+    }
+
+    /* Read modifier flags */
+    for (fl=0; (unsigned)*s-' '<32 && (FLAGMASK&(1U<<*s-' ')); s++)
+      fl |= 1U<<*s-' ';
+
+    /* Read field width */
+    if (*s=='*') {
+      if (isdigit(s[1]) && s[2]=='$') {
+        l10n=1;
+        nl_type[s[1]-'0'] = INT;
+        w = nl_arg[s[1]-'0'].i;
+        s+=3;
+      } else if (!l10n) {
+        w = f ? va_arg(*ap, int) : 0;
+        s++;
+      } else goto inval;
+      if (w<0) fl|=LEFT_ADJ, w=-w;
+    } else if ((w=getint(&s))<0) goto overflow;
+
+    /* Read precision */
+    if (*s=='.' && s[1]=='*') {
+      if (isdigit(s[2]) && s[3]=='$') {
+        nl_type[s[2]-'0'] = INT;
+        p = nl_arg[s[2]-'0'].i;
+        s+=4;
+      } else if (!l10n) {
+        p = f ? va_arg(*ap, int) : 0;
+        s+=2;
+      } else goto inval;
+      xp = (p>=0);
+    } else if (*s=='.') {
+      s++;
+      p = getint(&s);
+      xp = 1;
+    } else {
+      p = -1;
+      xp = 0;
+    }
+
+    /* Format specifier state machine */
+    st=0;
+    do {
+      if (OOB(*s)) goto inval;
+      ps=st;
+      st=states[st]S(*s++);
+    } while (st-1<STOP);
+    if (!st) goto inval;
+
+    /* Check validity of argument type (nl/normal) */
+    if (st==NOARG) {
+      if (argpos>=0) goto inval;
+    } else {
+      if (argpos>=0) nl_type[argpos]=st, arg=nl_arg[argpos];
+      else if (f) pop_arg(&arg, st, ap);
+      else return 0;
+    }
+
+    if (!f) continue;
+
+    prefix = "-+   0X0x";
+    pl = 0;
+    t = s[-1];
+
+    /* Transform ls,lc -> S,C */
+    if (ps && (t&15)==3) t&=~32;
+
+    /* - and 0 flags are mutually exclusive */
+    if (fl & LEFT_ADJ) fl &= ~ZERO_PAD;
+
+    switch(t) {
+    case 'n':
+      switch(ps) {
+      case BARE: *(int *)arg.p = cnt; break;
+      case LPRE: *(long *)arg.p = cnt; break;
+      case LLPRE: *(long long *)arg.p = cnt; break;
+      case HPRE: *(unsigned short *)arg.p = cnt; break;
+      case HHPRE: *(unsigned char *)arg.p = cnt; break;
+      case ZTPRE: *(size_t *)arg.p = cnt; break;
+      case JPRE: *(uintmax_t *)arg.p = cnt; break;
+      }
+      continue;
+    case 'p':
+      p = MAX(p, 2*sizeof(void*));
+      t = 'x';
+      fl |= ALT_FORM;
+    case 'x': case 'X':
+      a = fmt_x(arg.i, z, t&32);
+      if (arg.i && (fl & ALT_FORM)) prefix+=(t>>4), pl=2;
+      if (0) {
+    case 'o':
+      a = fmt_o(arg.i, z);
+      if ((fl&ALT_FORM) && p<z-a+1) p=z-a+1;
+      } if (0) {
+    case 'd': case 'i':
+      pl=1;
+      if (arg.i>INTMAX_MAX) {
+        arg.i=-arg.i;
+      } else if (fl & MARK_POS) {
+        prefix++;
+      } else if (fl & PAD_POS) {
+        prefix+=2;
+      } else pl=0;
+    case 'u':
+      a = fmt_u(arg.i, z);
+      }
+      if (xp && p<0) goto overflow;
+      if (xp) fl &= ~ZERO_PAD;
+      if (!arg.i && !p) {
+        a=z;
+        break;
+      }
+      p = MAX(p, z-a + !arg.i);
+      break;
+    case 'c':
+      *(a=z-(p=1))=arg.i;
+      fl &= ~ZERO_PAD;
+      break;
+    case 's':
+      a = arg.p ? arg.p : "(null)";
+      z = a + strnlen(a, p<0 ? INT_MAX : p);
+      if (p<0 && *z) goto overflow;
+      p = z-a;
+      fl &= ~ZERO_PAD;
+      break;
+    }
+
+    if (p < z-a) p = z-a;
+    if (p > INT_MAX-pl) goto overflow;
+    if (w < pl+p) w = pl+p;
+    if (w > INT_MAX-cnt) goto overflow;
+
+    pad(f, ' ', w, pl+p, fl);
+    out(f, prefix, pl);
+    pad(f, '0', w, pl+p, fl^ZERO_PAD);
+    pad(f, '0', p, z-a, 0);
+    out(f, a, z-a);
+    pad(f, ' ', w, pl+p, fl^LEFT_ADJ);
+
+    l = w;
+  }
+
+  if (f) return cnt;
+  if (!l10n) return 0;
+
+  for (i=1; i<=NL_ARGMAX && nl_type[i]; i++)
+    pop_arg(nl_arg+i, nl_type[i], ap);
+  for (; i<=NL_ARGMAX && !nl_type[i]; i++);
+  if (i<=NL_ARGMAX) goto inval;
+  return 1;
+
+inval:
+  errno = EINVAL;
+  return -1;
+overflow:
+  errno = EOVERFLOW;
+  return -1;
+}
+
+int vfprintf(FILE *restrict f, const char *restrict fmt, va_list ap)
+{
+  va_list ap2;
+  int nl_type[NL_ARGMAX+1] = {0};
+  union arg nl_arg[NL_ARGMAX+1];
+  int ret;
+
+  /* the copy allows passing va_list* even if va_list is an array */
+  va_copy(ap2, ap);
+  if (printf_core(0, fmt, &ap2, nl_arg, nl_type) < 0) {
+    va_end(ap2);
+    return -1;
+  }
+
+  else ret = printf_core(f, fmt, &ap2, nl_arg, nl_type);
+  va_end(ap2);
+  return ret;
+}
+
+int printf(char const *fmt, ...) {
+  FILE f = {.f = (int (*)(void *, char const *, int)) & ringbuffer_add, .arg = comm_out_buf};
+
+  va_list ap;
+  size_t l = 0;
+  va_start(ap, fmt);
+  l = vfprintf(&f, fmt, ap);
+  va_end(ap);
+
+  return l + ringbuffer_add(comm_out_buf, "\r\n", 2);
+}

ringbuffer.c

@@ -29,8 +29,8 @@ unsigned ringbuffer_level(ringbuffer *buf) {
   return size - tail + head;
 }
 
-unsigned ringbuffer_add(ringbuffer *buf, void *dat_, unsigned len) {
+unsigned ringbuffer_add(ringbuffer *buf, void const *dat_, unsigned len) {
-  uint8_t *dat = dat_;
+  uint8_t const *dat  = dat_;
   unsigned rem        = ringbuffer_free(buf);
   unsigned head       = buf->head;
   unsigned written    = 0;

ringbuffer.h

@@ -12,24 +12,41 @@ typedef struct {
   uint8_t data[];
 } ringbuffer;
 
-#define RINGBUFFER_STORAGE(NAME, SIZE) uint8_t buf_##NAME##__LINE__[sizeof(ringbuffer) + SIZE]; ringbuffer *NAME = (ringbuffer*)buf_##NAME##__LINE__;
+#define RINGBUFFER_STORAGE(NAME, SIZE)                           \
-#define RINGBUFFER_INIT(NAME, SIZE) NAME->head = NAME->tail = 0; NAME->size = SIZE;
+  uint8_t buf_##NAME##__LINE__[sizeof(ringbuffer) + SIZE]; \
+  ringbuffer *NAME = (ringbuffer *)buf_##NAME##__LINE__;
+#define RINGBUFFER_INIT(NAME, SIZE)  \
+  NAME->head = NAME->tail = 0; \
+  NAME->size              = SIZE;
 #define RINGBUFFER(NAME, SIZE)          \
   RINGBUFFER_STORAGE(NAME, SIZE); \
   RINGBUFFER_INIT(NAME, SIZE);
 
+/** Fully empties the ringbuffer */
 void ringbuffer_rst(ringbuffer *buf);
 
+/** The total capacity of the ringbuffer */
 unsigned ringbuffer_capacity(ringbuffer *buf);
+
+/** The free capacity of the ringbuffer */
 unsigned ringbuffer_free(ringbuffer *buf);
+
+/** The used capacity of the ringbuffer */
 unsigned ringbuffer_level(ringbuffer *buf);
 
+/** Non-zero if the ringbuffer is empty */
 unsigned ringbuffer_empty(ringbuffer *buf);
 
-unsigned ringbuffer_add(ringbuffer *buf, void *dat, unsigned len);
+/** Add data to the ringbuffer, return actual length of added data */
+unsigned ringbuffer_add(ringbuffer *buf, void const *dat, unsigned len);
 
+/** Get data from the ringbuffer, return actual length of retrieved data */
 unsigned ringbuffer_get(ringbuffer *buf, void *dst, unsigned len);
+
+/** Get data from the ringbuffer, but do not remove it. Return actual length of retrieved data. */
 unsigned ringbuffer_peek(ringbuffer *buf, void *dst, unsigned len);
+
+/** Remove data from the ringbuffer without reading it */
 void ringbuffer_skip(ringbuffer *buf, unsigned len);
 
 #ifdef __cplusplus

stdio_impl.h

@@ -0,0 +1,12 @@
+#ifndef STDIO_IMPL_H
+#define STDIO_IMPL_H
+#include <ctype.h>
+#include <stdarg.h>
+typedef struct {
+  int (*f)(void *, char const *, int);
+  void *arg;
+} FILE;
+
+int vfprintf(FILE *restrict f, const char *restrict fmt, va_list ap);
+int printf(char const *fmt, ...);
+#endif