[SC64][FW][SW] New framework for SC64 IRQ handling (#68)

2024-11-24 14:46:53 +01:00 · 2024-08-16 13:52:01 +02:00 · 2024-08-16 13:52:01 +02:00 · 92fb4a85df
commit 92fb4a85df
parent 1259687902
41 changed files with 1547 additions and 746 deletions
--- a/docs/01_memory_map.md
+++ b/docs/01_memory_map.md
@ -3,15 +3,19 @@
    - [Address decoding limitations](#address-decoding-limitations)
    - [Flash mapped sections](#flash-mapped-sections)
 - [SC64 registers](#sc64-registers)
-    - [`0x1FFF_0000`: **STATUS/COMMAND**](#0x1fff_0000-statuscommand)
-    - [`0x1FFF_0004`: **DATA0** and `0x1FFF_0008`: **DATA1**](#0x1fff_0004-data0-and-0x1fff_0008-data1)
+    - [`0x1FFF_0000`: **SCR**](#0x1fff_0000-scr)
+    - [`0x1FFF_0004`: **DATA0** / `0x1FFF_0008`: **DATA1**](#0x1fff_0004-data0--0x1fff_0008-data1)
    - [`0x1FFF_000C`: **IDENTIFIER**](#0x1fff_000c-identifier)
    - [`0x1FFF_0010`: **KEY**](#0x1fff_0010-key)
+    - [`0x1FFF_0014`: **IRQ**](#0x1fff_0014-irq)
+    - [`0x1FFF_0018`: **AUX**](#0x1fff_0018-aux)
 - [Command execution flow](#command-execution-flow)
+    - [Without interrupt](#without-interrupt)
+    - [With interrupt](#with-interrupt)

---

-## Internal memory map
+
+# Internal memory map

 This mapping is used internally by FPGA/μC and when accessing flashcart from USB side.

@ -27,11 +31,11 @@ This mapping is used internally by FPGA/μC and when accessing flashcart from US

 - Note [1]: Flash memory region `0x04E0_0000` - `0x04FD_FFFF` is write protected as it contains N64 bootloader. This section can be overwritten only via firmware update process.
 - Note [2]: Due to BlockRAM usage optimization this section is read only.
- - Note [3]: Read returns `0`. Maximum accessibe address space is 128 MiB.
+ - Note [3]: Read returns `0`. Maximum accessible address space is 128 MiB.

---

-## PI memory map
+
+# PI memory map

 This mapping is used when accessing flashcart from N64 side.

@ -51,11 +55,11 @@ 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` | 28 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).
- - Note [3]: 32 kiB chunks are accesed at `0x0800_0000`, `0x0804_0000` and `0x0808_0000`.
+ - Note [3]: 32 kiB chunks are accessed at `0x0800_0000`, `0x0804_0000` and `0x0808_0000`.
 - Note [4]: FlashRAM read access is multiplexed between mem and reg bus, writes are always mapped to reg bus.
 - Note [5]: Write access is available when `ROM_WRITE_ENABLE` config is enabled.
 - Note [6]: This address overlaps last 128 kiB of ROM space allowing SRAM and FlashRAM save types to work with games occupying almost all of ROM space (for example Pokemon Stadium 2). Reads are redirected to last 128 kiB of flash.
@ -80,59 +84,69 @@ Special commands are provided for performing flash erase and program.
 During those operations avoid accessing flash mapped sections.
 Data read will be corrupted and erase/program operations slows down.

---

-## SC64 registers
+
+# SC64 registers

 SC64 contains small register region used for communication between N64 and controller code running on the μC.
-Protocol is command based with support for up to 256 diferrent commands and two 32-bit argument/result values per operation.
-Support for interrupts is provided but currently no command relies on it, 64DD IRQ is handled separately.
+Protocol is command based with support for up to 256 different commands and two 32-bit argument/result values per operation.
+Command execution finish can be optionally signaled with "cart interrupt"

 | name           | address       | size    | access | usage                            |
-| ------------------ | ------------- | ------- | ------ | ---------------------------------- |
-| **STATUS/COMMAND** | `0x1FFF_0000` | 4 bytes | RW     | Command execution and status       |
+| -------------- | ------------- | ------- | ------ | -------------------------------- |
+| **SCR**        | `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      | IRQ clear and enable             |
+| **AUX**        | `0x1FFF_0018` | 4 bytes | RW     | Auxiliary interrupt data channel |

 ---

-#### `0x1FFF_0000`: **STATUS/COMMAND** 
+### `0x1FFF_0000`: **SCR**

 | 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              |
-| N/A           | [28:8] | N/A    | Unused, write `0` for future compatibility            |
+| `BTN_IRQ_PENDING` | [29]   | R      | `1` if flashcart has raised a "button pressed" interrupt     |
+| `BTN_IRQ_MASK`    | [28]   | R      | `1` means "button pressed" interrupt is enabled (always `1`) |
+| `CMD_IRQ_PENDING` | [27]   | R      | `1` if flashcart has raised a "command finish" interrupt     |
+| `CMD_IRQ_MASK`    | [26]   | R      | `1` means "command finish" interrupt is enabled (always `1`) |
+| `USB_IRQ_PENDING` | [25]   | R      | `1` if flashcart has raised an "USB not empty" interrupt     |
+| `USB_IRQ_MASK`    | [24]   | R      | `1` means "USB not empty" interrupt is enabled               |
+| `AUX_IRQ_PENDING` | [23]   | R      | `1` if flashcart has raised an "AUX not empty" interrupt     |
+| `AUX_IRQ_MASK`    | [22]   | R      | `1` means "AUX not empty" interrupt is enabled               |
+| N/A               | [21: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.

 ---

-####  `0x1FFF_0004`: **DATA0** and `0x1FFF_0008`: **DATA1**
+###  `0x1FFF_0004`: **DATA0** / `0x1FFF_0008`: **DATA1**

 | name      | bits   | access | meaning                            |
 | --------- | ------ | ------ | ---------------------------------- |
 | `DATA0/1` | [31:0] | RW     | Command argument (W) or result (R) |

-Note: Result is valid only when command has executed and `CMD_BUSY` bit is reset. When `CMD_ERROR` is set then **DATA0** register contains error code.
+Note: Result is valid only when command has finished execution and `CMD_BUSY` bit is reset.
+When `CMD_ERROR` is set then **DATA0** register contains error code.

 ---

-#### `0x1FFF_000C`: **IDENTIFIER**
+### `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.

 ---

-#### `0x1FFF_0010`: **KEY**
+### `0x1FFF_0010`: **KEY**

 | name  | bits   | access | meaning                                    |
 | ----- | ------ | ------ | ------------------------------------------ |
@ -140,19 +154,86 @@ Note: Writing any value to this register will clear pending flashcart interrupt.

 Note: By default from cold boot (power on) or console reset (NMI) flashcart will disable access to SC64 specific memory regions.
 **KEY** register is always enabled and listening for writes regardless of lock/unlock state.
-To enable SC64 registers it is necesarry to provide sequence of values to this register.
+To enable SC64 registers it is necessary to provide sequence of values to this register.
 Value `0x00000000` will reset sequencer state.
-Two consequentive writes of values `0x5F554E4C` and `0x4F434B5F` will unlock all SC64 registers if flashcart is in lock state.
+Two consecutive writes of values `0x5F554E4C` and `0x4F434B5F` will unlock all SC64 registers if flashcart is in lock state.
 Value `0xFFFFFFFF` will lock all SC64 registers if flashcart is in unlock state.

 ---

-## Command execution flow
+### `0x1FFF_0014`: **IRQ**

-1. Check if command is already executing by reading `CMD_BUSY` bit in **STATUS/COMMAND** register (optional).
+| name              | bits    | access | meaning                                        |
+| ----------------- | ------- | ------ | ---------------------------------------------- |
+| `BTN_CLEAR`       | [31]    | W      | Write `1` to clear "button pressed" interrupt  |
+| `CMD_CLEAR`       | [30]    | W      | Write `1` to clear "command finish" interrupt  |
+| `USB_CLEAR`       | [29]    | W      | Write `1` to clear "USB not empty" interrupt   |
+| `AUX_CLEAR`       | [28]    | W      | Write `1` to clear "AUX not empty" interrupt   |
+| N/A               | [27:12] | N/A    | Unused, write `0` for future compatibility     |
+| `USB_IRQ_DISABLE` | [11]    | W      | Write `1` to disable "USB not empty" interrupt |
+| `USB_IRQ_ENABLE`  | [10]    | W      | Write `1` to enable "USB not empty" interrupt  |
+| `AUX_IRQ_DISABLE` | [9]     | W      | Write `1` to disable "AUX not empty" interrupt |
+| `AUX_IRQ_ENABLE`  | [8]     | W      | Write `1` to enable "AUX not empty" interrupt  |
+| N/A               | [7:0]   | N/A    | Unused, write `0` for future compatibility     |
+
+Note: All interrupts are cleared and disabled when any of the following events occur:
+ - Hard reset
+ - NMI reset
+ - SC64 registers lock
+
+SC64 interrupts are completely disabled when register access is not enabled.
+
+---
+
+### `0x1FFF_0018`: **AUX**
+
+This register can be used as a very simple interface to the PC via USB.
+Writing to this register generates an USB transfer with the contents of the written data.
+New data available in the register are signaled via cart interrupt that needs to be enabled beforehand by setting `AUX_IRQ_ENABLE` bit in the **IRQ** register.
+Status can be also manually polled by checking `AUX_IRQ_PENDING` bit in **SCR** register.
+Interrupt needs to be acknowledged by setting `AUX_CLEAR` bit in the **IRQ** register.
+There is no flow control, use this register as a ping-pong interface.
+For example, PC sends AUX data, N64 receives interrupt, reads the data then writes to this register with a response.
+This flow can be reversed if needed - N64 can be the initiating side.
+
+| name   | bits   | access | meaning        |
+| ------ | ------ | ------ | -------------- |
+| `DATA` | [31:0] | RW     | Arbitrary data |
+
+This register is used by the upload process in the `sc64deployer` to notify running app on the N64 about certain events.
+All `DATA` values with upper 8 bits set to `1` (`0xFFxxxxxx`) are reserved for internal use by the SC64.
+Refrain from using these values in your app for uses other than listed below.
+Currently defined reserved `DATA` values are:
+
+ - `0xFF000000` - **Ping** - no-op command to test if app running on the N64 is listening to the AUX events.
+ - `0xFF000001` - **Halt** - causes the running app to stop all activity and wait in preparation for uploading new ROM to the SC64.
+ App still should listen to the AUX interrupt and respond to other messages.
+ - `0xFF000002` - **Reboot** - causes the running app to perform soft reboot by reloading IPL3 from the ROM and start executing it.
+
+App running on the N64 shall respond to the AUX message with the same `DATA` value as incoming event for all events listed above, unless it's specified otherwise.
+
+
+
+# Command execution flow
+
+### Without interrupt
+
+1. Check if command is already executing by reading `CMD_BUSY` bit in **SCR** 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.
-4. Wait for `CMD_BUSY` bit in **STATUS/COMMAND** register to go low.
-5. Check if `CMD_ERROR` bit in **STATUS/COMMAND** is set:
+3. Write command ID to **SCR** register.
+4. Wait for `CMD_BUSY` bit in **SCR** register to go low.
+5. Check if `CMD_ERROR` bit in **SCR** 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 **SCR** 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 **SCR** 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 **SCR** register is set high.
+6. Clear interrupt by setting `CMD_CLEAR` bit high in the **IRQ** register.
+7. Check if `CMD_ERROR` bit in **SCR** 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.
--- a/docs/03_usb_interface.md
+++ b/docs/03_usb_interface.md
@ -43,13 +43,17 @@
    - [`arg0` (type)](#arg0-type)
    - [`arg1` (length)](#arg1-length-2)
    - [`data` (data)](#data-data-1)
+  - [`X`: **AUX\_WRITE**](#x-aux_write)
+    - [`arg0` (data)](#arg0-data)
  - [`D`: **DD\_SET\_BLOCK\_READY**](#d-dd_set_block_ready)
    - [`arg0` (error)](#arg0-error)
  - [`W`: **WRITEBACK\_ENABLE**](#w-writeback_enable)
 - [Asynchronous packets](#asynchronous-packets)
+  - [`X`: **AUX\_DATA**](#x-aux_data)
+    - [`data` (data)](#data-data-2)
  - [`B`: **BUTTON**](#b-button)
  - [`U`: **DATA**](#u-data)
-    - [`data` (data)](#data-data-2)
+    - [`data` (data)](#data-data-3)
  - [`G`: **DATA\_FLUSHED**](#g-data_flushed)
  - [`D`: **DISK\_REQUEST**](#d-disk_request)
    - [`data` (disk\_info/block\_data)](#data-disk_infoblock_data)
@ -149,7 +153,7 @@ Available packet IDs are listed in the [asynchronous packets](#asynchronous-pack
 ## Supported commands

 | id  | name                                            | arg0         | arg1          | data | response         | description                                                    |
-| --- | ----------------------------------------------- | ------------ | ------------- | ---- | ---------------- | ------------------------------------------------------------- |
+| --- | ----------------------------------------------- | ------------ | ------------- | ---- | ---------------- | -------------------------------------------------------------- |
 | `v` | [**IDENTIFIER_GET**](#v-identifier_get)         | ---          | ---           | ---  | identifier       | Get flashcart identifier `SCv2`                                |
 | `V` | [**VERSION_GET**](#v-version_get)               | ---          | ---           | ---  | version          | Get flashcart firmware version                                 |
 | `R` | [**STATE_RESET**](#r-state_reset)               | ---          | ---           | ---  | ---              | Reset flashcart state (CIC params and config options)          |
@ -163,6 +167,7 @@ Available packet IDs are listed in the [asynchronous packets](#asynchronous-pack
 | `m` | [**MEMORY_READ**](#m-memory_read)               | address      | length        | ---  | data             | Read data from specified memory address                        |
 | `M` | [**MEMORY_WRITE**](#m-memory_write)             | address      | length        | data | ---              | Write data to specified memory address                         |
 | `U` | [**USB_WRITE**](#u-usb_write)                   | type         | length        | data | N/A              | Send data to be received by app running on N64 (no response!)  |
+| `X` | [**AUX_WRITE**](#x-aux_write)                   | data         | ---           | ---  | ---              | Send small auxiliary data to be received by app running on N64 |
 | `D` | [**DD_SET_BLOCK_READY**](#d-dd_set_block_ready) | error        | ---           | ---  | ---              | Notify flashcart about 64DD block readiness                    |
 | `W` | [**WRITEBACK_ENABLE**](#w-writeback_enable)     | ---          | ---           | ---  | ---              | Enable save writeback through USB packet                       |
 | `p` | **FLASH_WAIT_BUSY**                             | wait         | ---           | ---  | erase_block_size | Wait until flash ready / Get flash block erase size            |
@ -447,6 +452,21 @@ If N64 acknowledge the request, then data is written to the flashcart memory to

 ---

+### `X`: **AUX_WRITE**
+
+**Send small auxiliary data to be received by app running on N64**
+
+#### `arg0` (data)
+| bits     | description |
+| -------- | ----------- |
+| `[31:0]` | Data        |
+
+_This command does not send response data._
+
+This command puts 32 bits of data to the AUX register accessible from the N64 side, and generates cart interrupt (if enabled).
+
+---
+
 ### `D`: **DD_SET_BLOCK_READY**

 **Notify flashcart about 64DD block readiness**
@ -482,6 +502,7 @@ Save data is sent via [**SAVE_WRITEBACK**](#s-save_writeback) asynchronous packe

 | id  | name                                    | data                 | description                                                           |
 | --- | --------------------------------------- | -------------------- | --------------------------------------------------------------------- |
+| `X` | [**AUX_DATA**](#x-aux_data)             | data                 | Data was written to the `AUX` register from the N64 side              |
 | `B` | [**BUTTON**](#b-button)                 | ---                  | Button on the back of the SC64 was pressed                            |
 | `U` | [**DATA**](#u-data)                     | data                 | Data sent from the N64                                                |
 | `G` | [**DATA_FLUSHED**](#g-data_flushed)     | ---                  | Data from [`U` **USB_WRITE**](#u-usb_write) USB command was discarded |
@ -490,6 +511,20 @@ Save data is sent via [**SAVE_WRITEBACK**](#s-save_writeback) asynchronous packe
 | `S` | [**SAVE_WRITEBACK**](#s-save_writeback) | save_contents        | Flushed save data                                                     |
 | `F` | [**UPDATE_STATUS**](#f-update_status)   | progress             | Firmware update progress                                              |

+
+---
+
+### `X`: **AUX_DATA**
+
+**Data was written to the `AUX` register from the N64 side**
+
+This packet is sent when N64 writes to the `AUX` register in the SC64 register block.
+
+#### `data` (data)
+| offset | type     | description |
+| ------ | -------- | ----------- |
+| `0`    | uint32_t | Data        |
+
 ---

 ### `B`: **BUTTON**
--- a/fw/project/lcmxo2/.gitignore
+++ b/fw/project/lcmxo2/.gitignore
@ -11,6 +11,7 @@
 *.tcl
 *.tpf
 *.trc
+*.txt
 *.xml
 impl*/

--- a/fw/project/lcmxo2/build.sh
+++ b/fw/project/lcmxo2/build.sh
@ -4,6 +4,8 @@ set -e

 source $bindir/diamond_env

+printf "[$(hostname)]\nSYSTEM=Linux\nCORENUM=$(nproc --all)\n" > $(dirname $0)/multicore.txt
+
 diamondc build.tcl

 MINIMUM_FREQ=$(cat impl1/sc64_impl1.twr \
--- a/fw/project/lcmxo2/release.sty
+++ b/fw/project/lcmxo2/release.sty
@ -97,9 +97,9 @@
    <Property name="PROP_MAP_RegRetiming" value="True" time="0"/>
    <Property name="PROP_MAP_SigCrossRef" value="False" time="0"/>
    <Property name="PROP_MAP_SymCrossRef" value="False" time="0"/>
-    <Property name="PROP_MAP_TimingDriven" value="False" time="0"/>
+    <Property name="PROP_MAP_TimingDriven" value="True" time="0"/>
    <Property name="PROP_MAP_TimingDrivenNodeRep" value="False" time="0"/>
-    <Property name="PROP_MAP_TimingDrivenPack" value="False" time="0"/>
+    <Property name="PROP_MAP_TimingDrivenPack" value="True" time="0"/>
    <Property name="PROP_PARSTA_AnalysisOption" value="Standard Setup and Hold Analysis" time="0"/>
    <Property name="PROP_PARSTA_AutoTiming" value="True" time="0"/>
    <Property name="PROP_PARSTA_CheckUnconstrainedConns" value="False" time="0"/>
@ -118,15 +118,15 @@
    <Property name="PROP_PAR_NewRouteParDes" value="NBR" time="0"/>
    <Property name="PROP_PAR_PARClockSkew" value="Off" time="0"/>
    <Property name="PROP_PAR_PARModArgs" value="" time="0"/>
-    <Property name="PROP_PAR_ParMultiNodeList" value="" time="0"/>
+    <Property name="PROP_PAR_ParMultiNodeList" value="multicore.txt" time="0"/>
    <Property name="PROP_PAR_ParRunPlaceOnly" value="False" time="0"/>
-    <Property name="PROP_PAR_PlcIterParDes" value="16" time="0"/>
+    <Property name="PROP_PAR_PlcIterParDes" value="32" time="0"/>
    <Property name="PROP_PAR_PlcStCostTblParDes" value="1" time="0"/>
    <Property name="PROP_PAR_PrefErrorOut" value="True" time="0"/>
    <Property name="PROP_PAR_RemoveDir" value="True" time="0"/>
-    <Property name="PROP_PAR_RouteDlyRedParDes" value="0" time="0"/>
-    <Property name="PROP_PAR_RoutePassParDes" value="20" time="0"/>
-    <Property name="PROP_PAR_RouteResOptParDes" value="0" time="0"/>
+    <Property name="PROP_PAR_RouteDlyRedParDes" value="10" time="0"/>
+    <Property name="PROP_PAR_RoutePassParDes" value="100" time="0"/>
+    <Property name="PROP_PAR_RouteResOptParDes" value="6" time="0"/>
    <Property name="PROP_PAR_RoutingCDP" value="1" time="0"/>
    <Property name="PROP_PAR_RoutingCDR" value="1" time="0"/>
    <Property name="PROP_PAR_RunParWithTrce" value="False" time="0"/>
@ -173,7 +173,7 @@
    <Property name="PROP_SYN_EdfArrangeVHDLFiles" value="True" time="0"/>
    <Property name="PROP_SYN_EdfDefEnumEncode" value="Default" time="0"/>
    <Property name="PROP_SYN_EdfFanout" value="1000" time="0"/>
-    <Property name="PROP_SYN_EdfFrequency" value="110" time="0"/>
+    <Property name="PROP_SYN_EdfFrequency" value="100" time="0"/>
    <Property name="PROP_SYN_EdfGSR" value="False" time="0"/>
    <Property name="PROP_SYN_EdfInsertIO" value="False" time="0"/>
    <Property name="PROP_SYN_EdfNumCritPath" value="" time="0"/>
--- a/fw/rtl/mcu/mcu_top.sv
+++ b/fw/rtl/mcu/mcu_top.sv
@ -360,7 +360,8 @@ module mcu_top (
        REG_DEBUG_0,
        REG_DEBUG_1,
        REG_CIC_0,
-        REG_CIC_1
+        REG_CIC_1,
+        REG_AUX
    } reg_address_e;

    logic bootloader_skip;
@ -370,8 +371,12 @@ module mcu_top (

    logic dd_bm_ack;

+    logic [31:0] debug_buffer;
+
    logic cic_invalid_region;

+    logic aux_pending;
+

    // Register read logic

@ -459,7 +464,9 @@ module mcu_top (

                REG_CFG_CMD: begin
                    reg_rdata <= {
-                        23'd0,
+                        19'd0,
+                        aux_pending,
+                        3'd0,
                        n64_scb.cfg_pending,
                        n64_scb.cfg_cmd
                    };
@ -644,15 +651,18 @@ module mcu_top (
                end

                REG_DEBUG_0: begin
-                    reg_rdata <= n64_scb.pi_debug[31:0];
+                    reg_rdata <= n64_scb.pi_debug_address;
+                    debug_buffer <= {
+                        6'd0,
+                        n64_scb.pi_debug_direction,
+                        n64_scb.pi_debug_rw_count,
+                        n64_scb.cic_debug_step,
+                        n64_scb.pi_debug_fifo_flags
+                    };
                end

                REG_DEBUG_1: begin
-                    reg_rdata <= {
-                        24'd0,
-                        n64_scb.cic_debug,
-                        n64_scb.pi_debug[35:32]
-                    };
+                    reg_rdata <= debug_buffer;
                end

                REG_CIC_0: begin
@ -670,6 +680,10 @@ module mcu_top (
                REG_CIC_1: begin
                    reg_rdata <= n64_scb.cic_checksum[31:0];
                end
+
+                REG_AUX: begin
+                    reg_rdata <= n64_scb.aux_rdata;
+                end
            endcase
        end
    end
@ -700,7 +714,10 @@ module mcu_top (

        n64_scb.cfg_done <= 1'b0;
        n64_scb.cfg_error <= 1'b0;
-        n64_scb.cfg_irq <= 1'b0;
+
+        n64_scb.btn_irq <= 1'b0;
+        n64_scb.usb_irq <= 1'b0;
+        n64_scb.aux_irq <= 1'b0;

        n64_scb.flashram_done <= 1'b0;

@ -731,6 +748,10 @@ module mcu_top (
            cic_invalid_region <= 1'b1;
        end

+        if (n64_scb.aux_pending) begin
+            aux_pending <= 1'b1;
+        end
+
        if (reset) begin
            mcu_int <= 1'b0;
            sd_scb.clock_mode <= 2'd0;
@ -755,6 +776,7 @@ module mcu_top (
            n64_scb.cic_region <= 1'b0;
            n64_scb.cic_seed <= 8'h3F;
            n64_scb.cic_checksum <= 48'hA536C0F1D859;
+            aux_pending <= 1'b0;
        end else if (reg_write) begin
            case (address)
                REG_MEM_ADDRESS: begin
@ -771,6 +793,7 @@ module mcu_top (
                end

                REG_USB_SCR: begin
+                    n64_scb.usb_irq <= reg_wdata[31];
                    usb_scb.write_buffer_flush <= reg_wdata[5];
                    usb_scb.reset_off_ack <= reg_wdata[4];
                    usb_scb.reset_on_ack <= reg_wdata[3];
@ -820,10 +843,13 @@ module mcu_top (

                REG_CFG_CMD: begin
                    {
-                        n64_scb.cfg_irq,
+                        n64_scb.btn_irq,
                        n64_scb.cfg_error,
                        n64_scb.cfg_done
                    } <= reg_wdata[11:9];
+                    if (reg_wdata[13]) begin
+                        aux_pending <= 1'b0;
+                    end
                end

                REG_FLASHRAM_SCR: begin
@ -947,6 +973,11 @@ module mcu_top (
                REG_CIC_1: begin
                    n64_scb.cic_checksum[31:0] <= reg_wdata;
                end
+
+                REG_AUX: begin
+                    n64_scb.aux_irq <= 1'b1;
+                    n64_scb.aux_wdata <= reg_wdata;
+                end
            endcase
        end
    end
--- a/fw/rtl/n64/n64_cfg.sv
+++ b/fw/rtl/n64/n64_cfg.sv
@ -19,10 +19,39 @@ module n64_cfg (
        REG_IDENTIFIER_H,
        REG_IDENTIFIER_L,
        REG_KEY_H,
-        REG_KEY_L
+        REG_KEY_L,
+        REG_IRQ_H,
+        REG_IRQ_L,
+        REG_AUX_H,
+        REG_AUX_L
    } e_reg;

-    logic cfg_error;
+    logic cmd_error;
+    logic cmd_irq_request;
+    logic cmd_irq;
+
+    logic btn_irq;
+    logic usb_irq;
+    logic aux_irq;
+
+    logic btn_irq_mask;
+    logic cmd_irq_mask;
+    logic usb_irq_mask;
+    logic aux_irq_mask;
+
+    always_ff @(posedge clk) begin
+        irq <= (
+            (btn_irq && btn_irq_mask) ||
+            (cmd_irq && cmd_irq_mask) ||
+            (usb_irq && usb_irq_mask) ||
+            (aux_irq && aux_irq_mask)
+        );
+    end
+
+    always_comb begin
+        btn_irq_mask = 1'b1;
+        cmd_irq_mask = 1'b1;
+    end

    always_comb begin
        reg_bus.rdata = 16'd0;
@ -30,11 +59,18 @@ module n64_cfg (
            case (reg_bus.address[4:1])
                REG_STATUS: reg_bus.rdata = {
                    n64_scb.cfg_pending,
-                    cfg_error,
-                    irq,
-                    13'd0
+                    cmd_error,
+                    btn_irq,
+                    btn_irq_mask,
+                    cmd_irq,
+                    cmd_irq_mask,
+                    usb_irq,
+                    usb_irq_mask,
+                    aux_irq,
+                    aux_irq_mask,
+                    6'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 +79,10 @@ 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;
+                REG_AUX_H: reg_bus.rdata = n64_scb.aux_wdata[31:16];
+                REG_AUX_L: reg_bus.rdata = n64_scb.aux_wdata[15:0];
            endcase
        end
    end
@ -51,48 +91,145 @@ module n64_cfg (
    logic lock_sequence_counter;

    always_ff @(posedge clk) begin
-        if (n64_scb.cfg_done) begin
+        n64_scb.aux_pending <= 1'b0;
+
+        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;
+        if (n64_scb.cfg_unlock) begin
+            if (n64_scb.btn_irq) begin
+                btn_irq <= 1'b1;
+            end
+
+            if (n64_scb.usb_irq) begin
+                usb_irq <= 1'b1;
+            end
+
+            if (n64_scb.aux_irq) begin
+                aux_irq <= 1'b1;
+            end
        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;
-            cfg_error <= 1'b0;
+            cmd_error <= 1'b0;
+            cmd_irq_request <= 1'b0;
+            btn_irq <= 1'b0;
+            cmd_irq <= 1'b0;
+            usb_irq <= 1'b0;
+            aux_irq <= 1'b0;
+            usb_irq_mask <= 1'b0;
+            aux_irq_mask <= 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;
+            btn_irq <= 1'b0;
+            cmd_irq <= 1'b0;
+            usb_irq <= 1'b0;
+            aux_irq <= 1'b0;
+            usb_irq_mask <= 1'b0;
+            aux_irq_mask <= 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;
-                    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_1_L: n64_scb.cfg_rdata[1][15:0] <= reg_bus.wdata;
-                    REG_IDENTIFIER_H: irq <= 1'b0;
+                    end
+
+                    REG_DATA_0_H: begin
+                        if (!n64_scb.cfg_pending) begin
+                            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_IDENTIFIER_H: begin
+                        btn_irq <= 1'b0;
+                    end
+
                    REG_KEY_H, REG_KEY_L: begin
                        lock_sequence_counter <= lock_sequence_counter + 1'd1;
                        if (reg_bus.wdata != 16'hFFFF) begin
                            lock_sequence_counter <= 1'd0;
                        end
                        if (lock_sequence_counter == 1'd1) begin
-                            n64_scb.cfg_unlock <= (reg_bus.wdata != 16'hFFFF);
+                            if (reg_bus.wdata == 16'hFFFF) begin
+                                n64_scb.cfg_unlock <= 1'b0;
+                                cmd_irq_request <= 1'b0;
+                                btn_irq <= 1'b0;
+                                cmd_irq <= 1'b0;
+                                usb_irq <= 1'b0;
+                                aux_irq <= 1'b0;
+                                usb_irq_mask <= 1'b0;
+                                aux_irq_mask <= 1'b0;
                            end
                        end
+                    end
+
+                    REG_IRQ_H: begin
+                        btn_irq <= (reg_bus.wdata[15] ? 1'b0 : btn_irq);
+                        cmd_irq <= (reg_bus.wdata[14] ? 1'b0 : cmd_irq);
+                        usb_irq <= (reg_bus.wdata[13] ? 1'b0 : usb_irq);
+                        aux_irq <= (reg_bus.wdata[12] ? 1'b0 : aux_irq);
+                    end
+
+                    REG_IRQ_L: begin
+                        if (reg_bus.wdata[10]) begin
+                            usb_irq_mask <= 1'b1;
+                        end
+                        if (reg_bus.wdata[11]) begin
+                            usb_irq_mask <= 1'b0;
+                        end
+
+                        if (reg_bus.wdata[8]) begin
+                            aux_irq_mask <= 1'b1;
+                        end
+                        if (reg_bus.wdata[9]) begin
+                            aux_irq_mask <= 1'b0;
+                        end
+                    end
+
+                    REG_AUX_H: begin
+                        n64_scb.aux_rdata[31:16] <= reg_bus.wdata;
+                    end
+
+                    REG_AUX_L: begin
+                        n64_scb.aux_pending <= 1'b1;
+                        n64_scb.aux_rdata[15:0] <= reg_bus.wdata;
+                    end
                endcase
            end
        end
--- a/fw/rtl/n64/n64_cic.sv
+++ b/fw/rtl/n64/n64_cic.sv
@ -192,7 +192,7 @@ module n64_cic (
                        end

                        2'b11: begin
-                            n64_scb.cic_debug <= dbus_wdata[3:0];
+                            n64_scb.cic_debug_step <= dbus_wdata[3:0];
                        end
                    endcase
                end
@ -200,7 +200,7 @@ module n64_cic (
        end

        if (reset) begin
-            n64_scb.cic_debug <= 3'd0;
+            n64_scb.cic_debug_step <= 3'd0;
        end

        if (reset || !cic_reset) begin
@ -236,7 +236,7 @@ module n64_cic (
                        cic_dq
                    };

-                    2'b11: dbus_rdata = {28'd0, n64_scb.cic_debug};
+                    2'b11: dbus_rdata = {28'd0, n64_scb.cic_debug_step};
                endcase
            end
        endcase
--- a/fw/rtl/n64/n64_pi.sv
+++ b/fw/rtl/n64/n64_pi.sv
@ -127,12 +127,21 @@ module n64_pi (

    // Debug: last accessed PI address

+    logic [15:0] pi_debug_address_buffer;
+
    always_ff @(posedge clk) begin
        if (aleh_op) begin
-            n64_scb.pi_debug[31:16] <= n64_pi_dq_in;
+            pi_debug_address_buffer <= n64_pi_dq_in;
        end
        if (alel_op) begin
-            n64_scb.pi_debug[15:0] <= n64_pi_dq_in;
+            n64_scb.pi_debug_address <= {pi_debug_address_buffer, n64_pi_dq_in};
+            n64_scb.pi_debug_rw_count <= 17'd0;
+        end
+        if (pi_reset && (pi_mode == PI_MODE_VALID)) begin
+            if ((last_read && !pi_read) || (last_write && !pi_write)) begin
+                n64_scb.pi_debug_rw_count <= n64_scb.pi_debug_rw_count + 1'd1;
+                n64_scb.pi_debug_direction <= !pi_write;
+            end
        end
    end

@ -310,7 +319,7 @@ module n64_pi (
        read_fifo_read <= 1'b0;

        if (!pi_reset) begin
-            n64_scb.pi_debug[33:32] <= 2'b00;
+            n64_scb.pi_debug_fifo_flags[1:0] <= 2'b00;
        end

        if (reset || !pi_reset || alel_op) begin
@ -321,9 +330,9 @@ module n64_pi (
            if (read_op) begin
                if (read_fifo_empty) begin
                    read_fifo_wait <= 1'b1;
-                    n64_scb.pi_debug[32] <= 1'b1;
+                    n64_scb.pi_debug_fifo_flags[0] <= 1'b1;
                    if (read_fifo_wait) begin
-                        n64_scb.pi_debug[33] <= 1'b1;
+                        n64_scb.pi_debug_fifo_flags[1] <= 1'b1;
                    end
                end else begin
                    read_fifo_read <= 1'b1;
@ -377,7 +386,7 @@ module n64_pi (
        write_fifo_write <= 1'b0;

        if (!pi_reset) begin
-            n64_scb.pi_debug[35:34] <= 2'b00;
+            n64_scb.pi_debug_fifo_flags[3:2] <= 2'b00;
        end

        if (reset) begin
@ -388,9 +397,9 @@ module n64_pi (
            if (write_op) begin
                if (write_fifo_full) begin
                    write_fifo_wait <= 1'b1;
-                    n64_scb.pi_debug[34] <= 1'b1;
+                    n64_scb.pi_debug_fifo_flags[2] <= 1'b1;
                    if (write_fifo_wait) begin
-                        n64_scb.pi_debug[35] <= 1'b1;
+                        n64_scb.pi_debug_fifo_flags[3] <= 1'b1;
                    end
                end else begin
                    write_fifo_write <= 1'b1;
@ -476,7 +485,11 @@ module n64_pi (

    // Reg bus controller

+    logic reg_bus_address_increment;
+
    always_ff @(posedge clk) begin
+        reg_bus_address_increment <= read_op || write_op;
+
        if (aleh_op) begin
            reg_bus.address[16] <= n64_pi_dq_in[0];
        end
@ -485,15 +498,13 @@ module n64_pi (
            reg_bus.address[15:0] <= n64_pi_dq_in;
        end

-        if (read_op || write_op) begin
+        if (reg_bus_address_increment) begin
            reg_bus.address <= reg_bus.address + 2'd2;
        end
-    end

-    always_comb begin
-        reg_bus.read = read_op && (read_port == PORT_REG);
-        reg_bus.write = write_op && (write_port == PORT_REG);
-        reg_bus.wdata = n64_pi_dq_in;
+        reg_bus.read <= read_op && (read_port == PORT_REG);
+        reg_bus.write <= write_op && (write_port == PORT_REG);
+        reg_bus.wdata <= n64_pi_dq_in;
    end

 endmodule
--- a/fw/rtl/n64/n64_scb.sv
+++ b/fw/rtl/n64/n64_scb.sv
@ -47,12 +47,19 @@ interface n64_scb ();
    logic cfg_pending;
    logic cfg_done;
    logic cfg_error;
-    logic cfg_irq;
    logic [7:0] cfg_cmd;
    logic [31:0] cfg_rdata [0:1];
    logic [31:0] cfg_wdata [0:1];
    logic [31:0] cfg_identifier;

+    logic btn_irq;
+    logic usb_irq;
+    logic aux_irq;
+
+    logic aux_pending;
+    logic [31:0] aux_rdata;
+    logic [31:0] aux_wdata;
+
    logic [15:0] save_count;

    logic cic_invalid_region;
@ -61,11 +68,14 @@ interface n64_scb ();
    logic cic_region;
    logic [7:0] cic_seed;
    logic [47:0] cic_checksum;
-    logic [3:0] cic_debug;
+    logic [3:0] cic_debug_step;

    logic pi_sdram_active;
    logic pi_flash_active;
-    logic [35:0] pi_debug;
+    logic [31:0] pi_debug_address;
+    logic [16:0] pi_debug_rw_count;
+    logic pi_debug_direction;
+    logic [3:0] pi_debug_fifo_flags;

    modport controller (
        input n64_reset,
@ -98,12 +108,19 @@ interface n64_scb ();
        input cfg_pending,
        output cfg_done,
        output cfg_error,
-        output cfg_irq,
        input cfg_cmd,
        input cfg_rdata,
        output cfg_wdata,
        output cfg_identifier,

+        output btn_irq,
+        output usb_irq,
+        output aux_irq,
+
+        input aux_pending,
+        input aux_rdata,
+        output aux_wdata,
+
        input save_count,

        input cic_invalid_region,
@ -112,9 +129,12 @@ interface n64_scb ();
        output cic_region,
        output cic_seed,
        output cic_checksum,
-        input cic_debug,
+        input cic_debug_step,

-        input pi_debug
+        input pi_debug_address,
+        input pi_debug_rw_count,
+        input pi_debug_direction,
+        input pi_debug_fifo_flags
    );

    modport pi (
@ -139,7 +159,11 @@ interface n64_scb ();

        output pi_sdram_active,
        output pi_flash_active,
-        output pi_debug
+
+        output pi_debug_address,
+        output pi_debug_rw_count,
+        output pi_debug_direction,
+        output pi_debug_fifo_flags
    );

    modport flashram (
@ -206,11 +230,18 @@ interface n64_scb ();
        output cfg_pending,
        input cfg_done,
        input cfg_error,
-        input cfg_irq,
        output cfg_cmd,
        output cfg_rdata,
        input cfg_wdata,
-        input cfg_identifier
+        input cfg_identifier,
+
+        input btn_irq,
+        input usb_irq,
+        input aux_irq,
+
+        output aux_pending,
+        output aux_rdata,
+        input aux_wdata
    );

    modport save_counter (
@ -228,7 +259,7 @@ interface n64_scb ();
        input cic_region,
        input cic_seed,
        input cic_checksum,
-        output cic_debug
+        output cic_debug_step
    );

    modport arbiter (
--- a/fw/rtl/n64/n64_top.sv
+++ b/fw/rtl/n64/n64_top.sv
@ -26,13 +26,19 @@ module n64_top (

    logic n64_dd_irq;
    logic n64_cfg_irq;
-    logic n64_irq_oe;
+
+    logic irq_data;
+    logic irq_dq;
+    logic [1:0] irq_oe;
+
+    assign irq_data = (n64_dd_irq || n64_cfg_irq);

    always @(posedge clk) begin
-        n64_irq_oe <= (n64_dd_irq || n64_cfg_irq);
+        irq_dq <= (~irq_data);
+        irq_oe <= {irq_oe[0], irq_data};
    end

-    assign n64_irq = n64_irq_oe ? 1'b0 : 1'bZ;
+    assign n64_irq = irq_oe[1] ? irq_dq : 1'bZ;

    n64_reg_bus reg_bus ();

--- a/sw/bootloader/Makefile
+++ b/sw/bootloader/Makefile
@ -11,7 +11,7 @@ N64_ELFCOMPRESS = $(N64_BINDIR)/n64elfcompress
 N64_TOOL = $(N64_BINDIR)/n64tool
 PYTHON = python3

-FLAGS = -march=vr4300 -mtune=vr4300 $(USER_FLAGS)
+FLAGS = -march=vr4300 -mtune=vr4300 -mfix4300 $(USER_FLAGS)
 CFLAGS = -Os -Wall -ffunction-sections -fdata-sections -ffreestanding -MMD -MP
 ASFLAGS = -Wa,-I$(N64_INST)/mips64-elf/lib
 LDFLAGS = -lc -nostartfiles -Wl,--gc-sections
@ -31,7 +31,8 @@ SRC_FILES = \
 	exception.S \
 	font.c \
 	init.c \
-	interrupt.c \
+	interrupts.c \
+	interrupts.S \
 	io.c \
 	main.c \
 	menu.c \
--- a/sw/bootloader/src/boot.c
+++ b/sw/bootloader/src/boot.c
@ -146,5 +146,5 @@ void boot (boot_params_t *params) {
        "t3"
    );

-    while (1);
+    while (true);
 }
--- a/sw/bootloader/src/display.c
+++ b/sw/bootloader/src/display.c
@ -1,4 +1,3 @@
-#include <stdbool.h>
 #include <stdio.h>
 #include "display.h"
 #include "font.h"
@ -181,6 +180,10 @@ void display_init (uint32_t *background) {
    }
 }

+bool display_ready (void) {
+    return vi_configured;
+}
+
 void display_vprintf (const char *fmt, va_list args) {
    char line[256];

--- a/sw/bootloader/src/display.h
+++ b/sw/bootloader/src/display.h
@ -3,10 +3,12 @@


 #include <stdarg.h>
+#include <stdbool.h>
 #include <stdint.h>


 void display_init (uint32_t *background);
+bool display_ready (void);
 void display_vprintf (const char *fmt, va_list args);
 void display_printf (const char* fmt, ...);

--- a/sw/bootloader/src/error.c
+++ b/sw/bootloader/src/error.c
@ -1,13 +1,23 @@
 #include <stdarg.h>
-#include "exception.h"
+#include "display.h"
+#include "init.h"
+#include "version.h"
+#include "../assets/assets.h"


 void error_display (const char *fmt, ...) {
    va_list args;

-    va_start(args, fmt);
-    EXCEPTION_TRIGGER(TRIGGER_CODE_ERROR);
-    va_end(args);
+    deinit();

-    while (1);
+    display_init((uint32_t *) (&assets_sc64_logo_640_240_dimmed));
+
+    version_print();
+    display_printf("[ Runtime error ]\n");
+    va_start(args, fmt);
+    display_vprintf(fmt, args);
+    va_end(args);
+    display_printf("\n");
+
+    while (true);
 }
--- a/sw/bootloader/src/exception.S
+++ b/sw/bootloader/src/exception.S
@ -1,8 +1,6 @@
 #include "vr4300.h"


-#define WATCHDOG_TIMEOUT            (5 * (93750000UL / 2))
-
 #define ZR_OFFSET                   (0)
 #define AT_OFFSET                   (8)
 #define V0_OFFSET                   (16)
@ -35,11 +33,13 @@
 #define SP_OFFSET                   (232)
 #define S8_OFFSET                   (240)
 #define RA_OFFSET                   (248)
-#define C0_STATUS_OFFSET            (256)
-#define C0_CAUSE_OFFSET             (260)
-#define C0_EPC_OFFSET               (264)
-#define C0_BADVADDR_OFFSET          (272)
-#define SAVE_REGISTERS_SIZE         (280)
+#define HI_OFFSET                   (256)
+#define LO_OFFSET                   (264)
+#define C0_EPC_OFFSET               (272)
+#define C0_BADVADDR_OFFSET          (280)
+#define C0_STATUS_OFFSET            (288)
+#define C0_CAUSE_OFFSET             (292)
+#define SAVE_REGISTERS_SIZE         (296)


 .section .text.exception_vector
@ -51,14 +51,18 @@ exception_xtlb_miss:
    .org 0x0080
    j exception_handler

+exception_ecc:
+    .org 0x0100
+    j exception_handler
+
 exception_other:
    .org 0x0180
    j exception_handler


 .section .text.exception_handler
+.type exception_handler, %function
 exception_handler:
-    .type exception_handler, %function
    .set noat
    la $k0, (_esp - SAVE_REGISTERS_SIZE)
    sd $zero, ZR_OFFSET($k0)
@ -91,22 +95,20 @@ exception_handler:
    sd $sp, SP_OFFSET($k0)
    sd $s8, S8_OFFSET($k0)
    sd $ra, RA_OFFSET($k0)
+    mfhi $t0
+    mflo $t1
+    sd $t0, HI_OFFSET($k0)
+    sd $t1, LO_OFFSET($k0)
    .set at

    move $sp, $k0

 exception_check_type:
-    mfc0 $a0, C0_CAUSE
-    sw $a0, C0_CAUSE_OFFSET($k0)
-    move $a1, $a0
-    move $t0, $a0
-    andi $t0, C0_CR_IP7
-    andi $a0, C0_CR_EC_MASK
-    srl $a0, $a0, C0_CR_EC_BIT
-    andi $a1, C0_CR_IP_MASK
-    srl $a1, $a1, C0_CR_IP_BIT
-    bne $t0, $zero, exception_fatal
-    beq $a0, $zero, exception_interrupt
+    mfc0 $t0, C0_CAUSE
+    sw $t0, C0_CAUSE_OFFSET($k0)
+    andi $a0, $t0, C0_CR_EC_MASK
+    srl $a0, C0_CR_EC_BIT
+    beqz $a0, exception_interrupt

 exception_fatal:
    sd $k0, K0_OFFSET($k0)
@ -117,19 +119,27 @@ exception_fatal:
    sd $t0, C0_EPC_OFFSET($k0)
    dmfc0 $t0, C0_BADVADDR
    sd $t0, C0_BADVADDR_OFFSET($k0)
-    move $a2, $k0
-    la $t1, exception_fatal_handler
-    jalr $t1
+    move $a1, $k0
+    jal exception_fatal_handler
    ld $t0, C0_EPC_OFFSET($k0)
    dmtc0 $t0, C0_EPC
    j exception_restore

 exception_interrupt:
-    la $t1, exception_interrupt_handler
-    jalr $t1
+    andi $a0, $t0, C0_CR_IP_MASK
+    srl $a0, C0_CR_IP_BIT
+    mfc0 $t0, C0_STATUS
+    andi $t0, C0_SR_IM_MASK
+    srl $t0, C0_SR_IM_BIT
+    and $a0, $t0
+    jal interrupts_handler

 exception_restore:
    .set noat
+    ld $t0, HI_OFFSET($k0)
+    ld $t1, LO_OFFSET($k0)
+    mthi $t0
+    mtlo $t1
    ld $at, AT_OFFSET($k0)
    ld $v0, V0_OFFSET($k0)
    ld $v1, V1_OFFSET($k0)
@ -162,51 +172,3 @@ exception_restore:
    .set at

    eret
-
-
-.section .text.exception_enable_interrupts
-exception_enable_interrupts:
-    .type exception_enable_interrupts, %function
-    .global exception_enable_interrupts
-    mfc0 $t0, C0_STATUS
-    li $t1, C0_SR_IE
-    or $t0, $t0, $t1
-    mtc0 $t0, C0_STATUS
-    jr $ra
-
-
-.section .text.exception_disable_interrupts
-exception_disable_interrupts:
-    .type exception_disable_interrupts, %function
-    .global exception_disable_interrupts
-    mfc0 $t0, C0_STATUS
-    li $t1, ~(C0_SR_IE)
-    and $t0, $t0, $t1
-    mtc0 $t0, C0_STATUS
-    jr $ra
-
-
-.section .text.exception_enable_watchdog
-exception_enable_watchdog:
-    .type exception_enable_watchdog, %function
-    .global exception_enable_watchdog
-    mtc0 $zero, C0_COUNT
-    li $t1, WATCHDOG_TIMEOUT
-    mtc0 $t1, C0_COMPARE
-    mfc0 $t0, C0_STATUS
-    li $t1, C0_SR_IM7
-    or $t0, $t0, $t1
-    mtc0 $t0, C0_STATUS
-    jr $ra
-
-
-.section .text.exception_disable_watchdog
-exception_disable_watchdog:
-    .type exception_disable_watchdog, %function
-    .global exception_disable_watchdog
-    mfc0 $t0, C0_STATUS
-    li $t1, ~(C0_SR_IM7)
-    and $t0, $t0, $t1
-    mtc0 $t0, C0_STATUS
-    mtc0 $zero, C0_COMPARE
-    jr $ra
--- a/sw/bootloader/src/exception.c
+++ b/sw/bootloader/src/exception.c
@ -1,6 +1,5 @@
 #include <stdarg.h>
 #include "display.h"
-#include "exception_regs.h"
 #include "exception.h"
 #include "io.h"
 #include "version.h"
@ -8,15 +7,6 @@
 #include "../assets/assets.h"


-#define EXCEPTION_INTERRUPT     (0)
-#define EXCEPTION_SYSCALL       (8)
-
-#define INTERRUPT_MASK_TIMER    (1 << 7)
-
-#define SYSCALL_CODE_MASK       (0x03FFFFC0UL)
-#define SYSCALL_CODE_BIT        (6)
-
-
 static const char *exception_get_description (uint8_t exception_code) {
    switch (exception_code) {
        case 0:  return "Interrupt";
@ -41,18 +31,13 @@ static const char *exception_get_description (uint8_t exception_code) {
 }


-void exception_fatal_handler (uint32_t exception_code, uint32_t interrupt_mask, exception_t *e) {
-    version_t *version = version_get();
-    uint32_t *instruction_address = (((uint32_t *) (e->epc.u32)) + ((e->cr & C0_CR_BD) ? 1 : 0));
-
+void exception_fatal_handler (uint32_t exception_code, exception_t *e) {
    display_init((uint32_t *) (&assets_sc64_logo_640_240_dimmed));

-    display_printf("[ SC64 bootloader metadata ]\n");
-    display_printf("branch: %s | tag: %s\n", version->git_branch, version->git_tag);
-    display_printf("sha: %s\n", version->git_sha);
-    display_printf("msg: %s\n\n", version->git_message);
+    uint32_t exception_address = e->epc.u32 + (e->cr & C0_CR_BD ? 4 : 0);

-    if (exception_code != EXCEPTION_SYSCALL) {
+    version_print();
+    display_printf("[ Unhandled exception ] @ 0x%08X\n", exception_address);
    display_printf("%s\n", exception_get_description(exception_code));
    display_printf(" pc: 0x%08lX  sr: 0x%08lX  cr: 0x%08lX  va: 0x%08lX\n", e->epc.u32, e->sr, e->cr, e->badvaddr.u32);
    display_printf(" zr: 0x%08lX  at: 0x%08lX  v0: 0x%08lX  v1: 0x%08lX\n", e->zr.u32, e->at.u32, e->v0.u32, e->v1.u32);
@ -62,27 +47,9 @@ void exception_fatal_handler (uint32_t exception_code, uint32_t interrupt_mask,
    display_printf(" s0: 0x%08lX  s1: 0x%08lX  s2: 0x%08lX  s3: 0x%08lX\n", e->s0.u32, e->s1.u32, e->s2.u32, e->s3.u32);
    display_printf(" s4: 0x%08lX  s5: 0x%08lX  s6: 0x%08lX  s7: 0x%08lX\n", e->s4.u32, e->s5.u32, e->s6.u32, e->s7.u32);
    display_printf(" t8: 0x%08lX  t9: 0x%08lX  k0: 0x%08lX  k1: 0x%08lX\n", e->t8.u32, e->t9.u32, e->k0.u32, e->k1.u32);
-        display_printf(" gp: 0x%08lX  sp: 0x%08lX  s8: 0x%08lX  ra: 0x%08lX\n\n", e->gp.u32, e->sp.u32, e->s8.u32, e->ra.u32);
-    } else {
-        display_printf("[ Runtime error ]\n");
-    }
+    display_printf(" gp: 0x%08lX  sp: 0x%08lX  s8: 0x%08lX  ra: 0x%08lX\n", e->gp.u32, e->sp.u32, e->s8.u32, e->ra.u32);
+    display_printf(" hi: 0x%016lX\n", e->hi.u64);
+    display_printf(" lo: 0x%016lX\n", e->lo.u64);

-    if (exception_code == EXCEPTION_INTERRUPT) {
-        if (interrupt_mask & INTERRUPT_MASK_TIMER) {
-            exception_disable_watchdog();
-            display_printf("Still loading after 5 second limit...\n\n");
-            return;
-        }
-    } else if (exception_code == EXCEPTION_SYSCALL) {
-        uint32_t code = (((*instruction_address) & SYSCALL_CODE_MASK) >> SYSCALL_CODE_BIT);
-
-        if (code == TRIGGER_CODE_ERROR) {
-            const char *fmt = (const char *) (e->a0.u32);
-            va_list args = *((va_list *) (e->sp.u32));
-            display_vprintf(fmt, args);
-            display_printf("\n");
-        }
-    }
-
-    while (1);
+    while (true);
 }
--- a/sw/bootloader/src/exception.h
+++ b/sw/bootloader/src/exception.h
@ -2,15 +2,57 @@
 #define EXCEPTION_H__


-#define TRIGGER_CODE_ERROR          (0)
-
-#define EXCEPTION_TRIGGER(code)     { asm volatile ("syscall %[c]\n" :: [c] "i" (code)); }
+#include <stdint.h>


-void exception_enable_interrupts (void);
-void exception_disable_interrupts (void);
-void exception_enable_watchdog (void);
-void exception_disable_watchdog (void);
+typedef union {
+    uint64_t u64;
+    struct {
+        uint32_t u32_h;
+        uint32_t u32;
+    };
+} uint64_32_t;
+
+typedef struct {
+    uint64_32_t zr;
+    uint64_32_t at;
+    uint64_32_t v0;
+    uint64_32_t v1;
+    uint64_32_t a0;
+    uint64_32_t a1;
+    uint64_32_t a2;
+    uint64_32_t a3;
+    uint64_32_t t0;
+    uint64_32_t t1;
+    uint64_32_t t2;
+    uint64_32_t t3;
+    uint64_32_t t4;
+    uint64_32_t t5;
+    uint64_32_t t6;
+    uint64_32_t t7;
+    uint64_32_t s0;
+    uint64_32_t s1;
+    uint64_32_t s2;
+    uint64_32_t s3;
+    uint64_32_t s4;
+    uint64_32_t s5;
+    uint64_32_t s6;
+    uint64_32_t s7;
+    uint64_32_t t8;
+    uint64_32_t t9;
+    uint64_32_t k0;
+    uint64_32_t k1;
+    uint64_32_t gp;
+    uint64_32_t sp;
+    uint64_32_t s8;
+    uint64_32_t ra;
+    uint64_32_t hi;
+    uint64_32_t lo;
+    uint64_32_t epc;
+    uint64_32_t badvaddr;
+    uint32_t sr;
+    uint32_t cr;
+} exception_t;


 #endif
--- a/sw/bootloader/src/exception_regs.h
+++ b/sw/bootloader/src/exception_regs.h
@ -1,56 +0,0 @@
-#ifndef EXCEPTION_REGS_H__
-#define EXCEPTION_REGS_H__
-
-
-#include <stdint.h>
-
-
-typedef union {
-    uint64_t u64;
-    struct {
-        uint32_t u32_h;
-        uint32_t u32;
-    };
-} uint64_32_t;
-
-typedef struct {
-    uint64_32_t zr;
-    uint64_32_t at;
-    uint64_32_t v0;
-    uint64_32_t v1;
-    uint64_32_t a0;
-    uint64_32_t a1;
-    uint64_32_t a2;
-    uint64_32_t a3;
-    uint64_32_t t0;
-    uint64_32_t t1;
-    uint64_32_t t2;
-    uint64_32_t t3;
-    uint64_32_t t4;
-    uint64_32_t t5;
-    uint64_32_t t6;
-    uint64_32_t t7;
-    uint64_32_t s0;
-    uint64_32_t s1;
-    uint64_32_t s2;
-    uint64_32_t s3;
-    uint64_32_t s4;
-    uint64_32_t s5;
-    uint64_32_t s6;
-    uint64_32_t s7;
-    uint64_32_t t8;
-    uint64_32_t t9;
-    uint64_32_t k0;
-    uint64_32_t k1;
-    uint64_32_t gp;
-    uint64_32_t sp;
-    uint64_32_t s8;
-    uint64_32_t ra;
-    uint32_t sr;
-    uint32_t cr;
-    uint64_32_t epc;
-    uint64_32_t badvaddr;
-} exception_t;
-
-
-#endif
--- a/sw/bootloader/src/init.c
+++ b/sw/bootloader/src/init.c
@ -1,6 +1,6 @@
 #include "error.h"
-#include "exception.h"
 #include "init.h"
+#include "interrupts.h"
 #include "io.h"
 #include "sc64.h"
 #include "test.h"
@ -24,22 +24,22 @@ void init (init_tv_type_t tv_type, init_reset_type_t reset_type, uint32_t entrop
        error_display("SC64 hardware not detected");
    }

-    exception_enable_watchdog();
-    exception_enable_interrupts();
+    interrupts_init();
+    interrupts_start_watchdog();

    if ((error = sc64_set_config(CFG_ID_BOOTLOADER_SWITCH, false)) != SC64_OK) {
        error_display("Command CONFIG_SET [BOOTLOADER_SWITCH] failed\n (%08X) - %s", error, sc64_error_description(error));
    }

    if (test_check()) {
-        exception_disable_watchdog();
+        interrupts_stop_watchdog();
        test_execute();
    }
 }

 void deinit (void) {
-    exception_disable_interrupts();
-    exception_disable_watchdog();
+    interrupts_stop_watchdog();
+    interrupts_disable();

    sc64_lock();
 }
--- a/sw/bootloader/src/interrupt.c
+++ b/sw/bootloader/src/interrupt.c
@ -1,6 +0,0 @@
-#include "exception_regs.h"
-
-
-void exception_interrupt_handler (uint32_t exception_code, uint32_t interrupt_mask, exception_t *e) {
-    while (1);
-}
--- a/sw/bootloader/src/interrupts.S
+++ b/sw/bootloader/src/interrupts.S
@ -0,0 +1,58 @@
+#include "vr4300.h"
+
+
+#define WATCHDOG_TIMEOUT    (5 * (93750000UL / 2))
+
+
+.section .text.interrupts
+
+
+.type interrupts_init, %function
+.global interrupts_init
+interrupts_init:
+    li $t1, (C0_SR_IM4 | C0_SR_IM3 | C0_SR_IE)
+    mfc0 $t0, C0_STATUS
+    or $t0, $t1
+    mtc0 $t0, C0_STATUS
+    jr $ra
+
+
+.type interrupts_disable, %function
+.global interrupts_disable
+interrupts_disable:
+    li $t0, ~(C0_SR_IE)
+    mfc0 $v0, C0_STATUS
+    and $t0, $v0
+    mtc0 $t0, C0_STATUS
+    jr $ra
+
+
+.type interrupts_restore, %function
+.global interrupts_restore
+interrupts_restore:
+    mtc0 $a0, C0_STATUS
+    jr $ra
+
+
+.type interrupts_start_watchdog, %function
+.global interrupts_start_watchdog
+interrupts_start_watchdog:
+    mtc0 $zero, C0_COUNT
+    li $t1, WATCHDOG_TIMEOUT
+    mtc0 $t1, C0_COMPARE
+    li $t1, C0_SR_IM7
+    mfc0 $t0, C0_STATUS
+    or $t0, $t1
+    mtc0 $t0, C0_STATUS
+    jr $ra
+
+
+.type interrupts_stop_watchdog, %function
+.global interrupts_stop_watchdog
+interrupts_stop_watchdog:
+    li $t1, ~(C0_SR_IM7)
+    mfc0 $t0, C0_STATUS
+    and $t0, $t1
+    mtc0 $t0, C0_STATUS
+    mtc0 $zero, C0_COMPARE
+    jr $ra
--- a/sw/bootloader/src/interrupts.c
+++ b/sw/bootloader/src/interrupts.c
@ -0,0 +1,87 @@
+#include "display.h"
+#include "sc64.h"
+#include "version.h"
+#include "../assets/assets.h"
+
+
+typedef enum {
+    INTERRUPT_NONE = 0,
+    INTERRUPT_SW_0 = (1 << 0),
+    INTERRUPT_SW_1 = (1 << 1),
+    INTERRUPT_RCP = (1 << 2),
+    INTERRUPT_CART = (1 << 3),
+    INTERRUPT_PRENMI = (1 << 4),
+    INTERRUPT_HW_5 = (1 << 5),
+    INTERRUPT_HW_6 = (1 << 6),
+    INTERRUPT_TIMER = (1 << 7),
+} interrupt_t;
+
+
+void interrupts_handler (uint8_t interrupts) {
+    if (interrupts == INTERRUPT_NONE) {
+        display_init((uint32_t *) (&assets_sc64_logo_640_240_dimmed));
+
+        version_print();
+        display_printf("[ Empty interrupt ]\n");
+        display_printf("There is no interrupt to handle\n");
+
+        while (true);
+    }
+
+    if (interrupts & INTERRUPT_CART) {
+        interrupts &= ~(INTERRUPT_CART);
+
+        sc64_irq_t irq = sc64_irq_pending();
+
+        if (irq != SC64_IRQ_NONE) {
+            sc64_irq_callback(irq);
+        }
+    }
+
+    if (interrupts & INTERRUPT_PRENMI) {
+        interrupts &= ~(INTERRUPT_PRENMI);
+
+        if (display_ready()) {
+            display_init(NULL);
+
+            display_printf("Resetting...\n");
+        }
+
+        while (true);
+    }
+
+    if (interrupts & INTERRUPT_TIMER) {
+        interrupts &= ~(INTERRUPT_TIMER);
+
+        display_init((uint32_t *) (&assets_sc64_logo_640_240_dimmed));
+
+        version_print();
+        display_printf("[ Watchdog timeout ]\n");
+        display_printf("SC64 bootloader did not finish loading in 5 seconds\n");
+
+        while (true);
+    }
+
+    if (interrupts != INTERRUPT_NONE) {
+        display_init((uint32_t *) (&assets_sc64_logo_640_240_dimmed));
+
+        version_print();
+        display_printf("[ Unhandled interrupt(s) ]\n");
+        display_printf("Pending (0x%02X):\n", interrupts);
+        for (int i = 0; i < 8; i++) {
+            switch (interrupts & (1 << i)) {
+                case INTERRUPT_SW_0: display_printf(" (0) Software interrupt\n"); break;
+                case INTERRUPT_SW_1: display_printf(" (1) Software interrupt\n"); break;
+                case INTERRUPT_RCP: display_printf(" (2) RCP interrupt\n"); break;
+                case INTERRUPT_CART: display_printf(" (3) CART interrupt\n"); break;
+                case INTERRUPT_PRENMI: display_printf(" (4) Pre NMI interrupt\n"); break;
+                case INTERRUPT_HW_5: display_printf(" (5) Hardware interrupt\n"); break;
+                case INTERRUPT_HW_6: display_printf(" (6) Hardware interrupt\n"); break;
+                case INTERRUPT_TIMER: display_printf(" (7) Timer interrupt\n"); break;
+                default: break;
+            }
+        }
+
+        while (true);
+    }
+}
--- a/sw/bootloader/src/interrupts.h
+++ b/sw/bootloader/src/interrupts.h
@ -0,0 +1,22 @@
+#ifndef INTERRUPTS_H__
+#define INTERRUPTS_H__
+
+
+#include <stdint.h>
+
+
+#define WITH_INTERRUPTS_DISABLED(x) { \
+    uint32_t __sr = interrupts_disable(); \
+    { x } \
+    interrupts_restore(__sr); \
+}
+
+
+void interrupts_init (void);
+uint32_t interrupts_disable (void);
+void interrupts_restore (uint32_t sr);
+void interrupts_start_watchdog (void);
+void interrupts_stop_watchdog (void);
+
+
+#endif
--- a/sw/bootloader/src/io.c
+++ b/sw/bootloader/src/io.c
@ -1,3 +1,4 @@
+#include "interrupts.h"
 #include "io.h"
 #include "vr4300.h"

@ -72,26 +73,39 @@ uint32_t pi_busy (void) {
 }

 uint32_t pi_io_read (io32_t *address) {
-    return cpu_io_read(address);
+    uint32_t value;
+    WITH_INTERRUPTS_DISABLED({
+        while (pi_busy());
+        value = cpu_io_read(address);
+    });
+    return value;
 }

 void pi_io_write (io32_t *address, uint32_t value) {
-    cpu_io_write(address, value);
+    WITH_INTERRUPTS_DISABLED({
        while (pi_busy());
+        cpu_io_write(address, value);
+    });
 }

 void pi_dma_read (io32_t *address, void *buffer, size_t length) {
    cache_data_hit_writeback_invalidate(buffer, length);
+    WITH_INTERRUPTS_DISABLED({
+        while (pi_busy());
        cpu_io_write(&PI->PADDR, (uint32_t) (PHYSICAL(address)));
        cpu_io_write(&PI->MADDR, (uint32_t) (PHYSICAL(buffer)));
        cpu_io_write(&PI->WDMA, length - 1);
+    });
    while (pi_busy());
 }

 void pi_dma_write (io32_t *address, void *buffer, size_t length) {
    cache_data_hit_writeback(buffer, length);
+    WITH_INTERRUPTS_DISABLED({
+        while (pi_busy());
        cpu_io_write(&PI->PADDR, (uint32_t) (PHYSICAL(address)));
        cpu_io_write(&PI->MADDR, (uint32_t) (PHYSICAL(buffer)));
        cpu_io_write(&PI->RDMA, length - 1);
+    });
    while (pi_busy());
 }
--- a/sw/bootloader/src/sc64.c
+++ b/sw/bootloader/src/sc64.c
@ -1,21 +1,32 @@
+#include "error.h"
 #include "io.h"
 #include "sc64.h"


 typedef struct {
-    io32_t SR_CMD;
+    io32_t SCR;
    io32_t DATA[2];
    io32_t IDENTIFIER;
    io32_t KEY;
+    io32_t IRQ;
+    io32_t AUX;
 } sc64_regs_t;

 #define SC64_REGS_BASE              (0x1FFF0000UL)
 #define SC64_REGS                   ((sc64_regs_t *) SC64_REGS_BASE)


-#define SC64_SR_IRQ_PENDING         (1 << 29)
-#define SC64_SR_CMD_ERROR           (1 << 30)
-#define SC64_SR_CPU_BUSY            (1 << 31)
+#define SC64_SCR_CPU_BUSY           (1 << 31)
+#define SC64_SCR_CMD_ERROR          (1 << 30)
+#define SC64_SCR_BTN_IRQ_PENDING    (1 << 29)
+#define SC64_SCR_BTN_IRQ_MASK       (1 << 28)
+#define SC64_SCR_CMD_IRQ_PENDING    (1 << 27)
+#define SC64_SCR_CMD_IRQ_MASK       (1 << 26)
+#define SC64_SCR_USB_IRQ_PENDING    (1 << 25)
+#define SC64_SCR_USB_IRQ_MASK       (1 << 24)
+#define SC64_SCR_AUX_IRQ_PENDING    (1 << 23)
+#define SC64_SCR_AUX_IRQ_MASK       (1 << 22)
+#define SC64_SCR_CMD_IRQ_REQUEST    (1 << 8)

 #define SC64_V2_IDENTIFIER          (0x53437632)

@ -24,6 +35,15 @@ typedef struct {
 #define SC64_KEY_UNLOCK_2           (0x4F434B5FUL)
 #define SC64_KEY_LOCK               (0xFFFFFFFFUL)

+#define SC64_IRQ_BTN_CLEAR          (1 << 31)
+#define SC64_IRQ_CMD_CLEAR          (1 << 30)
+#define SC64_IRQ_USB_CLEAR          (1 << 29)
+#define SC64_IRQ_AUX_CLEAR          (1 << 28)
+#define SC64_IRQ_USB_DISABLE        (1 << 11)
+#define SC64_IRQ_USB_ENABLE         (1 << 10)
+#define SC64_IRQ_AUX_DISABLE        (1 << 9)
+#define SC64_IRQ_AUX_ENABLE         (1 << 8)
+

 typedef enum {
    CMD_ID_IDENTIFIER_GET       = 'v',
@ -67,18 +87,32 @@ typedef struct {
 } sc64_cmd_t;


+static bool use_cmd_irq = false;
+static volatile bool wait_cmd_irq = false;
+
+
 static sc64_error_t sc64_execute_cmd (sc64_cmd_t *cmd) {
+    uint32_t sr;
+
    pi_io_write(&SC64_REGS->DATA[0], cmd->arg[0]);
    pi_io_write(&SC64_REGS->DATA[1], cmd->arg[1]);

-    pi_io_write(&SC64_REGS->SR_CMD, (cmd->id & 0xFF));
-
-    uint32_t sr;
+    if (use_cmd_irq) {
+        wait_cmd_irq = true;
+        pi_io_write(&SC64_REGS->SCR, (SC64_SCR_CMD_IRQ_REQUEST | (cmd->id & 0xFF)));
+        while (wait_cmd_irq);
+        sr = pi_io_read(&SC64_REGS->SCR);
+        if (sr & SC64_SCR_CPU_BUSY) {
+            error_display("[Unexpected] SC64 CMD busy flag set");
+        }
+    } else {
+        pi_io_write(&SC64_REGS->SCR, (cmd->id & 0xFF));
        do {
-        sr = pi_io_read(&SC64_REGS->SR_CMD);
-    } while (sr & SC64_SR_CPU_BUSY);
+            sr = pi_io_read(&SC64_REGS->SCR);
+        } while (sr & SC64_SCR_CPU_BUSY);
+    }

-    if (sr & SC64_SR_CMD_ERROR) {
+    if (sr & SC64_SCR_CMD_ERROR) {
        return (sc64_error_t) (pi_io_read(&SC64_REGS->DATA[0]));
    }

@ -88,14 +122,38 @@ static sc64_error_t sc64_execute_cmd (sc64_cmd_t *cmd) {
    return SC64_OK;
 }

+static void sc64_btn_irq_callback (void) {
+    error_display("[Unexpected] SC64 button pressed interrupt received");
+}
+
+static void sc64_cmd_irq_callback (void) {
+    if (wait_cmd_irq) {
+        wait_cmd_irq = false;
+    } else {
+        error_display("[Unexpected] SC64 command finish interrupt received");
+    }
+}
+
+static void sc64_usb_irq_callback (void) {
+    error_display("[Unexpected] SC64 USB not empty interrupt received");
+}
+
+static void sc64_aux_irq_callback (void) {
+    error_display("[Unexpected] SC64 AUX not empty interrupt received");
+}
+

 const char *sc64_error_description (sc64_error_t error) {
+    if (error == SC64_OK) {
+        return "No error";
+    }
+
    sc64_error_type_t type = (sc64_error_type_t) ((error >> 24) & 0xFF);
    error &= 0xFFFFFF;

    if (type == ERROR_TYPE_CFG) {
        switch ((sc64_cfg_error_t) (error)) {
-            case SC64_OK: return "No error";
+            case CFG_OK: return "No error (CFG)";
            case CFG_ERROR_UNKNOWN_COMMAND: return "Unknown command";
            case CFG_ERROR_INVALID_ARGUMENT: return "Invalid argument";
            case CFG_ERROR_INVALID_ADDRESS: return "Invalid address";
@ -158,18 +216,70 @@ void sc64_lock (void) {
 bool sc64_check_presence (void) {
    bool detected = (pi_io_read(&SC64_REGS->IDENTIFIER) == SC64_V2_IDENTIFIER);
    if (detected) {
-        while (pi_io_read(&SC64_REGS->SR_CMD) & SC64_SR_CPU_BUSY);
+        while (pi_io_read(&SC64_REGS->SCR) & SC64_SCR_CPU_BUSY);
    }
    return detected;
 }


-bool sc64_irq_pending (void) {
-    return (pi_io_read(&SC64_REGS->SR_CMD) & SC64_SR_IRQ_PENDING);
+void sc64_cmd_irq_enable (bool enable) {
+    use_cmd_irq = enable;
 }

-void sc64_irq_clear (void) {
-    pi_io_write(&SC64_REGS->IDENTIFIER, 0);
+void sc64_usb_irq_enable (bool enable) {
+    pi_io_write(&SC64_REGS->IRQ, enable ? SC64_IRQ_USB_ENABLE : SC64_IRQ_USB_DISABLE);
+}
+
+void sc64_aux_irq_enable (bool enable) {
+    pi_io_write(&SC64_REGS->IRQ, enable ? SC64_IRQ_AUX_ENABLE : SC64_IRQ_AUX_DISABLE);
+}
+
+sc64_irq_t sc64_irq_pending (void) {
+    uint32_t sr = pi_io_read(&SC64_REGS->SCR);
+    sc64_irq_t irq = SC64_IRQ_NONE;
+    if ((sr & SC64_SCR_BTN_IRQ_MASK) && (sr & SC64_SCR_BTN_IRQ_PENDING)) {
+        irq |= SC64_IRQ_BTN;
+    }
+    if ((sr & SC64_SCR_CMD_IRQ_MASK) && (sr & SC64_SCR_CMD_IRQ_PENDING)) {
+        irq |= SC64_IRQ_CMD;
+    }
+    if ((sr & SC64_SCR_USB_IRQ_MASK) && (sr & SC64_SCR_USB_IRQ_PENDING)) {
+        irq |= SC64_IRQ_USB;
+    }
+    if ((sr & SC64_SCR_AUX_IRQ_MASK) && (sr & SC64_SCR_AUX_IRQ_PENDING)) {
+        irq |= SC64_IRQ_AUX;
+    }
+    return irq;
+}
+
+void sc64_irq_callback (sc64_irq_t irq) {
+    uint32_t clear = 0;
+    if (irq & SC64_IRQ_BTN) {
+        clear |= SC64_IRQ_BTN_CLEAR;
+    }
+    if (irq & SC64_IRQ_CMD) {
+        clear |= SC64_IRQ_CMD_CLEAR;
+    }
+    if (irq & SC64_IRQ_USB) {
+        clear |= SC64_IRQ_USB_CLEAR;
+    }
+    if (irq & SC64_IRQ_AUX) {
+        clear |= SC64_IRQ_AUX_CLEAR;
+    }
+    pi_io_write(&SC64_REGS->IRQ, clear);
+    if (irq & SC64_IRQ_BTN) {
+        sc64_btn_irq_callback();
+    }
+    if (irq & SC64_IRQ_CMD) {
+        sc64_cmd_irq_callback();
+    }
+    if (irq & SC64_IRQ_USB) {
+        sc64_usb_irq_callback();
+    }
+    if (irq & SC64_IRQ_AUX) {
+        sc64_aux_irq_callback();
+    }
+    while (pi_busy());
 }


--- a/sw/bootloader/src/sc64.h
+++ b/sw/bootloader/src/sc64.h
@ -7,13 +7,17 @@
 #include <stdint.h>


+#define SC64_OK (0)
+
+
 typedef enum {
+    ERROR_TYPE_OBSOLETE = 0,
    ERROR_TYPE_CFG = 1,
    ERROR_TYPE_SD_CARD = 2,
 } sc64_error_type_t;

 typedef enum {
-    SC64_OK = 0,
+    CFG_OK = 0,
    CFG_ERROR_UNKNOWN_COMMAND = 1,
    CFG_ERROR_INVALID_ARGUMENT = 2,
    CFG_ERROR_INVALID_ADDRESS = 3,
@ -160,6 +164,14 @@ typedef enum {
    SD_CARD_STATUS_BYTE_SWAP = (1 << 4),
 } sc64_sd_card_status_t;

+typedef enum {
+    SC64_IRQ_NONE = 0,
+    SC64_IRQ_BTN = (1 << 0),
+    SC64_IRQ_CMD = (1 << 1),
+    SC64_IRQ_USB = (1 << 2),
+    SC64_IRQ_AUX = (1 << 3),
+} sc64_irq_t;
+

 typedef struct {
    volatile uint8_t BUFFER[8192];
@ -178,8 +190,11 @@ void sc64_unlock (void);
 void sc64_lock (void);
 bool sc64_check_presence (void);

-bool sc64_irq_pending (void);
-void sc64_irq_clear (void);
+void sc64_cmd_irq_enable (bool enable);
+void sc64_usb_irq_enable (bool enable);
+void sc64_aux_irq_enable (bool enable);
+sc64_irq_t sc64_irq_pending (void);
+void sc64_irq_callback (sc64_irq_t irq);

 sc64_error_t sc64_get_identifier (uint32_t *identifier);
 sc64_error_t sc64_get_version (uint16_t *major, uint16_t *minor, uint32_t *revision);
--- a/sw/bootloader/src/startup.S
+++ b/sw/bootloader/src/startup.S
@ -1,22 +1,27 @@
-.section .text.entry_handler, "ax", %progbits
+#include "vr4300.h"

+
+#define RSP_DMEM_ADDRESS    0xA4000000
+
+
+.section .text.entry_handler, "ax", %progbits
+.type entry_handler, %function
+.global entry_handler
 entry_handler:
-    .type entry_handler, %function
-    .global entry_handler
+    li $t0, (C0_SR_CU1 | C0_SR_CU0 | C0_SR_FR)
+	mtc0 $t0, C0_STATUS

    la $gp, _gp
    la $sp, _sp

-    lui $t0, 0xA400
+    li $t0, RSP_DMEM_ADDRESS    # IPL3 Boot flags location
    lbu $a0, 9($t0)             # TV type
    lbu $a1, 10($t0)            # Reset type
    lw $a2, 4($t0)              # Entropy

-    la $t0, init
-    jalr $t0
+    jal init

-    la $t0, main
-    jalr $t0
+    jal main

 loop:
    j loop
--- a/sw/bootloader/src/test.c
+++ b/sw/bootloader/src/test.c
@ -115,6 +115,22 @@ static void test_sc64_cfg (void) {
    display_printf("%02d:%02d:%02d", FROM_BCD(t.hour), FROM_BCD(t.minute), FROM_BCD(t.second));
    display_printf(" (%s)", weekdays[FROM_BCD(t.weekday)]);
    display_printf("\n");
+
+    int count = 65536;
+
+    for (int i = 0; i < count; i++) {
+        if ((i % (count / 64)) == 0) {
+            display_printf(".");
+        }
+        if ((error = sc64_get_identifier(&identifier)) != SC64_OK) {
+            error_display("Command IDENTIFIER_GET failed\n (%08X) - %s", error, sc64_error_description(error));
+        }
+        if (identifier != 0x53437632) {
+            error_display("Invalid identifier received: 0x%08X", identifier);
+        }
+    }
+
+    display_printf("\n");
 }

 static void test_pi (void) {
@ -539,8 +555,20 @@ static struct {
 void test_execute (void) {
    sc64_error_t error;

+    const int test_count = sizeof(tests) / sizeof(tests[0]);
+    int current = 0;
+
+    display_init(NULL);
+    display_printf("SC64 Test suite (%d / %d)\n\n", 0, test_count);
+
+    display_printf("Initializing...\n");
+
    pi_io_config(0x0F, 0x05, 0x0C, 0x02);

+    sc64_cmd_irq_enable(true);
+    sc64_usb_irq_enable(true);
+    sc64_aux_irq_enable(true);
+
    if ((error = sc64_set_config(CFG_ID_ROM_WRITE_ENABLE, true)) != SC64_OK) {
        error_display("Command CONFIG_SET [ROM_WRITE_ENABLE] failed\n (%08X) - %s", error, sc64_error_description(error));
    }
@ -551,9 +579,6 @@ void test_execute (void) {

    random_seed = __entropy + c0_count();

-    const int test_count = sizeof(tests) / sizeof(tests[0]);
-    int current = 0;
-
    while (true) {
        display_init(NULL);
        display_printf("SC64 Test suite (%d / %d)\n\n", current + 1, test_count);
--- a/sw/bootloader/src/version.c
+++ b/sw/bootloader/src/version.c
@ -1,7 +1,12 @@
-#include "version.h"
+#include "display.h"


-static version_t version = {
+const struct {
+    const char *git_branch;
+    const char *git_tag;
+    const char *git_sha;
+    const char *git_message;
+} version = {
 #ifdef GIT_BRANCH
    .git_branch = GIT_BRANCH,
 #else
@ -29,6 +34,10 @@ static version_t version = {
 };


-version_t *version_get (void) {
-    return &version;
+void version_print (void) {
+    display_printf("[ SC64 bootloader metadata ]\n");
+    display_printf("branch: %s | tag: %s\n", version.git_branch, version.git_tag);
+    display_printf("sha: %s\n", version.git_sha);
+    display_printf("msg: %s\n", version.git_message);
+    display_printf("\n");
 }
--- a/sw/bootloader/src/version.h
+++ b/sw/bootloader/src/version.h
@ -2,15 +2,7 @@
 #define VERSION_H__


-typedef const struct {
-    const char *git_branch;
-    const char *git_tag;
-    const char *git_sha;
-    const char *git_message;
-} version_t;
-
-
-const version_t *version_get (void);
+void version_print (void);


 #endif
--- a/sw/bootloader/src/vr4300.h
+++ b/sw/bootloader/src/vr4300.h
@ -9,6 +9,7 @@
 #define CACHE_LINE_SIZE_I           (32)
 #define CACHE_LINE_SIZE_D           (16)

+
 #define C0_BADVADDR                 $8
 #define C0_COUNT                    $9
 #define C0_COMPARE                  $11
@ -48,6 +49,9 @@
 #define C0_SR_CU2                   (1 << 30)
 #define C0_SR_CU3                   (1 << 31)

+#define C0_SR_IM_MASK               (C0_SR_IM7 | C0_SR_IM6 | C0_SR_IM5 | C0_SR_IM4 | C0_SR_IM3 | C0_SR_IM2 | C0_SR_IM1 | C0_SR_IM0)
+#define C0_SR_IM_BIT                (8)
+

 #define C0_CR_EC0                   (1 << 2)
 #define C0_CR_EC1                   (1 << 3)
--- a/sw/controller/src/button.c
+++ b/sw/controller/src/button.c
@ -75,7 +75,7 @@ void button_process (void) {
    if (p.trigger) {
        switch (p.mode) {
            case BUTTON_MODE_N64_IRQ:
-                fpga_reg_set(REG_CFG_CMD, CFG_CMD_IRQ);
+                fpga_reg_set(REG_CFG_CMD, CFG_CMD_BTN_IRQ);
                p.trigger = false;
                break;

--- a/sw/controller/src/cfg.c
+++ b/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,
@ -99,6 +125,9 @@ typedef enum {
 } cfg_error_t;

 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;
@ -111,8 +140,53 @@ struct process {
 static struct process p;


-static void cfg_set_usb_output_ready (void) {
-    p.usb_output_ready = true;
+static bool cfg_cmd_check (void) {
+    uint32_t reg = fpga_reg_get(REG_CFG_CMD);
+
+    if (reg & CFG_CMD_AUX_PENDING) {
+        usb_tx_info_t packet_info;
+        usb_create_packet(&packet_info, PACKET_CMD_AUX_DATA);
+        packet_info.data_length = 4;
+        packet_info.data[0] = fpga_reg_get(REG_AUX);
+        if (usb_enqueue_packet(&packet_info)) {
+            fpga_reg_set(REG_CFG_CMD, CFG_CMD_AUX_DONE);
+        }
+    }
+
+    if (!p.cmd_queued) {
+        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 (error_type_t type, uint32_t error) {
+    p.cmd_queued = false;
+    fpga_reg_set(REG_CFG_DATA_0, ((type & 0xFF) << 24) | (error & 0xFFFFFF));
+    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) {
@ -171,20 +245,6 @@ static bool cfg_translate_address (uint32_t *address, uint32_t length, translate
    return true;
 }

-static void cfg_set_error (error_type_t type, uint32_t error) {
-    fpga_reg_set(REG_CFG_DATA_0, ((type & 0xFF) << 24) | (error & 0xFFFFFF));
-    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;
@ -249,6 +309,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);
@ -479,91 +543,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;
-    uint32_t args[2];
-    uint32_t prev_cfg[2];
+    if (cfg_cmd_check()) {
+        return;
+    }
+
+    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(ERROR_TYPE_CFG, CFG_ERROR_INVALID_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(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ID);
+            }
+            p.data[1] = prev[1];
+            break;
+        }
+
+        case CMD_ID_SETTING_GET:
+            if (cfg_query_setting(p.data)) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ID);
+            }
+            break;
+
+        case CMD_ID_SETTING_SET:
+            if (cfg_update_setting(p.data)) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_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(ERROR_TYPE_CFG, CFG_ERROR_INVALID_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(ERROR_TYPE_CFG, CFG_ERROR_INVALID_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(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ID);
-                    return;
-                }
-                break;
-
-            case 'C':
-                prev_cfg[0] = args[0];
-                cfg_query(prev_cfg);
-                if (cfg_update(args)) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ID);
-                    return;
-                }
-                args[1] = prev_cfg[1];
-                break;
-
-            case 'a':
-                if (cfg_query_setting(args)) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ID);
-                    return;
-                }
-                break;
-
-            case 'A':
-                if (cfg_update_setting(args)) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_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(ERROR_TYPE_CFG, CFG_ERROR_INVALID_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(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
-                    return;
-                }
            usb_create_packet(&packet_info, PACKET_CMD_DEBUG_OUTPUT);
            packet_info.data_length = 4;
-                packet_info.data[0] = args[1];
-                packet_info.dma_length = (args[1] & 0xFFFFFF);
-                packet_info.dma_address = args[0];
+            packet_info.data[0] = p.data[1];
+            packet_info.dma_length = (p.data[1] & 0xFFFFFF);
+            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;
@ -571,179 +624,161 @@ void cfg_process (void) {
                return;
            }
            break;
+        }

-            case 'u':
-                usb_get_read_info(args);
+        case CMD_ID_USB_READ_STATUS:
+            usb_get_read_info(p.data);
            break;

-            case 'U':
-                args[0] = p.usb_output_ready ? 0 : (1 << 31);
+        case CMD_ID_USB_WRITE_STATUS:
+            p.data[0] = p.usb_output_ready ? 0 : (1 << 31);
            break;

-            case 'i':
-                switch (args[1]) {
+        case CMD_ID_SD_CARD_OP:
+            switch (p.data[1]) {
                case SD_CARD_OP_DEINIT:
                    sd_card_deinit();
                    break;
+
                case SD_CARD_OP_INIT: {
                    led_activity_on();
                    sd_error_t error = sd_card_init();
                    led_activity_off();
                    if (error != SD_OK) {
-                            cfg_set_error(ERROR_TYPE_SD_CARD, error);
-                            return;
+                        return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, error);
                    }
                    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))) {
-                            cfg_set_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ADDRESS);
-                            return;
+
+                case SD_CARD_OP_GET_INFO:
+                    if (cfg_translate_address(&p.data[0], SD_CARD_INFO_SIZE, (SDRAM | BRAM))) {
+                        return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ADDRESS);
                    }
-                        sd_error_t error = sd_card_get_info(args[0]);
+                    sd_error_t error = sd_card_get_info(p.data[0]);
                    if (error != SD_OK) {
-                            cfg_set_error(ERROR_TYPE_SD_CARD, error);
-                            return;
+                        return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, error);
                    }
                    break;
-                    }
+
                case SD_CARD_OP_BYTE_SWAP_ON: {
                    sd_error_t error = sd_set_byte_swap(true);
                    if (error != SD_OK) {
-                            cfg_set_error(ERROR_TYPE_SD_CARD, error);
-                            return;
+                        return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, error);
                    }
                    break;
                }
+
                case SD_CARD_OP_BYTE_SWAP_OFF: {
                    sd_error_t error = sd_set_byte_swap(false);
                    if (error != SD_OK) {
-                            cfg_set_error(ERROR_TYPE_SD_CARD, error);
-                            return;
+                        return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, error);
                    }
                    break;
                }
+
                default:
-                        cfg_set_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_OPERATION);
-                        return;
+                    return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_OPERATION);
            }
            break;

-            case 'I':
-                p.sd_card_sector = args[0];
+        case CMD_ID_SD_SECTOR_SET:
+            p.sd_card_sector = p.data[0];
            break;

-            case 's': {
-                if (args[1] >= 0x800000) {
-                    cfg_set_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ARGUMENT);
-                    return;
+        case CMD_ID_SD_READ: {
+            if (p.data[1] >= 0x800000) {
+                return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ARGUMENT);
            }
-                if (cfg_translate_address(&args[0], args[1] * SD_SECTOR_SIZE, (SDRAM | FLASH | BRAM))) {
-                    cfg_set_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ADDRESS);
-                    return;
+            if (cfg_translate_address(&p.data[0], (p.data[1] * SD_SECTOR_SIZE), (SDRAM | FLASH | BRAM))) {
+                return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ADDRESS);
            }
            led_activity_on();
-                sd_error_t error = sd_read_sectors(args[0], p.sd_card_sector, args[1]);
+            sd_error_t error = sd_read_sectors(p.data[0], p.sd_card_sector, p.data[1]);
            led_activity_off();
            if (error != SD_OK) {
-                    cfg_set_error(ERROR_TYPE_SD_CARD, error);
-                    return;
+                return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, error);
            }
-                p.sd_card_sector += args[1];
+            p.sd_card_sector += p.data[1];
            break;
        }

-            case 'S': {
-                if (args[1] >= 0x800000) {
-                    cfg_set_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ARGUMENT);
-                    return;
+        case CMD_ID_SD_WRITE: {
+            if (p.data[1] >= 0x800000) {
+                return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ARGUMENT);
            }
-                if (cfg_translate_address(&args[0], args[1] * SD_SECTOR_SIZE, (SDRAM | FLASH | BRAM))) {
-                    cfg_set_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ADDRESS);
-                    return;
+            if (cfg_translate_address(&p.data[0], (p.data[1] * SD_SECTOR_SIZE), (SDRAM | FLASH | BRAM))) {
+                return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, SD_ERROR_INVALID_ADDRESS);
            }
            led_activity_on();
-                sd_error_t error = sd_write_sectors(args[0], p.sd_card_sector, args[1]);
+            sd_error_t error = sd_write_sectors(p.data[0], p.sd_card_sector, p.data[1]);
            led_activity_off();
            if (error != SD_OK) {
-                    cfg_set_error(ERROR_TYPE_SD_CARD, error);
-                    return;
+                return cfg_cmd_reply_error(ERROR_TYPE_SD_CARD, error);
            }
-                p.sd_card_sector += args[1];
+            p.sd_card_sector += p.data[1];
            break;
        }

-            case 'D':
-                if (cfg_translate_address(&args[0], args[1], (SDRAM | BRAM))) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
-                    return;
+        case CMD_ID_DISK_MAPPING_SET:
+            if (cfg_translate_address(&p.data[0], p.data[1], (SDRAM | BRAM))) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
            }
-                dd_set_disk_mapping(args[0], args[1]);
+            dd_set_disk_mapping(p.data[0], p.data[1]);
            break;

-            case 'w':
-                args[0] = writeback_pending();
+        case CMD_ID_WRITEBACK_PENDING:
+            p.data[0] = writeback_pending();
            break;

-            case 'W':
-                if (cfg_translate_address(&args[0], WRITEBACK_SECTOR_TABLE_SIZE, (SDRAM | BRAM))) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
-                    return;
+        case CMD_ID_WRITEBACK_SD_INFO:
+            if (cfg_translate_address(&p.data[0], WRITEBACK_SECTOR_TABLE_SIZE, (SDRAM | BRAM))) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
            }
-                writeback_load_sector_table(args[0]);
+            writeback_load_sector_table(p.data[0]);
            writeback_enable(WRITEBACK_SD);
            break;

-            case 'K':
-                if (args[1] >= DATA_BUFFER_SIZE) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ARGUMENT);
-                    return;
+        case CMD_ID_FLASH_PROGRAM:
+            if (p.data[1] >= DATA_BUFFER_SIZE) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ARGUMENT);
            }
-                if (cfg_translate_address(&args[0], args[1], FLASH)) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
-                    return;
+            if (cfg_translate_address(&p.data[0], p.data[1], FLASH)) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
            }
-                if (flash_program(DATA_BUFFER_ADDRESS, args[0], args[1])) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ARGUMENT);
-                    return;
+            if (flash_program(DATA_BUFFER_ADDRESS, p.data[0], p.data[1])) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ARGUMENT);
            }
            break;

-            case 'p':
-                if (args[0]) {
+        case CMD_ID_FLASH_WAIT_BUSY:
+            if (p.data[0]) {
                flash_wait_busy();
            }
-                args[0] = FLASH_ERASE_BLOCK_SIZE;
+            p.data[0] = FLASH_ERASE_BLOCK_SIZE;
            break;

-            case 'P':
-                if (cfg_translate_address(&args[0], FLASH_ERASE_BLOCK_SIZE, FLASH)) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
-                    return;
+        case CMD_ID_FLASH_ERASE_BLOCK:
+            if (cfg_translate_address(&p.data[0], FLASH_ERASE_BLOCK_SIZE, FLASH)) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ADDRESS);
            }
-                if (flash_erase_block(args[0])) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ARGUMENT);
-                    return;
+            if (flash_erase_block(p.data[0])) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ARGUMENT);
            }
            break;

-            case '%':
-                if (cfg_read_diagnostic_data(args)) {
-                    cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ID);
-                    return;
+        case CMD_ID_DIAGNOSTIC_GET:
+            if (cfg_read_diagnostic_data(p.data)) {
+                return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_INVALID_ID);
            }
            break;

        default:
-                cfg_set_error(ERROR_TYPE_CFG, CFG_ERROR_UNKNOWN_COMMAND);
-                return;
+            return cfg_cmd_reply_error(ERROR_TYPE_CFG, CFG_ERROR_UNKNOWN_COMMAND);
    }

-        fpga_reg_set(REG_CFG_DATA_0, args[0]);
-        fpga_reg_set(REG_CFG_DATA_1, args[1]);
-        fpga_reg_set(REG_CFG_CMD, CFG_CMD_DONE);
-    }
+    cfg_cmd_reply_success();
 }
--- a/sw/controller/src/fpga.h
+++ b/sw/controller/src/fpga.h
@ -57,6 +57,7 @@ typedef enum {
    REG_DEBUG_1,
    REG_CIC_0,
    REG_CIC_1,
+    REG_AUX,
 } fpga_reg_t;


@ -89,6 +90,7 @@ typedef enum {
 #define USB_SCR_RESET_STATE             (1 << 28)
 #define USB_SCR_PWRSAV                  (1 << 29)
 #define USB_SCR_FIFO_FLUSH_BUSY         (1 << 30)
+#define USB_SCR_IRQ                     (1 << 31)

 #define DMA_SCR_START                   (1 << 0)
 #define DMA_SCR_STOP                    (1 << 1)
@ -115,7 +117,9 @@ typedef enum {
 #define CFG_CMD_PENDING                 (1 << 8)
 #define CFG_CMD_DONE                    (1 << 9)
 #define CFG_CMD_ERROR                   (1 << 10)
-#define CFG_CMD_IRQ                     (1 << 11)
+#define CFG_CMD_BTN_IRQ                 (1 << 11)
+#define CFG_CMD_AUX_PENDING             (1 << 12)
+#define CFG_CMD_AUX_DONE                (1 << 13)

 #define FLASHRAM_SCR_DONE               (1 << 0)
 #define FLASHRAM_SCR_PENDING            (1 << 1)
--- a/sw/controller/src/usb.c
+++ b/sw/controller/src/usb.c
@ -231,9 +231,6 @@ static void usb_rx_process (void) {
            p.response_info.data_length = 0;
            p.response_info.dma_length = 0;
            p.response_info.done_callback = NULL;
-            if (p.rx_cmd == 'U') {
-                timer_countdown_start(TIMER_ID_USB, DEBUG_WRITE_TIMEOUT_MS);
-            }
        }
    }

@ -243,6 +240,10 @@ static void usb_rx_process (void) {
            if (p.rx_counter == 2) {
                p.rx_counter = 0;
                p.rx_state = RX_STATE_DATA;
+                if ((p.rx_cmd == 'U') && (p.rx_args[0] > 0)) {
+                    fpga_reg_set(REG_USB_SCR, USB_SCR_IRQ);
+                    timer_countdown_start(TIMER_ID_USB, DEBUG_WRITE_TIMEOUT_MS);
+                }
                break;
            }
        }
@ -377,6 +378,12 @@ static void usb_rx_process (void) {
                }
                break;

+            case 'X':
+                fpga_reg_set(REG_AUX, p.rx_args[0]);
+                p.rx_state = RX_STATE_IDLE;
+                p.response_pending = true;
+                break;
+
            case 'D':
                dd_set_block_ready(p.rx_args[0] == 0);
                p.rx_state = RX_STATE_IDLE;
--- a/sw/controller/src/usb.h
+++ b/sw/controller/src/usb.h
@ -7,6 +7,7 @@


 typedef enum packet_cmd {
+    PACKET_CMD_AUX_DATA = 'X',
    PACKET_CMD_BUTTON_TRIGGER = 'B',
    PACKET_CMD_DATA_FLUSHED = 'G',
    PACKET_CMD_DEBUG_OUTPUT = 'U',
--- a/sw/deployer/src/main.rs
+++ b/sw/deployer/src/main.rs
@ -87,6 +87,10 @@ struct UploadArgs {
    /// Path to the ROM file
    rom: PathBuf,

+    /// Attempt to reboot the console (requires specific support in the running game)
+    #[arg(short = 'a', long)]
+    reboot: bool,
+
    /// Path to the save file
    #[arg(short, long)]
    save: Option<PathBuf>,
@ -136,6 +140,10 @@ struct _64DDArgs {
    #[arg(short, long)]
    rom: Option<PathBuf>,

+    /// Attempt to reboot the console (requires specific support in the running game)
+    #[arg(short = 'a', long)]
+    reboot: bool,
+
    /// Path to the save file (also used by save writeback mechanism)
    #[arg(short, long, requires = "rom")]
    save: Option<PathBuf>,
@ -365,6 +373,13 @@ fn handle_list_command() -> Result<(), sc64::Error> {
 fn handle_upload_command(connection: Connection, args: &UploadArgs) -> Result<(), sc64::Error> {
    let mut sc64 = init_sc64(connection, true)?;

+    if args.reboot && !sc64.try_notify_via_aux(sc64::AuxMessage::Halt)? {
+        println!(
+            "{}",
+            "Warning: no response for [Halt] AUX message".bright_yellow()
+        );
+    }
+
    sc64.reset_state()?;

    let (mut rom_file, rom_name, rom_length) = open_file(&args.rom)?;
@ -410,6 +425,13 @@ fn handle_upload_command(connection: Connection, args: &UploadArgs) -> Result<()

    sc64.calculate_cic_parameters(args.cic_seed)?;

+    if args.reboot && !sc64.try_notify_via_aux(sc64::AuxMessage::Reboot)? {
+        println!(
+            "{}",
+            "Warning: no response for [Reboot] AUX message".bright_yellow()
+        );
+    }
+
    Ok(())
 }

@ -448,6 +470,13 @@ fn handle_64dd_command(connection: Connection, args: &_64DDArgs) -> Result<(), s
            .bright_green()
    );

+    if args.reboot && !sc64.try_notify_via_aux(sc64::AuxMessage::Halt)? {
+        println!(
+            "{}",
+            "Warning: no response for [Halt] AUX message".bright_yellow()
+        );
+    }
+
    sc64.reset_state()?;

    if let Some(rom) = &args.rom {
@ -558,6 +587,13 @@ fn handle_64dd_command(connection: Connection, args: &_64DDArgs) -> Result<(), s

    sc64.set_save_writeback(true)?;

+    if args.reboot && !sc64.try_notify_via_aux(sc64::AuxMessage::Reboot)? {
+        println!(
+            "{}",
+            "Warning: no response for [Reboot] AUX message".bright_yellow()
+        );
+    }
+
    let exit = setup_exit_flag();
    while !exit.load(Ordering::Relaxed) {
        if let Some(data_packet) = sc64.receive_data_packet()? {
@ -758,10 +794,7 @@ fn handle_info_command(connection: Connection) -> Result<(), sc64::Error> {
    println!(" LED blink:         {}", state.led_enable);
    println!(" IS-Viewer 64:      {}", state.isviewer);
    println!("{}", "SummerCart64 diagnostic information:".bold());
-    println!(
-        " Last PI address:   0x{:08X}",
-        state.fpga_debug_data.last_pi_address
-    );
+    println!(" PI I/O access:     {}", state.fpga_debug_data.pi_io_access);
    println!(
        " PI FIFO flags:     {}",
        state.fpga_debug_data.pi_fifo_flags
--- a/sw/deployer/src/sc64/mod.rs
+++ b/sw/deployer/src/sc64/mod.rs
@ -13,10 +13,10 @@ pub use self::{
    link::list_local_devices,
    server::ServerEvent,
    types::{
-        BootMode, ButtonMode, ButtonState, CicSeed, DataPacket, DdDiskState, DdDriveType, DdMode,
-        DebugPacket, DiagnosticData, DiskPacket, DiskPacketKind, FpgaDebugData, ISViewer,
-        MemoryTestPattern, MemoryTestPatternResult, SaveType, SaveWriteback, SpeedTestDirection,
-        Switch, TvType,
+        AuxMessage, BootMode, ButtonMode, ButtonState, CicSeed, DataPacket, DdDiskState,
+        DdDriveType, DdMode, DebugPacket, DiagnosticData, DiskPacket, DiskPacketKind,
+        FpgaDebugData, ISViewer, MemoryTestPattern, MemoryTestPatternResult, SaveType,
+        SaveWriteback, SpeedTestDirection, Switch, TvType,
    },
 };

@ -230,6 +230,11 @@ impl SC64 {
        Ok(())
    }

+    fn command_aux_write(&mut self, data: u32) -> Result<(), Error> {
+        self.link.execute_command(b'X', [data, 0], &[])?;
+        Ok(())
+    }
+
    fn command_dd_set_block_ready(&mut self, error: bool) -> Result<(), Error> {
        self.link.execute_command(b'D', [error as u32, 0], &[])?;
        Ok(())
@ -566,6 +571,41 @@ impl SC64 {
        self.command_usb_write(debug_packet.datatype, &debug_packet.data)
    }

+    pub fn send_aux_packet(&mut self, data: AuxMessage) -> Result<(), Error> {
+        self.command_aux_write(data.into())
+    }
+
+    pub fn send_and_receive_aux_packet(
+        &mut self,
+        data: AuxMessage,
+        timeout: std::time::Duration,
+    ) -> Result<Option<AuxMessage>, Error> {
+        self.send_aux_packet(data)?;
+        let reply_timeout = std::time::Instant::now();
+        loop {
+            match self.receive_data_packet()? {
+                Some(packet) => match packet {
+                    DataPacket::AuxData(data) => {
+                        return Ok(Some(data));
+                    }
+                    _ => {}
+                },
+                None => {}
+            }
+            if reply_timeout.elapsed() > timeout {
+                return Ok(None);
+            }
+        }
+    }
+
+    pub fn try_notify_via_aux(&mut self, message: AuxMessage) -> Result<bool, Error> {
+        let timeout = std::time::Duration::from_millis(500);
+        if let Some(response) = self.send_and_receive_aux_packet(message, timeout)? {
+            return Ok(message == response);
+        }
+        Ok(false)
+    }
+
    pub fn check_device(&mut self) -> Result<(), Error> {
        let identifier = self.command_identifier_get().map_err(|e| {
            Error::new(format!("Couldn't get SC64 device identifier: {e}").as_str())
--- a/sw/deployer/src/sc64/types.rs
+++ b/sw/deployer/src/sc64/types.rs
@ -614,6 +614,7 @@ impl From<Setting> for [u32; 2] {
 }

 pub enum DataPacket {
+    AuxData(AuxMessage),
    Button,
    DataFlushed,
    DebugData(DebugPacket),
@ -627,27 +628,53 @@ impl TryFrom<AsynchronousPacket> for DataPacket {
    type Error = Error;
    fn try_from(value: AsynchronousPacket) -> Result<Self, Self::Error> {
        Ok(match value.id {
+            b'X' => Self::AuxData(value.data.try_into()?),
            b'B' => Self::Button,
            b'G' => Self::DataFlushed,
            b'U' => Self::DebugData(value.data.try_into()?),
            b'D' => Self::DiskRequest(value.data.try_into()?),
            b'I' => Self::IsViewer64(value.data),
            b'S' => Self::SaveWriteback(value.data.try_into()?),
-            b'F' => {
-                if value.data.len() != 4 {
-                    return Err(Error::new(
-                        "Incorrect data length for update status data packet",
-                    ));
-                }
-                Self::UpdateStatus(
-                    u32::from_be_bytes(value.data[0..4].try_into().unwrap()).try_into()?,
-                )
-            }
+            b'F' => Self::UpdateStatus(value.data.try_into()?),
            _ => return Err(Error::new("Unknown data packet code")),
        })
    }
 }

+#[derive(Clone, Copy, PartialEq, Eq)]
+pub enum AuxMessage {
+    Ping,
+    Halt,
+    Reboot,
+    Other(u32),
+}
+
+impl From<AuxMessage> for u32 {
+    fn from(value: AuxMessage) -> Self {
+        match value {
+            AuxMessage::Ping => 0xFF000000,
+            AuxMessage::Halt => 0xFF000001,
+            AuxMessage::Reboot => 0xFF000002,
+            AuxMessage::Other(message) => message,
+        }
+    }
+}
+
+impl TryFrom<Vec<u8>> for AuxMessage {
+    type Error = Error;
+    fn try_from(value: Vec<u8>) -> Result<Self, Self::Error> {
+        if value.len() != 4 {
+            return Err(Error::new("Invalid data length for AUX data packet"));
+        }
+        Ok(match u32::from_be_bytes(value[0..4].try_into().unwrap()) {
+            0xFF000000 => AuxMessage::Ping,
+            0xFF000001 => AuxMessage::Halt,
+            0xFF000002 => AuxMessage::Reboot,
+            message => AuxMessage::Other(message),
+        })
+    }
+}
+
 pub struct DebugPacket {
    pub datatype: u8,
    pub data: Vec<u8>,
@ -803,10 +830,15 @@ impl Display for UpdateStatus {
    }
 }

-impl TryFrom<u32> for UpdateStatus {
+impl TryFrom<Vec<u8>> for UpdateStatus {
    type Error = Error;
-    fn try_from(value: u32) -> Result<Self, Self::Error> {
-        Ok(match value {
+    fn try_from(value: Vec<u8>) -> Result<Self, Self::Error> {
+        if value.len() != 4 {
+            return Err(Error::new(
+                "Incorrect data length for update status data packet",
+            ));
+        }
+        Ok(match u32::from_be_bytes(value[0..4].try_into().unwrap()) {
            1 => Self::MCU,
            2 => Self::FPGA,
            3 => Self::Bootloader,
@ -817,6 +849,90 @@ impl TryFrom<u32> for UpdateStatus {
    }
 }

+pub enum PiIODirection {
+    Read,
+    Write,
+}
+
+impl Display for PiIODirection {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.write_str(match self {
+            Self::Read => "read",
+            Self::Write => "written",
+        })
+    }
+}
+
+pub struct PiIOAccess {
+    pub address: u32,
+    pub count: u32,
+    pub direction: PiIODirection,
+}
+
+impl From<&[u8; 8]> for PiIOAccess {
+    fn from(value: &[u8; 8]) -> Self {
+        let address = u32::from_be_bytes(value[0..4].try_into().unwrap());
+        let info = u32::from_be_bytes(value[4..8].try_into().unwrap());
+        let count = (info >> 8) & 0x1FFFF;
+        let direction = if (info & (1 << 25)) == 0 {
+            PiIODirection::Read
+        } else {
+            PiIODirection::Write
+        };
+        PiIOAccess {
+            address,
+            count,
+            direction,
+        }
+    }
+}
+
+impl Display for PiIOAccess {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.write_fmt(format_args!("0x{:08X}", self.address))?;
+        if self.count > 0 {
+            f.write_fmt(format_args!(" {} bytes {}", self.count * 2, self.direction))?;
+        }
+        Ok(())
+    }
+}
+
+pub struct PiFifoFlags {
+    pub read_fifo_wait: bool,
+    pub read_fifo_failure: bool,
+    pub write_fifo_wait: bool,
+    pub write_fifo_failure: bool,
+}
+
+impl Display for PiFifoFlags {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        let mapping = vec![
+            (self.read_fifo_wait, "Read wait"),
+            (self.read_fifo_failure, "Read failure"),
+            (self.write_fifo_wait, "Write wait"),
+            (self.write_fifo_failure, "Write failure"),
+        ];
+        let filtered: Vec<&str> = mapping.into_iter().filter(|x| x.0).map(|x| x.1).collect();
+        if filtered.len() > 0 {
+            f.write_str(filtered.join(", ").as_str())?;
+        } else {
+            f.write_str("None")?;
+        }
+        Ok(())
+    }
+}
+
+impl From<&[u8; 8]> for PiFifoFlags {
+    fn from(value: &[u8; 8]) -> Self {
+        PiFifoFlags {
+            read_fifo_wait: (value[7] & (1 << 0)) != 0,
+            read_fifo_failure: (value[7] & (1 << 1)) != 0,
+            write_fifo_wait: (value[7] & (1 << 2)) != 0,
+            write_fifo_failure: (value[7] & (1 << 3)) != 0,
+        }
+    }
+}
+
 pub enum CicStep {
    Unavailable,
    PowerOff,
@ -859,10 +975,10 @@ impl Display for CicStep {
    }
 }

-impl TryFrom<u8> for CicStep {
-    type Error = Error;
-    fn try_from(value: u8) -> Result<Self, Self::Error> {
-        Ok(match value {
+impl From<&[u8; 8]> for CicStep {
+    fn from(value: &[u8; 8]) -> Self {
+        let cic_step = (value[7] >> 4) & 0x0F;
+        match cic_step {
            0 => Self::Unavailable,
            1 => Self::PowerOff,
            2 => Self::ConfigLoad,
@ -879,49 +995,12 @@ impl TryFrom<u8> for CicStep {
            13 => Self::DieInvalidRegion,
            14 => Self::DieCommand,
            _ => Self::Unknown,
-        })
        }
-}
-
-pub struct PiFifoFlags {
-    pub read_fifo_wait: bool,
-    pub read_fifo_failure: bool,
-    pub write_fifo_wait: bool,
-    pub write_fifo_failure: bool,
-}
-
-impl Display for PiFifoFlags {
-    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
-        let mapping = vec![
-            (self.read_fifo_wait, "Read wait"),
-            (self.read_fifo_failure, "Read failure"),
-            (self.write_fifo_wait, "Write wait"),
-            (self.write_fifo_failure, "Write failure"),
-        ];
-        let filtered: Vec<&str> = mapping.into_iter().filter(|x| x.0).map(|x| x.1).collect();
-        if filtered.len() > 0 {
-            f.write_str(filtered.join(", ").as_str())?;
-        } else {
-            f.write_str("None")?;
-        }
-        Ok(())
-    }
-}
-
-impl TryFrom<u8> for PiFifoFlags {
-    type Error = Error;
-    fn try_from(value: u8) -> Result<Self, Self::Error> {
-        Ok(PiFifoFlags {
-            read_fifo_wait: (value & (1 << 0)) != 0,
-            read_fifo_failure: (value & (1 << 1)) != 0,
-            write_fifo_wait: (value & (1 << 2)) != 0,
-            write_fifo_failure: (value & (1 << 3)) != 0,
-        })
    }
 }

 pub struct FpgaDebugData {
-    pub last_pi_address: u32,
+    pub pi_io_access: PiIOAccess,
    pub pi_fifo_flags: PiFifoFlags,
    pub cic_step: CicStep,
 }
@ -932,10 +1011,11 @@ impl TryFrom<Vec<u8>> for FpgaDebugData {
        if value.len() != 8 {
            return Err(Error::new("Invalid data length for FPGA debug data"));
        }
+        let data: &[u8; 8] = &value[0..8].try_into().unwrap();
        Ok(FpgaDebugData {
-            last_pi_address: u32::from_be_bytes(value[0..4].try_into().unwrap()),
-            pi_fifo_flags: (value[7] & 0x0F).try_into().unwrap(),
-            cic_step: ((value[7] >> 4) & 0x0F).try_into().unwrap(),
+            pi_io_access: data.into(),
+            pi_fifo_flags: data.into(),
+            cic_step: data.into(),
        })
    }
 }
@ -1020,7 +1100,7 @@ impl Display for DiagnosticData {

 pub enum SpeedTestDirection {
    Read,
-    Write
+    Write,
 }

 pub enum MemoryTestPattern {