diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 2e7a153831..0e4bbe9d7d 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4755,6 +4755,14 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_SDCARD DEFS( OWNER_SDCARD_DETECT, PG_SDCARD_CONFIG, sdcardConfig_t, cardDetectTag ), #endif +#if defined(STM32H7) && defined(USE_SDCARD_SDIO) + DEFS( OWNER_SDIO_CK, PG_SDIO_PIN_CONFIG, sdioPinConfig_t, CKPin ), + DEFS( OWNER_SDIO_CMD, PG_SDIO_PIN_CONFIG, sdioPinConfig_t, CMDPin ), + DEFS( OWNER_SDIO_D0, PG_SDIO_PIN_CONFIG, sdioPinConfig_t, D0Pin ), + DEFS( OWNER_SDIO_D1, PG_SDIO_PIN_CONFIG, sdioPinConfig_t, D1Pin ), + DEFS( OWNER_SDIO_D2, PG_SDIO_PIN_CONFIG, sdioPinConfig_t, D2Pin ), + DEFS( OWNER_SDIO_D3, PG_SDIO_PIN_CONFIG, sdioPinConfig_t, D3Pin ), +#endif #ifdef USE_PINIO DEFA( OWNER_PINIO, PG_PINIO_CONFIG, pinioConfig_t, ioTag, PINIO_COUNT ), #endif diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 242c939e50..20a541b573 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -39,6 +39,7 @@ #include "drivers/camera_control.h" #include "drivers/light_led.h" #include "drivers/pinio.h" +#include "drivers/sdio.h" #include "drivers/vtx_common.h" #include "drivers/vtx_table.h" @@ -1153,6 +1154,9 @@ const clivalue_t valueTable[] = { { "sdio_clk_bypass", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDIO_CONFIG, offsetof(sdioConfig_t, clockBypass) }, { "sdio_use_cache", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDIO_CONFIG, offsetof(sdioConfig_t, useCache) }, { "sdio_use_4bit_width", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDIO_CONFIG, offsetof(sdioConfig_t, use4BitWidth) }, +#ifdef STM32H7 + { "sdio_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SDIODEV_COUNT }, PG_SDIO_CONFIG, offsetof(sdioConfig_t, device) }, +#endif #endif // PG_OSD_CONFIG diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 4447dc9815..d9e09bb002 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -47,6 +47,12 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "I2C_SCL", "I2C_SDA", "SDCARD", + "SDIO_CK", + "SDIO_CMD", + "SDIO_D0", + "SDIO_D1", + "SDIO_D2", + "SDIO_D3", "SDCARD_CS", "SDCARD_DETECT", "FLASH_CS", diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 757d4d34b0..98226a903b 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -45,6 +45,12 @@ typedef enum { OWNER_I2C_SCL, OWNER_I2C_SDA, OWNER_SDCARD, + OWNER_SDIO_CK, + OWNER_SDIO_CMD, + OWNER_SDIO_D0, + OWNER_SDIO_D1, + OWNER_SDIO_D2, + OWNER_SDIO_D3, OWNER_SDCARD_CS, OWNER_SDCARD_DETECT, OWNER_FLASH_CS, diff --git a/src/main/drivers/sdio.h b/src/main/drivers/sdio.h new file mode 100644 index 0000000000..cd60548123 --- /dev/null +++ b/src/main/drivers/sdio.h @@ -0,0 +1,35 @@ +/* + * 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 . + */ + +#define SDIO_CFG_TO_DEV(x) ((x) - 1) +#define SDIO_DEV_TO_CFG(x) ((x) + 1) + +typedef enum { + SDIOINVALID = -1, + SDIODEV_1 = 0, + SDIODEV_2, +} SDIODevice; + +#define SDIODEV_COUNT 2 + +#if defined(STM32H7) +void sdioPinConfigure(); +void SDIO_GPIO_Init(void); +#endif diff --git a/src/main/drivers/sdio_h7xx.c b/src/main/drivers/sdio_h7xx.c index 5001300ce9..aba446cb05 100644 --- a/src/main/drivers/sdio_h7xx.c +++ b/src/main/drivers/sdio_h7xx.c @@ -38,6 +38,7 @@ #include "drivers/io.h" #include "drivers/io_impl.h" +#include "drivers/sdio.h" typedef struct SD_Handle_s { @@ -55,75 +56,170 @@ SD_CardType_t SD_CardType; static SD_Handle_t SD_Handle; +typedef struct sdioPin_s { + ioTag_t pin; + uint8_t af; +} sdioPin_t; + +#define SDIO_PIN_D0 0 +#define SDIO_PIN_D1 1 +#define SDIO_PIN_D2 2 +#define SDIO_PIN_D3 3 +#define SDIO_PIN_CK 4 +#define SDIO_PIN_CMD 5 +#define SDIO_PIN_COUNT 6 + +#define SDIO_MAX_PINDEFS 2 + +typedef struct sdioHardware_s { + SDMMC_TypeDef *instance; + IRQn_Type irqn; + sdioPin_t sdioPinCK[SDIO_MAX_PINDEFS]; + sdioPin_t sdioPinCMD[SDIO_MAX_PINDEFS]; + sdioPin_t sdioPinD0[SDIO_MAX_PINDEFS]; + sdioPin_t sdioPinD1[SDIO_MAX_PINDEFS]; + sdioPin_t sdioPinD2[SDIO_MAX_PINDEFS]; + sdioPin_t sdioPinD3[SDIO_MAX_PINDEFS]; +} sdioHardware_t; + +// Possible pin assignments + +#define PINDEF(device, pin, afnum) { DEFIO_TAG_E(pin), GPIO_AF ## afnum ## _SDMMC ## device } + +static const sdioHardware_t sdioPinHardware[SDIODEV_COUNT] = { + { + .instance = SDMMC1, + .irqn = SDMMC1_IRQn, + .sdioPinCK = { PINDEF(1, PC12, 12) }, + .sdioPinCMD = { PINDEF(1, PD2, 12) }, + .sdioPinD0 = { PINDEF(1, PC8, 12) }, + .sdioPinD1 = { PINDEF(1, PC9, 12) }, + .sdioPinD2 = { PINDEF(1, PC10, 12) }, + .sdioPinD3 = { PINDEF(1, PC11, 12) }, + }, + { + .instance = SDMMC2, + .irqn = SDMMC2_IRQn, + .sdioPinCK = { PINDEF(2, PC1, 9), PINDEF(2, PD6, 11) }, + .sdioPinCMD = { PINDEF(2, PA0, 9), PINDEF(2, PD7, 11) }, + .sdioPinD0 = { PINDEF(2, PB14, 9) }, + .sdioPinD1 = { PINDEF(2, PB15, 9) }, + .sdioPinD2 = { PINDEF(2, PB3, 9) }, + .sdioPinD3 = { PINDEF(2, PB4, 9) }, + } +}; + +#undef PINDEF + +// Active configuration +static const sdioHardware_t *sdioHardware; +static sdioPin_t sdioPin[SDIO_PIN_COUNT]; + +static const sdioPin_t *sdioFindPinDef(const sdioPin_t *pindefs, ioTag_t pin) +{ + for (unsigned index = 0; index < SDIO_MAX_PINDEFS; index++) { + if (pindefs[index].pin == pin) { + return &pindefs[index]; + } + } + + return NULL; +} + +#define SDIOFINDPIN(pinname) { \ + const sdioPin_t *pindef; \ + pindef = sdioFindPinDef(sdioHardware->sdioPin ## pinname, sdioPinConfig()->pinname ## Pin); \ + if (pindef) { \ + sdioPin[SDIO_PIN_ ## pinname] = *pindef; \ + } \ + } struct dummy + +void sdioPinConfigure(void) +{ + SDIODevice device = SDIO_CFG_TO_DEV(sdioConfig()->device); + + if (device == SDIOINVALID) { + return; + } + + sdioHardware = &sdioPinHardware[device]; + + SDIOFINDPIN(CK); + SDIOFINDPIN(CMD); + SDIOFINDPIN(D0); + + if (sdioConfig()->use4BitWidth) { + SDIOFINDPIN(D1); + SDIOFINDPIN(D2); + SDIOFINDPIN(D3); + } +} + +#undef SDIOFINDPIN + +#define IOCFG_SDMMC IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) + void HAL_SD_MspInit(SD_HandleTypeDef* hsd) { - if(hsd->Instance==SDMMC1) - { - /* Peripheral clock enable */ - //__HAL_RCC_SDMMC1_CLK_ENABLE(); // DC - already done in enableGPIOPowerUsageAndNoiseReductions - //__HAL_RCC_GPIOC_CLK_ENABLE(); // DC - already done in enableGPIOPowerUsageAndNoiseReductions - //__HAL_RCC_GPIOD_CLK_ENABLE(); // DC - already done in enableGPIOPowerUsageAndNoiseReductions + UNUSED(hsd); - /**SDMMC1 GPIO Configuration - PC8 ------> SDMMC1_D0 - PC9 ------> SDMMC1_D1 - PC10 ------> SDMMC1_D2 - PC11 ------> SDMMC1_D3 - PC12 ------> SDMMC1_CK - PD2 ------> SDMMC1_CMD - */ + if (!sdioHardware) { + return; + } + + if (sdioHardware->instance == SDMMC1) { + __HAL_RCC_SDMMC1_CLK_ENABLE(); + } else if (sdioHardware->instance == SDMMC2) { + __HAL_RCC_SDMMC2_CLK_ENABLE(); + } uint8_t is4BitWidth = sdioConfig()->use4BitWidth; - const IO_t d0 = IOGetByTag(IO_TAG(PC8)); - const IO_t d1 = IOGetByTag(IO_TAG(PC9)); - const IO_t d2 = IOGetByTag(IO_TAG(PC10)); - const IO_t d3 = IOGetByTag(IO_TAG(PC11)); - const IO_t clk = IOGetByTag(IO_TAG(PC12)); - const IO_t cmd = IOGetByTag(IO_TAG(PD2)); + const IO_t clk = IOGetByTag(sdioPin[SDIO_PIN_CK].pin); + const IO_t cmd = IOGetByTag(sdioPin[SDIO_PIN_CMD].pin); + const IO_t d0 = IOGetByTag(sdioPin[SDIO_PIN_D0].pin); + const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin); + const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin); + const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin); -#define SDMMC_DATA IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) -#define SDMMC_CMD IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) -#define SDMMC_CLK IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL) + IOConfigGPIOAF(clk, IOCFG_SDMMC, sdioPin[SDIO_PIN_CK].af); + IOConfigGPIOAF(cmd, IOCFG_SDMMC, sdioPin[SDIO_PIN_CMD].af); + IOConfigGPIOAF(d0, IOCFG_SDMMC, sdioPin[SDIO_PIN_D0].af); - IOConfigGPIOAF(d0, SDMMC_DATA, GPIO_AF12_SDMMC1); if(is4BitWidth) { - IOConfigGPIOAF(d1, SDMMC_DATA, GPIO_AF12_SDMMC1); - IOConfigGPIOAF(d2, SDMMC_DATA, GPIO_AF12_SDMMC1); - IOConfigGPIOAF(d3, SDMMC_DATA, GPIO_AF12_SDMMC1); + IOConfigGPIOAF(d1, IOCFG_SDMMC, sdioPin[SDIO_PIN_D1].af); + IOConfigGPIOAF(d2, IOCFG_SDMMC, sdioPin[SDIO_PIN_D2].af); + IOConfigGPIOAF(d3, IOCFG_SDMMC, sdioPin[SDIO_PIN_D3].af); } - IOConfigGPIOAF(clk, SDMMC_CLK, GPIO_AF12_SDMMC1); - IOConfigGPIOAF(cmd, SDMMC_CMD, GPIO_AF12_SDMMC1); - HAL_NVIC_SetPriority(SDMMC1_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(SDMMC1_IRQn); - } + HAL_NVIC_SetPriority(sdioHardware->irqn, 0, 0); + HAL_NVIC_EnableIRQ(sdioHardware->irqn); } void SDIO_GPIO_Init(void) { - - /* GPIO Ports Clock Enable */ - //__HAL_RCC_GPIOC_CLK_ENABLE(); // DC - already done in enableGPIOPowerUsageAndNoiseReductions - //__HAL_RCC_GPIOD_CLK_ENABLE(); // DC - already done in enableGPIOPowerUsageAndNoiseReductions + if (!sdioHardware) { + return; + } uint8_t is4BitWidth = sdioConfig()->use4BitWidth; - const IO_t d0 = IOGetByTag(IO_TAG(PC8)); - const IO_t d1 = IOGetByTag(IO_TAG(PC9)); - const IO_t d2 = IOGetByTag(IO_TAG(PC10)); - const IO_t d3 = IOGetByTag(IO_TAG(PC11)); - const IO_t clk = IOGetByTag(IO_TAG(PC12)); - const IO_t cmd = IOGetByTag(IO_TAG(PD2)); + const IO_t clk = IOGetByTag(sdioPin[SDIO_PIN_CK].pin); + const IO_t cmd = IOGetByTag(sdioPin[SDIO_PIN_CMD].pin); + const IO_t d0 = IOGetByTag(sdioPin[SDIO_PIN_D0].pin); + const IO_t d1 = IOGetByTag(sdioPin[SDIO_PIN_D1].pin); + const IO_t d2 = IOGetByTag(sdioPin[SDIO_PIN_D2].pin); + const IO_t d3 = IOGetByTag(sdioPin[SDIO_PIN_D3].pin); + + IOInit(clk, OWNER_SDIO_CK, 0); + IOInit(cmd, OWNER_SDIO_CMD, 0); + IOInit(d0, OWNER_SDIO_D0, 0); - IOInit(d0, OWNER_SDCARD, 0); if (is4BitWidth) { - IOInit(d1, OWNER_SDCARD, 0); - IOInit(d2, OWNER_SDCARD, 0); - IOInit(d3, OWNER_SDCARD, 0); + IOInit(d1, OWNER_SDIO_D1, 0); + IOInit(d2, OWNER_SDIO_D2, 0); + IOInit(d3, OWNER_SDIO_D3, 0); } - IOInit(clk, OWNER_SDCARD, 0); - IOInit(cmd, OWNER_SDCARD, 0); // // Setting all the SDIO pins to high for a short time results in more robust initialisation. @@ -149,8 +245,6 @@ void SDIO_GPIO_Init(void) void SD_Initialize_LL(DMA_Stream_TypeDef *dma) { UNUSED(dma); - - __HAL_RCC_SDMMC1_CLK_ENABLE(); } bool SD_GetState(void) @@ -174,7 +268,8 @@ bool SD_Init(void) memset(&hsd1, 0, sizeof(hsd1)); - hsd1.Instance = SDMMC1; + hsd1.Instance = sdioHardware->instance; + hsd1.Init.ClockEdge = SDMMC_CLOCK_EDGE_RISING; hsd1.Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_ENABLE; if (sdioConfig()->use4BitWidth) { @@ -185,7 +280,8 @@ bool SD_Init(void) hsd1.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE; hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV - status = HAL_SD_Init(&hsd1); + status = HAL_SD_Init(&hsd1); // Will call HAL_SD_MspInit + if (status != HAL_OK) { return failureResult; } @@ -526,13 +622,14 @@ void HAL_SD_AbortCallback(SD_HandleTypeDef *hsd) SD_Handle.RXCplt = 0; } - -/** -* @brief This function handles SDMMC1 global interrupt. -*/ void SDMMC1_IRQHandler(void) { HAL_SD_IRQHandler(&hsd1); } +void SDMMC2_IRQHandler(void) +{ + HAL_SD_IRQHandler(&hsd1); +} + #endif diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 7bdd605dd1..220b152f6b 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -69,6 +69,7 @@ #include "drivers/serial_softserial.h" #include "drivers/serial_uart.h" #include "drivers/sdcard.h" +#include "drivers/sdio.h" #include "drivers/sound_beeper.h" #include "drivers/system.h" #include "drivers/time.h" @@ -181,8 +182,6 @@ serialPort_t *loopbackPort; uint8_t systemState = SYSTEM_STATE_INITIALISING; -void SDIO_GPIO_Init(void); - void processLoopback(void) { #ifdef SOFTSERIAL_LOOPBACK @@ -318,6 +317,7 @@ void init(void) pgResetAll(); #if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too + sdioPinConfigure(); SDIO_GPIO_Init(); #endif #ifdef USE_SDCARD_SPI @@ -618,6 +618,7 @@ void init(void) #if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too if (!(initFlags & SD_INIT_ATTEMPTED)) { + sdioPinConfigure(); SDIO_GPIO_Init(); } #endif diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 7976e44006..66a084d100 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -146,7 +146,8 @@ #define PG_STATS_CONFIG 547 #define PG_QUADSPI_CONFIG 548 #define PG_TIMER_UP_CONFIG 549 // used to store dmaopt for TIMx_UP channel -#define PG_BETAFLIGHT_END 549 +#define PG_SDIO_PIN_CONFIG 550 +#define PG_BETAFLIGHT_END 550 // OSD configuration (subject to change) diff --git a/src/main/pg/sdio.c b/src/main/pg/sdio.c index 42cf2ea0e6..70ec8c2aec 100644 --- a/src/main/pg/sdio.c +++ b/src/main/pg/sdio.c @@ -22,6 +22,8 @@ #if defined(USE_SDCARD_SDIO) +#include "drivers/io.h" +#include "drivers/sdio.h" #include "pg/pg_ids.h" #include "pg/sdio.h" @@ -30,8 +32,22 @@ PG_REGISTER_WITH_RESET_TEMPLATE(sdioConfig_t, sdioConfig, PG_SDIO_CONFIG, 0); PG_RESET_TEMPLATE(sdioConfig_t, sdioConfig, .clockBypass = 0, .useCache = 0, - .use4BitWidth = true, + .use4BitWidth = SDIO_USE_4BIT, .dmaopt = SDCARD_SDIO_DMA_OPT, + .device = SDIO_DEV_TO_CFG(SDIO_DEVICE), ); +#ifdef STM32H7 +PG_REGISTER_WITH_RESET_TEMPLATE(sdioPinConfig_t, sdioPinConfig, PG_SDIO_PIN_CONFIG, 0); + +PG_RESET_TEMPLATE(sdioPinConfig_t, sdioPinConfig, + .CKPin = IO_TAG(SDIO_CK_PIN), + .CMDPin = IO_TAG(SDIO_CMD_PIN), + .D0Pin = IO_TAG(SDIO_D0_PIN), + .D1Pin = IO_TAG(SDIO_D1_PIN), + .D2Pin = IO_TAG(SDIO_D2_PIN), + .D3Pin = IO_TAG(SDIO_D3_PIN), +); +#endif + #endif diff --git a/src/main/pg/sdio.h b/src/main/pg/sdio.h index e01eae0417..aa9841ca9b 100644 --- a/src/main/pg/sdio.h +++ b/src/main/pg/sdio.h @@ -20,6 +20,7 @@ #pragma once +#include "drivers/io_types.h" #include "pg/pg.h" typedef struct sdioConfig_s { @@ -27,6 +28,18 @@ typedef struct sdioConfig_s { uint8_t useCache; uint8_t use4BitWidth; int8_t dmaopt; + uint8_t device; } sdioConfig_t; PG_DECLARE(sdioConfig_t, sdioConfig); + +typedef struct sdioPinConfig_s { + ioTag_t CKPin; + ioTag_t CMDPin; + ioTag_t D0Pin; + ioTag_t D1Pin; + ioTag_t D2Pin; + ioTag_t D3Pin; +} sdioPinConfig_t; + +PG_DECLARE(sdioPinConfig_t, sdioPinConfig); diff --git a/src/main/target/common_defaults_post.h b/src/main/target/common_defaults_post.h index 4710cae721..9033c78602 100644 --- a/src/main/target/common_defaults_post.h +++ b/src/main/target/common_defaults_post.h @@ -422,6 +422,30 @@ #ifndef SDCARD_SDIO_DMA_OPT #define SDCARD_SDIO_DMA_OPT (-1) #endif +#ifndef SDIO_DEVICE +#define SDIO_DEVICE SDIOINVALID +#endif +#ifndef SDIO_USE_4BIT +#define SDIO_USE_4BIT false +#endif +#ifndef SDIO_CK_PIN +#define SDIO_CK_PIN NONE +#endif +#ifndef SDIO_CMD_PIN +#define SDIO_CMD_PIN NONE +#endif +#ifndef SDIO_D0_PIN +#define SDIO_D0_PIN NONE +#endif +#ifndef SDIO_D1_PIN +#define SDIO_D1_PIN NONE +#endif +#ifndef SDIO_D2_PIN +#define SDIO_D2_PIN NONE +#endif +#ifndef SDIO_D3_PIN +#define SDIO_D3_PIN NONE +#endif #endif // USE_SDCARD_SDIO #endif // USE_SDCARD