/***********************************************************************************************************
* Authors: otavio.borges
* On: 2026-06-30
* Name: sd_card.c
* Description: Controls SD card R/W and commands. Using SPI2 peripheral.
*   Dev. Kit routes SD Card to SPI2 (MIBSPI2). Pins (and muxing) are listed as below:
*   - MISO - Ball D2 (SPI2SOMI);
*   - MOSI - Ball D1 (SPI2SIMO);
*   - CD (as NHET212) - Ball D3 (SPI2ENA);
*   - CS0 - Ball N3 (SPI2CS0);
*   - CLK - Ball E2 (SPI2CLK);
**********************************************************************************************************/

#define SPI_MB_LENGTH                 0x7FU
#define SD_CARD_DATA_TOKEN            0xFEU

#include <stdint.h>
#include <errno.h>

#include "sd_card.h"
#include "util.h"
#include "log.h"
#include "rtos_wrapper.h"

#include "ff.h"
#include "HL_sys_dma.h"
#include "HL_mibspi.h"
#include "HL_gio.h"
#include "HL_het.h"
#include "FreeRTOS.h"
#include "os_task.h"

static int _sd_card_transfer(const uint8_t *tx, uint8_t *rx, size_t len);
static int _sd_card_cmd(sd_cmd_t cmd, uint32_t argument, uint8_t *resp);
static void _sd_card_r12errno(uint8_t r1);
static int _sd_card_wait_for(uint8_t mask, uint8_t value, uint8_t *resp, uint32_t tries);
#ifndef NDEBUG
static int _sd_card_wait_for2(uint8_t mask, uint8_t value, uint8_t *resp, uint32_t tries);
#endif

sd_card_state_t sd_card_state = SD_CARD_STATE_OFF;
sd_card_type_t sd_card_type = SD_CARD_TYPE_UNKNOWN;

DEFINE_MUTEX(_mtx_dma_complete);
DEFINE_MUTEX(_mtx_spi_access);

static inline void _sd_card_spi_select(void)
{
    try_lock_mutex(_mtx_spi_access, portMAX_DELAY);
    SD_CARD_DEFAULT_SPI->PC5 = 1;
    DELAY_MS(1U);
}

static inline void _sd_card_spi_deselect(int extended)
{
    DELAY_MS(1U);
    SD_CARD_DEFAULT_SPI->PC4 = 1;
    if (extended)
        _sd_card_transfer(NULL, NULL, 32);
    else
        _sd_card_transfer(NULL, NULL, 1);
    try_unlock_mutex(_mtx_spi_access);
}

