X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fdrv%2Fsd.c;h=cfac8f740a2962b28ced535b9a5ec3c1724f0175;hb=7f6d545066c243a9f38c77dff806d7f91d1893af;hp=0e5b19dc0ae07631efac7bc2798beb69a4c3d2e4;hpb=8ab7a5bf4029b97d8913ea3e63499074f08b9c61;p=bertos.git

diff --git a/bertos/drv/sd.c b/bertos/drv/sd.c
index 0e5b19dc..cfac8f74 100644
--- a/bertos/drv/sd.c
+++ b/bertos/drv/sd.c
@@ -240,6 +240,7 @@ static int16_t sd_getCSD(Sd *sd, CardCSD *csd)
 
 static size_t sd_readDirect(struct KBlock *b, block_idx_t idx, void *buf, size_t offset, size_t size)
 {
+
 	Sd *sd = SD_CAST(b);
 	LOG_INFO("reading from block %ld, offset %d, size %d\n", idx, offset, size);
 
@@ -424,6 +425,70 @@ static bool sd_blockInit(Sd *sd, KFile *ch)
 	DB(sd->b.priv.type = KBT_SD);
 	sd->ch = ch;
 
+
+#if CPU_CM3_SAM3X8
+
+	/* Wait a few moments for supply voltage to stabilize */
+	timer_delay(SD_START_DELAY);
+
+	sd_sendInit();
+	sd_goIdle();
+
+	sd_sendIfCond(sd);
+
+	ticks_t start = timer_clock();
+	bool sd_power_on = false;
+	do
+	{
+		if (!sd_sendAppOpCond(sd))
+		{
+			sd_power_on = true;
+			break;
+		}
+		cpu_relax();
+	}
+	while (timer_clock() - start < SD_INIT_TIMEOUT);
+
+
+	if (sd_power_on)
+	{
+
+		if(!sd_getCid(sd, 0, SD_SEND_ALL_CID))
+		{
+			sd_dumpCid(sd);
+		}
+
+		if (!sd_getRelativeAddr(sd))
+		{
+			LOG_INFO("RCA: %0lx\n", sd->addr);
+		}
+
+
+		if (!sd_getCsd(sd))
+		{
+			sd_dumpCsd(sd);
+		}
+
+		if (!sd_appStatus(sd))
+		{
+			LOG_INFO("STATUS: %ld\n", sd->status);
+		}
+
+		if (sd->status & SD_CARD_IS_LOCKED)
+		{
+			LOG_INFO("SD is locked!\n");
+		}
+		else if (sd->status & SD_READY_FOR_DATA)
+		{
+			sd_selectCard(sd);
+			sd_set_BlockLen(sd, SD_DEFAULT_BLOCKLEN);
+			sd_setBus4bit(sd);
+			sd_setHightSpeed(sd);
+			sd_deSelectCard(sd);
+		}
+
+	}
+#else
 	SD_CS_INIT();
 	SD_CS_OFF();
 
@@ -492,6 +557,7 @@ static bool sd_blockInit(Sd *sd, KFile *ch)
 	sd->b.blk_size = SD_DEFAULT_BLOCKLEN;
 	sd->b.blk_cnt = csd.block_num * (csd.block_len / SD_DEFAULT_BLOCKLEN);
 	LOG_INFO("blk_size %d, blk_cnt %ld\n", sd->b.blk_size, sd->b.blk_cnt);
+#endif
 
 #if CONFIG_SD_AUTOASSIGN_FAT
 	disk_assignDrive(&sd->b, 0);
@@ -527,7 +593,7 @@ static bool sd_blockInit(Sd *sd, KFile *ch)
 
 /* OCR bit definitions */
 #define SD_OCR_S18R     (1 << 24)    /* 1.8V switching request */
-#define SD_ROCR_S18A        SD_OCR_S18R  /* 1.8V switching accepted by card */
+#define SD_ROCR_S18A    SD_OCR_S18R  /* 1.8V switching accepted by card */
 #define SD_OCR_XPC      (1 << 28)    /* SDXC power control */
 
 /*
@@ -559,43 +625,48 @@ static bool sd_blockInit(Sd *sd, KFile *ch)
 #define SCR_SPEC_VER_1      1   /* Implements system specification 1.10 */
 #define SCR_SPEC_VER_2      2   /* Implements system specification 2.00-3.0X */
 
-#define UNSTUFF_BITS(resp, start, size)                   \
-    ({                              \
-        const uint32_t __size = size;                \
-        const uint32_t __mask = (__size < 32 ? 1 << __size : 0) - 1; \
-        const uint32_t __off = 3 - ((start) / 32);           \
-        const uint32_t __shft = (start) & 31;            \
-        uint32_t __res;                      \
-                                    \
-        __res = resp[__off] >> __shft;              \
-        if (__size + __shft > 32)               \
-            __res |= resp[__off-1] << ((32 - __shft) % 32); \
-        __res & __mask;                     \
-    })
-
-
-#define BCD_TO_INT_32BIT(bcd)  ((uint32_t )((bcd) & 0xf) * 1 +  \
-								(((bcd) >> 4) & 0xf)  * 10 +      \
-								(((bcd) >> 8) & 0xf)  * 100 +     \
-								(((bcd) >> 12) & 0xf) * 1000 +   \
-								(((bcd) >> 16) & 0xf) * 10000 +   \
-								(((bcd) >> 20) & 0xf) * 100000 +  \
-								(((bcd) >> 24) & 0xf) * 1000000 + \
-								(((bcd) >> 28) & 0xf) * 10000000) \
+
+#define SD_ADDR_TO_RCA(addr)    (uint32_t)(((addr) << 16) & 0xFFFF0000)
+#define SD_GET_STATE(status)    (uint8_t)(((status) & SD_STATUS_CURR_MASK) >> SD_STATUS_CURR_SHIFT)
+
+#define SD_STATUS_APP_CMD      BV(5)
+#define SD_STATUS_READY        BV(8)
+#define SD_STATUS_CURR_MASK    0x1E00
+#define SD_STATUS_CURR_SHIFT   9
+
 
 LOG_INFOB(
 static void dump(const char *label, uint32_t *r, size_t len)
 {
 	ASSERT(r);
-	kprintf("%s [ ", label);
-	for (size_t i = 0; i < len; i++)
-		kprintf("%lx ", r[i]);
-	kputs("]\n");
+	size_t i;
+	int j = 0;
+	kprintf("\n%s [\n", label);
+	for (i = 0; i < len; i++)
+	{
+		if (j == 5)
+		{
+			kputs("\n");
+			j = 0;
+		}
+		kprintf("%08lx", r[i]);
+		j++;
+	}
+	kprintf("\n] len=%d\n\n", i);
 }
 )
 
