rp2_sd: Add write support to SD driver

Add write support to rp2_sd driver.

Cleanup standalone-mp3 test tool and add write test mode.

Signed-off-by: Matthias Blankertz <matthias@blankertz.org>

software/modules/rp2_sd/module.c

@@ -79,6 +79,25 @@ static mp_obj_t sdcard_readblocks(mp_obj_t self_obj, mp_obj_t block_obj, mp_obj_
 }
 static MP_DEFINE_CONST_FUN_OBJ_3(sdcard_readblocks_obj, sdcard_readblocks);
 
+static mp_obj_t sdcard_writeblocks(mp_obj_t self_obj, mp_obj_t block_obj, mp_obj_t buf_obj)
+{
+    struct sdcard_obj *self = MP_OBJ_TO_PTR(self_obj);
+    const int start_block = mp_obj_get_int(block_obj);
+    mp_buffer_info_t bufinfo;
+    if (!mp_get_buffer(buf_obj, &bufinfo, MP_BUFFER_READ))
+        mp_raise_ValueError("Not a read buffer");
+    if (bufinfo.len % SD_SECTOR_SIZE != 0)
+        mp_raise_ValueError("Buffer length is invalid");
+    const int nblocks = bufinfo.len / SD_SECTOR_SIZE;
+    for (int block = 0; block < nblocks; block++) {
+        // TODO: Implement CMD25 write multiple blocks
+        if (!sd_writeblock(&self->sd_context, start_block + block, bufinfo.buf + block * SD_SECTOR_SIZE))
+            mp_raise_OSError(MP_EIO);
+    }
+    return mp_const_none;
+}
+static MP_DEFINE_CONST_FUN_OBJ_3(sdcard_writeblocks_obj, sdcard_writeblocks);
+
 static mp_obj_t sdcard_ioctl(mp_obj_t self_obj, mp_obj_t op_obj, mp_obj_t arg_obj)
 {
     struct sdcard_obj *self = MP_OBJ_TO_PTR(self_obj);
@@ -99,6 +118,7 @@ static const mp_rom_map_elem_t sdcard_locals_dict_table[] = {
     {MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&sdcard_deinit_obj)},
     {MP_ROM_QSTR(MP_QSTR_ioctl), MP_ROM_PTR(&sdcard_ioctl_obj)},
     {MP_ROM_QSTR(MP_QSTR_readblocks), MP_ROM_PTR(&sdcard_readblocks_obj)},
+    {MP_ROM_QSTR(MP_QSTR_writeblocks), MP_ROM_PTR(&sdcard_writeblocks_obj)},
 };
 static MP_DEFINE_CONST_DICT(sdcard_locals_dict, sdcard_locals_dict_table);
 

software/modules/rp2_sd/sd.c

@@ -265,3 +265,11 @@ bool sd_readblock_complete(struct sd_context *sd_context)
 }
 
 bool sd_readblock_is_complete(struct sd_context *sd_context) { return sd_cmd_read_is_complete(); }
+
+bool sd_writeblock(struct sd_context *sd_context, size_t sector_num, uint8_t buffer[const static SD_SECTOR_SIZE])
+{
+    if (!sd_context->initialized || sector_num >= sd_context->blocks)
+        return false;
+
+    return sd_cmd_write(24, sector_num, SD_SECTOR_SIZE, buffer);
+}

software/modules/rp2_sd/sd.h

