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