/*
 * shimatta_sdio-driver.c
 *
 *  Created on: Apr 30, 2015
 *  Mario Hüttel
 */

#include "shimatta_sdio.h"
#include "shimatta_sdio_config.h"
#include <cmsis/core_cm4.h>
#include <stm32f4xx.h>

extern void sdio_wait_ms(unsigned int i);

#define SETAF(PORT,PIN,AF)  PORT->AFR[(PIN < 8 ? 0 : 1)] |= AF << ((PIN < 8 ? PIN : (PIN - 8)) * 4)

#define READCTRL ((BLOCKSIZE << 4) | SDIO_DCTRL_DMAEN)
#define DMAP2M (DMA_SxCR_CHSEL_2 | DMA_SxCR_PBURST_0 | DMA_SxCR_MBURST_0 | DMA_SxCR_MSIZE_1 | DMA_SxCR_PSIZE_1 | DMA_SxCR_MINC | DMA_SxCR_PFCTRL)
#define DMAM2P (DMA_SxCR_CHSEL_2 | DMA_SxCR_PBURST_0 | DMA_SxCR_MBURST_0 | DMA_SxCR_MSIZE_1 | DMA_SxCR_PSIZE_1 | DMA_SxCR_MINC | DMA_SxCR_PFCTRL | DMA_SxCR_DIR_0)
#define SHORT_ANS 1
#define LONG_ANS 3
#define NO_ANS 0

#define CCRCFAIL 1
#define CTIMEOUT 2
#define CNOTEXPETED 3

/* OCR Register Masks */
#define OCS_CCS (1<<30)
#define OCS_BUSY (1<<31)

enum acmd41_ret {ACMD41_RESP_INIT = 0, ACMD41_RESP_ERR, ACMD41_RESP_SDSC, ACMD41_RESP_SDXC};
enum cmd8_ret {CMD8_RESP_TIMEOUT = 0, CMD8_VOLTAGE_ACCEPTED, CMD8_VOLTAGE_DENIED};
typedef uint8_t CID_t;

/*
void SDIO_init_hw();

int SDIO_send_cmd(uint8_t CMD, uint32_t arg, uint8_t expectedAns);
int SDIO_get_response(uint8_t expectedCMD, uint8_t typeOfAns, uint32_t* responseBuffer);


void SDIO_wait_cmd_sent(void);
ACMD41_RESP_t SDIO_init_card_ACMD41(uint8_t HCS);
int SDIO_switch_appmode_CMD55(void);
int SDIO_send_all_send_cid_CMD2(void);
int SDIO_send_relative_address_CMD3(uint16_t* rca);
int SDIO_send_go_idle_CMD0(void);
CMD8_RESP_t SDIO_send_iface_condition_CMD8(void);
int SDIO_send_block_length_CMD16(uint32_t blocklen);
int SDIO_send_bus_width_ACMD6(uint8_t bus_width);
int SDIO_send_csd_CMD9(uint16_t rca, uint32_t *responsebuffer);
int SDIO_send_select_card_CMD7(uint16_t rca);
int SDIO_check_status_register_CMD13(uint16_t rca, uint32_t *status);
void SDIO_init_detect_pins(void);
int checkNotInserted(void); // Returns 0 if inserted!
int checkWriteProtection(void); // returns 0 if write protected
void switchPrescaler(uint8_t clkdiv);
int SDIO_send_write_block_CMD24(uint32_t addr);
int SDIO_send_read_block_CMD17(uint32_t addr);
int SDIO_get_sector_count(uint16_t rca, uint32_t *sector_count);
*/

//BYTE rxtxbuffer[1<<BLOCKSIZE]; //Data RX and TX Buffer not needed anymore. thanks to DMA
static struct sd_info card_info; // = {.type = CARD_NONE};

/**
 * @brief checkNotInserted
 * @return return 0 if card is inserted, else 1
 */
static int sdio_check_inserted() {
#if SDIO_ENABLE_INS
    return ((INS_PORT->IDR & INS_PIN) == (INS_ACTIVE_LEVEL<<INS_PIN) ? 0 : 1);
#else
    return 0; // Assume Card is inserted
#endif
}

/**
 * @brief checkWriteProtection
 * @return 0 if card is writable.
 */
static int sdio_check_write_protection() {
#if SDIO_ENABLE_WRITEPROT
    return ((WRITEPROT_PORT->IDR & WRITEPROT_PIN) == (WRITEPROT_ACTIVE_LEVEL<<WRITEPROT_PIN) ? 1 : 0);
#else
    return 0; // Assume Card is not write protected
#endif
}

