Merge pull request 'Add SDCard write support' (#31) from standalone-mp3 into main

Reviewed-on: #31
Reviewed-by: Stefan Kratochwil <kratochwil-la@gmx.de>

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

@@ -208,9 +208,74 @@ bool sd_cmd_read_complete(void)
     sd_spi_wait_complete();
     gpio_put(sd_spi_context.ss, true);
     sd_spi_read_blocking(0xff, &buf, 1);
+#ifdef SD_READ_CRC_CHECK
+    const uint16_t expect_crc = sd_crc16(sd_spi_context.sd_dma_context.len, sd_spi_context.sd_dma_context.read_buf);
+    const uint16_t act_crc = sd_spi_context.sd_dma_context.crc_buf[0] << 8 | sd_spi_context.sd_dma_context.crc_buf[1];
+    if (act_crc != expect_crc) {
+#ifdef SD_DEBUG
+        printf("read CRC fail: got %04hx, expected %04hx\n", act_crc, expect_crc);
+#endif
+        return false;
+    }
+#endif
     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 +308,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]) */
-/* { */
-/*     const uint16_t poly = 0b1000000100001; */
-/*     uint16_t crc = 0; */
-/*     for (size_t pos = 0; pos < len; ++pos) { */
-/*         crc ^= data[pos] << 8; */
-/*         for (int bit = 0; bit < 8; ++bit) { */
-/*             crc = (crc << 1) ^ ((crc & 0x8000) ? poly : 0); */
-/*         } */
-/*     } */
-/*     return crc; */
-/* } */
+inline static uint16_t sd_crc16(size_t len, const uint8_t data[const static len])
+{
+    const uint16_t poly = 0b1000000100001;
+    uint16_t crc = 0;
+    for (size_t pos = 0; pos < len; ++pos) {
+        crc ^= data[pos] << 8;
+        for (int bit = 0; bit < 8; ++bit) {
+            crc = (crc << 1) ^ ((crc & 0x8000) ? poly : 0);
+        }
+    }
+    return crc;
+}

software/tools/standalone-mp3/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 3.13)
+
+# Workaround for pico-sdk host toolchain issue, see directory for details
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../lib/micropython/ports/rp2/tools_patch")
+
+# initialize pico-sdk from submodule
+# note: this must happen before project()
+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)
+option(ENABLE_SD_READ_CRC "Enable crc check when reading from sd card" OFF)
+option(ENABLE_SD_DEBUG "Enable debug output for sd card driver" OFF)
+
+# initialize the Raspberry Pi Pico SDK
+pico_sdk_init()
+
+set(SD_LIB_DIR "${CMAKE_CURRENT_LIST_DIR}/../../modules/rp2_sd")
+
+set(SD_LIB_SRCS
+    "${SD_LIB_DIR}/sd.c"
+    "${SD_LIB_DIR}/sd_spi.c"
+)
+
+
+add_executable(standalone_mp3
+    main.c
+    i2s.c
+    ${CMAKE_CURRENT_BINARY_DIR}/sd_spi_pio.pio.h
+    ${CMAKE_CURRENT_BINARY_DIR}/i2s_max98357.pio.h
+    ${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()
+
+if(ENABLE_SD_READ_CRC)
+    target_compile_definitions(standalone_mp3 PRIVATE SD_READ_CRC_CHECK)
+endif()
+
+if(ENABLE_SD_DEBUG)
+    target_compile_definitions(standalone_mp3 PRIVATE SD_DEBUG)
+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)
+
+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 -DSD_DEBUG)
+
+
+pico_add_extra_outputs(standalone_mp3)
+pico_enable_stdio_uart(standalone_mp3 1)
+
+set_property(TARGET standalone_mp3 APPEND_STRING PROPERTY LINK_FLAGS "-Wl,--print-memory-usage")
+

software/tools/standalone-mp3/i2s.c

