2 files changed, 66 insertions, 0 deletions
diff --git a/src/driver/Kconfig b/src/driver/Kconfig
index 0b1b98b..3c96d9e 100644
--- a/src/driver/Kconfig
+++ b/src/driver/Kconfig
@@ -80,6 +80,20 @@ config driver_sharp96
 bool "sharp LS013B4DN 96x96px Transflective LC Display"
 depends on ( arch_msp430fr5969lp || arch_msp430fr5994lp ) && meta_driver_spi
 
+config driver_ssd1306
+bool "SSD1306 I2C OLED display"
+depends on meta_driver_i2c
+
+config driver_ssd1306_width
+int "Display width"
+default 128
+depends on driver_ssd1306
+
+config driver_ssd1306_height
+int "Display height"
+default 64
+depends on driver_ssd1306
+
 config driver_softi2c
 bool "Software I2C"
 #depends on !driver_i2c
diff --git a/src/driver/ssd1306.cc b/src/driver/ssd1306.cc
new file mode 100644
index 0000000..69bd28c
--- /dev/null
+++ b/src/driver/ssd1306.cc
@@ -0,0 +1,52 @@
+/*
+ * Copyright 2021 Daniel Friesel
+ *
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+#include "driver/ssd1306.h"
+#include "driver/i2c.h"
+#include "driver/soft_i2c.h"
+
+void SSD1306::writeCommand(uint8_t command)
+{
+	txbuf[0] = 0x80;
+	txbuf[1] = command;
+	i2c.xmit(address, 2, txbuf, 0, rxbuf);
+}
+
+void SSD1306::init()
+{
+	unsigned char i;
+	for (i = 0; i < sizeof(init1); i++) {
+		writeCommand(init1[i]);
+	}
+	writeCommand(height - 1);
+	for (i = 0; i < sizeof(init2); i++) {
+		writeCommand(init2[i]);
+	}
+	writeCommand(height == 32 ? 0x02 : 0x12);
+	for (i = 0; i < sizeof(init3); i++) {
+		writeCommand(init3[i]);
+	}
+}
+
+void SSD1306::showImage(unsigned char* data, uint16_t length)
+{
+	uint8_t i;
+	uint8_t j;
+	txbuf[0] = 0x40;
+	for (i = 0; i < length / 128; i++) {
+		for (j = 0; j < 128; j++) {
+			txbuf[j+1] = data[(uint16_t)i*128 + j];
+		}
+		i2c.xmit(address, 129, txbuf, 0, rxbuf);
+	}
+	if (length % 128) {
+		for (j = 0; j < length % 128; j++) {
+			txbuf[j+1] = data[(uint16_t)i*128 + j];
+		}
+		i2c.xmit(address, (length % 128) + 1, txbuf, 0, rxbuf);
+	}
+}
+
+SSD1306 ssd1306;