static void sdio_wait_cmd_sent()
{
    while (!(SDIO->STA & SDIO_STA_CMDSENT));
    SDIO->ICR |= SDIO_ICR_CMDSENTC;
}

static int sdio_send_cmd(uint8_t CMD, uint32_t arg, uint8_t expectedAns){
    //Clear Flags
    SDIO->ICR = SDIO_ICR_CCRCFAILC | SDIO_ICR_CMDRENDC | SDIO_ICR_CTIMEOUTC | SDIO_ICR_CMDSENTC;
    //Send command
    SDIO->ARG = arg;
    SDIO->CMD = (CMD & SDIO_CMD_CMDINDEX) | SDIO_CMD_CPSMEN | /*SDIO_CMD_WAITPEND |*/ ((expectedAns << 6) & SDIO_CMD_WAITRESP);
    return 0;
}

static int sdio_get_response(uint8_t expected_command, uint8_t type_of_answer, uint32_t *response_buffer) {
    uint32_t sdio_status;

    /* Wait until command isn't active anymore */
    while (SDIO->STA & SDIO_STA_CMDACT);

    /* Wait for error or success */
    while (1) {
        sdio_status = SDIO->STA;

        /* Check if a valid response was received */
        if (sdio_status & SDIO_STA_CMDREND)
            break;

        if ((sdio_status & SDIO_STA_CMDSENT) && (type_of_answer == NO_ANS))
            break; // No response required

        /* Exclude ACMD41 and CMD2 from valid CRC check */
        if ((sdio_status & SDIO_STA_CCRCFAIL)) {
            if(expected_command == 0xff) {
                break;
            } else {
                return -CCRCFAIL;
            }
        }

        if (sdio_status & SDIO_STA_CTIMEOUT)
            return -CTIMEOUT;
    }
    //Valid Respone Received
    if (((SDIO->RESPCMD & SDIO_RESPCMD_RESPCMD) != expected_command) && (expected_command != 0xff))
        return -CNOTEXPETED; //Not the expected respose

    /* If case of a correct Response */
    *(response_buffer++) = SDIO->RESP1;
    /* Long response */
    if (type_of_answer == LONG_ANS) {
        *(response_buffer++) = SDIO->RESP2;
        *(response_buffer++) = SDIO->RESP3;
        *(response_buffer++) = SDIO->RESP4;
    }

    return 0;
}

static int sdio_switch_appmode_cmd55()
{
    int retry = 0x20;
    union sdio_status_conv converter;
    uint32_t response;
    do {
        //Execute Command and check for valid response
        sdio_send_cmd(55, (card_info.rca<<16)&0xFFFF0000, SHORT_ANS);

        if (!sdio_get_response(55, SHORT_ANS, &response))
        {
            //Response valid. Check if Card has accepted switch to application command mode
            converter.value = response;
            if (converter.statusstruct.APP_CMD == 1)
                return 0;
        }
    }while(--retry > 0);

    return -1;
}

enum acmd41_ret sdio_init_card_acmd41(uint8_t HCS){
    uint32_t response;
    int retry = 0x20;
    if (sdio_switch_appmode_cmd55()) return ACMD41_RESP_ERR;
    do {
        sdio_send_cmd(41, (HCS ? (1<<30) : 0) | (1<<28) | (1<<20) |(1<<21)|(1<<22) |(1<<23)|(1<<19), SHORT_ANS);
        if (!sdio_get_response(0xFF, SHORT_ANS, &response)) {
            if (response & OCS_BUSY) { // Card is ready... Who knows why this bit is called busy...
                if (response & OCS_CCS) {
                    return ACMD41_RESP_SDXC;
                } else {
                    return ACMD41_RESP_SDSC;
                }
            } else {
                return ACMD41_RESP_INIT;
            }
        }
    }while(--retry > 0);

    return ACMD41_RESP_ERR;
}

static int sdio_send_csd_cmd9(uint16_t rca, uint32_t *responsebuffer) {
    int timeout = 0x20;
    int res;

    do {
        sdio_send_cmd(9, (rca<<16)&0xFFFF0000, LONG_ANS);
        if (!(res = sdio_get_response(0xFF, LONG_ANS, responsebuffer))) {
            break;
        }
    } while(--timeout > 0);

    return res;
}

