10 files changed, 291 insertions, 50 deletions

diff --git a/firmware/export/config/sansafuzeplus.h b/firmware/export/config/sansafuzeplus.h
index 680f7cfd2d..5a78d37056 100644
--- a/firmware/export/config/sansafuzeplus.h
+++ b/firmware/export/config/sansafuzeplus.h
@@ -171,12 +171,15 @@
 #define HAVE_USBSTACK
 //#define USB_HANDLED_BY_OF
 #define USE_ROCKBOX_USB
-#define HAVE_BOOTLOADER_USB_MODE
 #define USB_VENDOR_ID 0x0781
 #define USB_PRODUCT_ID 0x74e1
 #define HAVE_USB_HID_MOUSE
 //#define HAVE_BOOTLOADER_USB_MODE
 
+/* The fuze+ actually interesting partition table does not use 512-byte sector
+ * (usually 2048 logical sector size) */
+#define MAX_LOG_SECTOR_SIZE 2048
+
 /* Define this if you have adjustable CPU frequency */
 #define HAVE_ADJUSTABLE_CPU_FREQ
 
diff --git a/firmware/target/arm/imx233/clkctrl-imx233.c b/firmware/target/arm/imx233/clkctrl-imx233.c
index 7701b84c41..ee77a77493 100644
--- a/firmware/target/arm/imx233/clkctrl-imx233.c
+++ b/firmware/target/arm/imx233/clkctrl-imx233.c
@@ -60,15 +60,25 @@ void imx233_set_clock_divisor(enum imx233_clock_t clk, int div)
     switch(clk)
     {
         case CLK_PIX:
-            __REG_CLR(HW_CLKCTRL_PIX) = (1 << 12) - 1;
+            __REG_CLR(HW_CLKCTRL_PIX) = HW_CLKCTRL_PIX__DIV_BM;
             __REG_SET(HW_CLKCTRL_PIX) = div;
             while(HW_CLKCTRL_PIX & __CLK_BUSY);
             break;
         case CLK_SSP:
-            __REG_CLR(HW_CLKCTRL_SSP) = (1 << 9) - 1;
+            __REG_CLR(HW_CLKCTRL_SSP) = HW_CLKCTRL_SSP__DIV_BM;
             __REG_SET(HW_CLKCTRL_SSP) = div;
             while(HW_CLKCTRL_SSP & __CLK_BUSY);
             break;
+        case CLK_CPU:
+            __REG_CLR(HW_CLKCTRL_CPU) = HW_CLKCTRL_CPU__DIV_CPU_BM;
+            __REG_SET(HW_CLKCTRL_CPU) = div;
+            while(HW_CLKCTRL_CPU & HW_CLKCTRL_CPU__BUSY_REF_CPU);
+            break;
+        case CLK_AHB:
+            __REG_CLR(HW_CLKCTRL_HBUS) = HW_CLKCTRL_HBUS__DIV_BM;
+            __REG_SET(HW_CLKCTRL_HBUS) = div;
+            while(HW_CLKCTRL_HBUS & __CLK_BUSY);
+            break;
         default: return;
     }
 }
@@ -81,6 +91,7 @@ void imx233_set_fractional_divisor(enum imx233_clock_t clk, int fracdiv)
     {
         case CLK_PIX: REG = &HW_CLKCTRL_FRAC_PIX; break;
         case CLK_IO: REG = &HW_CLKCTRL_FRAC_IO; break;
+        case CLK_CPU: REG = &HW_CLKCTRL_FRAC_CPU; break;
         default: return;
     }
 
@@ -97,6 +108,7 @@ void imx233_set_bypass_pll(enum imx233_clock_t clk, bool bypass)
     {
         case CLK_PIX: msk = HW_CLKCTRL_CLKSEQ__BYPASS_PIX; break;
         case CLK_SSP: msk = HW_CLKCTRL_CLKSEQ__BYPASS_SSP; break;
+        case CLK_CPU: msk = HW_CLKCTRL_CLKSEQ__BYPASS_CPU; break;
         default: return;
     }
 