int sd_card_init(void)
{
    uint32_t start, timeout, gio_direction, idx;

    gioREG->GCR0 = 1;
    gioPORTA->DIR |= (1U);
    
    /* Configure Card Detect as input (on N2HET2[12]) */
    gio_direction = hetPORT2->DIR;
    gio_direction &= ~(1U << SD_CARD_PIN_CD);
    gioSetDirection(hetPORT2, gio_direction);

    if (_mtx_dma_complete == NULL)
    {
        CREATE_SEM(_mtx_dma_complete);
    }

    if (_mtx_spi_access == NULL)
    {
        CREATE_MUTEX(_mtx_spi_access);
    }

    SD_CARD_DEFAULT_SPI->GCR0 = 0U;
    SD_CARD_DEFAULT_SPI->GCR0 = 1U;

    /** enable MIBSPI2 multibuffered mode and enable buffer RAM */
    SD_CARD_DEFAULT_SPI->MIBSPIE = (SD_CARD_DEFAULT_SPI->MIBSPIE & 0xFFFFFFFEU) | 1U;

    /** MIBSPI2 master mode and clock configuration */
    SD_CARD_DEFAULT_SPI->GCR1 = (SD_CARD_DEFAULT_SPI->GCR1 & 0xFFFFFFFCU) | ((uint32)((uint32)1U << 1U)  /* CLOKMOD */
                  | 1U);  /* MASTER */

    /** MIBSPI2 enable pin configuration */
    SD_CARD_DEFAULT_SPI->INT0 = (SD_CARD_DEFAULT_SPI->INT0 & 0xFEFFFFFFU) | (uint32)((uint32)0U << 24U);  /* ENABLE HIGHZ */

    /** - Delays */
    SD_CARD_DEFAULT_SPI->DELAY = (uint32)((uint32)1U << 24U)  /* C2TDELAY */
                      | (uint32)((uint32)1U << 16U)  /* T2CDELAY */
                      | (uint32)((uint32)0U << 8U)   /* T2EDELAY */
                      | (uint32)((uint32)0U << 0U);  /* C2EDELAY */

    /** - Data Format 0 */
    SD_CARD_DEFAULT_SPI->FMT0 = (uint32)((uint32)0U << 24U)  /* wdelay */
                     | (uint32)((uint32)0U << 23U)  /* parity Polarity */
                     | (uint32)((uint32)0U << 22U)  /* parity enable */
                     | (uint32)((uint32)0U << 21U)  /* wait on enable */
                     | (uint32)((uint32)0U << 20U)  /* shift direction */
                     | (uint32)((uint32)0U << 17U)  /* clock polarity */
                     | (uint32)((uint32)1U << 16U)  /* clock phase */
                     | (uint32)((uint32)255U << 8U)  /* baudrate prescale */
                     | (uint32)((uint32)8U << 0U); /* data word length */

    /** - Data Format 1 */
    SD_CARD_DEFAULT_SPI->FMT1 = (uint32)((uint32)0U << 24U)  /* wdelay */
                     | (uint32)((uint32)0U << 23U)  /* parity Polarity */
                     | (uint32)((uint32)0U << 22U)  /* parity enable */
                     | (uint32)((uint32)0U << 21U)  /* wait on enable */
                     | (uint32)((uint32)0U << 20U)  /* shift direction */
                     | (uint32)((uint32)0U << 17U)  /* clock polarity */
                     | (uint32)((uint32)0U << 16U)  /* clock phase */
                     | (uint32)((uint32)74U << 8U)  /* baudrate prescale */
                     | (uint32)((uint32)8U << 0U); /* data word length */

    /** - Data Format 2 */
    SD_CARD_DEFAULT_SPI->FMT2 = (uint32)((uint32)0U << 24U)  /* wdelay */
                     | (uint32)((uint32)0U << 23U)  /* parity Polarity */
                     | (uint32)((uint32)0U << 22U)  /* parity enable */
                     | (uint32)((uint32)0U << 21U)  /* wait on enable */
                     | (uint32)((uint32)0U << 20U)  /* shift direction */
                     | (uint32)((uint32)0U << 17U)  /* clock polarity */
                     | (uint32)((uint32)0U << 16U)  /* clock phase */
                     | (uint32)((uint32)74U << 8U)  /* baudrate prescale */
                     | (uint32)((uint32)16U << 0U); /* data word length */

    /** - Data Format 3 */
    SD_CARD_DEFAULT_SPI->FMT3 = (uint32)((uint32)0U << 24U)  /* wdelay */
                     | (uint32)((uint32)0U << 23U)  /* parity Polarity */
                     | (uint32)((uint32)0U << 22U)  /* parity enable */
                     | (uint32)((uint32)0U << 21U)  /* wait on enable */
                     | (uint32)((uint32)0U << 20U)  /* shift direction */
                     | (uint32)((uint32)0U << 17U)  /* clock polarity */
                     | (uint32)((uint32)0U << 16U)  /* clock phase */
                     | (uint32)((uint32)74U << 8U)  /* baudrate prescale */
                     | (uint32)((uint32)16U << 0U); /* data word length */

    /** - Default Chip Select */
    SD_CARD_DEFAULT_SPI->DEF = (uint32)(0xFFU);

    /** - wait for buffer initialization complete before accessing MibSPI registers */
    SET_DELAY(start, 500U);
    while ((SD_CARD_DEFAULT_SPI->FLG & 0x01000000U) != 0U)
    {
        GET_TIME(timeout);
        if (timeout > start)
        {
            errno = ETIMEDOUT;
            return -1;
        }
    }

    /** enable MIBSPI RAM Parity */
    SD_CARD_DEFAULT_SPI->PAR_ECC_CTRL = (SD_CARD_DEFAULT_SPI->PAR_ECC_CTRL & 0xFFFFFFF0U) | (0x00000005U);

    /** - initialize transfer groups */
    SD_CARD_DEFAULT_SPI->TGCTRL[0U] = (uint32)((uint32)1U << 30U)  /* oneshot */
                           | (uint32)((uint32)0U << 29U)  /* pcurrent reset */
                           | (uint32)((uint32)TRG_ALWAYS << 20U)  /* trigger event */
                           | (uint32)((uint32)TRG_DISABLED << 16U)  /* trigger source */
                           | (uint32)((uint32)0U << 8U);  /* start buffer */

    SD_CARD_DEFAULT_SPI->TGCTRL[1U] = (uint32)((uint32)1U << 30U)  /* oneshot */
                           | (uint32)((uint32)0U << 29U)  /* pcurrent reset */
                           | (uint32)((uint32)TRG_ALWAYS << 20U)  /* trigger event */
                           | (uint32)((uint32)TRG_DISABLED << 16U)  /* trigger source */
                           | (uint32)((uint32)SPI_MB_LENGTH << 8U);  /* start buffer */

    SD_CARD_DEFAULT_SPI->TGCTRL[2U] = 0;
    SD_CARD_DEFAULT_SPI->TGCTRL[3U] = 0;
    SD_CARD_DEFAULT_SPI->TGCTRL[4U] = 0;
    SD_CARD_DEFAULT_SPI->TGCTRL[5U] = 0;
    SD_CARD_DEFAULT_SPI->TGCTRL[6U] = 0;
    SD_CARD_DEFAULT_SPI->TGCTRL[7U] = 0;
    SD_CARD_DEFAULT_SPI->TGCTRL[8U] = 0;

    SD_CARD_DEFAULT_SPI->LTGPEND = (uint32_t)(SPI_MB_LENGTH << 8U);

    /** - initialize buffer ram */
    for (idx = 0; idx < SPI_MB_LENGTH; idx++)
    {
        SD_CARD_DEFAULT_RAM->tx[idx].control = (uint16)((uint16)5U << 13U)  /* buffer mode */
                                      | (uint16)((uint16)0U << 12U)  /* chip select hold */
                                      | (uint16)((uint16)0U << 10U)  /* enable WDELAY */
                                      | (uint16)((uint16)0U << 11U)  /* lock transmission */
                                      | (uint16)((uint16)0U << 8U)  /* data format */
                                      | ((uint16)(~((uint16)0xFFU ^ (uint16)CS_NONE)) & (uint16)0x00FFU);  /* chip select */
    }

    SD_CARD_DEFAULT_SPI->TGITENST = (1U << 16U);
    /** - set interrupt levels */
    SD_CARD_DEFAULT_SPI->LVL = (uint32)((uint32)0U << 9U)  /* TXINT */
                    | (uint32)((uint32)0U << 8U)  /* RXINT */
                    | (uint32)((uint32)0U << 6U)  /* OVRNINT */
                    | (uint32)((uint32)0U << 4U)  /* BITERR */
                    | (uint32)((uint32)0U << 3U)  /* DESYNC */
                    | (uint32)((uint32)0U << 2U)  /* PARERR */
                    | (uint32)((uint32)0U << 1U)  /* TIMEOUT */
                    | (uint32)((uint32)0U << 0U); /* DLENERR */

    /** - clear any pending interrupts */
    SD_CARD_DEFAULT_SPI->FLG |= 0xFFFFU;

    /** - enable interrupts */
    SD_CARD_DEFAULT_SPI->INT0 = (SD_CARD_DEFAULT_SPI->INT0 & 0xFFFF0000U)
                     | (uint32)((uint32)0U << 9U)  /* TXINT */
                     | (uint32)((uint32)0U << 8U)  /* RXINT */
                     | (uint32)((uint32)0U << 6U)  /* OVRNINT */
                     | (uint32)((uint32)0U << 4U)  /* BITERR */
                     | (uint32)((uint32)0U << 3U)  /* DESYNC */
                     | (uint32)((uint32)0U << 2U)  /* PARERR */
                     | (uint32)((uint32)0U << 1U)  /* TIMEOUT */
                     | (uint32)((uint32)0U << 0U); /* DLENERR */

    /** @b initialize @b MIBSPI2 @b Port */

    /** - MIBSPI2 Port output values */
    SD_CARD_DEFAULT_SPI->PC3 = (uint32)((uint32)1U << 0U)  /* SCS[0] */
                    | (uint32)((uint32)1U << 1U)  /* SCS[1] */
                    | (uint32)((uint32)0U << 8U)  /* ENA */
                    | (uint32)((uint32)0U << 9U)  /* CLK */
                    | (uint32)((uint32)0U << 10U)  /* SIMO */
                    | (uint32)((uint32)0U << 11U); /* SOMI */

    /** - MIBSPI2 Port direction */
    SD_CARD_DEFAULT_SPI->PC1 = (uint32)((uint32)1U << 0U)  /* SCS[0] */
                    | (uint32)((uint32)1U << 1U)  /* SCS[1] */
                    | (uint32)((uint32)0U << 8U)  /* ENA */
                    | (uint32)((uint32)1U << 9U)  /* CLK */
                    | (uint32)((uint32)1U << 10U)  /* SIMO */
                    | (uint32)((uint32)0U << 11U); /* SOMI */

    /** - MIBSPI2 Port open drain enable */
    SD_CARD_DEFAULT_SPI->PC6 = (uint32)((uint32)0U << 0U)  /* SCS[0] */
                    | (uint32)((uint32)0U << 1U)  /* SCS[1] */
                    | (uint32)((uint32)0U << 8U)  /* ENA */
                    | (uint32)((uint32)0U << 9U)  /* CLK */
                    | (uint32)((uint32)0U << 10U)  /* SIMO */
                    | (uint32)((uint32)0U << 11U); /* SOMI */


    /** - MIBSPI2 Port pullup / pulldown selection */
    SD_CARD_DEFAULT_SPI->PC8 = (uint32)((uint32)1U << 0U)  /* SCS[0] */
                    | (uint32)((uint32)1U << 1U)  /* SCS[1] */
                    | (uint32)((uint32)1U << 8U)  /* ENA */
                    | (uint32)((uint32)1U << 9U)  /* CLK */
                    | (uint32)((uint32)1U << 10U)  /* SIMO */
                    | (uint32)((uint32)1U << 11U); /* SOMI */


    /** - MIBSPI2 Port pullup / pulldown enable*/
    SD_CARD_DEFAULT_SPI->PC7 = (uint32)((uint32)0U << 0U)  /* SCS[0] */
                    | (uint32)((uint32)0U << 1U)  /* SCS[1] */
                    | (uint32)((uint32)0U << 8U)  /* ENA */
                    | (uint32)((uint32)0U << 9U)  /* CLK */
                    | (uint32)((uint32)1U << 10U)  /* SIMO */
                    | (uint32)((uint32)1U << 11U); /* SOMI */


    /* MIBSPI2 set all pins to functional */
    SD_CARD_DEFAULT_SPI->PC0 = (uint32)((uint32)0U << 0U)  /* SCS[0] */
                    | (uint32)((uint32)0U << 1U)  /* SCS[1] */
                    | (uint32)((uint32)0U << 8U)  /* ENA */
                    | (uint32)((uint32)1U << 9U)  /* CLK */
                    | (uint32)((uint32)1U << 10U)  /* SIMO */
                    | (uint32)((uint32)1U << 11U); /* SOMI */

    /** - Finally start MIBSPI2 */
    SD_CARD_DEFAULT_SPI->GCR1 = (SD_CARD_DEFAULT_SPI->GCR1 & 0xFEFFFFFFU) | 0x01000000U;

    return 0;
}