static void sdio_write_buffer(uint32_t dlen, uint32_t blklen, uint8_t *buff)
{
	int count;
	int byte_count;
	int byte_max;
	uint32_t fifo;

	SDIO->DLEN = dlen;

	/* Init Transfer */
	SDIO->ICR = SDIO_ICR_CCRCFAILC | SDIO_ICR_DCRCFAILC | SDIO_ICR_CTIMEOUTC | SDIO_ICR_DTIMEOUTC |
					SDIO_ICR_TXUNDERRC | SDIO_ICR_RXOVERRC | SDIO_ICR_CMDRENDC | SDIO_ICR_CMDSENTC | SDIO_ICR_DATAENDC |
					SDIO_ICR_STBITERRC | SDIO_ICR_DBCKENDC | SDIO_ICR_SDIOITC | SDIO_ICR_CEATAENDC;
	SDIO->DCTRL = (blklen<<4) | SDIO_DCTRL_DTEN;

	for (count = 0; count < dlen; count += 4) {
		fifo = 0;

		if ((dlen - count) < 4)
			byte_max = dlen - count;
		else
			byte_max = 4;

		for (byte_count = 0; byte_count < byte_max; byte_count++)
		{
			fifo >>= 8;
			fifo |= (((uint32_t)*(buff++)) << 24) & 0xFF000000;
		}

		while (SDIO->STA & SDIO_STA_TXFIFOF);

		SDIO->FIFO = fifo;

	}

	while (SDIO->STA & SDIO_STA_TXACT);

}

static int sdio_send_write_block_cmd24(uint32_t addr)
{
    return -1;
}

static int sdio_check_status_register_cmd13(uint16_t rca, uint32_t *status)
{
    int timeout = 0x20;
    uint32_t response;
    int res;
    do {
        sdio_send_cmd(13, (rca<<16)&0xFFFF0000, SHORT_ANS);
        if (!(res = sdio_get_response(13, SHORT_ANS, &response))) {
            *status = response;
            break;
        }
    } while(--timeout > 0);

    return res;
}

static int sdio_send_bus_width_acmd6(uint8_t bus_width)
{
    uint32_t response;
    int retry = 0x20;
    union sdio_status_conv status;
    int ret;
    if (sdio_switch_appmode_cmd55()) return -1;
    do {
        sdio_send_cmd(0x6, (bus_width == 4 ? 0x2 : 0x0), SHORT_ANS);
        if (!(ret = sdio_get_response(0x6, SHORT_ANS, &response))) {
            status.value = response;
            return 0;
        }

    } while (--retry > 0);

    return ret;

}

static int sdio_get_sector_count(uint16_t rca, uint32_t *sector_count)
{
    uint32_t csd[4];
    int res;
    uint32_t size, mult, read_len, csd_rev;

    if ((res = sdio_send_csd_cmd9(rca, csd))) {
        return -1;
    }

    csd_rev = ((csd[0] >> 30) & (0x3));

    if (csd_rev == 0) { // SD v1 Card
        size = ((csd[1] & 0x3FF) <<2) | (((csd[2]) & ((1<<31) | (1<<30)))>>30);
        mult = ((csd[2] & ((1<<17)|(1<<16)|(1<<15)))>>15);
        read_len = (1<<((csd[1] & ((1<<19)|(1<<18)|(1<<17)|(1<<16)))>>16));
        *sector_count = (((size +1)*(1<<(mult+2))*read_len) >> BLOCKSIZE);
    } else if (csd_rev == 1) { // SD v2 Card
        size = (((csd[1] & 0x3F)<<16) | ((csd[2] & 0xFFFF0000) >> 16));
        *sector_count = (size << (19-BLOCKSIZE));
    }

    return 0;
}

static void sdio_switch_prescaler(uint8_t clkdiv)
{
    uint32_t reg;
    reg = SDIO->CLKCR;
    /* Clear prescaler */
    reg &= ~SDIO_CLKCR_CLKDIV;
    /* Set bits */
    reg |= (SDIO_CLKCR_CLKDIV & clkdiv);
    SDIO->CLKCR = reg;
}

/**
 * @brief initDetectandProtectionPins
 */
static void sdio_init_detect_pins()
{
#if SDIO_ENABLE_WRITEPROT==1
    WRITEPROT_PORT->PUPDR |= ((WRITEPROT_PULLUP ? 1 : 0)<<WRITEPROT_PIN*2);
#endif /* SDIO_ENABLE_WRITEPROT */
#if SDIO_ENABLE_INS==1
    INS_PORT->PUPDR |= ((INS_PULLUP? 1 : 0)<<INS_PIN*2);
#endif /* SDIO_ENABLE_INS */
    __DSB();
}