diff --git a/firmware/target/arm/imx233/clkctrl-imx233.h b/firmware/target/arm/imx233/clkctrl-imx233.h
index 1533b52cce..2a12129171 100644
--- a/firmware/target/arm/imx233/clkctrl-imx233.h
+++ b/firmware/target/arm/imx233/clkctrl-imx233.h
@@ -27,15 +27,34 @@
 
 #define HW_CLKCTRL_BASE     0x80040000
 
+#define HW_CLKCTRL_PLLCTRL0 (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x0))
+#define HW_CLKCTRL_PLLCTRL0__DIV_SEL_BP 20
+#define HW_CLKCTRL_PLLCTRL0__DIV_SEL_BM (3 << 20)
+
+#define HW_CLKCTRL_PLLCTRL1 (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x10))
+
+#define HW_CLKCTRL_CPU      (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x20))
+#define HW_CLKCTRL_CPU__DIV_CPU_BP  0
+#define HW_CLKCTRL_CPU__DIV_CPU_BM  0x3f
+#define HW_CLKCTRL_CPU__BUSY_REF_CPU    (1 << 28)
+
+#define HW_CLKCTRL_HBUS     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x30))
+#define HW_CLKCTRL_HBUS__DIV_BP     0
+#define HW_CLKCTRL_HBUS__DIV_BM     0x1f
+
 #define HW_CLKCTRL_XTAL     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x50))
 #define HW_CLKCTRL_XTAL__TIMROT_CLK32K_GATE (1 << 26)
 
 #define HW_CLKCTRL_PIX      (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x60))
+#define HW_CLKCTRL_PIX__DIV_BM  0xfff
+
 #define HW_CLKCTRL_SSP      (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x70))
+#define HW_CLKCTRL_SSP__DIV_BM  0x1ff
 
 #define HW_CLKCTRL_CLKSEQ   (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x110))
 #define HW_CLKCTRL_CLKSEQ__BYPASS_PIX   (1 << 1)
 #define HW_CLKCTRL_CLKSEQ__BYPASS_SSP   (1 << 5)
+#define HW_CLKCTRL_CLKSEQ__BYPASS_CPU   (1 << 7)
 
 #define HW_CLKCTRL_FRAC     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0xf0))
 #define HW_CLKCTRL_FRAC_CPU (*(volatile uint8_t *)(HW_CLKCTRL_BASE + 0xf0))
@@ -52,9 +71,11 @@
 
 enum imx233_clock_t
 {
-    CLK_PIX,
-    CLK_SSP,
-    CLK_IO,
+    CLK_PIX, /* div, frac */
+    CLK_SSP, /* div, frac */
+    CLK_IO, /* div */
+    CLK_CPU, /* div, frac */
+    CLK_AHB /* div */
 };
 
 void imx233_enable_timrot_xtal_clk32k(bool enable);
diff --git a/firmware/target/arm/imx233/mmc-imx233.c b/firmware/target/arm/imx233/mmc-imx233.c
index f56ff3725c..dafe63f3d3 100644
--- a/firmware/target/arm/imx233/mmc-imx233.c
+++ b/firmware/target/arm/imx233/mmc-imx233.c
@@ -22,36 +22,54 @@
 #include "system.h"
 #include "mmc.h"
 #include "sdmmc.h"
+#include "storage.h"
 #include "ssp-imx233.h"
 #include "pinctrl-imx233.h"
 #include "button-target.h"
 
+/**
+ * This code assumes a single eMMC internal flash
+ */
+
 #ifdef SANSA_FUZEPLUS
 #define MMC_SSP     2
 #else
 #error You need to configure the ssp to use
 #endif
 