@@ -21,3 +21,5 @@ bool sd_readblock(struct sd_context *context, size_t sector_num, uint8_t buffer[
 bool sd_readblock_start(struct sd_context *context, size_t sector_num, uint8_t buffer[static SD_SECTOR_SIZE]);
 bool sd_readblock_complete(struct sd_context *context);
 bool sd_readblock_is_complete(struct sd_context *context);
+
+bool sd_writeblock(struct sd_context *context, size_t sector_num, uint8_t buffer[const static SD_SECTOR_SIZE]);

software/modules/rp2_sd/sd_spi.c

@@ -211,6 +211,61 @@ bool sd_cmd_read_complete(void)
     return (sd_spi_context.sd_dma_context.read_token_buf == 0xfe);
 }
 
+bool sd_cmd_write(uint8_t cmd, uint32_t arg, unsigned datalen, uint8_t data[const static datalen])
+{
+    uint8_t buf[2];
+    const uint16_t crc = sd_crc16(datalen, data);
+    sd_spi_cmd_send(cmd, arg);
+    // Read up to 8 garbage bytes (0xff), followed by R1 (MSB is zero)
+    bool got_r1 = false;
+    for (int timeout = 0; timeout < 8; ++timeout) {
+        sd_spi_read_blocking(0xff, buf, 1);
+        if (!(buf[0] & 0x80)) {
+            got_r1 = true;
+            break;
+        }
+    }
+    if (!got_r1 || buf[0] != 0x00)
+        goto abort;
+    buf[0] = 0xfe;
+    sd_spi_write_blocking(buf, 1);
+    sd_spi_write_blocking(data, datalen);
+    buf[0] = crc >> 8;
+    buf[1] = crc;
+    sd_spi_write_blocking(buf, 2);
+    sd_spi_read_blocking(0xff, buf, 1);
+    if ((buf[0] & 0x1f) != 0x5) {
+#ifdef SD_DEBUG
+        printf("Write fail: %2hhx\n", buf[0]);
+#endif
+        goto abort;
+    }
+
+    int timeout = 0;
+    bool got_done = false;
+    for (timeout = 0; timeout < 8192; ++timeout) {
+        sd_spi_read_blocking(0xff, buf, 1);
+        if (buf[0] != 0x0) {
+            got_done = true;
+            break;
+        }
+    }
+#ifdef SD_DEBUG
+    printf("dbg write end: %d, %2hhx\n", timeout, buf[0]);
+#endif
+    if (!got_done)
+        goto abort;
+
+    gpio_put(sd_spi_context.ss, true);
+    sd_spi_read_blocking(0xff, buf, 1);
+    return true;
+
+abort:
+    gpio_put(sd_spi_context.ss, true);
+    sd_spi_read_blocking(0xff, buf, 1);
+    return false;
+}
+
 bool sd_spi_init(int mosi, int miso, int sck, int ss)
 {
     if (sd_spi_context.initialized)
@@ -243,6 +298,7 @@ bool sd_spi_init(int mosi, int miso, int sck, int ss)
     channel_config_set_transfer_data_size(&sd_spi_context.spi_dma_rd_cfg, DMA_SIZE_8);
     sd_spi_context.spi_dma_rd_crc_cfg = dma_channel_get_default_config(sd_spi_context.spi_dma_rd_crc);
     channel_config_set_read_increment(&sd_spi_context.spi_dma_rd_crc_cfg, false);
+    channel_config_set_write_increment(&sd_spi_context.spi_dma_rd_crc_cfg, true);
     channel_config_set_dreq(&sd_spi_context.spi_dma_rd_crc_cfg, pio_get_dreq(SD_PIO, sd_spi_context.spi_sm, false));
     channel_config_set_transfer_data_size(&sd_spi_context.spi_dma_rd_crc_cfg, DMA_SIZE_8);
     sd_spi_context.spi_dma_wr_cfg = dma_channel_get_default_config(sd_spi_context.spi_dma_wr);

software/modules/rp2_sd/sd_spi.h

@@ -25,3 +25,5 @@ void sd_spi_dbg_clk(const int div, const int frac);
 bool sd_cmd_read_start(uint8_t cmd, uint32_t arg, unsigned datalen, uint8_t data[static datalen]);
 bool sd_cmd_read_complete(void);
 bool sd_cmd_read_is_complete(void);
+
+bool sd_cmd_write(uint8_t cmd, uint32_t arg, unsigned datalen, uint8_t data[const static datalen]);

software/modules/rp2_sd/sd_util.h

@@ -16,15 +16,15 @@ inline static uint8_t sd_crc7(size_t len, const uint8_t data[const static len])
     return crc >> 1;
 }
 
-/* inline static uint16_t sd_crc16(size_t len, const uint8_t data[const static len]) */
+inline static uint16_t sd_crc16(size_t len, const uint8_t data[const static len])
-/* { */
+{
-/*     const uint16_t poly = 0b1000000100001; */
+    const uint16_t poly = 0b1000000100001;
-/*     uint16_t crc = 0; */
+    uint16_t crc = 0;
-/*     for (size_t pos = 0; pos < len; ++pos) { */
+    for (size_t pos = 0; pos < len; ++pos) {
-/*         crc ^= data[pos] << 8; */
+        crc ^= data[pos] << 8;
-/*         for (int bit = 0; bit < 8; ++bit) { */
+        for (int bit = 0; bit < 8; ++bit) {
-/*             crc = (crc << 1) ^ ((crc & 0x8000) ? poly : 0); */
+            crc = (crc << 1) ^ ((crc & 0x8000) ? poly : 0);
-/*         } */
+        }
-/*     } */
+    }
-/*     return crc; */
+    return crc;
-/* } */
+}

software/tools/standalone-mp3/CMakeLists.txt

@@ -9,6 +9,9 @@ include(../../lib/micropython/lib/pico-sdk/pico_sdk_init.cmake)
 
 project(standalone_mp3)
 
+option(ENABLE_WRITE_TEST "Enable write test" OFF)
+option(ENABLE_PLAY_TEST "Enable mp3 playback test" OFF)
+
 # initialize the Raspberry Pi Pico SDK
 pico_sdk_init()
 
@@ -28,6 +31,14 @@ add_executable(standalone_mp3
     ${SD_LIB_SRCS}
 )
 
+if(ENABLE_WRITE_TEST)
+    target_compile_definitions(standalone_mp3 PRIVATE WRITE_TEST)
+endif()
+
+if(ENABLE_PLAY_TEST)
+    target_compile_definitions(standalone_mp3 PRIVATE PLAY_TEST)
+endif()
+
 pico_generate_pio_header(standalone_mp3 ${SD_LIB_DIR}/sd_spi_pio.pio)
 
 pico_generate_pio_header(standalone_mp3 ${CMAKE_CURRENT_LIST_DIR}/i2s_max98357.pio)
@@ -36,7 +47,7 @@ add_subdirectory(../../lib/helix_mp3 helix_mp3)
 
 target_link_libraries(standalone_mp3 PRIVATE pico_stdlib hardware_dma hardware_spi hardware_sync hardware_pio helix_mp3)
 target_include_directories(standalone_mp3 PRIVATE ${SD_LIB_DIR})
-target_compile_options(standalone_mp3 PRIVATE -Og)
+target_compile_options(standalone_mp3 PRIVATE -Og -DSD_DEBUG)
 
 
 pico_add_extra_outputs(standalone_mp3)

software/tools/standalone-mp3/main.c

@@ -29,17 +29,8 @@ void __time_critical_func(volume_adjust)(int16_t *restrict buf, size_t samples,
     }
 }
 