static void sdio_init_hw()
{
    //Init Clocks
    RCC->AHB1ENR    |= PORTCLKMASK | RCC_AHB1ENR_DMA2EN;
    RCC->APB2ENR    |= RCC_APB2ENR_SDIOEN;
    //Init Alternate Functions
    CLKPORT->MODER  |= (2<<CLKPIN*2);
    D0PORT->MODER   |= (2<<D0PIN*2);
    D0PORT->PUPDR   |= (1<<D0PIN*2);
    CMDPORT->MODER  |= (2<<CMDPIN*2);
    CMDPORT->PUPDR	|= (1<<CMDPIN*2);
#if BUSWIDTH==4
    D1PORT->MODER   |= (2<<D1PIN*2);
    D1PORT->PUPDR   |= (1<<D1PIN*2);
    D2PORT->MODER   |= (2<<D2PIN*2);
    D2PORT->PUPDR   |= (1<<D2PIN*2);
    D3PORT->MODER   |= (2<<D3PIN*2);
    D3PORT->PUPDR   |= (1<<D3PIN*2);
#endif
    //CLKPORT->AFR[(CLKPIN < 8 ? 0 : 1)] |= ALTFUNC << ((CLKPIN < 8 ? CLKPIN : (CLKPIN - 8)) * 4);
    SETAF(CLKPORT,  CLKPIN, ALTFUNC);
    SETAF(CMDPORT, CMDPIN, ALTFUNC);
    SETAF(D0PORT,   D0PIN,  ALTFUNC);
#if BUSWIDTH==4
    SETAF(D1PORT, D1PIN,  ALTFUNC);
    SETAF(D2PORT,   D2PIN,  ALTFUNC);
    SETAF(D3PORT,   D3PIN,  ALTFUNC);
#endif


    //Init Module

    //Set CLK Control Register
    SDIO->CLKCR = (HW_FLOW<<14) | ((BUSWIDTH == 4 ? 1 : 0)<<11) | SDIO_CLKCR_CLKEN |
            (INITCLK & SDIO_CLKCR_CLKDIV);

    //Set Data Timeout
    SDIO->DTIMER = DTIMEOUT;

    //Set Data Parameters
    //SDIO->DCTRL = (BLOCKSIZE << 4) | SDIO_DCTRL_DMAEN;
    //Set Power Register: Power up Card CLK
    SDIO->POWER = SDIO_POWER_PWRCTRL_0 | SDIO_POWER_PWRCTRL_1;

}

static int sdio_send_read_block_cmd17(uint32_t addr)
{
    uint32_t response;

    sdio_send_cmd(17, addr, SHORT_ANS);
    return sdio_get_response(17, SHORT_ANS, &response);
}

static int sdio_send_all_send_cid_cmd2()
{
    uint32_t response[4];
    int ret;
    int retry = 0x20;
    do {
        sdio_send_cmd(2, 0, LONG_ANS);
        if (!(ret = sdio_get_response(0xFF, LONG_ANS, response))) return 0;
    }while(retry-- > 0);
    return ret;
}

static int sdio_send_relative_address_cmd3(uint16_t* rca) {
    uint32_t response;
    int retry = 0x20;
    do {
        sdio_send_cmd(3, 0, SHORT_ANS);
        if (!sdio_get_response(3, SHORT_ANS, &response)) {
            // TODO: Do some *optional* checking
            *rca = ((response & 0xFFFF0000) >> 16);
            return 0;
        }
    }while(retry-- > 0);
    return -1;
}

static int sdio_send_go_idle_cmd0() {
    sdio_send_cmd(0, 0x0, NO_ANS);
    sdio_wait_cmd_sent();
    return 0;
}

static enum cmd8_ret sdio_send_iface_condition_cmd8() {
    uint32_t response;
    int res = 0;
    int retry = 0x20;
    do {
        sdio_send_cmd(8, 0x1CC, SHORT_ANS); // 3.3V supply requesR
        res = sdio_get_response(8, SHORT_ANS, &response);
        if (res == 0) {
            if (response & 0x100)
                return CMD8_VOLTAGE_ACCEPTED;
            else
                return CMD8_VOLTAGE_DENIED;
        }
    }while(retry-- > 0);