+#define MMC_RCA     1
+
+/** When set, this values restrict the windows of the read and writes */
+static unsigned mmc_window_start = 0;
+static unsigned mmc_window_end = INT_MAX;
+
+static struct mutex mmc_mutex;
+
 int mmc_init(void)
 {
+    mutex_init(&mmc_mutex);
+    
     imx233_ssp_start(MMC_SSP);
     imx233_ssp_softreset(MMC_SSP);
     imx233_ssp_set_mode(MMC_SSP, HW_SSP_CTRL1__SSP_MODE__SD_MMC);
     #ifdef SANSA_FUZEPLUS
-    /** Sansa Fuze+ has an internal eMMC 8-bit wide flash, power gate is pin PWM3 */
+    /** Sansa Fuze+ has an internal eMMC 8-bit wide flash, power gate is pin PWM3
+     * and power up time is 20ms */
     imx233_set_pin_function(1, 29, PINCTRL_FUNCTION_GPIO);
     imx233_enable_gpio_output(1, 29, true);
     imx233_set_gpio_output(1, 29, false);
+    sleep(HZ / 5);
 
     imx233_ssp_setup_ssp2_sd_mmc_pins(true, 8, PINCTRL_DRIVE_8mA);
     #endif
-    /* SSPCLK @ 120MHz
-     * gives bitrate of 120 / 100 / 3 = 400kHz */
-    imx233_ssp_set_timings(MMC_SSP, 100, 2);
-    imx233_ssp_set_timeout(MMC_SSP, 0xffff);
+    /* SSPCLK @ 96MHz
+     * gives bitrate of 96000 / 240 / 1 = 400kHz */
+    imx233_ssp_set_timings(MMC_SSP, 240, 0, 0xffff);
     imx233_ssp_sd_mmc_power_up_sequence(MMC_SSP);
+    imx233_ssp_set_bus_width(MMC_SSP, 1);
+    imx233_ssp_set_block_size(MMC_SSP, 9);
     /* go to idle state */
-    int ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, SD_GO_IDLE_STATE, 0, SSP_NO_RESP, NULL, 0, false, NULL);
+    int ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, SD_GO_IDLE_STATE, 0, SSP_NO_RESP, NULL, 0, false, false, NULL);
     if(ret != 0)
         return -1;
     /* send op cond until the card respond with busy bit set; it must complete within 1sec */
@@ -59,18 +77,82 @@ int mmc_init(void)
     do
     {
         uint32_t ocr;
-        ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 1, 0x40ff8000, SSP_SHORT_RESP, NULL, 0, false, &ocr);
+        ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 1, 0x40ff8000, SSP_SHORT_RESP, NULL, 0, false, false, &ocr);
         if(ret == 0 && ocr & (1 << 31))
             break;
     }while(!TIME_AFTER(current_tick, timeout));
 
     if(ret != 0)
         return -2;
-    
+    /* get CID */
     uint32_t cid[4];
-    ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 2, 0, SSP_LONG_RESP, NULL, 0, false, cid);
+    ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 2, 0, SSP_LONG_RESP, NULL, 0, false, false, cid);
     if(ret != 0)
         return -3;
+    /* Set RCA */
+    uint32_t status;
+    ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 3, MMC_RCA << 16, SSP_SHORT_RESP, NULL, 0, false, false, &status);
+    if(ret != 0)
+        return -4;
+    /* Select card */
+    ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 7, MMC_RCA << 16, SSP_SHORT_RESP, NULL, 0, false, false, &status);
+    if(ret != 0)
+        return -5;
+    /* Check TRAN state */
+    ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 13, MMC_RCA << 16, SSP_SHORT_RESP, NULL, 0, false, false, &status);
+    if(ret != 0)
+        return -6;
+    if(((status >> 9) & 0xf) != 4)
+        return -7;
+    /* Switch to 8-bit bus */
+    ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 6, 0x3b70200, SSP_SHORT_RESP, NULL, 0, true, false, &status);
+    if(ret != 0)
+        return -8;
+    /* switch error ? */
+    if(status & 0x80)
+        return -9;
+    imx233_ssp_set_bus_width(MMC_SSP, 8);
+    /* Switch to high speed mode */
+    ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 6, 0x3b90100, SSP_SHORT_RESP, NULL, 0, true, false, &status);
+    if(ret != 0)
+        return -10;
+    /* switch error ?*/
+    if(status & 0x80)
+        return -11;
+    /* SSPCLK @ 96MHz
+     * gives bitrate of 96 / 2 / 1 = 48MHz */
+    imx233_ssp_set_timings(MMC_SSP, 2, 0, 0xffff);
+
+    #ifdef SANSA_FUZEPLUS
+    /**
+     * The Fuze+ uses a strange layout: is has a first MBR at sector 0 with four entries:
+     * 1) Actual user partition
+     * 2) Sigmatel boot partition
+     * 3)4) Other (certificate related ?) partitions
+     * The partition 1) has type 1 but it's actually a type 5 (logical partition) with
+     * a second partition table with usually one entry which is the FAT32 one.
+     * The first table uses 512-byte sector size and the second one usually uses
+     * 2048-byte logical sector size.
+     *
+     * We restrict mmc window to the user partition */
+    uint8_t mbr[512];
+    mmc_window_start = 0;
+    mmc_window_end = INT_MAX;
+    ret = mmc_read_sectors(IF_MD2(0,) 0, 1, mbr);
+    if(ret != 0)
+        return -100;
+    if(mbr[510] != 0x55 || mbr[511] != 0xAA)
+        return -101; /* invalid MBR */
+    /* sanity check that the first partition is greater than 2Gib */
+    uint8_t *ent = &mbr[446];
+    mmc_window_start = ent[8] | ent[9] << 8 | ent[10] << 16 | ent[11] << 24;
+    mmc_window_end = (ent[12] | ent[13] << 8 | ent[14] << 16 | ent[15] << 24) +
+        mmc_window_start;
+    if(ent[4] == 0x53)
+        return -102; /* sigmatel partition */
+    if((mmc_window_end - mmc_window_start) < 4 * 1024 * 1024)
+        return -103; /* partition too small */
+    #endif
 
     return 0;
 }
