mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 01:05:21 +03:00
Merge pull request #7572 from bkleiner/bkleiner-h7-usb-msc
h7: usb msc support for sdio
This commit is contained in:
commit
c4ecdd71ea
12 changed files with 731 additions and 36 deletions
|
@ -158,7 +158,8 @@ main_sources(STM32H7_SRC
|
|||
drivers/system_stm32h7xx.c
|
||||
drivers/serial_uart_stm32h7xx.c
|
||||
drivers/serial_uart_hal.c
|
||||
# drivers/sdcard/sdmmc_sdio_h7xx.c
|
||||
drivers/sdio.h
|
||||
drivers/sdcard/sdmmc_sdio_h7xx.c
|
||||
)
|
||||
|
||||
main_sources(STM32H7_MSC_SRC
|
||||
|
@ -188,8 +189,6 @@ function(target_stm32h7xx)
|
|||
|
||||
OPENOCD_TARGET stm32h7x
|
||||
|
||||
DISABLE_MSC # This should be temporary
|
||||
|
||||
# BOOTLOADER
|
||||
|
||||
${ARGN}
|
||||
|
|
|
@ -82,12 +82,13 @@ typedef struct _USBD_STORAGE
|
|||
|
||||
typedef struct
|
||||
{
|
||||
// bot_data at start of structure to ensure cache alignment
|
||||
uint8_t bot_data[MSC_MEDIA_PACKET];
|
||||
uint32_t max_lun;
|
||||
uint32_t interface;
|
||||
uint8_t bot_state;
|
||||
uint8_t bot_status;
|
||||
uint16_t bot_data_length;
|
||||
uint8_t bot_data[MSC_MEDIA_PACKET];
|
||||
USBD_MSC_BOT_CBWTypeDef cbw;
|
||||
USBD_MSC_BOT_CSWTypeDef csw;
|
||||
|
||||
|
|
|
@ -265,6 +265,8 @@ __ALIGN_BEGIN uint8_t USBD_MSC_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC]
|
|||
* @}
|
||||
*/
|
||||
|
||||
static USBD_MSC_BOT_HandleTypeDef ClassData __attribute__((aligned(32)));
|
||||
|
||||
|
||||
/** @defgroup MSC_CORE_Private_Functions
|
||||
* @{
|
||||
|
@ -301,7 +303,7 @@ uint8_t USBD_MSC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx)
|
|||
USBD_LL_OpenEP(pdev, MSC_EPIN_ADDR, USBD_EP_TYPE_BULK, MSC_MAX_FS_PACKET);
|
||||
pdev->ep_in[MSC_EPIN_ADDR & 0xFU].is_used = 1U;
|
||||
}
|
||||
pdev->pMSC_ClassData = USBD_malloc(sizeof(USBD_MSC_BOT_HandleTypeDef));
|
||||
pdev->pMSC_ClassData = (void*)&ClassData;
|
||||
|
||||
if (pdev->pMSC_ClassData == NULL)
|
||||
{
|
||||
|
|
|
@ -181,7 +181,8 @@ int8_t SCSI_ProcessCmd(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *cmd)
|
|||
*/
|
||||
static int8_t SCSI_TestUnitReady(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
/* case 9 : Hi > D0 */
|
||||
if (hmsc->cbw.dDataLength != 0U)
|
||||
|
@ -191,7 +192,7 @@ static int8_t SCSI_TestUnitReady(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t
|
|||
return -1;
|
||||
}
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->IsReady(lun) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsReady(lun) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
hmsc->bot_state = USBD_BOT_NO_DATA;
|
||||
|
@ -214,7 +215,7 @@ static int8_t SCSI_Inquiry(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *par
|
|||
{
|
||||
uint8_t *pPage;
|
||||
uint16_t len;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
if (params[1] & 0x01U)/*Evpd is set*/
|
||||
{
|
||||
|
@ -229,7 +230,7 @@ static int8_t SCSI_Inquiry(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *par
|
|||
}
|
||||
else
|
||||
{
|
||||
pPage = (uint8_t *)(void *) & ((USBD_StorageTypeDef *)pdev->pUserData)->pInquiry[lun * STANDARD_INQUIRY_DATA_LEN];
|
||||
pPage = (uint8_t *)(void *) & ((USBD_StorageTypeDef *)pdev->pMSC_UserData)->pInquiry[lun * STANDARD_INQUIRY_DATA_LEN];
|
||||
len = (uint16_t)pPage[4] + 5U;
|
||||
|
||||
if (params[4] <= len)
|
||||
|
@ -257,9 +258,10 @@ static int8_t SCSI_Inquiry(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *par
|
|||
*/
|
||||
static int8_t SCSI_ReadCapacity10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->GetCapacity(lun, &hmsc->scsi_blk_nbr, &hmsc->scsi_blk_size) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->GetCapacity(lun, &hmsc->scsi_blk_nbr, &hmsc->scsi_blk_size) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
return -1;
|
||||
|
@ -290,7 +292,8 @@ static int8_t SCSI_ReadCapacity10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_
|
|||
*/
|
||||
static int8_t SCSI_ReadFormatCapacity(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
uint16_t blk_size;
|
||||
uint32_t blk_nbr;
|
||||
|
@ -301,7 +304,7 @@ static int8_t SCSI_ReadFormatCapacity(USBD_HandleTypeDef *pdev, uint8_t lun, ui
|
|||
hmsc->bot_data[i] = 0U;
|
||||
}
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->GetCapacity(lun, &blk_nbr, &blk_size) != 0U)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->GetCapacity(lun, &blk_nbr, &blk_size) != 0U)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
return -1;
|
||||
|
@ -332,7 +335,9 @@ static int8_t SCSI_ReadFormatCapacity(USBD_HandleTypeDef *pdev, uint8_t lun, ui
|
|||
*/
|
||||
static int8_t SCSI_ModeSense6(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(lun);
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
uint16_t len = 8U;
|
||||
hmsc->bot_data_length = len;
|
||||
|
||||
|
@ -353,8 +358,10 @@ static int8_t SCSI_ModeSense6(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *p
|
|||
*/
|
||||
static int8_t SCSI_ModeSense10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
UNUSED(lun);
|
||||
UNUSED(params);
|
||||
uint16_t len = 8U;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
hmsc->bot_data_length = len;
|
||||
|
||||
|
@ -377,8 +384,9 @@ static int8_t SCSI_ModeSense10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *
|
|||
|
||||
static int8_t SCSI_RequestSense(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
UNUSED(lun);
|
||||
uint8_t i;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
for (i = 0U ; i < REQUEST_SENSE_DATA_LEN; i++)
|
||||
{
|
||||
|
@ -421,7 +429,8 @@ static int8_t SCSI_RequestSense(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t
|
|||
*/
|
||||
void SCSI_SenseCode(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t sKey, uint8_t ASC)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
UNUSED(lun);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
|
||||
hmsc->scsi_sense[hmsc->scsi_sense_tail].Skey = sKey;
|
||||
hmsc->scsi_sense[hmsc->scsi_sense_tail].w.ASC = ASC << 8;
|
||||
|
@ -440,7 +449,9 @@ void SCSI_SenseCode(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t sKey, uint8_
|
|||
*/
|
||||
static int8_t SCSI_StartStopUnit(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
UNUSED(lun);
|
||||
UNUSED(params);
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
hmsc->bot_data_length = 0U;
|
||||
return 0;
|
||||
}
|
||||
|
@ -454,7 +465,7 @@ static int8_t SCSI_StartStopUnit(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t
|
|||
*/
|
||||
static int8_t SCSI_Read10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
|
||||
if (hmsc->bot_state == USBD_BOT_IDLE) /* Idle */
|
||||
{
|
||||
|
@ -465,7 +476,7 @@ static int8_t SCSI_Read10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params
|
|||
return -1;
|
||||
}
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->IsReady(lun) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsReady(lun) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
return -1;
|
||||
|
@ -508,7 +519,7 @@ static int8_t SCSI_Read10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params
|
|||
|
||||
static int8_t SCSI_Write10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
uint32_t len;
|
||||
|
||||
if (hmsc->bot_state == USBD_BOT_IDLE) /* Idle */
|
||||
|
@ -521,14 +532,14 @@ static int8_t SCSI_Write10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *para
|
|||
}
|
||||
|
||||
/* Check whether Media is ready */
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->IsReady(lun) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsReady(lun) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, MEDIUM_NOT_PRESENT);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Check If media is write-protected */
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->IsWriteProtected(lun) != 0)
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0)
|
||||
{
|
||||
SCSI_SenseCode(pdev, lun, NOT_READY, WRITE_PROTECTED);
|
||||
return -1;
|
||||
|
@ -582,7 +593,7 @@ static int8_t SCSI_Write10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *para
|
|||
|
||||
static int8_t SCSI_Verify10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *params)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
|
||||
if ((params[1] & 0x02U) == 0x02U)
|
||||
{
|
||||
|
@ -610,7 +621,7 @@ static int8_t SCSI_Verify10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *par
|
|||
static int8_t SCSI_CheckAddressRange(USBD_HandleTypeDef *pdev, uint8_t lun,
|
||||
uint32_t blk_offset, uint32_t blk_nbr)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
|
||||
if ((blk_offset + blk_nbr) > hmsc->scsi_blk_nbr)
|
||||
{
|
||||
|
@ -628,12 +639,12 @@ static int8_t SCSI_CheckAddressRange(USBD_HandleTypeDef *pdev, uint8_t lun,
|
|||
*/
|
||||
static int8_t SCSI_ProcessRead(USBD_HandleTypeDef *pdev, uint8_t lun)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *)pdev->pMSC_ClassData;
|
||||
uint32_t len = hmsc->scsi_blk_len * hmsc->scsi_blk_size;
|
||||
|
||||
len = MIN(len, MSC_MEDIA_PACKET);
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->Read(lun,
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->Read(lun,
|
||||
hmsc->bot_data,
|
||||
hmsc->scsi_blk_addr,
|
||||
(len / hmsc->scsi_blk_size)) < 0)
|
||||
|
@ -666,12 +677,12 @@ static int8_t SCSI_ProcessRead(USBD_HandleTypeDef *pdev, uint8_t lun)
|
|||
|
||||
static int8_t SCSI_ProcessWrite(USBD_HandleTypeDef *pdev, uint8_t lun)
|
||||
{
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pClassData;
|
||||
USBD_MSC_BOT_HandleTypeDef *hmsc = (USBD_MSC_BOT_HandleTypeDef *) pdev->pMSC_ClassData;
|
||||
uint32_t len = hmsc->scsi_blk_len * hmsc->scsi_blk_size;
|
||||
|
||||
len = MIN(len, MSC_MEDIA_PACKET);
|
||||
|
||||
if (((USBD_StorageTypeDef *)pdev->pUserData)->Write(lun, hmsc->bot_data,
|
||||
if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->Write(lun, hmsc->bot_data,
|
||||
hmsc->scsi_blk_addr,
|
||||
(len / hmsc->scsi_blk_size)) < 0)
|
||||
{
|
||||
|
|
|
@ -241,11 +241,12 @@ static sdcardOperationStatus_e sdcard_endWriteBlocks(void)
|
|||
*/
|
||||
static bool sdcardSdio_poll(void)
|
||||
{
|
||||
#if !defined(STM32H7) // H7 uses IDMA
|
||||
if (!sdcard.dma) {
|
||||
sdcard.state = SDCARD_STATE_NOT_PRESENT;
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif
|
||||
doMore:
|
||||
switch (sdcard.state) {
|
||||
case SDCARD_STATE_RESET:
|
||||
|
@ -545,6 +546,7 @@ static bool sdcardSdio_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_op
|
|||
*/
|
||||
void sdcardSdio_init(void)
|
||||
{
|
||||
#if !defined(STM32H7) // H7 uses IDMA
|
||||
sdcard.dma = dmaGetByTag(SDCARD_SDIO_DMA);
|
||||
|
||||
if (!sdcard.dma) {
|
||||
|
@ -564,7 +566,12 @@ void sdcardSdio_init(void)
|
|||
sdcard.state = SDCARD_STATE_NOT_PRESENT;
|
||||
return;
|
||||
}
|
||||
|
||||
#else
|
||||
if (!SD_Initialize_LL(0)) {
|
||||
sdcard.state = SDCARD_STATE_NOT_PRESENT;
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
// We don't support hot insertion
|
||||
if (!sdcard_isInserted()) {
|
||||
sdcard.state = SDCARD_STATE_NOT_PRESENT;
|
||||
|
|
|
@ -38,6 +38,10 @@
|
|||
|
||||
#ifdef STM32F7
|
||||
#include "stm32f7xx.h"
|
||||
#endif
|
||||
|
||||
#ifdef STM32H7
|
||||
#include "stm32h7xx.h"
|
||||
#endif
|
||||
|
||||
/* SDCARD pinouts
|
||||
|
|
614
src/main/drivers/sdcard/sdmmc_sdio_h7xx.c
Normal file
614
src/main/drivers/sdcard/sdmmc_sdio_h7xx.c
Normal file
|
@ -0,0 +1,614 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Original author: Alain (https://github.com/aroyer-qc)
|
||||
* Modified for BF source: Chris Hockuba (https://github.com/conkerkh)
|
||||
*/
|
||||
|
||||
/* Include(s) -------------------------------------------------------------------------------------------------------*/
|
||||
|
||||
#include "stdbool.h"
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_SDCARD_SDIO
|
||||
|
||||
#include "sdmmc_sdio.h"
|
||||
#include "stm32h7xx.h"
|
||||
|
||||
#include "drivers/sdio.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/io_impl.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/rcc.h"
|
||||
#include "drivers/dma.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
typedef struct SD_Handle_s
|
||||
{
|
||||
uint32_t CSD[4]; // SD card specific data table
|
||||
uint32_t CID[4]; // SD card identification number table
|
||||
volatile uint32_t RXCplt; // SD RX Complete is equal 0 when no transfer
|
||||
volatile uint32_t TXCplt; // SD TX Complete is equal 0 when no transfer
|
||||
} SD_Handle_t;
|
||||
|
||||
SD_HandleTypeDef hsd1;
|
||||
|
||||
SD_CardInfo_t SD_CardInfo;
|
||||
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];
|
||||
|
||||
void sdioPinConfigure(void)
|
||||
{
|
||||
if (SDCARD_SDIO_DEVICE == SDIOINVALID) {
|
||||
return;
|
||||
}
|
||||
|
||||
sdioHardware = &sdioPinHardware[SDCARD_SDIO_DEVICE];
|
||||
|
||||
sdioPin[SDIO_PIN_CK] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCK[0];
|
||||
sdioPin[SDIO_PIN_CMD] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCMD[0];
|
||||
sdioPin[SDIO_PIN_D0] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD0[0];
|
||||
|
||||
#ifdef SDCARD_SDIO_4BIT
|
||||
sdioPin[SDIO_PIN_D1] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD1[0];
|
||||
sdioPin[SDIO_PIN_D2] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD2[0];
|
||||
sdioPin[SDIO_PIN_D3] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD3[0];
|
||||
#endif
|
||||
}
|
||||
|
||||
#define IOCFG_SDMMC IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
|
||||
|
||||
void HAL_SD_MspInit(SD_HandleTypeDef* hsd)
|
||||
{
|
||||
UNUSED(hsd);
|
||||
|
||||
if (!sdioHardware) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (sdioHardware->instance == SDMMC1) {
|
||||
__HAL_RCC_SDMMC1_CLK_DISABLE();
|
||||
__HAL_RCC_SDMMC1_FORCE_RESET();
|
||||
__HAL_RCC_SDMMC1_RELEASE_RESET();
|
||||
__HAL_RCC_SDMMC1_CLK_ENABLE();
|
||||
} else if (sdioHardware->instance == SDMMC2) {
|
||||
__HAL_RCC_SDMMC2_CLK_DISABLE();
|
||||
__HAL_RCC_SDMMC2_FORCE_RESET();
|
||||
__HAL_RCC_SDMMC2_RELEASE_RESET();
|
||||
__HAL_RCC_SDMMC2_CLK_ENABLE();
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
#ifdef SDCARD_SDIO_4BIT
|
||||
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);
|
||||
#endif
|
||||
|
||||
HAL_NVIC_SetPriority(sdioHardware->irqn, 0, 0);
|
||||
HAL_NVIC_EnableIRQ(sdioHardware->irqn);
|
||||
}
|
||||
|
||||
void SDIO_GPIO_Init(void)
|
||||
{
|
||||
if (!sdioHardware) {
|
||||
return;
|
||||
}
|
||||
|
||||
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_SDCARD, RESOURCE_NONE, 0);
|
||||
IOInit(cmd, OWNER_SDCARD, RESOURCE_NONE, 0);
|
||||
IOInit(d0, OWNER_SDCARD, RESOURCE_NONE, 0);
|
||||
|
||||
#ifdef SDCARD_SDIO_4BIT
|
||||
IOInit(d1, OWNER_SDCARD, RESOURCE_NONE, 0);
|
||||
IOInit(d2, OWNER_SDCARD, RESOURCE_NONE, 0);
|
||||
IOInit(d3, OWNER_SDCARD, RESOURCE_NONE, 0);
|
||||
#endif
|
||||
|
||||
//
|
||||
// Setting all the SDIO pins to high for a short time results in more robust initialisation.
|
||||
//
|
||||
IOHi(d0);
|
||||
IOConfigGPIO(d0, IOCFG_OUT_PP);
|
||||
|
||||
#ifdef SDCARD_SDIO_4BIT
|
||||
IOHi(d1);
|
||||
IOHi(d2);
|
||||
IOHi(d3);
|
||||
IOConfigGPIO(d1, IOCFG_OUT_PP);
|
||||
IOConfigGPIO(d2, IOCFG_OUT_PP);
|
||||
IOConfigGPIO(d3, IOCFG_OUT_PP);
|
||||
#endif
|
||||
|
||||
IOHi(clk);
|
||||
IOHi(cmd);
|
||||
IOConfigGPIO(clk, IOCFG_OUT_PP);
|
||||
IOConfigGPIO(cmd, IOCFG_OUT_PP);
|
||||
}
|
||||
|
||||
bool SD_Initialize_LL(DMA_Stream_TypeDef *dma)
|
||||
{
|
||||
UNUSED(dma);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SD_GetState(void)
|
||||
{
|
||||
HAL_SD_CardStateTypedef cardState = HAL_SD_GetCardState(&hsd1);
|
||||
|
||||
return (cardState == HAL_SD_CARD_TRANSFER);
|
||||
}
|
||||
|
||||
bool SD_Init(void)
|
||||
{
|
||||
HAL_StatusTypeDef status;
|
||||
|
||||
memset(&hsd1, 0, sizeof(hsd1));
|
||||
|
||||
hsd1.Instance = sdioHardware->instance;
|
||||
|
||||
hsd1.Init.ClockEdge = SDMMC_CLOCK_EDGE_RISING;
|
||||
hsd1.Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_ENABLE;
|
||||
#ifdef SDCARD_SDIO_4BIT
|
||||
hsd1.Init.BusWide = SDMMC_BUS_WIDE_4B;
|
||||
#else
|
||||
hsd1.Init.BusWide = SDMMC_BUS_WIDE_1B; // FIXME untested
|
||||
#endif
|
||||
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); // Will call HAL_SD_MspInit
|
||||
|
||||
if (status != HAL_OK) {
|
||||
return SD_ERROR;
|
||||
}
|
||||
|
||||
switch(hsd1.SdCard.CardType) {
|
||||
case CARD_SDSC:
|
||||
switch (hsd1.SdCard.CardVersion) {
|
||||
case CARD_V1_X:
|
||||
SD_CardType = SD_STD_CAPACITY_V1_1;
|
||||
break;
|
||||
case CARD_V2_X:
|
||||
SD_CardType = SD_STD_CAPACITY_V2_0;
|
||||
break;
|
||||
default:
|
||||
return SD_ERROR;
|
||||
}
|
||||
break;
|
||||
|
||||
case CARD_SDHC_SDXC:
|
||||
SD_CardType = SD_HIGH_CAPACITY;
|
||||
break;
|
||||
|
||||
default:
|
||||
return SD_ERROR;
|
||||
}
|
||||
|
||||
// STATIC_ASSERT(sizeof(SD_Handle.CSD) == sizeof(hsd1.CSD), hal-csd-size-error);
|
||||
memcpy(&SD_Handle.CSD, &hsd1.CSD, sizeof(SD_Handle.CSD));
|
||||
|
||||
// STATIC_ASSERT(sizeof(SD_Handle.CID) == sizeof(hsd1.CID), hal-cid-size-error);
|
||||
memcpy(&SD_Handle.CID, &hsd1.CID, sizeof(SD_Handle.CID));
|
||||
|
||||
return SD_OK;
|
||||
}
|
||||
|
||||
SD_Error_t SD_GetCardInfo(void)
|
||||
{
|
||||
SD_Error_t ErrorState = SD_OK;
|
||||
|
||||
// fill in SD_CardInfo
|
||||
|
||||
uint32_t Temp = 0;
|
||||
|
||||
// Byte 0
|
||||
Temp = (SD_Handle.CSD[0] & 0xFF000000) >> 24;
|
||||
SD_CardInfo.SD_csd.CSDStruct = (uint8_t)((Temp & 0xC0) >> 6);
|
||||
SD_CardInfo.SD_csd.SysSpecVersion = (uint8_t)((Temp & 0x3C) >> 2);
|
||||
SD_CardInfo.SD_csd.Reserved1 = Temp & 0x03;
|
||||
|
||||
// Byte 1
|
||||
Temp = (SD_Handle.CSD[0] & 0x00FF0000) >> 16;
|
||||
SD_CardInfo.SD_csd.TAAC = (uint8_t)Temp;
|
||||
|
||||
// Byte 2
|
||||
Temp = (SD_Handle.CSD[0] & 0x0000FF00) >> 8;
|
||||
SD_CardInfo.SD_csd.NSAC = (uint8_t)Temp;
|
||||
|
||||
// Byte 3
|
||||
Temp = SD_Handle.CSD[0] & 0x000000FF;
|
||||
SD_CardInfo.SD_csd.MaxBusClkFrec = (uint8_t)Temp;
|
||||
|
||||
// Byte 4
|
||||
Temp = (SD_Handle.CSD[1] & 0xFF000000) >> 24;
|
||||
SD_CardInfo.SD_csd.CardComdClasses = (uint16_t)(Temp << 4);
|
||||
|
||||
// Byte 5
|
||||
Temp = (SD_Handle.CSD[1] & 0x00FF0000) >> 16;
|
||||
SD_CardInfo.SD_csd.CardComdClasses |= (uint16_t)((Temp & 0xF0) >> 4);
|
||||
SD_CardInfo.SD_csd.RdBlockLen = (uint8_t)(Temp & 0x0F);
|
||||
|
||||
// Byte 6
|
||||
Temp = (SD_Handle.CSD[1] & 0x0000FF00) >> 8;
|
||||
SD_CardInfo.SD_csd.PartBlockRead = (uint8_t)((Temp & 0x80) >> 7);
|
||||
SD_CardInfo.SD_csd.WrBlockMisalign = (uint8_t)((Temp & 0x40) >> 6);
|
||||
SD_CardInfo.SD_csd.RdBlockMisalign = (uint8_t)((Temp & 0x20) >> 5);
|
||||
SD_CardInfo.SD_csd.DSRImpl = (uint8_t)((Temp & 0x10) >> 4);
|
||||
SD_CardInfo.SD_csd.Reserved2 = 0; /*!< Reserved */
|
||||
|
||||
if((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0)) {
|
||||
SD_CardInfo.SD_csd.DeviceSize = (Temp & 0x03) << 10;
|
||||
|
||||
// Byte 7
|
||||
Temp = (uint8_t)(SD_Handle.CSD[1] & 0x000000FF);
|
||||
SD_CardInfo.SD_csd.DeviceSize |= (Temp) << 2;
|
||||
|
||||
// Byte 8
|
||||
Temp = (uint8_t)((SD_Handle.CSD[2] & 0xFF000000) >> 24);
|
||||
SD_CardInfo.SD_csd.DeviceSize |= (Temp & 0xC0) >> 6;
|
||||
|
||||
SD_CardInfo.SD_csd.MaxRdCurrentVDDMin = (Temp & 0x38) >> 3;
|
||||
SD_CardInfo.SD_csd.MaxRdCurrentVDDMax = (Temp & 0x07);
|
||||
|
||||
// Byte 9
|
||||
Temp = (uint8_t)((SD_Handle.CSD[2] & 0x00FF0000) >> 16);
|
||||
SD_CardInfo.SD_csd.MaxWrCurrentVDDMin = (Temp & 0xE0) >> 5;
|
||||
SD_CardInfo.SD_csd.MaxWrCurrentVDDMax = (Temp & 0x1C) >> 2;
|
||||
SD_CardInfo.SD_csd.DeviceSizeMul = (Temp & 0x03) << 1;
|
||||
|
||||
// Byte 10
|
||||
Temp = (uint8_t)((SD_Handle.CSD[2] & 0x0000FF00) >> 8);
|
||||
SD_CardInfo.SD_csd.DeviceSizeMul |= (Temp & 0x80) >> 7;
|
||||
|
||||
SD_CardInfo.CardCapacity = (SD_CardInfo.SD_csd.DeviceSize + 1) ;
|
||||
SD_CardInfo.CardCapacity *= (1 << (SD_CardInfo.SD_csd.DeviceSizeMul + 2));
|
||||
SD_CardInfo.CardBlockSize = 1 << (SD_CardInfo.SD_csd.RdBlockLen);
|
||||
SD_CardInfo.CardCapacity = SD_CardInfo.CardCapacity * SD_CardInfo.CardBlockSize / 512; // In 512 byte blocks
|
||||
} else if(SD_CardType == SD_HIGH_CAPACITY) {
|
||||
// Byte 7
|
||||
Temp = (uint8_t)(SD_Handle.CSD[1] & 0x000000FF);
|
||||
SD_CardInfo.SD_csd.DeviceSize = (Temp & 0x3F) << 16;
|
||||
|
||||
// Byte 8
|
||||
Temp = (uint8_t)((SD_Handle.CSD[2] & 0xFF000000) >> 24);
|
||||
|
||||
SD_CardInfo.SD_csd.DeviceSize |= (Temp << 8);
|
||||
|
||||
// Byte 9
|
||||
Temp = (uint8_t)((SD_Handle.CSD[2] & 0x00FF0000) >> 16);
|
||||
|
||||
SD_CardInfo.SD_csd.DeviceSize |= (Temp);
|
||||
|
||||
// Byte 10
|
||||
Temp = (uint8_t)((SD_Handle.CSD[2] & 0x0000FF00) >> 8);
|
||||
|
||||
SD_CardInfo.CardCapacity = ((uint64_t)SD_CardInfo.SD_csd.DeviceSize + 1) * 1024;
|
||||
SD_CardInfo.CardBlockSize = 512;
|
||||
} else {
|
||||
// Not supported card type
|
||||
ErrorState = SD_ERROR;
|
||||
}
|
||||
|
||||
SD_CardInfo.SD_csd.EraseGrSize = (Temp & 0x40) >> 6;
|
||||
SD_CardInfo.SD_csd.EraseGrMul = (Temp & 0x3F) << 1;
|
||||
|
||||
// Byte 11
|
||||
Temp = (uint8_t)(SD_Handle.CSD[2] & 0x000000FF);
|
||||
SD_CardInfo.SD_csd.EraseGrMul |= (Temp & 0x80) >> 7;
|
||||
SD_CardInfo.SD_csd.WrProtectGrSize = (Temp & 0x7F);
|
||||
|
||||
// Byte 12
|
||||
Temp = (uint8_t)((SD_Handle.CSD[3] & 0xFF000000) >> 24);
|
||||
SD_CardInfo.SD_csd.WrProtectGrEnable = (Temp & 0x80) >> 7;
|
||||
SD_CardInfo.SD_csd.ManDeflECC = (Temp & 0x60) >> 5;
|
||||
SD_CardInfo.SD_csd.WrSpeedFact = (Temp & 0x1C) >> 2;
|
||||
SD_CardInfo.SD_csd.MaxWrBlockLen = (Temp & 0x03) << 2;
|
||||
|
||||
// Byte 13
|
||||
Temp = (uint8_t)((SD_Handle.CSD[3] & 0x00FF0000) >> 16);
|
||||
SD_CardInfo.SD_csd.MaxWrBlockLen |= (Temp & 0xC0) >> 6;
|
||||
SD_CardInfo.SD_csd.WriteBlockPaPartial = (Temp & 0x20) >> 5;
|
||||
SD_CardInfo.SD_csd.Reserved3 = 0;
|
||||
SD_CardInfo.SD_csd.ContentProtectAppli = (Temp & 0x01);
|
||||
|
||||
// Byte 14
|
||||
Temp = (uint8_t)((SD_Handle.CSD[3] & 0x0000FF00) >> 8);
|
||||
SD_CardInfo.SD_csd.FileFormatGrouop = (Temp & 0x80) >> 7;
|
||||
SD_CardInfo.SD_csd.CopyFlag = (Temp & 0x40) >> 6;
|
||||
SD_CardInfo.SD_csd.PermWrProtect = (Temp & 0x20) >> 5;
|
||||
SD_CardInfo.SD_csd.TempWrProtect = (Temp & 0x10) >> 4;
|
||||
SD_CardInfo.SD_csd.FileFormat = (Temp & 0x0C) >> 2;
|
||||
SD_CardInfo.SD_csd.ECC = (Temp & 0x03);
|
||||
|
||||
// Byte 15
|
||||
Temp = (uint8_t)(SD_Handle.CSD[3] & 0x000000FF);
|
||||
SD_CardInfo.SD_csd.CSD_CRC = (Temp & 0xFE) >> 1;
|
||||
SD_CardInfo.SD_csd.Reserved4 = 1;
|
||||
|
||||
// Byte 0
|
||||
Temp = (uint8_t)((SD_Handle.CID[0] & 0xFF000000) >> 24);
|
||||
SD_CardInfo.SD_cid.ManufacturerID = Temp;
|
||||
|
||||
// Byte 1
|
||||
Temp = (uint8_t)((SD_Handle.CID[0] & 0x00FF0000) >> 16);
|
||||
SD_CardInfo.SD_cid.OEM_AppliID = Temp << 8;
|
||||
|
||||
// Byte 2
|
||||
Temp = (uint8_t)((SD_Handle.CID[0] & 0x000000FF00) >> 8);
|
||||
SD_CardInfo.SD_cid.OEM_AppliID |= Temp;
|
||||
|
||||
// Byte 3
|
||||
Temp = (uint8_t)(SD_Handle.CID[0] & 0x000000FF);
|
||||
SD_CardInfo.SD_cid.ProdName1 = Temp << 24;
|
||||
|
||||
// Byte 4
|
||||
Temp = (uint8_t)((SD_Handle.CID[1] & 0xFF000000) >> 24);
|
||||
SD_CardInfo.SD_cid.ProdName1 |= Temp << 16;
|
||||
|
||||
// Byte 5
|
||||
Temp = (uint8_t)((SD_Handle.CID[1] & 0x00FF0000) >> 16);
|
||||
SD_CardInfo.SD_cid.ProdName1 |= Temp << 8;
|
||||
|
||||
// Byte 6
|
||||
Temp = (uint8_t)((SD_Handle.CID[1] & 0x0000FF00) >> 8);
|
||||
SD_CardInfo.SD_cid.ProdName1 |= Temp;
|
||||
|
||||
// Byte 7
|
||||
Temp = (uint8_t)(SD_Handle.CID[1] & 0x000000FF);
|
||||
SD_CardInfo.SD_cid.ProdName2 = Temp;
|
||||
|
||||
// Byte 8
|
||||
Temp = (uint8_t)((SD_Handle.CID[2] & 0xFF000000) >> 24);
|
||||
SD_CardInfo.SD_cid.ProdRev = Temp;
|
||||
|
||||
// Byte 9
|
||||
Temp = (uint8_t)((SD_Handle.CID[2] & 0x00FF0000) >> 16);
|
||||
SD_CardInfo.SD_cid.ProdSN = Temp << 24;
|
||||
|
||||
// Byte 10
|
||||
Temp = (uint8_t)((SD_Handle.CID[2] & 0x0000FF00) >> 8);
|
||||
SD_CardInfo.SD_cid.ProdSN |= Temp << 16;
|
||||
|
||||
// Byte 11
|
||||
Temp = (uint8_t)(SD_Handle.CID[2] & 0x000000FF);
|
||||
SD_CardInfo.SD_cid.ProdSN |= Temp << 8;
|
||||
|
||||
// Byte 12
|
||||
Temp = (uint8_t)((SD_Handle.CID[3] & 0xFF000000) >> 24);
|
||||
SD_CardInfo.SD_cid.ProdSN |= Temp;
|
||||
|
||||
// Byte 13
|
||||
Temp = (uint8_t)((SD_Handle.CID[3] & 0x00FF0000) >> 16);
|
||||
SD_CardInfo.SD_cid.Reserved1 |= (Temp & 0xF0) >> 4;
|
||||
SD_CardInfo.SD_cid.ManufactDate = (Temp & 0x0F) << 8;
|
||||
|
||||
// Byte 14
|
||||
Temp = (uint8_t)((SD_Handle.CID[3] & 0x0000FF00) >> 8);
|
||||
SD_CardInfo.SD_cid.ManufactDate |= Temp;
|
||||
|
||||
// Byte 15
|
||||
Temp = (uint8_t)(SD_Handle.CID[3] & 0x000000FF);
|
||||
SD_CardInfo.SD_cid.CID_CRC = (Temp & 0xFE) >> 1;
|
||||
SD_CardInfo.SD_cid.Reserved2 = 1;
|
||||
|
||||
return ErrorState;
|
||||
}
|
||||
|
||||
SD_Error_t SD_CheckWrite(void) {
|
||||
if (SD_Handle.TXCplt != 0) return SD_BUSY;
|
||||
return SD_OK;
|
||||
}
|
||||
|
||||
SD_Error_t SD_CheckRead(void) {
|
||||
if (SD_Handle.RXCplt != 0) return SD_BUSY;
|
||||
return SD_OK;
|
||||
}
|
||||
|
||||
SD_Error_t SD_WriteBlocks_DMA(uint64_t WriteAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks)
|
||||
{
|
||||
SD_Error_t ErrorState = SD_OK;
|
||||
SD_Handle.TXCplt = 1;
|
||||
|
||||
if (BlockSize != 512) {
|
||||
return SD_ERROR; // unsupported.
|
||||
}
|
||||
|
||||
if ((uint32_t)buffer & 0x1f) {
|
||||
return SD_ADDR_MISALIGNED;
|
||||
}
|
||||
|
||||
// Ensure the data is flushed to main memory
|
||||
SCB_CleanDCache_by_Addr(buffer, NumberOfBlocks * BlockSize);
|
||||
|
||||
HAL_StatusTypeDef status;
|
||||
if ((status = HAL_SD_WriteBlocks_DMA(&hsd1, (uint8_t *)buffer, WriteAddress, NumberOfBlocks)) != HAL_OK) {
|
||||
return SD_ERROR;
|
||||
}
|
||||
|
||||
return ErrorState;
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
uint32_t *buffer;
|
||||
uint32_t BlockSize;
|
||||
uint32_t NumberOfBlocks;
|
||||
} sdReadParameters_t;
|
||||
|
||||
sdReadParameters_t sdReadParameters;
|
||||
|
||||
SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks)
|
||||
{
|
||||
SD_Error_t ErrorState = SD_OK;
|
||||
|
||||
if (BlockSize != 512) {
|
||||
return SD_ERROR; // unsupported.
|
||||
}
|
||||
|
||||
if ((uint32_t)buffer & 0x1f) {
|
||||
return SD_ADDR_MISALIGNED;
|
||||
}
|
||||
|
||||
SD_Handle.RXCplt = 1;
|
||||
|
||||
sdReadParameters.buffer = buffer;
|
||||
sdReadParameters.BlockSize = BlockSize;
|
||||
sdReadParameters.NumberOfBlocks = NumberOfBlocks;
|
||||
|
||||
HAL_StatusTypeDef status;
|
||||
if ((status = HAL_SD_ReadBlocks_DMA(&hsd1, (uint8_t *)buffer, ReadAddress, NumberOfBlocks)) != HAL_OK) {
|
||||
return SD_ERROR;
|
||||
}
|
||||
|
||||
return ErrorState;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Tx Transfer completed callback
|
||||
* @param hsd: SD handle
|
||||
* @retval None
|
||||
*/
|
||||
void HAL_SD_TxCpltCallback(SD_HandleTypeDef *hsd)
|
||||
{
|
||||
UNUSED(hsd);
|
||||
|
||||
SD_Handle.TXCplt = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Rx Transfer completed callback
|
||||
* @param hsd: SD handle
|
||||
* @retval None
|
||||
*/
|
||||
void HAL_SD_RxCpltCallback(SD_HandleTypeDef *hsd)
|
||||
{
|
||||
UNUSED(hsd);
|
||||
|
||||
SD_Handle.RXCplt = 0;
|
||||
|
||||
/*
|
||||
the SCB_InvalidateDCache_by_Addr() requires a 32-Byte aligned address,
|
||||
adjust the address and the D-Cache size to invalidate accordingly.
|
||||
*/
|
||||
uint32_t alignedAddr = (uint32_t)sdReadParameters.buffer & ~0x1F;
|
||||
SCB_InvalidateDCache_by_Addr((uint32_t*)alignedAddr, sdReadParameters.NumberOfBlocks * sdReadParameters.BlockSize + ((uint32_t)sdReadParameters.buffer - alignedAddr));
|
||||
}
|
||||
|
||||
void HAL_SD_AbortCallback(SD_HandleTypeDef *hsd)
|
||||
{
|
||||
UNUSED(hsd);
|
||||
|
||||
SD_Handle.TXCplt = 0;
|
||||
SD_Handle.RXCplt = 0;
|
||||
}
|
||||
|
||||
void SDMMC1_IRQHandler(void)
|
||||
{
|
||||
HAL_SD_IRQHandler(&hsd1);
|
||||
}
|
||||
|
||||
void SDMMC2_IRQHandler(void)
|
||||
{
|
||||
HAL_SD_IRQHandler(&hsd1);
|
||||
}
|
||||
|
||||
#endif
|
35
src/main/drivers/sdio.h
Normal file
35
src/main/drivers/sdio.h
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
void SDIO_GPIO_Init(void);
|
||||
#endif
|
|
@ -82,6 +82,7 @@
|
|||
#include "msc/emfat_file.h"
|
||||
#endif
|
||||
#include "drivers/sdcard/sdcard.h"
|
||||
#include "drivers/sdio.h"
|
||||
#include "drivers/io_port_expander.h"
|
||||
|
||||
#include "fc/cli.h"
|
||||
|
@ -367,6 +368,11 @@ void init(void)
|
|||
updateHardwareRevision();
|
||||
#endif
|
||||
|
||||
#if defined(USE_SDCARD_SDIO) && defined(STM32H7)
|
||||
sdioPinConfigure();
|
||||
SDIO_GPIO_Init();
|
||||
#endif
|
||||
|
||||
#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
|
||||
|
|
|
@ -435,7 +435,11 @@ typedef struct afatfs_t {
|
|||
} initState;
|
||||
#endif
|
||||
|
||||
#ifdef STM32H7
|
||||
uint8_t *cache;
|
||||
#else
|
||||
uint8_t cache[AFATFS_SECTOR_SIZE * AFATFS_NUM_CACHE_SECTORS];
|
||||
#endif
|
||||
afatfsCacheBlockDescriptor_t cacheDescriptor[AFATFS_NUM_CACHE_SECTORS];
|
||||
uint32_t cacheTimer;
|
||||
|
||||
|
@ -481,6 +485,10 @@ typedef struct afatfs_t {
|
|||
uint32_t rootDirectorySectors; // Zero on FAT32, for FAT16 the number of sectors that the root directory occupies
|
||||
} afatfs_t;
|
||||
|
||||
#ifdef STM32H7
|
||||
static uint8_t afatfs_cache[AFATFS_SECTOR_SIZE * AFATFS_NUM_CACHE_SECTORS] __attribute__((aligned(32)));
|
||||
#endif
|
||||
|
||||
static afatfs_t afatfs;
|
||||
|
||||
static void afatfs_fileOperationContinue(afatfsFile_t *file);
|
||||
|
@ -3616,6 +3624,9 @@ afatfsError_e afatfs_getLastError(void)
|
|||
|
||||
void afatfs_init(void)
|
||||
{
|
||||
#ifdef STM32H7
|
||||
afatfs.cache = afatfs_cache;
|
||||
#endif
|
||||
afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_INITIALIZATION;
|
||||
afatfs.initPhase = AFATFS_INITIALIZATION_READ_MBR;
|
||||
afatfs.lastClusterAllocated = FAT_SMALLEST_LEGAL_CLUSTER_NUMBER;
|
||||
|
|
|
@ -156,7 +156,11 @@ static int8_t STORAGE_Init (uint8_t lun)
|
|||
|
||||
UNUSED(lun);
|
||||
LED0_OFF;
|
||||
#if defined(STM32H7) // H7 uses IDMA
|
||||
SD_Initialize_LL(0);
|
||||
#else
|
||||
SD_Initialize_LL(SDIO_DMA);
|
||||
#endif
|
||||
if (SD_Init() != 0) return 1;
|
||||
LED0_ON;
|
||||
return 0;
|
||||
|
|
|
@ -168,11 +168,12 @@
|
|||
#define SERIALRX_UART SERIAL_PORT_USART6
|
||||
|
||||
// *************** SDIO SD BLACKBOX*******************
|
||||
//#define USE_SDCARD
|
||||
//#define USE_SDCARD_SDIO
|
||||
//#define SDCARD_SDIO_DMA DMA_TAG(2,3,4)
|
||||
//#define SDCARD_SDIO_4BIT
|
||||
//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||
#define USE_SDCARD
|
||||
#define USE_SDCARD_SDIO
|
||||
#define SDCARD_SDIO_DEVICE SDIODEV_1
|
||||
#define SDCARD_SDIO_4BIT
|
||||
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||
|
||||
// *************** ADC *****************************
|
||||
#define USE_ADC
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue