1 files changed, 63 insertions, 1 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_;