@@ -81,13 +163,49 @@ int mmc_num_drives(int first_drive)
     return 1;
 }
 
-int mmc_read_sectors(IF_MD2(int drive,) unsigned long start, int count, void* buf)
+#ifdef STORAGE_GET_INFO
+void mmc_get_info(IF_MD2(int drive,) struct storage_info *info)
+{
+#ifdef HAVE_MULTIDRIVE
+    (void) drive;
+#endif
+    info->sector_size = 512;
+    info->num_sectors = 0xECC000 - 0x0001ac00;
+    info->vendor = "Rockbox";
+    info->product = "Internal Storage";
+    info->revision = "0.00";
+}
+#endif
+
+int mmc_read_sectors(IF_MD2(int drive,) unsigned long start, int count, void *buf)
 {
     IF_MD((void) drive);
-    (void) start;
-    (void) count;
-    (void) buf;
-    return -1;
+    /* check window */
+    start += mmc_window_start;
+    if((start + count) > mmc_window_end)
+        return -201;
+    /* get mutex (needed because we done multiple commands for count > 0 */
+    mutex_lock(&mmc_mutex);
+    int ret = 0;
+    uint32_t resp;
+
+    if(count == 1)
+    {
+        ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 17, start, SSP_SHORT_RESP, buf,
+            count, false, true, &resp);
+    }
+    else
+    {
+        ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 23, count, SSP_SHORT_RESP, NULL,
+            0, false, false, &resp);
+        if(ret == 0)
+            ret = imx233_ssp_sd_mmc_transfer(MMC_SSP, 18, start, SSP_SHORT_RESP, buf,
+                count, false, true, &resp);
+    }
+
+    mutex_unlock(&mmc_mutex);
+
+    return ret;
 }
 
 int mmc_write_sectors(IF_MD2(int drive,) unsigned long start, int count, const void* buf)
