spi-sd-card.c, spi-sd-card.h: removed high-level driver IF, added generic block I/O functions

3 files changed, 197 insertions, 385 deletions

diff --git a/c/src/ChangeLog b/c/src/ChangeLog
index 536eb07a68..b525656e4a 100644
--- a/c/src/ChangeLog
+++ b/c/src/ChangeLog
@@ -1,3 +1,8 @@
+2008-11-23	Sebastian Huber <sebastian.huber@embedded-brains.de>
+
+	* libchip/i2c/spi-sd-card.h, libchip/i2c/spi-sd-card.c: Removed high
+	level LibI2C interface.  Added generic block IO functions.
+
 2008-12-22	Ralf Corsépius <ralf.corsepius@rtems.org>
 
 	* optman/rtems/no-barrier.c, optman/rtems/no-dpmem.c,
diff --git a/c/src/libchip/i2c/spi-sd-card.c b/c/src/libchip/i2c/spi-sd-card.c
index f662a01939..bf2cf25b09 100644
--- a/c/src/libchip/i2c/spi-sd-card.c
+++ b/c/src/libchip/i2c/spi-sd-card.c
@@ -297,7 +297,7 @@ static inline uint32_t sd_card_max_access_time( const uint8_t *csd, uint32_t tra
 
 static inline int sd_card_query( sd_card_driver_entry *e, uint8_t *in, int n)
 {
-	return rtems_libi2c_read_bytes( e->minor, in, n);
+	return rtems_libi2c_read_bytes( e->bus, in, n);
 }
 
 static int sd_card_wait( sd_card_driver_entry *e)
@@ -346,7 +346,7 @@ static int sd_card_send_command( sd_card_driver_entry *e, uint32_t command, uint
 	/* Write command and read response */
 	SD_CARD_COMMAND_SET_COMMAND( e->command, command);
 	SD_CARD_COMMAND_SET_ARGUMENT( e->command, argument);
-	rv = rtems_libi2c_ioctl( e->minor, RTEMS_LIBI2C_IOCTL_READ_WRITE, &rw);
+	rv = rtems_libi2c_ioctl( e->bus, RTEMS_LIBI2C_IOCTL_READ_WRITE, &rw);
 	RTEMS_CHECK_RV( rv, "Write command and read response");
 
 	/* Check respose */
@@ -388,7 +388,7 @@ static int sd_card_stop_multiple_block_read( sd_card_driver_entry *e)
 	int rv = 0;
 
 	SD_CARD_COMMAND_SET_COMMAND( e->command, SD_CARD_CMD_STOP_TRANSMISSION);
-	rv = rtems_libi2c_write_bytes( e->minor, e->command, SD_CARD_COMMAND_SIZE);
+	rv = rtems_libi2c_write_bytes( e->bus, e->command, SD_CARD_COMMAND_SIZE);
 	RTEMS_CHECK_RV( rv, "Write stop transfer token");
 
 	return 0;
@@ -404,7 +404,7 @@ static int sd_card_stop_multiple_block_write( sd_card_driver_entry *e)
 	RTEMS_CHECK_RV( rv, "Wait");
 
 	/* Send stop token */
-	rv = rtems_libi2c_write_bytes( e->minor, stop_transfer, 3);
+	rv = rtems_libi2c_write_bytes( e->bus, stop_transfer, 3);
 	RTEMS_CHECK_RV( rv, "Write stop transfer token");
 
 	/* Card is now busy */
@@ -434,7 +434,7 @@ static int sd_card_read( sd_card_driver_entry *e, uint8_t start_token, uint8_t *
 
 	SD_CARD_INVALIDATE_RESPONSE_INDEX( e);
 
-	while (1) {
+	while (true) {
 		RTEMS_DEBUG_PRINT( "Search from %u to %u\n", r, response_size - 1);
 
 		/* Search the data start token in in current response buffer */
@@ -504,15 +504,15 @@ static int sd_card_write( sd_card_driver_entry *e, uint8_t start_token, uint8_t
 	RTEMS_CHECK_RV( rv, "Wait");
 
 	/* Write data start token */
-	rv = rtems_libi2c_write_bytes( e->minor, &start_token, 1);
+	rv = rtems_libi2c_write_bytes( e->bus, &start_token, 1);
 	RTEMS_CHECK_RV( rv, "Write data start token");
 
 	/* Write data */
-	o = rtems_libi2c_write_bytes( e->minor, out, n);
+	o = rtems_libi2c_write_bytes( e->bus, out, n);
 	RTEMS_CHECK_RV( o, "Write data");
 
 	/* Write CRC 16 */
-	rv = rtems_libi2c_write_bytes( e->minor, crc16, 2);
+	rv = rtems_libi2c_write_bytes( e->bus, crc16, 2);
 	RTEMS_CHECK_RV( rv, "Write CRC 16");
 
 	/* Read data response */
@@ -534,13 +534,13 @@ static inline rtems_status_code sd_card_start( sd_card_driver_entry *e)
 	rtems_status_code sc = RTEMS_SUCCESSFUL;
 	int rv = 0;
 
-	sc = rtems_libi2c_send_start( e->minor);
+	sc = rtems_libi2c_send_start( e->bus);
 	RTEMS_CHECK_SC( sc, "Send start");
 
-	rv = rtems_libi2c_ioctl( e->minor, RTEMS_LIBI2C_IOCTL_SET_TFRMODE, &e->transfer_mode);
+	rv = rtems_libi2c_ioctl( e->bus, RTEMS_LIBI2C_IOCTL_SET_TFRMODE, &e->transfer_mode);
 	RTEMS_CHECK_RV_SC( rv, "Set transfer mode");
 
-	sc = rtems_libi2c_send_addr( e->minor, 1);
+	sc = rtems_libi2c_send_addr( e->bus, 1);
 	RTEMS_CHECK_SC( sc, "Send address");
 
 	return RTEMS_SUCCESSFUL;
@@ -550,11 +550,155 @@ static inline rtems_status_code sd_card_stop( sd_card_driver_entry *e)
 {
 	rtems_status_code sc = RTEMS_SUCCESSFUL;
 
-	sc = rtems_libi2c_send_stop( e->minor);
+	sc = rtems_libi2c_send_stop( e->bus);
 	RTEMS_CHECK_SC( sc, "Send stop");
 
 	return RTEMS_SUCCESSFUL;
 }
+
+static rtems_status_code sd_card_init( sd_card_driver_entry *e)
+{
+	rtems_status_code sc = RTEMS_SUCCESSFUL;
+	int rv = 0;
+	uint8_t block [SD_CARD_BLOCK_SIZE_DEFAULT];
+	uint32_t transfer_speed = 0;
+	uint32_t read_block_size = 0;
+	uint32_t write_block_size = 0;
+
+	/* Start */
+	sc = sd_card_start( e);
+	RTEMS_CLEANUP_SC( sc, sd_card_driver_init_cleanup, "Start");
+
+	/* Wait until card is not busy */
+	rv = sd_card_wait( e);
+	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Wait");
+
+	/* Send idle tokens for at least 74 clock cycles with active chip select */
+	memset( block, SD_CARD_IDLE_TOKEN, SD_CARD_BLOCK_SIZE_DEFAULT);
+	rv = rtems_libi2c_write_bytes( e->bus, block, SD_CARD_BLOCK_SIZE_DEFAULT);
+	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Active chip select delay");
+
+	/* Stop */
+	sc = sd_card_stop( e);
+	RTEMS_CHECK_SC( sc, "Stop");
+
+	/* Start with inactive chip select */
+	sc = rtems_libi2c_send_start( e->bus);
+	RTEMS_CHECK_SC( sc, "Send start");
+
+	/* Set transfer mode */
+	rv = rtems_libi2c_ioctl( e->bus, RTEMS_LIBI2C_IOCTL_SET_TFRMODE, &e->transfer_mode);
+	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Set transfer mode");
+
+	/* Send idle tokens with inactive chip select */
+	rv = sd_card_query( e, e->response, SD_CARD_COMMAND_SIZE);
+	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Inactive chip select delay");
+
+	/* Activate chip select */
+	sc = rtems_libi2c_send_addr( e->bus, 1);
+	RTEMS_CLEANUP_SC( sc, sd_card_driver_init_cleanup, "Send address");
+
+	/* Stop multiple block write */
+	sd_card_stop_multiple_block_write( e);
+
+	/* Get card status */
+	sd_card_send_command( e, SD_CARD_CMD_SEND_STATUS, 0);
+
+	/* Stop multiple block read */
+	sd_card_stop_multiple_block_read( e);
+
+	/* Switch to SPI mode */
+	rv = sd_card_send_command( e, SD_CARD_CMD_GO_IDLE_STATE, 0);
+	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_GO_IDLE_STATE");
+
+	/* Initialize card */
+	while (true) {
+		rv = sd_card_send_command( e, SD_CARD_CMD_APP_CMD, 0);
+		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_APP_CMD");
+		rv = sd_card_send_command( e, SD_CARD_ACMD_SD_SEND_OP_COND, 0);
+		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_ACMD_SD_SEND_OP_COND");
+	
+		/* Not idle? */
+		if (SD_CARD_IS_NOT_IDLE_RESPONSE( e->response [e->response_index])) {
+			break;
+		}
+
+		/* Invoke the scheduler */
+		rtems_task_wake_after( RTEMS_YIELD_PROCESSOR);
+	};
+
+	/* Card Identification */
+	if (e->verbose) {
+		rv = sd_card_send_command( e, SD_CARD_CMD_SEND_CID, 0);
+		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_SEND_CID");
+		rv = sd_card_read( e, SD_CARD_START_BLOCK_SINGLE_BLOCK_READ, block, SD_CARD_CID_SIZE);
+		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Read: SD_CARD_CMD_SEND_CID");
+		RTEMS_SYSLOG( "*** Card Identification ***\n");
+		RTEMS_SYSLOG( "Manufacturer ID          : %" PRIu8 "\n", SD_CARD_CID_GET_MID( block));
+		RTEMS_SYSLOG( "OEM/Application ID       : %" PRIu16 "\n", SD_CARD_CID_GET_OID( block));
+		RTEMS_SYSLOG(
+			"Product name             : %c%c%c%c%c%c\n",
+			SD_CARD_CID_GET_PNM( block, 0),
+			SD_CARD_CID_GET_PNM( block, 1),
+			SD_CARD_CID_GET_PNM( block, 2),
+			SD_CARD_CID_GET_PNM( block, 3),
+			SD_CARD_CID_GET_PNM( block, 4),
+			SD_CARD_CID_GET_PNM( block, 5)
+		);
+		RTEMS_SYSLOG( "Product revision         : %" PRIu8 "\n", SD_CARD_CID_GET_PRV( block));
+		RTEMS_SYSLOG( "Product serial number    : %" PRIu32 "\n", SD_CARD_CID_GET_PSN( block));
+		RTEMS_SYSLOG( "Manufacturing date       : %" PRIu8 "\n", SD_CARD_CID_GET_MDT( block));
+		RTEMS_SYSLOG( "7-bit CRC checksum       : %" PRIu8 "\n",  SD_CARD_CID_GET_CRC7( block));
+	}
+
+	/* Card Specific Data */
+	rv = sd_card_send_command( e, SD_CARD_CMD_SEND_CSD, 0);
+	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_SEND_CSD");
+	rv = sd_card_read( e, SD_CARD_START_BLOCK_SINGLE_BLOCK_READ, block, SD_CARD_CSD_SIZE);
+	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Read: SD_CARD_CMD_SEND_CSD");
+	transfer_speed = sd_card_transfer_speed( block);
+	e->transfer_mode.baudrate = transfer_speed;
+	e->n_ac_max = sd_card_max_access_time( block, transfer_speed);
+	read_block_size = 1U << SD_CARD_CSD_GET_READ_BLK_LEN( block);
+	e->block_size_shift = SD_CARD_CSD_GET_READ_BLK_LEN( block);
+	write_block_size = 1U << e->block_size_shift;
+	if (read_block_size < write_block_size) {
+		RTEMS_SYSLOG_ERROR( "Read block size smaller than write block size\n");
+		return -RTEMS_IO_ERROR;
+	}
+	e->block_size = write_block_size;
+	e->block_number = sd_card_block_number( block);
+	if (e->verbose) {
+		RTEMS_SYSLOG( "*** Card Specific Data ***\n");
+		RTEMS_SYSLOG( "CSD structure            : %" PRIu8 "\n", SD_CARD_CSD_GET_CSD_STRUCTURE( block));
+		RTEMS_SYSLOG( "Spec version             : %" PRIu8 "\n", SD_CARD_CSD_GET_SPEC_VERS( block));
+		RTEMS_SYSLOG( "Access time [ns]         : %" PRIu32 "\n", sd_card_access_time( block));
+		RTEMS_SYSLOG( "Max access time [N]      : %" PRIu32 "\n", e->n_ac_max);
+		RTEMS_SYSLOG( "Max read block size [B]  : %" PRIu32 "\n", read_block_size);
+		RTEMS_SYSLOG( "Max write block size [B] : %" PRIu32 "\n", write_block_size);
+		RTEMS_SYSLOG( "Block size [B]           : %" PRIu32 "\n", e->block_size);
+		RTEMS_SYSLOG( "Block number             : %" PRIu32 "\n", e->block_number);
+		RTEMS_SYSLOG( "Capacity [B]             : %" PRIu32 "\n", sd_card_capacity( block));
+		RTEMS_SYSLOG( "Max transfer speed [b/s] : %" PRIu32 "\n", transfer_speed);
+	}
+
+	/* Set read block size */
+	rv = sd_card_send_command( e, SD_CARD_CMD_SET_BLOCKLEN, e->block_size);
+	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_SET_BLOCKLEN");
+
+	/* Stop */
+	sc = sd_card_stop( e);
+	RTEMS_CHECK_SC( sc, "Stop");
+
+	return RTEMS_SUCCESSFUL;
+
+sd_card_driver_init_cleanup:
+
+	/* Stop */
+	sd_card_stop( e);
+
+	return sc;
+}
 /** @} */
 
 /**
@@ -569,11 +713,9 @@ static int sd_card_disk_block_read( sd_card_driver_entry *e, rtems_blkdev_reques
 	uint32_t start_address = RTEMS_BLKDEV_START_BLOCK (r) << e->block_size_shift;
 	uint32_t i = 0;
 
-	RTEMS_DEBUG_PRINT( "start = %u, bufnum = %u\n", r->start, r->bufnum);
-
 #ifdef DEBUG
 	/* Check request */
-  if (r->bufs[0].block >= e->block_number) {
+	if (r->bufs[0].block >= e->block_number) {
 		RTEMS_SYSLOG_ERROR( "Start block number out of range");
 		return -RTEMS_INTERNAL_ERROR;
 	} else if (r->bufnum > e->block_number - RTEMS_BLKDEV_START_BLOCK (r)) {
@@ -592,7 +734,7 @@ static int sd_card_disk_block_read( sd_card_driver_entry *e, rtems_blkdev_reques
 		if (r->bufs [0].length != e->block_size) {
 			RTEMS_DO_CLEANUP_RV( -RTEMS_INTERNAL_ERROR, rv, sd_card_disk_block_read_cleanup, "Buffer and disk block size are not equal");
 		}
-		RTEMS_DEBUG_PRINT( "[%02u]: buffer = 0x%08x, size = %u\n", 0, r->bufs [0].buffer, r->bufs [0].length);
+		RTEMS_DEBUG_PRINT( "[01:01]: buffer = 0x%08x, size = %u\n", r->bufs [0].buffer, r->bufs [0].length);
 #endif /* DEBUG */
 
 		/* Single block read */
@@ -612,7 +754,7 @@ static int sd_card_disk_block_read( sd_card_driver_entry *e, rtems_blkdev_reques
 			if (r->bufs [i].length != e->block_size) {
 				RTEMS_DO_CLEANUP_RV( -RTEMS_INTERNAL_ERROR, rv, sd_card_disk_block_read_stop_cleanup, "Buffer and disk block size are not equal");
 			}
-			RTEMS_DEBUG_PRINT( "[%02u]: buffer = 0x%08x, size = %u\n", i, r->bufs [i].buffer, r->bufs [i].length);
+			RTEMS_DEBUG_PRINT( "[%02u:%02u]: buffer = 0x%08x, size = %u\n", i + 1, r->bufnum, r->bufs [i].buffer, r->bufs [i].length);
 #endif /* DEBUG */
 
 			rv = sd_card_read( e, SD_CARD_START_BLOCK_MULTIPLE_BLOCK_READ, (uint8_t *) r->bufs [i].buffer, (int) e->block_size);
@@ -656,11 +798,9 @@ static int sd_card_disk_block_write( sd_card_driver_entry *e, rtems_blkdev_reque
 	uint32_t start_address = RTEMS_BLKDEV_START_BLOCK (r) << e->block_size_shift;
 	uint32_t i = 0;
 
-	RTEMS_DEBUG_PRINT( "start = %u, count = %u, bufnum = %u\n", r->start, r->count, r->bufnum);
-
 #ifdef DEBUG
 	/* Check request */
-  if (r->bufs[0].block >= e->block_number) {
+	if (r->bufs[0].block >= e->block_number) {
 		RTEMS_SYSLOG_ERROR( "Start block number out of range");
 		return -RTEMS_INTERNAL_ERROR;
 	} else if (r->bufnum > e->block_number - RTEMS_BLKDEV_START_BLOCK (r)) {
@@ -679,7 +819,7 @@ static int sd_card_disk_block_write( sd_card_driver_entry *e, rtems_blkdev_reque
 		if (r->bufs [0].length != e->block_size) {
 			RTEMS_DO_CLEANUP_RV( -RTEMS_INTERNAL_ERROR, rv, sd_card_disk_block_write_cleanup, "Buffer and disk block size are not equal");
 		}
-		RTEMS_DEBUG_PRINT( "[%02u]: buffer = 0x%08x, size = %u\n", 0, r->bufs [0].buffer, r->bufs [0].length);
+		RTEMS_DEBUG_PRINT( "[01:01]: buffer = 0x%08x, size = %u\n", r->bufs [0].buffer, r->bufs [0].length);
 #endif /* DEBUG */
 
 		/* Single block write */
@@ -699,7 +839,7 @@ static int sd_card_disk_block_write( sd_card_driver_entry *e, rtems_blkdev_reque
 			if (r->bufs [i].length != e->block_size) {
 				RTEMS_DO_CLEANUP_RV( -RTEMS_INTERNAL_ERROR, rv, sd_card_disk_block_write_stop_cleanup, "Buffer and disk block size are not equal");
 			}
-			RTEMS_DEBUG_PRINT( "[%02u]: buffer = 0x%08x, size = %u\n", i, r->bufs [i].buffer, r->bufs [i].length);
+			RTEMS_DEBUG_PRINT( "[%02u:%02u]: buffer = 0x%08x, size = %u\n", i + 1, r->bufnum, r->bufs [i].buffer, r->bufs [i].length);
 #endif /* DEBUG */
 
 			rv = sd_card_write( e, SD_CARD_START_BLOCK_MULTIPLE_BLOCK_WRITE, (uint8_t *) r->bufs [i].buffer, (int) e->block_size);
@@ -771,380 +911,48 @@ static int sd_card_disk_ioctl( dev_t dev, uint32_t req, void *arg)
 
 static rtems_status_code sd_card_disk_init( rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
 {
-	/* Do nothing */
-
-	return RTEMS_SUCCESSFUL;
-}
-
-/** @} */
-
-static const rtems_driver_address_table sd_card_disk_ops = {
-	.initialization_entry = sd_card_disk_init,
-	.open_entry = NULL,
-	.close_entry = NULL,
-	.read_entry = NULL,
-	.write_entry = NULL,
-	.control_entry = NULL
-};
-
-static rtems_device_major_number sd_card_disk_major = 0;
-
-static int sd_card_driver_first = 1;
-
-/**
- * @name LibI2C Driver Functions
- * @{
- */
-
-static inline int sd_card_driver_get_entry( rtems_device_minor_number minor, sd_card_driver_entry **e)
-{
-	return rtems_libi2c_ioctl( minor, RTEMS_LIBI2C_IOCTL_GET_DRV_T, e);
-}
-
-static rtems_status_code sd_card_driver_init( rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
-{
 	rtems_status_code sc = RTEMS_SUCCESSFUL;
-	int rv = 0;
 	sd_card_driver_entry *e = NULL;
-	uint8_t block [SD_CARD_BLOCK_SIZE_DEFAULT];
-	uint32_t transfer_speed = 0;
-	uint32_t read_block_size = 0;
-	uint32_t write_block_size = 0;
 	dev_t dev = 0;
 
-	/* Get driver entry */
-	rv = sd_card_driver_get_entry( minor, &e);
-	RTEMS_CHECK_RV_SC( rv, "Get driver entry");
-
-	/* Start */
-	sc = sd_card_start( e);
-	RTEMS_CLEANUP_SC( sc, sd_card_driver_init_cleanup, "Start");
-
-	/* Save minor number for disk operations */
-	e->minor = minor;
-
-	/* Register disk driver */
-	if (sd_card_driver_first) {
-		sd_card_driver_first = 0;
-		sc = rtems_io_register_driver( 0, &sd_card_disk_ops, &sd_card_disk_major);
-		RTEMS_CLEANUP_SC( sc, sd_card_driver_init_cleanup, "Register disk IO driver");
-	}
-
-	/* Wait until card is not busy */
-	rv = sd_card_wait( e);
-	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Wait");
-
-	/* Send idle tokens for at least 74 clock cycles with active chip select */
-	memset( block, SD_CARD_IDLE_TOKEN, SD_CARD_BLOCK_SIZE_DEFAULT);
-	rv = rtems_libi2c_write_bytes( e->minor, block, SD_CARD_BLOCK_SIZE_DEFAULT);
-	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Active chip select delay");
-
-	/* Stop */
-	sc = sd_card_stop( e);
-	RTEMS_CHECK_SC( sc, "Stop");
-
-	/* Start with inactive chip select */
-	sc = rtems_libi2c_send_start( e->minor);
-	RTEMS_CHECK_SC( sc, "Send start");
-
-	/* Set transfer mode */
-	rv = rtems_libi2c_ioctl( e->minor, RTEMS_LIBI2C_IOCTL_SET_TFRMODE, &e->transfer_mode);
-	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Set transfer mode");
-
-	/* Send idle tokens with inactive chip select */
-	rv = sd_card_query( e, e->response, SD_CARD_COMMAND_SIZE);
-	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Inactive chip select delay");
-
-	/* Activate chip select */
-	sc = rtems_libi2c_send_addr( e->minor, 1);
-	RTEMS_CLEANUP_SC( sc, sd_card_driver_init_cleanup, "Send address");
-
-	/* Stop multiple block write */
-	sd_card_stop_multiple_block_write( e);
-
-	/* Get card status */
-	sd_card_send_command( e, SD_CARD_CMD_SEND_STATUS, 0);
-
-	/* Stop multiple block read */
-	sd_card_stop_multiple_block_read( e);
-
-	/* Switch to SPI mode */
-	rv = sd_card_send_command( e, SD_CARD_CMD_GO_IDLE_STATE, 0);
-	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_GO_IDLE_STATE");
-
-	/* Initialize card */
-	while (1) {
-		rv = sd_card_send_command( e, SD_CARD_CMD_APP_CMD, 0);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_APP_CMD");
-		rv = sd_card_send_command( e, SD_CARD_ACMD_SD_SEND_OP_COND, 0);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_ACMD_SD_SEND_OP_COND");
-	
-		/* Not idle? */
-		if (SD_CARD_IS_NOT_IDLE_RESPONSE( e->response [e->response_index])) {
-			break;
-		}
-
-		/* Invoke the scheduler */
-		rtems_task_wake_after( RTEMS_YIELD_PROCESSOR);
-	};
-
-	/* Card Identification */
-	if (e->verbose) {
-		rv = sd_card_send_command( e, SD_CARD_CMD_SEND_CID, 0);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_SEND_CID");
-		rv = sd_card_read( e, SD_CARD_START_BLOCK_SINGLE_BLOCK_READ, block, SD_CARD_CID_SIZE);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Read: SD_CARD_CMD_SEND_CID");
-		RTEMS_SYSLOG( "*** Card Identification ***\n");
-		RTEMS_SYSLOG( "Manufacturer ID          : %" PRIu8 "\n", SD_CARD_CID_GET_MID( block));
-		RTEMS_SYSLOG( "OEM/Application ID       : %" PRIu16 "\n", SD_CARD_CID_GET_OID( block));
-		RTEMS_SYSLOG(
-			"Product name             : %c%c%c%c%c%c\n",
-			SD_CARD_CID_GET_PNM( block, 0),
-			SD_CARD_CID_GET_PNM( block, 1),
-			SD_CARD_CID_GET_PNM( block, 2),
-			SD_CARD_CID_GET_PNM( block, 3),
-			SD_CARD_CID_GET_PNM( block, 4),
-			SD_CARD_CID_GET_PNM( block, 5)
-		);
-		RTEMS_SYSLOG( "Product revision         : %" PRIu8 "\n", SD_CARD_CID_GET_PRV( block));
-		RTEMS_SYSLOG( "Product serial number    : %" PRIu32 "\n", SD_CARD_CID_GET_PSN( block));
-		RTEMS_SYSLOG( "Manufacturing date       : %" PRIu8 "\n", SD_CARD_CID_GET_MDT( block));
-		RTEMS_SYSLOG( "7-bit CRC checksum       : %" PRIu8 "\n",  SD_CARD_CID_GET_CRC7( block));
-	}
-
-	/* Card Specific Data */
-	rv = sd_card_send_command( e, SD_CARD_CMD_SEND_CSD, 0);
-	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_SEND_CSD");
-	rv = sd_card_read( e, SD_CARD_START_BLOCK_SINGLE_BLOCK_READ, block, SD_CARD_CSD_SIZE);
-	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Read: SD_CARD_CMD_SEND_CSD");
-	transfer_speed = sd_card_transfer_speed( block);
-	e->transfer_mode.baudrate = transfer_speed;
-	e->n_ac_max = sd_card_max_access_time( block, transfer_speed);
-	read_block_size = 1U << SD_CARD_CSD_GET_READ_BLK_LEN( block);
-	e->block_size_shift = SD_CARD_CSD_GET_READ_BLK_LEN( block);
-	write_block_size = 1U << e->block_size_shift;
-	if (read_block_size < write_block_size) {
-		RTEMS_SYSLOG_ERROR( "Read block size smaller than write block size\n");
-		return -RTEMS_IO_ERROR;
-	}
-	e->block_size = write_block_size;
-	e->block_number = sd_card_block_number( block);
-	if (e->verbose) {
-		RTEMS_SYSLOG( "*** Card Specific Data ***\n");
-		RTEMS_SYSLOG( "CSD structure            : %" PRIu8 "\n", SD_CARD_CSD_GET_CSD_STRUCTURE( block));
-		RTEMS_SYSLOG( "Spec version             : %" PRIu8 "\n", SD_CARD_CSD_GET_SPEC_VERS( block));
-		RTEMS_SYSLOG( "Access time [ns]         : %" PRIu32 "\n", sd_card_access_time( block));
-		RTEMS_SYSLOG( "Max access time [N]      : %" PRIu32 "\n", e->n_ac_max);
-		RTEMS_SYSLOG( "Max read block size [B]  : %" PRIu32 "\n", read_block_size);
-		RTEMS_SYSLOG( "Max write block size [B] : %" PRIu32 "\n", write_block_size);
-		RTEMS_SYSLOG( "Block size [B]           : %" PRIu32 "\n", e->block_size);
-		RTEMS_SYSLOG( "Block number             : %" PRIu32 "\n", e->block_number);
-		RTEMS_SYSLOG( "Capacity [B]             : %" PRIu32 "\n", sd_card_capacity( block));
-		RTEMS_SYSLOG( "Max transfer speed [b/s] : %" PRIu32 "\n", transfer_speed);
-	}
-
-	/* Set read block size */
-	rv = sd_card_send_command( e, SD_CARD_CMD_SET_BLOCKLEN, e->block_size);
-	RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_init_cleanup, "Send: SD_CARD_CMD_SET_BLOCKLEN");
-
-	/* Create disk device */
-	dev = rtems_filesystem_make_dev_t( sd_card_disk_major, (rtems_device_minor_number) e->table_index);
+	/* Initialize disk IO */
 	sc = rtems_disk_io_initialize();
-	RTEMS_CLEANUP_SC( sc, sd_card_driver_init_cleanup, "Initialize RTEMS disk IO");
-	sc = rtems_disk_create_phys( dev, (int) e->block_size, (int) e->block_number, sd_card_disk_ioctl, e->disk_device_name);
-	RTEMS_CLEANUP_SC( sc, sd_card_driver_init_cleanup, "Create disk device");
-
-	/* Stop */
-	sc = sd_card_stop( e);
-	RTEMS_CHECK_SC( sc, "Stop");
+	RTEMS_CHECK_SC( sc, "Initialize RTEMS disk IO");
 
-	return RTEMS_SUCCESSFUL;
-
-sd_card_driver_init_cleanup:
-
-	/* Stop */
-	sd_card_stop( e);
-
-	return sc;
-}
-
-static rtems_status_code sd_card_driver_read( rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
-{
-	rtems_status_code sc = RTEMS_SUCCESSFUL;
-	int rv = 0;
-	rtems_libio_rw_args_t *rw = (rtems_libio_rw_args_t *) arg;
-	sd_card_driver_entry *e = NULL;
-	uint32_t block_size_mask = 0;
-	uint32_t block_count = 0;
-	uint32_t start_block = 0;
-	uint32_t i = 0;
-
-	/* Clear moved bytes counter */
-	rw->bytes_moved = 0;
-
-	/* Get driver entry */
-	rv = sd_card_driver_get_entry( minor, &e);
-	RTEMS_CHECK_RV_SC( rv, "Get driver entry");
-
-	/* Start */
-	sc = sd_card_start( e);
-	RTEMS_CLEANUP_SC( sc, sd_card_driver_read_cleanup, "Start");
-
-	/* Check arguments */
-	block_size_mask = e->block_size - 1;
-	block_count = (uint32_t) rw->count >> e->block_size_shift;
-	start_block = (uint32_t) rw->offset >> e->block_size_shift;
-	if (rw->offset & block_size_mask) {
-		RTEMS_DO_CLEANUP_SC( RTEMS_INVALID_ADDRESS, sc, sd_card_driver_read_cleanup, "Invalid offset");
-	} else if ((rw->count & block_size_mask) || (start_block >= e->block_number) || (block_count > e->block_number - start_block)) {
-		RTEMS_DO_CLEANUP_SC( RTEMS_INVALID_NUMBER, sc, sd_card_driver_read_cleanup, "Invalid count or out of range");
-	}
-
-	if (block_count == 0) {
-		/* Do nothing */
-	} else if (block_count == 1) {
-		/* Single block read */
-		rv = sd_card_send_command( e, SD_CARD_CMD_READ_SINGLE_BLOCK, (uint32_t) rw->offset);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_read_cleanup, "Send: SD_CARD_CMD_READ_SINGLE_BLOCK");
-		rv = sd_card_read( e, SD_CARD_START_BLOCK_SINGLE_BLOCK_READ, (uint8_t *) rw->buffer, (int) e->block_size);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_read_cleanup, "Read: SD_CARD_CMD_READ_SINGLE_BLOCK");
-
-		/* Set moved bytes counter */
-		rw->bytes_moved = (uint32_t) rv;
-	} else {
-		/* Start multiple block read */
-		rv = sd_card_send_command( e, SD_CARD_CMD_READ_MULTIPLE_BLOCK, (uint32_t) rw->offset);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_read_stop_cleanup, "Send: SD_CARD_CMD_READ_MULTIPLE_BLOCK");
-
-		/* Multiple block read */
-		for (i = 0; i < block_count; ++i) {
-			rv = sd_card_read( e, SD_CARD_START_BLOCK_MULTIPLE_BLOCK_READ, (uint8_t *) rw->buffer + (i << e->block_size_shift), (int) e->block_size);
-			RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_read_stop_cleanup, "Read block");
+	for (minor = 0; minor < sd_card_driver_table_size; ++minor) {
+		e = &sd_card_driver_table [minor];
 
-			/* Update moved bytes counter */
-			rw->bytes_moved += (uint32_t) rv;
-		}
+		/* Initialize SD Card */
+		sc = sd_card_init( e);
+		RTEMS_CHECK_SC( sc, "Initialize SD Card");
 
-		/* Stop multiple block read */
-		rv = sd_card_stop_multiple_block_read( e);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_read_cleanup, "Stop multiple block read");
+		/* Create disk device */
+		dev = rtems_filesystem_make_dev_t( major, minor);
+		sc = rtems_disk_create_phys( dev, (int) e->block_size, (int) e->block_number, sd_card_disk_ioctl, e->device_name);
+		RTEMS_CHECK_SC( sc, "Create disk device");
 	}
 
-	/* Stop */
-	sc = sd_card_stop( e);
-	RTEMS_CHECK_SC( sc, "Stop");
-
 	return RTEMS_SUCCESSFUL;
+}
 
-sd_card_driver_read_stop_cleanup:
-
-	/* Stop multiple block read */
-	sd_card_stop_multiple_block_read( e);
-
-sd_card_driver_read_cleanup:
-
-	/* Stop */
-	sd_card_stop( e);
+/** @} */
 
-	return sc;
-}
+static const rtems_driver_address_table sd_card_disk_ops = {
+	.initialization_entry = sd_card_disk_init,
+	.open_entry = rtems_blkdev_generic_open,
+	.close_entry = rtems_blkdev_generic_close,
+	.read_entry = rtems_blkdev_generic_read,
+	.write_entry = rtems_blkdev_generic_write,
+	.control_entry = rtems_blkdev_generic_ioctl
+};
 
-static rtems_status_code sd_card_driver_write( rtems_device_major_number major, rtems_device_minor_number minor, void *arg)
+rtems_status_code sd_card_register( void)
 {
 	rtems_status_code sc = RTEMS_SUCCESSFUL;
-	int rv = 0;
-	rtems_libio_rw_args_t *rw = (rtems_libio_rw_args_t *) arg;
-	sd_card_driver_entry *e = NULL;
-	uint32_t block_size_mask = 0;
-	uint32_t block_count = 0;
-	uint32_t start_block = 0;
-	uint32_t i = 0;
-
-	/* Clear moved bytes counter */
-	rw->bytes_moved = 0;
-
-	/* Get driver entry */
-	rv = sd_card_driver_get_entry( minor, &e);
-	RTEMS_CHECK_RV_SC( rv, "Get driver entry");
-
-	/* Start */
-	sc = sd_card_start( e);
-	RTEMS_CLEANUP_SC( sc, sd_card_driver_write_cleanup, "Start");
-
-	/* Check arguments */
-	block_size_mask = e->block_size - 1;
-	block_count = (uint32_t) rw->count >> e->block_size_shift;
-	start_block = (uint32_t) rw->offset >> e->block_size_shift;
-	if (rw->offset & block_size_mask) {
-		RTEMS_DO_CLEANUP_SC( RTEMS_INVALID_ADDRESS, sc, sd_card_driver_write_cleanup, "Invalid offset");
-	} else if ((rw->count & block_size_mask) || (start_block >= e->block_number) || (block_count > e->block_number - start_block)) {
-		RTEMS_DO_CLEANUP_SC( RTEMS_INVALID_NUMBER, sc, sd_card_driver_write_cleanup, "Invalid count or out of range");
-	}
-
-	if (block_count == 0) {
-		/* Do nothing */
-	} else if (block_count == 1) {
-		/* Single block write */
-		rv = sd_card_send_command( e, SD_CARD_CMD_WRITE_BLOCK, (uint32_t) rw->offset);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_write_cleanup, "Send: SD_CARD_CMD_WRITE_BLOCK");
-		rv = sd_card_write( e, SD_CARD_START_BLOCK_SINGLE_BLOCK_WRITE, (uint8_t *) rw->buffer, (int) e->block_size);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_write_cleanup, "Write: SD_CARD_CMD_WRITE_BLOCK");
-
-		/* Set moved bytes counter */
-		rw->bytes_moved = (uint32_t) rv;
-	} else {
-		/* Start multiple block write */
-		rv = sd_card_send_command( e, SD_CARD_CMD_WRITE_MULTIPLE_BLOCK, (uint32_t) rw->offset);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_write_stop_cleanup, "Send: SD_CARD_CMD_WRITE_MULTIPLE_BLOCK");
-
-		/* Multiple block write */
-		for (i = 0; i < block_count; ++i) {
-			rv = sd_card_write( e, SD_CARD_START_BLOCK_MULTIPLE_BLOCK_WRITE, (uint8_t *) rw->buffer + (i << e->block_size_shift), (int) e->block_size);
-			RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_write_stop_cleanup, "Write: SD_CARD_CMD_WRITE_MULTIPLE_BLOCK");
-
-			/* Update moved bytes counter */
-			rw->bytes_moved += (uint32_t) rv;
-		}
-
-		/* Stop multiple block write */
-		rv = sd_card_stop_multiple_block_write( e);
-		RTEMS_CLEANUP_RV_SC( rv, sc, sd_card_driver_write_cleanup, "Stop multiple block write");
-	}
-
-	/* Get card status */
-	rv = sd_card_send_command( e, SD_CARD_CMD_SEND_STATUS, 0);
-	RTEMS_CHECK_RV( rv, "Send: SD_CARD_CMD_SEND_STATUS");
+	rtems_device_major_number major = 0;
 
-	/* Stop */
-	sc = sd_card_stop( e);
-	RTEMS_CHECK_SC( sc, "Stop");
+	sc = rtems_io_register_driver( 0, &sd_card_disk_ops, &major);
+	RTEMS_CHECK_SC( sc, "Register disk SD Card driver");
 
 	return RTEMS_SUCCESSFUL;
-
-sd_card_driver_write_stop_cleanup:
-
-	/* Stop multiple block write */
-	sd_card_stop_multiple_block_write( e);
-
-sd_card_driver_write_cleanup:
-
-	/* Get card status */
-	rv = sd_card_send_command( e, SD_CARD_CMD_SEND_STATUS, 0);
-	RTEMS_CHECK_RV( rv, "Send: SD_CARD_CMD_SEND_STATUS");
-
-	/* Stop */
-	sd_card_stop( e);
-
-	return sc;
 }
-
-/** @} */
-
-const rtems_driver_address_table sd_card_driver_ops = {
-	.initialization_entry = sd_card_driver_init,
-	.open_entry = NULL,
-	.close_entry = NULL,
-	.read_entry = sd_card_driver_read,
-	.write_entry = sd_card_driver_write,
-	.control_entry = NULL
-};
diff --git a/c/src/libchip/i2c/spi-sd-card.h b/c/src/libchip/i2c/spi-sd-card.h
index 49d901d2ce..97a7918c70 100644
--- a/c/src/libchip/i2c/spi-sd-card.h
+++ b/c/src/libchip/i2c/spi-sd-card.h
@@ -55,11 +55,8 @@ extern "C" {
 #define SD_CARD_N_AC_MAX_DEFAULT 8
 
 typedef struct {
-	rtems_libi2c_drv_t driver;
-	int table_index;
-	rtems_device_minor_number minor;
 	const char *device_name;
-	const char *disk_device_name;
+	int bus;
 	rtems_libi2c_tfr_mode_t transfer_mode;
 	uint8_t command [SD_CARD_COMMAND_SIZE];
 	uint8_t response [SD_CARD_COMMAND_SIZE];
@@ -73,10 +70,12 @@ typedef struct {
 	bool schedule_if_busy;
 } sd_card_driver_entry;
 
-extern const rtems_driver_address_table sd_card_driver_ops;
-
 extern sd_card_driver_entry sd_card_driver_table [];
 
+extern size_t sd_card_driver_table_size;
+
+rtems_status_code sd_card_register( void);
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */