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 #if 0
 139
 140 static uint bmp_read(uint reg, uint bytes)
 141 {
 142   LL_I2C_ClearFlag_STOP(I2C1);
 143   LL_I2C_ClearFlag_BERR(I2C1);
 144   LL_I2C_HandleTransfer(I2C1, 0xee, LL_I2C_ADDRSLAVE_7BIT, 1, LL_I2C_MODE_SOFTEND, LL_I2C_GENERATE_START_WRITE);
 145   while (!LL_I2C_IsActiveFlag_TXE(I2C1))
 146     ;
 147   LL_I2C_TransmitData8(I2C1, reg);
 148   while (!LL_I2C_IsActiveFlag_TC(I2C1))
 149     ;
 150   LL_I2C_HandleTransfer(I2C1, 0xef, LL_I2C_ADDRSLAVE_7BIT, bytes, LL_I2C_MODE_AUTOEND, LL_I2C_GENERATE_RESTART_7BIT_READ);
 151
 152   uint d = 0;
 153   for (uint i=0; i<bytes; i++)
 154     {
 155       while (!LL_I2C_IsActiveFlag_RXNE(I2C1))
 156         ;
 157       d = (d << 8) | LL_I2C_ReceiveData8(I2C1);
 158     }
 159
 160   return d;
 161 }
 162
 163 static uint bmp_measure(uint type, uint bytes)
 164 {
 165   LL_I2C_HandleTransfer(I2C1, 0xee, LL_I2C_ADDRSLAVE_7BIT, 2, LL_I2C_MODE_AUTOEND, LL_I2C_GENERATE_START_WRITE);
 166   while (!LL_I2C_IsActiveFlag_TXE(I2C1))
 167     ;
 168   LL_I2C_TransmitData8(I2C1, 0xf4);
 169   while (!LL_I2C_IsActiveFlag_TXE(I2C1))
 170     ;
 171   LL_I2C_TransmitData8(I2C1, type);
 172   while (!LL_I2C_IsActiveFlag_STOP(I2C1))
 173     ;
 174
 175   while (!LL_GPIO_IsInputPinSet(BMP_DONE_GPIO_Port, BMP_DONE_Pin))
 176     ;
 177
 178   return bmp_read(0xf6, bytes);
 179 }
 180
 181 // Formulae from BMP085 specs
 182 void bmp_recalc(uint UT, uint UP, uint oss, u16 cc[11], int *tt, int *pp)
 183 {
 184   s16 AC1 = cc[0];
 185   s16 AC2 = cc[1];
 186   s16 AC3 = cc[2];
 187   u16 AC4 = cc[3];
 188   u16 AC5 = cc[4];
 189   u16 AC6 = cc[5];
 190   s16 B1 = cc[6];
 191   s16 B2 = cc[7];
 192   s16 MB = cc[8];
 193   s16 MC = cc[9];
 194   s16 MD = cc[10];
 195   UP >>= (8-oss);
 196
 197   int X1 = (UT-AC6)*AC5 / (1<<15);
 198   int X2 = MC*(1<<11) / (X1+MD);
 199   int B5 = X1 + X2;
 200   int T = (B5+8) / (1<<4);
 201   *tt = T;
 202
 203   int B6 = B5 - 4000;
 204   X1 = (B2*(B6*B6/(1<<12))) / (1<<11);
 205   X2 = AC2 * B6 / (1<<11);
 206   int X3 = X1 + X2;
 207   int B3 = (((AC1*4 + X3) << oss) + 2) / 4;
 208   X1 = AC3 * B6 / (1<<13);
 209   X2 = (B1*(B6*B6/(1<<12))) / (1<<16);
 210   X3 = ((X1+X2) + 2) / (1<<2);
 211   uint B4 = (uint)(AC4 * (X3 + 32768)) / (1U<<15);
 212   uint B7 = (uint)(UP-B3) * (uint)(50000>>oss);
 213   int p;
 214   if (B7 < 0x80000000)
 215     p = (B7*2) / B4;
 216   else
 217     p = B7 / B4 * 2;
 218   X1 = (p/(1<<8)) * (p/(1<<8));
 219   X1 = (X1*3038) / (1<<16);
 220   X2 = (-7357*p) / (1<<16);
 221   p = p + (X1 + X2 + 3791) / (1<<4);
 222   *pp = p;
 223 }
 224
 225 void run_test(void)
 226 {
 227   for (;;)
 228     {
 229       LL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
 230
 231       debug_puts("Constants:");
 232       u16 cc[11];
 233       for (uint i=0; i<11; i++)
 234         {
 235           cc[i] = bmp_read(0xaa + 2*i, 2);
 236           debug_printf(" %04x", cc[i]);
 237         }
 238       debug_puts("\r\n");
 239
 240       uint raw_temp = bmp_measure(0x2e, 2);
 241       debug_printf("Raw temperature: %04x\r\n", raw_temp);
 242
 243       uint oss = 3;     // Over-sampling setting
 244       uint raw_press = bmp_measure(0xf4 | (oss<<6), 3);
 245       debug_printf("Raw pressure: %06x\r\n", raw_press);
 246
 247       int temp, press;
 248       bmp_recalc(raw_temp, raw_press, oss, cc, &temp, &press);
 249       debug_printf("Temperature: %d ddegC\r\n", temp);
 250       debug_printf("Pressure: %d Pa\r\n", press);
 251
 252       LL_mDelay(1000);
 253     }
 254 }
 255
 256 #else
 257
 258 static const byte Gentium23x32[] = {
 259         0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF8, 0x1F, 0x00, 0x80, 0xFF, 0xFF, 0x00, 0xE0, 0xFF, 0xFF, 0x03, 0xF0, 0xFF, 0xFF, 0x0F, 0xF8, 0xFF, 0xFF, 0x1F, 0xFC, 0xFF, 0xFF, 0x3F, 0xFE, 0x01, 0xE0, 0x3F, 0x3E, 0x00, 0x00, 0x7F, 0x1F, 0x00, 0x00, 0x7C, 0x0F, 0x00, 0x00, 0x78, 0x0F, 0x00, 0x00, 0x78, 0x1F, 0x00, 0x00, 0x78, 0x3F, 0x00, 0x00, 0x78, 0x7F, 0x00, 0x00, 0x3E, 0xFE, 0x07, 0xC0, 0x3F, 0xFE, 0xFF, 0xFF, 0x1F, 0xFC, 0xFF, 0xFF, 0x0F, 0xF8, 0xFF, 0xFF, 0x07, 0xE0, 0xFF, 0xFF, 0x03, 0x80, 0xFF, 0xFF, 0x00, 0x00, 0xFC, 0x0F, 0x00,  // Code for char 0
 260         0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x70, 0x78, 0x00, 0x00, 0x70, 0x78, 0x00, 0x00, 0x78, 0x78, 0x00, 0x00, 0x78, 0x7C, 0x00, 0x00, 0x78, 0x3C, 0x00, 0x00, 0x78, 0xFE, 0xFF, 0xFF, 0x7F, 0xFE, 0xFF, 0xFF, 0x7F, 0xFE, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0x7F, 0xFE, 0xFF, 0xFF, 0x7F, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00,  // Code for char 1
 261         0x15, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x80, 0x01, 0x00, 0x78, 0xE0, 0x03, 0x00, 0x7C, 0xF0, 0x03, 0x00, 0x7E, 0xF8, 0x03, 0x80, 0x7F, 0xFC, 0x01, 0xC0, 0x7F, 0xFE, 0x01, 0xE0, 0x7F, 0x3E, 0x00, 0xF0, 0x7F, 0x1E, 0x00, 0xFC, 0x7F, 0x0F, 0x00, 0xFE, 0x79, 0x0F, 0x00, 0xFF, 0x78, 0x0F, 0xC0, 0x7F, 0x78, 0x1F, 0xE0, 0x3F, 0x78, 0x3F, 0xF8, 0x0F, 0x78, 0xFF, 0xFF, 0x07, 0x78, 0xFE, 0xFF, 0x03, 0x78, 0xFE, 0xFF, 0x00, 0x7C, 0xFC, 0x7F, 0x80, 0x7F, 0xF8, 0x1F, 0x80, 0x7F, 0xE0, 0x07, 0x80, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Code for char 2
 262         0x15, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0xC0, 0x00, 0x00, 0x1F, 0xF0, 0x01, 0x00, 0x1E, 0xF8, 0x01, 0x00, 0x3E, 0xFC, 0x01, 0x00, 0x3C, 0xFE, 0x00, 0x00, 0x7C, 0xFE, 0x00, 0x00, 0x78, 0x1E, 0xE0, 0x00, 0x78, 0x0F, 0xF0, 0x00, 0x78, 0x0F, 0xF0, 0x00, 0x78, 0x0F, 0xF0, 0x00, 0x78, 0x1F, 0xF8, 0x01, 0x7C, 0x3F, 0xFC, 0x01, 0x7C, 0xFF, 0xFF, 0x07, 0x3F, 0xFE, 0xFF, 0xFF, 0x3F, 0xFE, 0xFF, 0xFF, 0x1F, 0xFC, 0xCF, 0xFF, 0x0F, 0xF8, 0xC7, 0xFF, 0x07, 0xE0, 0x83, 0xFF, 0x03, 0x00, 0x00, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Code for char 3
 263         0x17, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x38, 0x00, 0x00, 0x00, 0x3E, 0x00, 0x00, 0x80, 0x3F, 0x00, 0x00, 0xC0, 0x3F, 0x00, 0x00, 0xF0, 0x3F, 0x00, 0x00, 0xF8, 0x3F, 0x00, 0x00, 0xFE, 0x3D, 0x00, 0x00, 0xFF, 0x3C, 0x70, 0xC0, 0x3F, 0x3C, 0x70, 0xE0, 0x1F, 0x3C, 0x70, 0xF8, 0x07, 0x3C, 0x78, 0xFC, 0x03, 0x3C, 0x78, 0xFE, 0xFF, 0xFF, 0x7F, 0xFE, 0xFF, 0xFF, 0x7F, 0xFE, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0x7F, 0xFE, 0xFF, 0xFF, 0x7F, 0x00, 0x00, 0x3C, 0x78, 0x00, 0x00, 0x3C, 0x70, 0x00, 0x00, 0x1C, 0x70, 0x00, 0x00, 0x08, 0x00,  // Code for char 4
 264         0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0xC0, 0x00, 0x1F, 0x00, 0xFE, 0x01, 0x1E, 0xFC, 0xFF, 0x01, 0x3E, 0xFC, 0xFF, 0x00, 0x3C, 0xFC, 0xFF, 0x00, 0x3C, 0xFC, 0x7F, 0x00, 0x78, 0x3C, 0x78, 0x00, 0x78, 0x3C, 0x78, 0x00, 0x78, 0x3C, 0x78, 0x00, 0x78, 0x3C, 0x78, 0x00, 0x78, 0x3C, 0xF8, 0x00, 0x7C, 0x3C, 0xF8, 0x00, 0x7E, 0x3C, 0xF0, 0x03, 0x3F, 0x3C, 0xF0, 0xFF, 0x3F, 0x3C, 0xF0, 0xFF, 0x1F, 0x1E, 0xE0, 0xFF, 0x0F, 0x0F, 0xC0, 0xFF, 0x07, 0x06, 0x80, 0xFF, 0x03, 0x00, 0x00, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00,  // Code for char 5
 265         0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0x7F, 0x00, 0x00, 0xF8, 0xFF, 0x01, 0x00, 0xFE, 0xFF, 0x07, 0x80, 0xFF, 0xFF, 0x0F, 0xC0, 0xFF, 0xFF, 0x1F, 0xE0, 0xFF, 0xFF, 0x3F, 0xF0, 0xFF, 0xC1, 0x3F, 0xF8, 0xF7, 0x00, 0x7E, 0xF8, 0x71, 0x00, 0x7C, 0xFC, 0x78, 0x00, 0x78, 0x7C, 0x78, 0x00, 0x78, 0x3E, 0x78, 0x00, 0x78, 0x1E, 0xF8, 0x00, 0x7C, 0x1E, 0xF8, 0x03, 0x3E, 0x0F, 0xF0, 0xFF, 0x3F, 0x0F, 0xF0, 0xFF, 0x1F, 0x0F, 0xE0, 0xFF, 0x0F, 0x06, 0xE0, 0xFF, 0x07, 0x00, 0x80, 0xFF, 0x03, 0x00, 0x00, 0x7E, 0x00,  // Code for char 6
 266         0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0x01, 0x00, 0x00, 0xF8, 0x01, 0x00, 0x00, 0xFC, 0x01, 0x00, 0x00, 0xFC, 0x01, 0x00, 0x20, 0x7C, 0x00, 0x00, 0x78, 0x3C, 0x00, 0x00, 0x7E, 0x3C, 0x00, 0x80, 0x7F, 0x3C, 0x00, 0xE0, 0x3F, 0x3C, 0x00, 0xF8, 0x3F, 0x3C, 0x00, 0xFE, 0x1F, 0x3C, 0x80, 0xFF, 0x0F, 0x3C, 0xE0, 0xFF, 0x01, 0x3C, 0xF8, 0x7F, 0x00, 0x3C, 0xFE, 0x0F, 0x00, 0xFC, 0xFF, 0x03, 0x00, 0xFC, 0xFF, 0x00, 0x00, 0xFC, 0x1F, 0x00, 0x00, 0xFC, 0x07, 0x00, 0x00, 0xFC, 0x01, 0x00, 0x00, 0x7C, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00, 0x00,  // Code for char 7
 267         0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x03, 0xC0, 0x07, 0xFC, 0x07, 0xF0, 0x1F, 0xFE, 0x1F, 0xF8, 0x3F, 0xFF, 0x1F, 0xFC, 0xBF, 0xFF, 0x3F, 0xFE, 0xFF, 0xFF, 0x3F, 0xFE, 0xFF, 0x0F, 0x7E, 0x1F, 0xFE, 0x03, 0x7C, 0x0F, 0xFC, 0x01, 0x78, 0x0F, 0xF8, 0x01, 0x78, 0x0F, 0xF8, 0x01, 0x78, 0x0F, 0xF8, 0x03, 0x78, 0x1F, 0xFC, 0x07, 0x7C, 0xFF, 0xFF, 0x0F, 0x3E, 0xFE, 0xFF, 0xFF, 0x3F, 0xFE, 0xDF, 0xFF, 0x1F, 0xFC, 0xCF, 0xFF, 0x0F, 0xF8, 0x87, 0xFF, 0x07, 0xF0, 0x01, 0xFF, 0x03, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00,  // Code for char 8
 268         0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x00, 0x00, 0xC0, 0xFF, 0x00, 0x00, 0xF0, 0xFF, 0x01, 0x60, 0xF8, 0xFF, 0x03, 0xE0, 0xFC, 0xFF, 0x07, 0xF0, 0xFC, 0xFF, 0x07, 0xF0, 0x7E, 0xE0, 0x0F, 0xF8, 0x1E, 0x80, 0x0F, 0x78, 0x0F, 0x00, 0x0F, 0x7C, 0x0F, 0x00, 0x0F, 0x7C, 0x0F, 0x00, 0x0F, 0x3E, 0x0F, 0x00, 0x0F, 0x3F, 0x1F, 0x00, 0x87, 0x1F, 0x3F, 0x80, 0xE7, 0x1F, 0xFE, 0xC1, 0xFB, 0x0F, 0xFE, 0xFF, 0xFF, 0x07, 0xFC, 0xFF, 0xFF, 0x03, 0xF8, 0xFF, 0xFF, 0x01, 0xF0, 0xFF, 0x7F, 0x00, 0xC0, 0xFF, 0x1F, 0x00, 0x00, 0xFF, 0x03, 0x00,  // Code for char 9
 269         0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 0x03, 0xC0, 0x07, 0xF0, 0x07, 0xE0, 0x0F, 0xF8, 0x07, 0xF0, 0x0F, 0xF8, 0x07, 0xF0, 0x0F, 0xF8, 0x07, 0xF0, 0x0F, 0xF8, 0x07, 0xF0, 0x0F, 0xF8, 0x03, 0xF0, 0x07, 0xF0, 0x01, 0xE0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00   // Code for char :
 270 };
 271
 272 // Based on https://github.com/adafruit/Adafruit_SSD1306
 273
 274 #define SSD1306_SETLOWCOLUMN 0x00
 275 #define SSD1306_SETHIGHCOLUMN 0x10
 276 #define SSD1306_MEMORYMODE 0x20
 277 #define SSD1306_SETSTARTLINE 0x40
 278 #define SSD1306_SETCONTRAST 0x81
 279 #define SSD1306_CHARGEPUMP 0x8D
 280 #define SSD1306_SEGREMAP 0xA0
 281 #define SSD1306_DISPLAYALLON_RESUME 0xA4
 282 #define SSD1306_DISPLAYALLON 0xA5
 283 #define SSD1306_NORMALDISPLAY 0xA6
 284 #define SSD1306_INVERTDISPLAY 0xA7
 285 #define SSD1306_SETMULTIPLEX 0xA8
 286 #define SSD1306_DISPLAYOFF 0xAE
 287 #define SSD1306_DISPLAYON 0xAF
 288 #define SSD1306_SETSTARTPAGE 0xB0
 289 #define SSD1306_COMSCANINC 0xC0
 290 #define SSD1306_COMSCANDEC 0xC8
 291 #define SSD1306_SETDISPLAYOFFSET 0xD3
 292 #define SSD1306_SETCOMPINS 0xDA
 293 #define SSD1306_SETVCOMDETECT 0xDB
 294 #define SSD1306_SETDISPLAYCLOCKDIV 0xD5
 295 #define SSD1306_SETPRECHARGE 0xD9
 296 #define SSD1306_NOP 0xE3
 297
 298 static const byte display_init_cmds[] = {
 299   SSD1306_DISPLAYOFF,
 300   SSD1306_SETDISPLAYCLOCKDIV, 0x80,  // the suggested ratio 0x80
 301   SSD1306_SETMULTIPLEX, 0x1F,        // ratio 32
 302   SSD1306_SETDISPLAYOFFSET,0x0,      // no offset
 303   SSD1306_SETSTARTLINE | 0x0,        // line #0
 304   SSD1306_CHARGEPUMP, 0x14,          // internal vcc
 305   SSD1306_MEMORYMODE, 0x02,          // page mode
 306   SSD1306_SEGREMAP | 0x0,            // column 0 mapped to SEG0
 307   SSD1306_COMSCANINC,                // column scan direction not reversed
 308   SSD1306_SETCOMPINS, 0x02,          // sequential COM pins, disable remap
 309   SSD1306_SETCONTRAST, 0x7F,         // contrast level 127
 310   SSD1306_SETPRECHARGE, 0xF1,        // pre-charge period (1, 15)
 311   SSD1306_SETVCOMDETECT, 0x40,       // vcomh regulator level-
 312   SSD1306_DISPLAYALLON_RESUME,
 313   SSD1306_NORMALDISPLAY,
 314   SSD1306_DISPLAYON,
 315 };
 316
 317 static void display_cmd(byte cmd)
 318 {
 319   LL_I2C_ClearFlag_STOP(I2C1);
 320   LL_I2C_HandleTransfer(I2C1, 0x79, LL_I2C_ADDRSLAVE_7BIT, 2, LL_I2C_MODE_AUTOEND, LL_I2C_GENERATE_START_WRITE);
 321   while (!LL_I2C_IsActiveFlag_TXE(I2C1))
 322     ;
 323   LL_I2C_TransmitData8(I2C1, 0x00);             // Will send a command
 324   while (!LL_I2C_IsActiveFlag_TXE(I2C1))
 325     ;
 326   LL_I2C_TransmitData8(I2C1, cmd);
 327   while (!LL_I2C_IsActiveFlag_STOP(I2C1))
 328     ;
 329 }
 330
 331 static void display_data_start(uint cnt)
 332 {
 333   LL_I2C_ClearFlag_STOP(I2C1);
 334   LL_I2C_HandleTransfer(I2C1, 0x79, LL_I2C_ADDRSLAVE_7BIT, cnt + 1, LL_I2C_MODE_AUTOEND, LL_I2C_GENERATE_START_WRITE);
 335   while (!LL_I2C_IsActiveFlag_TXE(I2C1))
 336     ;
 337   LL_I2C_TransmitData8(I2C1, 0x40);             // Will send data
 338 }
 339
 340 static void display_data(byte d)
 341 {
 342   while (!LL_I2C_IsActiveFlag_TXE(I2C1))
 343     ;
 344   LL_I2C_TransmitData8(I2C1, d);
 345 }
 346
 347 static void display_data_end(void)
 348 {
 349   while (!LL_I2C_IsActiveFlag_STOP(I2C1))
 350     ;
 351 }
 352
 353 static void display_init(void)
 354 {
 355   for (uint i=0; i < sizeof(display_init_cmds); i++)
 356     display_cmd(display_init_cmds[i]);
 357
 358   for (uint p=0; p<4; p++)
 359     {
 360       display_cmd(SSD1306_SETSTARTPAGE + p);
 361       display_cmd(SSD1306_SETHIGHCOLUMN);
 362       display_cmd(SSD1306_SETLOWCOLUMN);
 363       display_data_start(128);
 364       for (uint i=0; i<128; i++)
 365         {
 366           byte x = 0;
 367           // x = Gentium23x32[(23*4+1)*(i/23) + 1 + 4*(i%23) + p];
 368           display_data(x);
 369         }
 370       display_data_end();
 371     }
 372 }
 373
 374 static void counter(uint cnt)
 375 {
 376   byte d[5];
 377   for (uint i=0; i<5; i++)
 378     {
 379       d[4-i] = cnt % 10;
 380       cnt /= 10;
 381     }
 382
 383   for (uint p=0; p<4; p++)
 384     {
 385       display_cmd(SSD1306_SETSTARTPAGE + p);
 386       display_cmd(SSD1306_SETHIGHCOLUMN);
 387       display_cmd(SSD1306_SETLOWCOLUMN);
 388       display_data_start(5*24);
 389       for (uint i=0; i<5; i++)
 390         {
 391           for (uint j=0; j<23; j++)
 392             {
 393               byte x = Gentium23x32[(23*4+1)*d[i] + 1 + 4*j + p];
 394               display_data(x);
 395             }
 396           display_data(0);
 397         }
 398       display_data_end();
 399     }
 400 }
 401
 402 void run_test(void)
 403 {
 404   uint cnt = 0;
 405
 406   debug_puts("Init\r\n");
 407   display_init();
 408
 409   for (;;)
 410     {
 411       LL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
 412
 413       debug_printf("Tick tock: %d\r\n", cnt);
 414
 415       counter(cnt);
 416
 417       LL_mDelay(1000);
 418       cnt++;
 419     }
 420 }
 421
 422 #endif