diff --git a/firmware/target/arm/imx233/sd-imx233.c b/firmware/target/arm/imx233/sd-imx233.c
index 6f30c7a3c6..ccd8bf35c1 100644
--- a/firmware/target/arm/imx233/sd-imx233.c
+++ b/firmware/target/arm/imx233/sd-imx233.c
@@ -25,7 +25,7 @@
 
 int sd_init(void)
 {
-    return -1;
+    return 0;
 }
 
 int sd_read_sectors(IF_MD2(int drive,) unsigned long start, int count,
diff --git a/firmware/target/arm/imx233/ssp-imx233.c b/firmware/target/arm/imx233/ssp-imx233.c
index c6be869ce6..22824964f3 100644
--- a/firmware/target/arm/imx233/ssp-imx233.c
+++ b/firmware/target/arm/imx233/ssp-imx233.c
@@ -42,6 +42,8 @@ static int ssp_in_use = 0;
 static struct mutex ssp_mutex[2];
 static struct semaphore ssp_sema[2];
 static struct ssp_dma_command_t ssp_dma_cmd[2];
+static uint32_t ssp_bus_width[2];
+static unsigned ssp_log_block_size[2];
 
 void INT_SSP(int ssp)
 {
@@ -84,6 +86,7 @@ void imx233_ssp_init(void)
     semaphore_init(&ssp_sema[1], 1, 0);
     mutex_init(&ssp_mutex[0]);
     mutex_init(&ssp_mutex[1]);
+    ssp_bus_width[0] = ssp_bus_width[1] = HW_SSP_CTRL0__BUS_WIDTH__ONE_BIT;
 }
 
 void imx233_ssp_start(int ssp)
@@ -96,11 +99,12 @@ void imx233_ssp_start(int ssp)
     /* If first block to start, start SSP clock */
     if(ssp_in_use == 0)
     {
+        /** 2.3.1: the clk_ssp maximum frequency is 102.858 MHz */
         /* fracdiv = 18 => clk_io = pll = 480Mhz
-         * intdiv = 4 => clk_ssp = 120Mhz */
+         * intdiv = 5 => clk_ssp = 96Mhz */
         imx233_set_fractional_divisor(CLK_IO, 18);
         imx233_enable_clock(CLK_SSP, false);
-        imx233_set_clock_divisor(CLK_SSP, 4);
+        imx233_set_clock_divisor(CLK_SSP, 5);
         imx233_set_bypass_pll(CLK_SSP, false); /* use IO */
         imx233_enable_clock(CLK_SSP, true);
     }
@@ -127,18 +131,9 @@ void imx233_ssp_softreset(int ssp)
     imx233_reset_block(&HW_SSP_CTRL0(ssp));
 }
 
-void imx233_ssp_set_timings(int ssp, int divide, int rate)
+void imx233_ssp_set_timings(int ssp, int divide, int rate, int timeout)
 {
-    __REG_CLR(HW_SSP_TIMING(ssp)) =
-        HW_SSP_TIMING__CLOCK_DIVIDE_BM | HW_SSP_TIMING__CLOCK_RATE_BM;
-    __REG_SET(HW_SSP_TIMING(ssp)) =
-        divide << HW_SSP_TIMING__CLOCK_DIVIDE_BP | rate;
-}
-
-void imx233_ssp_set_timeout(int ssp, int timeout)
-{
-    __REG_CLR(HW_SSP_TIMING(ssp)) = HW_SSP_TIMING__CLOCK_TIMEOUT_BM;
-    __REG_SET(HW_SSP_TIMING(ssp)) =
+    HW_SSP_TIMING(ssp) = divide << HW_SSP_TIMING__CLOCK_DIVIDE_BP | rate |
         timeout << HW_SSP_TIMING__CLOCK_TIMEOUT_BP;
 }
 
@@ -238,22 +233,43 @@ void imx233_ssp_set_mode(int ssp, unsigned mode)
     }
 }
 
