1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
|
#include "ws2812b.h"
#include <string.h>
#include "byte_math.h"
#include "spi.h"
#include "sysled.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_GRB) {
uint8_t tmp = color.r;
color.r = color.g;
color.g = tmp;
}
color.r = byte_scale(color.r, 255 - color.a);
color.g = byte_scale(color.g, 255 - color.a);
color.b = byte_scale(color.b, 255 - color.a);
out->first_bits =
BYTE_1(COLOR_BYTE_1(color.r)) | BYTE_2(COLOR_BYTE_2(color.r)) |
BYTE_3(COLOR_BYTE_3(color.r)) | 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.b)) | BYTE_4(COLOR_BYTE_2(color.b));
out->last_bits = COLOR_BYTE_3(color.b);
}
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)) {
set_sysled(
1); // Little indication to tell if we are waiting for the DMA.
dma_loc = *dma_now;
}
set_sysled(0);
}
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;
}
rgb_t blend(rgb_t c1, rgb_t c2)
{
rgb_t r;
uint8_t prop = 255 - c1.a;
uint8_t rprop = 255 - prop;
r.r = clip((int)byte_scale(c1.r, prop) + byte_scale(c2.r, rprop));
r.g = clip((int)byte_scale(c1.g, prop) + byte_scale(c2.g, rprop));
r.b = clip((int)byte_scale(c1.b, prop) + byte_scale(c2.b, rprop));
r.a = 255 - (prop + byte_scale(255 - c2.a, rprop));
return r;
}
rgb_t mat_mul(rgb_t rgb, const rgb_mat_t* mat)
{
rgb_t ret;
ret.r = byte_scale(rgb.r, mat->mat[0][0]) +
byte_scale(rgb.g, mat->mat[0][1]) +
byte_scale(rgb.b, mat->mat[0][2]) +
byte_scale(rgb.a, mat->mat[0][3]);
ret.g = byte_scale(rgb.r, mat->mat[1][0]) +
byte_scale(rgb.g, mat->mat[1][1]) +
byte_scale(rgb.b, mat->mat[1][2]) +
byte_scale(rgb.a, mat->mat[1][3]);
ret.b = byte_scale(rgb.r, mat->mat[2][0]) +
byte_scale(rgb.g, mat->mat[2][1]) +
byte_scale(rgb.b, mat->mat[2][2]) +
byte_scale(rgb.a, mat->mat[2][3]);
ret.a = byte_scale(rgb.r, mat->mat[3][0]) +
byte_scale(rgb.g, mat->mat[3][1]) +
byte_scale(rgb.b, mat->mat[3][2]) +
byte_scale(rgb.a, mat->mat[3][3]);
return ret;
}
|
