diff --git a/src/platform/PICO/bus_spi_pico.c b/src/platform/PICO/bus_spi_pico.c
index c560c2cc45..5f86ba2394 100644
--- a/src/platform/PICO/bus_spi_pico.c
+++ b/src/platform/PICO/bus_spi_pico.c
@@ -33,19 +33,114 @@
#include "drivers/bus_spi_impl.h"
#include "drivers/exti.h"
#include "drivers/io.h"
+#include "drivers/io_def.h"
+#include "drivers/io_impl.h"
#include "hardware/spi.h"
#include "hardware/gpio.h"
+#include "hardware/clocks.h"
+#include "hardware/dma.h"
+
+#include "pg/bus_spi.h"
+
+#define SPI_SPEED_20MHZ 2000000
+
+const spiHardware_t spiHardware[] = {
+#ifdef RP2350B
+ {
+ .device = SPIDEV_0,
+ .reg = SPI0,
+ .sckPins = {
+ { DEFIO_TAG_E(P6) },
+ { DEFIO_TAG_E(P18) },
+ { DEFIO_TAG_E(P22) },
+ },
+ .misoPins = {
+ { DEFIO_TAG_E(P0) },
+ { DEFIO_TAG_E(P4) },
+ { DEFIO_TAG_E(P16) },
+ { DEFIO_TAG_E(P20) },
+ },
+ .mosiPins = {
+ { DEFIO_TAG_E(P3) },
+ { DEFIO_TAG_E(P7) },
+ { DEFIO_TAG_E(P19) },
+ { DEFIO_TAG_E(P23) },
+ },
+ },
+ {
+ .device = SPIDEV_1,
+ .reg = SPI1,
+ .sckPins = {
+ { DEFIO_TAG_E(P10) },
+ { DEFIO_TAG_E(P14) },
+ },
+ .misoPins = {
+ { DEFIO_TAG_E(P12) },
+ { DEFIO_TAG_E(P28) },
+ },
+ .mosiPins = {
+ { DEFIO_TAG_E(P11) },
+ { DEFIO_TAG_E(P15) },
+ },
+ },
+#endif
+};
void spiPinConfigure(const struct spiPinConfig_s *pConfig)
{
- UNUSED(pConfig);
+ for (size_t hwindex = 0 ; hwindex < ARRAYLEN(spiHardware) ; hwindex++) {
+ const spiHardware_t *hw = &spiHardware[hwindex];
+
+ if (!hw->reg) {
+ continue;
+ }
+
+ SPIDevice device = hw->device;
+ spiDevice_t *pDev = &spiDevice[device];
+
+ for (int pindex = 0 ; pindex < MAX_SPI_PIN_SEL ; pindex++) {
+ if (pConfig[device].ioTagSck == hw->sckPins[pindex].pin) {
+ pDev->sck = hw->sckPins[pindex].pin;
+ }
+ if (pConfig[device].ioTagMiso == hw->misoPins[pindex].pin) {
+ pDev->miso = hw->misoPins[pindex].pin;
+ }
+ if (pConfig[device].ioTagMosi == hw->mosiPins[pindex].pin) {
+ pDev->mosi = hw->mosiPins[pindex].pin;
+ }
+ }
+
+ if (pDev->sck && pDev->miso && pDev->mosi) {
+ pDev->dev = hw->reg;
+ pDev->leadingEdge = false;
+ }
+ }
+}
+
+static spi_inst_t *getSpiInstanceByDevice(SPI0_Type *spi)
+{
+ if (spi == SPI0) {
+ return spi0;
+ } else if (spi == SPI1) {
+ return spi1;
+ }
+ return NULL;
}
void spiInitDevice(SPIDevice device)
{
- //TODO: implement
- UNUSED(device);
+ const spiDevice_t *spi = &spiDevice[device];
+
+ if (!spi->dev) {
+ return;
+ }
+
+ spi_init(getSpiInstanceByDevice(spi->dev), SPI_SPEED_20MHZ);
+
+ gpio_set_function(IO_PINBYTAG(spi->miso), GPIO_FUNC_SPI);
+ gpio_set_function(IO_PINBYTAG(spi->mosi), GPIO_FUNC_SPI);
+ gpio_set_function(IO_PINBYTAG(spi->sck), GPIO_FUNC_SPI);
}
void spiInternalResetDescriptors(busDevice_t *bus)
@@ -62,15 +157,39 @@ void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
void spiInternalInitStream(const extDevice_t *dev, bool preInit)
{
- //TODO: implement
- UNUSED(dev);
- UNUSED(preInit);
+ int dma_tx = dma_claim_unused_channel(true);
+ int dma_rx = dma_claim_unused_channel(true);
+
+ dev->bus->dmaTx->channel = dma_tx;
+ dev->bus->dmaRx->channel = dma_rx;
+ dev->bus->dmaTx->irqHandlerCallback = NULL;
+ dev->bus->dmaRx->irqHandlerCallback = spiInternalResetStream;
+
+ const spiDevice_t *spi = &spiDevice[spiDeviceByInstance(dev->bus->busType_u.spi.instance)];
+ dma_channel_config config = dma_channel_get_default_config(dma_tx);
+ channel_config_set_transfer_data_size(&config, DMA_SIZE_8);
+ channel_config_set_dreq(&config, spi_get_dreq(spi, true));
+
+ dma_channel_configure(dma_tx, &config, &spi_get_hw(spi)->dr, dev->txBuf, 0, false);
+
+ config = dma_channel_get_default_config(dma_rx);
+ channel_config_set_transfer_data_size(&config, DMA_SIZE_8);
+ channel_config_set_dreq(&config, spi_get_dreq(spi, false));
+
+ dma_channel_configure(dma_rx, &config, dev->rxBuf, &spi_get_hw(spi)->dr, 0, false);
+
+ dev->
}
void spiInternalStartDMA(const extDevice_t *dev)
{
- //TODO: implement
- UNUSED(dev);
+ const spiDevice_t *spi = &spiDevice[spiDeviceByInstance(dev->bus->busType_u.spi.instance)];
+
+ dma_channel_set_trans_count(dev->bus->dmaTx, dev->bus->curSegment->len + 1, false);
+ dma_channel_set_trans_count(dev->bus->dmaRx, dev->bus->curSegment->len + 1, false);
+
+ dma_channel_start(dev->bus->dmaTx);
+ dma_channel_start(dev->bus->dmaRx);
}
void spiInternalStopDMA (const extDevice_t *dev)
@@ -88,7 +207,13 @@ void spiSequenceStart(const extDevice_t *dev)
uint16_t spiCalculateDivider(uint32_t freq)
{
- UNUSED(freq);
- return 0;
+ /*
+ Divider is probably not needed for the PICO as the baud rate on the
+ SPI bus can be set directly.
+
+ In anycase max SPI clock is half the system clock frequency.
+ Therefore the minimum divider is 2.
+ */
+ return MAX(2, (((clock_get_hz(clk_sys) + (freq / 2)) / freq) + 1) & ~1);
}
#endif
diff --git a/src/platform/PICO/dma_pico.c b/src/platform/PICO/dma_pico.c
new file mode 100644
index 0000000000..1faf3f50b9
--- /dev/null
+++ b/src/platform/PICO/dma_pico.c
@@ -0,0 +1,36 @@
+/*
+ * 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 .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+#include "drivers/dma.h"
+#include "dma_pico.h"
+
+dmaChannelDescriptor_t dmaDescriptors[DMA_HANDLER_END-1] = {
+
+};
+
+void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
+{
+}
diff --git a/src/platform/PICO/dma_pico.h b/src/platform/PICO/dma_pico.h
new file mode 100644
index 0000000000..eede01045f
--- /dev/null
+++ b/src/platform/PICO/dma_pico.h
@@ -0,0 +1,24 @@
+
+
+typedef enum {
+ DMA_NONE = 0,
+ DMA_CH1_HANDLER = 1,
+ DMA_CH2_HANDLER,
+ DMA_CH3_HANDLER,
+ DMA_CH4_HANDLER,
+ DMA_CH5_HANDLER,
+ DMA_CH6_HANDLER,
+ DMA_CH7_HANDLER,
+ DMA_CH8_HANDLER,
+ DMA_CH9_HANDLER,
+ DMA_CH10_HANDLER,
+ DMA_CH11_HANDLER,
+ DMA_CH12_HANDLER,
+#ifdef RP2350
+ DMA_CH13_HANDLER,
+ DMA_CH14_HANDLER,
+ DMA_CH15_HANDLER,
+ DMA_CH16_HANDLER,
+#endif
+ DMA_HANDLER_END
+} dmaIdentifier_e;
diff --git a/src/platform/PICO/platform_mcu.h b/src/platform/PICO/platform_mcu.h
index 20a5cec8cd..5b71ba735b 100644
--- a/src/platform/PICO/platform_mcu.h
+++ b/src/platform/PICO/platform_mcu.h
@@ -101,3 +101,5 @@ extern uint32_t systemUniqueId[3];
#define UART_TX_BUFFER_ATTRIBUTE
#define UART_RX_BUFFER_ATTRIBUTE
+
+#define MAX_SPI_PIN_SEL 4
diff --git a/src/platform/PICO/target/RP2350B/target.h b/src/platform/PICO/target/RP2350B/target.h
index 41c60cc488..9455eb0384 100644
--- a/src/platform/PICO/target/RP2350B/target.h
+++ b/src/platform/PICO/target/RP2350B/target.h
@@ -224,5 +224,3 @@ SPARE3 P47
#define UARTHARDWARE_MAX_PINS 4
#define UART_TRAIT_AF_PORT 1
-
-#define MAX_SPI_PIN_SEL 4