void sd_card_power_off(void)
{
    f_unmount("0:");
    sd_card_state = SD_CARD_STATE_OFF;
}

int sd_card_detected(void)
{
    return gioGetBit(hetPORT2, SD_CARD_PIN_CD);
}

void sd_card_max_speed(void)
{
    SD_CARD_DEFAULT_SPI->FMT0 &= ~(uint32)(0xFFU << 8U);

    /* MCLK = 75Mhz - PRESCALE = 74 -> 75/(74+1) = 1MHz */
    SD_CARD_DEFAULT_SPI->FMT0 |= (uint32)(74U << 8U);
}

void sd_card_initial_clock(void)
{
    try_lock_mutex(_mtx_spi_access, portMAX_DELAY);
    SD_CARD_DEFAULT_SPI->PC4 = 1;
    (void)_sd_card_transfer(NULL, NULL, 10);
    try_unlock_mutex(_mtx_spi_access);
}

int sd_card_send_cmd(sd_cmd_t cmd, uint32_t argument, uint8_t *resp)
{
    int result;

    _sd_card_spi_select();
    if (_sd_card_wait_for(0xFFU, 0xFFU, NULL, 100U) < 0)
    {
        _sd_card_spi_deselect(0);
        errno = ENOMSG;
        return -1;
    }

    result = _sd_card_cmd(cmd, argument, resp);

    _sd_card_spi_deselect(0);
    return result;
}

