sd.c

Go to the documentation of this file.

 
#include <stdbool.h>
#include <string.h>
 
#include "sd.h"
#include "common.h"
#include "err.h"
#include "init.h"
#include "io.h"
#include "log.h"
#include "modules.h"
#include "cmd.h"
#include "configuration.h"
#include "stm32f401re.h"
#include "stm32f401re_gpio.h"
#include "stm32f401re_rcc.h"
#include "stm32f401re_spi.h"
#include "arm_cortex_m4_systick.h"

typedef struct {
    uint8_t r1;     
    uint8_t r3[4];  
    uint8_t r7[4];  
} CMD_RESPONSE_ts;

typedef enum {
    OCR_1_7V_1_6V     = 4,  
    OCR_1_8V_1_7V,           
    OCR_1_9V_1_8V,           
    OCR_2_0V_1_9V,           
    OCR_2_1V_2_0V,           
    OCR_2_2V_2_1V,           
    OCR_2_3V_2_2V,           
    OCR_2_4V_2_3V,           
    OCR_2_5V_2_4V,           
    OCR_2_6V_2_5V,           
    OCR_2_7V_2_6V,           
    OCR_2_8V_2_7V,           
    OCR_2_9V_2_8V,           
    OCR_3_0V_2_9V,           
    OCR_3_1V_3_0V,           
    OCR_3_2V_3_1V,           
    OCR_3_3V_3_2V,           
    OCR_3_4V_3_3V,           
    OCR_3_5V_3_4V,           
    OCR_3_6V_3_5V,           
    OCR_CAPACITY_STATUS = 30, 
    OCR_PWRUP_STATUS          
} OCR_te;

typedef enum {
    SD_MIN_OPERATING_VOLTAGE_3_5V, 
    SD_MIN_OPERATING_VOLTAGE_3_4V, 
    SD_MIN_OPERATING_VOLTAGE_3_3V, 
    SD_MIN_OPERATING_VOLTAGE_3_2V, 
    SD_MIN_OPERATING_VOLTAGE_3_1V, 
    SD_MIN_OPERATING_VOLTAGE_3_0V, 
    SD_MIN_OPERATING_VOLTAGE_2_9V, 
    SD_MIN_OPERATING_VOLTAGE_2_8V, 
    SD_MIN_OPERATING_VOLTAGE_2_7V, 
    SD_MIN_OPERATING_VOLTAGE_2_6V, 
    SD_MIN_OPERATING_VOLTAGE_2_5V, 
    SD_MIN_OPERATING_VOLTAGE_2_4V, 
    SD_MIN_OPERATING_VOLTAGE_2_3V, 
    SD_MIN_OPERATING_VOLTAGE_2_2V, 
    SD_MIN_OPERATING_VOLTAGE_2_1V, 
    SD_MIN_OPERATING_VOLTAGE_2_0V, 
    SD_MIN_OPERATING_VOLTAGE_1_9V, 
    SD_MIN_OPERATING_VOLTAGE_1_8V, 
    SD_MIN_OPERATING_VOLTAGE_1_7V, 
    SD_MIN_OPERATING_VOLTAGE_1_6V  
} SD_MIN_OPERATING_VOLTAGE_te;

typedef enum {
    SD_MAX_OPERATING_VOLTAGE_3_6V, 
    SD_MAX_OPERATING_VOLTAGE_3_5V, 
    SD_MAX_OPERATING_VOLTAGE_3_4V, 
    SD_MAX_OPERATING_VOLTAGE_3_3V, 
    SD_MAX_OPERATING_VOLTAGE_3_2V, 
    SD_MAX_OPERATING_VOLTAGE_3_1V, 
    SD_MAX_OPERATING_VOLTAGE_3_0V, 
    SD_MAX_OPERATING_VOLTAGE_2_9V, 
    SD_MAX_OPERATING_VOLTAGE_2_8V, 
    SD_MAX_OPERATING_VOLTAGE_2_7V, 
    SD_MAX_OPERATING_VOLTAGE_2_6V, 
    SD_MAX_OPERATING_VOLTAGE_2_5V, 
    SD_MAX_OPERATING_VOLTAGE_2_4V, 
    SD_MAX_OPERATING_VOLTAGE_2_3V, 
    SD_MAX_OPERATING_VOLTAGE_2_2V, 
    SD_MAX_OPERATING_VOLTAGE_2_1V, 
    SD_MAX_OPERATING_VOLTAGE_2_0V, 
    SD_MAX_OPERATING_VOLTAGE_1_9V, 
    SD_MAX_OPERATING_VOLTAGE_1_8V, 
    SD_MAX_OPERATING_VOLTAGE_1_7V  
} SD_MAX_OPERATIING_VOLTAGE_te;

typedef enum {
    SD_ADDR_MODE_BYTE,  
    SD_ADDR_MODE_BLOCK  
} SD_ADDR_MODE_te;

typedef enum {
    SD_PWRUP_STATUS_BUSY, 
    SD_PWRUP_STATUS_READY 
} SD_PWRUP_STATUS_te;

typedef enum {
    SDSC, 
    SDHC  
} SD_TYPE_te;

typedef struct {
    uint32_t capacity_bytes;    
    uint32_t capacity_mb;       
    uint16_t block_len;         
    uint32_t block_count;       
    uint8_t  tran_speed;        
    uint16_t ccc;               
    uint8_t  csd_structure;     

    struct {
        uint16_t c_size;        
        uint8_t  c_size_mult;   
        uint8_t  read_bl_len;   
    } v1;

    struct {
        uint32_t c_size;        
    } v2;
 
    uint8_t taac;               
    uint8_t nsac;               
    uint8_t read_bl_partial;    
    uint8_t write_blk_misalign; 
    uint8_t read_blk_misalign;  
    uint8_t dsr_imp;            
    uint8_t erase_blk_en;       
    uint8_t sector_size;        
    uint8_t wp_grp_size;        
    uint8_t wp_grp_enable;      
    uint8_t r2w_factor;         
    uint8_t write_bl_len;       
    uint8_t write_bl_partial;   
    uint8_t file_format_grp;    
    uint8_t copy;               
    uint8_t perm_write_protect;
    uint8_t tmp_write_protect;  
    uint8_t file_format;        
    uint8_t wp_upc;             
    uint8_t crc;                
} SD_CSD_INFO_ts;

struct sd_handle_s {
    char name[CONFIG_SD_MAX_NAME_LEN];                  
    SPI_REGDEF_ts *spi_instance;                        
    GPIO_REGDEF_ts *sclk_gpio_port;                     
    GPIO_REGDEF_ts *cs_gpio_port;                       
    GPIO_REGDEF_ts *miso_gpio_port;                     
    GPIO_REGDEF_ts *mosi_gpio_port;                     
    GPIO_PIN_te sclk_gpio_pin;                          
    GPIO_PIN_te cs_gpio_pin;                            
    GPIO_PIN_te miso_gpio_pin;                          
    GPIO_PIN_te mosi_gpio_pin;                          
    GPIO_ALTERNATE_FUNCTION_te gpio_alternate_function; 
    SD_PWRUP_STATUS_te pwrup_status;                    
    SD_ADDR_MODE_te addr_mode;                          
    SD_TYPE_te type;                                    
    SD_MIN_OPERATING_VOLTAGE_te min_operating_voltage;  
    SD_MAX_OPERATIING_VOLTAGE_te max_operating_voltage; 
    uint32_t block_len;                                 
    uint32_t block_count;                               
    uint32_t capacity_mb;                               
    bool initialized;                                   
    bool in_use;                                        
};

