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