nucleo-test/Src/test.c

   1 #include "main.h"
   2
   3 #include <stdarg.h>
   4 #include <stdint.h>
   5 #include <string.h>
   6
   7 typedef unsigned int uint;
   8 typedef uint8_t byte;
   9 typedef uint16_t u16;
  10 typedef int16_t s16;
  11
  12 void debug_putc(int c)
  13 {
  14   while (!LL_USART_IsActiveFlag_TXE(USART2))
  15     ;
  16   LL_USART_TransmitData8(USART2, c);
  17 }
  18
  19 void debug_puts(const char *s)
  20 {
  21   while (*s)
  22     debug_putc(*s++);
  23 }
  24
  25 enum printf_flags {
  26   PF_ZERO_PAD = 1,
  27   PF_SIGNED = 2,
  28   PF_NEGATIVE = 4,
  29   PF_UPPERCASE = 8,
  30   PF_LEFT = 16,
  31 };
  32
  33 static void printf_string(const char *s, uint width, uint flags)
  34 {
  35   uint len = strlen(s);
  36   uint pad = (len < width) ? width - len : 0;
  37   char pad_char = (flags & PF_ZERO_PAD) ? '0' : ' ';
  38
  39   if (flags & PF_LEFT)
  40     debug_puts(s);
  41   while (pad--)
  42     debug_putc(pad_char);
  43   if (!(flags & PF_LEFT))
  44     debug_puts(s);
  45 }
  46
  47 static void printf_number(uint i, uint width, uint flags, uint base)
  48 {
  49   char buf[16];
  50   char *w = buf + sizeof(buf);
  51
  52   if (flags & PF_SIGNED)
  53     {
  54       if ((int) i < 0)
  55         {
  56           i = - (int) i;
  57           flags |= PF_NEGATIVE;
  58         }
  59     }
  60
  61   *--w = 0;
  62   do
  63     {
  64       uint digit = i % base;
  65       if (digit < 10)
  66         *--w = '0' + digit;
  67       else
  68         *--w = ((flags & PF_UPPERCASE) ? 'A' : 'a') + digit - 10;
  69       i /= base;
  70     }
  71   while (i);
  72
  73   if (flags & PF_NEGATIVE)
  74     *--w = '-';
  75
  76   printf_string(w, width, flags);
  77 }
  78
  79 void debug_printf(const char *fmt, ...)
  80 {
  81   va_list args;
  82   va_start(args, fmt);
  83
  84   while (*fmt)
  85     {
  86       int c = *fmt++;
  87       if (c != '%')
  88         {
  89           debug_putc(c);
  90           continue;
  91         }
  92
  93       uint width = 0;
  94       uint flags = 0;
  95
  96       if (*fmt == '-')
  97         {
  98           fmt++;
  99           flags |= PF_LEFT;
 100         }
 101
 102       if (*fmt == '0')
 103         {
 104           fmt++;
 105           flags |= PF_ZERO_PAD;
 106         }
 107
 108       while (*fmt >= '0' && *fmt <= '9')
 109         width = 10*width + *fmt++ - '0';
 110
 111       c = *fmt++;
 112       switch (c)
 113         {
 114         case 'd':
 115           printf_number(va_arg(args, int), width, flags | PF_SIGNED, 10);
 116           break;
 117         case 'u':
 118           printf_number(va_arg(args, int), width, flags, 10);
 119           break;
 120         case 'X':
 121           flags |= PF_UPPERCASE;
 122           // fall-thru
 123         case 'x':
 124           printf_number(va_arg(args, int), width, flags, 16);
 125           break;
 126         case 's':
 127           printf_string(va_arg(args, char *), width, flags);
 128           break;
 129         default:
 130           debug_putc(c);
 131           continue;
 132         }
 133     }
 134
 135   va_end(args);
 136 }
 137
 138 void run_test(void)
 139 {
 140   uint cnt = 0;
 141
 142   for (;;)
 143     {
 144       debug_printf(">>> %d\r\n", cnt);
 145       cnt++;
 146       LL_GPIO_SetOutputPin(LD2_GPIO_Port, LD2_Pin);
 147       LL_GPIO_SetOutputPin(MEGGY1_GPIO_Port, MEGGY1_Pin);
 148       LL_GPIO_SetOutputPin(MEGGY2_GPIO_Port, MEGGY2_Pin);
 149       LL_GPIO_SetOutputPin(MEGGY3_GPIO_Port, MEGGY3_Pin);
 150       if (LL_GPIO_IsInputPinSet(BLUE_BUTTON_GPIO_Port, BLUE_BUTTON_Pin))
 151         LL_GPIO_SetOutputPin(MEGGY4_GPIO_Port, MEGGY4_Pin);
 152       LL_mDelay(100);
 153       LL_GPIO_ResetOutputPin(LD2_GPIO_Port, LD2_Pin);
 154       LL_GPIO_ResetOutputPin(MEGGY1_GPIO_Port, MEGGY1_Pin);
 155       LL_GPIO_ResetOutputPin(MEGGY2_GPIO_Port, MEGGY2_Pin);
 156       LL_GPIO_ResetOutputPin(MEGGY3_GPIO_Port, MEGGY3_Pin);
 157       LL_GPIO_ResetOutputPin(MEGGY4_GPIO_Port, MEGGY4_Pin);
 158       LL_mDelay(100);
 159     }
 160 }