@@ -0,0 +1,116 @@
+#include "i2s.h"
+#include "i2s_max98357.pio.h"
+
+#include <hardware/dma.h>
+#include <hardware/sync.h>
+
+#include <string.h>
+#include <stdio.h>
+
+#define audiocore_pio pio1
+
+#define AUDIO_BUFS 3
+
+struct i2s_context {
+    unsigned pio_program_offset;
+    int pio_sm;
+    int dma_ch;
+    dma_channel_config dma_config;
+    uint32_t dma_buf[AUDIO_BUFS][I2S_DMA_BUF_SIZE];
+    int cur_playing;
+    bool has_data[AUDIO_BUFS];
+    bool playback_active;
+};
+
+static struct i2s_context i2s_context;
+
+#define OUT_PIN 8
+#define SIDESET_BASE 6
+
+static void dma_isr(void)
+{
+    if (!dma_channel_get_irq1_status(i2s_context.dma_ch))
+        return;
+    dma_channel_acknowledge_irq1(i2s_context.dma_ch);
+    const int next_buf = (i2s_context.cur_playing + 1) % AUDIO_BUFS;
+    if (i2s_context.playback_active && i2s_context.has_data[next_buf]) {
+        i2s_context.cur_playing = next_buf;
+        i2s_context.has_data[next_buf] = false;
+    } else {
+        memset(i2s_context.dma_buf[i2s_context.cur_playing], 0, sizeof(uint32_t) * I2S_DMA_BUF_SIZE);
+        if (i2s_context.playback_active)
+            printf("x");
+    }
+    dma_channel_transfer_from_buffer_now(i2s_context.dma_ch, i2s_context.dma_buf[i2s_context.cur_playing], I2S_DMA_BUF_SIZE);
+}
+
+static void setup_dma_config(void)
+{
+    i2s_context.dma_config = dma_channel_get_default_config(i2s_context.dma_ch);
+    channel_config_set_dreq(&i2s_context.dma_config, pio_get_dreq(pio1, i2s_context.pio_sm, true));
+}
+
+uint32_t *i2s_next_buf(void)
+{
+    const long flags = save_and_disable_interrupts();
+    const int next_buf = (i2s_context.cur_playing + 1) % AUDIO_BUFS;
+    uint32_t *ret = NULL;
+    if (!i2s_context.has_data[next_buf]) {
+        ret = i2s_context.dma_buf[next_buf];
+    } else {
+        const int next_buf_2 = (next_buf + 1) % AUDIO_BUFS;
+        if (!i2s_context.has_data[next_buf_2]) {
+            ret = i2s_context.dma_buf[next_buf_2];
+        }
+    }
+    restore_interrupts(flags);
+    return ret;
+}
+
+void i2s_commit_buf(uint32_t *buf)
+{
+    const long flags = save_and_disable_interrupts();
+    const int next_buf = (i2s_context.cur_playing + 1) % AUDIO_BUFS;
+    const int next_buf_2 = (next_buf + 1) % AUDIO_BUFS;
+    if (i2s_context.dma_buf[next_buf] == buf) {
+        i2s_context.has_data[next_buf] = true;
+    } else if (i2s_context.dma_buf[next_buf_2] == buf) {
+        i2s_context.has_data[next_buf_2] = true;
+        i2s_context.playback_active = true;
+    } else {
+        assert(false);
+    }
+    restore_interrupts(flags);
+}
+
+bool i2s_init(int samplerate)
+{
+    memset(i2s_context.dma_buf, 0, sizeof(i2s_context.dma_buf[0][0]) * 2* I2S_DMA_BUF_SIZE);
+    if (!pio_can_add_program(audiocore_pio, (const pio_program_t *)&i2s_max98357_program))
+        return false;
+    i2s_context.pio_sm = pio_claim_unused_sm(audiocore_pio, false);
+    if (i2s_context.pio_sm == -1)
+        return false;
+    i2s_context.pio_program_offset = pio_add_program(audiocore_pio, (const pio_program_t *)&i2s_max98357_program);
+    i2s_max98357_program_init(audiocore_pio, i2s_context.pio_sm, i2s_context.pio_program_offset, OUT_PIN, SIDESET_BASE,
+                              samplerate);
+
+    i2s_context.dma_ch = dma_claim_unused_channel(false);
+    if (i2s_context.dma_ch == -1)
+        goto out_dma_claim;
+    i2s_context.playback_active = false;
+    setup_dma_config();
+    irq_add_shared_handler(DMA_IRQ_1, &dma_isr, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
+    dma_channel_set_irq1_enabled(i2s_context.dma_ch, true);
+    irq_set_enabled(DMA_IRQ_1, true);
+    dma_channel_configure(i2s_context.dma_ch, &i2s_context.dma_config, &audiocore_pio->txf[i2s_context.pio_sm],
+                          i2s_context.dma_buf, I2S_DMA_BUF_SIZE, true);
+    pio_sm_set_enabled(audiocore_pio, i2s_context.pio_sm, true);
+
+    return true;
+
+out_dma_claim:
+    pio_remove_program(audiocore_pio, (const pio_program_t *)&i2s_max98357_program, i2s_context.pio_program_offset);
+    pio_sm_unclaim(audiocore_pio, i2s_context.pio_sm);
+    return false;
+}

software/tools/standalone-mp3/i2s.h

@@ -0,0 +1,11 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define I2S_DMA_BUF_SIZE (1152)
+
+bool i2s_init(int samplerate);
+
+uint32_t *i2s_next_buf(void);
+void i2s_commit_buf(uint32_t *buf);

software/tools/standalone-mp3/i2s_max98357.pio

@@ -0,0 +1,59 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2024 Matthias Blankertz <matthias@blankertz.org>
+
+	.program i2s_max98357
+	.side_set 2
+
+	.lang_opt python sideset_init = pico.PIO.OUT_LOW
+	.lang_opt python out_init = pico.PIO.OUT_LOW
+	.lang_opt python out_shiftdir = pcio.PIO.SHIFT_LEFT
+	.lang_opt python autopull = True
+	
+// data - DOUT
+// sideset - 2-BCLK, 1-LRCLK
+
+	set	x,15			side 0
+	nop				side 1
+startup_loop:
+	nop				side 2
+	jmp	x-- startup_loop	side 3
+	nop				side 0
+	set	x, 14			side 1
+
+left_loop:
+	.wrap_target
+	out	pins, 1			side 0
+	jmp	x-- left_loop		side 1
+	out	pins, 1			side 2
+	set	x, 14			side 3
+right_loop:	
+	out	pins, 1			side 2
+	jmp	x-- right_loop		side 3
+	out	pins, 1			side 0
+	set	x, 14			side 1
+	.wrap
+
+% c-sdk {
+#include "hardware/clocks.h"
+
+static inline void i2s_max98357_program_init(PIO pio, uint sm, uint offset, uint pin, uint sideset, uint samplerate) {
+    pio_gpio_init(pio, pin);
+    pio_gpio_init(pio, sideset);
+    pio_gpio_init(pio, sideset+1);
+    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
+    pio_sm_set_consecutive_pindirs(pio, sm, sideset, 2, true);
+
+    pio_sm_config c = i2s_max98357_program_get_default_config(offset);
+    sm_config_set_out_pins(&c, pin, 1);
+    sm_config_set_sideset_pins(&c, sideset);
+    sm_config_set_out_shift(&c, false, true, 32);
+    sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
+
+    const unsigned i2s_freq = samplerate * 2 * 16 * 2;
+    const float div = clock_get_hz(clk_sys) / (float)i2s_freq;
+    sm_config_set_clkdiv(&c, div);
+
+    pio_sm_init(pio, sm, offset, &c);
+    //pio_sm_set_enabled(pio, sm, true);
+}
+%}

software/tools/standalone-mp3/main.c

@@ -0,0 +1,188 @@
+#include "i2s.h"
+#include "sd.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <hardware/clocks.h>
+#include <hardware/gpio.h>
+#include <hardware/spi.h>
+#include <hardware/sync.h>
+#include <pico/stdio.h>
+#include <pico/stdlib.h>
+#include <pico/time.h>
+
+#include "mp3dec.h"
+#include "sd_spi.h"
+
+extern void sd_spi_dbg_clk(const int div, const int frac);
+
+extern void sd_spi_dbg_loop(void);
+
+#define MAX_VOLUME 0x8000u
+
+void __time_critical_func(volume_adjust)(int16_t *restrict buf, size_t samples, uint16_t scalef)
+{
+    for (size_t pos = 0; pos < samples; ++pos) {
+        buf[pos] = ((int32_t)buf[pos] * scalef) >> 15;
+    }
+}
+
+static int __time_critical_func(play_mp3)(struct sd_context *sd_context)
+{
+    HMP3Decoder mp3dec = MP3InitDecoder();
+
+    if (!i2s_init(44100)) {
+        return 1;
+    }
+
+    uint8_t mp3buffer[4 * 512];
+    for (int i = 0; i < sizeof(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);
+
+    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)) {
+            sd_readblock_complete(sd_context);
+            bytes_left += 512;
+            pending_read = false;
+        }
+        if (!pending_read && (sizeof(mp3buffer) - bytes_left >= 512)) {
+            // If there is not enough space for an mp3 frame, or if there is less than one SD block to the end, move
+            // remaining data to start of buffer
+            if (readptr - mp3buffer >= sizeof(mp3buffer) - 1044 ||
+                readptr - mp3buffer > sizeof(mp3buffer) - 512 - bytes_left) {
+                memmove(mp3buffer, readptr, bytes_left);
+                readptr = mp3buffer;
+            }
+            sd_readblock_start(sd_context, next_sector++, readptr + bytes_left);
+            pending_read = true;
+        }
+        if (bytes_left == 0) {
+            // Can't do anything without input, wait and try again
+            __wfe();
+            continue;
+        }
+
+        // Synchronize MP3 stream if neccessary
+        if (!synced) {
+            const int ofs = MP3FindSyncWord(readptr, bytes_left);
+            if (ofs == -1) {
+                printf("MP3 sync word not found\n");
+                readptr += bytes_left;
+                bytes_left = 0;
+                continue; // try again
+            }
+            readptr += ofs;
+            bytes_left -= ofs;
+            printf("MP3 sync word found after %zu bytes\n", ofs);
+            synced = true;
+        }
+
+        // Get an output buffer
+        uint32_t *const buf = i2s_next_buf();
+        if (!buf) {
+            // No output needed, wait and try again
+            __wfe();
+            continue;
+        }
+
+        // Decode one frame
+        unsigned char *const old_readptr = readptr;
+        const int old_bytes_left = bytes_left;
+        const int status = MP3Decode(mp3dec, &readptr, &bytes_left, (short *)buf, 0);
+        if (status) {
+            if (status == ERR_MP3_INDATA_UNDERFLOW) {
+                readptr = old_readptr;
+                bytes_left = old_bytes_left;
+                printf("INDATA_UNDERFLOW\n");
+                sd_readblock_complete(sd_context);
+                continue;
+            } else /*if (status== ERR_MP3_MAINDATA_UNDERFLOW)*/ {
+                --bytes_left;
+                ++readptr;
+                synced = false;
+                continue;
+            }
+            printf("MP3Decode failed: %d\n", status);
+            break;
+        }
+
+        MP3FrameInfo info;
+        MP3GetLastFrameInfo(mp3dec, &info);
+        if (first) {
+            printf("bitrate %d, nChans %d, samprate %d, bitsPerSample %d, outputSamps %d, layer %d, version %d\n",
+                   info.bitrate, info.nChans, info.samprate, info.bitsPerSample, info.outputSamps, info.layer,
+                   info.version);
+            first = false;
+        }
+        if (info.outputSamps != 2304) {
+            printf("Unexpected number of output samples: %d\n", info.outputSamps);
+            return 1;
+        }
+        volume_adjust((int16_t *)buf, info.outputSamps, MAX_VOLUME >> 4);
+
+        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) {
+            if (!sd_readblock(sd_context, i, data_buffer + SD_SECTOR_SIZE * i)) {
+                printf("sd_readblock(%d) failed\n", i);
+                return;
+            }
+        }
+
+        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;
+        }
+
+        if(!sd_writeblock(sd_context, 0, data_buffer)) {
+            printf("sd_writeblock failed\n");
+            return;
+        }
+        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");
+}