struct internal_state_s {
    SD_HANDLE_ts sds[CONFIG_SD_MAX_OBJECTS];

    MODULES_te subsys;

    LOG_LEVEL_te log_level;

    uint32_t sd_num;

    bool initialized;

    bool started;
};

struct internal_state_s internal_state;
 
/* ---- Forward declarations for internal helpers ---- */
static ERR_te sd_go_idle_state(SD_HANDLE_ts *sd_handle);
static ERR_te sd_send_if_cond(SD_HANDLE_ts *sd_handle, bool *match_o, bool *no_resp_o);
static ERR_te sd_app_send_op_cond(SD_HANDLE_ts *sd_handle, uint32_t arg);
static ERR_te sd_read_ocr(SD_HANDLE_ts *sd_handle);
static ERR_te sd_send_op_cond(SD_HANDLE_ts *sd_handle);
static ERR_te sd_set_blocklen(SD_HANDLE_ts *sd_handle);
static ERR_te sd_read_csd(SD_HANDLE_ts *sd_handle);
static ERR_te decode_csd_v1(const uint8_t *csd_raw, SD_CSD_INFO_ts *csd_info_o);
static ERR_te decode_csd_v2(const uint8_t *csd_raw, SD_CSD_INFO_ts *csd_info_o);
static ERR_te sd_send_cmd(SPI_REGDEF_ts *spi_instance, uint8_t index, uint32_t arg, bool acmd, CMD_RESPONSE_ts *cmd_response_o);
static ERR_te sd_cease_comms(SD_HANDLE_ts *sd_handle, bool deinit);
 
/* ---- Forward declarations for command handlers ---- */
static ERR_te sd_cmd_list_handler(uint32_t argc, char **argv);
static ERR_te sd_cmd_info_handler(uint32_t argc, char **argv);

CMD_INFO_ts sd_cmds[] = {
    {
        .name = "list",
        .help = "Lists active sd objects, usage: sd list",
        .handler = sd_cmd_list_handler
    },
    {
        .name = "info",
        .help = "Shows sd object info, usage: sd info <sd object>",
        .handler = sd_cmd_info_handler
    }
};

CMD_CLIENT_INFO_ts sd_cmd_client_info = {
    .cmds_ptr = sd_cmds,
    .num_cmds = sizeof(sd_cmds) / sizeof(sd_cmds[0]),
    .log_level_ptr = &internal_state.log_level,
    .name = "sd"
};


ERR_te sd_init_subsys(void) {
    ERR_te err;
 
    if(internal_state.initialized) {
        return ERR_MODULE_ALREADY_INITIALIZED;
    }
 
    internal_state = (struct internal_state_s){ 0 };
 
    internal_state.log_level = LOG_LEVEL_INFO;
    internal_state.subsys = MODULES_SD;
    internal_state.initialized = true;
    internal_state.started = false;
 
    init_log();
    init_systick();
 
    err = cmd_register(&sd_cmd_client_info);
    if(err != ERR_OK) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_init_subsys: cmd_register error"
        );
        return err;
    }
    LOG_INFO(
        internal_state.subsys,
        internal_state.log_level,
        "sd_init_subsys: subsys initialized"
    );
 
    return ERR_OK;
}

ERR_te sd_deinit_subsys(void) {
    if(internal_state.initialized) {
        internal_state = (struct internal_state_s){ 0 };
 
        cmd_deregister(&sd_cmd_client_info);
    }
    else {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_deinit_subsys: subsys is not initialized"
        );
 
        return ERR_DEINITIALIZATION_FAILURE;
    }
    LOG_INFO(
        internal_state.subsys,
        internal_state.log_level,
        "sd_deinit_subsys: subsys deinitialized"
    );
 
    return ERR_OK;
}

ERR_te sd_start_subsys(void) {
    if(internal_state.initialized && !internal_state.started) {
        internal_state.started = true;
 
        LOG_INFO(
            internal_state.subsys,
            internal_state.log_level,
            "sd_start_subsys: subsys started"
        );
    }
    else {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_start_subsys: subsys not initialized or already started"
        );
 
        return ERR_UNKNOWN;
    }
 
    return ERR_OK;
}

ERR_te sd_stop_subsys(void) {
    if(internal_state.initialized && internal_state.started) {
        internal_state.started = false;
 
        LOG_INFO(
            internal_state.subsys,
            internal_state.log_level,
            "sd_stop_subsys: subsys stopped"
        );
    }
    else {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_stop_subsys: subsys not initialized or already already stopped"
        );
 
        return ERR_UNKNOWN;
    }
 
    return ERR_OK;
}

