PICO: Initial implementation of led strip

src/platform/PICO/light_ws2811strip_pico.c

@@ -0,0 +1,211 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef USE_LED_STRIP
+
+#include "common/color.h"
+
+#include "drivers/dma.h"
+#include "platform/dma.h"
+#include "drivers/io.h"
+#include "drivers/light_ws2811strip.h"
+#include "hardware/clocks.h"
+#include "hardware/pio.h"
+
+#define ws2812_wrap_target 0
+#define ws2812_wrap 3
+#define ws2812_pio_version 0
+
+#define WS2812_T1 3
+#define WS2812_T2 3
+#define WS2812_T3 4
+
+#define WS2812_BYTES_PER_LED 4                                                // Number of bytes per WS2812 LED (RGB)
+#define WS2812_BUFFER_COUNT WS2811_LED_STRIP_LENGTH * WS2812_BYTES_PER_LED    // Size of WS2812 buffer
+
+static IO_t ledStripIO = IO_NONE;
+static ioTag_t ledStripIoTag;
+static ledStripFormatRGB_e ledStripFormat = LED_GRB; // Default format is RGB
+
+// DMA channel
+static uint8_t dma_chan;
+
+// Buffer to hold the LED color data
+static uint8_t led_data[WS2812_BUFFER_COUNT];
+
+static const uint16_t ws2812_program_instructions[] = {
+            //     .wrap_target
+    0x6321, //  0: out    x, 1            side 0 [3]
+    0x1223, //  1: jmp    !x, 3           side 1 [2]
+    0x1200, //  2: jmp    0               side 1 [2]
+    0xa242, //  3: nop                    side 0 [2]
+            //     .wrap
+};
+
+static const struct pio_program ws2812_program = {
+    .instructions = ws2812_program_instructions,
+    .length = 4,
+    .origin = -1,
+    .pio_version = ws2812_pio_version,
+    .used_gpio_ranges = 0x0
+};
+
+static inline pio_sm_config ws2812_program_get_default_config(uint offset)
+{
+    pio_sm_config c = pio_get_default_sm_config();
+    sm_config_set_wrap(&c, offset + ws2812_wrap_target, offset + ws2812_wrap);
+    sm_config_set_sideset(&c, 1, false, false);
+    return c;
+}
+
+static inline void ws2812_program_init(PIO pio, uint sm, uint offset, uint pin, float freq, bool rgbw)
+{
+    pio_gpio_init(pio, pin);
+    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
+    pio_sm_config c = ws2812_program_get_default_config(offset);
+    sm_config_set_sideset_pins(&c, pin);
+    sm_config_set_out_shift(&c, false, true, rgbw ? 32 : 24);
+    sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
+    int cycles_per_bit = WS2812_T1 + WS2812_T2 + WS2812_T3;
+    float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit);
+    sm_config_set_clkdiv(&c, div);
+    pio_sm_init(pio, sm, offset, &c);
+    pio_sm_set_enabled(pio, sm, true);
+}
+
+static FAST_IRQ_HANDLER void ws2811LedStripDmaHandler(dmaChannelDescriptor_t* descriptor)
+{
+    UNUSED(descriptor);
+    ws2811LedDataTransferInProgress = false;
+}
+
+void ws2811LedStripInit(ioTag_t ioTag, ledStripFormatRGB_e ledFormat)
+{
+    ledStripFormat = ledFormat; // Store the format globally
+    memset(led_data, 0, sizeof(led_data));
+    ledStripIoTag = ioTag;
+}
+
+bool ws2811LedStripHardwareInit(void)
+{
+    if (!ledStripIoTag) {
+        return false;
+    }
+
+    ledStripIO = IOGetByTag(ledStripIoTag);
+    if (!IOIsFreeOrPreinit(ledStripIO)) {
+        ledStripIO = IO_NONE; // Reset if IO is not free
+        return false;
+    }
+    IOInit(ledStripIO, OWNER_LED_STRIP, 0);
+
+    // This will find a free pio and state machine for our program and load it for us
+    PIO pio;
+    uint sm;
+    uint offset;
+
+    int pinIndex = DEFIO_TAG_PIN(ledStripIoTag);
+    bool success = pio_claim_free_sm_and_add_program_for_gpio_range(&ws2812_program, &pio, &sm, &offset, pinIndex, 1, true);
+    if (!success) {
+        return false; // No free PIO or state machine available
+    }
+    ws2812_program_init(pio, sm, offset, pinIndex, WS2811_CARRIER_HZ, false);
+
+    // --- DMA Configuration ---
+    const dmaIdentifier_e dma_id = dmaGetFreeIdentifier();
+    if (dma_id == DMA_NONE) {
+        return false; // No free DMA channel available
+    }
+
+    dma_chan = DMA_IDENTIFIER_TO_CHANNEL(dma_id);
+    dma_channel_config c = dma_channel_get_default_config(dma_chan);
+    channel_config_set_transfer_data_size(&c, DMA_SIZE_8);
+    channel_config_set_read_increment(&c, true);
+    channel_config_set_write_increment(&c, false);
+    channel_config_set_dreq(&c, pio_get_dreq(pio, sm, true));
+
+    dma_channel_configure(
+        dma_chan,
+        &c,
+        &pio->txf[sm],       // Write address (PIO TX FIFO)
+        NULL,                // Read address (set later)
+        WS2812_BUFFER_COUNT, // Number of transfers
+        false                // Don't start immediately
+    );
+
+    // --- Interrupt Configuration ---
+    dmaSetHandler(dma_id, ws2811LedStripDmaHandler, 0, 0);
+
+    return true;
+}
+
+void ws2811LedStripStartTransfer(void)
+{
+    if (ledStripIO == IO_NONE) {
+        ws2811LedDataTransferInProgress = false;
+        return; // Not initialized
+    }
+
+    const uint8_t dma_bytes_per_led = ledStripFormat == LED_GRBW ? 4 : 3; // GRBW uses 4 bytes per LED, GRB and RGB use 3 bytes per LED
+    // Set the read address to the led_data buffer
+    dma_channel_set_read_addr(dma_chan, led_data, false);
+    // Start the DMA transfer
+    dma_channel_set_trans_count(dma_chan, WS2811_LED_STRIP_LENGTH * dma_bytes_per_led, true);
+}
+
+void ws2811LedStripUpdateTransferBuffer(rgbColor24bpp_t *color, unsigned ledIndex)
+{
+    if (ledIndex >= WS2811_LED_STRIP_LENGTH) {
+        return; // Index out of bounds
+    }
+
+    switch(ledStripFormat) {
+    case LED_RGB: // WS2811 drivers use RGB format
+        led_data[ledIndex * WS2812_BYTES_PER_LED] = color->rgb.r;
+        led_data[ledIndex * WS2812_BYTES_PER_LED + 1] = color->rgb.g;
+        led_data[ledIndex * WS2812_BYTES_PER_LED + 2] = color->rgb.b;
+        break;
+
+    case LED_GRBW: // SK6812 drivers use this
+        // Reconstruct white channel from RGB, making the intensity a bit nonlinear, but that's fine for this use case
+        {
+            uint8_t white = MIN(MIN(color->rgb.r, color->rgb.g), color->rgb.b);
+            led_data[ledIndex * WS2812_BYTES_PER_LED] = color->rgb.g; // Green
+            led_data[ledIndex * WS2812_BYTES_PER_LED + 1] = color->rgb.r; // Red
+            led_data[ledIndex * WS2812_BYTES_PER_LED + 2] = color->rgb.b; // Blue
+            led_data[ledIndex * WS2812_BYTES_PER_LED + 3] = white; // White
+        }
+        break;
+    case LED_GRB: // WS2812 drivers use GRB format
+    default:
+        led_data[ledIndex * WS2812_BYTES_PER_LED] = color->rgb.g;
+        led_data[ledIndex * WS2812_BYTES_PER_LED + 1] = color->rgb.r;
+        led_data[ledIndex * WS2812_BYTES_PER_LED + 2] = color->rgb.b;
+        break;
+    }
+}
+#endif