--- /dev/null
+#include "util.h"
+#include "main.h"
+#include "app.h"
+
+static const byte Gentium23x32[] = {
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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 :
+};
+
+// Based on https://github.com/adafruit/Adafruit_SSD1306
+
+#define SSD1306_SETLOWCOLUMN 0x00
+#define SSD1306_SETHIGHCOLUMN 0x10
+#define SSD1306_MEMORYMODE 0x20
+#define SSD1306_SETSTARTLINE 0x40
+#define SSD1306_SETCONTRAST 0x81
+#define SSD1306_CHARGEPUMP 0x8D
+#define SSD1306_SEGREMAP 0xA0
+#define SSD1306_DISPLAYALLON_RESUME 0xA4
+#define SSD1306_DISPLAYALLON 0xA5
+#define SSD1306_NORMALDISPLAY 0xA6
+#define SSD1306_INVERTDISPLAY 0xA7
+#define SSD1306_SETMULTIPLEX 0xA8
+#define SSD1306_DISPLAYOFF 0xAE
+#define SSD1306_DISPLAYON 0xAF
+#define SSD1306_SETSTARTPAGE 0xB0
+#define SSD1306_COMSCANINC 0xC0
+#define SSD1306_COMSCANDEC 0xC8
+#define SSD1306_SETDISPLAYOFFSET 0xD3
+#define SSD1306_SETCOMPINS 0xDA
+#define SSD1306_SETVCOMDETECT 0xDB
+#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
+#define SSD1306_SETPRECHARGE 0xD9
+#define SSD1306_NOP 0xE3
+
+static const byte display_init_cmds[] = {
+ SSD1306_DISPLAYOFF,
+ SSD1306_SETDISPLAYCLOCKDIV, 0x80, // the suggested ratio 0x80
+ SSD1306_SETMULTIPLEX, 0x1F, // ratio 32
+ SSD1306_SETDISPLAYOFFSET,0x0, // no offset
+ SSD1306_SETSTARTLINE | 0x0, // line #0
+ SSD1306_CHARGEPUMP, 0x14, // internal vcc
+ SSD1306_MEMORYMODE, 0x02, // page mode
+ SSD1306_SEGREMAP | 0x0, // column 0 mapped to SEG0
+ SSD1306_COMSCANINC, // column scan direction not reversed
+ SSD1306_SETCOMPINS, 0x02, // sequential COM pins, disable remap
+ SSD1306_SETCONTRAST, 0x7F, // contrast level 127
+ SSD1306_SETPRECHARGE, 0xF1, // pre-charge period (1, 15)
+ SSD1306_SETVCOMDETECT, 0x40, // vcomh regulator level-
+ // SSD1306_DISPLAYALLON_RESUME,
+ SSD1306_DISPLAYALLON,
+ SSD1306_NORMALDISPLAY,
+ SSD1306_DISPLAYON,
+};
+
+static void display_send_byte(byte d)
+{
+ while (!LL_I2C_IsActiveFlag_TXE(I2C2))
+ ;
+ LL_I2C_TransmitData8(I2C2, d);
+}
+
+static void display_cmd(byte cmd)
+{
+ LL_I2C_GenerateStartCondition(I2C2);
+ while (!LL_I2C_IsActiveFlag_SB(I2C2))
+ ;
+ LL_I2C_TransmitData8(I2C2, 0x78); // Address
+ while (!LL_I2C_IsActiveFlag_ADDR(I2C2))
+ ;
+ LL_I2C_ClearFlag_ADDR(I2C2);
+ display_send_byte(0x00); // Will send a command
+ display_send_byte(cmd);
+ LL_I2C_GenerateStopCondition(I2C2);
+}
+
+static void display_data_start(void)
+{
+ LL_I2C_GenerateStartCondition(I2C2);
+ while (!LL_I2C_IsActiveFlag_SB(I2C2))
+ ;
+ LL_I2C_TransmitData8(I2C2, 0x78); // Address
+ while (!LL_I2C_IsActiveFlag_ADDR(I2C2))
+ ;
+ LL_I2C_ClearFlag_ADDR(I2C2);
+ display_send_byte(0x40); // Will send data
+}
+
+static void display_data(byte d)
+{
+ display_send_byte(d);
+}
+
+static void display_data_end(void)
+{
+ LL_I2C_GenerateStopCondition(I2C2);
+}
+
+void display_init(void)
+{
+ for (uint i=0; i < sizeof(display_init_cmds); i++)
+ display_cmd(display_init_cmds[i]);
+
+ for (uint p=0; p<4; p++)
+ {
+ display_cmd(SSD1306_SETSTARTPAGE + p);
+ display_cmd(SSD1306_SETHIGHCOLUMN);
+ display_cmd(SSD1306_SETLOWCOLUMN);
+ display_data_start();
+ for (uint i=0; i<128; i++)
+ {
+ byte x = 0xaa;
+ // x = Gentium23x32[(23*4+1)*(i/23) + 1 + 4*(i%23) + p];
+ display_data(x);
+ }
+ display_data_end();
+ }
+}
+
+void display_counter(uint cnt)
+{
+#if 0
+ byte d[5];
+ for (uint i=0; i<5; i++)
+ {
+ d[4-i] = cnt % 10;
+ cnt /= 10;
+ }
+
+ for (uint p=0; p<4; p++)
+ {
+ display_cmd(SSD1306_SETSTARTPAGE + p);
+ display_cmd(SSD1306_SETHIGHCOLUMN);
+ display_cmd(SSD1306_SETLOWCOLUMN);
+ display_data_start();
+ for (uint i=0; i<5; i++)
+ {
+ for (uint j=0; j<23; j++)
+ {
+ byte x = Gentium23x32[(23*4+1)*d[i] + 1 + 4*j + p];
+ display_data(x);
+ }
+ display_data(0);
+ display_data(0);
+ display_data(0);
+ }
+ display_data_end();
+ }
+#endif
+}