ERR_te sd_init_handle(SD_CFG_ts *sd_cfg, SD_HANDLE_ts **sd_handle_o) {
    ERR_te err;
    uint8_t free_index;
 
    if(!internal_state.initialized) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_init_handle: subsys not initialized"
        );
 
        return ERR_INITIALIZATION_FAILURE;
    }
 
    if(internal_state.sd_num == CONFIG_SD_MAX_OBJECTS) {
        LOG_CRITICAL(
            internal_state.subsys,
            internal_state.log_level,
            "sd_init_handle: subsystem out of memory space"
        );
 
        return ERR_NOT_ENOUGH_SPACE;
    }
 
    if(!sd_cfg) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_init_handle: invalid argument"
        );
        return ERR_INVALID_ARGUMENT;
    }
 
    bool found = false;
 
    for(uint32_t i = 0; i < CONFIG_SD_MAX_OBJECTS; i++) {
        if(!internal_state.sds[i].in_use) {
            free_index = i;
            found = true;
            break;
        }
    }
 
    if(!found)
        return ERR_NOT_ENOUGH_SPACE;
 
    GPIO_CFG_ts sd_sclk = { 0 };
    sd_sclk.port = sd_cfg->sclk_gpio_port;
    sd_sclk.pin = sd_cfg->sclk_gpio_pin;
    sd_sclk.mode = GPIO_MODE_ALTERNATE_FUNCTION;
    sd_sclk.alternate_function = sd_cfg->gpio_alternate_function;
    gpio_init(&sd_sclk);
 
    GPIO_CFG_ts sd_cs = { 0 };
    sd_cs.port = sd_cfg->cs_gpio_port;
    sd_cs.pin = sd_cfg->cs_gpio_pin;
    sd_cs.mode = GPIO_MODE_OUTPUT;
    gpio_init(&sd_cs);
 
    GPIO_CFG_ts sd_mosi = { 0 };
    sd_mosi.port = sd_cfg->mosi_gpio_port;
    sd_mosi.pin = sd_cfg->mosi_gpio_pin;
    sd_mosi.mode = GPIO_MODE_ALTERNATE_FUNCTION;
    sd_mosi.alternate_function = sd_cfg->gpio_alternate_function;
    gpio_init(&sd_mosi);
 
    GPIO_CFG_ts sd_miso = { 0 };
    sd_miso.port = sd_cfg->miso_gpio_port;
    sd_miso.pin = sd_cfg->miso_gpio_pin;
    sd_miso.mode = GPIO_MODE_ALTERNATE_FUNCTION;
    sd_miso.alternate_function = sd_cfg->gpio_alternate_function;
    gpio_init(&sd_miso);
 
    // Configure SPI
    SPI_CFG_ts sd_spi = { 0 };
    sd_spi.instance = sd_cfg->spi_instance;
    sd_spi.bit_first = SPI_BIT_FIRST_MSB;
    sd_spi.clock_phase = SPI_CLOCK_PHASE_FIRST_TRANSITION;
    sd_spi.clock_polarity = SPI_CLOCK_POLARITY_0_IDLE;
    sd_spi.data_frame_format = SPI_DATA_FRAME_FORMATE_8_BIT;
    sd_spi.slave_select_mode = SPI_SLAVE_SELECT_MODE_SW;
    sd_spi.mode = SPI_MODE_MASTER;
 
    IO_CFG_ts io_cs_conf = { 0 };
    IO_HANDLE_ts *io_cs_handle = { 0 };
    io_cs_conf.gpio_handle = &sd_miso;
    str_cpy(io_cs_conf.name, "sd_cs", get_str_len("sd_cs") + 1);
    io_init_subsys();
    io_init_handle(&io_cs_conf, &io_cs_handle);
    io_start_subsys();
 
    // Set SPI speed between 100 and 400 KHz for initialization, then initialize
    uint32_t spi_pclk = 0;
 
    if(sd_cfg->spi_instance == SPI1 || sd_cfg->spi_instance == SPI4) {
        spi_pclk = rcc_get_apb2_clk();
    }
    else if(sd_cfg->spi_instance == SPI2 || sd_cfg->spi_instance == SPI3) {
        spi_pclk = rcc_get_apb1_clk();
    }
 
    uint32_t spi_pclk_cpy = spi_pclk;
    uint32_t div_factor = 2;
    while(div_factor <= 256) {
        spi_pclk = spi_pclk / div_factor;
 
        if(spi_pclk >= 100000 && spi_pclk <= 400000) {
            break;
        }
 
        spi_pclk = spi_pclk_cpy;
        div_factor *= 2;
    }
 
    switch (div_factor) {
        case 2:   sd_spi.master_sclk_speed = SPI_MASTER_SCLK_SPEED_PCLK_DIV_2;   break;
        case 4:   sd_spi.master_sclk_speed = SPI_MASTER_SCLK_SPEED_PCLK_DIV_4;   break;
        case 8:   sd_spi.master_sclk_speed = SPI_MASTER_SCLK_SPEED_PCLK_DIV_8;   break;
        case 16:  sd_spi.master_sclk_speed = SPI_MASTER_SCLK_SPEED_PCLK_DIV_16;  break;
        case 32:  sd_spi.master_sclk_speed = SPI_MASTER_SCLK_SPEED_PCLK_DIV_32;  break;
        case 64:  sd_spi.master_sclk_speed = SPI_MASTER_SCLK_SPEED_PCLK_DIV_64;  break;
        case 128: sd_spi.master_sclk_speed = SPI_MASTER_SCLK_SPEED_PCLK_DIV_128; break;
        case 256: sd_spi.master_sclk_speed = SPI_MASTER_SCLK_SPEED_PCLK_DIV_256; break;
    }
 
    spi_init(&sd_spi);
 
    str_cpy(
        internal_state.sds[free_index].name,
        sd_cfg->name,
        get_str_len(sd_cfg->name) + 1
    );
 
    internal_state.sds[free_index].spi_instance = sd_cfg->spi_instance;
    internal_state.sds[free_index].sclk_gpio_port = sd_cfg->sclk_gpio_port;
    internal_state.sds[free_index].cs_gpio_port = sd_cfg->cs_gpio_port;
    internal_state.sds[free_index].sclk_gpio_pin = sd_cfg->sclk_gpio_pin;
    internal_state.sds[free_index].miso_gpio_port = sd_cfg->miso_gpio_port;
    internal_state.sds[free_index].mosi_gpio_port = sd_cfg->mosi_gpio_port;
    internal_state.sds[free_index].cs_gpio_pin = sd_cfg->cs_gpio_pin;
    internal_state.sds[free_index].sclk_gpio_pin = sd_cfg->sclk_gpio_pin;
    internal_state.sds[free_index].miso_gpio_pin = sd_cfg->miso_gpio_pin;
    internal_state.sds[free_index].mosi_gpio_pin = sd_cfg->mosi_gpio_pin;
    internal_state.sds[free_index].gpio_alternate_function = sd_cfg->gpio_alternate_function;
    internal_state.sds[free_index].in_use = true;
 
    *sd_handle_o = &internal_state.sds[free_index];
 
    internal_state.sd_num++;
 
    // Wait for >= 1 ms after supply reached at least 2.2V
    DELAY(CONFIG_SD_HW_INIT_WAIT_TIME);
 
    // Enable SPI communication
    spi_set_comm(sd_cfg->spi_instance, ENABLE);
 
    // 1. Start initialization — set CS high and send at least 74 dummy clocks
    gpio_write(sd_cfg->cs_gpio_port, sd_cfg->cs_gpio_pin, HIGH);
 
    uint8_t dummy_tx = 0xFF;
    for(uint8_t i = 0; i < 10; i++) {
        spi_send(sd_cfg->spi_instance, &dummy_tx, 1);
    }
 
    gpio_write(sd_cfg->cs_gpio_port, sd_cfg->cs_gpio_pin, LOW);
 
    // 2. Enter idle state
    err = sd_go_idle_state(&internal_state.sds[free_index]);
    if(err != ERR_OK) {
        LOG_CRITICAL(internal_state.subsys,
            internal_state.log_level,
            "sd_init_handle: failed to enter idle state, deinitializing handle"
        );
 
        sd_cease_comms(&internal_state.sds[free_index], true);
 
        return ERR_UNSUCCESFUL_ACTION;
    }
 
    // 3. Detect card type and version
    bool match = false;
    bool no_response = false;
 
    err = sd_send_if_cond(&internal_state.sds[free_index], &match, &no_response);
    if(err != ERR_OK) {
        LOG_CRITICAL(internal_state.subsys,
            internal_state.log_level,
            "sd_init_handle: failed to get SD card type, deinitializing handle"
        );
 
        sd_cease_comms(&internal_state.sds[free_index], true);
 
        return ERR_UNSUCCESFUL_ACTION;
    }
 
    // 3.1 SD Ver.2 — determine byte or block addressing from OCR
    if(match) {
        err = sd_app_send_op_cond(&internal_state.sds[free_index], 0x40000000);
        if(err != ERR_OK) {
            LOG_CRITICAL(internal_state.subsys,
                internal_state.log_level,
                "sd_init_handle: failed to initiate initialization of SD Ver.2, deinitializing handle"
            );
 
            sd_cease_comms(&internal_state.sds[free_index], true);
 
            return ERR_UNSUCCESFUL_ACTION;
        }
 
        err = sd_read_ocr(&internal_state.sds[free_index]);
        if(err != ERR_OK) {
            LOG_CRITICAL(internal_state.subsys,
                internal_state.log_level,
                "sd_init_handle: failed to obtain SD Ver 2. information, deinitializing handle"
            );
 
            return ERR_UNSUCCESFUL_ACTION;
        }
    }
    // 3.2 SD Ver.1 or MMC Ver.3
    else if(no_response) {
        err = sd_app_send_op_cond(&internal_state.sds[free_index], 0);
        // SD Ver.1
        if(err == ERR_OK) {
            internal_state.sds[free_index].addr_mode = SD_ADDR_MODE_BYTE;
        }
        // MMC Ver.3 or unknown
        else if(err == ERR_TIMEOUT) {
            err = sd_send_op_cond(&internal_state.sds[free_index]);
            // Unknown card
            if(err != ERR_OK) {
                LOG_CRITICAL(internal_state.subsys,
                    internal_state.log_level,
                    "sd_init_handle: unknown card, deinitializing handle"
                );
 
                sd_cease_comms(&internal_state.sds[free_index], true);
 
                return ERR_UNSUCCESFUL_ACTION;
            }
            // MMC Ver.3
            else {
                internal_state.sds[free_index].addr_mode = SD_ADDR_MODE_BYTE;
            }
        }
        // Unknown card
        else {
            LOG_CRITICAL(internal_state.subsys,
                internal_state.log_level,
                "sd_init_handle: unknown card, deinitializing handle"
            );
 
            sd_cease_comms(&internal_state.sds[free_index], true);
 
            return ERR_UNSUCCESFUL_ACTION;
        }
    }
 
    if(internal_state.sds[free_index].addr_mode == SD_ADDR_MODE_BYTE) {
        err = sd_set_blocklen(&internal_state.sds[free_index]);
        if(err != ERR_OK) {
            LOG_CRITICAL(internal_state.subsys,
                internal_state.log_level,
                "sd_init_handle: failed to initialize, deinitializing handle"
            );
 
            sd_cease_comms(&internal_state.sds[free_index], true);
 
            return ERR_UNSUCCESFUL_ACTION;
        }
    }
 
    internal_state.sds[free_index].initialized = true;
 
    LOG_INFO(
        internal_state.subsys,
        internal_state.log_level,
        "sd_init_handle: sd handle %s initialized",
        internal_state.sds[free_index].name
    );
 
    // Read CSD to populate capacity, block size, and block count
    sd_read_csd(&internal_state.sds[free_index]);
 
    sd_cease_comms(&internal_state.sds[free_index], false);
 
    // Ramp SPI to maximum speed after initialization
    spi_set_pclk_div(internal_state.sds[free_index].spi_instance, SPI_MASTER_SCLK_SPEED_PCLK_DIV_2);
 
    return ERR_OK;
}