    return CMD8_RESP_TIMEOUT;
}

static int sdio_send_block_length_cmd16(uint32_t blocklen) {
    int timeout = 0x20;
    int res;
    uint32_t response;

    do {
        sdio_send_cmd(16, blocklen, SHORT_ANS);
        if (!(res = sdio_get_response(16, SHORT_ANS, &response))) {
            return 0;
        }
    }while(--timeout > 0);

    return res;
}

static int sdio_send_select_card_cmd7(uint16_t rca) {
    int timeout = 0x20;
    uint32_t response;
    union sdio_status_conv status;
    int res;

    /* Send CMD7. Selects card */
    do {
        sdio_send_cmd(7, (rca<<16)&0xFFFF0000, SHORT_ANS);
        if (!(res = sdio_get_response(7, SHORT_ANS, &response))) {
            break;
        }
    } while(--timeout > 0);

    /* Check, if card in in TRANS state */
    if (sdio_check_status_register_cmd13(rca, &(status.value)))
        res = -1;
    if (status.statusstruct.CURRENT_STATE != CURRENT_STATE_TRAN)
        res = -2;
    return res;
}

DSTATUS sdio_status()
{
    DSTATUS returnval = 0;

    if (sdio_check_inserted()) {
        returnval |= STA_NODISK;
    }
    if (card_info.type == CARD_NONE) {
        returnval |= STA_NOINIT;
    }
    if (sdio_check_write_protection()) {
        returnval |= STA_PROTECT;
    }
    return returnval;
}

DRESULT sdio_disk_ioctl(BYTE cmd, void* buff){
    DRESULT res = RES_OK;
    switch(cmd) {
    case GET_BLOCK_SIZE:
        *((DWORD*)buff) = (DWORD)0x01;
        break;
    case GET_SECTOR_SIZE:
        *((WORD*)buff) = (WORD)(1<<BLOCKSIZE);
        break;
    case GET_SECTOR_COUNT:
        if (card_info.type != CARD_NONE) {
            *((DWORD*)buff) = (DWORD)card_info.sector_count;
        } else {
            res = RES_ERROR;
        }
        break;
    case CTRL_SYNC:
        res = RES_OK;
        //No cache
        //Nothing to do
        break;
    default:
        res = RES_PARERR;
        break;
    }
    return res;
}

DWORD __attribute__((weak)) get_fattime()
{
    return (1<<16) | (1<<24); // return Jan. 1st 1980 00:00:00
}

DSTATUS sdio_initialize(){
    int timeout = 0x3000;
    enum cmd8_ret res8;
    enum acmd41_ret resa41;
    uint8_t hcs_flag = 0;
    card_info.rca = 0;
    card_info.type = CARD_NONE;
    enum sdio_card_type detected_card = CARD_NONE;

    sdio_init_hw();
    sdio_wait_ms(2);
    sdio_init_detect_pins();
    if (sdio_check_inserted()) {
        return STA_NOINIT | STA_NODISK;
    }

    sdio_send_go_idle_cmd0();
    sdio_wait_ms(2);
    res8 = sdio_send_iface_condition_cmd8();
    switch (res8) {
    case CMD8_VOLTAGE_ACCEPTED: // SDV2 Card
        hcs_flag = 1;
        break;
    case CMD8_VOLTAGE_DENIED: // should not happen
        return STA_NOINIT;
        break;
    case CMD8_RESP_TIMEOUT: // SDV1 Card
        hcs_flag=0;
        break;
    default:
        return STA_NOINIT;
        break;
    }

    do {
        //SDIO_wait_ms(2);
        resa41 = sdio_init_card_acmd41(hcs_flag);
    } while((resa41 == ACMD41_RESP_INIT) && (--timeout > 0));

    switch (resa41) {
    case ACMD41_RESP_SDSC:
        detected_card = (hcs_flag ? SD_V2_SC : SD_V1);
        break;
    case ACMD41_RESP_SDXC:
        detected_card = SD_V2_HC;
        break;
    default:
        return STA_NOINIT;
        break;
    }

    if (sdio_send_all_send_cid_cmd2())
        return STA_NOINIT;

    if (sdio_send_relative_address_cmd3(&card_info.rca))
        return STA_NOINIT;
    if (sdio_get_sector_count(card_info.rca, &card_info.sector_count))
        return STA_NOINIT;
    if (sdio_send_select_card_cmd7(card_info.rca))
        return STA_NOINIT;

    if (sdio_send_block_length_cmd16((uint32_t)(1<<BLOCKSIZE)))
        return STA_NOINIT;

    //sdio_unlock_card();


    if (sdio_send_bus_width_acmd6(BUSWIDTH))
        return STA_NOINIT;

    sdio_switch_prescaler(WORKCLK);

    card_info.type = detected_card;

    if (sdio_check_write_protection()) {
        return STA_PROTECT;
    } else
        return 0;
}

