1 files changed, 150 insertions, 0 deletions
diff --git a/src/leddrv.c b/src/leddrv.c
new file mode 100644
index 0000000..9eeba05
--- /dev/null
+++ b/src/leddrv.c
@@ -0,0 +1,150 @@
+#include "leddrv.h"
+
+#define LED_DRIVE_STRENTH 0
+#define LED_PINCOUNT (23)
+typedef enum {
+	FLOATING,
+	LOW,
+	HIGH,
+} tristate_t;
+
+typedef struct {
+	uint32_t *port_buf;
+	uint32_t *cfg_buf;
+	uint32_t pin;
+} pinctrl_t;
+
+static void gpio_buf_set(pinctrl_t pinctl, tristate_t state)
+{
+	if (state == FLOATING) {
+		*(pinctl.cfg_buf) &= ~pinctl.pin;
+	} else {
+		if (state == HIGH) 
+			*(pinctl.port_buf) |= pinctl.pin;
+		else 
+			*(pinctl.port_buf) &= ~pinctl.pin;
+
+		*(pinctl.cfg_buf) |= pinctl.pin;
+	}
+}
+
+static void gpio_buf_apply(
+				volatile uint8_t *gpio_base, 
+				uint32_t *port, uint32_t *cfg, 
+				uint32_t *mask)
+{
+#if LED_DRIVE_STRENTH != 0
+	uint32_t *drv = (uint32_t *)(gpio_base + GPIO_PD_DRV);
+	*drv = (*drv & ~*mask) | (*cfg & *mask);
+#endif
+	uint32_t *dir = (uint32_t *)(gpio_base + GPIO_DIR);
+	*dir = (*dir & ~*mask) | (*cfg & *mask);
+
+	uint32_t *out = (uint32_t *)(gpio_base + GPIO_OUT);
+	*out = (*out & ~*mask) | (*port & *mask);
+}
+
+static uint32_t PA_buf;
+static uint32_t PB_buf;
+static uint32_t PAcfg_buf;
+static uint32_t PBcfg_buf;
+static uint32_t PA_mask;
+static uint32_t PB_mask;
+
+#define GPIO_APPLY_ALL() \
+	gpio_buf_apply(BA_PA, &PA_buf, &PAcfg_buf, &PA_mask); \
+	gpio_buf_apply(BA_PB, &PB_buf, &PBcfg_buf, &PB_mask)
+
+#define PINCTRL(x, pin) {	\
+	&P##x##_buf, 		\
+	&P##x##cfg_buf, 	\
+	GPIO_Pin_##pin 			\
+}
+
+static const pinctrl_t led_pins[LED_PINCOUNT] = {
+	PINCTRL(A, 15), // A 
+	PINCTRL(B, 18), // B
+	PINCTRL(B, 0),  // C
+	PINCTRL(B, 7),  // D
+	PINCTRL(A, 12), // E
+	PINCTRL(A, 10), // F
+	PINCTRL(A, 11), // G
+	PINCTRL(B, 9),  // H
+	PINCTRL(B, 8),  // I
+	PINCTRL(B, 15), // J
+	PINCTRL(B, 14), // K
+	PINCTRL(B, 13), // L
+	PINCTRL(B, 12), // M
+	PINCTRL(B, 5),  // N
+	PINCTRL(A, 4),  // O
+	PINCTRL(B, 3),  // P
+	PINCTRL(B, 4),  // Q
+	PINCTRL(B, 2),  // R
+	PINCTRL(B, 1),  // S
+	PINCTRL(B, 23), // T
+	PINCTRL(B, 21), // U
+	PINCTRL(B, 20), // V
+	PINCTRL(B, 19), // W
+};
+
+void led_init()
+{
+	for (int i=0; i<LED_PINCOUNT; i++) {
+		if (led_pins[i].port_buf == &PA_buf)
+			PA_mask |= led_pins[i].pin;
+		else
+			PB_mask |= led_pins[i].pin;
+	}
+}
+
+void leds_releaseall() {
+	for (int i=0; i<LED_PINCOUNT; i++)
+		gpio_buf_set(led_pins[i], FLOATING);
+	GPIO_APPLY_ALL();
+}
+
+static void led_write2dcol_raw(int dcol, uint32_t val)
+{
+	// TODO: assert params
+	gpio_buf_set(led_pins[dcol], HIGH);
+	for (int i=0; i<LED_PINCOUNT; i++) {
+		if (i == dcol) continue;
+		gpio_buf_set(led_pins[i], (val & 0x01) ? LOW : FLOATING); // danger: floating=0 (led off) or low=1 (led on)
+		val >>= 1;
+	}
+	GPIO_APPLY_ALL();
+}
+
+static uint32_t combine_cols(uint16_t col1_val, uint16_t col2_val) 
+{
+	uint32_t dval = 0;
+	dval |= ((col1_val & 0x01) << (LED_ROWS*2));
+	dval |= ((col2_val & 0x01) << (LED_ROWS*2+1));
+	for (int i=0; i<LED_ROWS; i++) {
+		col1_val >>= 1;
+		col2_val >>= 1;
+
+		dval >>= 2;
+		dval |= ((col1_val & 0x01) << (LED_ROWS*2));
+		dval |= ((col2_val & 0x01) << (LED_ROWS*2+1));
+	}
+	return dval;
+}
+
+void led_write2dcol(int dcol, uint16_t col1_val, uint16_t col2_val)
+{
+	led_write2dcol_raw(dcol, combine_cols(col1_val, col2_val));
+}
+
+void led_write2row_raw(int row, int which_half, uint32_t val)
+{
+	row = row*2 + (which_half != 0);
+
+	gpio_buf_set(led_pins[row], LOW);
+	for (int i=0; i<LED_PINCOUNT; i++) {
+		if (i == row) continue;
+		gpio_buf_set(led_pins[i], (val & 0x01) ? HIGH : FLOATING);
+		val >>= 1;
+	}
+	GPIO_APPLY_ALL();
+}