ERR_te sd_deinit_handle(SD_HANDLE_ts *sd_handle) {
    if(!sd_handle->initialized) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_deinit_handle: handle not initialized"
        );
        return ERR_ILLEGAL_ACTION;
    }
 
    if(internal_state.sd_num == 0) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_deinit_handle: no such handle to deinitialize"
        );
 
        return ERR_UNINITIALZIED_OBJECT;
    }
 
    for(uint32_t i = 0; i < CONFIG_SD_MAX_OBJECTS; i++) {
        if(&internal_state.sds[i] == sd_handle) {
            uint8_t name_len = get_str_len(internal_state.sds[i].name) + 1;
            char name[name_len];
            str_cpy(name, internal_state.sds[i].name, name_len);
 
            spi_deinit(internal_state.sds[i].spi_instance);
 
            internal_state.sds[i] = (SD_HANDLE_ts){ 0 };
 
            internal_state.sd_num--;
 
            LOG_INFO(
                internal_state.subsys,
                internal_state.log_level,
                "sd_deinit_handle: sd handle %s deinitialized",
                name
            );
 
            break;
        }
 
        if(i == CONFIG_SD_MAX_OBJECTS - 1) {
            LOG_ERROR(
                internal_state.subsys,
                internal_state.log_level,
                "sd_deinit_handle: no such handle to deinitialize"
            );
 
            return ERR_UNINITIALZIED_OBJECT;
        }
    }
 
    return ERR_OK;
}

ERR_te sd_get_handle_init(SD_HANDLE_ts *sd_handle, bool *handle_init_o) {
    *handle_init_o = sd_handle->initialized;
 
    return ERR_OK;
}

ERR_te sd_get_sector_count(SD_HANDLE_ts *sd_handle, uint32_t *sector_count_o) {
    *sector_count_o = sd_handle->block_count;
 
    return ERR_OK;
}

ERR_te sd_get_sector_size(SD_HANDLE_ts *sd_handle, uint32_t *sector_size_o) {
    *sector_size_o = sd_handle->block_len;
 
    return ERR_OK;
}

ERR_te sd_read(SD_HANDLE_ts *sd_handle, uint8_t *read_buf, uint32_t start_sector, uint32_t num_sectors) {
    ERR_te err;
    uint32_t start_time = 0;
    uint32_t elapsed_time = 0;
    CMD_RESPONSE_ts cmd_response = { 0 };
 
    if(!internal_state.initialized || !internal_state.started || !sd_handle->initialized) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_read: subsys or handle not initialized or started"
        );
 
        return ERR_INITIALIZATION_FAILURE;
    }
 
    if(sd_handle == NULL || num_sectors == 0) {
        return ERR_INVALID_ARGUMENT;
    }
 
    spi_set_comm(sd_handle->spi_instance, ENABLE);
    gpio_write(sd_handle->cs_gpio_port, sd_handle->cs_gpio_pin, LOW);
 
    if(sd_handle->addr_mode == SD_ADDR_MODE_BYTE) {
        start_sector = start_sector * sd_handle->block_len;
    }
 
    if(num_sectors == 1) {
        err = sd_send_cmd(sd_handle->spi_instance, 17, start_sector, false, &cmd_response);
        if(err != ERR_OK) {
            sd_cease_comms(sd_handle, true);
 
            return err;
        }
 
        if(cmd_response.r1 != 0x0) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_UNKNOWN;
        }
 
        uint8_t token = 0xFF;
        start_time = systick_get_ms();
 
        do {
            elapsed_time = systick_get_ms() - start_time;
            spi_receive(sd_handle->spi_instance, &token, 1);
        } while(token == 0xFF && elapsed_time <= CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT);
 
        if(elapsed_time > CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_TIMEOUT;
        }
 
        if(token != 0xFE) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_UNKNOWN;
        }
 
        spi_receive(sd_handle->spi_instance, read_buf, sd_handle->block_len);
 
        uint8_t crc[2];
        spi_receive(sd_handle->spi_instance, crc, 2);
    }
    else {
        err = sd_send_cmd(sd_handle->spi_instance, 18, start_sector, false, &cmd_response);
        if(err != ERR_OK) {
            sd_cease_comms(sd_handle, true);
 
            return err;
        }
 
        if(cmd_response.r1 != 0x0) {
            sd_cease_comms(sd_handle, true);
            return ERR_UNKNOWN;
        }
 
        for(uint32_t i = 0; i < num_sectors; i++) {
            uint8_t token = 0xFF;
            start_time = systick_get_ms();
 
            do {
                elapsed_time = systick_get_ms() - start_time;
                spi_receive(sd_handle->spi_instance, &token, 1);
            } while(token == 0xFF && elapsed_time <= CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT);
 
            if(elapsed_time > CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT) {
                sd_cease_comms(sd_handle, true);
 
                return ERR_TIMEOUT;
            }
 
            if(token != 0xFE) {
                sd_cease_comms(sd_handle, true);
 
                return ERR_UNKNOWN;
            }
 
            spi_receive(sd_handle->spi_instance, read_buf + (i * sd_handle->block_len), sd_handle->block_len);
 
            uint8_t crc[2];
            spi_receive(sd_handle->spi_instance, crc, 2);
        }
        uint8_t retry = 0;
        do {
            err = sd_send_cmd(sd_handle->spi_instance, 12, 0, false, &cmd_response);
            if(err != ERR_OK) {
                sd_cease_comms(sd_handle, true);
 
                return err;
            }
        } while(cmd_response.r1 != 0x0 && retry++ < CONFIG_SD_INVALID_RESP_RETRY_NUM);
 
        if(cmd_response.r1 != 0x00) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_UNKNOWN;
        }
    }
 
    sd_cease_comms(sd_handle, false);
 
    return ERR_OK;
}