+static const uint32_t tran_exp[] = {
+    10000,      100000,     1000000,    10000000,
+    0,      0,      0,      0
+};
+
+static const uint8_t tran_mant[] = {
+    0,  10, 12, 13, 15, 20, 25, 30,
+    35, 40, 45, 50, 55, 60, 70, 80,
+};
 
-static int sd_decode_csd(SDcsd *csd, uint32_t *resp, size_t len)
+static int sd_decodeCsd(SDcsd *csd, uint32_t *resp, size_t len)
 {
 	ASSERT(csd);
 	ASSERT(resp);
@@ -604,7 +675,19 @@ static int sd_decode_csd(SDcsd *csd, uint32_t *resp, size_t len)
     csd->structure = UNSTUFF_BITS(resp, 126, 2);
 	csd->ccc = UNSTUFF_BITS(resp, 84, 12);
 
-    if (csd->structure == 0) /* Version 1.00 layer */
+    csd->max_data_rate  = tran_exp[UNSTUFF_BITS(resp, 96, 3)] * tran_mant[UNSTUFF_BITS(resp, 99, 4)];
+
+	/*
+	 * CSD structure:
+	 * - 0:
+	 * 		- Version 1.01-1.10
+	 * 		- Version 2.00/Standard Capacity
+	 * - 1:
+	 * 		- Version 2.00/High Capacity
+	 * - >1: not defined.
+	 */
+
+    if (csd->structure == 0)
 	{
 		// (C_size + 1) x 2^(C_SIZE_MUL+2)
 		csd->blk_num = (1 + UNSTUFF_BITS(resp, 62, 12)) << (UNSTUFF_BITS(resp, 47, 3) + 2);
@@ -633,7 +716,7 @@ static int sd_decode_csd(SDcsd *csd, uint32_t *resp, size_t len)
 
 		return 0;
 	}
-	else if (csd->structure == 1) /* Version 2.00 layer */
+	else if (csd->structure == 1)
 	{
 		kprintf("csize %ld\n", UNSTUFF_BITS(resp, 48, 22));
         csd->capacity  = (1 + UNSTUFF_BITS(resp, 48, 22)) << 10;
@@ -662,107 +745,127 @@ static int sd_decode_csd(SDcsd *csd, uint32_t *resp, size_t len)
 }
 
 
-void sd_dump_csd(SDcsd *csd)
+void sd_dumpCsd(Sd *sd)
 {
-	ASSERT(csd);
+	ASSERT(sd);
 
-	LOG_INFO("VERSION: %d.0\n", csd->structure ? 2 : 1);
-    LOG_INFO("CARD COMMAND CLASS: %d\n", csd->ccc);
-	LOG_INFO("WRITE BLK LEN BITS: %ld\n", csd->write_blk_bits);
-	LOG_INFO("READ BLK LEN BITS: %ld\n", csd->read_blk_bits);
-	LOG_INFO("ERASE SIZE: %ld\n", csd->erase_size);
-	LOG_INFO("BLK NUM: %ld\n", csd->blk_num);
-	LOG_INFO("BLK LEN: %ld\n", csd->blk_len);
-	LOG_INFO("CAPACITY %ld\n", csd->capacity);
-	LOG_INFO("FLAG Write: WP %d, W MISALIGN %d\n", csd->write_partial, csd->write_misalign);
-	LOG_INFO("FLAG Read: RP %d, R MISALIGN %d\n", csd->read_partial, csd->read_misalign);
+	LOG_INFO("VERSION: %d.0\n", sd->csd.structure ? 2 : 1);
+    LOG_INFO("CARD COMMAND CLASS: %d\n", sd->csd.ccc);
+	LOG_INFO("MAX DATA RATE: %ld\n", sd->csd.max_data_rate);
+	LOG_INFO("WRITE BLK LEN BITS: %ld\n", sd->csd.write_blk_bits);
+	LOG_INFO("READ BLK LEN BITS: %ld\n", sd->csd.read_blk_bits);
+	LOG_INFO("ERASE SIZE: %ld\n", sd->csd.erase_size);
+	LOG_INFO("BLK NUM: %ld\n", sd->csd.blk_num);
+	LOG_INFO("BLK LEN: %ld\n", sd->csd.blk_len);
+	LOG_INFO("CAPACITY %ld\n", sd->csd.capacity);
+	LOG_INFO("FLAG Write: WP %d, W MISALIGN %d\n", sd->csd.write_partial, sd->csd.write_misalign);
+	LOG_INFO("FLAG Read: RP %d, R MISALIGN %d\n", sd->csd.read_partial, sd->csd.read_misalign);
 
 }
 
-void sd_dump_cid(SDcid *cid)
+void sd_dumpCid(Sd *sd)
 {
-	ASSERT(cid);
+	ASSERT(sd);
 
-	LOG_INFO("MANFID: %d\n", cid->manfid);
-    LOG_INFO("OEMID: %d\n", cid->oemid);
-	LOG_INFO("SERIAL: %ld\n", cid->serial);
-    LOG_INFO("PROD_NAME: %s\n", cid->prod_name);
-    LOG_INFO("REV: %d.%d\n", cid->m_rev, cid->l_rev);
-    LOG_INFO("OFF,Y,M: %lx, %ld %ld\n", cid->year_off, (BCD_TO_INT_32BIT(cid->year_off) / 12) + 2000,
-												(BCD_TO_INT_32BIT(cid->year_off) % 12));
+	LOG_INFO("MANFID: %d\n", sd->cid.manfid);
+    LOG_INFO("OEMID: %d\n", sd->cid.oemid);
+	LOG_INFO("SERIAL: %ld\n", sd->cid.serial);
+    LOG_INFO("PROD_NAME: %s\n", sd->cid.prod_name);
+    LOG_INFO("REV: %d.%d\n", sd->cid.m_rev, sd->cid.l_rev);
+    LOG_INFO("OFF,Y,M: %lx, %ld %ld\n", sd->cid.year_off, (BCD_TO_INT_32BIT(sd->cid.year_off) / 12) + 2000,
+												(BCD_TO_INT_32BIT(sd->cid.year_off) % 12));
 }
 
-void sd_send_init(void)
+void sd_dumpSsr(Sd *sd)
 {
+	ASSERT(sd);
+
+	LOG_INFO("BUS_WIDTH: %d\n", sd->ssr.bus_width);
+    LOG_INFO("TYPE: %d\n", sd->ssr.card_type);
+	LOG_INFO("AU_TYPE: %d\n", sd->ssr.au_size);
+	LOG_INFO("ERASE_SIZE: %d\n", sd->ssr.erase_size);
+	LOG_INFO("SPEED_CLASS: %d\n", sd->ssr.speed_class);
+}
+
+
+void sd_sendInit(void)
+{
+	hsmci_init(NULL); //TODO: REMOVE IT!
+
 	if (hsmci_sendCmd(0, 0, HSMCI_CMDR_SPCMD_INIT | HSMCI_CMDR_RSPTYP_NORESP))
 		LOG_ERR("INIT: %lx\n", HSMCI_SR);
 }
 
 
-void sd_go_idle(void)
+void sd_goIdle(void)
 {
+	hsmci_setSpeed(HSMCI_INIT_SPEED, false);
 	if (hsmci_sendCmd(0, 0, HSMCI_CMDR_RSPTYP_NORESP))
 		LOG_ERR("GO_IDLE: %lx\n", HSMCI_SR);
 }
 
-int sd_send_if_cond(void)
+int sd_sendIfCond(Sd *sd)
 {
 	if (hsmci_sendCmd(8, CMD8_V_RANGE_27V_36V, HSMCI_CMDR_RSPTYP_48_BIT))
 	{
 		LOG_ERR("IF_COND %lx\n", HSMCI_SR);
 		return -1;
 	}
-	else
+	hsmci_readResp(&(sd->status), 1);
+	if (((sd->status) & 0xFFF) == CMD8_V_RANGE_27V_36V)
 	{
-		uint32_t r = HSMCI_RSPR;
-		if ((r & 0xFFF) == CMD8_V_RANGE_27V_36V)
-		{
-			LOG_INFO("IF_COND: %lx\n", r);
-			return 0;
-		}
-		LOG_ERR("IF_COND: %lx\n", r);
+		LOG_INFO("IF_COND: %lx\n", (sd->status));
+		return 0;
 	}
+	LOG_ERR("IF_COND: %lx\n", (sd->status));
+
 	return -1;
 }
 
-int sd_send_app_op_cond(void)
+int sd_sendAppOpCond(Sd *sd)
 {
 	if (hsmci_sendCmd(55, 0, HSMCI_CMDR_RSPTYP_48_BIT))
 	{
 		LOG_ERR("APP_CMD %lx\n", HSMCI_SR);
 		return -1;
 	}
-	else
-	{
-		LOG_INFO("APP_CMD %lx\n", HSMCI_RSPR);
-	}
 
-	if (hsmci_sendCmd(41, SD_HOST_VOLTAGE_RANGE | SD_OCR_CCS, HSMCI_CMDR_RSPTYP_48_BIT))// se cmd 8 va ok.
-	{
-		LOG_ERR("APP_OP_COND %lx\n", HSMCI_SR);
-		return -1;
-	}
-	else
+	hsmci_readResp(&(sd->status), 1);
+	if ((sd->status) & (SD_STATUS_APP_CMD | SD_STATUS_READY))
 	{
-		uint32_t status = HSMCI_RSPR;
-		if (status & SD_OCR_BUSY)
+		if (hsmci_sendCmd(41, SD_HOST_VOLTAGE_RANGE | SD_OCR_CCS, HSMCI_CMDR_RSPTYP_48_BIT))// se cmd 8 va ok.
 		{
-			LOG_INFO("SD power up! Hight Capability [%d]\n", (bool)(status & SD_OCR_CCS));
-			return 0;
+			LOG_ERR("APP_OP_COND %lx\n", HSMCI_SR);
+			return -1;
 		}
+		else
+		{
+			hsmci_readResp(&(sd->status), 1);
+			if ((sd->status) & SD_OCR_BUSY)
+			{
+				LOG_INFO("SD power up! Hight Capability [%d]\n", (bool)((sd->status) & SD_OCR_CCS));
+				return 0;
+			}
 
-		LOG_ERR("sd not ready.\n");
+			LOG_ERR("sd not ready.\n");
+		}
 	}
 
 	return -1;
 }
 
-int sd_get_cid(SDcid *cid)
+
+int sd_getCid(Sd *sd, uint32_t addr, uint8_t flag)
 {
-	ASSERT(cid);
-	memset(cid, 0, sizeof(cid));
+	ASSERT(sd);
+	memset(&(sd->cid), 0, sizeof(SDcid));
+
+	uint8_t idx = 9; // CMD9 get cid from gived sd address (RCA)
+	if (flag & SD_SEND_ALL_CID)
+		idx = 2;
 
-	if (hsmci_sendCmd(2, 0, HSMCI_CMDR_RSPTYP_136_BIT))
+
+	if (hsmci_sendCmd(idx, SD_ADDR_TO_RCA(addr), HSMCI_CMDR_RSPTYP_136_BIT))
 	{
 		LOG_ERR("GET_CID %lx\n", HSMCI_SR);
 		return -1;
@@ -773,30 +876,29 @@ int sd_get_cid(SDcid *cid)
 		hsmci_readResp(resp, 4);
 		LOG_INFOB(dump("CID", resp, 4););
 
-		cid->manfid        = UNSTUFF_BITS(resp, 120, 8);
-		cid->oemid         = UNSTUFF_BITS(resp, 104, 16);
-		cid->prod_name[0]      = UNSTUFF_BITS(resp, 96, 8);
-		cid->prod_name[1]      = UNSTUFF_BITS(resp, 88, 8);
-		cid->prod_name[2]      = UNSTUFF_BITS(resp, 80, 8);
-		cid->prod_name[3]      = UNSTUFF_BITS(resp, 72, 8);
-		cid->prod_name[4]      = UNSTUFF_BITS(resp, 64, 8);
-		cid->m_rev         = UNSTUFF_BITS(resp, 60, 4);
-		cid->l_rev         = UNSTUFF_BITS(resp, 56, 4);
-		cid->serial        = (uint32_t)UNSTUFF_BITS(resp, 24, 32);
-		cid->year_off      = UNSTUFF_BITS(resp, 8, 12);
+		sd->cid.manfid        = UNSTUFF_BITS(resp, 120, 8);
+		sd->cid.oemid         = UNSTUFF_BITS(resp, 104, 16);
+		sd->cid.prod_name[0]      = UNSTUFF_BITS(resp, 96, 8);
+		sd->cid.prod_name[1]      = UNSTUFF_BITS(resp, 88, 8);
+		sd->cid.prod_name[2]      = UNSTUFF_BITS(resp, 80, 8);
+		sd->cid.prod_name[3]      = UNSTUFF_BITS(resp, 72, 8);
+		sd->cid.prod_name[4]      = UNSTUFF_BITS(resp, 64, 8);
+		sd->cid.m_rev         = UNSTUFF_BITS(resp, 60, 4);
+		sd->cid.l_rev         = UNSTUFF_BITS(resp, 56, 4);
+		sd->cid.serial        = (uint32_t)UNSTUFF_BITS(resp, 24, 32);
+		sd->cid.year_off      = UNSTUFF_BITS(resp, 8, 12);
 	}
 
 	return 0;
 }
 
-int sd_get_csd(SDcsd *csd, uint32_t addr)
+int sd_getCsd(Sd *sd)
 {
-	ASSERT(csd);
-	memset(csd, 0, sizeof(csd));
+	ASSERT(sd);
+	memset(&(sd->csd), 0, sizeof(SDcsd));
 
-	uint32_t arg = (addr << 16) & 0xFFFF0000;
-	kprintf("Send to rca: %lx\n", arg);
-	if (hsmci_sendCmd(9, arg, HSMCI_CMDR_RSPTYP_136_BIT))
+	LOG_INFO("Send to RCA: %lx\n", SD_ADDR_TO_RCA(sd->addr));
+	if (hsmci_sendCmd(9, SD_ADDR_TO_RCA(sd->addr), HSMCI_CMDR_RSPTYP_136_BIT))
 	{
 		LOG_ERR("GET_CSD %lx\n", HSMCI_SR);
 		return -1;
@@ -806,44 +908,292 @@ int sd_get_csd(SDcsd *csd, uint32_t addr)
 		uint32_t resp[4];
 		hsmci_readResp(resp, 4);
 		LOG_INFOB(dump("CSD", resp, 4););
-		sd_decode_csd(csd, resp, 4);
+		sd_decodeCsd(&(sd->csd), resp, 4);
 	}
 
 	return 0;
 }
 
-int sd_app_status(uint32_t *resp, size_t len)
+int sd_getRelativeAddr(Sd *sd)
 {
-	if (hsmci_sendCmd(13, 0, HSMCI_CMDR_RSPTYP_48_BIT))
+	ASSERT(sd);
+	if (hsmci_sendCmd(3, 0, HSMCI_CMDR_RSPTYP_48_BIT))
 	{
-		LOG_ERR("STATUS: %lx\n", HSMCI_SR);
+		LOG_ERR("RCA: %lx\n", HSMCI_SR);
 		return -1;
 	}
-	else
-		hsmci_readResp(resp, len);
+
+	hsmci_readResp(&sd->addr, 1);
+	sd->addr = sd->addr >> 16;
+
+	LOG_INFOB(dump("RCA", &sd->addr, 1););
 
 	return 0;
 }
 
-int sd_send_relative_addr(uint32_t *addr)
+int sd_appStatus(Sd *sd)
 {
+	ASSERT(sd);
+	LOG_INFO("Send to RCA: %lx\n", SD_ADDR_TO_RCA(sd->addr));
+	if (hsmci_sendCmd(13, SD_ADDR_TO_RCA(sd->addr), HSMCI_CMDR_RSPTYP_48_BIT))
+	{
+		LOG_ERR("STATUS: %lx\n", HSMCI_SR);
+		return -1;
+	}
 
-	if (hsmci_sendCmd(3, 0, HSMCI_CMDR_RSPTYP_48_BIT))
+	hsmci_readResp(&(sd->status), 1);
+	LOG_INFOB(dump("STATUS", &(sd->status), 1););
+
+	LOG_INFO("State[%d]\n", SD_GET_STATE(sd->status));
+
+	if (sd->status & SD_STATUS_READY)
+		return 0;
+
+	return -1;
+}
+
+
+INLINE int sd_cardSelection(Sd *sd, uint32_t rca)
+{
+	ASSERT(sd);
+	LOG_INFO("Select RCA: %lx\n", rca);
+	if (hsmci_sendCmd(7, rca, HSMCI_CMDR_RSPTYP_R1B))
 	{
-		LOG_ERR("RCA: %lx\n", HSMCI_SR);
+		LOG_ERR("SELECT_SD: %lx\n", HSMCI_SR);
 		return -1;
 	}
-	else
+
+	HSMCI_CHECK_BUSY();
+	hsmci_readResp(&(sd->status), 1);
+	LOG_INFOB(dump("SELECT_SD", &(sd->status), 1););
+
+	LOG_INFO("State[%d]\n", SD_GET_STATE(sd->status));
+
+	if (sd->status & SD_STATUS_READY)
+		return 0;
+
+	return -1;
+}
+
+int sd_selectCard(Sd *sd)
+{
+	ASSERT(sd);
+	uint32_t rca = SD_ADDR_TO_RCA(sd->addr);
+	LOG_INFO("Select RCA: %lx\n", rca);
+	if (hsmci_sendCmd(7, rca, HSMCI_CMDR_RSPTYP_R1B))
 	{
-		hsmci_readResp(addr, 1);
-		LOG_INFOB(dump("RCA", addr, 1););
-		*addr = *addr >> 16;
+		LOG_ERR("SELECT_SD: %lx\n", HSMCI_SR);
+		return -1;
+	}
+
+	HSMCI_CHECK_BUSY();
+	hsmci_readResp(&(sd->status), 1);
+
+	LOG_INFOB(dump("SELECT_SD", &(sd->status), 1););
+	LOG_INFO("State[%d]\n", SD_GET_STATE(sd->status));
+
+	if (sd->status & SD_STATUS_READY)
+		return 0;
+
+	return -1;
+}
+
+int sd_deSelectCard(Sd *sd)
+{
+	ASSERT(sd);
+
+	uint32_t rca = 0;
+	if (!sd->addr)
+		rca = SD_ADDR_TO_RCA(sd->addr + 1);
 
+	LOG_INFO("Select RCA: %lx\n", rca);
+
+	if (hsmci_sendCmd(7, rca, HSMCI_CMDR_RSPTYP_NORESP))
+	{
+		LOG_ERR("DESELECT_SD: %lx\n", HSMCI_SR);
+		return -1;
 	}
 
 	return 0;
+
 }
 
+
+int sd_setBusWidth(Sd *sd, size_t len)
+{
+	ASSERT(sd);
+
+	if (hsmci_sendCmd(55, SD_ADDR_TO_RCA(sd->addr), HSMCI_CMDR_RSPTYP_48_BIT))
+	{
+		LOG_ERR("APP_CMD %lx\n", HSMCI_SR);
+		return -1;
+	}
+
+	hsmci_readResp(&(sd->status), 1);
+	if ((sd->status) & (SD_STATUS_APP_CMD | SD_STATUS_READY))
+	{
+		hsmci_setBusWidth(len);
+
+		uint8_t arg = 0;
+		if (len == 4)
+			arg = 2;
+
+		if (hsmci_sendCmd(6, arg, HSMCI_CMDR_RSPTYP_48_BIT))
+		{
+			LOG_ERR("SET_BUS_WIDTH CMD: %lx\n", HSMCI_SR);
+			return -1;
+		}
+
+		hsmci_readResp(&(sd->status), 1);
+
+		LOG_INFOB(dump("SET_BUS_WIDTH", &(sd->status), 1););
+		LOG_INFO("State[%d]\n", SD_GET_STATE(sd->status));
+
+		if (sd->status & SD_STATUS_READY)
+			return 0;
+	}
+
+	LOG_ERR("SET_BUS_WIDTH REP %lx\n", (sd->status));
+	return -1;
+}
+
+
+int sd_set_BlockLen(Sd *sd, size_t len)
+{
+	ASSERT(sd);
+
+	if (hsmci_sendCmd(16, len, HSMCI_CMDR_RSPTYP_48_BIT))
+	{
+		LOG_ERR("SET_BLK_LEN: %lx\n", HSMCI_SR);
+		return -1;
+	}
+
+	hsmci_readResp(&(sd->status), 1);
+
+	LOG_INFOB(dump("SET_BLK_LEN", &(sd->status), 1););
+	LOG_INFO("State[%d]\n", SD_GET_STATE(sd->status));
+
+	sd->csd.blk_len = len;
+
+	if (sd->status & SD_STATUS_READY)
+		return 0;
+
+	return -1;
+}
+
+int sd_getStatus(Sd *sd, uint32_t *buf, size_t words)
+{
+	ASSERT(sd);
+
+	// Status reply with 512bit data, so the block size in byte is 64
+	hsmci_prgRxDMA(buf, words, 64);
+
+	if (hsmci_sendCmd(55, SD_ADDR_TO_RCA(sd->addr), HSMCI_CMDR_RSPTYP_48_BIT))
+	{
+		LOG_ERR("APP_CMD %lx\n", HSMCI_SR);
+		return -1;
+	}
+
+	uint32_t status = HSMCI_RSPR;
+	if (status & (SD_STATUS_APP_CMD | SD_STATUS_READY))
+	{
+		if (hsmci_sendCmd(13, 0, HSMCI_CMDR_RSPTYP_48_BIT |
+				BV(HSMCI_CMDR_TRDIR) | HSMCI_CMDR_TRCMD_START_DATA | HSMCI_CMDR_TRTYP_SINGLE))
+		{
+			LOG_ERR("STATUS CMD: %lx\n", HSMCI_SR);
+			return -1;
+		}
+
+		hsmci_readResp(&(sd->status), 1);
+		LOG_INFOB(dump("STATUS", &(sd->status), 1););
+		LOG_INFO("State[%d]\n", SD_GET_STATE(sd->status));
+
+		if (sd->status & SD_STATUS_READY)
+		{
+			hsmci_waitTransfer();
+
+			LOG_INFOB(dump("STATUS", buf, words););
+			memset(&(sd->ssr), 0, sizeof(SDssr));
+
+			sd->ssr.bus_width  = UNSTUFF_BITS(buf, 510, 2);
+			sd->ssr.card_type  = UNSTUFF_BITS(buf, 480, 16);
+			sd->ssr.au_size  = UNSTUFF_BITS(buf, 432, 8);
+			sd->ssr.speed_class  = UNSTUFF_BITS(buf, 440, 8);
+			sd->ssr.erase_size = UNSTUFF_BITS(buf, 408, 24);
+
+			return 0;
+		}
+	}
+
+	return -1;
+}
+
+
+
+int sd_readSingleBlock(Sd *sd, size_t index, uint32_t *buf, size_t words)
+{
+	ASSERT(sd);
+	ASSERT(buf);
+
+	hsmci_prgRxDMA(buf, words, sd->csd.blk_len);
+
+	if (hsmci_sendCmd(17, index * sd->csd.blk_len, HSMCI_CMDR_RSPTYP_48_BIT |
+			BV(HSMCI_CMDR_TRDIR) | HSMCI_CMDR_TRCMD_START_DATA | HSMCI_CMDR_TRTYP_SINGLE))
+	{
+		LOG_ERR("SIGLE_BLK_READ: %lx\n", HSMCI_SR);
+		return -1;
+	}
+	hsmci_readResp(&(sd->status), 1);
+
+	LOG_INFOB(dump("SIGLE_BLK_READ", &(sd->status), 1););
+	LOG_INFO("State[%d]\n", SD_GET_STATE(sd->status));
+
+	if (sd->status & SD_STATUS_READY)
+	{
+		hsmci_waitTransfer();
+		return words;
+	}
+
+	return -1;
+}
+
+
+int sd_writeSingleBlock(Sd *sd, size_t index, uint32_t *buf, size_t words)
+{
+	ASSERT(sd);
+	ASSERT(buf);
+
+	hsmci_prgTxDMA(buf, words, sd->csd.blk_len);
+
+	if (hsmci_sendCmd(24, index * sd->csd.blk_len, HSMCI_CMDR_RSPTYP_48_BIT |
+						HSMCI_CMDR_TRCMD_START_DATA | HSMCI_CMDR_TRTYP_SINGLE))
+	{
+		LOG_ERR("SIGLE_BLK_WRITE: %lx\n", HSMCI_SR);
+		return -1;
+	}
+	hsmci_readResp(&(sd->status), 1);
+
+	LOG_INFOB(dump("SIGLE_BLK_WR", &(sd->status), 1););
+	LOG_INFO("State[%d]\n", SD_GET_STATE(sd->status));
+
+	if (sd->status & SD_STATUS_READY)
+	{
+		hsmci_waitTransfer();
+		return words;
+	}
+
+	return -1;
+}
+
+
+
+void sd_setHightSpeed(Sd *sd)
+{
+	(void)sd;
+	hsmci_setSpeed(2100000, true);
+}
+
+
 #endif