int sd_card_send_acmd(sd_cmd_t cmd, uint32_t argument, uint8_t expected_r1)
{
    uint8_t resp;

    _sd_card_spi_select();
    if (_sd_card_wait_for(0xFFU, 0xFFU, NULL, 100U) < 0)
    {
        _sd_card_spi_deselect(0);
        errno = ENOMSG;
        return -1;
    }

    if (_sd_card_cmd(CMD55, 0, &resp) < 0)
    {
        _sd_card_spi_deselect(0);
        return -1;
    }

    _sd_card_transfer(NULL, NULL, 1);
    if (_sd_card_cmd(cmd, argument, &resp) < 0)
    {
        _sd_card_spi_deselect(0);
        return -1;
    }

    if (resp != expected_r1)
    {
        _sd_card_r12errno(resp);

        _sd_card_spi_deselect(0);
        return -1;
    }

    _sd_card_spi_deselect(0);
    return 0;
}

int sd_card_get_ocr(sd_cmd_t cmd, uint32_t argument, uint8_t *resp, uint32_t *ocr)
{
    _sd_card_spi_select();
    if (_sd_card_wait_for(0xFFU, 0xFFU, NULL, 100U) < 0)
    {
        _sd_card_spi_deselect(0);
        errno = ENOMSG;
        return -1;
    }

    if (_sd_card_cmd(cmd, argument, resp) < 0)
    {
        _sd_card_spi_deselect(0);
        return -1;
    }

    /* We received R1, read R3 or R7 word */
    if (_sd_card_transfer(NULL, (uint8_t *)ocr, sizeof(uint32_t)) < 0)
    {
        _sd_card_spi_deselect(0);
        return -1;
    }

    _sd_card_spi_deselect(0);
    return 0;
}