-int __time_critical_func(main)()
+static int __time_critical_func(play_mp3)(struct sd_context *sd_context)
 {
-    stdio_init_all();
-    printf("sysclk is %d Hz\n", clock_get_hz(clk_sys));
-
-    struct sd_context sd_context;
-
-    if (!sd_init(&sd_context, 3, 4, 2, 5, 15000000)) {
-        return 1;
-    }
-
     HMP3Decoder mp3dec = MP3InitDecoder();
 
     if (!i2s_init(44100)) {
@@ -48,21 +39,20 @@ int __time_critical_func(main)()
 
     uint8_t mp3buffer[4 * 512];
     for (int i = 0; i < sizeof(mp3buffer) / 512; ++i) {
-        sd_readblock(&sd_context, i, mp3buffer + 512 * i);
+        sd_readblock(sd_context, i, mp3buffer + 512 * i);
     }
     size_t next_sector = sizeof(mp3buffer) / 512;
 
     unsigned char *readptr = mp3buffer;
     int bytes_left = sizeof(mp3buffer);
 
-    int pos = 0;
     bool first = true;
     bool pending_read = false;
     bool synced = false;
     while (true) {
         /* Get some input data */
-        if (pending_read && sd_readblock_is_complete(&sd_context)) {
+        if (pending_read && sd_readblock_is_complete(sd_context)) {
-            sd_readblock_complete(&sd_context);
+            sd_readblock_complete(sd_context);
             bytes_left += 512;
             pending_read = false;
         }
@@ -74,7 +64,7 @@ int __time_critical_func(main)()
                 memmove(mp3buffer, readptr, bytes_left);
                 readptr = mp3buffer;
             }
-            sd_readblock_start(&sd_context, next_sector++, readptr + bytes_left);
+            sd_readblock_start(sd_context, next_sector++, readptr + bytes_left);
             pending_read = true;
         }
         if (bytes_left == 0) {
@@ -115,7 +105,7 @@ int __time_critical_func(main)()
                 readptr = old_readptr;
                 bytes_left = old_bytes_left;
                 printf("INDATA_UNDERFLOW\n");
-                sd_readblock_complete(&sd_context);
+                sd_readblock_complete(sd_context);
                 continue;
             } else /*if (status== ERR_MP3_MAINDATA_UNDERFLOW)*/ {
                 --bytes_left;
@@ -143,6 +133,50 @@ int __time_critical_func(main)()
 
         i2s_commit_buf(buf);
     }
+}
+
+static void write_test(struct sd_context *sd_context)
+{
+    uint8_t data_buffer[4096];
+    do {
+        for (int i = 0; i < sizeof(data_buffer) / SD_SECTOR_SIZE; ++i) {
+            sd_readblock(sd_context, i, data_buffer + SD_SECTOR_SIZE * i);
+        }
+
+        for (int line = 0; line < 32; ++line) {
+            printf("%04hx ", line * 16);
+            for (int item = 0; item < 16; ++item) {
+                printf("%02hhx%c", data_buffer[line * 16 + item], (item == 15) ? '\n' : ' ');
+            }
+        }
+
+        for (int i = 0; i < SD_SECTOR_SIZE; ++i) {
+            data_buffer[i] ^= 0xff;
+        }
+
+        sd_writeblock(sd_context, 0, data_buffer);
+        sleep_ms(1000);
+    } while (data_buffer[SD_SECTOR_SIZE - 1] != 0xAA);
+}
+
+int main()
+{
+    stdio_init_all();
+    printf("sysclk is %d Hz\n", clock_get_hz(clk_sys));
+
+    struct sd_context sd_context;
+
+    if (!sd_init(&sd_context, 3, 4, 2, 5, 15000000)) {
+        return 1;
+    }
+
+#ifdef WRITE_TEST
+    write_test(&sd_context);
+#endif
+
+#ifdef PLAY_TEST
+    play_mp3(&sd_context);
+#endif
 
     printf("Done.\n");
 }