ERR_te sd_write(SD_HANDLE_ts *sd_handle, uint8_t *write_buf, uint32_t start_sector, uint32_t num_sectors) {
    ERR_te err;
    uint32_t start_time = 0;
    uint32_t elapsed_time = 0;
 
    if(!internal_state.initialized || !internal_state.started) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_write: subsys not initialized or started"
        );
 
        return ERR_INITIALIZATION_FAILURE;
    }
 
    if(sd_handle == NULL || num_sectors == 0) {
        return ERR_INVALID_ARGUMENT;
    }
 
    spi_set_comm(sd_handle->spi_instance, ENABLE);
    gpio_write(sd_handle->cs_gpio_port, sd_handle->cs_gpio_pin, LOW);
 
    if(sd_handle->addr_mode == SD_ADDR_MODE_BYTE) {
        start_sector = start_sector * sd_handle->block_len;
    }
 
    CMD_RESPONSE_ts cmd_response = { 0 };
 
    if(num_sectors == 1) {
        err = sd_send_cmd(sd_handle->spi_instance, 24, start_sector, false, &cmd_response);
        if(err != ERR_OK) {
            sd_cease_comms(sd_handle, true);
 
            return err;
        }
 
        if(cmd_response.r1 != 0x0) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_UNKNOWN;
        }
 
        uint8_t token = 0xFE;
        spi_send(sd_handle->spi_instance, &token, 1);
        spi_send(sd_handle->spi_instance, write_buf, sd_handle->block_len);
 
        uint8_t dummy_crc[2] = { 0xFF, 0xFF };
        spi_send(sd_handle->spi_instance, dummy_crc, 2);
 
        uint8_t data_resp = 0xFF;
        start_time = systick_get_ms();
 
        do {
            elapsed_time = systick_get_ms() - start_time;
            spi_receive(sd_handle->spi_instance, &data_resp, 1);
        } while(data_resp == 0xFF && elapsed_time <= CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT);
 
        if(elapsed_time > CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_TIMEOUT;
        }
 
        if((data_resp & 0b11111) != 0b00101) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_UNKNOWN;
        }
 
        // Wait until card releases MISO (busy signal)
        uint8_t busy = 0x00;
        start_time = systick_get_ms();
        do {
            spi_receive(sd_handle->spi_instance, &busy, 1);
            elapsed_time = systick_get_ms() - start_time;
        } while(busy == 0x00 && elapsed_time <= CONFIG_SD_BUSY_TIMOUT);
 
        if(elapsed_time > CONFIG_SD_BUSY_TIMOUT) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_TIMEOUT;
        }
    }
    else {
        err = sd_send_cmd(sd_handle->spi_instance, 25, start_sector, false, &cmd_response);
        if(err != ERR_OK) {
            sd_cease_comms(sd_handle, true);
 
            return err;
        }
 
        if(cmd_response.r1 != 0x0) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_UNKNOWN;
        }
 
        for(uint32_t i = 0; i < num_sectors; i++) {
            uint8_t token = 0xFC;
            spi_send(sd_handle->spi_instance, &token, 1);
            spi_send(sd_handle->spi_instance, write_buf + (i * sd_handle->block_len), sd_handle->block_len);
 
            uint8_t dummy_crc[2] = { 0xFF, 0xFF };
            spi_send(sd_handle->spi_instance, dummy_crc, 2);
 
            uint8_t data_resp = 0xFF;
            start_time = systick_get_ms();
 
            do {
                elapsed_time = systick_get_ms() - start_time;
                spi_receive(sd_handle->spi_instance, &data_resp, 1);
            } while(data_resp == 0xFF && elapsed_time <= CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT);
 
            if(elapsed_time > CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT) {
                sd_cease_comms(sd_handle, true);
 
                return ERR_TIMEOUT;
            }
 
            if((data_resp & 0b11111) != 0b00101) {
                sd_cease_comms(sd_handle, true);
 
                return ERR_UNKNOWN;
            }
 
            uint8_t busy = 0x00;
            start_time = systick_get_ms();
            do {
                spi_receive(sd_handle->spi_instance, &busy, 1);
                elapsed_time = systick_get_ms() - start_time;
            } while(busy == 0x00 && elapsed_time <= CONFIG_SD_BUSY_TIMOUT);
 
            if(elapsed_time > CONFIG_SD_BUSY_TIMOUT) {
                sd_cease_comms(sd_handle, true);
 
                return ERR_TIMEOUT;
            }
        }
 
        // Send stop transmission token
        uint8_t stop_tran_token = 0xFD;
        spi_send(sd_handle->spi_instance, &stop_tran_token, 1);
 
        uint8_t busy = 0x00;
        start_time = systick_get_ms();
        do {
            spi_receive(sd_handle->spi_instance, &busy, 1);
            elapsed_time = systick_get_ms() - start_time;
        } while(busy == 0x00 && elapsed_time <= CONFIG_SD_BUSY_TIMOUT);
 
        if(elapsed_time > CONFIG_SD_BUSY_TIMOUT) {
            sd_cease_comms(sd_handle, true);
 
            return ERR_TIMEOUT;
        }
    }
 
    sd_cease_comms(sd_handle, false);
 
    return ERR_OK;
}



