ws2812_vendor.c (11052B)
1 // Copyright 2022 Stefan Kerkmann (@KarlK90) 2 // SPDX-License-Identifier: GPL-2.0-or-later 3 4 #include "ws2812.h" 5 6 // Keep this exact include order otherwise we run into naming conflicts between 7 // pico-sdk and rp2040.h which we don't control. 8 #include "hardware/timer.h" 9 #include "hardware/clocks.h" 10 #include <hal.h> 11 #include "hardware/pio.h" 12 13 #include "gpio.h" 14 #include "debug.h" 15 #include "wait.h" 16 #include "util.h" 17 18 #if !defined(MCU_RP) 19 # error PIO Driver is only available for Raspberry Pi 2040 MCUs! 20 #endif 21 22 #if defined(WS2812_PIO_USE_PIO1) 23 static const PIO pio = pio1; 24 #else 25 static const PIO pio = pio0; 26 #endif 27 28 #if !defined(RP_DMA_PRIORITY_WS2812) 29 # define RP_DMA_PRIORITY_WS2812 3 30 #endif 31 32 #if defined(WS2812_EXTERNAL_PULLUP) 33 # pragma message "The GPIOs of the RP2040 are NOT 5V tolerant! Make sure to NOT apply any voltage over 3.3V to the RGB data pin." 34 #endif 35 36 /*================== WS2812 PIO TIMINGS =================*/ 37 38 // WS2812_T1L rounded to 50ns intervals and split into two wait timings 39 #define PIO_T1L (WS2812_T1L / 50) 40 #define PIO_T1L_A (MAX(CEILING(PIO_T1L, 2) - 1, 0)) 41 #define PIO_T1L_B (MAX(PIO_T1L / 2 - 1, 0)) 42 43 // WS2812_T0L rounded to 50ns intervals 44 #define PIO_T0L (MAX(WS2812_T0L / 50 - PIO_T1L, 0)) 45 #define PIO_T0L_A (MAX(PIO_T0L - 1, 0)) 46 47 // WS2812_T0H rounded to 50ns intervals 48 #define PIO_T0H (WS2812_T0H / 50) 49 #define PIO_T0H_A MAX(PIO_T0H - 1, 0) 50 51 // WS2812_T1H rounded to 50ns intervals and split into two wait timings 52 #define PIO_T1H (MAX(WS2812_T1H / 50 - PIO_T0H, 0)) 53 #define PIO_T1H_A (MAX((CEILING(PIO_T1H, 2) - 1), 0)) 54 #define PIO_T1H_B (MAX((PIO_T1H / 2) - 1, 0)) 55 56 #if (WS2812_T0L % 50) != 0 57 # pragma message "WS2812_T0L is not given in an 50ns interval, it will be rounded to the next 50ns" 58 #endif 59 60 #if (WS2812_T0H % 50) != 0 61 # pragma message "WS2812_T0H is not given in an 50ns interval, it will be rounded to the next 50ns" 62 #endif 63 64 #if (WS2812_T1L % 50) != 0 65 # pragma message "WS2812_T0L is not given in an 50ns interval, it will be rounded to the next 50ns" 66 #endif 67 68 #if (WS2812_T1H % 50) != 0 69 # pragma message "WS2812_T0H is not given in an 50ns interval, it will be rounded to the next 50ns" 70 #endif 71 72 #if WS2812_T0L < WS2812_T1L 73 # error WS2812_T0L is shorter than WS2812_T1L, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 74 #endif 75 76 #if WS2812_T1H < WS2812_T0H 77 # error WS2812_T1H is shorter than WS2812_T0H, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 78 #endif 79 80 #if WS2812_T0L > (850 + WS2812_T1L) 81 # error WS2812_T0L is longer than 850ns + WS2812_T1L, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 82 #endif 83 84 #if WS2812_T0H > 850 85 # error WS2812_T0H is longer than 850ns, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 86 #endif 87 88 #if WS2812_T1H > (1700 + WS2812_T0H) 89 # error WS2812_T1H is longer than 1700ns + WS2812_T0H, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 90 #endif 91 92 #if WS2812_T1L > 1700 93 # error WS2812_T1L is longer than 1700ns, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 94 #endif 95 96 #if WS2812_T0L < (50 + WS2812_T1L) 97 # error WS2812_T0L is shorter than 50ns + WS2812_T1L, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 98 #endif 99 100 #if WS2812_T0H < 50 101 # error WS2812_T0H is shorter than 50ns, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 102 #endif 103 104 #if WS2812_T1H < (100 + WS2812_T0H) 105 # error WS2812_T1H is longer than 100ns + WS2812_T0H, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 106 #endif 107 108 #if WS2812_T1L < 100 109 # error WS2812_T1L is longer than 1700ns, this is impossible to express in the RP2040 PIO driver. Please correct your timings. 110 #endif 111 112 /** 113 * @brief Helper macro to binary patch the delay part of an per-compiled PIO 114 * opcode. 115 */ 116 #define PIO_DELAY(delay, opcode) (((delay & 0xF) << 8U) | opcode) 117 118 #define WS2812_WRAP_TARGET 0 119 #define WS2812_WRAP 5 120 121 static const uint16_t ws2812_program_instructions[] = { 122 // .wrap_target 123 PIO_DELAY(PIO_T1L_A, 0x6021), // 0: out x, 1 side 0 // T1L (max. 1700ns) 124 PIO_DELAY(PIO_T1L_B, 0xa042), // 1: nop side 0 // T1L 125 PIO_DELAY(PIO_T0H_A, 0x1025), // 2: jmp !x, 5 side 1 // T0H (max. 850ns) 126 PIO_DELAY(PIO_T1H_A, 0xb042), // 3: nop side 1 // T1H (max. 1700ns + T0H) 127 PIO_DELAY(PIO_T1H_B, 0x1000), // 4: jmp 0 side 1 // T1H 128 PIO_DELAY(PIO_T0L_A, 0xa042), // 5: nop side 0 // T0L (max. 850ns + T1L) 129 // .wrap 130 }; 131 132 static const pio_program_t ws2812_program = { 133 .instructions = ws2812_program_instructions, 134 .length = ARRAY_SIZE(ws2812_program_instructions), 135 .origin = -1, 136 }; 137 138 static uint32_t WS2812_BUFFER[WS2812_LED_COUNT]; 139 static const rp_dma_channel_t* dma_channel; 140 static uint32_t RP_DMA_MODE_WS2812; 141 static int STATE_MACHINE = -1; 142 143 static SEMAPHORE_DECL(TRANSFER_COUNTER, 1); 144 static absolute_time_t LAST_TRANSFER; 145 146 /** 147 * @brief Convert RGBW value into WS2812 compatible 32-bit data word. 148 */ 149 __always_inline static uint32_t rgbw8888_to_u32(uint8_t red, uint8_t green, uint8_t blue, uint8_t white) { 150 #if (WS2812_BYTE_ORDER == WS2812_BYTE_ORDER_GRB) 151 return ((uint32_t)green << 24) | ((uint32_t)red << 16) | ((uint32_t)blue << 8) | ((uint32_t)white); 152 #elif (WS2812_BYTE_ORDER == WS2812_BYTE_ORDER_RGB) 153 return ((uint32_t)red << 24) | ((uint32_t)green << 16) | ((uint32_t)blue << 8) | ((uint32_t)white); 154 #elif (WS2812_BYTE_ORDER == WS2812_BYTE_ORDER_BGR) 155 return ((uint32_t)blue << 24) | ((uint32_t)green << 16) | ((uint32_t)red << 8) | ((uint32_t)white); 156 #endif 157 } 158 159 static void ws2812_dma_callback(void* p, uint32_t ct) { 160 // We assume that there is at least one frame left in the OSR even if the TX 161 // FIFO is already empty. 162 rtcnt_t time_to_completion = (pio_sm_get_tx_fifo_level(pio, STATE_MACHINE) + 1) * MAX(WS2812_T1H + WS2812_T1L, WS2812_T0H + WS2812_T0L); 163 164 #if defined(WS2812_RGBW) 165 time_to_completion *= 32; 166 #else 167 time_to_completion *= 24; 168 #endif 169 170 // Convert from ns to us 171 time_to_completion /= 1000; 172 173 update_us_since_boot(&LAST_TRANSFER, time_us_64() + time_to_completion + WS2812_TRST_US); 174 175 osalSysLockFromISR(); 176 chSemSignalI(&TRANSFER_COUNTER); 177 osalSysUnlockFromISR(); 178 } 179 180 void ws2812_init(void) { 181 uint pio_idx = pio_get_index(pio); 182 /* Get PIOx peripheral out of reset state. */ 183 hal_lld_peripheral_unreset(pio_idx == 0 ? RESETS_ALLREG_PIO0 : RESETS_ALLREG_PIO1); 184 185 // clang-format off 186 iomode_t rgb_pin_mode = PAL_RP_PAD_SLEWFAST | 187 PAL_RP_GPIO_OE | 188 #if defined(WS2812_EXTERNAL_PULLUP) 189 PAL_RP_IOCTRL_OEOVER_DRVINVPERI | 190 #endif 191 (pio_idx == 0 ? PAL_MODE_ALTERNATE_PIO0 : PAL_MODE_ALTERNATE_PIO1); 192 // clang-format on 193 194 palSetLineMode(WS2812_DI_PIN, rgb_pin_mode); 195 196 STATE_MACHINE = pio_claim_unused_sm(pio, true); 197 if (STATE_MACHINE < 0) { 198 dprintln("ERROR: Failed to acquire state machine for WS2812 output!"); 199 return; 200 } 201 202 uint offset = pio_add_program(pio, &ws2812_program); 203 204 pio_sm_set_consecutive_pindirs(pio, STATE_MACHINE, WS2812_DI_PIN, 1, true); 205 206 pio_sm_config config = pio_get_default_sm_config(); 207 sm_config_set_wrap(&config, offset + WS2812_WRAP_TARGET, offset + WS2812_WRAP); 208 sm_config_set_sideset_pins(&config, WS2812_DI_PIN); 209 sm_config_set_fifo_join(&config, PIO_FIFO_JOIN_TX); 210 211 #if defined(WS2812_EXTERNAL_PULLUP) 212 /* Instruct side-set to change the pin-directions instead of outputting 213 * a logic level. We generate our levels the following way: 214 * 215 * 1: Set RGB data pin to high impedance input and let the pull-up drive the 216 * signal high. 217 * 218 * 0: Set RGB data pin to low impedance output and drive the pin low. 219 */ 220 sm_config_set_sideset(&config, 1, false, true); 221 #else 222 sm_config_set_sideset(&config, 1, false, false); 223 #endif 224 225 #if defined(WS2812_RGBW) 226 sm_config_set_out_shift(&config, false, true, 32); 227 #else 228 sm_config_set_out_shift(&config, false, true, 24); 229 #endif 230 231 // Every instruction takes 50ns to execute with a clock speed of 20 MHz, 232 // giving the WS2812 PIO driver its time resolution 233 float div = clock_get_hz(clk_sys) / (20.0f * MHZ); 234 sm_config_set_clkdiv(&config, div); 235 236 pio_sm_init(pio, STATE_MACHINE, offset, &config); 237 pio_sm_set_enabled(pio, STATE_MACHINE, true); 238 239 dma_channel = dmaChannelAlloc(RP_DMA_CHANNEL_ID_ANY, RP_DMA_PRIORITY_WS2812, (rp_dmaisr_t)ws2812_dma_callback, NULL); 240 dmaChannelEnableInterruptX(dma_channel); 241 dmaChannelSetDestinationX(dma_channel, (uint32_t)&pio->txf[STATE_MACHINE]); 242 243 // clang-format off 244 RP_DMA_MODE_WS2812 = DMA_CTRL_TRIG_INCR_READ | 245 DMA_CTRL_TRIG_DATA_SIZE_WORD | 246 DMA_CTRL_TRIG_TREQ_SEL(pio == pio0 ? STATE_MACHINE : STATE_MACHINE + 8) | 247 DMA_CTRL_TRIG_PRIORITY(RP_DMA_PRIORITY_WS2812); 248 // clang-format on 249 } 250 251 static inline void sync_ws2812_transfer(void) { 252 if (chSemWaitTimeout(&TRANSFER_COUNTER, TIME_MS2I(WS2812_LED_COUNT)) == MSG_TIMEOUT) { 253 // Abort the synchronization if we have to wait longer than the total 254 // count of LEDs in milliseconds. This is safely much longer than it 255 // would take to push all the data out. 256 dprintln("ERROR: WS2812 DMA transfer has stalled, aborting!"); 257 dmaChannelDisableX(dma_channel); 258 pio_sm_clear_fifos(pio, STATE_MACHINE); 259 pio_sm_restart(pio, STATE_MACHINE); 260 chSemReset(&TRANSFER_COUNTER, 0); 261 wait_us(WS2812_TRST_US); 262 return; 263 } 264 265 // Busy wait until last transfer has finished 266 busy_wait_until(LAST_TRANSFER); 267 } 268 269 ws2812_led_t ws2812_leds[WS2812_LED_COUNT]; 270 271 void ws2812_set_color(int index, uint8_t red, uint8_t green, uint8_t blue) { 272 ws2812_leds[index].r = red; 273 ws2812_leds[index].g = green; 274 ws2812_leds[index].b = blue; 275 #if defined(WS2812_RGBW) 276 ws2812_rgb_to_rgbw(&ws2812_leds[index]); 277 #endif 278 } 279 280 void ws2812_set_color_all(uint8_t red, uint8_t green, uint8_t blue) { 281 for (int i = 0; i < WS2812_LED_COUNT; i++) { 282 ws2812_set_color(i, red, green, blue); 283 } 284 } 285 286 void ws2812_flush(void) { 287 sync_ws2812_transfer(); 288 289 for (int i = 0; i < WS2812_LED_COUNT; i++) { 290 #if defined(WS2812_RGBW) 291 WS2812_BUFFER[i] = rgbw8888_to_u32(ws2812_leds[i].r, ws2812_leds[i].g, ws2812_leds[i].b, ws2812_leds[i].w); 292 #else 293 WS2812_BUFFER[i] = rgbw8888_to_u32(ws2812_leds[i].r, ws2812_leds[i].g, ws2812_leds[i].b, 0); 294 #endif 295 } 296 297 dmaChannelSetSourceX(dma_channel, (uint32_t)WS2812_BUFFER); 298 dmaChannelSetCounterX(dma_channel, WS2812_LED_COUNT); 299 dmaChannelSetModeX(dma_channel, RP_DMA_MODE_WS2812); 300 dmaChannelEnableX(dma_channel); 301 }