-enum imx233_ssp_error_t imx233_ssp_sd_mmc_transfer(int ssp, uint8_t cmd, uint32_t cmd_arg,
-    enum imx233_ssp_resp_t resp, void *buffer, int xfer_size, bool read, uint32_t *resp_ptr)
+void imx233_ssp_set_bus_width(int ssp, unsigned width)
+{
+    switch(width)
+    {
+        case 1: ssp_bus_width[ssp - 1] = HW_SSP_CTRL0__BUS_WIDTH__ONE_BIT; break;
+        case 4: ssp_bus_width[ssp - 1] = HW_SSP_CTRL0__BUS_WIDTH__FOUR_BIT; break;
+        case 8: ssp_bus_width[ssp - 1] = HW_SSP_CTRL0__BUS_WIDTH__EIGHT_BIT; break;
+    }
+}
+
+void imx233_ssp_set_block_size(int ssp, unsigned log_block_size)
+{
+    ssp_log_block_size[ssp - 1] = log_block_size;
+}
+
+enum imx233_ssp_error_t imx233_ssp_sd_mmc_transfer(int ssp, uint8_t cmd,
+    uint32_t cmd_arg, enum imx233_ssp_resp_t resp, void *buffer, unsigned block_count,
+    bool wait4irq, bool read, uint32_t *resp_ptr)
 {
     mutex_lock(&ssp_mutex[ssp - 1]);
     /* Enable all interrupts */
     imx233_enable_interrupt(INT_SRC_SSP_DMA(ssp), true);
     imx233_dma_enable_channel_interrupt(APB_SSP(ssp), true);
-    /* Assume only one block so ignore block_count and block_size */
-    ssp_dma_cmd[ssp - 1].cmd0 = cmd | HW_SSP_CMD0__APPEND_8CYC;
+
+    unsigned xfer_size = block_count * (1 << ssp_log_block_size[ssp - 1]);
+    
+    ssp_dma_cmd[ssp - 1].cmd0 = cmd | HW_SSP_CMD0__APPEND_8CYC |
+        ssp_log_block_size[ssp - 1] << HW_SSP_CMD0__BLOCK_SIZE_BP |
+        (block_count - 1) << HW_SSP_CMD0__BLOCK_COUNT_BP;
     ssp_dma_cmd[ssp - 1].cmd1 = cmd_arg;
     /* setup all flags and run */
     ssp_dma_cmd[ssp - 1].ctrl0 = xfer_size | HW_SSP_CTRL0__ENABLE |
-        HW_SSP_CTRL0__IGNORE_CRC | 
-        (resp != SSP_NO_RESP ? HW_SSP_CTRL0__GET_RESP | HW_SSP_CTRL0__WAIT_FOR_IRQ : 0) |
+        (buffer ?  0 : HW_SSP_CTRL0__IGNORE_CRC) |
+        (wait4irq ? HW_SSP_CTRL0__WAIT_FOR_IRQ : 0) |
+        (resp != SSP_NO_RESP ? HW_SSP_CTRL0__GET_RESP : 0) |
         (resp == SSP_LONG_RESP ? HW_SSP_CTRL0__LONG_RESP : 0) |
-        HW_SSP_CTRL0__BUS_WIDTH__ONE_BIT << HW_SSP_CTRL0__BUS_WIDTH_BP |
+        (ssp_bus_width[ssp - 1] << HW_SSP_CTRL0__BUS_WIDTH_BP) |
         (buffer ? HW_SSP_CTRL0__DATA_XFER : 0) |
         (read ? HW_SSP_CTRL0__READ : 0);
     /* setup the dma parameters */
@@ -263,10 +279,11 @@ enum imx233_ssp_error_t imx233_ssp_sd_mmc_transfer(int ssp, uint8_t cmd, uint32_
         (buffer == NULL ? HW_APB_CHx_CMD__COMMAND__NO_XFER :
          read ? HW_APB_CHx_CMD__COMMAND__WRITE : HW_APB_CHx_CMD__COMMAND__READ) |
         HW_APB_CHx_CMD__IRQONCMPLT | HW_APB_CHx_CMD__SEMAPHORE |
-        HW_APB_CHx_CMD__WAIT4ENDCMD | HW_APB_CHx_CMD__HALTONTERMINATE |
+        HW_APB_CHx_CMD__WAIT4ENDCMD |
         (3 << HW_APB_CHx_CMD__CMDWORDS_BP) |
         (xfer_size << HW_APB_CHx_CMD__XFER_COUNT_BP);
 
+    imx233_dma_reset_channel(APB_SSP(ssp));
     imx233_dma_start_command(APB_SSP(ssp), &ssp_dma_cmd[ssp - 1].dma);
 
     /* the SSP hardware already has a timeout but we never know; 1 sec is a maximum
@@ -283,7 +300,7 @@ enum imx233_ssp_error_t imx233_ssp_sd_mmc_transfer(int ssp, uint8_t cmd, uint32_
     else
         ret = SSP_ERROR;
 
-    if(ret == SSP_SUCCESS && resp_ptr != NULL)
+    if(resp_ptr != NULL)
     {
         if(resp != SSP_NO_RESP)
             *resp_ptr++ = HW_SSP_SDRESP0(ssp);
diff --git a/firmware/target/arm/imx233/ssp-imx233.h b/firmware/target/arm/imx233/ssp-imx233.h
index e9bdf62b27..c7c891ec0b 100644
--- a/firmware/target/arm/imx233/ssp-imx233.h
+++ b/firmware/target/arm/imx233/ssp-imx233.h
@@ -67,7 +67,7 @@
 #define HW_SSP_CMD0__BLOCK_SIZE_BM      (0xf << 16)
 #define HW_SSP_CMD0__BLOCK_SIZE_BP      16
 #define HW_SSP_CMD0__BLOCK_COUNT_BM     (0xff << 8)
-#define HW_SSP_CMD0__BLOCK_COUNT_BP     16
+#define HW_SSP_CMD0__BLOCK_COUNT_BP     8
 #define HW_SSP_CMD0__CMD_BM             0xff
 
 #define HW_SSP_CMD1(ssp)    (*(volatile uint32_t *)(HW_SSP_BASE(ssp) + 0x20))
@@ -147,12 +147,15 @@ void imx233_ssp_start(int ssp);
 void imx233_ssp_stop(int ssp);
 /* only softreset between start and stop or it might hang ! */
 void imx233_ssp_softreset(int ssp);
-void imx233_ssp_set_timings(int ssp, int divide, int rate);
-void imx233_ssp_set_timeout(int ssp, int timeout);
+void imx233_ssp_set_timings(int ssp, int divide, int rate, int timeout);
 void imx233_ssp_set_mode(int ssp, unsigned mode);
+void imx233_ssp_set_bus_width(int ssp, unsigned width);
+/* block_size uses the SSP format so it's actually the log_2 of the block_size */
+void imx233_ssp_set_block_size(int ssp, unsigned log_block_size);
 /* SD/MMC facilities */
-enum imx233_ssp_error_t imx233_ssp_sd_mmc_transfer(int ssp, uint8_t cmd, uint32_t cmd_arg,
-    enum imx233_ssp_resp_t resp, void *buffer, int xfer_size, bool read, uint32_t *resp_ptr);
+enum imx233_ssp_error_t imx233_ssp_sd_mmc_transfer(int ssp, uint8_t cmd,
+    uint32_t cmd_arg, enum imx233_ssp_resp_t resp, void *buffer, unsigned block_count,
+    bool wait4irq, bool read, uint32_t *resp_ptr);
 void imx233_ssp_setup_ssp2_sd_mmc_pins(bool enable_pullups, unsigned bus_width,
     unsigned drive_strength);
 /* SD/MMC requires that the card be provided the clock during an init sequence of
diff --git a/firmware/target/arm/imx233/system-imx233.c b/firmware/target/arm/imx233/system-imx233.c
index 9c843c48c1..6114ecca08 100644
--- a/firmware/target/arm/imx233/system-imx233.c
+++ b/firmware/target/arm/imx233/system-imx233.c
@@ -31,6 +31,7 @@
 #include "ssp-imx233.h"
 #include "lcd.h"
 #include "backlight-target.h"
+#include "button-target.h"
 
 #define default_interrupt(name) \
     extern __attribute__((weak, alias("UIRQ"))) void name(void)
@@ -104,7 +105,13 @@ void system_reboot(void)
 
 void system_exception_wait(void)
 {
-    /* what is this supposed to do ? */
+    /* make sure lcd and backlight are on */
+    _backlight_on();
+    _backlight_set_brightness(100);
+    /* wait until button release (if a button is pressed) */
+    while(button_read_device());
+    /* then wait until next button press */
+    while(!button_read_device());
 }
 
 void imx233_enable_interrupt(int src, bool enable)
@@ -177,3 +184,23 @@ void udelay(unsigned us)
     while(!imx233_us_elapsed(ref, us));
 }
 
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+void set_cpu_frequency(long frequency)
+{
+    switch(frequency)
+    {
+        case IMX233_CPUFREQ_454_MHz:
+            /* clk_h@clk_p/3 */
+            imx233_set_clock_divisor(CLK_AHB, 3);
+            /* clk_p@ref_cpu/1*18/19 */
+            imx233_set_fractional_divisor(CLK_CPU, 19);
+            imx233_set_clock_divisor(CLK_CPU, 1);
+            /* ref_cpu@480 MHz
+             * clk_p@454.74 MHz
+             * clk_h@151.58 MHz */
+            break;
+        default:
+            break;
+    }
+}
+#endif
diff --git a/firmware/target/arm/imx233/system-target.h b/firmware/target/arm/imx233/system-target.h
index 163eacb41f..0f7bde4896 100644
--- a/firmware/target/arm/imx233/system-target.h
+++ b/firmware/target/arm/imx233/system-target.h
@@ -31,10 +31,35 @@
 #define HW_DIGCTL_MICROSECONDS  (*(volatile uint32_t *)(HW_DIGCTL_BASE + 0xC0))
 
 #define HW_POWER_BASE           0x80044000