static ERR_te sd_send_cmd(SPI_REGDEF_ts *spi_instance, uint8_t index, uint32_t arg, bool acmd, CMD_RESPONSE_ts *cmd_response_o) {
    ERR_te err;
    uint32_t start_time;
    uint32_t elapsed_time;
 
    uint8_t crc = 0x00;
    uint8_t cmd[6];
 
    if(acmd) {
        CMD_RESPONSE_ts dummy;
        err = sd_send_cmd(spi_instance, 55, 0, false, &dummy);
        if(err != ERR_OK) {
            LOG_ERROR(
                internal_state.subsys,
                LOG_LEVEL_ERROR,
                "sd_send_cmd: unknown error"
            );
 
            return err;
        }
    }
 
    if(index == 0x00) {
        crc = 0x95; // CMD0 requires valid CRC
    }
    else if(index == 0x08) {
        crc = 0x87; // CMD8 requires valid CRC
    }
 
    cmd[0] = 0x40 | index;
    cmd[1] = (arg >> 24) & 0xFF;
    cmd[2] = (arg >> 16) & 0xFF;
    cmd[3] = (arg >> 8) & 0xFF;
    cmd[4] = arg & 0xFF;
    cmd[5] = crc;
 
    spi_send(spi_instance, cmd, sizeof(cmd));
 
    // CMD12 requires discarding the first byte (stuff byte) before the R1 response
    if(index == 12) {
        uint8_t stuff_byte;
        spi_receive(spi_instance, &stuff_byte, 1);
    }
 
    uint8_t r1 = 0xFF;
    start_time = systick_get_ms();
 
    do {
        spi_receive(spi_instance, &r1, 1);
        elapsed_time = systick_get_ms() - start_time;
    } while ((r1 & 0x80) && elapsed_time <= CONFIG_SD_R1_RESP_TIMEOUT);
 
    cmd_response_o->r1 = r1;
 
    if(elapsed_time > CONFIG_SD_R1_RESP_TIMEOUT) {
        LOG_ERROR(
            internal_state.subsys,
            LOG_LEVEL_ERROR,
            "sd_send_cmd: timed out"
        );
 
        return ERR_TIMEOUT;
    }
 
    if(
        index == 0  || index == 1  || index == 41 || index == 9  ||
        index == 10 || index == 16 || index == 17 || index == 18 ||
        index == 23 || index == 24 || index == 25 || index == 55
    ) {
        // R1 only — zero unused fields
        memset(cmd_response_o->r3, 0, sizeof(cmd_response_o->r3));
        memset(cmd_response_o->r7, 0, sizeof(cmd_response_o->r7));
    }
    else if(index == 58) {
        // R3 (OCR) — receive 4 trailing bytes
        memset(cmd_response_o->r7, 0, sizeof(cmd_response_o->r7));
        spi_receive(spi_instance, cmd_response_o->r3, 4);
    }
    else if(index == 8) {
        // R7 (interface condition) — receive 4 trailing bytes
        memset(cmd_response_o->r3, 0, sizeof(cmd_response_o->r3));
        spi_receive(spi_instance, cmd_response_o->r7, 4);
    }
    else if(index == 12) {
        // R1B — poll until card releases MISO (busy signal ends)
        memset(cmd_response_o->r3, 0, sizeof(cmd_response_o->r3));
        memset(cmd_response_o->r7, 0, sizeof(cmd_response_o->r7));
 
        uint8_t rx = 0x00;
        start_time = systick_get_ms();
 
        do {
            spi_receive(spi_instance, &rx, 1);
            elapsed_time = systick_get_ms() - start_time;
        } while(rx == 0x00 && elapsed_time <= CONFIG_SD_R1B_RESP_TIMEOUT);
 
        if(elapsed_time > CONFIG_SD_R1B_RESP_TIMEOUT) {
            LOG_ERROR(
                internal_state.subsys,
                LOG_LEVEL_ERROR,
                "sd_send_cmd: timed out"
            );
 
            return ERR_TIMEOUT;
        }
    }
 
    return ERR_OK;
}

static ERR_te sd_cease_comms(SD_HANDLE_ts *sd_handle, bool deinit) {
    gpio_write(sd_handle->cs_gpio_port, sd_handle->cs_gpio_pin, HIGH);
 
    uint8_t dummy[2] = { 0xFF, 0xFF };
    spi_send(sd_handle->spi_instance, dummy, 2);
 
    spi_set_comm(sd_handle->spi_instance, DISABLE);
 
    if(deinit) {
        // Bypass init guard to allow sd_deinit_handle to proceed
        sd_handle->initialized = true;
        sd_deinit_handle(sd_handle);
    }
 
    return ERR_OK;
}

static ERR_te sd_go_idle_state(SD_HANDLE_ts *sd_handle) {
    ERR_te err;
    CMD_RESPONSE_ts cmd_response = { 0 };
 
    uint8_t retries = 0;
    do {
        retries++;
        err = sd_send_cmd(sd_handle->spi_instance, 0, 0, false, &cmd_response);
        if(err != ERR_OK || !(cmd_response.r1 & 0x01)) {
            LOG_ERROR(
                internal_state.subsys,
                internal_state.log_level,
                "sd_go_idle_state: command execution failure, retry %d/%d in %d ms",
                retries, CONFIG_SD_CMD_SEND_ERROR_RETRY_NUM, CONFIG_SD_CMD_SEND_ERROR_RETRY_LATENCY
            );
            DELAY(CONFIG_SD_CMD_SEND_ERROR_RETRY_LATENCY);
        }
    } while ((err != ERR_OK || !(cmd_response.r1 & 0x01)) && retries < CONFIG_SD_CMD_SEND_ERROR_RETRY_NUM);
 
    if(err != ERR_OK || !(cmd_response.r1 & 0x01)) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_go_idle_state: command execution failure"
        );
 
        return ERR_UNSUCCESFUL_ACTION;
    }
 
    return ERR_OK;
}

static ERR_te sd_send_if_cond(SD_HANDLE_ts *sd_handle, bool *match_o, bool *no_resp_o) {
    ERR_te err;
    CMD_RESPONSE_ts cmd_response = { 0 };
 
    err = sd_send_cmd(sd_handle->spi_instance, 8, 0x000001AA, false, &cmd_response);
    if(err != ERR_OK) {
        *no_resp_o = true;
 
        return ERR_OK;
    }
 
    uint8_t expected[] = { 0x00, 0x00, 0x01, 0xAA };
    for(uint8_t i = 0; i < 4; i++) {
        if(cmd_response.r7[i] != expected[i]) {
            LOG_ERROR(
                internal_state.subsys,
                internal_state.log_level,
                "sd_send_if_cond: initialization failure"
            );
 
            return ERR_INITIALIZATION_FAILURE;
        }
    }
 
    *match_o = true;
 
    return ERR_OK;
}