int sd_card_read_block(block_type_t block, uint32_t sector, uint8_t *buffer)
{
    sd_cmd_t cmd;
    uint8_t token;
    uint32_t argument;
    size_t blocklen = SD_CARD_BLOCK_LENGTH;

    _sd_card_spi_select();
    _sd_card_transfer(NULL, NULL, 8); /* dummy clocks */

    if (_sd_card_wait_for(0xFFU, 0xFFU, NULL, 100U) < 0)
    {
        _sd_card_spi_deselect(0);
        errno = ENOMSG;
        return -1;
    }

    if (block == BLOCK_TYPE_CSD)
    {
        cmd = CMD9;
        blocklen = 16U;
        argument = 0;
    }
    else if (block == BLOCK_TYPE_CID)
    {
        cmd = CMD10;
        blocklen = 16U;
        argument = 0;
    }
    else
    {
        cmd = CMD17;
        argument = sector;
    }

    if (_sd_card_cmd(cmd, argument, &token) < 0)
    {
        _sd_card_spi_deselect(0);
        return -1;
    }

    if (token)
    {
        _sd_card_spi_deselect(0);

        _sd_card_r12errno(token);
        return -1;
    }

    if (_sd_card_wait_for(0xFFU, 0xFEU, &token, 100U) < 0)
    {
        _sd_card_spi_deselect(0);

        errno = EIO;
        return -1;
    }

    // DELAY_MS(1U); /* this is DANGEROUS! why? */
    if (_sd_card_transfer(NULL, buffer, blocklen) < 0)
    {
        _sd_card_spi_deselect(0);
        return -1;
    }

    /* discard CRC */
    _sd_card_transfer(NULL, NULL, 2);

    _sd_card_spi_deselect(0);
    return 0;
}