+
+#define HW_POWER_CTRL           (*(volatile uint32_t *)(HW_POWER_BASE + 0x0))
+
+#define HW_POWER_5VCTRL         (*(volatile uint32_t *)(HW_POWER_BASE + 0x10))
+
+#define HW_POWER_MINPWR         (*(volatile uint32_t *)(HW_POWER_BASE + 0x20))
+
+#define HW_POWER_CHARGE         (*(volatile uint32_t *)(HW_POWER_BASE + 0x30))
+
+#define HW_POWER_VDDDCTRL       (*(volatile uint32_t *)(HW_POWER_BASE + 0x40))
+#define HW_POWER_VDDDCTRL__TRG_BP   0
+#define HW_POWER_VDDDCTRL__TRG_BM   0x1f
+#define HW_POWER_VDDDCTRL__TRG_STEP 25 /* mV */
+#define HW_POWER_VDDDCTRL__TRG_MIN  800 /* mV */
+
+#define HW_POWER_VDDACTRL       (*(volatile uint32_t *)(HW_POWER_BASE + 0x50))
+
+#define HW_POWER_VDDIOCTRL      (*(volatile uint32_t *)(HW_POWER_BASE + 0x60))
+
+#define HW_POWER_VDDMEMCTRL     (*(volatile uint32_t *)(HW_POWER_BASE + 0x70))
+
+#define HW_POWER_MISC           (*(volatile uint32_t *)(HW_POWER_BASE + 0x90))
+
 #define HW_POWER_STS            (*(volatile uint32_t *)(HW_POWER_BASE + 0xc0))
 #define HW_POWER_STS__PSWITCH_BP    20
 #define HW_POWER_STS__PSWITCH_BM    (3 << 20)
 