static ERR_te sd_app_send_op_cond(SD_HANDLE_ts *sd_handle, uint32_t arg) {
    ERR_te err;
    CMD_RESPONSE_ts cmd_response = { 0 };
    uint32_t retries = 0;
 
    do {
        retries++;
        err = sd_send_cmd(sd_handle->spi_instance, 41, arg, true, &cmd_response);
        if(err == ERR_TIMEOUT) {
            LOG_ERROR(
                internal_state.subsys,
                internal_state.log_level,
                "sd_app_send_op_cond: R1 response timeout"
            );
 
            return ERR_TIMEOUT;
        }
 
        if(cmd_response.r1 != 0x00) {
            LOG_ERROR(
                internal_state.subsys,
                internal_state.log_level,
                "sd_app_send_op_cond: invalid R1 response, retry %d/%d in %d ms",
                retries, CONFIG_SD_INVALID_RESP_RETRY_NUM, CONFIG_SD_INVALID_RESP_RETRY_TIMEOUT
            );
            DELAY(CONFIG_SD_INVALID_RESP_RETRY_TIMEOUT);
        }
    } while(cmd_response.r1 == 0x01 && retries < CONFIG_SD_INVALID_RESP_RETRY_NUM);
 
    if(cmd_response.r1 != 0x00) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_app_send_op_cond: initialization failure"
        );
 
        return ERR_INITIALIZATION_FAILURE;
    }
 
    return ERR_OK;
}

static ERR_te sd_read_ocr(SD_HANDLE_ts *sd_handle) {
    ERR_te err;
    CMD_RESPONSE_ts cmd_response = { 0 };
 
    err = sd_send_cmd(sd_handle->spi_instance, 58, 0, false, &cmd_response);
    if(err != ERR_OK) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_read_ocr: failed to read ocr"
        );
 
        return ERR_UNSUCCESFUL_ACTION;
    }
 
    uint32_t ocr =
        ((uint32_t)cmd_response.r3[0] << 24) |
        ((uint32_t)cmd_response.r3[1] << 16) |
        ((uint32_t)cmd_response.r3[2] << 8)  |
        ((uint32_t)cmd_response.r3[3]);
 
    sd_handle->pwrup_status = ((ocr >> OCR_PWRUP_STATUS) & 0x1) ?
        SD_PWRUP_STATUS_READY : SD_PWRUP_STATUS_BUSY;
 
    sd_handle->addr_mode = ((ocr >> OCR_CAPACITY_STATUS) & 0x1) ?
        SD_ADDR_MODE_BLOCK : SD_ADDR_MODE_BYTE;
 
    bool voltage_low_found = false;
    for(uint8_t ocr_counter = 4; ocr_counter < 24; ocr_counter++) {
        if((ocr >> ocr_counter) & 0x1 && !voltage_low_found) {
            sd_handle->min_operating_voltage = (SD_MIN_OPERATING_VOLTAGE_te)(23 - ocr_counter);
            voltage_low_found = true;
        }
        else if((ocr >> ocr_counter) & 0x1 && voltage_low_found) {
            sd_handle->max_operating_voltage = (SD_MAX_OPERATIING_VOLTAGE_te)(23 - ocr_counter);
        }
    }
 
    return ERR_OK;
}

static ERR_te sd_send_op_cond(SD_HANDLE_ts *sd_handle) {
    ERR_te err;
    CMD_RESPONSE_ts cmd_response = { 0 };
    uint32_t retries = 0;
 
    do {
        retries++;
        err = sd_send_cmd(sd_handle->spi_instance, 1, 0, false, &cmd_response);
        if(err == ERR_TIMEOUT) {
            LOG_ERROR(
                internal_state.subsys,
                internal_state.log_level,
                "sd_send_op_cond: R1 response timeout"
            );
 
            return ERR_TIMEOUT;
        }
    } while(cmd_response.r1 == 0x01 && retries < CONFIG_SD_INVALID_RESP_RETRY_NUM);
 
    if(cmd_response.r1 != 0x00) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_send_op_cond: initialization failure"
        );
 
        return ERR_INITIALIZATION_FAILURE;
    }
 
    return ERR_OK;
}

static ERR_te sd_set_blocklen(SD_HANDLE_ts *sd_handle) {
    ERR_te err;
    CMD_RESPONSE_ts cmd_response = { 0 };
 
    err = sd_send_cmd(sd_handle->spi_instance, 16, 0x00000200, false, &cmd_response);
    if(err != ERR_OK) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_set_blocklen: failed to set block length"
        );
 
        return ERR_UNSUCCESFUL_ACTION;
    }
 
    if(cmd_response.r1 != 0x00)
        return ERR_UNSUCCESFUL_ACTION;
 
    return ERR_OK;
}

static ERR_te sd_read_csd(SD_HANDLE_ts *sd_handle) {
    ERR_te err;
    CMD_RESPONSE_ts cmd_response = { 0 };
    uint8_t csd_raw[16];
    SD_CSD_INFO_ts csd_info = { 0 };
 
    err = sd_send_cmd(sd_handle->spi_instance, 9, 0, false, &cmd_response);
    if (err != ERR_OK || cmd_response.r1 != 0x00) {
        LOG_ERROR(internal_state.subsys, internal_state.log_level,
                  "sd_read_csd: failed to send CMD9");
        return ERR_UNSUCCESFUL_ACTION;
    }
 
    uint8_t token = 0;
    uint32_t start_time = systick_get_ms();
    uint32_t elapsed_time = 0;
 
    do {
        spi_receive(sd_handle->spi_instance, &token, 1);
        elapsed_time = systick_get_ms() - start_time;
    } while (token != 0xFE && elapsed_time <= CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT);
 
    if (elapsed_time > CONFIG_SD_DATA_TOKEN_RECV_TIMEOUT || token != 0xFE) {
        LOG_ERROR(internal_state.subsys, internal_state.log_level,
                  "sd_read_csd: timeout waiting for data token");
        return ERR_UNSUCCESFUL_ACTION;
    }
 
    memset(csd_raw, 0, sizeof(csd_raw));
    spi_receive(sd_handle->spi_instance, csd_raw, 16);
 
    uint8_t crc[2];
    spi_receive(sd_handle->spi_instance, crc, 2);
 
    memset(&csd_info, 0, sizeof(SD_CSD_INFO_ts));
 
    uint8_t csd_structure = extract_bits(csd_raw, 126, 2);
 
    switch (csd_structure) {
        case 0:
            decode_csd_v1(csd_raw, &csd_info);
            break;
        case 1:
            decode_csd_v2(csd_raw, &csd_info);
            break;
        case 3:
            decode_csd_v2(csd_raw, &csd_info);
            // SDHC newer version — treated as v2
            break;
    }
 
    sd_handle->capacity_mb = csd_info.capacity_mb;
    sd_handle->block_count = csd_info.block_count;
    sd_handle->block_len   = csd_info.block_len;
 
    return ERR_OK;
}

