diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index f10e919d72..a83e3ef793 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -30,7 +30,7 @@ typedef enum { BUSTYPE_I2C, BUSTYPE_SPI, BUSTYPE_MPU_SLAVE, // Slave I2C on SPI master - BUSTYPE_GYRO_AUTO // Only used by acc/gyro bus auto detection code + BUSTYPE_GYRO_AUTO, // Only used by acc/gyro bus auto detection code } busType_e; struct spiDevice_s; diff --git a/src/main/drivers/bus_quadspi.c b/src/main/drivers/bus_quadspi.c new file mode 100644 index 0000000000..1ebc4903d5 --- /dev/null +++ b/src/main/drivers/bus_quadspi.c @@ -0,0 +1,273 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * + * Author: Dominic Clifton + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_QUADSPI + +#include "drivers/bus_quadspi.h" +#include "drivers/bus_quadspi_impl.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/rcc.h" + +#include "pg/bus_quadspi.h" + +quadSpiDevice_t quadSpiDevice[QUADSPIDEV_COUNT] = { 0 }; + +QUADSPIDevice quadSpiDeviceByInstance(QUADSPI_TypeDef *instance) +{ +#ifdef USE_QUADSPI_DEVICE_1 + if (instance == QUADSPI) { + return QUADSPIDEV_1; + } +#endif + + return QUADSPIINVALID; +} + +QUADSPI_TypeDef *quadSpiInstanceByDevice(QUADSPIDevice device) +{ + if (device == QUADSPIINVALID || device >= QUADSPIDEV_COUNT) { + return NULL; + } + + return quadSpiDevice[device].dev; +} + +bool quadSpiInit(QUADSPIDevice device) +{ + switch (device) { + case QUADSPIINVALID: + return false; + case QUADSPIDEV_1: +#ifdef USE_QUADSPI_DEVICE_1 + quadSpiInitDevice(device); + return true; +#else + break; +#endif + } + return false; +} + +uint32_t quadSpiTimeoutUserCallback(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if (device == QUADSPIINVALID) { + return -1; + } + quadSpiDevice[device].errorCount++; + return quadSpiDevice[device].errorCount; +} + +uint16_t quadSpiGetErrorCounter(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if (device == QUADSPIINVALID) { + return 0; + } + return quadSpiDevice[device].errorCount; +} + +void quadSpiResetErrorCounter(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if (device != QUADSPIINVALID) { + quadSpiDevice[device].errorCount = 0; + } +} + +const quadSpiHardware_t quadSpiHardware[] = { +#ifdef STM32H7 + { + .device = QUADSPIDEV_1, + .reg = QUADSPI, + .clkPins = { + { DEFIO_TAG_E(PB2), GPIO_AF9_QUADSPI }, + }, + .bk1IO0Pins = { + { DEFIO_TAG_E(PC9), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PD11), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PF8), GPIO_AF10_QUADSPI }, + }, + .bk1IO1Pins = { + { DEFIO_TAG_E(PC10), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PD12), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PF9), GPIO_AF10_QUADSPI }, + }, + .bk1IO2Pins = { + { DEFIO_TAG_E(PE2), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PF7), GPIO_AF9_QUADSPI }, + }, + .bk1IO3Pins = { + { DEFIO_TAG_E(PA1), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PD13), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PF6), GPIO_AF9_QUADSPI }, + }, + .bk1CSPins = { + { DEFIO_TAG_E(PB6), GPIO_AF10_QUADSPI }, + { DEFIO_TAG_E(PB10), GPIO_AF9_QUADSPI }, + { DEFIO_TAG_E(PG6), GPIO_AF10_QUADSPI }, + }, + .bk2IO0Pins = { + { DEFIO_TAG_E(PE7), GPIO_AF10_QUADSPI }, + //{ DEFIO_TAG_E(PH7), GPIO_AF9_QUADSPI }, // FIXME regenerate io_def_generated with support for GPIO 'H' + }, + .bk2IO1Pins = { + { DEFIO_TAG_E(PE8), GPIO_AF10_QUADSPI }, + //{ DEFIO_TAG_E(PH3), GPIO_AF9_QUADSPI }, // FIXME regenerate io_def_generated with support for GPIO 'H' + }, + .bk2IO2Pins = { + { DEFIO_TAG_E(PE9), GPIO_AF10_QUADSPI }, + { DEFIO_TAG_E(PG9), GPIO_AF9_QUADSPI }, + }, + .bk2IO3Pins = { + { DEFIO_TAG_E(PE10), GPIO_AF10_QUADSPI }, + { DEFIO_TAG_E(PG14), GPIO_AF9_QUADSPI }, + }, + .bk2CSPins = { + { DEFIO_TAG_E(PC11), GPIO_AF9_QUADSPI }, + }, + .rcc = RCC_AHB3(QSPI), + }, +#endif +}; + +void quadSpiPinConfigure(const quadSpiConfig_t *pConfig) +{ + for (size_t hwindex = 0; hwindex < ARRAYLEN(quadSpiHardware); hwindex++) { + const quadSpiHardware_t *hw = &quadSpiHardware[hwindex]; + + if (!hw->reg) { + continue; + } + + QUADSPIDevice device = hw->device; + quadSpiDevice_t *pDev = &quadSpiDevice[device]; + + for (int pindex = 0; pindex < MAX_QUADSPI_PIN_SEL; pindex++) { + if (pConfig[device].ioTagClk == hw->clkPins[pindex].pin) { + pDev->clk = hw->clkPins[pindex].pin; + } + // + // BK1 + // + if (pConfig[device].ioTagBK1IO0 == hw->bk1IO0Pins[pindex].pin) { + pDev->bk1IO0 = hw->bk1IO0Pins[pindex].pin; + pDev->bk1IO0AF = hw->bk1IO0Pins[pindex].af; + } + if (pConfig[device].ioTagBK1IO1 == hw->bk1IO1Pins[pindex].pin) { + pDev->bk1IO1 = hw->bk1IO1Pins[pindex].pin; + pDev->bk1IO1AF = hw->bk1IO1Pins[pindex].af; + } + if (pConfig[device].ioTagBK1IO2 == hw->bk1IO2Pins[pindex].pin) { + pDev->bk1IO2 = hw->bk1IO2Pins[pindex].pin; + pDev->bk1IO2AF = hw->bk1IO2Pins[pindex].af; + } + if (pConfig[device].ioTagBK1IO3 == hw->bk1IO3Pins[pindex].pin) { + pDev->bk1IO3 = hw->bk1IO3Pins[pindex].pin; + pDev->bk1IO3AF = hw->bk1IO3Pins[pindex].af; + } + if (pConfig[device].ioTagBK1CS == hw->bk1CSPins[pindex].pin) { + pDev->bk1CS = hw->bk1CSPins[pindex].pin; + pDev->bk1CSAF = hw->bk1CSPins[pindex].af; + } + // + // BK2 + // + if (pConfig[device].ioTagBK2IO0 == hw->bk2IO0Pins[pindex].pin) { + pDev->bk2IO0 = hw->bk2IO0Pins[pindex].pin; + pDev->bk2IO0AF = hw->bk2IO0Pins[pindex].af; + } + if (pConfig[device].ioTagBK2IO1 == hw->bk2IO1Pins[pindex].pin) { + pDev->bk2IO1 = hw->bk2IO1Pins[pindex].pin; + pDev->bk2IO1AF = hw->bk2IO1Pins[pindex].af; + } + if (pConfig[device].ioTagBK2IO2 == hw->bk2IO2Pins[pindex].pin) { + pDev->bk2IO2 = hw->bk2IO2Pins[pindex].pin; + pDev->bk2IO2AF = hw->bk2IO2Pins[pindex].af; + } + if (pConfig[device].ioTagBK2IO3 == hw->bk2IO3Pins[pindex].pin) { + pDev->bk2IO3 = hw->bk2IO3Pins[pindex].pin; + pDev->bk2IO3AF = hw->bk2IO3Pins[pindex].af; + } + if (pConfig[device].ioTagBK2CS == hw->bk2CSPins[pindex].pin) { + pDev->bk2CS = hw->bk2CSPins[pindex].pin; + pDev->bk2CSAF = hw->bk2CSPins[pindex].af; + } + } + + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + pDev->bk1CS = pConfig[device].ioTagBK1CS; + } + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + pDev->bk2CS = pConfig[device].ioTagBK2CS; + } + + bool haveResources = true; + + // clock pins + + haveResources = haveResources && pDev->clk; + + // data pins + + bool needBK1 = (pConfig[device].mode == QUADSPI_MODE_DUAL_FLASH) || (pConfig[device].mode == QUADSPI_MODE_BK1_ONLY); + if (needBK1) { + bool haveBK1Resources = pDev->bk1IO0 && pDev->bk1IO1 && pDev->bk1IO2 && pDev->bk1IO3 && pDev->bk1CS; + haveResources = haveResources && haveBK1Resources; + } + + bool needBK2 = (pConfig[device].mode == QUADSPI_MODE_DUAL_FLASH) || (pConfig[device].mode == QUADSPI_MODE_BK2_ONLY); + if (needBK2) { + bool haveBK2Resources = pDev->bk2IO0 && pDev->bk2IO1 && pDev->bk2IO2 && pDev->bk2IO3; + haveResources = haveResources && haveBK2Resources; + } + + // cs pins + + if (needBK1) { + haveResources = haveResources && pDev->bk1CS; + } + + bool needBK2CS = + (pConfig[device].mode == QUADSPI_MODE_DUAL_FLASH && (pConfig[device].csFlags & QUADSPI_CS_MODE_MASK) == QUADSPI_CS_MODE_SEPARATE) || + (pConfig[device].mode == QUADSPI_MODE_BK2_ONLY); + + if (needBK2CS) { + haveResources = haveResources && pDev->bk2CS; + } + + + if (haveResources) { + pDev->dev = hw->reg; + pDev->rcc = hw->rcc; + } + } +} + +#endif diff --git a/src/main/drivers/bus_quadspi.h b/src/main/drivers/bus_quadspi.h new file mode 100644 index 0000000000..4e0e03ffe0 --- /dev/null +++ b/src/main/drivers/bus_quadspi.h @@ -0,0 +1,137 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * + * Author: Dominic Clifton + */ + +#pragma once + +#include "drivers/io_types.h" +#include "drivers/rcc_types.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +/* + * Quad SPI supports 1/2/4 wire modes + * + * 1LINE is like SPI MISO/MOSI using D0 (MOSI)/D1(MISO). + * 2LINE uses D0, D1 (bidirectional). + * 4LINE uses D0..D3 (bidirectional) + * + * See ST Micros' AN4760 "Quad-SPI (QSPI) interface on STM32 microcontrollers" + */ + +#ifdef USE_QUADSPI + +#if !defined(STM32H7) +#error Quad SPI unsupported on this MCU +#endif + +#define QUADSPI_IO_AF_BK_IO_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) +#define QUADSPI_IO_AF_CLK_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) +#define QUADSPI_IO_AF_BK_CS_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP) +#define QUADSPI_IO_BK_CS_CFG IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_HIGH, GPIO_PULLUP) + +typedef enum { + QUADSPI_CLOCK_INITIALIZATION = 256, + /* QSPI freq = CLK /(1 + ClockPrescaler) = 200 MHz/(1+x) */ + QUADSPI_CLOCK_INITIALISATION = 255, // 0.78125 Mhz + QUADSPI_CLOCK_SLOW = 19, // 10.00000 Mhz + QUADSPI_CLOCK_STANDARD = 9, // 20.00000 MHz + QUADSPI_CLOCK_FAST = 3, // 50.00000 MHz + QUADSPI_CLOCK_ULTRAFAST = 1 //100.00000 MHz +} QUADSPIClockDivider_e; + +typedef enum QUADSPIDevice { + QUADSPIINVALID = -1, + QUADSPIDEV_1 = 0, +} QUADSPIDevice; + +#define QUADSPIDEV_COUNT 1 + +// Macros to convert between CLI bus number and SPIDevice. +#define QUADSPI_CFG_TO_DEV(x) ((x) - 1) +#define QUADSPI_DEV_TO_CFG(x) ((x) + 1) + +typedef enum { + QUADSPI_MODE_BK1_ONLY = 0, + QUADSPI_MODE_BK2_ONLY, + QUADSPI_MODE_DUAL_FLASH, +} quadSpiMode_e; + +// +// QUADSPI1_CS_FLAGS +// +#define QUADSPI_BK1_CS_MASK ((1 << 1) | (1 << 0)) + +#define QUADSPI_BK1_CS_NONE ((0 << 1) | (0 << 0)) +#define QUADSPI_BK1_CS_HARDWARE ((0 << 1) | (1 << 0)) // pin must support QSPI Alternate function for BK1_NCS +#define QUADSPI_BK1_CS_SOFTWARE ((1 << 1) | (0 << 0)) // use any GPIO pin for BK1 CS + +#define QUADSPI_BK2_CS_MASK ((1 << 3) | (1 << 2)) + +#define QUADSPI_BK2_CS_NONE ((0 << 3) | (0 << 2)) +#define QUADSPI_BK2_CS_HARDWARE ((0 << 3) | (1 << 2)) // pin must support QSPI Alternate function for BK2_NCS +#define QUADSPI_BK2_CS_SOFTWARE ((1 << 3) | (0 << 2)) // use any GPIO pin for BK2 CS + +#define QUADSPI_CS_MODE_MASK (1 << 4) + +#define QUADSPI_CS_MODE_SEPARATE (0 << 4) +#define QUADSPI_CS_MODE_LINKED (1 << 4) + +// H7 QSPI notes: +// Hardware supports BK1 and BK2 connected to both flash chips when using DUAL FLASH mode. +// Hardware does NOT support using BK1_NCS for single flash chip on BK2. +// It's possible to use BK1_NCS for single chip on BK2 using software CS via QUADSPI_BK2_CS_SOFTWARE + + +void quadSpiPreInit(void); + +bool quadSpiInit(QUADSPIDevice device); +void quadSpiSetDivisor(QUADSPI_TypeDef *instance, uint16_t divisor); + +bool quadSpiTransmit1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length); +bool quadSpiReceive1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length); + +bool quadSpiInstructionWithData1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length); + +bool quadSpiReceiveWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length); +bool quadSpiReceiveWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length); +bool quadSpiTransmitWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length); + + +bool quadSpiInstructionWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize); + +//bool quadSpiIsBusBusy(SPI_TypeDef *instance); + +uint16_t quadSpiGetErrorCounter(QUADSPI_TypeDef *instance); +void quadSpiResetErrorCounter(QUADSPI_TypeDef *instance); + +QUADSPIDevice quadSpiDeviceByInstance(QUADSPI_TypeDef *instance); +QUADSPI_TypeDef *quadSpiInstanceByDevice(QUADSPIDevice device); + +// +// Config +// + +struct quadSpiConfig_s; +void quadSpiPinConfigure(const struct quadSpiConfig_s *pConfig); + +#endif diff --git a/src/main/drivers/bus_quadspi_hal.c b/src/main/drivers/bus_quadspi_hal.c new file mode 100644 index 0000000000..e59b6296dd --- /dev/null +++ b/src/main/drivers/bus_quadspi_hal.c @@ -0,0 +1,492 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + * + * Author: Dominic Clifton + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_QUADSPI + +#include "bus_quadspi.h" +#include "bus_quadspi_impl.h" +#include "dma.h" +#include "io.h" +#include "io_impl.h" +#include "nvic.h" +#include "rcc.h" + +#include "pg/bus_quadspi.h" + +static void Error_Handler(void) { while (1) { } } + +void quadSpiInitDevice(QUADSPIDevice device) +{ + quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]); + + // Enable QUADSPI clock + RCC_ClockCmd(quadSpi->rcc, ENABLE); + RCC_ResetCmd(quadSpi->rcc, ENABLE); + + IOInit(IOGetByTag(quadSpi->clk), OWNER_QUADSPI_CLK, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1IO0), OWNER_QUADSPI_BK1IO0, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1IO1), OWNER_QUADSPI_BK1IO1, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1IO2), OWNER_QUADSPI_BK1IO2, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1IO3), OWNER_QUADSPI_BK1IO3, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk1CS), OWNER_QUADSPI_BK1CS, RESOURCE_INDEX(device)); + + IOInit(IOGetByTag(quadSpi->bk2IO0), OWNER_QUADSPI_BK2IO0, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk2IO1), OWNER_QUADSPI_BK2IO1, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk2IO2), OWNER_QUADSPI_BK2IO2, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk2IO3), OWNER_QUADSPI_BK2IO3, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(quadSpi->bk2CS), OWNER_QUADSPI_BK2CS, RESOURCE_INDEX(device)); + +#if defined(STM32H7) + // clock is only on AF9 + // IO and CS lines are on AF9 and AF10 + IOConfigGPIOAF(IOGetByTag(quadSpi->clk), QUADSPI_IO_AF_CLK_CFG, GPIO_AF9_QUADSPI); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO0), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO0AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO1), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO1AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO2), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO2AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO3), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO3AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO0), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO0AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO1), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO1AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO2), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO2AF); + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO3), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO3AF); + + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_HARDWARE) { + IOConfigGPIOAF(IOGetByTag(quadSpi->bk1CS), QUADSPI_IO_AF_BK_CS_CFG, quadSpi->bk1CSAF); + } else { + IOConfigGPIO(IOGetByTag(quadSpi->bk1CS), QUADSPI_IO_BK_CS_CFG); + } + + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_HARDWARE) { + IOConfigGPIOAF(IOGetByTag(quadSpi->bk2CS), QUADSPI_IO_AF_BK_CS_CFG, quadSpi->bk2CSAF); + } else { + IOConfigGPIO(IOGetByTag(quadSpi->bk2CS), QUADSPI_IO_BK_CS_CFG); + } +#endif + + quadSpi->hquadSpi.Instance = quadSpi->dev; + // DeInit QUADSPI hardware + HAL_QSPI_DeInit(&quadSpi->hquadSpi); + + quadSpi->hquadSpi.Init.ClockPrescaler = QUADSPI_CLOCK_INITIALISATION; + quadSpi->hquadSpi.Init.FifoThreshold = 1; + quadSpi->hquadSpi.Init.SampleShifting = QSPI_SAMPLE_SHIFTING_NONE; + quadSpi->hquadSpi.Init.FlashSize = 23; // address bits + 1 + quadSpi->hquadSpi.Init.ChipSelectHighTime = QSPI_CS_HIGH_TIME_1_CYCLE; + quadSpi->hquadSpi.Init.ClockMode = QSPI_CLOCK_MODE_0; + + switch (quadSpiConfig(device)->mode) { + case QUADSPI_MODE_BK1_ONLY: + quadSpi->hquadSpi.Init.FlashID = QSPI_FLASH_ID_1; + quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_DISABLE; + break; + case QUADSPI_MODE_BK2_ONLY: + quadSpi->hquadSpi.Init.FlashID = QSPI_FLASH_ID_2; + quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_DISABLE; + break; + case QUADSPI_MODE_DUAL_FLASH: + quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_ENABLE; + break; + } + + // Init QUADSPI hardware + if (HAL_QSPI_Init(&quadSpi->hquadSpi) != HAL_OK) + { + Error_Handler(); + } +} + +static const uint32_t quadSpi_addressSizeMap[] = { + QSPI_ADDRESS_8_BITS, + QSPI_ADDRESS_16_BITS, + QSPI_ADDRESS_24_BITS, + QSPI_ADDRESS_32_BITS +}; + +static uint32_t quadSpi_addressSizeFromValue(uint8_t addressSize) +{ + return quadSpi_addressSizeMap[((addressSize + 1) / 8) - 1]; // rounds to nearest QSPI_ADDRESS_* value that will hold the address. +} + +/** + * Return true if the bus is currently in the middle of a transmission. + */ +bool quadSpiIsBusBusy(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if(quadSpiDevice[device].hquadSpi.State == HAL_QSPI_STATE_BUSY) + return true; + else + return false; +} + +#define QUADSPI_DEFAULT_TIMEOUT 10 + +void quadSpiSelectDevice(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + + IO_t bk1CS = IOGetByTag(quadSpiDevice[device].bk1CS); + IO_t bk2CS = IOGetByTag(quadSpiDevice[device].bk2CS); + + switch(quadSpiConfig(device)->mode) { + case QUADSPI_MODE_DUAL_FLASH: + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + IOLo(bk1CS); + } + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + IOLo(bk2CS); + } + break; + case QUADSPI_MODE_BK1_ONLY: + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + IOLo(bk1CS); + } + break; + case QUADSPI_MODE_BK2_ONLY: + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + IOLo(bk2CS); + } + break; + } +} + +void quadSpiDeselectDevice(QUADSPI_TypeDef *instance) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + + IO_t bk1CS = IOGetByTag(quadSpiDevice[device].bk1CS); + IO_t bk2CS = IOGetByTag(quadSpiDevice[device].bk2CS); + + switch(quadSpiConfig(device)->mode) { + case QUADSPI_MODE_DUAL_FLASH: + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + IOHi(bk1CS); + } + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + IOHi(bk2CS); + } + break; + case QUADSPI_MODE_BK1_ONLY: + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) { + IOHi(bk1CS); + } + break; + case QUADSPI_MODE_BK2_ONLY: + if ((quadSpiConfig(device)->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) { + IOHi(bk2CS); + } + break; + } +} + + + +bool quadSpiTransmit1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_NONE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_NONE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.NbData = length; + + if (out) { + cmd.DataMode = QSPI_DATA_1_LINE; + } + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + if (out && length == 0) { + status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +bool quadSpiReceive1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_NONE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_1_LINE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT); + + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +bool quadSpiReceiveWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_1_LINE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + + +bool quadSpiReceiveWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_4_LINES; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + if (!timeout) { + status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} +bool quadSpiTransmitWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_1_LINE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + + if (!timeout) { + status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +bool quadSpiInstructionWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_1_LINE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_NONE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.Address = address; + cmd.AddressSize = quadSpi_addressSizeFromValue(addressSize); + cmd.NbData = 0; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout = (status != HAL_OK); + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + + return true; +} + +bool quadSpiInstructionWithData1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + HAL_StatusTypeDef status; + + QSPI_CommandTypeDef cmd; + cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE; + cmd.AddressMode = QSPI_ADDRESS_NONE; + cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + cmd.DataMode = QSPI_DATA_1_LINE; + cmd.DummyCycles = dummyCycles; + cmd.DdrMode = QSPI_DDR_MODE_DISABLE; + cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + cmd.Instruction = instruction; + cmd.NbData = length; + + quadSpiSelectDevice(instance); + + status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT); + bool timeout =(status != HAL_OK); + + if (!timeout) { + status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT); + timeout = (status != HAL_OK); + } + + quadSpiDeselectDevice(instance); + + if (timeout) { + quadSpiTimeoutUserCallback(instance); + return false; + } + + return true; +} + +void quadSpiSetDivisor(QUADSPI_TypeDef *instance, uint16_t divisor) +{ + QUADSPIDevice device = quadSpiDeviceByInstance(instance); + if (HAL_QSPI_DeInit(&quadSpiDevice[device].hquadSpi) != HAL_OK) + { + Error_Handler(); + } + + quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]); + + quadSpi->hquadSpi.Init.ClockPrescaler = divisor; + + HAL_QSPI_Init(&quadSpi->hquadSpi); +} +#endif diff --git a/src/main/drivers/bus_quadspi_impl.h b/src/main/drivers/bus_quadspi_impl.h new file mode 100644 index 0000000000..fb7d4d2c91 --- /dev/null +++ b/src/main/drivers/bus_quadspi_impl.h @@ -0,0 +1,92 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + * + * Author: Dominic Clifton + */ + +#pragma once + + +typedef struct quadSpiPinDef_s { + ioTag_t pin; +#if defined(STM32H7) + uint8_t af; +#endif +} quadSpiPinDef_t; + +#if defined(STM32H7) +#define MAX_QUADSPI_PIN_SEL 3 +#endif + +typedef struct quadSpiHardware_s { + QUADSPIDevice device; + QUADSPI_TypeDef *reg; + quadSpiPinDef_t clkPins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1IO0Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1IO1Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1IO2Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1IO3Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk1CSPins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2IO0Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2IO1Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2IO2Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2IO3Pins[MAX_QUADSPI_PIN_SEL]; + quadSpiPinDef_t bk2CSPins[MAX_QUADSPI_PIN_SEL]; + + rccPeriphTag_t rcc; +} quadSpiHardware_t; + +extern const quadSpiHardware_t quadSpiHardware[]; + +typedef struct QUADSPIDevice_s { + QUADSPI_TypeDef *dev; + ioTag_t clk; + ioTag_t bk1IO0; + ioTag_t bk1IO1; + ioTag_t bk1IO2; + ioTag_t bk1IO3; + ioTag_t bk1CS; + ioTag_t bk2IO0; + ioTag_t bk2IO1; + ioTag_t bk2IO2; + ioTag_t bk2IO3; + ioTag_t bk2CS; +#if defined(STM32H7) + uint8_t bk1IO0AF; + uint8_t bk1IO1AF; + uint8_t bk1IO2AF; + uint8_t bk1IO3AF; + uint8_t bk1CSAF; + uint8_t bk2IO0AF; + uint8_t bk2IO1AF; + uint8_t bk2IO2AF; + uint8_t bk2IO3AF; + uint8_t bk2CSAF; +#endif + rccPeriphTag_t rcc; + volatile uint16_t errorCount; +#if defined(USE_HAL_DRIVER) + QSPI_HandleTypeDef hquadSpi; +#endif +} quadSpiDevice_t; + +extern quadSpiDevice_t quadSpiDevice[QUADSPIDEV_COUNT]; + +void quadSpiInitDevice(QUADSPIDevice device); +uint32_t quadSpiTimeoutUserCallback(QUADSPI_TypeDef *instance); diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 6fe825dc1f..705db70c2d 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -121,9 +121,14 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, uint16_t spiGetErrorCounter(SPI_TypeDef *instance); void spiResetErrorCounter(SPI_TypeDef *instance); + SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); SPI_TypeDef *spiInstanceByDevice(SPIDevice device); +// +// BusDevice API +// + bool spiBusIsBusBusy(const busDevice_t *bus); bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length); @@ -150,5 +155,9 @@ uint8_t spiBusTransactionReadRegister(const busDevice_t *bus, uint8_t reg); bool spiBusTransactionReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); bool spiBusTransactionTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length); +// +// Config +// + struct spiPinConfig_s; void spiPinConfigure(const struct spiPinConfig_s *pConfig); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index ceabba8a9a..90cebf6234 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -87,4 +87,15 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "RX_SPI_CC2500_TX_EN", "RX_SPI_CC2500_LNA_EN", "RX_SPI_CC2500_ANT_SEL", + "QSPI_CLK", + "QSPI_BK1IO0", + "QSPI_BK1IO1", + "QSPI_BK1IO2", + "QSPI_BK1IO3", + "QSPI_BK1CS", + "QSPI_BK2IO0", + "QSPI_BK2IO1", + "QSPI_BK2IO2", + "QSPI_BK2IO3", + "QSPI_BK2CS", }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 233e74871d..99bc7b067a 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -85,6 +85,17 @@ typedef enum { OWNER_RX_SPI_CC2500_TX_EN, OWNER_RX_SPI_CC2500_LNA_EN, OWNER_RX_SPI_CC2500_ANT_SEL, + OWNER_QUADSPI_CLK, + OWNER_QUADSPI_BK1IO0, + OWNER_QUADSPI_BK1IO1, + OWNER_QUADSPI_BK1IO2, + OWNER_QUADSPI_BK1IO3, + OWNER_QUADSPI_BK1CS, + OWNER_QUADSPI_BK2IO0, + OWNER_QUADSPI_BK2IO1, + OWNER_QUADSPI_BK2IO2, + OWNER_QUADSPI_BK2IO3, + OWNER_QUADSPI_BK2CS, OWNER_TOTAL_COUNT } resourceOwner_e; diff --git a/src/main/fc/init.c b/src/main/fc/init.c index f10e772a6e..abf36819df 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -47,6 +47,7 @@ #include "drivers/adc.h" #include "drivers/bus.h" #include "drivers/bus_i2c.h" +#include "drivers/bus_quadspi.h" #include "drivers/bus_spi.h" #include "drivers/buttons.h" #include "drivers/camera_control.h" @@ -133,6 +134,7 @@ #include "pg/beeper_dev.h" #include "pg/bus_i2c.h" #include "pg/bus_spi.h" +#include "pg/bus_quadspi.h" #include "pg/flash.h" #include "pg/mco.h" #include "pg/pinio.h" @@ -456,6 +458,15 @@ void init(void) #endif #endif // USE_SPI +#ifdef USE_QUADSPI + quadSpiPinConfigure(quadSpiConfig(0)); + +#ifdef USE_QUADSPI_DEVICE_1 + quadSpiInit(QUADSPIDEV_1); +#endif +#endif // USE_QUAD_SPI + + #ifdef USE_USB_MSC /* MSC mode will start after init, but will not allow scheduler to run, * so there is no bottleneck in reading and writing data */ diff --git a/src/main/pg/bus_quadspi.c b/src/main/pg/bus_quadspi.c new file mode 100644 index 0000000000..e901530621 --- /dev/null +++ b/src/main/pg/bus_quadspi.c @@ -0,0 +1,92 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 "platform.h" + +#ifdef USE_QUADSPI + +#include "drivers/io.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "bus_quadspi.h" + +typedef struct quadSpiDefaultConfig_s { + QUADSPIDevice device; + ioTag_t clk; + + // Note: Either or both CS pin may be used in DUAL_FLASH mode, any unused pins should be IO_NONE + ioTag_t bk1IO0; + ioTag_t bk1IO1; + ioTag_t bk1IO2; + ioTag_t bk1IO3; + ioTag_t bk1CS; + + ioTag_t bk2IO0; + ioTag_t bk2IO1; + ioTag_t bk2IO2; + ioTag_t bk2IO3; + ioTag_t bk2CS; + + uint8_t mode; + + // CS pins can be under software control, useful when using BK1CS as the CS pin for BK2 in non-DUAL-FLASH mode. + uint8_t csFlags; +} quadSpiDefaultConfig_t; + +const quadSpiDefaultConfig_t quadSpiDefaultConfig[] = { +#ifdef USE_QUADSPI_DEVICE_1 + { + QUADSPIDEV_1, + IO_TAG(QUADSPI1_SCK_PIN), + IO_TAG(QUADSPI1_BK1_IO0_PIN), IO_TAG(QUADSPI1_BK1_IO1_PIN), IO_TAG(QUADSPI1_BK1_IO2_PIN), IO_TAG(QUADSPI1_BK1_IO3_PIN), IO_TAG(QUADSPI1_BK1_CS_PIN), + IO_TAG(QUADSPI1_BK2_IO0_PIN), IO_TAG(QUADSPI1_BK2_IO1_PIN), IO_TAG(QUADSPI1_BK2_IO2_PIN), IO_TAG(QUADSPI1_BK2_IO3_PIN), IO_TAG(QUADSPI1_BK2_CS_PIN), + QUADSPI1_MODE, + QUADSPI1_CS_FLAGS + }, +#endif +}; + +PG_REGISTER_ARRAY_WITH_RESET_FN(quadSpiConfig_t, QUADSPIDEV_COUNT, quadSpiConfig, PG_QUADSPI_CONFIG, 1); + +void pgResetFn_quadSpiConfig(quadSpiConfig_t *quadSpiConfig) +{ + for (size_t i = 0 ; i < ARRAYLEN(quadSpiDefaultConfig) ; i++) { + const quadSpiDefaultConfig_t *defconf = &quadSpiDefaultConfig[i]; + quadSpiConfig[defconf->device].ioTagClk = defconf->clk; + + quadSpiConfig[defconf->device].ioTagBK1IO0 = defconf->bk1IO0; + quadSpiConfig[defconf->device].ioTagBK1IO1 = defconf->bk1IO1; + quadSpiConfig[defconf->device].ioTagBK1IO2 = defconf->bk1IO2; + quadSpiConfig[defconf->device].ioTagBK1IO3 = defconf->bk1IO3; + quadSpiConfig[defconf->device].ioTagBK1CS = defconf->bk1CS; + + quadSpiConfig[defconf->device].ioTagBK2IO0 = defconf->bk2IO0; + quadSpiConfig[defconf->device].ioTagBK2IO1 = defconf->bk2IO1; + quadSpiConfig[defconf->device].ioTagBK2IO2 = defconf->bk2IO2; + quadSpiConfig[defconf->device].ioTagBK2IO3 = defconf->bk2IO3; + quadSpiConfig[defconf->device].ioTagBK2CS = defconf->bk2CS; + + quadSpiConfig[defconf->device].mode = defconf->mode; + quadSpiConfig[defconf->device].csFlags = defconf->csFlags; + } +} +#endif diff --git a/src/main/pg/bus_quadspi.h b/src/main/pg/bus_quadspi.h new file mode 100644 index 0000000000..22990761de --- /dev/null +++ b/src/main/pg/bus_quadspi.h @@ -0,0 +1,53 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are 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. + * + * Cleanflight and Betaflight are distributed in the hope that they + * 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 . + */ + +#pragma once + +#include "drivers/bus_quadspi.h" +#include "drivers/io_types.h" + +#include "pg/pg.h" + +#ifdef USE_QUADSPI + +typedef struct quadSpiConfig_s { + ioTag_t ioTagClk; + + ioTag_t ioTagBK1IO0; + ioTag_t ioTagBK1IO1; + ioTag_t ioTagBK1IO2; + ioTag_t ioTagBK1IO3; + ioTag_t ioTagBK1CS; + + ioTag_t ioTagBK2IO0; + ioTag_t ioTagBK2IO1; + ioTag_t ioTagBK2IO2; + ioTag_t ioTagBK2IO3; + ioTag_t ioTagBK2CS; + + uint8_t mode; + + // CS pins can be under software control, useful when using BK1CS as the CS pin for BK2 in non-DUAL-FLASH mode. + uint8_t csFlags; +} quadSpiConfig_t; + +PG_DECLARE_ARRAY(quadSpiConfig_t, QUADSPIDEV_COUNT, quadSpiConfig); + +#endif diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 99990d9fbe..f6e7829841 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -146,7 +146,8 @@ #define PG_LED_STRIP_STATUS_MODE_CONFIG 545 // Used to hold the configuration for the LED_STRIP status mode (not built on targets with limited flash) #define PG_VTX_TABLE_CONFIG 546 #define PG_STATS_CONFIG 547 -#define PG_BETAFLIGHT_END 547 +#define PG_QUADSPI_CONFIG 548 +#define PG_BETAFLIGHT_END 548 // OSD configuration (subject to change)