+#define HW_POWER_BATTMONITOR    (*(volatile uint32_t *)(HW_POWER_BASE + 0xe0))
+
 #define HW_POWER_RESET          (*(volatile uint32_t *)(HW_POWER_BASE + 0x100))
 #define HW_POWER_RESET__UNLOCK  0x3E770000
 #define HW_POWER_RESET__PWD     0x1
@@ -67,6 +92,22 @@
 #define INT_SRC_LCDIF_ERROR 46
 #define INT_SRC_NR_SOURCES  66
 
+/**
+ * Absolute maximum CPU speed: 454.74 MHz
+ * Intermediate CPU speeds: 392.73 MHz, 360MHz, 261.82 MHz, 64 MHz
+ * Absolute minimum CPU speed: 24 MHz */
+#define IMX233_CPUFREQ_454_MHz  454740000
+#define IMX233_CPUFREQ_392_MHz  392730000
+#define IMX233_CPUFREQ_360_MHz  360000000
+#define IMX233_CPUFREQ_261_MHz  261820000
+#define IMX233_CPUFREQ_64_MHz    64000000
+#define IMX233_CPUFREQ_24_MHz    24000000
+
+#define CPUFREQ_DEFAULT     IMX233_CPUFREQ_454_MHz
+#define CPUFREQ_NORMAL      IMX233_CPUFREQ_454_MHz
+#define CPUFREQ_MAX         IMX233_CPUFREQ_454_MHz
+#define CPUFREQ_SLEEP       IMX233_CPUFREQ_454_MHz
+
 void imx233_enable_interrupt(int src, bool enable);
 void imx233_softirq(int src, bool enable);
 void udelay(unsigned us);
diff --git a/firmware/target/arm/imx233/usb-imx233.c b/firmware/target/arm/imx233/usb-imx233.c
index 83e50839b4..e3b540fd1e 100644
--- a/firmware/target/arm/imx233/usb-imx233.c
+++ b/firmware/target/arm/imx233/usb-imx233.c
@@ -48,7 +48,6 @@ void usb_drv_int_enable(bool enable)
 
 void INT_USB_CTRL(void)
 {
-    printf("usb int");
     usb_drv_int();
 }