static ERR_te decode_csd_v1(const uint8_t *csd_raw, SD_CSD_INFO_ts *csd_info_o) {
    csd_info_o->csd_structure      = extract_bits(csd_raw, 126, 2);
    csd_info_o->taac               = extract_bits(csd_raw, 112, 8);
    csd_info_o->nsac               = extract_bits(csd_raw, 104, 8);
    csd_info_o->tran_speed         = extract_bits(csd_raw,  96, 8);
    csd_info_o->ccc                = extract_bits(csd_raw,  84, 12);
    csd_info_o->v1.read_bl_len     = extract_bits(csd_raw,  80, 4);
    csd_info_o->read_bl_partial    = extract_bits(csd_raw,  79, 1);
    csd_info_o->write_blk_misalign = extract_bits(csd_raw,  78, 1);
    csd_info_o->read_blk_misalign  = extract_bits(csd_raw,  77, 1);
    csd_info_o->dsr_imp            = extract_bits(csd_raw,  76, 1);
    csd_info_o->v1.c_size          = extract_bits(csd_raw,  62, 12);
    csd_info_o->v1.c_size_mult     = extract_bits(csd_raw,  47, 3);
    csd_info_o->erase_blk_en       = extract_bits(csd_raw,  46, 1);
    csd_info_o->sector_size        = extract_bits(csd_raw,  39, 7);
    csd_info_o->wp_grp_size        = extract_bits(csd_raw,  32, 7);
    csd_info_o->wp_grp_enable      = extract_bits(csd_raw,  31, 1);
    csd_info_o->r2w_factor         = extract_bits(csd_raw,  26, 3);
    csd_info_o->write_bl_len       = extract_bits(csd_raw,  22, 4);
    csd_info_o->write_bl_partial   = extract_bits(csd_raw,  21, 1);
    csd_info_o->file_format_grp    = extract_bits(csd_raw,  15, 1);
    csd_info_o->copy               = extract_bits(csd_raw,  14, 1);
    csd_info_o->perm_write_protect = extract_bits(csd_raw,  13, 1);
    csd_info_o->tmp_write_protect  = extract_bits(csd_raw,  12, 1);
    csd_info_o->file_format        = extract_bits(csd_raw,  10, 2);
    csd_info_o->wp_upc             = extract_bits(csd_raw,   9, 1);
    csd_info_o->crc                = extract_bits(csd_raw,   1, 7);
 
    csd_info_o->block_len    = 1 << csd_info_o->v1.read_bl_len;
    uint32_t mult            = 1 << (csd_info_o->v1.c_size_mult + 2);
    csd_info_o->block_count  = (csd_info_o->v1.c_size + 1) * mult;
    csd_info_o->capacity_bytes = csd_info_o->block_count * csd_info_o->block_len;
    csd_info_o->capacity_mb  = csd_info_o->capacity_bytes / (1024 * 1024);
 
    return ERR_OK;
}

static ERR_te decode_csd_v2(const uint8_t *csd_raw, SD_CSD_INFO_ts *csd_info_o) {
    csd_info_o->csd_structure      = extract_bits(csd_raw, 126, 2);
    csd_info_o->taac               = extract_bits(csd_raw, 112, 8);
    csd_info_o->nsac               = extract_bits(csd_raw, 104, 8);
    csd_info_o->tran_speed         = extract_bits(csd_raw,  96, 8);
    csd_info_o->ccc                = extract_bits(csd_raw,  84, 12);
    csd_info_o->v1.read_bl_len     = extract_bits(csd_raw,  80, 4);
    csd_info_o->read_bl_partial    = extract_bits(csd_raw,  79, 1);
    csd_info_o->write_blk_misalign = extract_bits(csd_raw,  78, 1);
    csd_info_o->read_blk_misalign  = extract_bits(csd_raw,  77, 1);
    csd_info_o->dsr_imp            = extract_bits(csd_raw,  76, 1);
    csd_info_o->v2.c_size          = extract_bits(csd_raw,  48, 22);
    csd_info_o->erase_blk_en       = extract_bits(csd_raw,  46, 1);
    csd_info_o->sector_size        = extract_bits(csd_raw,  39, 7);
    csd_info_o->wp_grp_size        = extract_bits(csd_raw,  32, 7);
    csd_info_o->wp_grp_enable      = extract_bits(csd_raw,  31, 1);
    csd_info_o->r2w_factor         = extract_bits(csd_raw,  26, 3);
    csd_info_o->write_bl_len       = extract_bits(csd_raw,  22, 4);
    csd_info_o->write_bl_partial   = extract_bits(csd_raw,  21, 1);
    csd_info_o->file_format_grp    = extract_bits(csd_raw,  15, 1);
    csd_info_o->copy               = extract_bits(csd_raw,  14, 1);
    csd_info_o->perm_write_protect = extract_bits(csd_raw,  13, 1);
    csd_info_o->tmp_write_protect  = extract_bits(csd_raw,  12, 1);
    csd_info_o->file_format        = extract_bits(csd_raw,  10, 2);
    csd_info_o->wp_upc             = extract_bits(csd_raw,   9, 1);
    csd_info_o->crc                = extract_bits(csd_raw,   1, 7);
 
    csd_info_o->block_len      = 512;
    csd_info_o->capacity_bytes = (csd_info_o->v2.c_size + 1) * 512 * 1024;
    csd_info_o->capacity_mb    = csd_info_o->capacity_bytes / (1024 * 1024);
    csd_info_o->block_count    = csd_info_o->capacity_bytes / 512;
 
    return ERR_OK;
}



static ERR_te sd_cmd_list_handler(uint32_t argc, char **argv) {
    if(argc != 2) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_cmd_list_handler: invalid arguments"
        );
        return ERR_INVALID_ARGUMENT;
    }
 
    for(uint32_t i = 0; i < CONFIG_SD_MAX_OBJECTS; i++) {
        if(internal_state.sds[i].in_use == true) {
            LOG_INFO(
                internal_state.subsys,
                internal_state.log_level,
                "%s",
                internal_state.sds[i].name
            );
        }
    }
 
    return ERR_OK;
}

static ERR_te sd_cmd_info_handler(uint32_t argc, char **argv) {
    if(argc != 3) {
        LOG_ERROR(
            internal_state.subsys,
            internal_state.log_level,
            "sd_cmd_info_handler: invalid arguments"
        );
        return ERR_INVALID_ARGUMENT;
    }
 
    for(uint32_t i = 0; i < CONFIG_SD_MAX_OBJECTS; i++) {
        if(str_cmp(internal_state.sds[i].name, argv[2]) == true) {
            char type_str[10];
            char addr_mode[10];
 
            switch(internal_state.sds[i].type) {
                case SDSC: str_cpy(type_str, "SDSC", get_str_len("SDSC") + 1); break;
                case SDHC: str_cpy(type_str, "SDHC", get_str_len("SDHC") + 1); break;
            }
 
            switch(internal_state.sds[i].addr_mode) {
                case SD_ADDR_MODE_BLOCK: str_cpy(addr_mode, "block", get_str_len("block") + 1); break;
                case SD_ADDR_MODE_BYTE:  str_cpy(addr_mode, "byte",  get_str_len("byte")  + 1); break;
            }
 
            LOG_INFO(
                internal_state.subsys,
                internal_state.log_level,
                "type: %s, addr mode: %s, capacity: %d mb, block size: %d byte, block count: %d",
                type_str,
                addr_mode,
                internal_state.sds[i].capacity_mb,
                internal_state.sds[i].block_len,
                internal_state.sds[i].block_count
            );
 
            break;
        }
 
        if(i == CONFIG_SD_MAX_OBJECTS - 1) {
            LOG_ERROR(
                internal_state.subsys,
                internal_state.log_level,
                "sd_cmd_info_handler: no such handle"
            );
 
            return ERR_INVALID_ARGUMENT;
        }
    }
 
    return ERR_OK;
}