Added some better implementation around the ws2812b driver.

2 files changed, 79 insertions, 91 deletions

diff --git a/src/drv/ws2812B/ws2812b.c b/src/drv/ws2812B/ws2812b.c
index 0fc519a..3cf3570 100644
--- a/src/drv/ws2812B/ws2812b.c
+++ b/src/drv/ws2812B/ws2812b.c
@@ -1,6 +1,5 @@
 #include "drv/ws2812B/ws2812b.h"
 
-#include "arch/stm32l4xxx/peripherals/spi.h"
 #include "kern/dma/dma_manager.h"
 #include "kern/mem.h"
 #include "kern/panic.h"
@@ -31,6 +30,69 @@ uint8_t byte_sin(uint8_t n)
   return sintable[n];
 }
 
+ws2812b_t* ws2812b_new(spi_select_t spi_select, int* ec)
+{
+  spi_opts_t spi_opts = DEFAULT_SPI_OPTS;
+  spi_opts.endianness = ENDIANNESS_BIG;
+  spi_t* spi = reserve_spi(spi_select, &spi_opts, ec);
+
+  if (*ec) {
+    return NULL;
+  }
+
+  ws2812b_t* drv = kalloc(sizeof(ws2812b_t));
+  drv->spi = spi;
+  return drv;
+}
+
+void ws2812b_delete(ws2812b_t* drv)
+{
+  release_spi(drv->spi);
+  kfree(drv);
+}
+
+void ws2812b_latch(ws2812b_t* drv)
+{
+  for (int i = 0; i < 20; ++i) {
+    spi_write_8_sync(drv->spi, 0);
+  }
+}
+
+#undef BIT
+#define BIT(b, n) (!!((b) & (1 << (n))))
+static inline void byte_compile(uint8_t byte, uint8_t compl [3])
+{
+  compl [0] = 0 | 1 << 7 | BIT(byte, 7) << 6 | 0 << 5 | 1 << 4 |
+              BIT(byte, 6) << 3 | 0 << 2 | 1 << 1 | BIT(byte, 5) << 0;
+  compl [1] = 0 | 0 << 7 | 1 << 6 | BIT(byte, 4) << 5 | 0 << 4 | 1 << 3 |
+              BIT(byte, 3) << 2 | 0 << 1 | 1 << 0;
+  compl [2] = 0 | BIT(byte, 2) << 7 | 0 << 6 | 1 << 5 | BIT(byte, 1) << 4 |
+              0 << 3 | 1 << 2 | BIT(byte, 0) << 1 | 0 << 0;
+}
+
+void ws2812b_write_rgb_sync(
+    ws2812b_t* drv, uint8_t red, uint8_t green, uint8_t blue)
+{
+  spi_t* spi = drv->spi;
+
+  uint8_t comp[3];
+
+  byte_compile(green, comp);
+  spi_write_8_sync(spi, comp[0]);
+  spi_write_8_sync(spi, comp[1]);
+  spi_write_8_sync(spi, comp[2]);
+
+  byte_compile(red, comp);
+  spi_write_8_sync(spi, comp[0]);
+  spi_write_8_sync(spi, comp[1]);
+  spi_write_8_sync(spi, comp[2]);
+
+  byte_compile(blue, comp);
+  spi_write_8_sync(spi, comp[0]);
+  spi_write_8_sync(spi, comp[1]);
+  spi_write_8_sync(spi, comp[2]);
+}
+
 uint8_t* ws2812b_compile_rgb(rgb_t* out_, size_t arr_len)
 {
   uint8_t* out = (uint8_t*)out_;
diff --git a/src/kern/main.c b/src/kern/main.c
index aeb884d..48967c7 100644
--- a/src/kern/main.c
+++ b/src/kern/main.c
@@ -1,10 +1,10 @@
 #include "arch.h"
 #include "arch/arm/cortex-m4/mpu.h"
 #include "arch/stm32l4xxx/peripherals/clock.h"
-#include "arch/stm32l4xxx/peripherals/spi.h"
 #include "arch/stm32l4xxx/peripherals/dma.h"
 #include "arch/stm32l4xxx/peripherals/irq.h"
 #include "arch/stm32l4xxx/peripherals/rcc.h"
+#include "arch/stm32l4xxx/peripherals/spi.h"
 #include "arch/stm32l4xxx/peripherals/system.h"
 #include "drv/ws2812B/ws2812b.h"
 #include "kern/delay.h"
@@ -28,7 +28,7 @@ void on_hard_fault()
 
 #ifdef ARCH_STM32L4
 
-spi_t* configure_spi()
+void configure_gpio()
 {
   int ec = 0;
 
@@ -47,17 +47,6 @@ spi_t* configure_spi()
   if (ec) {
     panic("Unable to set pin PA5 (ec=%d)\n", ec);
   }
-
-
-  spi_opts_t opts = DEFAULT_SPI_OPTS;
-  opts.endianness = ENDIANNESS_BIG;
-  spi_t* spi = reserve_spi(SPI_SELECT_SPI1, &opts, &ec);
-
-  if (ec) {
-    panic("Unable to reserve spi bus. (ec=%d)\n", ec);
-  }
-
-  return spi;
 }
 
 static uint8_t* compiled;
@@ -72,69 +61,6 @@ static void on_systick(void* nil)
   ++time;
 }
 
