initial irq overhaul

docs/01_memory_map.md

@@ -7,7 +7,10 @@
     - [`0x1FFF_0004`: **DATA0** and `0x1FFF_0008`: **DATA1**](#0x1fff_0004-data0-and-0x1fff_0008-data1)
     - [`0x1FFF_000C`: **IDENTIFIER**](#0x1fff_000c-identifier)
     - [`0x1FFF_0010`: **KEY**](#0x1fff_0010-key)
+    - [`0x1FFF_0014`: **IRQ**](#0x1fff_0014-irq)
 - [Command execution flow](#command-execution-flow)
+  - [Without interrupt](#without-interrupt)
+  - [With interrupt](#with-interrupt)
 
 ---
 
@@ -51,7 +54,7 @@ This mapping is used when accessing flashcart from N64 side.
 | EEPROM              | `0x1FFE_2000` | 2 kiB     | RW     | `0x0500_2000` | Block RAM             | mem bus     | SC64 register access is enabled   |
 | 64DD buffer [8]     | `0x1FFE_2800` | 256 bytes | RW     | `0x0500_2800` | Block RAM             | mem bus     | SC64 register access is enabled   |
 | FlashRAM buffer [8] | `0x1FFE_2900` | 128 bytes | R      | `0x0500_2900` | Block RAM             | mem bus     | SC64 register access is enabled   |
-| SC64 registers      | `0x1FFF_0000` | 20 bytes  | RW     | N/A           | Flashcart Interface   | reg bus     | SC64 register access is enabled   |
+| SC64 registers      | `0x1FFF_0000` | 24 bytes  | RW     | N/A           | Flashcart Interface   | reg bus     | SC64 register access is enabled   |
 
  - Note [1]: 64DD IPL share SDRAM memory space with ROM (last 4 MiB minus 128 kiB for saves). Write access is always disabled for this section.
  - Note [2]: SRAM and FlashRAM save types share SDRAM memory space with ROM (last 128 kiB).
@@ -89,23 +92,26 @@ Protocol is command based with support for up to 256 diferrent commands and two
 Support for interrupts is provided but currently no command relies on it, 64DD IRQ is handled separately.
 
 | name               | address       | size    | access | usage                            |
-| ------------------ | ------------- | ------- | ------ | ---------------------------------- |
+| ------------------ | ------------- | ------- | ------ | -------------------------------- |
 | **STATUS/COMMAND** | `0x1FFF_0000` | 4 bytes | RW     | Command execution and status     |
 | **DATA0**          | `0x1FFF_0004` | 4 bytes | RW     | Command argument/result 0        |
 | **DATA1**          | `0x1FFF_0008` | 4 bytes | RW     | Command argument/result 1        |
-| **IDENTIFIER**     | `0x1FFF_000C` | 4 bytes | RW     | Flashcart identifier and IRQ clear |
+| **IDENTIFIER**     | `0x1FFF_000C` | 4 bytes | RW     | Flashcart identifier             |
 | **KEY**            | `0x1FFF_0010` | 4 bytes | W      | SC64 register access lock/unlock |
+| **IRQ**            | `0x1FFF_0014` | 4 bytes | W      | Pending IRQ clear                |
 
 ---
 
 #### `0x1FFF_0000`: **STATUS/COMMAND** 
 
 | name              | bits   | access | meaning                                                     |
-| ------------- | ------ | ------ | ----------------------------------------------------- |
+| ----------------- | ------ | ------ | ----------------------------------------------------------- |
 | `CMD_BUSY`        | [31]   | R      | `1` if dispatched command is pending/executing              |
 | `CMD_ERROR`       | [30]   | R      | `1` if last executed command returned with error code       |
-| `IRQ_PENDING` | [29]   | R      | `1` if flashcart has raised an interrupt              |
+| `MCU_IRQ_PENDING` | [29]   | R      | `1` if flashcart has raised a MCU interrupt                 |
-| N/A           | [28:8] | N/A    | Unused, write `0` for future compatibility            |
+| `CMD_IRQ_PENDING` | [28]   | R      | `1` if flashcart has raised a command finish interrupt      |
+| N/A               | [27:9] | N/A    | Unused, write `0` for future compatibility                  |
+| `CMD_IRQ_REQUEST` | [8]    | RW     | Raise cart interrupt signal when command finishes execution |
 | `CMD_ID`          | [7:0]  | RW     | Command ID to be executed                                   |
 
 Note: Write to this register raises `CMD_BUSY` bit and clears `CMD_ERROR` bit. Flashcart then will start executing provided command.
@@ -125,10 +131,9 @@ Note: Result is valid only when command has executed and `CMD_BUSY` bit is reset
 #### `0x1FFF_000C`: **IDENTIFIER**
 
 | name         | bits   | access | meaning                             |
-| ------------ | ------ | ------ | ------------------------------------------------- |
+| ------------ | ------ | ------ | ----------------------------------- |
-| `IDENTIFIER` | [31:0] | RW     | Flashcart identifier (ASCII `SCv2`) and IRQ clear |
+| `IDENTIFIER` | [31:0] | R      | Flashcart identifier (ASCII `SCv2`) |
 
-Note: Writing any value to this register will clear pending flashcart interrupt.
 
 ---
 
@@ -147,8 +152,20 @@ Value `0xFFFFFFFF` will lock all SC64 registers if flashcart is in unlock state.
 
 ---
 
+#### `0x1FFF_0014`: **IRQ**
+
+| name        | bits   | access | meaning                                       |
+| ----------- | ------ | ------ | --------------------------------------------- |
+| `MCU_CLEAR` | [31]   | W      | Write `1` to clear a MCU interrupt            |
+| `CMD_CLEAR` | [30]   | W      | Write `1` to clear a command finish interrupt |
+| N/A         | [29:0] | N/A    | Unused, write `0` for future compatibility    |
+
+---
+
 ## Command execution flow
 
+### Without interrupt
+
 1. Check if command is already executing by reading `CMD_BUSY` bit in **STATUS/COMMAND** register (optional).
 2. Write command argument values to **DATA0** and **DATA1** registers, can be skipped if command doesn't require it.
 3. Write command ID to **STATUS/COMMAND** register.
@@ -156,3 +173,15 @@ Value `0xFFFFFFFF` will lock all SC64 registers if flashcart is in unlock state.
 5. Check if `CMD_ERROR` bit in **STATUS/COMMAND** is set:
    - If error is set then read **DATA0** register containing error code.
    - If error is not set then read **DATA0** and **DATA1** registers containing command result values, can be skipped if command doesn't return any values.
+
+### With interrupt
+
+1. Check if command is already executing by reading `CMD_BUSY` bit in **STATUS/COMMAND** register (optional).
+2. Write command argument values to **DATA0** and **DATA1** registers, can be skipped if command doesn't require it.
+3. Write command ID to **STATUS/COMMAND** register and set `CMD_IRQ_REQUEST` bit high.
+4. Wait for cart interrupt.
+5. Check (in cart interrupt handler) if `CMD_IRQ_PENDING` bit in **STATUS/COMMAND** register is set high.
+6. Clear interrupt by setting `CMD_CLEAR` bit high in the **IRQ** register.
+7. Check if `CMD_ERROR` bit in **STATUS/COMMAND** is set:
+   - If error is set then read **DATA0** register containing error code.
+   - If error is not set then read **DATA0** and **DATA1** registers containing command result values, can be skipped if command doesn't return any values.

fw/rtl/n64/n64_cfg.sv

@@ -19,10 +19,19 @@ module n64_cfg (
         REG_IDENTIFIER_H,
         REG_IDENTIFIER_L,
         REG_KEY_H,
-        REG_KEY_L
+        REG_KEY_L,
+        REG_IRQ_H,
+        REG_IRQ_L
     } e_reg;
 
-    logic cfg_error;
+    logic cmd_error;
+    logic cmd_irq_request;
+    logic cmd_irq;
+    logic mcu_irq;
+
+    always_ff @(posedge clk) begin
+        irq <= (cmd_irq || mcu_irq);
+    end
 
     always_comb begin
         reg_bus.rdata = 16'd0;
@@ -30,11 +39,12 @@ module n64_cfg (
             case (reg_bus.address[4:1])
                 REG_STATUS: reg_bus.rdata = {
                     n64_scb.cfg_pending,
-                    cfg_error,
+                    cmd_error,
-                    irq,
+                    mcu_irq,
-                    13'd0
+                    cmd_irq,
+                    12'd0
                 };
-                REG_COMMAND: reg_bus.rdata = {8'd0, n64_scb.cfg_cmd};
+                REG_COMMAND: reg_bus.rdata = {7'd0, cmd_irq_request, n64_scb.cfg_cmd};
                 REG_DATA_0_H: reg_bus.rdata = n64_scb.cfg_wdata[0][31:16];
                 REG_DATA_0_L: reg_bus.rdata = n64_scb.cfg_wdata[0][15:0];
                 REG_DATA_1_H: reg_bus.rdata = n64_scb.cfg_wdata[1][31:16];
@@ -43,6 +53,8 @@ module n64_cfg (
                 REG_IDENTIFIER_L: reg_bus.rdata = n64_scb.cfg_identifier[15:0];
                 REG_KEY_H: reg_bus.rdata = 16'd0;
                 REG_KEY_L: reg_bus.rdata = 16'd0;
+                REG_IRQ_H: reg_bus.rdata = 16'd0;
+                REG_IRQ_L: reg_bus.rdata = 16'd0;
             endcase
         end
     end
@@ -51,39 +63,71 @@ module n64_cfg (
     logic lock_sequence_counter;
 
     always_ff @(posedge clk) begin
-        if (n64_scb.cfg_done) begin
+        if (n64_scb.cfg_pending && n64_scb.cfg_done) begin
             n64_scb.cfg_pending <= 1'b0;
-            cfg_error <= n64_scb.cfg_error;
+            cmd_irq <= cmd_irq_request;
+            cmd_error <= n64_scb.cfg_error;
         end
 
         if (n64_scb.cfg_irq) begin
-            irq <= 1'b1;
+            mcu_irq <= 1'b1;
         end
 
         if (unlock_flag) begin
             n64_scb.cfg_unlock <= 1'b1;
         end
 
-        if (reset || n64_scb.n64_reset || n64_scb.n64_nmi) begin
+        if (reset) begin
             n64_scb.cfg_unlock <= 1'b0;
             n64_scb.cfg_pending <= 1'b0;
             n64_scb.cfg_cmd <= 8'h00;
-            irq <= 1'b0;
+            cmd_error <= 1'b0;
-            cfg_error <= 1'b0;
+            cmd_irq_request <= 1'b0;
+            cmd_irq <= 1'b0;
+            mcu_irq <= 1'b0;
+            lock_sequence_counter <= 1'd0;
+        end else if (n64_scb.n64_reset || n64_scb.n64_nmi) begin
+            n64_scb.cfg_unlock <= 1'b0;
+            cmd_irq_request <= 1'b0;
+            cmd_irq <= 1'b0;
+            mcu_irq <= 1'b0;
             lock_sequence_counter <= 1'd0;
         end else if (n64_scb.cfg_unlock) begin
             if (reg_bus.write && reg_bus.address[16] && (reg_bus.address[15:5] == 11'd0)) begin
                 case (reg_bus.address[4:1])
                     REG_COMMAND: begin
+                        if (!n64_scb.cfg_pending) begin
                             n64_scb.cfg_pending <= 1'b1;
+                            cmd_error <= 1'b0;
+                            cmd_irq_request <= reg_bus.wdata[8];
                             n64_scb.cfg_cmd <= reg_bus.wdata[7:0];
-                        cfg_error <= 1'b0;
                         end
-                    REG_DATA_0_H: n64_scb.cfg_rdata[0][31:16] <= reg_bus.wdata;
+                    end
-                    REG_DATA_0_L: n64_scb.cfg_rdata[0][15:0] <= reg_bus.wdata;
+
-                    REG_DATA_1_H: n64_scb.cfg_rdata[1][31:16] <= reg_bus.wdata;
+                    REG_DATA_0_H: begin
-                    REG_DATA_1_L: n64_scb.cfg_rdata[1][15:0] <= reg_bus.wdata;
+                        if (!n64_scb.cfg_pending) begin
-                    REG_IDENTIFIER_H: irq <= 1'b0;
+                            n64_scb.cfg_rdata[0][31:16] <= reg_bus.wdata;
+                        end
+                    end
+
+                    REG_DATA_0_L: begin
+                        if (!n64_scb.cfg_pending) begin
+                            n64_scb.cfg_rdata[0][15:0] <= reg_bus.wdata;
+                        end
+                    end
+
+                    REG_DATA_1_H: begin
+                        if (!n64_scb.cfg_pending) begin
+                            n64_scb.cfg_rdata[1][31:16] <= reg_bus.wdata;
+                        end
+                    end
+
+                    REG_DATA_1_L: begin
+                        if (!n64_scb.cfg_pending) begin
+                            n64_scb.cfg_rdata[1][15:0] <= reg_bus.wdata;
+                        end
+                    end
+
                     REG_KEY_H, REG_KEY_L: begin
                         lock_sequence_counter <= lock_sequence_counter + 1'd1;
                         if (reg_bus.wdata != 16'hFFFF) begin
@@ -93,6 +137,11 @@ module n64_cfg (
                             n64_scb.cfg_unlock <= (reg_bus.wdata != 16'hFFFF);
                         end
                     end
+
+                    REG_IRQ_H: begin
+                        mcu_irq <= (reg_bus.wdata[15] ? 1'b0 : mcu_irq);
+                        cmd_irq <= (reg_bus.wdata[14] ? 1'b0 : cmd_irq);
+                    end
                 endcase
             end
         end

sw/controller/src/cfg.c

@@ -18,6 +18,32 @@
 #define DATA_BUFFER_SIZE        (8192)
 
 
+typedef enum {
+    CMD_ID_IDENTIFIER_GET = 'v',
+    CMD_ID_VERSION_GET = 'V',
+    CMD_ID_CONFIG_GET = 'c',
+    CMD_ID_CONFIG_SET = 'C',
+    CMD_ID_SETTING_GET = 'a',
+    CMD_ID_SETTING_SET = 'A',
+    CMD_ID_TIME_GET = 't',
+    CMD_ID_TIME_SET = 'T',
+    CMD_ID_USB_READ = 'm',
+    CMD_ID_USB_WRITE = 'M',
+    CMD_ID_USB_READ_STATUS = 'u',
+    CMD_ID_USB_WRITE_STATUS = 'U',
+    CMD_ID_SD_CARD_OP = 'i',
+    CMD_ID_SD_SECTOR_SET = 'I',
+    CMD_ID_SD_READ = 's',
+    CMD_ID_SD_WRITE = 'S',
+    CMD_ID_DISK_MAPPING_SET = 'D',
+    CMD_ID_WRITEBACK_PENDING = 'w',
+    CMD_ID_WRITEBACK_SD_INFO = 'W',
+    CMD_ID_FLASH_PROGRAM = 'K',
+    CMD_ID_FLASH_WAIT_BUSY = 'p',
+    CMD_ID_FLASH_ERASE_BLOCK = 'P',
+    CMD_ID_DIAGNOSTIC_GET = '%',
+} cmd_id_t;
+
 typedef enum {
     CFG_ID_BOOTLOADER_SWITCH = 0,
     CFG_ID_ROM_WRITE_ENABLE = 1,
@@ -97,6 +123,9 @@ typedef enum {
 
 
 struct process {
+    bool cmd_queued;
+    cmd_id_t cmd;
+    uint32_t data[2];
     boot_mode_t boot_mode;
     save_type_t save_type;
     cic_seed_t cic_seed;
@@ -109,8 +138,43 @@ struct process {
 static struct process p;
 
 
-static void cfg_set_usb_output_ready (void) {
+static bool cfg_cmd_check (void) {
-    p.usb_output_ready = true;
+    if (!p.cmd_queued) {
+        uint32_t reg = fpga_reg_get(REG_CFG_CMD);
+
+        if (!(reg & CFG_CMD_PENDING)) {
+            return true;
+        }
+
+        p.cmd_queued = true;
+        p.cmd = (cmd_id_t) ((reg & CFG_CMD_MASK) >> CFG_CMD_BIT);
+        p.data[0] = fpga_reg_get(REG_CFG_DATA_0);
+        p.data[1] = fpga_reg_get(REG_CFG_DATA_1);
+    }
+
+    return false;
+}
+
+static void cfg_cmd_reply_success (void) {
+    p.cmd_queued = false;
+    fpga_reg_set(REG_CFG_DATA_0, p.data[0]);
+    fpga_reg_set(REG_CFG_DATA_1, p.data[1]);
+    fpga_reg_set(REG_CFG_CMD, CFG_CMD_DONE);
+}
+
+static void cfg_cmd_reply_error (cfg_error_t error) {
+    p.cmd_queued = false;
+    fpga_reg_set(REG_CFG_DATA_0, error);
+    fpga_reg_set(REG_CFG_DATA_1, 0);
+    fpga_reg_set(REG_CFG_CMD, CFG_CMD_ERROR | CFG_CMD_DONE);
+}
+
+static void cfg_change_scr_bits (uint32_t mask, bool value) {
+    if (value) {
+        fpga_reg_set(REG_CFG_SCR, fpga_reg_get(REG_CFG_SCR) | mask);
+    } else {
+        fpga_reg_set(REG_CFG_SCR, fpga_reg_get(REG_CFG_SCR) & (~mask));
+    }
 }
 
 static bool cfg_translate_address (uint32_t *address, uint32_t length, translate_type_t type) {
@@ -169,20 +233,6 @@ static bool cfg_translate_address (uint32_t *address, uint32_t length, translate
     return true;
 }
 
-static void cfg_set_error (cfg_error_t error) {
-    fpga_reg_set(REG_CFG_DATA_0, error);
-    fpga_reg_set(REG_CFG_DATA_1, 0);
-    fpga_reg_set(REG_CFG_CMD, CFG_CMD_ERROR | CFG_CMD_DONE);
-}
-
-static void cfg_change_scr_bits (uint32_t mask, bool value) {
-    if (value) {
-        fpga_reg_set(REG_CFG_SCR, fpga_reg_get(REG_CFG_SCR) | mask);
-    } else {
-        fpga_reg_set(REG_CFG_SCR, fpga_reg_get(REG_CFG_SCR) & (~mask));
-    }
-}
-
 static bool cfg_set_save_type (save_type_t save_type) {
     if (save_type >= __SAVE_TYPE_COUNT) {
         return true;
@@ -247,6 +297,10 @@ static bool cfg_read_diagnostic_data (uint32_t *args) {
     return false;
 }
 
+static void cfg_set_usb_output_ready (void) {
+    p.usb_output_ready = true;
+}
+
 
 uint32_t cfg_get_identifier (void) {
     return fpga_reg_get(REG_CFG_IDENTIFIER);
@@ -476,91 +530,80 @@ void cfg_reset_state (void) {
 void cfg_init (void) {
     fpga_reg_set(REG_CFG_SCR, CFG_SCR_BOOTLOADER_ENABLED);
     cfg_reset_state();
+    p.cmd_queued = false;
     p.usb_output_ready = true;
 }
 
 
 void cfg_process (void) {
-    uint32_t reg;
+    if (cfg_cmd_check()) {
-    uint32_t args[2];
+        return;
-    uint32_t prev_cfg[2];
+    }
+
+    switch (p.cmd) {
+        case CMD_ID_IDENTIFIER_GET:
+            p.data[0] = cfg_get_identifier();
+            break;
+
+        case CMD_ID_VERSION_GET:
+            version_firmware(&p.data[0], &p.data[1]);
+            break;
+
+        case CMD_ID_CONFIG_GET:
+            if (cfg_query(p.data)) {
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_CONFIG_ID);
+            }
+            break;
+
+        case CMD_ID_CONFIG_SET: {
+            uint32_t prev[2] = { p.data[0], 0 };
+            cfg_query(prev);
+            if (cfg_update(p.data)) {
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_CONFIG_ID);
+            }
+            p.data[1] = prev[1];
+            break;
+        }
+
+        case CMD_ID_SETTING_GET:
+            if (cfg_query_setting(p.data)) {
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_CONFIG_ID);
+            }
+            break;
+
+        case CMD_ID_SETTING_SET:
+            if (cfg_update_setting(p.data)) {
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_CONFIG_ID);
+            }
+            break;
+
+        case CMD_ID_TIME_GET:
+            cfg_get_time(p.data);
+            break;
+
+        case CMD_ID_TIME_SET:
+            cfg_set_time(p.data);
+            break;
+
+        case CMD_ID_USB_READ:
+            if (cfg_translate_address(&p.data[0], p.data[1], (SDRAM | BRAM))) {
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
+            }
+            if (!usb_prepare_read(p.data)) {
+                return;
+            }
+            break;
+
+        case CMD_ID_USB_WRITE: {
+            if (cfg_translate_address(&p.data[0], p.data[1] & 0xFFFFFF, (SDRAM | BRAM))) {
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
+            }
             usb_tx_info_t packet_info;
-
-    reg = fpga_reg_get(REG_CFG_CMD);
-
-    if (reg & CFG_CMD_PENDING) {
-        args[0] = fpga_reg_get(REG_CFG_DATA_0);
-        args[1] = fpga_reg_get(REG_CFG_DATA_1);
-        char cmd = (char) ((reg & CFG_CMD_MASK) >> CFG_CMD_BIT);
-
-        switch (cmd) {
-            case 'v':
-                args[0] = cfg_get_identifier();
-                break;
-
-            case 'V':
-                version_firmware(&args[0], &args[1]);
-                break;
-
-            case 'c':
-                if (cfg_query(args)) {
-                    cfg_set_error(CFG_ERROR_BAD_CONFIG_ID);
-                    return;
-                }
-                break;
-
-            case 'C':
-                prev_cfg[0] = args[0];
-                cfg_query(prev_cfg);
-                if (cfg_update(args)) {
-                    cfg_set_error(CFG_ERROR_BAD_CONFIG_ID);
-                    return;
-                }
-                args[1] = prev_cfg[1];
-                break;
-
-            case 'a':
-                if (cfg_query_setting(args)) {
-                    cfg_set_error(CFG_ERROR_BAD_CONFIG_ID);
-                    return;
-                }
-                break;
-
-            case 'A':
-                if (cfg_update_setting(args)) {
-                    cfg_set_error(CFG_ERROR_BAD_CONFIG_ID);
-                    return;
-                }
-                break;
-
-            case 't':
-                cfg_get_time(args);
-                break;
-
-            case 'T':
-                cfg_set_time(args);
-                break;
-
-            case 'm':
-                if (cfg_translate_address(&args[0], args[1], (SDRAM | BRAM))) {
-                    cfg_set_error(CFG_ERROR_BAD_ADDRESS);
-                    return;
-                }
-                if (!usb_prepare_read(args)) {
-                    return;
-                }
-                break;
-
-            case 'M':
-                if (cfg_translate_address(&args[0], args[1] & 0xFFFFFF, (SDRAM | BRAM))) {
-                    cfg_set_error(CFG_ERROR_BAD_ADDRESS);
-                    return;
-                }
             usb_create_packet(&packet_info, PACKET_CMD_DEBUG_OUTPUT);
             packet_info.data_length = 4;
-                packet_info.data[0] = args[1];
+            packet_info.data[0] = p.data[1];
-                packet_info.dma_length = (args[1] & 0xFFFFFF);
+            packet_info.dma_length = (p.data[1] & 0xFFFFFF);
-                packet_info.dma_address = args[0];
+            packet_info.dma_address = p.data[0];
             packet_info.done_callback = cfg_set_usb_output_ready;
             if (usb_enqueue_packet(&packet_info)) {
                 p.usb_output_ready = false;
@@ -568,173 +611,156 @@ void cfg_process (void) {
                 return;
             }
             break;
+        }
 
-            case 'u':
+        case CMD_ID_USB_READ_STATUS:
-                usb_get_read_info(args);
+            usb_get_read_info(p.data);
             break;
 
-            case 'U':
+        case CMD_ID_USB_WRITE_STATUS:
-                args[0] = p.usb_output_ready ? 0 : (1 << 31);
+            p.data[0] = p.usb_output_ready ? 0 : (1 << 31);
             break;
 
-            case 'i':
+        case CMD_ID_SD_CARD_OP:
-                switch (args[1]) {
+            switch (p.data[1]) {
                 case SD_CARD_OP_DEINIT:
                     sd_card_deinit();
                     break;
+
                 case SD_CARD_OP_INIT: {
                     led_activity_on();
                     bool error = sd_card_init();
                     led_activity_off();
                     if (error) {
-                            cfg_set_error(CFG_ERROR_SD_CARD);
+                        return cfg_cmd_reply_error(CFG_ERROR_SD_CARD);
-                            return;
                     }
                     break;
                 }
+
                 case SD_CARD_OP_GET_STATUS:
-                        args[1] = sd_card_get_status();
+                    p.data[1] = sd_card_get_status();
                     break;
+
                 case SD_CARD_OP_GET_INFO:
-                        if (cfg_translate_address(&args[0], SD_CARD_INFO_SIZE, (SDRAM | BRAM))) {
+                    if (cfg_translate_address(&p.data[0], SD_CARD_INFO_SIZE, (SDRAM | BRAM))) {
-                            cfg_set_error(CFG_ERROR_BAD_ADDRESS);
+                        return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
-                            return;
                     }
-                        if (sd_card_get_info(args[0])) {
+                    if (sd_card_get_info(p.data[0])) {
-                            cfg_set_error(CFG_ERROR_SD_CARD);
+                        return cfg_cmd_reply_error(CFG_ERROR_SD_CARD);
-                            return;
                     }
                     break;
+
                 case SD_CARD_OP_BYTE_SWAP_ON:
                     if (sd_set_byte_swap(true)) {
-                            cfg_set_error(CFG_ERROR_SD_CARD);
+                        return cfg_cmd_reply_error(CFG_ERROR_SD_CARD);
-                            return;
                     }
                     break;
+
                 case SD_CARD_OP_BYTE_SWAP_OFF:
                     if (sd_set_byte_swap(false)) {
-                            cfg_set_error(CFG_ERROR_SD_CARD);
+                        return cfg_cmd_reply_error(CFG_ERROR_SD_CARD);
-                            return;
                     }
                     break;
+
                 default:
-                        cfg_set_error(CFG_ERROR_BAD_ARGUMENT);
+                    return cfg_cmd_reply_error(CFG_ERROR_BAD_ARGUMENT);
-                        return;
             }
             break;
 
-            case 'I':
+        case CMD_ID_SD_SECTOR_SET:
-                p.sd_card_sector = args[0];
+            p.sd_card_sector = p.data[0];
             break;
 
-            case 's': {
+        case CMD_ID_SD_READ: {
-                if (args[1] >= 0x800000) {
+            if (p.data[1] >= 0x800000) {
-                    cfg_set_error(CFG_ERROR_BAD_ARGUMENT);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ARGUMENT);
-                    return;
             }
-                if (cfg_translate_address(&args[0], args[1] * SD_SECTOR_SIZE, (SDRAM | FLASH | BRAM))) {
+            if (cfg_translate_address(&p.data[0], p.data[1] * SD_SECTOR_SIZE, (SDRAM | FLASH | BRAM))) {
-                    cfg_set_error(CFG_ERROR_BAD_ADDRESS);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
-                    return;
             }
             led_activity_on();
-                bool error = sd_read_sectors(args[0], p.sd_card_sector, args[1]);
+            bool error = sd_read_sectors(p.data[0], p.sd_card_sector, p.data[1]);
             led_activity_off();
             if (error) {
-                    cfg_set_error(CFG_ERROR_SD_CARD);
+                return cfg_cmd_reply_error(CFG_ERROR_SD_CARD);
-                    return;
             }
-                p.sd_card_sector += args[1];
+            p.sd_card_sector += p.data[1];
             break;
         }
 
-            case 'S': {
+        case CMD_ID_SD_WRITE: {
-                if (args[1] >= 0x800000) {
+            if (p.data[1] >= 0x800000) {
-                    cfg_set_error(CFG_ERROR_BAD_ARGUMENT);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ARGUMENT);
-                    return;
             }
-                if (cfg_translate_address(&args[0], args[1] * SD_SECTOR_SIZE, (SDRAM | FLASH | BRAM))) {
+            if (cfg_translate_address(&p.data[0], p.data[1] * SD_SECTOR_SIZE, (SDRAM | FLASH | BRAM))) {
-                    cfg_set_error(CFG_ERROR_BAD_ADDRESS);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
-                    return;
             }
             led_activity_on();
-                bool error = sd_write_sectors(args[0], p.sd_card_sector, args[1]);
+            bool error = sd_write_sectors(p.data[0], p.sd_card_sector, p.data[1]);
             led_activity_off();
             if (error) {
-                    cfg_set_error(CFG_ERROR_SD_CARD);
+                return cfg_cmd_reply_error(CFG_ERROR_SD_CARD);
-                    return;
             }
-                p.sd_card_sector += args[1];
+            p.sd_card_sector += p.data[1];
             break;
         }
 
-            case 'D':
+        case CMD_ID_DISK_MAPPING_SET:
-                if (cfg_translate_address(&args[0], args[1], (SDRAM | BRAM))) {
+            if (cfg_translate_address(&p.data[0], p.data[1], (SDRAM | BRAM))) {
-                    cfg_set_error(CFG_ERROR_BAD_ADDRESS);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
-                    return;
             }
-                dd_set_disk_mapping(args[0], args[1]);
+            dd_set_disk_mapping(p.data[0], p.data[1]);
             break;
 
-            case 'w':
+        case CMD_ID_WRITEBACK_PENDING:
-                args[0] = writeback_pending();
+            p.data[0] = writeback_pending();
             break;
 
-            case 'W':
+        case CMD_ID_WRITEBACK_SD_INFO:
-                if (cfg_translate_address(&args[0], WRITEBACK_SECTOR_TABLE_SIZE, (SDRAM | BRAM))) {
+            if (cfg_translate_address(&p.data[0], WRITEBACK_SECTOR_TABLE_SIZE, (SDRAM | BRAM))) {
-                    cfg_set_error(CFG_ERROR_BAD_ADDRESS);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
-                    return;
             }
-                writeback_load_sector_table(args[0]);
+            writeback_load_sector_table(p.data[0]);
             writeback_enable(WRITEBACK_SD);
             break;
 
-            case 'K':
+        case CMD_ID_FLASH_PROGRAM:
-                if (args[1] >= DATA_BUFFER_SIZE) {
+            if (p.data[1] >= DATA_BUFFER_SIZE) {
-                    cfg_set_error(CFG_ERROR_BAD_ARGUMENT);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ARGUMENT);
-                    return;
             }
-                if (cfg_translate_address(&args[0], args[1], FLASH)) {
+            if (cfg_translate_address(&p.data[0], p.data[1], FLASH)) {
-                    cfg_set_error(CFG_ERROR_BAD_ADDRESS);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
-                    return;
             }
-                if (flash_program(DATA_BUFFER_ADDRESS, args[0], args[1])) {
+            if (flash_program(DATA_BUFFER_ADDRESS, p.data[0], p.data[1])) {
-                    cfg_set_error(CFG_ERROR_BAD_ARGUMENT);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ARGUMENT);
-                    return;
             }
             break;
 
-            case 'p':
+        case CMD_ID_FLASH_WAIT_BUSY:
-                if (args[0]) {
+            if (p.data[0]) {
                 flash_wait_busy();
             }
-                args[0] = FLASH_ERASE_BLOCK_SIZE;
+            p.data[0] = FLASH_ERASE_BLOCK_SIZE;
             break;
 
-            case 'P':
+        case CMD_ID_FLASH_ERASE_BLOCK:
-                if (cfg_translate_address(&args[0], FLASH_ERASE_BLOCK_SIZE, FLASH)) {
+            if (cfg_translate_address(&p.data[0], FLASH_ERASE_BLOCK_SIZE, FLASH)) {
-                    cfg_set_error(CFG_ERROR_BAD_ADDRESS);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ADDRESS);
-                    return;
             }
-                if (flash_erase_block(args[0])) {
+            if (flash_erase_block(p.data[0])) {
-                    cfg_set_error(CFG_ERROR_BAD_ARGUMENT);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_ARGUMENT);
-                    return;
             }
             break;
 
-            case '%':
+        case CMD_ID_DIAGNOSTIC_GET:
-                if (cfg_read_diagnostic_data(args)) {
+            if (cfg_read_diagnostic_data(p.data)) {
-                    cfg_set_error(CFG_ERROR_BAD_CONFIG_ID);
+                return cfg_cmd_reply_error(CFG_ERROR_BAD_CONFIG_ID);
-                    return;
             }
             break;
 
         default:
-                cfg_set_error(CFG_ERROR_UNKNOWN_CMD);
+            return cfg_cmd_reply_error(CFG_ERROR_UNKNOWN_CMD);
-                return;
     }
 
-        fpga_reg_set(REG_CFG_DATA_0, args[0]);
+    cfg_cmd_reply_success();
-        fpga_reg_set(REG_CFG_DATA_1, args[1]);
-        fpga_reg_set(REG_CFG_CMD, CFG_CMD_DONE);
-    }
 }