DRESULT sdio_disk_read(BYTE *buff, DWORD sector, UINT count){
    uint32_t addr;
    uint32_t sdio_status;
    uint32_t fifo;
    uint32_t counter;

    addr = (card_info.type == SD_V2_HC ? (sector) : (sector*512));
    for (; count > 0; count--) {

        /* configure read DMA */
//		DMA2->LIFCR = 0xffffffff;
//		DMA2->HIFCR = 0xffffffff;
//		DMASTREAM->NDTR = 0;
//		DMASTREAM->FCR = DMA_SxFCR_FTH_0 | DMA_SxFCR_FTH_1 | DMA_SxFCR_DMDIS;
//		DMASTREAM->M0AR = (uint32_t)(buff);
//		DMASTREAM->PAR = (uint32_t)&(SDIO->FIFO);
//		DMASTREAM->CR = DMAP2M | DMA_SxCR_PL_1 | DMA_SxCR_PL_1;
//		DMASTREAM->CR |= DMA_SxCR_EN;

        SDIO->DLEN = (1 << BLOCKSIZE);

        SDIO->ICR = SDIO_ICR_CCRCFAILC | SDIO_ICR_DCRCFAILC | SDIO_ICR_CTIMEOUTC | SDIO_ICR_DTIMEOUTC |
                        SDIO_ICR_TXUNDERRC | SDIO_ICR_RXOVERRC | SDIO_ICR_CMDRENDC | SDIO_ICR_CMDSENTC | SDIO_ICR_DATAENDC |
                        SDIO_ICR_STBITERRC | SDIO_ICR_DBCKENDC | SDIO_ICR_SDIOITC | SDIO_ICR_CEATAENDC;
        SDIO->DCTRL = (BLOCKSIZE<<4) | SDIO_DCTRL_DTDIR | /*SDIO_DCTRL_DMAEN |*/ SDIO_DCTRL_DTEN;

        /* Init Transfer */
        if (sdio_send_read_block_cmd17(addr)) {
            return RES_ERROR;
        }

        counter = 0;
        while (counter < (1<<(BLOCKSIZE-2)) || !(SDIO->STA & (SDIO_STA_DBCKEND | SDIO_STA_DATAEND))) { // TODO: Handle errors
            if (SDIO->STA & (SDIO_STA_DCRCFAIL | SDIO_STA_DTIMEOUT | SDIO_STA_STBITERR))
            {
                return RES_ERROR;
            }

            if (SDIO->STA & SDIO_STA_RXDAVL) {
                counter++;
                fifo = SDIO->FIFO;
                *(buff++) = (BYTE)(fifo & 0xFF);
                fifo >>= 8;
                *(buff++) = (BYTE)(fifo & 0xFF);
                fifo >>= 8;
                *(buff++) = (BYTE)(fifo & 0xFF);
                fifo >>= 8;
                *(buff++) = (BYTE)(fifo & 0xFF);
            }

        }
        if (SDIO->STA & SDIO_STA_DCRCFAIL) return RES_ERROR;

        //while(DMASTREAM->CR & DMA_SxCR_EN);
        while(1) {
            __DSB();
            __DMB();
            sdio_status = SDIO->STA;
            if (sdio_status & SDIO_STA_DCRCFAIL) {
                return RES_ERROR;
            }
            if (sdio_status & SDIO_STA_DTIMEOUT) {
                return RES_ERROR;
            }

            if (sdio_status & SDIO_STA_DATAEND) {

                if (!(sdio_status & SDIO_STA_RXACT)) {
                    break;
                }
            }
        }


        if (card_info.type == SD_V2_HC) {
            addr++;
        } else {
            addr += (1<<BLOCKSIZE);
        }
    }
    return RES_OK;
}

/**
 * @brief SDIO_disk_write
 * @param buff
 * @param sector
 * @param count
 * @warning Not yet implemented
 * @return
 */
DRESULT sdio_disk_write(const BYTE *buff, DWORD sector, UINT count)
{
    return RES_ERROR;
}