-static void write_rgb(spi_t* spi, uint8_t red, uint8_t green, uint8_t blue)
-{
-#undef BIT
-#define BIT(b, n) (!!((b) & (1 << (n))))
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(green, 7) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(green, 6) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(green, 5) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(green, 4) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(green, 3) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(green, 2) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(green, 1) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(green, 0) << 2) | (0 << 1) | (0 << 0));
-
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(red, 7) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(red, 6) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(red, 5) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(red, 4) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(red, 3) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(red, 2) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(red, 1) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(red, 0) << 2) | (0 << 1) | (0 << 0));
-
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(blue, 7) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(blue, 6) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(blue, 5) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(blue, 4) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(blue, 3) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(blue, 2) << 2) | (0 << 1) | (0 << 0));
-  spi_write_8_sync(
-      spi,
-      (1 << 7) | (BIT(blue, 1) << 6) | (0 << 5) | (0 << 4) | (1 << 3) |
-          (BIT(blue, 0) << 2) | (0 << 1) | (0 << 0));
-}
-
-void latch(spi_t* spi)
-{
-  for (int i = 0; i < 20; ++i) {
-    spi_write_8_sync(spi, 0);
-  }
-}
-
 #define min(a, b) (a) < (b) ? (a) : (b)
 
 static uint8_t amp(uint8_t in)
@@ -161,6 +87,14 @@ int main()
 
   systick_add_callback(on_systick, NULL);
   enable_systick(1000);
+  configure_gpio();
+
+  int ec;
+  ws2812b_t* drv = ws2812b_new(SPI_SELECT_SPI1, &ec);
+
+  if (ec || !drv) {
+    panic("Unable to create WS2812b driver :( (%d)\n", ec);
+  }
 
 #define SIZE 256
   rgb_t rgb[SIZE];
@@ -174,23 +108,15 @@ int main()
   uint32_t green = 0x40;
   uint32_t brightness = 255;
 
-  klogf("Configure Spi\n");
-  spi_t* spi = configure_spi();
-  klogf("Done Configuring Spi\n");
-
   for (int i = 0; i < 100; ++i) {
-    write_rgb(spi, 0, 0, 0);
+    ws2812b_write_rgb_sync(drv, 0, 0, 0);
   }
 
-  klogf("Latch\n");
-  latch(spi);
+  ws2812b_latch(drv);
 
   for (;;) {
     set_gpio_pin_high(sysled);
-
-    klogf("Frame\n");
-
-    latch(spi);
+    ws2812b_latch(drv);
 
     int i;
     for (i = 0; i < SIZE; ++i) {
@@ -201,8 +127,8 @@ int main()
 
       uint32_t white = amp(byte_sin(time / 6310 + i / 4));
 
-      write_rgb(
-          spi,
+      ws2812b_write_rgb_sync(
+          drv,
           bytescale(min(red + white, 255), brightness),
           bytescale(min(green + white, 255), brightness),
           bytescale(white, brightness));
@@ -210,7 +136,7 @@ int main()
 
     set_gpio_pin_low(sysled);
 
-    latch(spi);
+    ws2812b_latch(drv);
   }
 }