int write_bug_part = 0;
int sd_card_write_block(uint32_t sector, const uint8_t *buffer)
{
    /* token is a bit longer, preceding 0xFF to give some time for the SD card to prepare */
    uint8_t resp, token[4];
    int tries = 5, rtn = SD_CARD_DATA_TOKEN;
    uint8_t proper_resp = 0xFF;

    _sd_card_spi_select();
    _sd_card_transfer(NULL, NULL, 8); /* dummy clocks */
    if (_sd_card_wait_for(0xFFU, 0xFFU, NULL, 100U) < 0)
    {
        _sd_card_spi_deselect(0);
        errno = ENOMSG;
        return -1;
    }

    memset(token, 0xFFU, 4U);
    token[3] = SD_CARD_DATA_TOKEN;

    do
    {
        if (_sd_card_cmd(CMD24, sector, &resp) < 0)
        {
            _sd_card_spi_deselect(0);
            write_bug_part = 1;
            return -1;
        }

        if (resp)
        {
            SD_CARD_DEFAULT_SPI->PC4 = 1;
            if (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING)
            {
                vTaskDelay(10U);
            }
            else
            {
                util_delay(10000UL);
            }
            SD_CARD_DEFAULT_SPI->PC5 = 1;
        }
        else
        {
            break;
        }
    } while (--tries);

    if (resp)
    {
        _sd_card_spi_deselect(0);

        _sd_card_r12errno(resp);
        write_bug_part = 2;
        return -1;
    }

    // DELAY_MS(1U);  /*adding this delay made things weird. Is preemption between these two points scxrewing?*/
    if (_sd_card_transfer(token, NULL, 4) < 0)
    {
        _sd_card_spi_deselect(0);

        write_bug_part = 3;
        return -1;
    }

    /* Transmit data proper */
    if (_sd_card_transfer(buffer, NULL, SD_CARD_BLOCK_LENGTH) < 0)
    {
        _sd_card_spi_deselect(0);

        write_bug_part = 4;
        return -1;
    }

    /* discard CRC */
    _sd_card_transfer(NULL, NULL, 2);

    /* Get Data Response and wait for busy flag */

    if (_sd_card_wait_for(0x11, 0x01, &proper_resp, 50U) < 0)
    {
        _sd_card_spi_deselect(0);

        write_bug_part = 5;
        return -1;
    }

    switch (proper_resp & 0x0FU)
    {
    case 0x0BU:
        errno = EBADMSG;
        rtn = -1;
        write_bug_part = 6;
        break;
    case 0x0DU:
        errno = EIO;
        rtn = -1;
        write_bug_part = 7;
        break;
    }

    // printf("Busy flag: ");
    if (_sd_card_wait_for(0xFFU, 0xFFU, &resp, 2000U) < 0)
    {
        errno = ETIMEDOUT;
        write_bug_part = 8;
        rtn = -1;
    }

    /* Some extra clocks after the busy flag is cleared.
     * Although once MISO it's pulled to high, eventually a write attempt would crash (after many successful operations)
     *   Adding some additional transfers after flag is clear seems to give the SD card enough respite to continuously
     *   operate.
     */
    for (tries = 0; tries < 32; tries++)
    {
        _sd_card_transfer(NULL, NULL, 1U);
        DELAY_MS(1U);
    }
    _sd_card_spi_deselect(1);
    return rtn;
}

#if defined(__GNUC__)
#pragma GCC push_options
#pragma GCC target("general-regs-only")
__attribute__((interrupt))
void mibspi2HighInterrupt(void)
#else
#pragma INTERRUPT(mibspi2HighInterrupt, IRQ)
    void mibspi2HighInterrupt(void)
#endif
{
    BaseType_t hptw;

    SD_CARD_DEFAULT_SPI->TGINTFLG = (1U << 16U);
    xSemaphoreGiveFromISR(_mtx_dma_complete, &hptw);
    portYIELD_FROM_ISR(hptw);
}
#if defined(__GNUC__)
#pragma GCC pop_options
#endif

