[PATCH] SDHC driver for PowerPC QoriQ
Nickolay Semyonov
nbkolchin at gmail.com
Mon Jul 25 01:47:42 UTC 2016
From: Nickolay Semyonov <snob at wolpike.com>
Heavily tested with iozone.
---
c/src/lib/libbsp/powerpc/qoriq/Makefile.am | 3 +
c/src/lib/libbsp/powerpc/qoriq/esdhc/esdhc.c | 1215 ++++++++++++++++++++++++++
c/src/lib/libbsp/powerpc/qoriq/include/bsp.h | 2 +
3 files changed, 1220 insertions(+)
create mode 100644 c/src/lib/libbsp/powerpc/qoriq/esdhc/esdhc.c
diff --git a/c/src/lib/libbsp/powerpc/qoriq/Makefile.am b/c/src/lib/libbsp/powerpc/qoriq/Makefile.am
index dea5964..44a67b4 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/Makefile.am
+++ b/c/src/lib/libbsp/powerpc/qoriq/Makefile.am
@@ -109,6 +109,9 @@ libbsp_a_SOURCES += shmsupp/lock.S \
shmsupp/intercom.c \
shmsupp/intercom-mpci.c
+# ESDHC
+libbsp_a_SOURCES += esdhc/esdhc.c
+
libbsp_a_LIBADD = ../../../libcpu/@RTEMS_CPU@/shared/cpuIdent.rel \
../../../libcpu/@RTEMS_CPU@/shared/cache.rel \
../../../libcpu/@RTEMS_CPU@/@exceptions@/rtems-cpu.rel \
diff --git a/c/src/lib/libbsp/powerpc/qoriq/esdhc/esdhc.c b/c/src/lib/libbsp/powerpc/qoriq/esdhc/esdhc.c
new file mode 100644
index 0000000..ec06837
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/qoriq/esdhc/esdhc.c
@@ -0,0 +1,1215 @@
+/****************************************************************************************
+ * QorIQ ESDHC memory card driver for RTEMS.
+ *
+ * Copyright (c) 2016 Wolpike LTD.
+ * Author: Evgeniy Bobkov evgen at wolpike.com
+ *
+ * At the moment this driver only supports the SD cards, MMC cards are not supported.
+ ****************************************************************************************/
+
+#define RTEMS_STATUS_CHECKS_USE_PRINTK
+
+#include <rtems.h>
+#include <rtems/libio.h>
+#include <rtems/diskdevs.h>
+#include <rtems/blkdev.h>
+#include <rtems/status-checks.h>
+#include <errno.h>
+#include <rtems/irq.h>
+#include <bsp/qoriq.h>
+#include <bsp.h>
+#include <sys/endian.h>
+
+#ifndef RTEMS_BSP_ESDHC_MEMCARD_DEVICE_PATH
+#define RTEMS_BSP_ESDHC_MEMCARD_DEVICE_PATH "/dev/memcard"
+#endif
+
+/*
+ * Set to: 0 - no debugging messages, 1 - only error and some init messages,
+ * 2 - all init mesages, 3 - print each read/write request from upper layer,
+ * 4 - print each SD command, 5 - print all transferred data.
+ */
+#define DEBUG 1
+
+/* Use DMA transfers (1 = DMA, 0 = PIO). */
+#define USE_DMA 1
+
+/*
+ * If SD read or write command fails (for example due to data timeout),
+ * we can retry the attempt. Attention: minimal value for this macro is 1,
+ * because it includes the very first attempt.
+ */
+#define MAX_READ_WRITE_RETRIES 5
+
+/* Measure the delays of operations inside esdhc_exec_command() function */
+#define MEASURE_EXECCMD_DELAYS 0
+
+/*
+ * Check that write operation was ok by reading back what was written
+ * to SD card and comparing the write and readback buffers.
+ */
+#define CHECK_WRITE_BY_READBACK 0
+
+/* Run the card read speed test after ESDHC initialization */
+#define RUN_CARD_READ_SPEED_TEST 0
+
+/* Run the card write speed test after ESDHC initialization. SD CARD DATA WILL BE DESTROYED! */
+#define RUN_CARD_WRITE_SPEED_TEST 0
+
+/*** Standard SD commands ***************************************************************/
+
+/* These bits form the command type within the command spec */
+#define SD_R_CRC 0x0100 /* CRC presents */
+#define SD_R_CMD_CODE 0x0200 /* Response must contain the same command code as the request */
+#define SD_R_NORESP 0x0400 /* No response is expected */
+#define SD_R_136 0x0800 /* Response length is 136 bits */
+#define SD_R_48 0x1000 /* Response length is 48 bits */
+#define SD_R_48_BUSY 0x2000 /* Response length is 48 bits, check busy after response */
+#define SD_T_READ 0x4000 /* This is a read command */
+#define SD_T_WRITE 0x8000 /* This is a write command */
+
+/* Command response types */
+#define SD_R0 (SD_R_NORESP)
+#define SD_R1 (SD_R_CRC | SD_R_CMD_CODE | SD_R_48)
+#define SD_R1b (SD_R_CRC | SD_R_CMD_CODE | SD_R_48_BUSY)
+#define SD_R2 (SD_R_CRC | SD_R_136)
+#define SD_R3 (SD_R_48)
+#define SD_R4 (SD_R_48)
+#define SD_R5 (SD_R_CRC | SD_R_CMD_CODE | SD_R_48)
+#define SD_R6 (SD_R_CRC | SD_R_CMD_CODE | SD_R_48)
+#define SD_R7 (SD_R_CRC | SD_R_CMD_CODE | SD_R_48)
+
+/* Command specs */
+#define SD_CMD0 (0 | SD_R0)
+#define SD_CMD2 (2 | SD_R2)
+#define SD_CMD3 (3 | SD_R1)
+#define SD_CMD6 (6 | SD_R1 | SD_T_READ)
+#define SD_CMD7 (7 | SD_R1b)
+#define SD_CMD8 (8 | SD_R7)
+#define SD_CMD9 (9 | SD_R2)
+#define SD_CMD16 (16 | SD_R1)
+#define SD_CMD17 (17 | SD_R1 | SD_T_READ)
+#define SD_CMD18 (18 | SD_R1 | SD_T_READ)
+#define SD_CMD24 (24 | SD_R1 | SD_T_WRITE)
+#define SD_CMD25 (25 | SD_R1 | SD_T_WRITE)
+#define SD_CMD55 (55 | SD_R1)
+#define SD_ACMD6 (6 | SD_R1)
+#define SD_ACMD41 (41 | SD_R3)
+#define SD_ACMD51 (51 | SD_R1 | SD_T_READ)
+
+/*** Certain bits of certain hardware registers *****************************************/
+
+#define BLKATTR_SIZE_MASK 0x0000ffff
+#define BLKATTR_SIZE_SHIFT 0
+#define BLKATTR_CNT_MASK 0xffff0000
+#define BLKATTR_CNT_SHIFT 16
+
+#define XFERTYP_DMAEN 0x00000001
+#define XFERTYP_BCEN 0x00000002
+#define XFERTYP_AC12EN 0x00000004
+#define XFERTYP_DTDSEL 0x00000010
+#define XFERTYP_MSBSEL 0x00000020
+#define XFERTYP_RSPTYP_NORESP 0x00000000
+#define XFERTYP_RSPTYP_136 0x00010000
+#define XFERTYP_RSPTYP_48 0x00020000
+#define XFERTYP_RSPTYP_48_BUSY 0x00030000
+#define XFERTYP_CCCEN 0x00080000
+#define XFERTYP_CICEN 0x00100000
+#define XFERTYP_DPSEL 0x00200000
+#define XFERTYP_CMDTYP_NORMAL 0x00000000
+#define XFERTYP_CMDTYP_ABORT 0x00c00000
+#define XFERTYP_CMD_MASK 0x3f000000
+#define XFERTYP_CMD_SHIFT 24
+
+#define PRSSTAT_CIHB 0x00000001
+#define PRSSTAT_CDIHB 0x00000002
+#define PRSSTAT_DLA 0x00000004
+#define PRSSTAT_IPGOFF 0x00000010
+#define PRSSTAT_HCKOFF 0x00000020
+#define PRSSTAT_PEROFF 0x00000040
+#define PRSSTAT_SDOFF 0x00000080
+#define PRSSTAT_WTA 0x00000100
+#define PRSSTAT_RTA 0x00000200
+#define PRSSTAT_BWEN 0x00000400
+#define PRSSTAT_BREN 0x00000800
+#define PRSSTAT_CINS 0x00010000
+#define PRSSTAT_CDPL 0x00040000
+#define PRSSTAT_WPSPL 0x00080000
+#define PRSSTAT_CLSL 0x00800000
+#define PRSSTAT_DAT0_MASK 0xff000000
+#define PRSSTAT_DAT0_SHIFT 24
+
+#define PROCTL_DTW_MASK 0x00000006
+#define PROCTL_DTW_4BIT 0x00000002
+#define PROCTL_DTW_8BIT 0x00000004
+
+#define SYSCTL_IPGEN 0x00000001
+#define SYSCTL_HCKEN 0x00000002
+#define SYSCTL_PEREN 0x00000004
+#define SYSCTL_DVS_MASK 0x000000f0
+#define SYSCTL_DVS_SHIFT 4
+#define SYSCTL_SDCLKFS_MASK 0x0000ff00
+#define SYSCTL_SDCLKFS_SHIFT 8
+#define SYSCTL_DTOCV_MASK 0x000f0000
+#define SYSCTL_DTOCV_SHIFT 16
+#define SYSCTL_RSTA 0x01000000
+#define SYSCTL_RSTC 0x02000000
+#define SYSCTL_RSTD 0x04000000
+#define SYSCTL_INITA 0x08000000
+
+#define IRQSTAT_CC 0x00000001
+#define IRQSTAT_TC 0x00000002
+#define IRQSTAT_BGE 0x00000004
+#define IRQSTAT_DINT 0x00000008
+#define IRQSTAT_BWR 0x00000010
+#define IRQSTAT_BRR 0x00000020
+#define IRQSTAT_CINS 0x00000040
+#define IRQSTAT_CRM 0x00000080
+#define IRQSTAT_CTOE 0x00010000
+#define IRQSTAT_CCE 0x00020000
+#define IRQSTAT_CEBE 0x00040000
+#define IRQSTAT_CIE 0x00080000
+#define IRQSTAT_DTOE 0x00100000
+#define IRQSTAT_DCE 0x00200000
+#define IRQSTAT_DEBE 0x00400000
+#define IRQSTAT_AC12E 0x01000000
+#define IRQSTAT_DMAE 0x10000000
+
+#define HOSTCAPBLT_DMAS 0x00400000
+#define HOSTCAPBLT_HSS 0x00200000
+
+#define WML_RD_MASK 0x000000ff
+#define WML_RD_SHIFT 0
+#define WML_RD_MAX 0x10
+#define WML_WR_MASK 0x00ff0000
+#define WML_WR_SHIFT 16
+#define WML_WR_MAX 0x80
+
+/****************************************************************************************/
+
+#define BLOCK_SIZE 512
+
+#define NUM_DMA_BLOCKS 128
+
+/*
+ * These checks are required to guarantee the esdhc_memcard_disk_block_read_write() algorithm
+ * will work with any user options.
+ */
+#if (NUM_DMA_BLOCKS < CONFIGURE_BDBUF_MAX_READ_AHEAD_BLOCKS)
+#undef NUM_DMA_BLOCKS
+#define NUM_DMA_BLOCKS CONFIGURE_BDBUF_MAX_READ_AHEAD_BLOCKS
+#endif
+#if (NUM_DMA_BLOCKS < CONFIGURE_BDBUF_MAX_WRITE_BLOCKS)
+#undef NUM_DMA_BLOCKS
+#define NUM_DMA_BLOCKS CONFIGURE_BDBUF_MAX_WRITE_BLOCKS
+#endif
+
+
+/* Contiguous buffer for DMA transfers */
+static uint8_t dma_blocks[NUM_DMA_BLOCKS][BLOCK_SIZE] __attribute__ ((aligned(BLOCK_SIZE)));
+
+/* We can store this info in global variables because there is only one ESDHC interface in the system */
+static uint32_t high_capacity;
+static uint32_t block_count;
+
+/* Semaphore to protect the execution of hardware operations */
+static rtems_id esdhc_sema_id = RTEMS_ID_NONE;
+
+
+static void check_write_by_readback(rtems_blkdev_request *wr);
+
+static inline void dbg_printk(int dbg_level, const char *format, ...)
+{
+#if (DEBUG > 0)
+ if (dbg_level <= DEBUG)
+ {
+ va_list args;
+
+ va_start(args, format);
+ vprintk(format, args);
+ va_end(args);
+ }
+#endif
+}
+
+#if (DEBUG >= 5)
+static void print_block(const uint8_t *data, uint32_t len, uint32_t block, int do_write)
+{
+ int i;
+ unsigned char c;
+ printk("%s data: block %u, buf %p:\n", do_write ? "Writing" : "Reading", block, data);
+
+#if 0
+ /* Print as ASCII or HEX (depending on symbol code) */
+ for (i = 0; i < len; i++)
+ {
+ c = data[i];
+ if ((c >= 0x20) && (c <= 0x7e))
+ printk("%c", c);
+ else
+ printk("%02x ", c);
+ if (((i + 1) % 64) == 0)
+ printk("\n");
+ }
+ printk("\n");
+#endif
+
+ /* Print again only as HEX */
+ for (i = 0; i < len; i++)
+ {
+ c = data[i];
+ printk("%02x ", c);
+ if (((i + 1) % 64) == 0)
+ printk("\n");
+ }
+ printk("\n");
+}
+#endif
+
+#if MEASURE_EXECCMD_DELAYS || RUN_CARD_READ_SPEED_TEST || RUN_CARD_WRITE_SPEED_TEST
+static long long timeval_diff_us(struct timeval *s, struct timeval *e)
+{
+ return 1000000LL * (e->tv_sec - s->tv_sec) + (e->tv_usec - s->tv_usec);
+}
+#endif
+
+
+static void esdhc_memcard_set_clock_divider(uint32_t sdclkfs, uint32_t dvs)
+{
+ qoriq.esdhc.sysctl = (qoriq.esdhc.sysctl & ~(SYSCTL_SDCLKFS_MASK | SYSCTL_DVS_MASK)) |
+ (sdclkfs << SYSCTL_SDCLKFS_SHIFT) | (dvs << SYSCTL_DVS_SHIFT);
+ dbg_printk(2, "%s: 0x%08x\n", __func__, qoriq.esdhc.sysctl);
+}
+
+static void esdhc_memcard_set_clock(uint32_t desired_clock_hz)
+{
+ /* Base clock is bus_clock/2 */
+ uint32_t desired_div = BSP_bus_frequency / 2 / desired_clock_hz;
+ uint32_t power2_div = 1;
+
+ uint32_t sdclkfs, dvs;
+
+ while(power2_div < desired_div)
+ power2_div *= 2;
+
+ if (power2_div <= 0x100)
+ {
+ sdclkfs = power2_div / 2;
+ dvs = 0;
+ }
+ else
+ {
+ sdclkfs = 0x80;
+ dvs = power2_div / 0x100 - 1;
+ }
+
+ dbg_printk(2, "%s: BSP_bus_frequency %u, desired_div 0x%x, power2_div 0x%x, sdclkfs 0x%x dvs 0x%x\n",
+ __func__, BSP_bus_frequency, desired_div, power2_div, sdclkfs, dvs);
+
+ esdhc_memcard_set_clock_divider(sdclkfs, dvs);
+}
+
+/* Find out whether it's a data read command */
+static inline int is_read_cmd(uint32_t cmd_type)
+{
+ return (cmd_type & SD_T_READ);
+}
+
+/* Find out whether it's a data write command */
+static inline int is_write_cmd(uint32_t cmd_type)
+{
+ return (cmd_type & SD_T_WRITE);
+}
+
+/* Compute the XFERTYP value for the specified command */
+static uint32_t compute_xfertyp_for_cmd(uint8_t cmd, uint32_t cmd_type, uint32_t num_data_blocks)
+{
+ uint32_t xfertyp = (cmd << XFERTYP_CMD_SHIFT);
+
+ if (is_read_cmd(cmd_type))
+ xfertyp |= XFERTYP_DPSEL | XFERTYP_DTDSEL;
+ else if (is_write_cmd(cmd_type))
+ xfertyp |= XFERTYP_DPSEL;
+
+#if USE_DMA
+ if (is_read_cmd(cmd_type) || is_write_cmd(cmd_type))
+ xfertyp |= XFERTYP_DMAEN;
+#endif
+
+ if (num_data_blocks > 1)
+ xfertyp |= XFERTYP_MSBSEL | XFERTYP_BCEN | XFERTYP_AC12EN;
+
+ if (cmd_type & SD_R_CRC)
+ xfertyp |= XFERTYP_CCCEN;
+
+ if (cmd_type & SD_R_CMD_CODE)
+ xfertyp |= XFERTYP_CICEN;
+
+ if (cmd_type & SD_R_NORESP)
+ xfertyp |= XFERTYP_RSPTYP_NORESP;
+ else if (cmd_type & SD_R_136)
+ xfertyp |= XFERTYP_RSPTYP_136;
+ else if (cmd_type & SD_R_48)
+ xfertyp |= XFERTYP_RSPTYP_48;
+ else if (cmd_type & SD_R_48_BUSY)
+ xfertyp |= XFERTYP_RSPTYP_48_BUSY;
+
+ return xfertyp;
+}
+
+#if !USE_DMA
+static void
+esdhc_memcard_pio_read_write(uint8_t *data, uint32_t num_data_blocks, uint32_t data_block_size, int do_write)
+{
+ uint32_t size = num_data_blocks * data_block_size;
+ uint32_t prsstat = do_write ? PRSSTAT_BWEN : PRSSTAT_BREN;
+ uint32_t burst_max = do_write ? WML_WR_MAX : WML_RD_MAX;
+ uint32_t burst = 0; /* Number of 4-bytes words remaining in current burst */
+
+ while (size)
+ {
+ if (burst == 0)
+ {
+ /* TODO: check errors in irqstat in order to avoid the evernal loop? */
+ while ((qoriq.esdhc.prsstat & prsstat) == 0);
+ burst = burst_max;
+ }
+
+ if (do_write)
+ {
+ qoriq.esdhc.datport = htole32(*((uint32_t *)data));
+ }
+ else /* read */
+ {
+ *(uint32_t *)data = le32toh(qoriq.esdhc.datport);
+ }
+
+ data += 4;
+ size -= 4;
+ burst--;
+ }
+}
+#endif
+
+/* Execute an SD command */
+static rtems_status_code esdhc_exec_command(uint32_t cmd_spec, uint32_t arg,
+ uint8_t *data/*write in/read out*/,
+ uint32_t num_data_blocks, uint32_t data_block_size,
+ uint32_t *resp/*out*/)
+{
+ uint8_t cmd = cmd_spec & 0x3f;
+ uint32_t cmd_type = cmd_spec & ~0x3f;
+ rtems_status_code sc = RTEMS_SUCCESSFUL;
+ uint32_t xfertyp;
+ uint32_t irqstat;
+ uint32_t sysctl;
+ uint32_t cmdrsp0, cmdrsp1, cmdrsp2, cmdrsp3;
+#if MEASURE_EXECCMD_DELAYS
+ struct timeval t[5];
+#endif
+
+ dbg_printk(4, "%s: start cmd %u (arg 0x%08x, data %p nblocks %u block_size %u)\n",
+ __func__, cmd, arg, data, num_data_blocks, data_block_size);
+
+#if (DEBUG >= 5)
+ if (is_write_cmd(cmd_type))
+ {
+ uint32_t i;
+ for (i = 0; i < num_data_blocks; i++)
+ print_block(data + i * data_block_size, data_block_size, arg + i, is_write_cmd(cmd_type));
+ }
+#endif
+
+#if MEASURE_EXECCMD_DELAYS
+ memset(t, 0, sizeof(t));
+ gettimeofday(&t[0], NULL);
+#endif
+
+ xfertyp = compute_xfertyp_for_cmd(cmd, cmd_type, num_data_blocks);
+
+ /* Wait for DLA, CDIHB and CIHB bits to clear */
+ while (qoriq.esdhc.prsstat & (PRSSTAT_DLA | PRSSTAT_CDIHB | PRSSTAT_CIHB));
+
+ /* Setup the data registers */
+ if (is_read_cmd(cmd_type) || is_write_cmd(cmd_type))
+ {
+ if (!data) /* Sanity check */
+ {
+ sc = RTEMS_INVALID_ADDRESS;
+ goto finish;
+ }
+ /* Set the watermarks */
+ qoriq.esdhc.wml = (WML_WR_MAX << WML_WR_SHIFT) | (WML_RD_MAX << WML_RD_SHIFT);
+ /* Set amount of trasferred data */
+ qoriq.esdhc.blkattr = (num_data_blocks << BLKATTR_CNT_SHIFT) | (data_block_size << BLKATTR_SIZE_SHIFT);
+ /* Set the data timeout */
+ qoriq.esdhc.sysctl = (qoriq.esdhc.sysctl & ~SYSCTL_DTOCV_MASK) | (0xe << SYSCTL_DTOCV_SHIFT);
+#if USE_DMA
+ /* Set the DMA buffer address */
+ qoriq.esdhc.dsaddr = (uint32_t)data;
+ /* Invalidate or flush the cache lines depending on read or write command */
+ if (is_read_cmd(cmd_type))
+ {
+ /* We need to flush before invalidation, because there can be valid _adjacent_ data */
+ rtems_cache_flush_multiple_data_lines(data, num_data_blocks * data_block_size);
+ rtems_cache_invalidate_multiple_data_lines(data, num_data_blocks * data_block_size);
+ }
+ else
+ rtems_cache_flush_multiple_data_lines(data, num_data_blocks * data_block_size);
+#endif
+ }
+
+#if MEASURE_EXECCMD_DELAYS
+ gettimeofday(&t[1], NULL);
+#endif
+
+ /* Mask all interrupts */
+ qoriq.esdhc.irqsigen = 0;
+ /* Set the command argument */
+ qoriq.esdhc.cmdarg = arg;
+ /* Set XFERTYP register to start the command */
+ qoriq.esdhc.xfertyp = xfertyp;
+
+ /* Wait till the command is completed or timeout occurs */
+ while ((qoriq.esdhc.irqstat & (IRQSTAT_CC | IRQSTAT_CTOE)) == 0);
+
+#if MEASURE_EXECCMD_DELAYS
+ gettimeofday(&t[2], NULL);
+#endif
+
+ irqstat = qoriq.esdhc.irqstat;
+
+ if (irqstat & IRQSTAT_CTOE)
+ {
+ dbg_printk(1, "%s: cmd %u command timeout irqstat 0x%08x\n", __func__, cmd, irqstat);
+ sc = RTEMS_TIMEOUT;
+ goto done;
+ }
+
+ if (irqstat & (IRQSTAT_CIE | IRQSTAT_CEBE | IRQSTAT_CCE))
+ {
+ dbg_printk(1, "%s: cmd %u command error irqstat 0x%08x\n", __func__, cmd, irqstat);
+ sc = RTEMS_IO_ERROR;
+ goto done;
+ }
+
+ if (cmd_type & SD_R_136)
+ {
+ cmdrsp0 = qoriq.esdhc.cmdrsp0;
+ cmdrsp1 = qoriq.esdhc.cmdrsp1;
+ cmdrsp2 = qoriq.esdhc.cmdrsp2;
+ cmdrsp3 = qoriq.esdhc.cmdrsp3;
+ if (resp)
+ {
+ resp[0] = (cmdrsp3 << 8) | (cmdrsp2 >> 24);
+ resp[1] = (cmdrsp2 << 8) | (cmdrsp1 >> 24);
+ resp[2] = (cmdrsp1 << 8) | (cmdrsp0 >> 24);
+ resp[3] = (cmdrsp0 << 8);
+ }
+ dbg_printk(4, "%s: cmd %u cmdrsp %08x %08x %08x %08x\n", __func__,
+ cmd, cmdrsp0, cmdrsp1, cmdrsp2, cmdrsp3);
+ }
+ else
+ {
+ cmdrsp0 = qoriq.esdhc.cmdrsp0;
+ if (resp)
+ resp[0] = cmdrsp0;
+ dbg_printk(4, "%s: cmd %u cmdrsp %08x\n", __func__, cmd, cmdrsp0);
+ }
+
+ if (data)
+ {
+ /* Wait till all data is transferred */
+#if USE_DMA
+ do {
+ irqstat = qoriq.esdhc.irqstat;
+
+ if (irqstat & IRQSTAT_DTOE)
+ {
+ dbg_printk(1, "%s: cmd %u data timeout irqstat 0x%08x\n", __func__, cmd, irqstat);
+ sc = RTEMS_TIMEOUT;
+ goto done;
+ }
+
+ if (irqstat & (IRQSTAT_DEBE | IRQSTAT_DCE | IRQSTAT_DTOE | IRQSTAT_DMAE))
+ {
+ dbg_printk(1, "%s: cmd %u data error irqstat 0x%08x\n", __func__, cmd, irqstat);
+ sc = RTEMS_IO_ERROR;
+ goto done;
+ }
+
+ } while ((irqstat & (IRQSTAT_TC | IRQSTAT_DINT)) != (IRQSTAT_TC | IRQSTAT_DINT));
+
+ /* Invalidate once again just in case if CPU prefetched some data */
+ if (is_read_cmd(cmd_type))
+ rtems_cache_invalidate_multiple_data_lines(data, num_data_blocks * data_block_size);
+#else
+ esdhc_memcard_pio_read_write(data, num_data_blocks, data_block_size, is_write_cmd(cmd_type));
+#endif
+ }
+
+#if MEASURE_EXECCMD_DELAYS
+ gettimeofday(&t[3], NULL);
+#endif
+
+done:
+ /* Clear all bits */
+ qoriq.esdhc.irqstat = 0xffffffff;
+
+ /* Reset the SDHC_CMD and SDHC_DAT lines if error happened */
+ if (sc != RTEMS_SUCCESSFUL)
+ {
+ if (is_read_cmd(cmd_type) || is_write_cmd(cmd_type))
+ sysctl = SYSCTL_RSTC | SYSCTL_RSTD;
+ else
+ sysctl = SYSCTL_RSTC;
+
+ qoriq.esdhc.sysctl |= sysctl;
+
+ while ((qoriq.esdhc.sysctl & sysctl) != 0);
+ }
+
+finish:
+#if MEASURE_EXECCMD_DELAYS
+ gettimeofday(&t[4], NULL);
+ printk("%s: cmd %u delays %llu %llu %llu %llu %llu\n", __func__, cmd,
+ timeval_diff_us(&t[0], &t[1]),
+ timeval_diff_us(&t[1], &t[2]),
+ timeval_diff_us(&t[2], &t[3]),
+ timeval_diff_us(&t[3], &t[4]),
+ timeval_diff_us(&t[0], &t[4]));
+#endif
+
+#if (DEBUG >= 5)
+ if (is_read_cmd(cmd_type))
+ {
+ uint32_t i;
+ for (i = 0; i < num_data_blocks; i++)
+ print_block(data + i * data_block_size, data_block_size, arg + i, is_write_cmd(cmd_type));
+ }
+#endif
+
+ if (sc == RTEMS_SUCCESSFUL)
+ {
+ dbg_printk(4, "%s: finish cmd %u (arg %u, data %p nblocks %u block_size %u), ok\n",
+ __func__, cmd, arg, data, num_data_blocks, data_block_size);
+ }
+ else
+ {
+ dbg_printk(1, "%s: finish cmd %u (arg %u, data %p nblocks %u block_size %u), failed (sc %u)\n",
+ __func__, cmd, arg, data, num_data_blocks, data_block_size, sc);
+ }
+
+ return sc;
+}
+
+static int esdhc_memcard_disk_block_read_write(rtems_blkdev_request *r, int do_write)
+{
+ rtems_status_code sc = RTEMS_SUCCESSFUL;
+ uint32_t cmd_spec;
+ uint32_t response[4];
+ uint32_t start_bufidx;
+ uint32_t bufidx = 0;
+ uint32_t start_block;
+ uint32_t num_blocks;
+ uint32_t blocks_per_buf;
+ uint32_t offset;
+ uint32_t i;
+
+#if (DEBUG >= 3)
+ dbg_printk(3, "%s.1: %s bufnum %u\n", __func__, do_write ? "write" : "read", r->bufnum);
+ dbg_printk(3, "%s.2: blocks ", __func__);
+ for (i = 0; i < r->bufnum; i++)
+ dbg_printk(3, "[%u %u/%u] ", r->bufs[i].block, r->bufs[i].length, r->bufs[i].length / BLOCK_SIZE);
+ dbg_printk(3, "\n");
+#endif
+
+ while (bufidx < r->bufnum)
+ {
+ start_bufidx = bufidx;
+ start_block = r->bufs[bufidx].block;
+
+ /* The num_blocks variable counts the number of adjacent blocks */
+ for (num_blocks = 0;
+ (bufidx < r->bufnum) && (num_blocks < NUM_DMA_BLOCKS);
+ num_blocks += blocks_per_buf, bufidx++)
+ {
+ if (r->bufs[bufidx].block != start_block + num_blocks)
+ break; /* Blocks in this buffer are not adjacent with previous, break */
+
+ /* Paranoid check */
+ if ((r->bufs[bufidx].length % BLOCK_SIZE) != 0)
+ {
+ dbg_printk(1, "%s: buffer length=%u is not multiple of BLOCK_SIZE=%u\n", r->bufs[bufidx].length, BLOCK_SIZE);
+ sc = RTEMS_INVALID_SIZE;
+ goto finish;
+ }
+
+ blocks_per_buf = r->bufs[bufidx].length / BLOCK_SIZE;
+ if (blocks_per_buf > NUM_DMA_BLOCKS - num_blocks)
+ break; /* No free space left in dma_blocks[] for all blocks of this buffer, break */
+
+ if (do_write)
+ {
+ /* Copy the current buffer to the appropriate place in the contiguous DMA buffer */
+ memcpy(dma_blocks[num_blocks], r->bufs[bufidx].buffer, r->bufs[bufidx].length);
+ }
+ }
+
+ if (num_blocks == 1)
+ {
+ /*CMD17 - read one block, CMD24 - write one block */
+ cmd_spec = do_write ? SD_CMD24 : SD_CMD17;
+ }
+ else
+ {
+ /*CMD18 - read multiple blocks, CMD25 - write multiple blocks */
+ cmd_spec = do_write ? SD_CMD25 : SD_CMD18;
+ }
+
+ offset = start_block; /* offset in blocks */
+ if (!high_capacity)
+ offset *= BLOCK_SIZE; /* offset in bytes */
+
+#if (DEBUG >= 3)
+ dbg_printk(3, "%s.3: bufnum %u num_blocks %u start_bufidx %u bufidx %u start_block %u offset %u\n",
+ __func__, r->bufnum, num_blocks, start_bufidx, bufidx, start_block, offset);
+#endif
+
+ sc = esdhc_exec_command(cmd_spec, offset, (uint8_t *)dma_blocks, num_blocks, BLOCK_SIZE, response);
+ if (sc != RTEMS_SUCCESSFUL)
+ break;
+
+ if (!do_write)
+ {
+ /* Read out the data from the contiguous DMA buffer */
+ num_blocks = 0;
+ for (i = start_bufidx; i < bufidx; i++)
+ {
+ memcpy(r->bufs[i].buffer, dma_blocks[num_blocks], r->bufs[i].length);
+ num_blocks += r->bufs[i].length / BLOCK_SIZE;
+ }
+ }
+ }
+
+finish:
+ rtems_blkdev_request_done(r, sc);
+
+ if (sc != RTEMS_SUCCESSFUL)
+ {
+ errno = EIO;
+ return -EIO;
+ }
+ else
+ return 0;
+}
+
+static int esdhc_memcard_disk_block_read_write_retriable(rtems_blkdev_request *r, int do_write)
+{
+ uint32_t i;
+ int rc;
+
+ rc = rtems_semaphore_obtain(esdhc_sema_id, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+ if (rc != RTEMS_SUCCESSFUL)
+ {
+ errno = EACCES;
+ return -EACCES;
+ }
+
+ for (i = 0; i < MAX_READ_WRITE_RETRIES; i++)
+ {
+ rc = esdhc_memcard_disk_block_read_write(r, do_write);
+ if (rc == 0)
+ break; /* Operiation succeeded */
+ else if (i < MAX_READ_WRITE_RETRIES - 1)
+ dbg_printk(1, "%s: operation failed with rc=%d, retrying\n", __func__, rc);
+ }
+
+#if CHECK_WRITE_BY_READBACK
+ if (do_write && (rc == 0))
+ check_write_by_readback(r);
+#endif
+
+ rtems_semaphore_release(esdhc_sema_id);
+
+ return rc;
+}
+
+static int esdhc_memcard_disk_block_read(rtems_blkdev_request *r)
+{
+ return esdhc_memcard_disk_block_read_write_retriable(r, 0);
+}
+
+static int esdhc_memcard_disk_block_write(rtems_blkdev_request *r)
+{
+ return esdhc_memcard_disk_block_read_write_retriable(r, 1);
+}
+
+
+#if CHECK_WRITE_BY_READBACK
+static void init_dummy_sg_buffer(rtems_blkdev_sg_buffer *sg, uint32_t block,
+ uint32_t length, uint8_t pattern)
+{
+ sg->block = block;
+ sg->length = length;
+ sg->buffer = calloc(length, 1);
+ memset(sg->buffer, pattern, length);
+}
+
+static void cleanup_dummy_sg_buffer(rtems_blkdev_sg_buffer *sg)
+{
+ free(sg->buffer);
+}
+
+static void dummy_request_done(rtems_blkdev_request *req, rtems_status_code sc)
+{
+}
+
+static rtems_blkdev_request *create_dummy_request(const rtems_blkdev_request *wr)
+{
+ rtems_blkdev_request *rd;
+ uint32_t i;
+
+ rd = calloc(sizeof(*rd) + sizeof(rtems_blkdev_sg_buffer) * wr->bufnum, 1);
+ rd->bufnum = wr->bufnum;
+ rd->done = dummy_request_done;
+
+ for (i = 0; i < rd->bufnum; i++)
+ init_dummy_sg_buffer(&rd->bufs[i], wr->bufs[i].block, wr->bufs[i].length, 0xfe);
+
+ return rd;
+}
+
+static void destroy_dummy_request(rtems_blkdev_request *rd)
+{
+ uint32_t i;
+
+ for (i = 0; i < rd->bufnum; i++)
+ cleanup_dummy_sg_buffer(&rd->bufs[i]);
+
+ free(rd);
+}
+
+static void check_write_by_readback(rtems_blkdev_request *wr)
+{
+ rtems_blkdev_request *rd;
+ uint32_t i;
+
+ rd = create_dummy_request(wr);
+
+ esdhc_memcard_disk_block_read_write(rd, 0);
+
+ for (i = 0; i < rd->bufnum; i++)
+ {
+ if (memcmp(rd->bufs[i].buffer, wr->bufs[i].buffer, rd->bufs[i].length) != 0)
+ {
+ printk("%s: write-read mismatch, buf %u start_block %u len %u\n",
+ __func__, i, rd->bufs[i].block, rd->bufs[i].length);
+ }
+ }
+
+ destroy_dummy_request(rd);
+}
+#endif
+
+
+#if RUN_CARD_READ_SPEED_TEST || RUN_CARD_WRITE_SPEED_TEST
+static void print_data(const uint8_t *data, uint32_t len)
+{
+ int i;
+ printk("Data at %p is:\n", data);
+ for (i = 0; i < len; i++)
+ {
+ printk("%02x ", data[i]);
+ if (((i + 1) % 64) == 0)
+ printk("\n");
+ }
+ printk("\n");
+}
+
+static void check_buf_is_zeroed(char *msg, uint8_t *buf, uint32_t total_size)
+{
+ uint32_t nonzero = 0;
+ uint32_t i;
+
+ for (i = 0; i < total_size; i++)
+ {
+ if (buf[i] != 0)
+ {
+ nonzero++;
+ if (nonzero <= 10) /* don't print too much */
+ printk("Found nonzero byte %02x at position %u (%p)\n", buf[i], i, &buf[i]);
+ }
+ }
+ printk("Amount of nonzero bytes in %p buffer %s is %u\n", buf, msg, nonzero);
+}
+
+static void esdhc_memcard_speed_test(int do_write)
+{
+ uint32_t total_size = 4 * 1024 * 1024; /* total area size to read or write */
+ uint32_t num_blocks = total_size / BLOCK_SIZE;
+ uint32_t start_block = 0;
+ uint8_t *buf = (uint8_t *)malloc(total_size);
+ uint32_t response[4];
+ rtems_status_code sc;
+ struct timeval start, end;
+ uint64_t delay;
+ uint64_t speed = 0;
+
+ printk("%s: measuring ESDHC card %s performance (size %u, buf %p), please wait...\n",
+ __func__, do_write ? "write" : "read", total_size, buf);
+
+ memset(buf, 0, total_size);
+ check_buf_is_zeroed("before SD operation", buf, total_size);
+
+ gettimeofday(&start, NULL);
+
+ if (do_write)
+ sc = esdhc_exec_command(SD_CMD25, start_block, buf, num_blocks, BLOCK_SIZE, response);
+ else /* read */
+ sc = esdhc_exec_command(SD_CMD18, start_block, buf, num_blocks, BLOCK_SIZE, response);
+
+ gettimeofday(&end, NULL);
+
+ if (!do_write)
+ {
+ /* This check is useful if previous test has written zeros */
+ check_buf_is_zeroed("after reading SD card", buf, total_size);
+ }
+
+ free(buf);
+
+ delay = timeval_diff_us(&start, &end);
+ if (delay > 0)
+ speed = (uint64_t)total_size * 1000000ULL / delay;
+
+#if 0 /* Print the MBR */
+ if (!do_write)
+ print_data(buf, BLOCK_SIZE);
+#endif
+
+ printk("%s: sc %u, ESDHC card %s speed is %llu bytes/sec\n", __func__,
+ sc, do_write ? "write" : "read", speed);
+}
+
+static void esdhc_memcard_read_speed_test(void)
+{
+ esdhc_memcard_speed_test(0);
+}
+
+static void esdhc_memcard_write_speed_test(void)
+{
+ esdhc_memcard_speed_test(1);
+}
+#endif
+
+
+static rtems_status_code esdhc_try_set_high_speed(uint32_t rca, uint32_t *sd_speed_hz)
+{
+ uint32_t data[16] __attribute__((aligned(PPC_DEFAULT_CACHE_LINE_SIZE)));
+ uint32_t max_retries = 10;
+ rtems_status_code sc;
+ uint32_t response[4];
+ uint32_t sd_version;
+ uint32_t i;
+
+ *sd_speed_hz = 25000000; /* Default */
+
+ if (!(qoriq.esdhc.hostcapblt & HOSTCAPBLT_HSS))
+ return RTEMS_SUCCESSFUL; /* Host doesn't support high speed */
+
+ memset(data, 0, sizeof(data));
+
+
+ /* ACMD51 - get the SD configuration register */
+ for (i = 0; i < max_retries; i++)
+ {
+ sc = esdhc_exec_command(SD_CMD55, rca << 16, NULL, 0, 0, NULL);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+ sc = esdhc_exec_command(SD_ACMD51, 0, (uint8_t *)data, 1, 8, response);
+ if (sc == RTEMS_SUCCESSFUL)
+ break;
+
+ if (i < max_retries - 1)
+ dbg_printk(2, "%s: SD card is busy, retrying %u\n", __func__, i + 1);
+ else
+ return RTEMS_SUCCESSFUL; /* Give up and use 25MHz */
+ }
+
+ sd_version = (be32toh(data[0]) >> 24) & 0xf;
+ dbg_printk(2, "%s: %08x %08x, SD version 0x%x\n", __func__, be32toh(data[0]), be32toh(data[1]), sd_version);
+
+ if (sd_version == 0)
+ return RTEMS_SUCCESSFUL; /* SD version 1.0 doen't support CMD6, use 25MHz */
+
+
+ /* CMD6 - switch function (check mode) */
+ for (i = 0; i < max_retries; i++)
+ {
+ sc = esdhc_exec_command(SD_CMD6, 0x00fffff1, (uint8_t *)data, 1, 64, response);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+ /* Break if the high speed function is not busy, otherwise retry */
+ if (!(be32toh(data[7]) & 0x00020000))
+ break;
+
+ if (i < max_retries - 1)
+ dbg_printk(2, "%s: SD card is busy, retrying %u\n", __func__, i + 1);
+ else
+ return RTEMS_SUCCESSFUL; /* Give up and use 25MHz */
+ }
+
+ /* Check if the high speed is supported */
+ if (!(be32toh(data[3]) & 0x00020000))
+ {
+ dbg_printk(2, "%s: 0x%08x, high speed is not supported\n", __func__, be32toh(data[3]));
+ return RTEMS_SUCCESSFUL; /* High speed is not supported, use 25MHz */
+ }
+
+ /* CMD6 - switch function (switch mode) */
+ sc = esdhc_exec_command(SD_CMD6, 0x80fffff1, (uint8_t *)data, 1, 64, response);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+ if ((be32toh(data[4]) & 0x0f000000) == 0x01000000)
+ {
+ dbg_printk(2, "%s: 0x%08x, high speed 50MHz is established\n", __func__, be32toh(data[4]));
+ *sd_speed_hz = 50000000; /* High speed is established */
+ }
+
+ return RTEMS_SUCCESSFUL;
+}
+
+static rtems_status_code esdhc_memcard_init(void)
+{
+ static const uint32_t trans_speed_unit[] =
+ { 10000, 100000, 1000000, 10000000 };
+ static const uint32_t trans_speed_mult[] =
+ { 0, 10, 12, 13, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 70, 80 };
+ rtems_status_code sc = RTEMS_SUCCESSFUL;
+ uint32_t response[4];
+ uint32_t rca;
+ uint32_t block_size;
+ uint32_t trans_speed;
+ uint32_t sd_speed_hz;
+ uint32_t cap_size;
+ uint32_t cap_mult;
+ uint64_t capacity;
+ uint32_t i;
+
+
+#if USE_DMA
+ if ((qoriq.esdhc.hostcapblt & HOSTCAPBLT_DMAS) == 0)
+ return RTEMS_NOT_IMPLEMENTED; /* DMA is not supported by this hardware */
+#endif
+
+ /* Reset by RSTA bit */
+ qoriq.esdhc.sysctl |= SYSCTL_RSTA;
+ /* Wait RSTA bit to clear */
+ while (qoriq.esdhc.sysctl & SYSCTL_RSTA);
+ /* Set data timeout to max */
+ qoriq.esdhc.sysctl = (qoriq.esdhc.sysctl & ~SYSCTL_DTOCV_MASK) | (0xe << SYSCTL_DTOCV_SHIFT);
+ /* Set data bus width as 4 bits */
+ qoriq.esdhc.proctl = (qoriq.esdhc.proctl & ~PROCTL_DTW_MASK) | PROCTL_DTW_4BIT;
+ /* Set max clock divider = 1/(256*16) = 1/4096 */
+ esdhc_memcard_set_clock_divider(0x80, 0xf);
+ /* Wait for CDIHB and CIHB bits to clear */
+ while (qoriq.esdhc.prsstat & (PRSSTAT_CDIHB | PRSSTAT_CIHB));
+ /* Set INITA bit (it sends 80 clock ticks for card to power-up) */
+ qoriq.esdhc.sysctl |= SYSCTL_INITA;
+ /* Wait INITA bit to clear */
+ while (qoriq.esdhc.sysctl & SYSCTL_INITA);
+
+
+ /* CMD0 */
+ sc = esdhc_exec_command(SD_CMD0, 0, NULL, 0, 0, NULL);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+
+ /* CMD8 */
+ sc = esdhc_exec_command(SD_CMD8, 0x1aa, NULL, 0, 0, response);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+ if ((response[0] & 0xff) != 0xaa)
+ return RTEMS_NOT_CONFIGURED;
+
+
+ /* ACMD41 - initialize (max 100 retries) */
+ for (i = 0; i < 100; i++)
+ {
+ sc = esdhc_exec_command(SD_CMD55, 0, NULL, 0, 0, NULL);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+ /* Say to the card that we support SDHC and 2.7-3.6V voltage */
+ sc = esdhc_exec_command(SD_ACMD41, 0x40ff8000, NULL, 0, 0, response);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+ if (response[0] & 0x80000000)
+ break; /* Card is ready */
+
+ if (i < 99)
+ dbg_printk(2, "%s: SD card is busy, retrying %u\n", __func__, i + 1);
+ else
+ return RTEMS_IO_ERROR;
+ }
+
+ high_capacity = !!(response[0] & 0x40000000);
+
+ dbg_printk(2, "%s: high_capacity %u\n", __func__, high_capacity);
+
+
+ /* CMD2 - get CID */
+ sc = esdhc_exec_command(SD_CMD2, 0, NULL, 0, 0, NULL);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+ /* CMD3 - get RCA */
+ sc = esdhc_exec_command(SD_CMD3, 0, NULL, 0, 0, response);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+ rca = response[0] >> 16;
+ dbg_printk(2, "%s: RCA %04x\n", __func__, rca);
+
+ /* CMD9 - get CSD */
+ sc = esdhc_exec_command(SD_CMD9, rca << 16, NULL, 0, 0, response);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+ if (high_capacity)
+ {
+ cap_size = ((response[1] & 0x3f) << 16) | (response[2] >> 16);
+ cap_mult = 8;
+ }
+ else
+ {
+ cap_size = ((response[1] & 0x3ff) << 2) | (response[2] >> 30);
+ cap_mult = (response[2] & 0x00038000) >> 15;
+ }
+
+ block_count = (cap_size + 1) << (cap_mult + 2);
+ block_size = 1 << ((response[1] >> 16) & 0xf);
+ capacity = block_count * block_size;
+
+ trans_speed = trans_speed_unit[(response[0] & 0x7)] * trans_speed_mult[((response[0] >> 3) & 0xf)];
+
+ dbg_printk(2, "%s: cap_size %u cap_mult %u block_count %u block_size %u capacity %llu trans_speed %u\n",
+ __func__, cap_size, cap_mult, block_count, block_size, capacity, trans_speed);
+
+
+ /* CMD7 - select card */
+ sc = esdhc_exec_command(SD_CMD7, rca << 16, NULL, 0, 0, NULL);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+
+ /* ACMD6 - set bus width as 4 bits */
+ sc = esdhc_exec_command(SD_CMD55, rca << 16, NULL, 0, 0, NULL);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+ sc = esdhc_exec_command(SD_ACMD6, 0x2, NULL, 0, 0, NULL);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+
+ if ((block_size % BLOCK_SIZE) != 0) /* Sanity check */
+ return RTEMS_NOT_CONFIGURED;
+ else if (block_size > BLOCK_SIZE)
+ {
+ /* CMD16 - set block length = 512 bytes */
+ sc = esdhc_exec_command(SD_CMD16, BLOCK_SIZE, NULL, 0, 0, response);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+ /* Adjust block_count to count the blocks of 512 bytes size */
+ block_count *= block_size / BLOCK_SIZE;
+ }
+
+
+ /* Try to set high speed */
+ sc = esdhc_try_set_high_speed(rca, &sd_speed_hz);
+ if (sc != RTEMS_SUCCESSFUL) return sc;
+
+ esdhc_memcard_set_clock(sd_speed_hz);
+
+ printk("%s: SD card initialized with %uMHz speed.\n", __func__, sd_speed_hz/1000000);
+
+
+#if RUN_CARD_WRITE_SPEED_TEST
+ esdhc_memcard_write_speed_test();
+#endif
+#if RUN_CARD_READ_SPEED_TEST
+ esdhc_memcard_read_speed_test();
+#endif
+
+ return RTEMS_SUCCESSFUL;
+}
+
+static int esdhc_memcard_disk_ioctl(rtems_disk_device *dd, uint32_t req, void *arg)
+{
+ dbg_printk(5, "%s: req 0x%08x\n", __func__, req);
+
+ if (req == RTEMS_BLKIO_REQUEST)
+ {
+ rtems_blkdev_request *r = (rtems_blkdev_request *)arg;
+ switch (r->req)
+ {
+ case RTEMS_BLKDEV_REQ_READ:
+ return esdhc_memcard_disk_block_read(r);
+ case RTEMS_BLKDEV_REQ_WRITE:
+ return esdhc_memcard_disk_block_write(r);
+ default:
+ errno = EINVAL;
+ return -1;
+ }
+ }
+ else if (req == RTEMS_BLKIO_CAPABILITIES)
+ {
+ *(uint32_t *)arg = RTEMS_BLKDEV_CAP_MULTISECTOR_CONT;
+ return 0;
+ }
+ else
+ {
+ return rtems_blkdev_ioctl(dd, req, arg);
+ }
+}
+
+static rtems_status_code esdhc_memcard_disk_init(rtems_device_major_number major,
+ rtems_device_minor_number minor,
+ void *arg)
+{
+ rtems_status_code sc;
+ dev_t dev;
+
+ dbg_printk(2, "%s: arg %p\n", __func__, arg);
+
+ sc = rtems_semaphore_create(rtems_build_name('s','d','h','c'), 1,
+ RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE,
+ 0, &esdhc_sema_id);
+ RTEMS_CHECK_SC(sc, "ESDHC: Cannot create semaphore");
+
+ sc = rtems_disk_io_initialize();
+ RTEMS_CHECK_SC(sc, "ESDHC: Initialize RTEMS disk IO");
+
+ dev = rtems_filesystem_make_dev_t(major, 0);
+
+ sc = esdhc_memcard_init();
+ RTEMS_CHECK_SC(sc, "ESDHC: Initialize memory card");
+
+ sc = rtems_disk_create_phys(dev, BLOCK_SIZE, block_count, esdhc_memcard_disk_ioctl,
+ NULL, RTEMS_BSP_ESDHC_MEMCARD_DEVICE_PATH);
+ RTEMS_CHECK_SC(sc, "ESDHC: Create disk device");
+
+ return RTEMS_SUCCESSFUL;
+}
+
+
+static const rtems_driver_address_table esdhc_memcard_disk_ops =
+{
+ .initialization_entry = esdhc_memcard_disk_init,
+ RTEMS_GENERIC_BLOCK_DEVICE_DRIVER_ENTRIES
+};
+
+
+rtems_status_code bsp_register_esdhc_memcard(void)
+{
+ rtems_status_code sc = RTEMS_SUCCESSFUL;
+ rtems_device_major_number major = 0;
+
+ sc = rtems_io_register_driver(0, &esdhc_memcard_disk_ops, &major);
+ RTEMS_CHECK_SC(sc, "ESDHC: Register disk memory card driver");
+
+ dbg_printk(2, "%s: major %d\n", __func__, major);
+
+ return RTEMS_SUCCESSFUL;
+}
diff --git a/c/src/lib/libbsp/powerpc/qoriq/include/bsp.h b/c/src/lib/libbsp/powerpc/qoriq/include/bsp.h
index a0d0092..f4d20de 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/include/bsp.h
+++ b/c/src/lib/libbsp/powerpc/qoriq/include/bsp.h
@@ -82,6 +82,8 @@ void *bsp_idle_thread( uintptr_t ignored );
#define RTEMS_BSP_NETWORK_DRIVER_NAME3 "tsec3"
#define RTEMS_BSP_NETWORK_DRIVER_NAME4 "intercom1"
+#define RTEMS_BSP_ESDHC_MEMCARD_DEVICE_PATH "/dev/memcard"
+
#ifdef __cplusplus
}
#endif /* __cplusplus */
--
2.7.4 (Apple Git-66)
More information about the devel
mailing list