1 files changed, 75 insertions, 0 deletions
diff --git a/src/ssd1306.c b/src/ssd1306.c
new file mode 100644
index 0000000..c766d5f
--- /dev/null
+++ b/src/ssd1306.c
@@ -0,0 +1,75 @@
+#include "ssd1306.h"
+
+#include <hardware/gpio.h>
+#include <pico/error.h>
+
+int ssd_init(ssd1306_t *display, i2c_inst_t *i2c, uint8_t addr) {
+  display->i2c = i2c;
+  display->addr = addr;
+  display->len = 0;
+
+  return PICO_OK;
+}
+
+int ssd_push(ssd1306_t *display, uint8_t byte) {
+  if (display->len < SSD1306_BUFFER_LEN) {
+    display->buf[display->len] = byte;
+    display->len++;
+    return PICO_OK;
+  } else {
+    return PICO_ERROR_BUFFER_TOO_SMALL;
+  }
+}
+
+int ssd_send(ssd1306_t *display) {
+  int ret = i2c_write_blocking(
+    display->i2c,
+    display->addr,
+    display->buf,
+    display->len,
+    false
+  );
+  display->len = 0;
+
+  return ret;
+}
+
+int ssd_power_on(ssd1306_t *display) {
+  ssd_push(display, SSD1306_CTRL_CMD);
+  
+  ssd_push(display, SSD1306_DISPLAY_OFF);
+  
+  ssd_push(display, SSD1306_SET_CLOCK_DIV);
+  ssd_push(display, 0x80);
+
+  ssd_push(display, SSD1306_SET_MULTIPLEX);
+  ssd_push(display, 0x1f);
+
+  ssd_push(display, SSD1306_START_LINE(0));
+
+  ssd_push(display, SSD1306_SET_CHARGE_PUMP);
+  ssd_push(display, SSD1306_CHARGE_PUMP_ON);
+
+  ssd_push(display, SSD1306_SET_MEMORY_MODE);
+  ssd_push(display, SSD1306_MEM_MODE_HORIZONTAL);
+
+  ssd_push(display, SSD1306_SEG_REMAP_COL127);
+  ssd_push(display, SSD1306_COM_SCAN_REMAP);
+  ssd_push(display, SSD1306_COM_PINS_SEQUENTIAL);
+
+  ssd_push(display, SSD1306_SET_CONTRAST);
+  ssd_push(display, 0xcf);
+
+  ssd_push(display, SSD1306_SET_PRECHARGE);
+  ssd_push(display, 0xf1);
+
+  ssd_push(display, SSD1306_SET_VCOMH);
+  ssd_push(display, SSD1306_VCOMH_0_77);
+
+  ssd_push(display, SSD1306_ENTIRE_DISPLAY_RESUME);
+  ssd_push(display, SSD1306_NORMAL_DISPLAY);
+  ssd_push(display, SSD1306_SCROLL_DEACTIVATE);
+  ssd_push(display, SSD1306_DISPLAY_ON);
+
+  return ssd_send(display);
+}