static int _sd_card_transfer(const uint8_t *tx, uint8_t *rx, size_t len)
{
    size_t idx, chunk, offset = 0;
    volatile uint16_t rx_buf;

    while (offset < len)
    {
        if ((len - offset) > SPI_MB_LENGTH)
            chunk = SPI_MB_LENGTH;
        else
            chunk = (len - offset);

        for (idx = 0; idx < chunk; idx++)
        {
            if (tx)
                SD_CARD_DEFAULT_RAM->tx[idx].data = (uint16_t)tx[(idx + offset)];
            else
                SD_CARD_DEFAULT_RAM->tx[idx].data = (uint16_t)0xFFU;
        }

        /* Clear semaphore pending signals, just in case */
        xSemaphoreTake(_mtx_dma_complete, 0);

        /* Define the buffer size to be written */
        taskENTER_CRITICAL();
        SD_CARD_DEFAULT_SPI->TGCTRL[1] &= ~(0xFFU << 8U);
        SD_CARD_DEFAULT_SPI->TGCTRL[1] |= (chunk << 8U);
        SD_CARD_DEFAULT_SPI->LTGPEND = (chunk << 8U);
        SD_CARD_DEFAULT_SPI->TGINTFLG = 0xFFFF0000U;
        SD_CARD_DEFAULT_SPI->TGCTRL[0] |= (1U << 31U);
        taskEXIT_CRITICAL();

        if (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING)
        {
            if (xSemaphoreTake(_mtx_dma_complete, pdMS_TO_TICKS(10000U)) == pdFALSE)
            {
                errno = ETIMEDOUT;
                return -1;
            }
        }

        /* Wait until SPI flag is done */
        taskENTER_CRITICAL();
        while ((SD_CARD_DEFAULT_SPI->LTGPEND & (1U << 24U)))
            NOP();
        taskEXIT_CRITICAL();

        if (rx)
        {
            for (idx = 0; idx < chunk; idx++)
            {
                rx_buf = SD_CARD_DEFAULT_RAM->rx[idx].data;
                rx[(idx + offset)] = rx_buf;
            }
        }

        offset += chunk;
    }

    /* For now, always return the full length */
    return len;
}

static int _sd_card_cmd(sd_cmd_t cmd, uint32_t argument, uint8_t *resp)
{
    /* buffer long enough to hold the whole command sequence: 1 cmd, 4 for argument, 1 for CRC, 9 Ncr (max. wait), 1 for resp */
    static const size_t command_buffer_length = 1U + 4U + 1U;
    uint8_t tx_buff[command_buffer_length];
    uint8_t rx_buff[command_buffer_length];

    memset(rx_buff, 0xFFU, command_buffer_length);
    memset(tx_buff, 0xFFU, command_buffer_length);
    tx_buff[0] = 0x40U | cmd;
    memcpy(tx_buff + 1, &argument, 4);
    if (cmd == CMD0)
        tx_buff[5] = 0x95U;
    else if (cmd == CMD8)
        tx_buff[5] = 0x87U;
    else
        tx_buff[5] = 0xFFU;

    if (_sd_card_transfer(tx_buff, rx_buff, command_buffer_length) < 0)
    {
        return -1;
    }

    /* wait and check for the response */
    if (_sd_card_wait_for(0x80U, 0, resp, 5000U) < 0)
    {
        errno = ETIMEDOUT;
        return -1;
    }

    return 0;
}

static void _sd_card_r12errno(uint8_t r1)
{
    if (r1 & 0x01)
        errno = EAGAIN;
    else if (r1 & 0x02)
        errno = EINTR;
    else if (r1 & 0x04)
        errno = ENOTSUP;
    else if (r1 & 0x08)
        errno = EBADMSG;
    else if (r1 & 0x10)
        errno = EPROTO;
    else
        errno = EINVAL;
}

static int _sd_card_wait_for(uint8_t mask, uint8_t value, uint8_t *resp, uint32_t tries)
{
    uint8_t pointer;
    /* if no resp, then make it internal */
    if (resp == NULL)
        resp = &pointer;

    do
    {
        if (_sd_card_transfer(NULL, resp, 1) < 0)
        {
            /* force an error and break */
            return -1;
        }

        if (((*resp)  & mask) == value)
        {
            break;
        }
        else
        {
            DELAY_MS(1U);
        }
    }
    while (--tries);

    if (tries == 0)
    {
        errno = ETIMEDOUT;
        return -1;
    }

    return 0;
}

#ifndef NDEBUG
static uint8_t super_pointer[30720U];
static int _sd_card_wait_for2(uint8_t mask, uint8_t value, uint8_t *resp, uint32_t tries)
{
    int idx = 0, is_ok = 0;
    /* if no resp, then make it internal */
    do
    {
        if (_sd_card_transfer(NULL, (super_pointer + idx), 1) < 0)
        {
            /* force an error and break */
            return -1;
        }

        if ((super_pointer[idx++] & mask) == value)
        {
            if (resp)
                *resp = super_pointer[idx - 1];

            is_ok = 1;
        }
        else
        {
            DELAY_MS(1U);
        }
    }
    while (--tries);

    // printf("Responses:");
    // for (jdx = 0; jdx < idx; jdx++)
    //     printf(" %02X", super_pointer[jdx]);
    // printf("\n");

    if (is_ok == 0)
    {
        errno = ETIMEDOUT;
        return -1;
    }

    return 0;
}
#endif