1 files changed, 115 insertions, 0 deletions
diff --git a/src/ws2812b.c b/src/ws2812b.c
new file mode 100644
index 0000000..8547b5d
--- /dev/null
+++ b/src/ws2812b.c
@@ -0,0 +1,115 @@
+#include "ws2812b.h"
+
+#include <string.h>
+
+#include "spi.h"
+
+// either 0b110 when (n) is true, otherwise 0b100
+#define BIT_TO_SPI_DATA(v, n) ((BIT_N(v, n)) ? 6 : 4)
+#define BIT_N(v, n) ((v) & (1 << (n)))
+
+#define BYTE_1(n) ((n) << 0)
+#define BYTE_2(n) ((n) << 8)
+#define BYTE_3(n) ((n) << 16)
+#define BYTE_4(n) ((n) << 24)
+
+#define COLOR_BYTE_1(c)                                          \
+  (BIT_TO_SPI_DATA((c), 7) << 5 | BIT_TO_SPI_DATA((c), 6) << 2 | \
+   BIT_TO_SPI_DATA((c), 5) >> 1)
+
+#define COLOR_BYTE_2(c)                                                \
+  ((BIT_TO_SPI_DATA((c), 5) & 1) << 7 | BIT_TO_SPI_DATA((c), 4) << 4 | \
+   BIT_TO_SPI_DATA((c), 3) << 1 | BIT_TO_SPI_DATA((c), 2) >> 2)
+
+#define COLOR_BYTE_3(c)                                                \
+  ((BIT_TO_SPI_DATA((c), 2) & 3) << 6 | BIT_TO_SPI_DATA((c), 1) << 3 | \
+   BIT_TO_SPI_DATA((c), 0) << 0)
+
+// Clips a pointer the bottom 14 bits for comparison with the DMA.
+#define CLIP_TO_DMA_BITS(ptr) ((uint16_t)(((uint32_t)(ptr)) & 0x3fff))
+
+inline static void complie_color_inline(
+    rgb_t color, struct rgb_compiled* out, enum ws2812b_byte_order byte_order)
+{
+  if (byte_order == BYTE_ORDER_GBR) {
+    uint8_t tmp = color.b;
+    color.b = color.g;
+    color.g = tmp;
+  }
+
+  out->first_bits =
+      BYTE_1(COLOR_BYTE_1(color.b)) | BYTE_2(COLOR_BYTE_2(color.b)) |
+      BYTE_3(COLOR_BYTE_3(color.b)) | BYTE_4(COLOR_BYTE_1(color.g));
+
+  out->second_bits =
+      BYTE_1(COLOR_BYTE_2(color.g)) | BYTE_2(COLOR_BYTE_3(color.g)) |
+      BYTE_3(COLOR_BYTE_1(color.r)) | BYTE_4(COLOR_BYTE_2(color.r));
+
+  out->last_bits = COLOR_BYTE_3(color.r);
+}
+
+void compile_color(
+    rgb_t color, struct rgb_compiled* out, enum ws2812b_byte_order byte_order)
+{
+  complie_color_inline(color, out, byte_order);
+}
+
+int write_rgb(struct ws2812b_buf* out, rgb_t color)
+{
+  struct rgb_compiled comp;
+  complie_color_inline(color, &comp, out->byte_order);
+
+  // Make sure there's enough memory.
+  if (out->total_alloc < out->cur + TOTAL_BYTES_PER_LED) {
+    return 1;
+  }
+
+  // Wait for DMA if it's running.
+  volatile uint16_t* dma_now = out->dma_now_reg;
+
+  // If the dma_now is not set, we won't wait for the DMA.
+  if (dma_now) {
+    uint16_t dma_loc = *dma_now;
+    uint16_t dma_end = *out->dma_end_reg;
+    while ((dma_loc - CLIP_TO_DMA_BITS(out->buf + out->cur) <
+            TOTAL_BYTES_PER_LED) &&
+           (dma_loc < dma_end)) {
+      dma_loc = *dma_now;
+    }
+  }
+
+  memcpy(out->buf + out->cur, comp.buf, WIRE_BYTES_PER_COLOR);
+  memset(out->buf + out->cur + WIRE_BYTES_PER_COLOR, 0, PADDING_BYTES);
+
+  out->cur += TOTAL_BYTES_PER_LED;
+
+  return 0;
+}
+
+void start_dma(struct ws2812b_buf* buf)
+{
+  buf->dma_end_reg = &SPI0->dma_end;
+  buf->dma_now_reg = &SPI0->dma_now;
+
+  dma_transfer(buf->buf, buf->cur);
+}
+
+void make_wsb2812b(struct ws2812b_buf* out, void* buf, size_t n_alloc)
+{
+  memset(out, 0, sizeof(*out));
+  out->buf = buf;
+  out->total_alloc = n_alloc;
+}
+
+void compiled_color_dbg_str(struct rgb_compiled* c, char* out)
+{
+  uint8_t cur = 0;
+  for (int i = 0; i < 9; ++i) {
+    cur = c->buf[i];
+    for (int j = 0; j < 8; ++j) {
+      *(out++) = (cur & 0x80) ? '1' : '0';
+      cur <<= 1;
+    }
+  }
+  *(out++) = 0;
+}