[SC64][FW][SW] Moved CIC emulation from MCU to FPGA (#56)

.gitattributes

@@ -1,4 +1,5 @@
 fw/project/lcmxo2/*.sty linguist-generated
+fw/rtl/serv/* -linguist-vendored
 fw/rtl/vendor/** -linguist-vendored
 fw/rtl/vendor/lcmxo2/generated/* linguist-generated
 hw/pcb/*.html linguist-generated

README.md

@@ -13,6 +13,7 @@
  - 64DD add-on emulation
  - IS-Viewer 64 debug interface
  - N64 bootloader with support for IPL3 registers spoofing and loading menu from SD card
+ - Dedicated open source menu written specifically for this flashcart - [N64FlashcartMenu](https://github.com/Polprzewodnikowy/N64FlashcartMenu)
  - Enhanced [UltraCIC_C](https://github.com/jago85/UltraCIC_C) emulation with automatic region switching and programmable seed/checksum values
  - PC app for communicating with flashcart (game/save data upload/download, feature enable control and debug terminal)
  - [UNFLoader](https://github.com/buu342/N64-UNFLoader) support
@@ -47,8 +48,15 @@ I'm also active at [N64brew](https://discord.gg/WqFgNWf) Discord server as `korg
 
 One option is to ask in `#summer-cart-64` channel on [N64brew](https://discord.gg/WqFgNWf) Discord server if someone is making a group order.
 
-If you want to order it yourself then I've prepared all necessary manufacturing files on [PCBWay Shared Project](https://www.pcbway.com/project/shareproject/SC64_an_open_source_Nintendo_64_flashcart_14b9688a.html) site.
+If you want to order it yourself then I've prepared all necessary manufacturing files on the [PCBWay Shared Project](https://www.pcbway.com/project/shareproject/SC64_an_open_source_Nintendo_64_flashcart_14b9688a.html) site.
-Full disclosure: for every order made through this link I will receive 10% of PCB manufacturing and PCB assembly service cost. This is a great way of supporting further project development.
+
+**Full disclosure**: for every order made through [this link](https://www.pcbway.com/project/shareproject/SC64_an_open_source_Nintendo_64_flashcart_14b9688a.html) I will receive 10% of PCB manufacturing and PCB assembly service cost (price of the components is not included in the split). This is a great way of supporting further project development.
+
+**Be careful**: this is an advanced project and it is assumed that you have enough knowledge about electronics.
+Selecting wrong options or giving PCB manufacturer wrong information might result in an undesired time and/or money loss.
+Boards also come unprogrammed from the manufacturer - you need to do **initial programming step** yourself after receiving the board.
+To avoid problems _**please**_ read **both** [build guide](./docs/06_build_guide.md) and description on the shared project page **in full**.
+If you have even slightest doubt about the ordering or programming process, it is better to leave it to someone experienced - ask in the Discord server mentioned above if that's the case.
 
 If you don't need a physical product but still want to support me then check my [GitHub sponsors](https://github.com/sponsors/Polprzewodnikowy) page.
 
@@ -63,7 +71,9 @@ If you don't need a physical product but still want to support me then check my
 ## Finished example
 
 [<img src="assets/sc64_finished_example.jpg" alt="SC64 finished example" width="800" />](assets/sc64_finished_example.jpg)
+
 [<img src="assets/sc64_pcb_front.jpg" alt="SC64 PCB front" width="800" />](assets/sc64_pcb_front.jpg)
+
 [<img src="assets/sc64_pcb_back.jpg" alt="SC64 PCB back" width="800" />](assets/sc64_pcb_back.jpg)
 
 ---
@@ -81,3 +91,4 @@ This project wouldn't be possible without these contributions:
 - [FatFs](http://elm-chan.org/fsw/ff/00index_e.html) FAT32/exFAT library being easiest to integrate in embedded environment.
 - [Yakumono's (@LuigiBlood)](https://twitter.com/LuigiBlood) extensive [64DD documentation](https://github.com/LuigiBlood/64dd/wiki) and its implementation in various emulators.
 - [Libdragon](https://github.com/DragonMinded/libdragon) open source N64 SDK project and its developers.
+- [SERV](https://github.com/olofk/serv) bit-serial 32-bit RISC-V CPU soft core.

build.sh

@@ -29,6 +29,7 @@ FILES=(
 
 BUILT_BOOTLOADER=false
 BUILT_CONTROLLER=false
+BUILT_CIC=false
 BUILT_FPGA=false
 BUILT_UPDATE=false
 BUILT_RELEASE=false
@@ -66,6 +67,19 @@ build_controller () {
     BUILT_CONTROLLER=true
 }
 
+build_cic () {
+    if [ "$BUILT_CIC" = true ]; then return; fi
+
+    pushd sw/cic > /dev/null
+    if [ "$FORCE_CLEAN" = true ]; then
+        ./build.sh clean
+    fi
+    ./build.sh all
+    popd > /dev/null
+
+    BUILT_CIC=true
+}
+
 build_fpga () {
     if [ "$BUILT_FPGA" = true ]; then return; fi
 
@@ -84,6 +98,7 @@ build_update () {
 
     build_bootloader
     build_controller
+    build_cic
     build_fpga
 
     pushd sw/tools > /dev/null
@@ -126,10 +141,11 @@ build_release () {
 
 print_usage () {
     echo "builder script for SC64"
-    echo "usage: ./build.sh [bootloader] [controller] [fpga] [update] [release] [-c] [--help]"
+    echo "usage: ./build.sh [bootloader] [controller] [cic] [fpga] [update] [release] [-c] [--help]"
     echo "parameters:"
     echo "  bootloader  - compile N64 bootloader software"
     echo "  controller  - compile MCU controller software"
+    echo "  cic         - compile CIC emulation software"
     echo "  fpga        - compile FPGA design"
     echo "  update      - compile all software and designs"
     echo "  release     - collect and zip files for release (triggers 'update' build)"
@@ -147,6 +163,7 @@ fi
 
 TRIGGER_BOOTLOADER=false
 TRIGGER_CONTROLLER=false
+TRIGGER_CIC=false
 TRIGGER_FPGA=false
 TRIGGER_UPDATE=false
 TRIGGER_RELEASE=false
@@ -159,6 +176,9 @@ while test $# -gt 0; do
         controller)
             TRIGGER_CONTROLLER=true
             ;;
+        cic)
+            TRIGGER_CIC=true
+            ;;
         fpga)
             TRIGGER_FPGA=true
             ;;
@@ -187,6 +207,7 @@ done
 
 if [ "$TRIGGER_BOOTLOADER" = true ]; then build_bootloader; fi
 if [ "$TRIGGER_CONTROLLER" = true ]; then build_controller; fi
+if [ "$TRIGGER_CIC" = true ]; then build_cic; fi
 if [ "$TRIGGER_FPGA" = true ]; then build_fpga; fi
 if [ "$TRIGGER_UPDATE" = true ]; then build_update; fi
 if [ "$TRIGGER_RELEASE" = true ]; then build_release; fi

docker_build.sh

@@ -1,6 +1,6 @@
 #!/bin/bash
 
-BUILDER_IMAGE="ghcr.io/polprzewodnikowy/sc64env:v1.5"
+BUILDER_IMAGE="ghcr.io/polprzewodnikowy/sc64env:v1.8"
 
 pushd $(dirname $0) > /dev/null
 

docs/02_n64_commands.md

@@ -7,11 +7,11 @@
 | id  | name                  | arg0           | arg1         | rsp0             | rsp1           | description                                                |
 | --- | --------------------- | -------------- | ------------ | ---------------- | -------------- | ---------------------------------------------------------- |
 | `v` | **IDENTIFIER_GET**    | ---            | ---          | identifier       | ---            | Get flashcart identifier `SCv2`                            |
-| `V` | **VERSION_GET**       | ---            | ---          | version          | ---            | Get flashcart firmware version                             |
+| `V` | **VERSION_GET**       | ---            | ---          | major/minor      | revision       | Get flashcart firmware version                             |
 | `c` | **CONFIG_GET**        | config_id      | ---          | ---              | current_value  | Get config option                                          |
 | `C` | **CONFIG_SET**        | config_id      | new_value    | ---              | previous_value | Set config option and get previous value                   |
-| `c` | **SETTING_GET**       | setting_id     | ---          | ---              | current_value  | Get persistent setting option                              |
+| `a` | **SETTING_GET**       | setting_id     | ---          | ---              | current_value  | Get persistent setting option                              |
-| `C` | **SETTING_SET**       | setting_id     | new_value    | ---              | ---            | Set persistent setting option                              |
+| `A` | **SETTING_SET**       | setting_id     | new_value    | ---              | ---            | Set persistent setting option                              |
 | `t` | **TIME_GET**          | ---            | ---          | time_0           | time_1         | Get current RTC value                                      |
 | `T` | **TIME_SET**          | time_0         | time_1       | ---              | ---            | Set new RTC value                                          |
 | `m` | **USB_READ**          | pi_address     | length       | ---              | ---            | Receive data from USB to flashcart                         |
@@ -23,7 +23,7 @@
 | `s` | **SD_READ**           | pi_address     | sector_count | ---              | ---            | Read sectors from SD card to flashcart                     |
 | `S` | **SD_WRITE**          | pi_address     | sector_count | ---              | ---            | Write sectors from flashcart to SD card                    |
 | `D` | **DISK_MAPPING_SET**  | pi_address     | table_size   | ---              | ---            | Set 64DD disk mapping for SD mode                          |
-| `w` | **WRITEBACK_PENDING** | pending_status | ---          | ---              | ---            | Get save writeback status (is write queued to the SD card) |
+| `w` | **WRITEBACK_PENDING** | ---            | ---          | pending_status   | ---            | Get save writeback status (is write queued to the SD card) |
 | `W` | **WRITEBACK_SD_INFO** | pi_address     | ---          | ---              | ---            | Load writeback SD sector table and enable it               |
 | `K` | **FLASH_PROGRAM**     | pi_address     | length       | ---              | ---            | Program flash with bytes loaded into data buffer           |
 | `p` | **FLASH_WAIT_BUSY**   | wait           | ---          | erase_block_size | ---            | Wait until flash ready / get block erase size              |

fw/project/lcmxo2/debug.sty

@@ -72,6 +72,7 @@
     <Property name="PROP_LST_RAMStyle" value="Auto" time="0"/>
     <Property name="PROP_LST_ROMStyle" value="Auto" time="0"/>
     <Property name="PROP_LST_RemoveDupRegs" value="True" time="0"/>
+    <Property name="PROP_LST_ReportTrimmedUserNets" value="False" time="0"/>
     <Property name="PROP_LST_ResolvedMixedDrivers" value="False" time="0"/>
     <Property name="PROP_LST_ResourceShare" value="True" time="0"/>
     <Property name="PROP_LST_UseIOReg" value="Auto" time="0"/>

fw/project/lcmxo2/release.sty

@@ -72,6 +72,7 @@
     <Property name="PROP_LST_RAMStyle" value="Auto" time="0"/>
     <Property name="PROP_LST_ROMStyle" value="Auto" time="0"/>
     <Property name="PROP_LST_RemoveDupRegs" value="True" time="0"/>
+    <Property name="PROP_LST_ReportTrimmedUserNets" value="False" time="0"/>
     <Property name="PROP_LST_ResolvedMixedDrivers" value="False" time="0"/>
     <Property name="PROP_LST_ResourceShare" value="True" time="0"/>
     <Property name="PROP_LST_UseIOReg" value="Auto" time="0"/>
@@ -93,7 +94,7 @@
     <Property name="PROP_MAP_MapModArgs" value="" time="0"/>
     <Property name="PROP_MAP_OvermapDevice" value="False" time="0"/>
     <Property name="PROP_MAP_PackLogMapDes" value="" time="0"/>
-    <Property name="PROP_MAP_RegRetiming" value="False" time="0"/>
+    <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"/>
@@ -119,12 +120,12 @@
     <Property name="PROP_PAR_PARModArgs" value="" time="0"/>
     <Property name="PROP_PAR_ParMultiNodeList" value="" time="0"/>
     <Property name="PROP_PAR_ParRunPlaceOnly" value="False" time="0"/>
-    <Property name="PROP_PAR_PlcIterParDes" value="10" time="0"/>
+    <Property name="PROP_PAR_PlcIterParDes" value="16" 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="10" time="0"/>
+    <Property name="PROP_PAR_RoutePassParDes" value="20" time="0"/>
     <Property name="PROP_PAR_RouteResOptParDes" value="0" time="0"/>
     <Property name="PROP_PAR_RoutingCDP" value="1" time="0"/>
     <Property name="PROP_PAR_RoutingCDR" value="1" time="0"/>
@@ -179,8 +180,8 @@
     <Property name="PROP_SYN_EdfNumStartEnd" value="" time="0"/>
     <Property name="PROP_SYN_EdfOutNetForm" value="None" time="0"/>
     <Property name="PROP_SYN_EdfPushTirstates" value="True" time="0"/>
-    <Property name="PROP_SYN_EdfResSharing" value="True" time="0"/>
+    <Property name="PROP_SYN_EdfResSharing" value="False" time="0"/>
-    <Property name="PROP_SYN_EdfRunRetiming" value="Pipelining Only" time="0"/>
+    <Property name="PROP_SYN_EdfRunRetiming" value="Pipelining and Retiming" time="0"/>
     <Property name="PROP_SYN_EdfSymFSM" value="True" time="0"/>
     <Property name="PROP_SYN_EdfUnconsClk" value="False" time="0"/>
     <Property name="PROP_SYN_EdfVerilogInput" value="Verilog 2001" time="0"/>

fw/project/lcmxo2/sc64.ldf

@@ -6,6 +6,12 @@
         <Source name="../../rtl/memory/mem_bus.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
+        <Source name="../../rtl/n64/n64_scb.sv" type="Verilog" type_short="Verilog">
+            <Options VerilogStandard="System Verilog"/>
+        </Source>
+        <Source name="../../rtl/sd/sd_scb.sv" type="Verilog" type_short="Verilog">
+            <Options VerilogStandard="System Verilog"/>
+        </Source>
         <Source name="../../rtl/fifo/fifo_bus.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
@@ -33,9 +39,15 @@
         <Source name="../../rtl/memory/memory_sdram.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
+        <Source name="../../rtl/n64/n64_reg_bus.sv" type="Verilog" type_short="Verilog">
+            <Options VerilogStandard="System Verilog"/>
+        </Source>
         <Source name="../../rtl/n64/n64_cfg.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
+        <Source name="../../rtl/n64/n64_cic.sv" type="Verilog" type_short="Verilog">
+            <Options VerilogStandard="System Verilog"/>
+        </Source>
         <Source name="../../rtl/n64/n64_dd.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
@@ -48,15 +60,9 @@
         <Source name="../../rtl/n64/n64_pi_fifo.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
-        <Source name="../../rtl/n64/n64_reg_bus.sv" type="Verilog" type_short="Verilog">
-            <Options VerilogStandard="System Verilog"/>
-        </Source>
         <Source name="../../rtl/n64/n64_save_counter.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
-        <Source name="../../rtl/n64/n64_scb.sv" type="Verilog" type_short="Verilog">
-            <Options VerilogStandard="System Verilog"/>
-        </Source>
         <Source name="../../rtl/n64/n64_si.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
@@ -78,9 +84,6 @@
         <Source name="../../rtl/sd/sd_dat.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
-        <Source name="../../rtl/sd/sd_scb.sv" type="Verilog" type_short="Verilog">
-            <Options VerilogStandard="System Verilog"/>
-        </Source>
         <Source name="../../rtl/sd/sd_top.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog"/>
         </Source>
@@ -108,6 +111,54 @@
         <Source name="../../rtl/vendor/lcmxo2/generated/pll_lattice_generated.v" type="Verilog" type_short="Verilog">
             <Options/>
         </Source>
+        <Source name="../../rtl/serv/serv_aligner.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_alu.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_bufreg.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_bufreg2.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_compdec.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_csr.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_ctrl.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_decode.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_immdec.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_mem_if.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_rf_if.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_rf_ram.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_rf_ram_if.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_rf_top.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_state.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
+        <Source name="../../rtl/serv/serv_top.v" type="Verilog" type_short="Verilog">
+            <Options/>
+        </Source>
         <Source name="../../rtl/top.sv" type="Verilog" type_short="Verilog">
             <Options VerilogStandard="System Verilog" top_module="top"/>
         </Source>

fw/rtl/mcu/mcu_top.sv

@@ -358,7 +358,9 @@ module mcu_top (
         REG_VENDOR_SCR,
         REG_VENDOR_DATA,
         REG_DEBUG_0,
-        REG_DEBUG_1
+        REG_DEBUG_1,
+        REG_CIC_0,
+        REG_CIC_1
     } reg_address_e;
 
     logic bootloader_skip;
@@ -368,6 +370,8 @@ module mcu_top (
 
     logic dd_bm_ack;
 
+    logic cic_invalid_region;
+
 
     // Register read logic
 
@@ -649,6 +653,22 @@ module mcu_top (
                         n64_scb.pi_debug[35:32]
                     };
                 end
+
+                REG_CIC_0: begin
+                    reg_rdata <= {
+                        4'd0,
+                        cic_invalid_region,
+                        n64_scb.cic_disabled,
+                        n64_scb.cic_64dd_mode,
+                        n64_scb.cic_region,
+                        n64_scb.cic_seed,
+                        n64_scb.cic_checksum[47:32]
+                    };
+                end
+
+                REG_CIC_1: begin
+                    reg_rdata <= n64_scb.cic_checksum[31:0];
+                end
             endcase
         end
     end
@@ -705,6 +725,10 @@ module mcu_top (
             dd_bm_ack <= 1'b1;
         end
 
+        if (n64_scb.cic_invalid_region) begin
+            cic_invalid_region <= 1'b1;
+        end
+
         if (reset) begin
             mcu_int <= 1'b0;
             sd_scb.clock_mode <= 2'd0;
@@ -723,6 +747,12 @@ module mcu_top (
             flash_scb.erase_pending <= 1'b0;
             dd_bm_ack <= 1'b0;
             n64_scb.rtc_wdata_valid <= 1'b0;
+            cic_invalid_region <= 1'b0;
+            n64_scb.cic_disabled <= 1'b0;
+            n64_scb.cic_64dd_mode <= 1'b0;
+            n64_scb.cic_region <= 1'b0;
+            n64_scb.cic_seed <= 8'h3F;
+            n64_scb.cic_checksum <= 48'hA536C0F1D859;
         end else if (reg_write) begin
             case (address)
                 REG_MEM_ADDRESS: begin
@@ -900,6 +930,21 @@ module mcu_top (
                 REG_VENDOR_DATA: begin
                     vendor_scb.data_wdata <= reg_wdata;
                 end
+
+                REG_CIC_0: begin
+                    if (reg_wdata[28]) begin
+                        cic_invalid_region <= 1'b0;
+                    end
+                    n64_scb.cic_disabled <= reg_wdata[26];
+                    n64_scb.cic_64dd_mode <= reg_wdata[25];
+                    n64_scb.cic_region <= reg_wdata[24];
+                    n64_scb.cic_seed <= reg_wdata[23:16];
+                    n64_scb.cic_checksum[47:32] <= reg_wdata[15:0];
+                end
+
+                REG_CIC_1: begin
+                    n64_scb.cic_checksum[31:0] <= reg_wdata;
+                end
             endcase
         end
     end

fw/rtl/n64/n64_cic.sv

@@ -0,0 +1,168 @@
+module n64_cic (
+    input clk,
+    input reset,
+
+    n64_scb.cic n64_scb,
+
+    input n64_reset,
+    input n64_cic_clk,
+    inout n64_cic_dq
+);
+
+    // Input/output synchronization
+
+    logic [1:0] n64_reset_ff;
+    logic [1:0] n64_cic_clk_ff;
+    logic [1:0] n64_cic_dq_ff;
+
+    always_ff @(posedge clk) begin
+        n64_reset_ff <= {n64_reset_ff[0], n64_reset};
+        n64_cic_clk_ff <= {n64_cic_clk_ff[0], n64_cic_clk};
+        n64_cic_dq_ff <= {n64_cic_dq_ff[0], n64_cic_dq};
+    end
+
+    logic cic_reset;
+    logic cic_clk;
+    logic cic_dq;
+
+    always_comb begin
+        cic_reset = n64_reset_ff[1];
+        cic_clk = n64_cic_clk_ff[1];
+        cic_dq = n64_cic_dq_ff[1];
+    end
+
+    logic cic_dq_out;
+
+    assign n64_cic_dq = cic_dq_out ? 1'bZ : 1'b0;
+
+
+    // SERV RISC-V CPU
+
+    logic [31:0] ibus_addr;
+    logic ibus_cycle;
+    logic [31:0] ibus_rdata;
+    logic ibus_ack;
+
+    logic [31:0] dbus_addr;
+    logic [31:0] dbus_wdata;
+    logic [3:0] dbus_wmask;
+    logic dbus_write;
+    logic dbus_cycle;
+    logic [31:0] dbus_rdata;
+    logic dbus_ack;
+
+    logic [31:0] ext_rs1;
+    logic [31:0] ext_rs2;
+    logic [2:0] ext_funct3;
+    logic mdu_valid;
+
+    serv_rf_top #(
+        .RESET_PC(32'h8000_0000),
+        .PRE_REGISTER(0),
+        .WITH_CSR(0)
+    ) serv_rf_top_inst (
+        .clk(clk),
+        .i_rst(reset),
+        .i_timer_irq(1'b0),
+
+        .o_ibus_adr(ibus_addr),
+        .o_ibus_cyc(ibus_cycle),
+        .i_ibus_rdt(ibus_rdata),
+        .i_ibus_ack(ibus_ack),
+
+        .o_dbus_adr(dbus_addr),
+        .o_dbus_dat(dbus_wdata),
+        .o_dbus_sel(dbus_wmask),
+        .o_dbus_we(dbus_write) ,
+        .o_dbus_cyc(dbus_cycle),
+        .i_dbus_rdt(dbus_rdata),
+        .i_dbus_ack(dbus_ack),
+
+        .o_ext_rs1(ext_rs1),
+        .o_ext_rs2(ext_rs2),
+        .o_ext_funct3(ext_funct3),
+        .i_ext_rd(32'd0),
+        .i_ext_ready(1'b0),
+        .o_mdu_valid(mdu_valid)
+    );
+
+
+    // CPU memory
+
+    logic [8:0] ram_addr;
+    logic [31:0] ram [0:511];
+    logic [31:0] ram_output;
+
+    assign ram_addr = ibus_cycle ? ibus_addr[10:2] : dbus_addr[10:2];
+    assign ibus_rdata = ram_output;
+
+    always_ff @(posedge clk) begin
+        ram_output <= ram[ram_addr];
+
+        ibus_ack <= ibus_cycle && !ibus_ack;
+    end
+
+    initial begin
+        $readmemh("../../../sw/cic/build/cic.mem", ram);
+    end
+
+
+    // Bus controller
+
+    always_ff @(posedge clk) begin
+        n64_scb.cic_invalid_region <= 1'b0;
+
+        dbus_ack <= dbus_cycle && !dbus_ack;
+
+        if (dbus_cycle && dbus_write) begin
+            case (dbus_addr[31:30])
+                2'b10: begin
+                    if (dbus_wmask[0]) ram[ram_addr][7:0] <= dbus_wdata[7:0];
+                    if (dbus_wmask[1]) ram[ram_addr][15:8] <= dbus_wdata[15:8];
+                    if (dbus_wmask[2]) ram[ram_addr][23:16] <= dbus_wdata[23:16];
+                    if (dbus_wmask[3]) ram[ram_addr][31:24] <= dbus_wdata[31:24];
+                end
+
+                2'b11: begin
+                    case (dbus_addr[3:2])
+                        2'b10: begin
+                            n64_scb.cic_invalid_region <= dbus_wdata[3];
+                            cic_dq_out <= dbus_wdata[0];
+                        end
+                    endcase
+                end
+            endcase
+        end
+
+        if (reset || !cic_reset) begin
+            cic_dq_out <= 1'b1;
+        end
+    end
+
+    always_comb begin
+        dbus_rdata = 32'd0;
+
+        case (dbus_addr[31:30])
+            2'b10: begin
+                dbus_rdata = ram_output;
+            end
+
+            2'b11: begin
+                case (dbus_addr[3:2])
+                    2'b00: dbus_rdata = {
+                        n64_scb.cic_disabled,
+                        n64_scb.cic_64dd_mode,
+                        n64_scb.cic_region,
+                        n64_scb.cic_seed,
+                        n64_scb.cic_checksum[47:32]
+                    };
+
+                    2'b01: dbus_rdata = n64_scb.cic_checksum[31:0];
+
+                    2'b10: dbus_rdata = {29'd0, cic_reset, cic_clk, cic_dq};
+                endcase
+            end
+        endcase
+    end
+
+endmodule

fw/rtl/n64/n64_pi.sv

@@ -20,16 +20,16 @@ module n64_pi (
 
     logic [1:0] n64_reset_ff;
     logic [1:0] n64_nmi_ff;
-    logic [3:0] n64_pi_alel_ff;
+    logic [2:0] n64_pi_alel_ff;
-    logic [3:0] n64_pi_aleh_ff;
+    logic [2:0] n64_pi_aleh_ff;
     logic [1:0] n64_pi_read_ff;
     logic [2:0] n64_pi_write_ff;
 
     always_ff @(posedge clk) begin
         n64_reset_ff <= {n64_reset_ff[0], n64_reset};
         n64_nmi_ff <= {n64_nmi_ff[0], n64_nmi};
-        n64_pi_aleh_ff <= {n64_pi_aleh_ff[2:0], n64_pi_aleh};
+        n64_pi_aleh_ff <= {n64_pi_aleh_ff[1:0], n64_pi_aleh};
-        n64_pi_alel_ff <= {n64_pi_alel_ff[2:0], n64_pi_alel};
+        n64_pi_alel_ff <= {n64_pi_alel_ff[1:0], n64_pi_alel};
         n64_pi_read_ff <= {n64_pi_read_ff[0], n64_pi_read};
         n64_pi_write_ff <= {n64_pi_write_ff[1:0], n64_pi_write};
     end
@@ -44,8 +44,8 @@ module n64_pi (
     always_comb begin
         pi_reset = n64_reset_ff[1];
         pi_nmi = n64_nmi_ff[1];
-        pi_aleh = n64_pi_aleh_ff[3];
+        pi_aleh = n64_pi_aleh_ff[2];
-        pi_alel = n64_pi_alel_ff[3];
+        pi_alel = n64_pi_alel_ff[2];
         pi_read = n64_pi_read_ff[1];
         pi_write = n64_pi_write_ff[2];
     end
@@ -98,11 +98,14 @@ module n64_pi (
     always_comb begin
         n64_scb.n64_reset = !last_reset && pi_reset;
         n64_scb.n64_nmi = !last_nmi && pi_nmi;
-        aleh_op = pi_reset && (last_pi_mode != PI_MODE_HIGH) && (pi_mode == PI_MODE_HIGH);
+    end
-        alel_op = pi_reset && (last_pi_mode == PI_MODE_HIGH) && (pi_mode == PI_MODE_LOW);
+
-        read_op = pi_reset && (pi_mode == PI_MODE_VALID) && (read_port != PORT_NONE) && (last_read && !pi_read);
+    always_ff @(posedge clk) begin
-        write_op = pi_reset && (pi_mode == PI_MODE_VALID) && (write_port != PORT_NONE) && (last_write && !pi_write);
+        aleh_op <= pi_reset && (last_pi_mode != PI_MODE_HIGH) && (pi_mode == PI_MODE_HIGH);
-        end_op = pi_reset && (last_pi_mode == PI_MODE_VALID) && (pi_mode != PI_MODE_VALID);
+        alel_op <= pi_reset && (last_pi_mode == PI_MODE_HIGH) && (pi_mode == PI_MODE_LOW);
+        read_op <= pi_reset && (pi_mode == PI_MODE_VALID) && (read_port != PORT_NONE) && (last_read && !pi_read);
+        write_op <= pi_reset && (pi_mode == PI_MODE_VALID) && (write_port != PORT_NONE) && (last_write && !pi_write);
+        end_op <= pi_reset && (last_pi_mode == PI_MODE_VALID) && (pi_mode != PI_MODE_VALID);
     end
 
 

fw/rtl/n64/n64_scb.sv

@@ -55,6 +55,13 @@ interface n64_scb ();
 
     logic [15:0] save_count;
 
+    logic cic_invalid_region;
+    logic cic_disabled;
+    logic cic_64dd_mode;
+    logic cic_region;
+    logic [7:0] cic_seed;
+    logic [47:0] cic_checksum;
+
     logic pi_sdram_active;
     logic pi_flash_active;
     logic [35:0] pi_debug;
@@ -98,6 +105,13 @@ interface n64_scb ();
 
         input save_count,
 
+        input cic_invalid_region,
+        output cic_disabled,
+        output cic_64dd_mode,
+        output cic_region,
+        output cic_seed,
+        output cic_checksum,
+
         input pi_debug
     );
 
@@ -205,6 +219,15 @@ interface n64_scb ();
         output save_count
     );
 
+    modport cic (
+        output cic_invalid_region,
+        input cic_disabled,
+        input cic_64dd_mode,
+        input cic_region,
+        input cic_seed,
+        input cic_checksum
+    );
+
     modport arbiter (
         input pi_sdram_active,
         input pi_flash_active

fw/rtl/n64/n64_top.sv

@@ -18,7 +18,10 @@ module n64_top (
     inout [15:0] n64_pi_ad,
 
     input n64_si_clk,
-    inout n64_si_dq
+    inout n64_si_dq,
+
+    input n64_cic_clk,
+    inout n64_cic_dq
 );
 
     logic n64_dd_irq;
@@ -101,4 +104,15 @@ module n64_top (
         .n64_scb(n64_scb)
     );
 
+    n64_cic n64_cic_inst (
+        .clk(clk),
+        .reset(reset),
+
+        .n64_scb(n64_scb),
+
+        .n64_reset(n64_reset),
+        .n64_cic_clk(n64_cic_clk),
+        .n64_cic_dq(n64_cic_dq)
+    );
+
 endmodule

fw/rtl/serv/serv_aligner.v

@@ -0,0 +1,67 @@
+module serv_aligner 
+   (
+    input wire clk,
+    input wire rst,
+    // serv_top
+    input  wire [31:0]  i_ibus_adr,
+    input  wire         i_ibus_cyc,
+    output wire [31:0]  o_ibus_rdt,
+    output wire         o_ibus_ack,
+    // serv_rf_top
+    output wire [31:0]  o_wb_ibus_adr,
+    output wire         o_wb_ibus_cyc,
+    input  wire [31:0]  i_wb_ibus_rdt,
+    input  wire         i_wb_ibus_ack);
+
+    wire [31:0] ibus_rdt_concat;
+    wire        ack_en;
+    
+    reg  [15:0] lower_hw;    
+    reg         ctrl_misal ; 
+
+    /* From SERV core to Memory
+
+    o_wb_ibus_adr: Carries address of instruction to memory. In case of misaligned access, 
+    which is caused by pc+2 due to compressed instruction, next instruction is fetched 
+    by pc+4 and concatenation is done to make the instruction aligned.
+
+    o_wb_ibus_cyc: Simply forwarded from SERV to Memory and is only altered by memory or SERV core.
+    */
+    assign o_wb_ibus_adr = ctrl_misal ? (i_ibus_adr+32'b100) : i_ibus_adr;
+    assign o_wb_ibus_cyc = i_ibus_cyc;
+
+    /* From Memory to SERV core
+
+        o_ibus_ack: Instruction bus acknowledge is send to SERV only when the aligned instruction,
+        either compressed or un-compressed, is ready to dispatch.
+
+        o_ibus_rdt: Carries the instruction from memory to SERV core. It can be either aligned
+        instruction coming from memory or made aligned by two bus transactions and concatenation.
+    */
+    assign o_ibus_ack = i_wb_ibus_ack & ack_en;
+    assign o_ibus_rdt = ctrl_misal ? ibus_rdt_concat : i_wb_ibus_rdt;
+
+    /* 16-bit register used to hold the upper half word of the current instruction in-case
+       concatenation will be required with the upper half word of upcoming instruction
+    */        
+    always @(posedge clk) begin
+        if(i_wb_ibus_ack)begin
+            lower_hw <= i_wb_ibus_rdt[31:16];
+        end
+    end
+
+    assign ibus_rdt_concat = {i_wb_ibus_rdt[15:0],lower_hw};
+    
+    /* Two control signals: ack_en, ctrl_misal are set to control the bus transactions between
+    SERV core and the memory
+    */
+    assign ack_en   = !(i_ibus_adr[1] & !ctrl_misal); 
+
+    always @(posedge clk ) begin
+        if(rst)
+            ctrl_misal <= 0;
+        else if(i_wb_ibus_ack & i_ibus_adr[1])
+            ctrl_misal <= !ctrl_misal;
+    end
+
+endmodule

fw/rtl/serv/serv_alu.v

@@ -0,0 +1,81 @@
+`default_nettype none
+module serv_alu
+  #(
+   parameter W = 1,
+   parameter B = W-1
+  )
+  (
+   input wire 	    clk,
+   //State
+   input wire 	    i_en,
+   input wire 	    i_cnt0,
+   output wire 	    o_cmp,
+   //Control
+   input wire 	    i_sub,
+   input wire [1:0] i_bool_op,
+   input wire 	    i_cmp_eq,
+   input wire 	    i_cmp_sig,
+   input wire [2:0] i_rd_sel,
+   //Data
+   input wire  [B:0] i_rs1,
+   input wire  [B:0] i_op_b,
+   input wire  [B:0] i_buf,
+   output wire [B:0] o_rd);
+
+   wire [B:0]  result_add;
+   wire [B:0]  result_slt;
+
+   reg 	       cmp_r;
+
+   wire        add_cy;
+   reg [B:0]   add_cy_r;
+
+   //Sign-extended operands
+   wire rs1_sx  = i_rs1[B] & i_cmp_sig;
+   wire op_b_sx = i_op_b[B]  & i_cmp_sig;
+
+   wire [B:0] add_b = i_op_b^{W{i_sub}};
+
+   assign {add_cy,result_add}   = i_rs1+add_b+add_cy_r;
+
+   wire result_lt = rs1_sx + ~op_b_sx + add_cy;
+
+   wire result_eq = !(|result_add) & (cmp_r | i_cnt0);
+
+   assign o_cmp = i_cmp_eq ? result_eq : result_lt;
+
+   /*
+    The result_bool expression implements the following operations between
+    i_rs1 and i_op_b depending on the value of i_bool_op
+
+    00 xor
+    01 0
+    10 or
+    11 and
+
+    i_bool_op will be 01 during shift operations, so by outputting zero under
+    this condition we can safely or result_bool with i_buf
+    */
+   wire [B:0] result_bool = ((i_rs1 ^ i_op_b) & ~{W{i_bool_op[0]}}) | ({W{i_bool_op[1]}} & i_op_b & i_rs1);
+
+   assign result_slt[0] = cmp_r & i_cnt0;
+   generate
+      if (W>1) begin : gen_w_gt_1
+	 assign result_slt[B:1] = {B{1'b0}};
+      end
+   endgenerate
+
+   assign o_rd = i_buf |
+                 ({W{i_rd_sel[0]}} & result_add) |
+                 ({W{i_rd_sel[1]}} & result_slt) |
+                 ({W{i_rd_sel[2]}} & result_bool);
+
+   always @(posedge clk) begin
+      add_cy_r    <= {W{1'b0}};
+      add_cy_r[0] <= i_en ? add_cy : i_sub;
+
+      if (i_en)
+	cmp_r <= o_cmp;
+   end
+
+endmodule

fw/rtl/serv/serv_bufreg.v

@@ -0,0 +1,51 @@
+module serv_bufreg #(
+      parameter [0:0] MDU = 0
+)(
+   input wire 	      i_clk,
+   //State
+   input wire 	      i_cnt0,
+   input wire 	      i_cnt1,
+   input wire 	      i_en,
+   input wire 	      i_init,
+   input wire           i_mdu_op,
+   output wire [1:0]    o_lsb,
+   //Control
+   input wire 	      i_rs1_en,
+   input wire 	      i_imm_en,
+   input wire 	      i_clr_lsb,
+   input wire 	      i_sh_signed, 
+   //Data
+   input wire 	      i_rs1,
+   input wire 	      i_imm,
+   output wire 	      o_q,
+   //External
+   output wire [31:0] o_dbus_adr,
+   //Extension
+   output wire [31:0] o_ext_rs1);
+
+   wire 	      c, q;
+   reg 		      c_r;
+   reg [31:2] 	      data;
+   reg [1:0]            lsb;
+
+   wire 	      clr_lsb = i_cnt0 & i_clr_lsb;
+
+   assign {c,q} = {1'b0,(i_rs1 & i_rs1_en)} + {1'b0,(i_imm & i_imm_en & !clr_lsb)} + c_r;
+
+   always @(posedge i_clk) begin
+      //Make sure carry is cleared before loading new data
+      c_r <= c & i_en;
+
+      if (i_en)
+	data <= {i_init ? q : (data[31] & i_sh_signed), data[31:3]};
+
+      if (i_init ? (i_cnt0 | i_cnt1) : i_en)
+	lsb <= {i_init ? q : data[2],lsb[1]};
+   end
+
+   assign o_q = lsb[0] & i_en;
+   assign o_dbus_adr = {data, 2'b00};
+   assign o_ext_rs1  = {o_dbus_adr[31:2],lsb};
+   assign o_lsb = (MDU & i_mdu_op) ? 2'b00 : lsb;
+
+endmodule

fw/rtl/serv/serv_bufreg2.v

@@ -0,0 +1,65 @@
+module serv_bufreg2
+  (
+   input wire 	      i_clk,
+   //State
+   input wire 	      i_en,
+   input wire 	      i_init,
+   input wire 	      i_cnt_done,
+   input wire [1:0]   i_lsb,
+   input wire 	      i_byte_valid,
+   output wire 	      o_sh_done,
+   output wire 	      o_sh_done_r,
+   //Control
+   input wire 	      i_op_b_sel,
+   input wire 	      i_shift_op,
+   //Data
+   input wire 	      i_rs2,
+   input wire 	      i_imm,
+   output wire 	      o_op_b,
+   output wire 	      o_q,
+   //External
+   output wire [31:0] o_dat,
+   input wire 	      i_load,
+   input wire [31:0]  i_dat);
+
+   reg [31:0] 	 dat;
+
+   assign o_op_b = i_op_b_sel ? i_rs2 : i_imm;
+
+   wire 	 dat_en = i_shift_op | (i_en & i_byte_valid);
+
+   /* The dat register has three different use cases for store, load and
+    shift operations.
+    store : Data to be written is shifted to the correct position in dat during
+            init by dat_en and is presented on the data bus as o_wb_dat
+    load  : Data from the bus gets latched into dat during i_wb_ack and is then
+            shifted out at the appropriate time to end up in the correct
+            position in rd
+    shift : Data is shifted in during init. After that, the six LSB are used as
+            a downcounter (with bit 5 initially set to 0) that triggers
+            o_sh_done and o_sh_done_r when they wrap around to indicate that
+            the requested number of shifts have been performed
+    */
+   wire [5:0] dat_shamt = (i_shift_op & !i_init) ?
+	      //Down counter mode
+	      dat[5:0]-1 :
+	      //Shift reg mode with optional clearing of bit 5
+	      {dat[6] & !(i_shift_op & i_cnt_done),dat[5:1]};
+
+   assign o_sh_done = dat_shamt[5];
+   assign o_sh_done_r = dat[5];
+
+   assign o_q =
+	       ((i_lsb == 2'd3) & dat[24]) |
+	       ((i_lsb == 2'd2) & dat[16]) |
+	       ((i_lsb == 2'd1) & dat[8]) |
+	       ((i_lsb == 2'd0) & dat[0]);
+
+   assign o_dat = dat;
+
+   always @(posedge i_clk) begin
+      if (dat_en | i_load)
+	dat <= i_load ? i_dat : {o_op_b, dat[31:7], dat_shamt};
+   end
+
+endmodule

fw/rtl/serv/serv_compdec.v

@@ -0,0 +1,234 @@
+/* Copyright lowRISC contributors.
+Copyright 2018 ETH Zurich and University of Bologna, see also CREDITS.md.
+Licensed under the Apache License, Version 2.0, see LICENSE for details.
+SPDX-License-Identifier: Apache-2.0 
+
+* Adapted to SERV by @Abdulwadoodd as part of the project under spring '22 LFX Mentorship program */
+
+/* Decodes RISC-V compressed instructions into their RV32i equivalent. */
+
+module serv_compdec 
+  (
+   input wire i_clk,
+   input  wire [31:0] i_instr,
+   input  wire i_ack,
+   output wire [31:0] o_instr,
+   output reg o_iscomp);
+
+  localparam OPCODE_LOAD     = 7'h03;
+  localparam OPCODE_OP_IMM   = 7'h13;
+  localparam OPCODE_STORE    = 7'h23;
+  localparam OPCODE_OP       = 7'h33;
+  localparam OPCODE_LUI      = 7'h37;
+  localparam OPCODE_BRANCH   = 7'h63;
+  localparam OPCODE_JALR     = 7'h67;
+  localparam OPCODE_JAL      = 7'h6f;
+
+  reg  [31:0] comp_instr;
+  reg  illegal_instr;
+
+  assign o_instr = illegal_instr ? i_instr : comp_instr;
+
+  always @(posedge i_clk) begin
+    if(i_ack)
+      o_iscomp <= !illegal_instr; 
+  end
+
+  always @ (*) begin
+    // By default, forward incoming instruction, mark it as legal.
+    comp_instr    = i_instr;
+    illegal_instr = 1'b0;
+
+    // Check if incoming instruction is compressed.
+    case (i_instr[1:0])
+      // C0
+      2'b00: begin
+        case (i_instr[15:14])
+          2'b00: begin
+            // c.addi4spn -> addi rd', x2, imm
+            comp_instr = {2'b0, i_instr[10:7], i_instr[12:11], i_instr[5],
+                      i_instr[6], 2'b00, 5'h02, 3'b000, 2'b01, i_instr[4:2], {OPCODE_OP_IMM}};
+          end
+
+          2'b01: begin
+            // c.lw -> lw rd', imm(rs1')
+            comp_instr = {5'b0, i_instr[5], i_instr[12:10], i_instr[6],
+                      2'b00, 2'b01, i_instr[9:7], 3'b010, 2'b01, i_instr[4:2], {OPCODE_LOAD}};
+          end
+
+          2'b11: begin
+            // c.sw -> sw rs2', imm(rs1')
+            comp_instr = {5'b0, i_instr[5], i_instr[12], 2'b01, i_instr[4:2],
+                      2'b01, i_instr[9:7], 3'b010, i_instr[11:10], i_instr[6],
+                      2'b00, {OPCODE_STORE}};
+          end
+
+          2'b10: begin
+            illegal_instr = 1'b1;
+          end
+
+        endcase
+      end
+
+      // C1
+ 
+      // Register address checks for RV32E are performed in the regular instruction decoder.
+      // If this check fails, an illegal instruction exception is triggered and the controller
+      // writes the actual faulting instruction to mtval.
+      2'b01: begin
+        case (i_instr[15:13])
+          3'b000: begin
+            // c.addi -> addi rd, rd, nzimm
+            // c.nop
+            comp_instr = {{6 {i_instr[12]}}, i_instr[12], i_instr[6:2],
+                      i_instr[11:7], 3'b0, i_instr[11:7], {OPCODE_OP_IMM}};
+          end
+
+          3'b001, 3'b101: begin
+            // 001: c.jal -> jal x1, imm
+            // 101: c.j   -> jal x0, imm
+            comp_instr = {i_instr[12], i_instr[8], i_instr[10:9], i_instr[6],
+                      i_instr[7], i_instr[2], i_instr[11], i_instr[5:3],
+                      {9 {i_instr[12]}}, 4'b0, ~i_instr[15], {OPCODE_JAL}};
+          end
+
+          3'b010: begin
+            // c.li -> addi rd, x0, nzimm
+            // (c.li hints are translated into an addi hint)
+            comp_instr = {{6 {i_instr[12]}}, i_instr[12], i_instr[6:2], 5'b0,
+                      3'b0, i_instr[11:7], {OPCODE_OP_IMM}};
+          end
+
+          3'b011: begin
+            // c.lui -> lui rd, imm
+            // (c.lui hints are translated into a lui hint)
+            comp_instr = {{15 {i_instr[12]}}, i_instr[6:2], i_instr[11:7], {OPCODE_LUI}};
+
+            if (i_instr[11:7] == 5'h02) begin
+              // c.addi16sp -> addi x2, x2, nzimm
+              comp_instr = {{3 {i_instr[12]}}, i_instr[4:3], i_instr[5], i_instr[2],
+                        i_instr[6], 4'b0, 5'h02, 3'b000, 5'h02, {OPCODE_OP_IMM}};
+            end
+
+          end
+
+          3'b100: begin
+            case (i_instr[11:10])
+              2'b00,
+              2'b01: begin
+                // 00: c.srli -> srli rd, rd, shamt
+                // 01: c.srai -> srai rd, rd, shamt
+                // (c.srli/c.srai hints are translated into a srli/srai hint)
+                comp_instr = {1'b0, i_instr[10], 5'b0, i_instr[6:2], 2'b01, i_instr[9:7],
+                          3'b101, 2'b01, i_instr[9:7], {OPCODE_OP_IMM}};
+              end
+
+              2'b10: begin
+                // c.andi -> andi rd, rd, imm
+                comp_instr = {{6 {i_instr[12]}}, i_instr[12], i_instr[6:2], 2'b01, i_instr[9:7],
+                          3'b111, 2'b01, i_instr[9:7], {OPCODE_OP_IMM}};
+              end
+
+              2'b11: begin
+                case (i_instr[6:5])
+                  2'b00: begin
+                    // c.sub -> sub rd', rd', rs2'
+                    comp_instr = {2'b01, 5'b0, 2'b01, i_instr[4:2], 2'b01, i_instr[9:7],
+                                  3'b000, 2'b01, i_instr[9:7], {OPCODE_OP}};
+                  end
+
+                  2'b01: begin
+                    // c.xor -> xor rd', rd', rs2'
+                    comp_instr = {7'b0, 2'b01, i_instr[4:2], 2'b01, i_instr[9:7], 3'b100,
+                              2'b01, i_instr[9:7], {OPCODE_OP}};
+                  end
+
+                  2'b10: begin
+                    // c.or  -> or  rd', rd', rs2'
+                    comp_instr = {7'b0, 2'b01, i_instr[4:2], 2'b01, i_instr[9:7], 3'b110,
+                              2'b01, i_instr[9:7], {OPCODE_OP}};
+                  end
+
+                  2'b11: begin
+                    // c.and -> and rd', rd', rs2'
+                    comp_instr = {7'b0, 2'b01, i_instr[4:2], 2'b01, i_instr[9:7], 3'b111,
+                              2'b01, i_instr[9:7], {OPCODE_OP}};
+                  end
+                endcase
+              end
+            endcase
+          end
+
+          3'b110, 3'b111: begin
+            // 0: c.beqz -> beq rs1', x0, imm
+            // 1: c.bnez -> bne rs1', x0, imm
+            comp_instr = {{4 {i_instr[12]}}, i_instr[6:5], i_instr[2], 5'b0, 2'b01,
+                      i_instr[9:7], 2'b00, i_instr[13], i_instr[11:10], i_instr[4:3],
+                      i_instr[12], {OPCODE_BRANCH}};
+          end
+        endcase
+      end
+
+      // C2
+
+      // Register address checks for RV32E are performed in the regular instruction decoder.
+      // If this check fails, an illegal instruction exception is triggered and the controller
+      // writes the actual faulting instruction to mtval.
+      2'b10: begin
+        case (i_instr[15:14])
+          2'b00: begin
+            // c.slli -> slli rd, rd, shamt
+            // (c.ssli hints are translated into a slli hint)
+            comp_instr = {7'b0, i_instr[6:2], i_instr[11:7], 3'b001, i_instr[11:7], {OPCODE_OP_IMM}};
+          end
+
+          2'b01: begin
+            // c.lwsp -> lw rd, imm(x2)
+            comp_instr = {4'b0, i_instr[3:2], i_instr[12], i_instr[6:4], 2'b00, 5'h02,
+                      3'b010, i_instr[11:7], OPCODE_LOAD};
+          end
+
+          2'b10: begin
+            if (i_instr[12] == 1'b0) begin
+              if (i_instr[6:2] != 5'b0) begin
+                // c.mv -> add rd/rs1, x0, rs2
+                // (c.mv hints are translated into an add hint)
+                comp_instr = {7'b0, i_instr[6:2], 5'b0, 3'b0, i_instr[11:7], {OPCODE_OP}};
+              end else begin
+                // c.jr -> jalr x0, rd/rs1, 0
+                comp_instr = {12'b0, i_instr[11:7], 3'b0, 5'b0, {OPCODE_JALR}};
+              end
+            end else begin
+              if (i_instr[6:2] != 5'b0) begin
+                // c.add -> add rd, rd, rs2
+                // (c.add hints are translated into an add hint)
+                comp_instr = {7'b0, i_instr[6:2], i_instr[11:7], 3'b0, i_instr[11:7], {OPCODE_OP}};
+              end else begin
+                if (i_instr[11:7] == 5'b0) begin
+                  // c.ebreak -> ebreak
+                  comp_instr = {32'h00_10_00_73};
+                end else begin
+                  // c.jalr -> jalr x1, rs1, 0
+                  comp_instr = {12'b0, i_instr[11:7], 3'b000, 5'b00001, {OPCODE_JALR}};
+                end
+              end
+            end
+          end
+
+          2'b11: begin
+            // c.swsp -> sw rs2, imm(x2)
+            comp_instr = {4'b0, i_instr[8:7], i_instr[12], i_instr[6:2], 5'h02, 3'b010,
+                      i_instr[11:9], 2'b00, {OPCODE_STORE}};
+          end
+        endcase
+      end
+
+      // Incoming instruction is not compressed.
+      2'b11: illegal_instr = 1'b1;
+
+    endcase
+  end
+  
+  endmodule
+
+

fw/rtl/serv/serv_csr.v

@@ -0,0 +1,142 @@
+`default_nettype none
+module serv_csr
+  #(parameter RESET_STRATEGY = "MINI")
+  (
+   input wire 	    i_clk,
+   input wire 	    i_rst,
+   //State
+   input wire 	    i_trig_irq,
+   input wire 	    i_en,
+   input wire 	    i_cnt0to3,
+   input wire 	    i_cnt3,
+   input wire 	    i_cnt7,
+   input wire 	    i_cnt_done,
+   input wire 	    i_mem_op,
+   input wire 	    i_mtip,
+   input wire 	    i_trap,
+   output reg 	    o_new_irq,
+   //Control
+   input wire 	    i_e_op,
+   input wire 	    i_ebreak,
+   input wire 	    i_mem_cmd,
+   input wire 	    i_mstatus_en,
+   input wire 	    i_mie_en,
+   input wire 	    i_mcause_en,
+   input wire [1:0] i_csr_source,
+   input wire 	    i_mret,
+   input wire 	    i_csr_d_sel,
+   //Data
+   input wire 	    i_rf_csr_out,
+   output wire 	    o_csr_in,
+   input wire 	    i_csr_imm,
+   input wire 	    i_rs1,
+   output wire 	    o_q);
+
+   localparam [1:0]
+     CSR_SOURCE_CSR = 2'b00,
+     CSR_SOURCE_EXT = 2'b01,
+     CSR_SOURCE_SET = 2'b10,
+     CSR_SOURCE_CLR = 2'b11;
+
+   reg 		    mstatus_mie;
+   reg 		    mstatus_mpie;
+   reg 		    mie_mtie;
+
+   reg 		mcause31;
+   reg [3:0] 	mcause3_0;
+   wire 	mcause;
+
+   wire 	csr_in;
+   wire 	csr_out;
+
+   reg 		timer_irq_r;
+
+   wire 	d = i_csr_d_sel ? i_csr_imm : i_rs1;
+
+   assign csr_in = (i_csr_source == CSR_SOURCE_EXT) ? d :
+		   (i_csr_source == CSR_SOURCE_SET) ? csr_out | d :
+		   (i_csr_source == CSR_SOURCE_CLR) ? csr_out & ~d :
+		   (i_csr_source == CSR_SOURCE_CSR) ? csr_out :
+		   1'bx;
+
+   assign csr_out = (i_mstatus_en & mstatus_mie & i_cnt3) |
+		    i_rf_csr_out |
+		    (i_mcause_en & i_en & mcause);
+
+   assign o_q = csr_out;
+
+   wire 	timer_irq = i_mtip & mstatus_mie & mie_mtie;
+
+   assign mcause = i_cnt0to3 ? mcause3_0[0] : //[3:0]
+		   i_cnt_done ? mcause31 //[31]
+		   : 1'b0;
+
+   assign o_csr_in = csr_in;
+
+   always @(posedge i_clk) begin
+      if (i_trig_irq) begin
+	 timer_irq_r <= timer_irq;
+	 o_new_irq   <= timer_irq & !timer_irq_r;
+      end
+
+      if (i_mie_en & i_cnt7)
+	mie_mtie <= csr_in;
+
+      /*
+       The mie bit in mstatus gets updated under three conditions
+
+       When a trap is taken, the bit is cleared
+       During an mret instruction, the bit is restored from mpie
+       During a mstatus CSR access instruction it's assigned when
+        bit 3 gets updated
+
+       These conditions are all mutually exclusibe
+       */
+      if ((i_trap & i_cnt_done) | i_mstatus_en & i_cnt3 | i_mret)
+	mstatus_mie <= !i_trap & (i_mret ?  mstatus_mpie : csr_in);
+
+      /*
+       Note: To save resources mstatus_mpie (mstatus bit 7) is not
+       readable or writable from sw
+       */
+      if (i_trap & i_cnt_done)
+	mstatus_mpie <= mstatus_mie;
+
+      /*
+       The four lowest bits in mcause hold the exception code
+
+       These bits get updated under three conditions
+
+       During an mcause CSR access function, they are assigned when
+       bits 0 to 3 gets updated
+
+       During an external interrupt the exception code is set to
+       7, since SERV only support timer interrupts
+
+       During an exception, the exception code is assigned to indicate
+       if it was caused by an ebreak instruction (3),
+       ecall instruction (11), misaligned load (4), misaligned store (6)
+       or misaligned jump (0)
+
+       The expressions below are derived from the following truth table
+       irq  => 0111 (timer=7)
+       e_op => x011 (ebreak=3, ecall=11)
+       mem  => 01x0 (store=6, load=4)
+       ctrl => 0000 (jump=0)
+       */
+      if (i_mcause_en & i_en & i_cnt0to3 | (i_trap & i_cnt_done)) begin
+	 mcause3_0[3] <= (i_e_op & !i_ebreak) | (!i_trap & csr_in);
+	 mcause3_0[2] <= o_new_irq | i_mem_op | (!i_trap & mcause3_0[3]);
+	 mcause3_0[1] <= o_new_irq | i_e_op | (i_mem_op & i_mem_cmd) | (!i_trap & mcause3_0[2]);
+	 mcause3_0[0] <= o_new_irq | i_e_op | (!i_trap & mcause3_0[1]);
+      end
+      if (i_mcause_en & i_cnt_done | i_trap)
+	mcause31 <= i_trap ? o_new_irq : csr_in;
+      if (i_rst)
+	if (RESET_STRATEGY != "NONE") begin
+	   o_new_irq <= 1'b0;
+	   mie_mtie <= 1'b0;
+	end
+   end
+
+endmodule

fw/rtl/serv/serv_ctrl.v

@@ -0,0 +1,84 @@
+`default_nettype none
+module serv_ctrl
+  #(parameter RESET_STRATEGY = "MINI",
+    parameter RESET_PC = 32'd0,
+    parameter WITH_CSR = 1)
+  (
+   input wire 	     clk,
+   input wire 	     i_rst,
+   //State
+   input wire 	     i_pc_en,
+   input wire 	     i_cnt12to31,
+   input wire 	     i_cnt0,
+   input wire        i_cnt1,
+   input wire 	     i_cnt2,
+   //Control
+   input wire 	     i_jump,
+   input wire 	     i_jal_or_jalr,
+   input wire 	     i_utype,
+   input wire 	     i_pc_rel,
+   input wire 	     i_trap,
+   input wire        i_iscomp,
+   //Data
+   input wire 	     i_imm,
+   input wire 	     i_buf,
+   input wire 	     i_csr_pc,
+   output wire 	     o_rd,
+   output wire 	     o_bad_pc,
+   //External
+   output reg [31:0] o_ibus_adr);
+
+   wire       pc_plus_4;
+   wire       pc_plus_4_cy;
+   reg 	      pc_plus_4_cy_r;
+   wire       pc_plus_offset;
+   wire       pc_plus_offset_cy;
+   reg 	      pc_plus_offset_cy_r;
+   wire       pc_plus_offset_aligned;
+   wire       plus_4;
+
+   wire       pc = o_ibus_adr[0];
+
+   wire       new_pc;
+
+   wire       offset_a;
+   wire       offset_b;
+
+  /*  If i_iscomp=1: increment pc by 2 else increment pc by 4  */
+
+   assign plus_4        = i_iscomp ? i_cnt1 : i_cnt2;
+
+   assign o_bad_pc = pc_plus_offset_aligned;
+
+   assign {pc_plus_4_cy,pc_plus_4} = pc+plus_4+pc_plus_4_cy_r;
+
+   generate
+      if (|WITH_CSR) begin : gen_csr
+	 assign new_pc = i_trap ? (i_csr_pc & !i_cnt0) : i_jump ? pc_plus_offset_aligned : pc_plus_4;
+      end else begin : gen_no_csr
+	 assign new_pc = i_jump ? pc_plus_offset_aligned : pc_plus_4;
+      end
+   endgenerate
+   assign o_rd  = (i_utype & pc_plus_offset_aligned) | (pc_plus_4 & i_jal_or_jalr);
+
+   assign offset_a = i_pc_rel & pc;
+   assign offset_b = i_utype ? (i_imm & i_cnt12to31): i_buf;
+   assign {pc_plus_offset_cy,pc_plus_offset} = offset_a+offset_b+pc_plus_offset_cy_r;
+
+   assign pc_plus_offset_aligned = pc_plus_offset & !i_cnt0;
+
+   initial if (RESET_STRATEGY == "NONE") o_ibus_adr = RESET_PC;
+
+   always @(posedge clk) begin
+      pc_plus_4_cy_r <= i_pc_en & pc_plus_4_cy;
+      pc_plus_offset_cy_r <= i_pc_en & pc_plus_offset_cy;
+
+      if (RESET_STRATEGY == "NONE") begin
+	 if (i_pc_en)
+	   o_ibus_adr <= {new_pc, o_ibus_adr[31:1]};
+      end else begin
+	 if (i_pc_en | i_rst)
+	   o_ibus_adr <= i_rst ? RESET_PC : {new_pc, o_ibus_adr[31:1]};
+      end
+   end
+endmodule

fw/rtl/serv/serv_decode.v

@@ -0,0 +1,365 @@
+`default_nettype none
+module serv_decode
+  #(parameter [0:0] PRE_REGISTER = 1,
+    parameter [0:0] MDU = 0)
+  (
+   input wire        clk,
+   //Input
+   input wire [31:2] i_wb_rdt,
+   input wire        i_wb_en,
+   //To state
+   output reg       o_sh_right,
+   output reg       o_bne_or_bge,
+   output reg       o_cond_branch,
+   output reg       o_e_op,
+   output reg       o_ebreak,
+   output reg       o_branch_op,
+   output reg       o_shift_op,
+   output reg       o_slt_or_branch,
+   output reg       o_rd_op,
+   output reg       o_two_stage_op,
+   output reg       o_dbus_en,
+   //MDU
+   output reg       o_mdu_op,
+   //Extension
+   output reg [2:0] o_ext_funct3,
+   //To bufreg
+   output reg       o_bufreg_rs1_en,
+   output reg       o_bufreg_imm_en,
+   output reg       o_bufreg_clr_lsb,
+   output reg       o_bufreg_sh_signed,
+   //To ctrl
+   output reg       o_ctrl_jal_or_jalr,
+   output reg       o_ctrl_utype,
+   output reg       o_ctrl_pc_rel,
+   output reg       o_ctrl_mret,
+   //To alu
+   output reg       o_alu_sub,
+   output reg [1:0] o_alu_bool_op,
+   output reg       o_alu_cmp_eq,
+   output reg       o_alu_cmp_sig,
+   output reg [2:0] o_alu_rd_sel,
+   //To mem IF
+   output reg       o_mem_signed,
+   output reg       o_mem_word,
+   output reg       o_mem_half,
+   output reg       o_mem_cmd,
+   //To CSR
+   output reg       o_csr_en,
+   output reg [1:0] o_csr_addr,
+   output reg       o_csr_mstatus_en,
+   output reg       o_csr_mie_en,
+   output reg       o_csr_mcause_en,
+   output reg [1:0] o_csr_source,
+   output reg       o_csr_d_sel,
+   output reg       o_csr_imm_en,
+   output reg       o_mtval_pc,
+   //To top
+   output reg [3:0] o_immdec_ctrl,
+   output reg [3:0] o_immdec_en,
+   output reg       o_op_b_source,
+   //To RF IF
+   output reg       o_rd_mem_en,
+   output reg       o_rd_csr_en,
+   output reg       o_rd_alu_en);
+
+   reg [4:0] opcode;
+   reg [2:0] funct3;
+   reg        op20;
+   reg        op21;
+   reg        op22;
+   reg        op26;
+
+   reg       imm25;
+   reg       imm30;
+
+   wire co_mdu_op     = MDU & (opcode == 5'b01100) & imm25;
+
+   wire co_two_stage_op =
+	~opcode[2] | (funct3[0] & ~funct3[1] & ~opcode[0] & ~opcode[4]) |
+	(funct3[1] & ~funct3[2] & ~opcode[0] & ~opcode[4]) | co_mdu_op;
+   wire co_shift_op = (opcode[2] & ~funct3[1]) & !co_mdu_op;
+   wire co_slt_or_branch = (opcode[4] | (funct3[1] & opcode[2]) | (imm30 & opcode[2] & opcode[3] & ~funct3[2])) & !co_mdu_op;
+   wire co_branch_op = opcode[4];
+   wire co_dbus_en    = ~opcode[2] & ~opcode[4];
+   wire co_mtval_pc   = opcode[4];
+   wire co_mem_word   = funct3[1];
+   wire co_rd_alu_en  = !opcode[0] & opcode[2] & !opcode[4] & !co_mdu_op;
+   wire co_rd_mem_en  = (!opcode[2] & !opcode[0]) | co_mdu_op;
+   wire [2:0] co_ext_funct3 = funct3;
+
+   //jal,branch =     imm
+   //jalr       = rs1+imm
+   //mem        = rs1+imm
+   //shift      = rs1
+   wire co_bufreg_rs1_en = !opcode[4] | (!opcode[1] & opcode[0]);
+   wire co_bufreg_imm_en = !opcode[2];
+
+   //Clear LSB of immediate for BRANCH and JAL ops
+   //True for BRANCH and JAL
+   //False for JALR/LOAD/STORE/OP/OPIMM?
+   wire co_bufreg_clr_lsb = opcode[4] & ((opcode[1:0] == 2'b00) | (opcode[1:0] == 2'b11));
+
+   //Conditional branch
+   //True for BRANCH
+   //False for JAL/JALR
+   wire co_cond_branch = !opcode[0];
+
+   wire co_ctrl_utype       = !opcode[4] & opcode[2] & opcode[0];
+   wire co_ctrl_jal_or_jalr = opcode[4] & opcode[0];
+
+   //PC-relative operations
+   //True for jal, b* auipc, ebreak
+   //False for jalr, lui
+   wire co_ctrl_pc_rel = (opcode[2:0] == 3'b000)  |
+                          (opcode[1:0] == 2'b11)  |
+                          (opcode[4] & opcode[2]) & op20|
+                          (opcode[4:3] == 2'b00);
+   //Write to RD
+   //True for OP-IMM, AUIPC, OP, LUI, SYSTEM, JALR, JAL, LOAD
+   //False for STORE, BRANCH, MISC-MEM
+   wire co_rd_op = (opcode[2] |
+                     (!opcode[2] & opcode[4] & opcode[0]) |
+                     (!opcode[2] & !opcode[3] & !opcode[0]));
+
+   //
+   //funct3
+   //
+
+   wire co_sh_right   = funct3[2];
+   wire co_bne_or_bge = funct3[0];
+
+   //Matches system ops except eceall/ebreak/mret
+   wire csr_op = opcode[4] & opcode[2] & (|funct3);
+
+
+   //op20
+   wire co_ebreak = op20;
+
+
+   //opcode & funct3 & op21
+
+   wire co_ctrl_mret = opcode[4] & opcode[2] & op21 & !(|funct3);
+   //Matches system opcodes except CSR accesses (funct3 == 0)
+   //and mret (!op21)
+   wire co_e_op = opcode[4] & opcode[2] & !op21 & !(|funct3);
+
+   //opcode & funct3 & imm30
+
+   wire co_bufreg_sh_signed = imm30;
+
+   /*
+    True for sub, b*, slt*
+    False for add*
+    op    opcode f3  i30
+    b*    11000  xxx x   t
+    addi  00100  000 x   f
+    slt*  0x100  01x x   t
+    add   01100  000 0   f
+    sub   01100  000 1   t
+    */
+   wire co_alu_sub = funct3[1] | funct3[0] | (opcode[3] & imm30) | opcode[4];
+
+   /*
+    Bits 26, 22, 21 and 20 are enough to uniquely identify the eight supported CSR regs
+    mtvec, mscratch, mepc and mtval are stored externally (normally in the RF) and are
+    treated differently from mstatus, mie and mcause which are stored in serv_csr.
+
+    The former get a 2-bit address as seen below while the latter get a
+    one-hot enable signal each.
+
+    Hex|2 222|Reg     |csr
+    adr|6 210|name    |addr
+    ---|-----|--------|----
+    300|0_000|mstatus | xx
+    304|0_100|mie     | xx
+    305|0_101|mtvec   | 01
+    340|1_000|mscratch| 00
+    341|1_001|mepc    | 10
+    342|1_010|mcause  | xx
+    343|1_011|mtval   | 11
+
+    */
+
+   //true  for mtvec,mscratch,mepc and mtval
+   //false for mstatus, mie, mcause
+   wire csr_valid = op20 | (op26 & !op21);
+
+   wire co_rd_csr_en = csr_op;
+
+   wire co_csr_en         = csr_op & csr_valid;
+   wire co_csr_mstatus_en = csr_op & !op26 & !op22;
+   wire co_csr_mie_en     = csr_op & !op26 &  op22 & !op20;
+   wire co_csr_mcause_en  = csr_op         &  op21 & !op20;
+
+   wire [1:0] co_csr_source = funct3[1:0];
+   wire co_csr_d_sel = funct3[2];
+   wire co_csr_imm_en = opcode[4] & opcode[2] & funct3[2];
+   wire [1:0] co_csr_addr = {op26 & op20, !op26 | op21};
+
+   wire co_alu_cmp_eq = funct3[2:1] == 2'b00;
+
+   wire co_alu_cmp_sig = ~((funct3[0] & funct3[1]) | (funct3[1] & funct3[2]));
+
+   wire co_mem_cmd  = opcode[3];
+   wire co_mem_signed = ~funct3[2];
+   wire co_mem_half   = funct3[0];
+
+   wire [1:0] co_alu_bool_op = funct3[1:0];
+
+   wire [3:0] co_immdec_ctrl;
+   //True for S (STORE) or B (BRANCH) type instructions
+   //False for J type instructions
+   assign co_immdec_ctrl[0] = opcode[3:0] == 4'b1000;
+   //True for OP-IMM, LOAD, STORE, JALR  (I S)
+   //False for LUI, AUIPC, JAL           (U J)
+   assign co_immdec_ctrl[1] = (opcode[1:0] == 2'b00) | (opcode[2:1] == 2'b00);
+   assign co_immdec_ctrl[2] = opcode[4] & !opcode[0];
+   assign co_immdec_ctrl[3] = opcode[4];
+
+   wire [3:0] co_immdec_en;
+   assign co_immdec_en[3] = opcode[4] | opcode[3] | opcode[2] | !opcode[0];                 //B I J S U
+   assign co_immdec_en[2] = (opcode[4] & opcode[2]) | !opcode[3] | opcode[0];               //  I J   U
+   assign co_immdec_en[1] = (opcode[2:1] == 2'b01) | (opcode[2] & opcode[0]) | co_csr_imm_en;//    J   U
+   assign co_immdec_en[0] = ~co_rd_op;                                                       //B     S
+
+   wire [2:0] co_alu_rd_sel;
+   assign co_alu_rd_sel[0] = (funct3 == 3'b000); // Add/sub
+   assign co_alu_rd_sel[1] = (funct3[2:1] == 2'b01); //SLT*
+   assign co_alu_rd_sel[2] = funct3[2]; //Bool
+
+   //0 (OP_B_SOURCE_IMM) when OPIMM
+   //1 (OP_B_SOURCE_RS2) when BRANCH or OP
+   wire co_op_b_source = opcode[3];
+
+   generate
+      if (PRE_REGISTER) begin : gen_pre_register
+
+         always @(posedge clk) begin
+            if (i_wb_en) begin
+               funct3 <= i_wb_rdt[14:12];
+               imm30  <= i_wb_rdt[30];
+               imm25  <= i_wb_rdt[25];
+               opcode <= i_wb_rdt[6:2];
+               op20   <= i_wb_rdt[20];
+               op21   <= i_wb_rdt[21];
+               op22   <= i_wb_rdt[22];
+               op26   <= i_wb_rdt[26];
+            end
+         end
+
+         always @(*) begin
+            o_sh_right         = co_sh_right;
+            o_bne_or_bge       = co_bne_or_bge;
+            o_cond_branch      = co_cond_branch;
+            o_dbus_en          = co_dbus_en;
+            o_mtval_pc         = co_mtval_pc;
+	    o_two_stage_op     = co_two_stage_op;
+            o_e_op             = co_e_op;
+            o_ebreak           = co_ebreak;
+            o_branch_op        = co_branch_op;
+            o_shift_op         = co_shift_op;
+            o_slt_or_branch    = co_slt_or_branch;
+            o_rd_op            = co_rd_op;
+            o_mdu_op           = co_mdu_op;
+            o_ext_funct3       = co_ext_funct3;
+            o_bufreg_rs1_en    = co_bufreg_rs1_en;
+            o_bufreg_imm_en    = co_bufreg_imm_en;
+            o_bufreg_clr_lsb   = co_bufreg_clr_lsb;
+            o_bufreg_sh_signed = co_bufreg_sh_signed;
+            o_ctrl_jal_or_jalr = co_ctrl_jal_or_jalr;
+            o_ctrl_utype       = co_ctrl_utype;
+            o_ctrl_pc_rel      = co_ctrl_pc_rel;
+            o_ctrl_mret        = co_ctrl_mret;
+            o_alu_sub          = co_alu_sub;
+            o_alu_bool_op      = co_alu_bool_op;
+            o_alu_cmp_eq       = co_alu_cmp_eq;
+            o_alu_cmp_sig      = co_alu_cmp_sig;
+            o_alu_rd_sel       = co_alu_rd_sel;
+            o_mem_signed       = co_mem_signed;
+            o_mem_word         = co_mem_word;
+            o_mem_half         = co_mem_half;
+            o_mem_cmd          = co_mem_cmd;
+            o_csr_en           = co_csr_en;
+            o_csr_addr         = co_csr_addr;
+            o_csr_mstatus_en   = co_csr_mstatus_en;
+            o_csr_mie_en       = co_csr_mie_en;
+            o_csr_mcause_en    = co_csr_mcause_en;
+            o_csr_source       = co_csr_source;
+            o_csr_d_sel        = co_csr_d_sel;
+            o_csr_imm_en       = co_csr_imm_en;
+            o_immdec_ctrl      = co_immdec_ctrl;
+            o_immdec_en        = co_immdec_en;
+            o_op_b_source      = co_op_b_source;
+            o_rd_csr_en        = co_rd_csr_en;
+            o_rd_alu_en        = co_rd_alu_en;
+            o_rd_mem_en        = co_rd_mem_en;
+         end
+
+      end else begin : gen_post_register
+
+         always @(*) begin
+            funct3  = i_wb_rdt[14:12];
+            imm30   = i_wb_rdt[30];
+            imm25   = i_wb_rdt[25];
+            opcode  = i_wb_rdt[6:2];
+            op20    = i_wb_rdt[20];
+            op21    = i_wb_rdt[21];
+            op22    = i_wb_rdt[22];
+            op26    = i_wb_rdt[26];
+         end
+
+         always @(posedge clk) begin
+            if (i_wb_en) begin
+               o_sh_right         <= co_sh_right;
+               o_bne_or_bge       <= co_bne_or_bge;
+               o_cond_branch      <= co_cond_branch;
+               o_e_op             <= co_e_op;
+               o_ebreak           <= co_ebreak;
+               o_two_stage_op     <= co_two_stage_op;
+               o_dbus_en          <= co_dbus_en;
+               o_mtval_pc         <= co_mtval_pc;
+               o_branch_op        <= co_branch_op;
+               o_shift_op         <= co_shift_op;
+               o_slt_or_branch    <= co_slt_or_branch;
+               o_rd_op            <= co_rd_op;
+               o_mdu_op           <= co_mdu_op;
+               o_ext_funct3       <= co_ext_funct3;
+               o_bufreg_rs1_en    <= co_bufreg_rs1_en;
+               o_bufreg_imm_en    <= co_bufreg_imm_en;
+               o_bufreg_clr_lsb   <= co_bufreg_clr_lsb;
+               o_bufreg_sh_signed <= co_bufreg_sh_signed;
+               o_ctrl_jal_or_jalr <= co_ctrl_jal_or_jalr;
+               o_ctrl_utype       <= co_ctrl_utype;
+               o_ctrl_pc_rel      <= co_ctrl_pc_rel;
+               o_ctrl_mret        <= co_ctrl_mret;
+               o_alu_sub          <= co_alu_sub;
+               o_alu_bool_op      <= co_alu_bool_op;
+               o_alu_cmp_eq       <= co_alu_cmp_eq;
+               o_alu_cmp_sig      <= co_alu_cmp_sig;
+               o_alu_rd_sel       <= co_alu_rd_sel;
+               o_mem_signed       <= co_mem_signed;
+               o_mem_word         <= co_mem_word;
+               o_mem_half         <= co_mem_half;
+               o_mem_cmd          <= co_mem_cmd;
+               o_csr_en           <= co_csr_en;
+               o_csr_addr         <= co_csr_addr;
+               o_csr_mstatus_en   <= co_csr_mstatus_en;
+               o_csr_mie_en       <= co_csr_mie_en;
+               o_csr_mcause_en    <= co_csr_mcause_en;
+               o_csr_source       <= co_csr_source;
+               o_csr_d_sel        <= co_csr_d_sel;
+               o_csr_imm_en       <= co_csr_imm_en;
+               o_immdec_ctrl      <= co_immdec_ctrl;
+               o_immdec_en        <= co_immdec_en;
+               o_op_b_source      <= co_op_b_source;
+               o_rd_csr_en        <= co_rd_csr_en;
+               o_rd_alu_en        <= co_rd_alu_en;
+               o_rd_mem_en        <= co_rd_mem_en;
+            end
+         end
+
+      end
+   endgenerate
+
+endmodule

fw/rtl/serv/serv_immdec.v

@@ -0,0 +1,95 @@
+`default_nettype none
+module serv_immdec
+  #(parameter SHARED_RFADDR_IMM_REGS = 1)
+  (
+   input wire 	     i_clk,
+   //State
+   input wire 	     i_cnt_en,
+   input wire 	     i_cnt_done,
+   //Control
+   input wire [3:0]  i_immdec_en,
+   input wire 	     i_csr_imm_en,
+   input wire [3:0]  i_ctrl,
+   output wire [4:0] o_rd_addr,
+   output wire [4:0] o_rs1_addr,
+   output wire [4:0] o_rs2_addr,
+   //Data
+   output wire 	     o_csr_imm,
+   output wire 	     o_imm,
+   //External
+   input wire 	     i_wb_en,
+   input wire [31:7] i_wb_rdt);
+
+   reg 		     imm31;
+
+   reg [8:0]  imm19_12_20;
+   reg 	      imm7;
+   reg [5:0]  imm30_25;
+   reg [4:0]  imm24_20;
+   reg [4:0]  imm11_7;
+
+   assign o_csr_imm = imm19_12_20[4];
+
+   wire       signbit = imm31 & !i_csr_imm_en;
+
+   generate
+      if (SHARED_RFADDR_IMM_REGS) begin : gen_shared_imm_regs
+	 assign o_rs1_addr = imm19_12_20[8:4];
+	 assign o_rs2_addr = imm24_20;
+	 assign o_rd_addr  = imm11_7;
+
+	 always @(posedge i_clk) begin
+	    if (i_wb_en) begin
+	       /* CSR immediates are always zero-extended, hence clear the signbit */
+	       imm31     <= i_wb_rdt[31];
+	    end
+	    if (i_wb_en | (i_cnt_en & i_immdec_en[1]))
+	      imm19_12_20 <= i_wb_en ? {i_wb_rdt[19:12],i_wb_rdt[20]} : {i_ctrl[3] ? signbit : imm24_20[0], imm19_12_20[8:1]};
+	    if (i_wb_en | (i_cnt_en))
+	      imm7        <= i_wb_en ? i_wb_rdt[7]                    : signbit;
+
+	    if (i_wb_en | (i_cnt_en & i_immdec_en[3]))
+	      imm30_25    <= i_wb_en ? i_wb_rdt[30:25] : {i_ctrl[2] ? imm7 : i_ctrl[1] ? signbit : imm19_12_20[0], imm30_25[5:1]};
+
+	    if (i_wb_en | (i_cnt_en & i_immdec_en[2]))
+	      imm24_20    <= i_wb_en ? i_wb_rdt[24:20] : {imm30_25[0], imm24_20[4:1]};
+
+	    if (i_wb_en | (i_cnt_en & i_immdec_en[0]))
+	      imm11_7     <= i_wb_en ? i_wb_rdt[11:7] : {imm30_25[0], imm11_7[4:1]};
+	 end
+      end else begin : gen_separate_imm_regs
+	 reg [4:0]  rd_addr;
+	 reg [4:0]  rs1_addr;
+	 reg [4:0]  rs2_addr;
+
+	 assign o_rd_addr  = rd_addr;
+	 assign o_rs1_addr = rs1_addr;
+	 assign o_rs2_addr = rs2_addr;
+	 always @(posedge i_clk) begin
+	    if (i_wb_en) begin
+	       /* CSR immediates are always zero-extended, hence clear the signbit */
+	       imm31       <= i_wb_rdt[31];
+	       imm19_12_20 <= {i_wb_rdt[19:12],i_wb_rdt[20]};
+	       imm7        <= i_wb_rdt[7];
+	       imm30_25    <= i_wb_rdt[30:25];
+	       imm24_20    <= i_wb_rdt[24:20];
+	       imm11_7     <= i_wb_rdt[11:7];
+
+               rd_addr  <= i_wb_rdt[11:7];
+               rs1_addr <= i_wb_rdt[19:15];
+               rs2_addr <= i_wb_rdt[24:20];
+	    end
+	    if (i_cnt_en) begin
+	       imm19_12_20 <= {i_ctrl[3] ? signbit : imm24_20[0], imm19_12_20[8:1]};
+	       imm7        <= signbit;
+	       imm30_25    <= {i_ctrl[2] ? imm7 : i_ctrl[1] ? signbit : imm19_12_20[0], imm30_25[5:1]};
+	       imm24_20    <= {imm30_25[0], imm24_20[4:1]};
+	       imm11_7     <= {imm30_25[0], imm11_7[4:1]};
+	    end
+	 end
+      end
+   endgenerate
+
+	 assign o_imm = i_cnt_done ? signbit : i_ctrl[0] ? imm11_7[0] : imm24_20[0];
+
+endmodule

fw/rtl/serv/serv_mem_if.v

@@ -0,0 +1,69 @@
+`default_nettype none
+module serv_mem_if
+  #(
+    parameter [0:0] WITH_CSR = 1,
+    parameter	    W = 1,
+    parameter	    B = W-1
+  )
+  (
+   input wire 	     i_clk,
+   //State
+   input wire [1:0]  i_bytecnt,
+   input wire [1:0]  i_lsb,
+   output wire 	     o_byte_valid,
+   output wire 	     o_misalign,
+   //Control
+   input wire 	     i_signed,
+   input wire 	     i_word,
+   input wire 	     i_half,
+   //MDU
+   input wire 	     i_mdu_op,
+   //Data
+   input wire [B:0] i_bufreg2_q,
+   output wire [B:0] o_rd,
+   //External interface
+   output wire [3:0] o_wb_sel);
+
+   reg signbit;
+
+   /*
+    Before a store operation, the data to be written needs to be shifted into
+    place. Depending on the address alignment, we need to shift different
+    amounts. One formula for calculating this is to say that we shift when
+    i_lsb + i_bytecnt < 4. Unfortunately, the synthesis tools don't seem to be
+    clever enough so the hideous expression below is used to achieve the same
+    thing in a more optimal way.
+    */
+   assign o_byte_valid
+     = (!i_lsb[0] & !i_lsb[1])         |
+       (!i_bytecnt[0] & !i_bytecnt[1]) |
+       (!i_bytecnt[1] & !i_lsb[1])     |
+       (!i_bytecnt[1] & !i_lsb[0])     |
+       (!i_bytecnt[0] & !i_lsb[1]);
+
+   wire dat_valid =
+	i_mdu_op |
+	i_word |
+	(i_bytecnt == 2'b00) |
+	(i_half & !i_bytecnt[1]);
+
+   assign o_rd = dat_valid ? i_bufreg2_q : {W{i_signed & signbit}};
+
+   assign o_wb_sel[3] = (i_lsb == 2'b11) | i_word | (i_half & i_lsb[1]);
+   assign o_wb_sel[2] = (i_lsb == 2'b10) | i_word;
+   assign o_wb_sel[1] = (i_lsb == 2'b01) | i_word | (i_half & !i_lsb[1]);
+   assign o_wb_sel[0] = (i_lsb == 2'b00);
+
+   always @(posedge i_clk) begin
+      if (dat_valid)
+        signbit <= i_bufreg2_q[B];
+   end
+
+   /*
+    mem_misalign is checked after the init stage to decide whether to do a data
+    bus transaction or go to the trap state. It is only guaranteed to be correct
+    at this time
+    */
+   assign o_misalign = WITH_CSR & ((i_lsb[0] & (i_word | i_half)) | (i_lsb[1] & i_word));
+
+endmodule

fw/rtl/serv/serv_rf_if.v

@@ -0,0 +1,149 @@
+`default_nettype none
+module serv_rf_if
+  #(parameter WITH_CSR = 1)
+  (//RF Interface
+   input wire 		      i_cnt_en,
+   output wire [4+WITH_CSR:0] o_wreg0,
+   output wire [4+WITH_CSR:0] o_wreg1,
+   output wire 		      o_wen0,
+   output wire 		      o_wen1,
+   output wire 		      o_wdata0,
+   output wire 		      o_wdata1,
+   output wire [4+WITH_CSR:0] o_rreg0,
+   output wire [4+WITH_CSR:0] o_rreg1,
+   input wire 		      i_rdata0,
+   input wire 		      i_rdata1,
+
+   //Trap interface
+   input wire 		      i_trap,
+   input wire 		      i_mret,
+   input wire 		      i_mepc,
+   input wire 		      i_mtval_pc,
+   input wire 		      i_bufreg_q,
+   input wire 		      i_bad_pc,
+   output wire 		      o_csr_pc,
+   //CSR interface
+   input wire 		      i_csr_en,
+   input wire [1:0] 	      i_csr_addr,
+   input wire 		      i_csr,
+   output wire 		      o_csr,
+   //RD write port
+   input wire 		      i_rd_wen,
+   input wire [4:0] 	      i_rd_waddr,
+   input wire 		      i_ctrl_rd,
+   input wire 		      i_alu_rd,
+   input wire 		      i_rd_alu_en,
+   input wire 		      i_csr_rd,
+   input wire 		      i_rd_csr_en,
+   input wire 		      i_mem_rd,
+   input wire 		      i_rd_mem_en,
+
+   //RS1 read port
+   input wire [4:0] 	      i_rs1_raddr,
+   output wire 		      o_rs1,
+   //RS2 read port
+   input wire [4:0] 	      i_rs2_raddr,
+   output wire 		      o_rs2);
+
+
+   /*
+    ********** Write side ***********
+    */
+
+   wire 	     rd_wen = i_rd_wen & (|i_rd_waddr);
+
+   generate
+   if (|WITH_CSR) begin : gen_csr
+   wire 	     rd = (i_ctrl_rd ) |
+			  (i_alu_rd & i_rd_alu_en) |
+			  (i_csr_rd & i_rd_csr_en) |
+			  (i_mem_rd & i_rd_mem_en);
+
+   wire 	     mtval = i_mtval_pc ? i_bad_pc : i_bufreg_q;
+
+   assign 	     o_wdata0 = i_trap ? mtval  : rd;
+   assign	     o_wdata1 = i_trap ? i_mepc : i_csr;
+
+   /* Port 0 handles writes to mtval during traps and rd otherwise
+    * Port 1 handles writes to mepc during traps and csr accesses otherwise
+    *
+    * GPR registers are mapped to address 0-31 (bits 0xxxxx).
+    * Following that are four CSR registers
+    * mscratch 100000
+    * mtvec    100001
+    * mepc     100010
+    * mtval    100011
+    */
+
+   assign o_wreg0 = i_trap ? {6'b100011} : {1'b0,i_rd_waddr};
+   assign o_wreg1 = i_trap ? {6'b100010} : {4'b1000,i_csr_addr};
+
+   assign       o_wen0 = i_cnt_en & (i_trap | rd_wen);
+   assign       o_wen1 = i_cnt_en & (i_trap | i_csr_en);
+
+   /*
+    ********** Read side ***********
+    */
+
+   //0 : RS1
+   //1 : RS2 / CSR
+
+   assign o_rreg0 = {1'b0, i_rs1_raddr};
+
+   /*
+    The address of the second read port (o_rreg1) can get assigned from four
+    different sources
+
+    Normal operations : i_rs2_raddr
+    CSR access        : i_csr_addr
+    trap              : MTVEC
+    mret              : MEPC
+
+    Address 0-31 in the RF are assigned to the GPRs. After that follows the four
+    CSRs on addresses 32-35
+
+    32 MSCRATCH
+    33 MTVEC
+    34 MEPC
+    35 MTVAL
+
+    The expression below is an optimized version of this logic
+    */
+   wire sel_rs2 = !(i_trap | i_mret | i_csr_en);
+   assign o_rreg1 = {~sel_rs2,
+		     i_rs2_raddr[4:2] & {3{sel_rs2}},
+		     {1'b0,i_trap} | {i_mret,1'b0} | ({2{i_csr_en}} & i_csr_addr) | ({2{sel_rs2}} & i_rs2_raddr[1:0])};
+
+   assign o_rs1 = i_rdata0;
+   assign o_rs2 = i_rdata1;
+   assign o_csr = i_rdata1 & i_csr_en;
+   assign o_csr_pc = i_rdata1;
+
+   end else begin : gen_no_csr
+      wire 	     rd = (i_ctrl_rd ) |
+			  (i_alu_rd & i_rd_alu_en) |
+			  (i_mem_rd & i_rd_mem_en);
+
+      assign 	     o_wdata0 = rd;
+      assign	     o_wdata1 = 1'b0;
+
+      assign o_wreg0 = i_rd_waddr;
+      assign o_wreg1 = 5'd0;
+
+      assign       o_wen0 = i_cnt_en & rd_wen;
+      assign       o_wen1 = 1'b0;
+
+   /*
+    ********** Read side ***********
+    */
+
+      assign o_rreg0 = i_rs1_raddr;
+      assign o_rreg1 = i_rs2_raddr;
+
+      assign o_rs1 = i_rdata0;
+      assign o_rs2 = i_rdata1;
+      assign o_csr = 1'b0;
+      assign o_csr_pc = 1'b0;
+   end // else: !if(WITH_CSR)
+   endgenerate
+endmodule

fw/rtl/serv/serv_rf_ram.v

@@ -0,0 +1,45 @@
+module serv_rf_ram
+  #(parameter width=0,
+    parameter csr_regs=4,
+    parameter depth=32*(32+csr_regs)/width)
+   (input wire i_clk,
+    input wire [$clog2(depth)-1:0] i_waddr,
+    input wire [width-1:0] 	   i_wdata,
+    input wire 			   i_wen,
+    input wire [$clog2(depth)-1:0] i_raddr,
+    input wire			   i_ren,
+    output wire [width-1:0] 	   o_rdata);
+
+   reg [width-1:0] 		   memory [0:depth-1];
+   reg [width-1:0] 		   rdata ;
+
+   always @(posedge i_clk) begin
+      if (i_wen)
+	memory[i_waddr] <= i_wdata;
+      rdata <= i_ren ? memory[i_raddr] : {width{1'bx}};
+   end
+
+   /* Reads from reg x0 needs to return 0
+    Check that the part of the read address corresponding to the register
+    is zero and gate the output
+    width LSB of reg index $clog2(width)
+    2     4                1
+    4     3                2
+    8     2                3
+    16    1                4
+    32    0                5
+    */
+   reg regzero;
+
+   always @(posedge i_clk)
+     regzero <= !(|i_raddr[$clog2(depth)-1:5-$clog2(width)]);
+
+   assign o_rdata = rdata & ~{width{regzero}};
+
+`ifdef SERV_CLEAR_RAM
+   integer i;
+   initial
+     for (i=0;i<depth;i=i+1)
+       memory[i] = {width{1'd0}};
+`endif
+endmodule

fw/rtl/serv/serv_rf_ram_if.v

@@ -0,0 +1,174 @@
+`default_nettype none
+module serv_rf_ram_if
+  #(//Data width. Adjust to preferred width of SRAM data interface
+    parameter width=8,
+
+    //Select reset strategy.
+    // "MINI" for resetting minimally required FFs
+    // "NONE" for relying on FFs having a defined value on startup
+    parameter reset_strategy="MINI",
+
+    //Number of CSR registers. These are allocated after the normal
+    // GPR registers in the RAM.
+    parameter csr_regs=4,
+
+    //Internal parameters calculated from above values. Do not change
+    parameter raw=$clog2(32+csr_regs), //Register address width
+    parameter l2w=$clog2(width), //log2 of width
+    parameter aw=5+raw-l2w) //Address width
+  (
+   //SERV side
+   input wire		   i_clk,
+   input wire		   i_rst,
+   input wire		   i_wreq,
+   input wire		   i_rreq,
+   output wire		   o_ready,
+   input wire [raw-1:0]	   i_wreg0,
+   input wire [raw-1:0]	   i_wreg1,
+   input wire		   i_wen0,
+   input wire		   i_wen1,
+   input wire		   i_wdata0,
+   input wire		   i_wdata1,
+   input wire [raw-1:0]	   i_rreg0,
+   input wire [raw-1:0]	   i_rreg1,
+   output wire		   o_rdata0,
+   output wire		   o_rdata1,
+   //RAM side
+   output wire [aw-1:0]	   o_waddr,
+   output wire [width-1:0] o_wdata,
+   output wire		   o_wen,
+   output wire [aw-1:0]	   o_raddr,
+   output wire		   o_ren,
+   input wire [width-1:0]  i_rdata);
+
+   reg 				   rgnt;
+   assign o_ready = rgnt | i_wreq;
+   reg [4:0] 	  rcnt;
+
+   reg 		  rtrig1;
+   /*
+    ********** Write side ***********
+    */
+
+   wire [4:0] 	     wcnt;
+
+   reg [width-1:0]   wdata0_r;
+   reg [width-0:0]   wdata1_r;
+
+   reg 		     wen0_r;
+   reg 		     wen1_r;
+   wire 	     wtrig0;
+   wire 	     wtrig1;
+
+   assign wtrig0 = rtrig1;
+
+   generate if (width == 2) begin : gen_w_eq_2
+      assign wtrig1 =  wcnt[0];
+   end else begin : gen_w_neq_2
+      reg wtrig0_r;
+      always @(posedge i_clk) wtrig0_r <= wtrig0;
+      assign wtrig1 = wtrig0_r;
+   end
+   endgenerate
+
+   assign 	     o_wdata = wtrig1 ?
+			       wdata1_r[width-1:0] :
+			       wdata0_r;
+
+   wire [raw-1:0] wreg  = wtrig1 ? i_wreg1 : i_wreg0;
+   generate if (width == 32) begin : gen_w_eq_32
+      assign o_waddr = wreg;
+   end else begin : gen_w_neq_32
+      assign o_waddr = {wreg, wcnt[4:l2w]};
+   end
+   endgenerate
+
+   assign o_wen = (wtrig0 & wen0_r) | (wtrig1 & wen1_r);
+
+   assign wcnt = rcnt-4;
+
+   always @(posedge i_clk) begin
+      if (wcnt[0]) begin
+	 wen0_r    <= i_wen0;
+	 wen1_r    <= i_wen1;
+      end
+
+      wdata0_r  <= {i_wdata0,wdata0_r[width-1:1]};
+      wdata1_r  <= {i_wdata1,wdata1_r[width-0:1]};
+
+   end
+
+   /*
+    ********** Read side ***********
+    */
+
+
+   wire 	  rtrig0;
+
+   wire [raw-1:0] rreg = rtrig0 ? i_rreg1 : i_rreg0;
+   generate if (width == 32) begin : gen_rreg_eq_32
+      assign o_raddr = rreg;
+   end else begin : gen_rreg_neq_32
+      assign o_raddr = {rreg, rcnt[4:l2w]};
+   end
+   endgenerate
+
+   reg [width-1:0]  rdata0;
+   reg [width-2:0]  rdata1;
+
+   reg 		    rgate;
+
+   assign o_rdata0 = rdata0[0];
+   assign o_rdata1 = rtrig1 ? i_rdata[0] : rdata1[0];
+
+   assign rtrig0 = (rcnt[l2w-1:0] == 1);
+
+   generate if (width == 2) begin : gen_ren_w_eq_2
+      assign o_ren = rgate;
+   end else begin : gen_ren_w_neq_2
+      assign o_ren = rgate & (rcnt[l2w-1:1] == 0);
+   end
+   endgenerate
+
+   reg 	      rreq_r;
+
+   generate if (width>2) begin : gen_rdata1_w_neq_2
+      always @(posedge i_clk) begin
+	 rdata1 <= {1'b0,rdata1[width-2:1]}; //Optimize?
+	 if (rtrig1)
+	   rdata1[width-2:0] <= i_rdata[width-1:1];
+      end
+   end else begin : gen_rdata1_w_eq_2
+      always @(posedge i_clk) if (rtrig1) rdata1 <= i_rdata[1];
+   end
+   endgenerate
+
+   always @(posedge i_clk) begin
+      if (&rcnt | i_rreq)
+	rgate <= i_rreq;
+
+      rtrig1 <= rtrig0;
+      rcnt <= rcnt+5'd1;
+      if (i_rreq | i_wreq)
+	 rcnt <= {3'd0,i_wreq,1'b0};
+
+      rreq_r <= i_rreq;
+      rgnt <= rreq_r;
+
+      rdata0 <= {1'b0,rdata0[width-1:1]};
+      if (rtrig0)
+	rdata0 <= i_rdata;
+
+      if (i_rst) begin
+	 if (reset_strategy != "NONE") begin
+	    rgate <= 1'b0;
+	    rgnt <= 1'b0;
+	    rreq_r <= 1'b0;
+	    rcnt <= 5'd0;
+	 end
+      end
+   end
+
+
+
+endmodule

fw/rtl/serv/serv_rf_top.v

@@ -0,0 +1,216 @@
+`default_nettype none
+
+module serv_rf_top
+  #(parameter RESET_PC = 32'd0,
+    /*  COMPRESSED=1: Enable the compressed decoder and allowed misaligned jump of pc
+        COMPRESSED=0: Disable the compressed decoder and does not allow the misaligned jump of pc
+    */
+    parameter [0:0] COMPRESSED = 0,
+    /*  
+      ALIGN = 1: Fetch the aligned instruction by making two bus transactions if the misaligned address 
+      is given to the instruction bus.  
+    */
+    parameter [0:0] ALIGN = COMPRESSED,
+    /* Multiplication and Division Unit
+       This parameter enables the interface for connecting SERV and MDU
+    */
+    parameter [0:0] MDU = 0,
+    /* Register signals before or after the decoder
+       0 : Register after the decoder. Faster but uses more resources
+       1 : (default) Register before the decoder. Slower but uses less resources
+     */
+    parameter PRE_REGISTER = 1,
+    /* Amount of reset applied to design
+       "NONE" : No reset at all. Relies on a POR to set correct initialization
+                 values and that core isn't reset during runtime
+       "MINI" : Standard setting. Resets the minimal amount of FFs needed to
+                 restart execution from the instruction at RESET_PC
+     */
+    parameter RESET_STRATEGY = "MINI",
+    parameter WITH_CSR = 1,
+    parameter RF_WIDTH = 2,
+	parameter RF_L2D   = $clog2((32+(WITH_CSR*4))*32/RF_WIDTH))
+  (
+   input wire 	      clk,
+   input wire 	      i_rst,
+   input wire 	      i_timer_irq,
+`ifdef RISCV_FORMAL
+   output wire 	      rvfi_valid,
+   output wire [63:0] rvfi_order,
+   output wire [31:0] rvfi_insn,
+   output wire 	      rvfi_trap,
+   output wire 	      rvfi_halt,
+   output wire 	      rvfi_intr,
+   output wire [1:0]  rvfi_mode,
+   output wire [1:0]  rvfi_ixl,
+   output wire [4:0]  rvfi_rs1_addr,
+   output wire [4:0]  rvfi_rs2_addr,
+   output wire [31:0] rvfi_rs1_rdata,
+   output wire [31:0] rvfi_rs2_rdata,
+   output wire [4:0]  rvfi_rd_addr,
+   output wire [31:0] rvfi_rd_wdata,
+   output wire [31:0] rvfi_pc_rdata,
+   output wire [31:0] rvfi_pc_wdata,
+   output wire [31:0] rvfi_mem_addr,
+   output wire [3:0]  rvfi_mem_rmask,
+   output wire [3:0]  rvfi_mem_wmask,
+   output wire [31:0] rvfi_mem_rdata,
+   output wire [31:0] rvfi_mem_wdata,
+`endif
+   output wire [31:0] o_ibus_adr,
+   output wire 	      o_ibus_cyc,
+   input wire [31:0]  i_ibus_rdt,
+   input wire 	      i_ibus_ack,
+   output wire [31:0] o_dbus_adr,
+   output wire [31:0] o_dbus_dat,
+   output wire [3:0]  o_dbus_sel,
+   output wire 	      o_dbus_we ,
+   output wire 	      o_dbus_cyc,
+   input wire [31:0]  i_dbus_rdt,
+   input wire 	      i_dbus_ack,
+   
+   // Extension
+   output wire [31:0] o_ext_rs1,
+   output wire [31:0] o_ext_rs2,
+   output wire [ 2:0] o_ext_funct3,
+   input  wire [31:0] i_ext_rd,
+   input  wire        i_ext_ready,
+   // MDU
+   output wire        o_mdu_valid);
+   
+   localparam CSR_REGS = WITH_CSR*4;
+
+   wire 	      rf_wreq;
+   wire 	      rf_rreq;
+   wire [4+WITH_CSR:0] wreg0;
+   wire [4+WITH_CSR:0] wreg1;
+   wire 	      wen0;
+   wire 	      wen1;
+   wire 	      wdata0;
+   wire 	      wdata1;
+   wire [4+WITH_CSR:0] rreg0;
+   wire [4+WITH_CSR:0] rreg1;
+   wire 	      rf_ready;
+   wire 	      rdata0;
+   wire 	      rdata1;
+
+   wire [RF_L2D-1:0]   waddr;
+   wire [RF_WIDTH-1:0] wdata;
+   wire 	       wen;
+   wire [RF_L2D-1:0]   raddr;
+   wire 	       ren;
+   wire [RF_WIDTH-1:0] rdata;
+
+   serv_rf_ram_if
+     #(.width    (RF_WIDTH),
+       .reset_strategy (RESET_STRATEGY),
+       .csr_regs (CSR_REGS))
+   rf_ram_if
+     (.i_clk    (clk),
+      .i_rst    (i_rst),
+      .i_wreq   (rf_wreq),
+      .i_rreq   (rf_rreq),
+      .o_ready  (rf_ready),
+      .i_wreg0  (wreg0),
+      .i_wreg1  (wreg1),
+      .i_wen0   (wen0),
+      .i_wen1   (wen1),
+      .i_wdata0 (wdata0),
+      .i_wdata1 (wdata1),
+      .i_rreg0  (rreg0),
+      .i_rreg1  (rreg1),
+      .o_rdata0 (rdata0),
+      .o_rdata1 (rdata1),
+      .o_waddr  (waddr),
+      .o_wdata  (wdata),
+      .o_wen    (wen),
+      .o_raddr  (raddr),
+      .o_ren    (ren),
+      .i_rdata  (rdata));
+
+   serv_rf_ram
+     #(.width (RF_WIDTH),
+       .csr_regs (CSR_REGS))
+   rf_ram
+     (.i_clk    (clk),
+      .i_waddr (waddr),
+      .i_wdata (wdata),
+      .i_wen   (wen),
+      .i_raddr (raddr),
+      .i_ren    (ren),
+      .o_rdata (rdata));
+
+   serv_top
+     #(.RESET_PC (RESET_PC),
+       .PRE_REGISTER (PRE_REGISTER),
+       .RESET_STRATEGY (RESET_STRATEGY),
+       .WITH_CSR (WITH_CSR),
+       .MDU(MDU),
+       .COMPRESSED(COMPRESSED),
+       .ALIGN(ALIGN))
+   cpu
+     (
+      .clk      (clk),
+      .i_rst    (i_rst),
+      .i_timer_irq  (i_timer_irq),
+`ifdef RISCV_FORMAL
+      .rvfi_valid     (rvfi_valid    ),
+      .rvfi_order     (rvfi_order    ),
+      .rvfi_insn      (rvfi_insn     ),
+      .rvfi_trap      (rvfi_trap     ),
+      .rvfi_halt      (rvfi_halt     ),
+      .rvfi_intr      (rvfi_intr     ),
+      .rvfi_mode      (rvfi_mode     ),
+      .rvfi_ixl       (rvfi_ixl      ),
+      .rvfi_rs1_addr  (rvfi_rs1_addr ),
+      .rvfi_rs2_addr  (rvfi_rs2_addr ),
+      .rvfi_rs1_rdata (rvfi_rs1_rdata),
+      .rvfi_rs2_rdata (rvfi_rs2_rdata),
+      .rvfi_rd_addr   (rvfi_rd_addr  ),
+      .rvfi_rd_wdata  (rvfi_rd_wdata ),
+      .rvfi_pc_rdata  (rvfi_pc_rdata ),
+      .rvfi_pc_wdata  (rvfi_pc_wdata ),
+      .rvfi_mem_addr  (rvfi_mem_addr ),
+      .rvfi_mem_rmask (rvfi_mem_rmask),
+      .rvfi_mem_wmask (rvfi_mem_wmask),
+      .rvfi_mem_rdata (rvfi_mem_rdata),
+      .rvfi_mem_wdata (rvfi_mem_wdata),
+`endif
+      .o_rf_rreq   (rf_rreq),
+      .o_rf_wreq   (rf_wreq),
+      .i_rf_ready  (rf_ready),
+      .o_wreg0     (wreg0),
+      .o_wreg1     (wreg1),
+      .o_wen0      (wen0),
+      .o_wen1      (wen1),
+      .o_wdata0    (wdata0),
+      .o_wdata1    (wdata1),
+      .o_rreg0     (rreg0),
+      .o_rreg1     (rreg1),
+      .i_rdata0    (rdata0),
+      .i_rdata1    (rdata1),
+
+      .o_ibus_adr   (o_ibus_adr),
+      .o_ibus_cyc   (o_ibus_cyc),
+      .i_ibus_rdt   (i_ibus_rdt),
+      .i_ibus_ack   (i_ibus_ack),
+
+      .o_dbus_adr   (o_dbus_adr),
+      .o_dbus_dat   (o_dbus_dat),
+      .o_dbus_sel   (o_dbus_sel),
+      .o_dbus_we    (o_dbus_we),
+      .o_dbus_cyc   (o_dbus_cyc),
+      .i_dbus_rdt   (i_dbus_rdt),
+      .i_dbus_ack   (i_dbus_ack),
+      
+      //Extension
+      .o_ext_funct3 (o_ext_funct3),
+      .i_ext_ready  (i_ext_ready),
+      .i_ext_rd     (i_ext_rd),
+      .o_ext_rs1    (o_ext_rs1),
+      .o_ext_rs2    (o_ext_rs2),
+      //MDU
+      .o_mdu_valid  (o_mdu_valid));
+
+endmodule
+`default_nettype wire

fw/rtl/serv/serv_state.v

@@ -0,0 +1,224 @@
+module serv_state
+  #(parameter RESET_STRATEGY = "MINI",
+    parameter [0:0] WITH_CSR = 1,
+    parameter [0:0] ALIGN =0,
+    parameter [0:0] MDU = 0,
+    parameter       W = 1
+  )
+  (
+   input wire 	     i_clk,
+   input wire 	     i_rst,
+   //State
+   input wire 	     i_new_irq,
+   input wire 	     i_alu_cmp,
+   output wire 	     o_init,
+   output reg 	     o_cnt_en,
+   output wire 	     o_cnt0to3,
+   output wire 	     o_cnt12to31,
+   output wire 	     o_cnt0,
+   output wire 	     o_cnt1,
+   output wire 	     o_cnt2,
+   output wire 	     o_cnt3,
+   output wire 	     o_cnt7,
+   output wire 	     o_cnt_done,
+   output wire 	     o_bufreg_en,
+   output wire 	     o_ctrl_pc_en,
+   output reg 	     o_ctrl_jump,
+   output wire 	     o_ctrl_trap,
+   input wire 	     i_ctrl_misalign,
+   input wire 	     i_sh_done,
+   input wire 	     i_sh_done_r,
+   output wire [1:0] o_mem_bytecnt,
+   input wire 	     i_mem_misalign,
+   //Control
+   input wire 	     i_bne_or_bge,
+   input wire 	     i_cond_branch,
+   input wire 	     i_dbus_en,
+   input wire 	     i_two_stage_op,
+   input wire 	     i_branch_op,
+   input wire 	     i_shift_op,
+   input wire 	     i_sh_right,
+   input wire 	     i_slt_or_branch,
+   input wire 	     i_e_op,
+   input wire 	     i_rd_op,
+   //MDU
+   input wire 	     i_mdu_op,
+   output wire 	     o_mdu_valid,
+   //Extension
+   input wire 	     i_mdu_ready,
+   //External
+   output wire 	     o_dbus_cyc,
+   input wire 	     i_dbus_ack,
+   output wire 	     o_ibus_cyc,
+   input wire 	     i_ibus_ack,
+   //RF Interface
+   output wire 	     o_rf_rreq,
+   output wire 	     o_rf_wreq,
+   input wire 	     i_rf_ready,
+   output wire 	     o_rf_rd_en);
+
+   reg 	stage_two_req;
+   reg 	init_done;
+   wire misalign_trap_sync;
+
+   reg [4:2] o_cnt;
+   reg [3:0] cnt_r;
+
+   reg 	     ibus_cyc;
+   //Update PC in RUN or TRAP states
+   assign o_ctrl_pc_en  = o_cnt_en & !o_init;
+
+   assign o_mem_bytecnt = o_cnt[4:3];
+
+   assign o_cnt0to3   = (o_cnt[4:2] == 3'd0);
+   assign o_cnt12to31 = (o_cnt[4] | (o_cnt[3:2] == 2'b11));
+   assign o_cnt0 = (o_cnt[4:2] == 3'd0) & cnt_r[0];
+   assign o_cnt1 = (o_cnt[4:2] == 3'd0) & cnt_r[1];
+   assign o_cnt2 = (o_cnt[4:2] == 3'd0) & cnt_r[2];
+   assign o_cnt3 = (o_cnt[4:2] == 3'd0) & cnt_r[3];
+   assign o_cnt7 = (o_cnt[4:2] == 3'd1) & cnt_r[3];
+
+   //Take branch for jump or branch instructions (opcode == 1x0xx) if
+   //a) It's an unconditional branch (opcode[0] == 1)
+   //b) It's a conditional branch (opcode[0] == 0) of type beq,blt,bltu (funct3[0] == 0) and ALU compare is true
+   //c) It's a conditional branch (opcode[0] == 0) of type bne,bge,bgeu (funct3[0] == 1) and ALU compare is false
+   //Only valid during the last cycle of INIT, when the branch condition has
+   //been calculated.
+   wire      take_branch = i_branch_op & (!i_cond_branch | (i_alu_cmp^i_bne_or_bge));
+
+   //valid signal for mdu
+   assign o_mdu_valid = MDU & !o_cnt_en & init_done & i_mdu_op;
+
+   //Prepare RF for writes when everything is ready to enter stage two
+   // and the first stage didn't cause a misalign exception
+   assign o_rf_wreq = !misalign_trap_sync & !o_cnt_en & init_done &
+	   	      ((i_shift_op & (i_sh_done | !i_sh_right)) |
+	   	       i_dbus_ack | (MDU & i_mdu_ready) |
+	   	       i_slt_or_branch);
+
+   assign o_dbus_cyc = !o_cnt_en & init_done & i_dbus_en & !i_mem_misalign;
+
+   //Prepare RF for reads when a new instruction is fetched
+   // or when stage one caused an exception (rreq implies a write request too)
+   assign o_rf_rreq = i_ibus_ack | (stage_two_req & misalign_trap_sync);
+
+   assign o_rf_rd_en = i_rd_op & !o_init;
+
+   /*
+    bufreg is used during mem. branch and shift operations
+
+    mem : bufreg is used for dbus address. Shift in data during phase 1.
+          Shift out during phase 2 if there was an misalignment exception.
+
+    branch : Shift in during phase 1. Shift out during phase 2
+
+    shift : Shift in during phase 1. Continue shifting between phases (except
+            for the first cycle after init). Shift out during phase 2
+    */
+   assign o_bufreg_en = (o_cnt_en & (o_init | ((o_ctrl_trap | i_branch_op) & i_two_stage_op))) | (i_shift_op & !stage_two_req & (i_sh_right | i_sh_done_r) & init_done);
+
+   assign o_ibus_cyc = ibus_cyc & !i_rst;
+
+   assign o_init = i_two_stage_op & !i_new_irq & !init_done;
+
+   assign o_cnt_done = (o_cnt[4:2] == 3'b111) & cnt_r[3];
+
+   always @(posedge i_clk) begin
+      //ibus_cyc changes on three conditions.
+      //1. i_rst is asserted. Together with the async gating above, o_ibus_cyc
+      //   will be asserted as soon as the reset is released. This is how the
+      //   first instruction is fetced
+      //2. o_cnt_done and o_ctrl_pc_en are asserted. This means that SERV just
+      //   finished updating the PC, is done with the current instruction and
+      //   o_ibus_cyc gets asserted to fetch a new instruction
+      //3. When i_ibus_ack, a new instruction is fetched and o_ibus_cyc gets
+      //   deasserted to finish the transaction
+      if (i_ibus_ack | o_cnt_done | i_rst)
+	ibus_cyc <= o_ctrl_pc_en | i_rst;
+
+      if (o_cnt_done) begin
+	 init_done <= o_init & !init_done;
+	 o_ctrl_jump <= o_init & take_branch;
+      end
+
+      //Need a strobe for the first cycle in the IDLE state after INIT
+      stage_two_req <= o_cnt_done & o_init;
+
+      if (i_rst) begin
+	 if (RESET_STRATEGY != "NONE") begin
+	    init_done <= 1'b0;
+	    o_ctrl_jump <= 1'b0;
+	    stage_two_req <= 1'b0;
+	 end
+      end
+   end
+
+   always @(posedge i_clk) begin
+      /*
+       Because SERV is 32-bit bit-serial we need a counter than can count 0-31
+       to keep track of which bit we are currently processing. o_cnt and cnt_r
+       are used together to create such a counter.
+       The top three bits (o_cnt) are implemented as a normal counter, but
+       instead of the two LSB, cnt_r is a 4-bit shift register which loops 0-3
+       When cnt_r[3] is 1, o_cnt will be increased.
+
+       The counting starts when the core is idle and the i_rf_ready signal
+       comes in from the RF module by shifting in the i_rf_ready bit as LSB of
+       the shift register. Counting is stopped by using o_cnt_done to block the
+       bit that was supposed to be shifted into bit 0 of cnt_r.
+
+       There are two benefit of doing the counter this way
+       1. We only need to check four bits instead of five when we want to check
+       if the counter is at a certain value. For 4-LUT architectures this means
+       we only need one LUT instead of two for each comparison.
+       2. We don't need a separate enable signal to turn on and off the counter
+       between stages, which saves an extra FF and a unique control signal. We
+       just need to check if cnt_r is not zero to see if the counter is
+       currently running
+       */
+      if (W == 4) begin
+          if (i_rf_ready) o_cnt_en <= 1; else
+          if (o_cnt_done) o_cnt_en <= 0;
+          o_cnt <= o_cnt + { 2'b0, o_cnt_en };
+      end else if (W == 1) begin
+          o_cnt <= o_cnt + {2'd0,cnt_r[3]};
+          cnt_r <= {cnt_r[2:0],(cnt_r[3] & !o_cnt_done) | (i_rf_ready & !o_cnt_en)};
+      end
+      if (i_rst) begin
+         if (RESET_STRATEGY != "NONE") begin
+            o_cnt   <= 3'd0;
+            if (W == 1)
+                cnt_r <= 4'b0000;
+            else if (W == 4)
+                o_cnt_en <= 1'b0;
+         end
+      end
+   end
+
+   always @(*)
+   if (W == 1)
+     o_cnt_en = |cnt_r;
+   else if (W == 4)
+     cnt_r = 4'b1111;
+
+   assign o_ctrl_trap = WITH_CSR & (i_e_op | i_new_irq | misalign_trap_sync);
+
+   generate
+      if (WITH_CSR) begin : gen_csr
+	 reg 	misalign_trap_sync_r;
+
+	 //trap_pending is only guaranteed to have correct value during the
+	 // last cycle of the init stage
+	 wire trap_pending = WITH_CSR & ((take_branch & i_ctrl_misalign & !ALIGN) |
+					 (i_dbus_en   & i_mem_misalign));
+
+	 always @(posedge i_clk) begin
+	    if (i_ibus_ack | o_cnt_done | i_rst)
+	      misalign_trap_sync_r <= !(i_ibus_ack | i_rst) & ((trap_pending & o_init) | misalign_trap_sync_r);
+	 end
+	 assign misalign_trap_sync = misalign_trap_sync_r;
+      end else begin : gen_no_csr
+	 assign misalign_trap_sync = 1'b0;
+      end
+   endgenerate
+endmodule

fw/rtl/serv/serv_top.v

@@ -0,0 +1,659 @@
+`default_nettype none
+
+module serv_top
+  #(parameter WITH_CSR = 1,
+    parameter PRE_REGISTER = 1,
+    parameter RESET_STRATEGY = "MINI",
+    parameter RESET_PC = 32'd0,
+    parameter [0:0] MDU = 1'b0,
+    parameter [0:0] COMPRESSED=0,
+    parameter [0:0] ALIGN = COMPRESSED)
+   (
+   input wire 		      clk,
+   input wire 		      i_rst,
+   input wire 		      i_timer_irq,
+`ifdef RISCV_FORMAL
+   output reg 		      rvfi_valid = 1'b0,
+   output reg [63:0] 	      rvfi_order = 64'd0,
+   output reg [31:0] 	      rvfi_insn = 32'd0,
+   output reg 		      rvfi_trap = 1'b0,
+   output reg 		      rvfi_halt = 1'b0,
+   output reg 		      rvfi_intr = 1'b0,
+   output reg [1:0] 	      rvfi_mode = 2'b11,
+   output reg [1:0] 	      rvfi_ixl = 2'b01,
+   output reg [4:0] 	      rvfi_rs1_addr,
+   output reg [4:0] 	      rvfi_rs2_addr,
+   output reg [31:0] 	      rvfi_rs1_rdata,
+   output reg [31:0] 	      rvfi_rs2_rdata,
+   output reg [4:0] 	      rvfi_rd_addr,
+   output reg [31:0] 	      rvfi_rd_wdata,
+   output reg [31:0] 	      rvfi_pc_rdata,
+   output reg [31:0] 	      rvfi_pc_wdata,
+   output reg [31:0] 	      rvfi_mem_addr,
+   output reg [3:0] 	      rvfi_mem_rmask,
+   output reg [3:0] 	      rvfi_mem_wmask,
+   output reg [31:0] 	      rvfi_mem_rdata,
+   output reg [31:0] 	      rvfi_mem_wdata,
+`endif
+   //RF Interface
+   output wire 		      o_rf_rreq,
+   output wire 		      o_rf_wreq,
+   input wire 		      i_rf_ready,
+   output wire [4+WITH_CSR:0] o_wreg0,
+   output wire [4+WITH_CSR:0] o_wreg1,
+   output wire 		      o_wen0,
+   output wire 		      o_wen1,
+   output wire 		      o_wdata0,
+   output wire 		      o_wdata1,
+   output wire [4+WITH_CSR:0] o_rreg0,
+   output wire [4+WITH_CSR:0] o_rreg1,
+   input wire 		      i_rdata0,
+   input wire 		      i_rdata1,
+
+   output wire [31:0] 	      o_ibus_adr,
+   output wire 		      o_ibus_cyc,
+   input wire [31:0] 	      i_ibus_rdt,
+   input wire 		      i_ibus_ack,
+   output wire [31:0] 	      o_dbus_adr,
+   output wire [31:0] 	      o_dbus_dat,
+   output wire [3:0] 	      o_dbus_sel,
+   output wire 		      o_dbus_we ,
+   output wire 		      o_dbus_cyc,
+   input wire [31:0] 	      i_dbus_rdt,
+   input wire 		      i_dbus_ack,
+   //Extension
+   output wire [ 2:0] o_ext_funct3,
+   input  wire        i_ext_ready,
+   input wire  [31:0] i_ext_rd,
+   output wire [31:0] o_ext_rs1,
+   output wire [31:0] o_ext_rs2,
+   //MDU
+   output wire        o_mdu_valid);
+
+   wire [4:0]    rd_addr;
+   wire [4:0]    rs1_addr;
+   wire [4:0]    rs2_addr;
+
+   wire [3:0] 	 immdec_ctrl;
+   wire [3:0] 	immdec_en;
+
+   wire          sh_right;
+   wire 	 bne_or_bge;
+   wire 	 cond_branch;
+   wire 	 two_stage_op;
+   wire 	 e_op;
+   wire 	 ebreak;
+   wire 	 branch_op;
+   wire 	 shift_op;
+   wire 	 slt_or_branch;
+   wire 	 rd_op;
+   wire   mdu_op;
+
+   wire 	 rd_alu_en;
+   wire 	 rd_csr_en;
+   wire 	 rd_mem_en;
+   wire          ctrl_rd;
+   wire          alu_rd;
+   wire          mem_rd;
+   wire          csr_rd;
+   wire 	 mtval_pc;
+
+   wire          ctrl_pc_en;
+   wire          jump;
+   wire          jal_or_jalr;
+   wire          utype;
+   wire 	 mret;
+   wire          imm;
+   wire 	 trap;
+   wire 	 pc_rel;
+   wire          iscomp;
+
+   wire          init;
+   wire          cnt_en;
+   wire 	 cnt0to3;
+   wire 	 cnt12to31;
+   wire          cnt0;
+   wire          cnt1;
+   wire          cnt2;
+   wire          cnt3;
+   wire          cnt7;
+
+   wire 	 cnt_done;
+
+   wire 	 bufreg_en;
+   wire          bufreg_sh_signed;
+   wire 	 bufreg_rs1_en;
+   wire 	 bufreg_imm_en;
+   wire 	 bufreg_clr_lsb;
+   wire 	 bufreg_q;
+   wire 	 bufreg2_q;
+   wire [31:0] dbus_rdt;
+   wire        dbus_ack;
+
+   wire          alu_sub;
+   wire [1:0] 	 alu_bool_op;
+   wire          alu_cmp_eq;
+   wire          alu_cmp_sig;
+   wire          alu_cmp;
+   wire [2:0]    alu_rd_sel;
+
+   wire          rs1;
+   wire          rs2;
+   wire          rd_en;
+
+   wire          op_b;
+   wire          op_b_sel;
+
+   wire          mem_signed;
+   wire          mem_word;
+   wire          mem_half;
+   wire [1:0] 	 mem_bytecnt;
+   wire 	 sh_done;
+   wire 	 sh_done_r;
+   wire 	 byte_valid;
+
+   wire 	 mem_misalign;
+
+   wire 	 bad_pc;
+
+   wire 	 csr_mstatus_en;
+   wire 	 csr_mie_en;
+   wire 	 csr_mcause_en;
+   wire [1:0]	 csr_source;
+   wire 	 csr_imm;
+   wire 	 csr_d_sel;
+   wire 	 csr_en;
+   wire [1:0] 	 csr_addr;
+   wire 	 csr_pc;
+   wire 	 csr_imm_en;
+   wire 	 csr_in;
+   wire 	 rf_csr_out;
+   wire 	 dbus_en;
+
+   wire 	 new_irq;
+
+   wire [1:0]   lsb;
+
+   wire [31:0] i_wb_rdt;
+
+   wire [31:0] wb_ibus_adr;
+   wire        wb_ibus_cyc;
+   wire [31:0] wb_ibus_rdt;
+   wire        wb_ibus_ack;
+
+   generate
+      if (ALIGN) begin : gen_align
+         serv_aligner  align
+           (
+            .clk(clk),
+            .rst(i_rst),
+            // serv_rf_top
+            .i_ibus_adr(wb_ibus_adr),
+            .i_ibus_cyc(wb_ibus_cyc),
+            .o_ibus_rdt(wb_ibus_rdt),
+            .o_ibus_ack(wb_ibus_ack),
+            // servant_arbiter
+            .o_wb_ibus_adr(o_ibus_adr),
+            .o_wb_ibus_cyc(o_ibus_cyc),
+            .i_wb_ibus_rdt(i_ibus_rdt),
+            .i_wb_ibus_ack(i_ibus_ack));
+      end else begin : gen_no_align
+         assign  o_ibus_adr  = wb_ibus_adr;
+         assign  o_ibus_cyc  = wb_ibus_cyc;
+         assign  wb_ibus_rdt = i_ibus_rdt;
+         assign  wb_ibus_ack = i_ibus_ack;
+        end
+   endgenerate
+
+   generate
+      if (COMPRESSED) begin : gen_compressed
+         serv_compdec compdec
+           (
+            .i_clk(clk),
+            .i_instr(wb_ibus_rdt),
+            .i_ack(wb_ibus_ack),
+            .o_instr(i_wb_rdt),
+            .o_iscomp(iscomp));
+      end else begin : gen_no_compressed
+         assign i_wb_rdt =  wb_ibus_rdt;
+         assign iscomp   =  1'b0;
+      end
+   endgenerate
+
+   serv_state
+     #(.RESET_STRATEGY (RESET_STRATEGY),
+       .WITH_CSR (WITH_CSR[0:0]),
+       .MDU(MDU),
+       .ALIGN(ALIGN))
+   state
+     (
+      .i_clk (clk),
+      .i_rst          (i_rst),
+      //State
+      .i_new_irq      (new_irq),
+      .i_alu_cmp      (alu_cmp),
+      .o_init         (init),
+      .o_cnt_en       (cnt_en),
+      .o_cnt0to3      (cnt0to3),
+      .o_cnt12to31    (cnt12to31),
+      .o_cnt0         (cnt0),
+      .o_cnt1         (cnt1),
+      .o_cnt2         (cnt2),
+      .o_cnt3         (cnt3),
+      .o_cnt7         (cnt7),
+      .o_cnt_done     (cnt_done),
+      .o_bufreg_en    (bufreg_en),
+      .o_ctrl_pc_en   (ctrl_pc_en),
+      .o_ctrl_jump    (jump),
+      .o_ctrl_trap    (trap),
+      .i_ctrl_misalign(lsb[1]),
+      .i_sh_done      (sh_done),
+      .i_sh_done_r    (sh_done_r),
+      .o_mem_bytecnt  (mem_bytecnt),
+      .i_mem_misalign (mem_misalign),
+      //Control
+      .i_bne_or_bge   (bne_or_bge),
+      .i_cond_branch  (cond_branch),
+      .i_dbus_en      (dbus_en),
+      .i_two_stage_op (two_stage_op),
+      .i_branch_op    (branch_op),
+      .i_shift_op     (shift_op),
+      .i_sh_right     (sh_right),
+      .i_slt_or_branch (slt_or_branch),
+      .i_e_op         (e_op),
+      .i_rd_op        (rd_op),
+      //MDU
+      .i_mdu_op       (mdu_op),
+      .o_mdu_valid    (o_mdu_valid),
+      //Extension
+      .i_mdu_ready    (i_ext_ready),
+      //External
+      .o_dbus_cyc     (o_dbus_cyc),
+      .i_dbus_ack     (i_dbus_ack),
+      .o_ibus_cyc     (wb_ibus_cyc),
+      .i_ibus_ack     (wb_ibus_ack),
+      //RF Interface
+      .o_rf_rreq      (o_rf_rreq),
+      .o_rf_wreq      (o_rf_wreq),
+      .i_rf_ready     (i_rf_ready),
+      .o_rf_rd_en     (rd_en));
+
+   serv_decode
+     #(.PRE_REGISTER (PRE_REGISTER),
+       .MDU(MDU))
+   decode
+     (
+      .clk (clk),
+      //Input
+      .i_wb_rdt           (i_wb_rdt[31:2]),
+      .i_wb_en            (wb_ibus_ack),
+      //To state
+      .o_bne_or_bge       (bne_or_bge),
+      .o_cond_branch      (cond_branch),
+      .o_dbus_en          (dbus_en),
+      .o_e_op             (e_op),
+      .o_ebreak           (ebreak),
+      .o_branch_op        (branch_op),
+      .o_shift_op         (shift_op),
+      .o_slt_or_branch    (slt_or_branch),
+      .o_rd_op            (rd_op),
+      .o_sh_right         (sh_right),
+      .o_mdu_op           (mdu_op),
+      .o_two_stage_op     (two_stage_op),
+      //Extension
+      .o_ext_funct3       (o_ext_funct3),
+
+      //To bufreg
+      .o_bufreg_rs1_en    (bufreg_rs1_en),
+      .o_bufreg_imm_en    (bufreg_imm_en),
+      .o_bufreg_clr_lsb   (bufreg_clr_lsb),
+      .o_bufreg_sh_signed (bufreg_sh_signed),
+      //To bufreg2
+      .o_op_b_source      (op_b_sel),
+      //To ctrl
+      .o_ctrl_jal_or_jalr (jal_or_jalr),
+      .o_ctrl_utype       (utype),
+      .o_ctrl_pc_rel      (pc_rel),
+      .o_ctrl_mret        (mret),
+      //To alu
+      .o_alu_sub          (alu_sub),
+      .o_alu_bool_op      (alu_bool_op),
+      .o_alu_cmp_eq       (alu_cmp_eq),
+      .o_alu_cmp_sig      (alu_cmp_sig),
+      .o_alu_rd_sel       (alu_rd_sel),
+      //To mem IF
+      .o_mem_cmd          (o_dbus_we),
+      .o_mem_signed       (mem_signed),
+      .o_mem_word         (mem_word),
+      .o_mem_half         (mem_half),
+      //To CSR
+      .o_csr_en           (csr_en),
+      .o_csr_addr         (csr_addr),
+      .o_csr_mstatus_en   (csr_mstatus_en),
+      .o_csr_mie_en       (csr_mie_en),
+      .o_csr_mcause_en    (csr_mcause_en),
+      .o_csr_source       (csr_source),
+      .o_csr_d_sel        (csr_d_sel),
+      .o_csr_imm_en       (csr_imm_en),
+      .o_mtval_pc         (mtval_pc      ),
+      //To top
+      .o_immdec_ctrl      (immdec_ctrl),
+      .o_immdec_en        (immdec_en),
+      //To RF IF
+      .o_rd_mem_en        (rd_mem_en),
+      .o_rd_csr_en        (rd_csr_en),
+      .o_rd_alu_en        (rd_alu_en));
+
+   serv_immdec immdec
+     (
+      .i_clk        (clk),
+      //State
+      .i_cnt_en     (cnt_en),
+      .i_cnt_done   (cnt_done),
+      //Control
+      .i_immdec_en        (immdec_en),
+      .i_csr_imm_en (csr_imm_en),
+      .i_ctrl       (immdec_ctrl),
+      .o_rd_addr    (rd_addr),
+      .o_rs1_addr   (rs1_addr),
+      .o_rs2_addr   (rs2_addr),
+      //Data
+      .o_csr_imm    (csr_imm),
+      .o_imm        (imm),
+      //External
+      .i_wb_en      (wb_ibus_ack),
+      .i_wb_rdt     (i_wb_rdt[31:7]));
+
+   serv_bufreg
+      #(.MDU(MDU))
+   bufreg
+     (
+      .i_clk    (clk),
+      //State
+      .i_cnt0   (cnt0),
+      .i_cnt1   (cnt1),
+      .i_en     (bufreg_en),
+      .i_init   (init),
+      .i_mdu_op (mdu_op),
+      .o_lsb    (lsb),
+      //Control
+      .i_sh_signed (bufreg_sh_signed),
+      .i_rs1_en    (bufreg_rs1_en),
+      .i_imm_en    (bufreg_imm_en),
+      .i_clr_lsb   (bufreg_clr_lsb),
+      //Data
+      .i_rs1    (rs1),
+      .i_imm    (imm),
+      .o_q      (bufreg_q),
+      //External
+      .o_dbus_adr (o_dbus_adr),
+      .o_ext_rs1  (o_ext_rs1));
+
+   serv_bufreg2 bufreg2
+     (
+      .i_clk        (clk),
+      //State
+      .i_en         (cnt_en),
+      .i_init       (init),
+      .i_cnt_done   (cnt_done),
+      .i_lsb        (lsb),
+      .i_byte_valid (byte_valid),
+      .o_sh_done    (sh_done),
+      .o_sh_done_r  (sh_done_r),
+      //Control
+      .i_op_b_sel   (op_b_sel),
+      .i_shift_op   (shift_op),
+      //Data
+      .i_rs2        (rs2),
+      .i_imm        (imm),
+      .o_op_b       (op_b),
+      .o_q          (bufreg2_q),
+      //External
+      .o_dat        (o_dbus_dat),
+      .i_load       (dbus_ack),
+      .i_dat        (dbus_rdt));
+
+   serv_ctrl
+     #(.RESET_PC (RESET_PC),
+       .RESET_STRATEGY (RESET_STRATEGY),
+       .WITH_CSR (WITH_CSR))
+   ctrl
+     (
+      .clk        (clk),
+      .i_rst      (i_rst),
+      //State
+      .i_pc_en    (ctrl_pc_en),
+      .i_cnt12to31 (cnt12to31),
+      .i_cnt0     (cnt0),
+      .i_cnt1     (cnt1),
+      .i_cnt2     (cnt2),
+      //Control
+      .i_jump     (jump),
+      .i_jal_or_jalr (jal_or_jalr),
+      .i_utype    (utype),
+      .i_pc_rel   (pc_rel),
+      .i_trap     (trap | mret),
+      .i_iscomp    (iscomp),
+      //Data
+      .i_imm      (imm),
+      .i_buf      (bufreg_q),
+      .i_csr_pc   (csr_pc),
+      .o_rd       (ctrl_rd),
+      .o_bad_pc   (bad_pc),
+      //External
+      .o_ibus_adr (wb_ibus_adr));
+
+   serv_alu alu
+     (
+      .clk        (clk),
+      //State
+      .i_en       (cnt_en),
+      .i_cnt0     (cnt0),
+      .o_cmp      (alu_cmp),
+      //Control
+      .i_sub      (alu_sub),
+      .i_bool_op  (alu_bool_op),
+      .i_cmp_eq   (alu_cmp_eq),
+      .i_cmp_sig  (alu_cmp_sig),
+      .i_rd_sel   (alu_rd_sel),
+      //Data
+      .i_rs1      (rs1),
+      .i_op_b     (op_b),
+      .i_buf      (bufreg_q),
+      .o_rd       (alu_rd));
+
+   serv_rf_if
+     #(.WITH_CSR (WITH_CSR))
+   rf_if
+     (//RF interface
+      .i_cnt_en    (cnt_en),
+      .o_wreg0     (o_wreg0),
+      .o_wreg1     (o_wreg1),
+      .o_wen0      (o_wen0),
+      .o_wen1      (o_wen1),
+      .o_wdata0    (o_wdata0),
+      .o_wdata1    (o_wdata1),
+      .o_rreg0     (o_rreg0),
+      .o_rreg1     (o_rreg1),
+      .i_rdata0    (i_rdata0),
+      .i_rdata1    (i_rdata1),
+
+      //Trap interface
+      .i_trap      (trap),
+      .i_mret      (mret),
+      .i_mepc      (wb_ibus_adr[0]),
+      .i_mtval_pc  (mtval_pc),
+      .i_bufreg_q  (bufreg_q),
+      .i_bad_pc    (bad_pc),
+      .o_csr_pc    (csr_pc),
+      //CSR write port
+      .i_csr_en    (csr_en),
+      .i_csr_addr  (csr_addr),
+      .i_csr       (csr_in),
+      //RD write port
+      .i_rd_wen    (rd_en),
+      .i_rd_waddr  (rd_addr),
+      .i_ctrl_rd   (ctrl_rd),
+      .i_alu_rd    (alu_rd),
+      .i_rd_alu_en (rd_alu_en),
+      .i_csr_rd    (csr_rd),
+      .i_rd_csr_en (rd_csr_en),
+      .i_mem_rd    (mem_rd),
+      .i_rd_mem_en (rd_mem_en),
+
+      //RS1 read port
+      .i_rs1_raddr (rs1_addr),
+      .o_rs1       (rs1),
+      //RS2 read port
+      .i_rs2_raddr (rs2_addr),
+      .o_rs2       (rs2),
+
+      //CSR read port
+      .o_csr       (rf_csr_out));
+
+   serv_mem_if
+     #(.WITH_CSR (WITH_CSR[0:0]))
+   mem_if
+     (
+      .i_clk        (clk),
+      //State
+      .i_bytecnt    (mem_bytecnt),
+      .i_lsb        (lsb),
+      .o_byte_valid (byte_valid),
+      .o_misalign   (mem_misalign),
+      //Control
+      .i_mdu_op     (mdu_op),
+      .i_signed     (mem_signed),
+      .i_word       (mem_word),
+      .i_half       (mem_half),
+      //Data
+      .i_bufreg2_q  (bufreg2_q),
+      .o_rd         (mem_rd),
+      //External interface
+      .o_wb_sel     (o_dbus_sel));
+
+   generate
+      if (|WITH_CSR) begin : gen_csr
+	 serv_csr
+	   #(.RESET_STRATEGY (RESET_STRATEGY))
+	 csr
+	   (
+	    .i_clk        (clk),
+	    .i_rst        (i_rst),
+	    //State
+	    .i_trig_irq   (wb_ibus_ack),
+	    .i_en         (cnt_en),
+	    .i_cnt0to3    (cnt0to3),
+	    .i_cnt3       (cnt3),
+	    .i_cnt7       (cnt7),
+	    .i_cnt_done   (cnt_done),
+	    .i_mem_op     (!mtval_pc),
+	    .i_mtip       (i_timer_irq),
+	    .i_trap       (trap),
+	    .o_new_irq    (new_irq),
+	    //Control
+	    .i_e_op       (e_op),
+	    .i_ebreak     (ebreak),
+	    .i_mem_cmd    (o_dbus_we),
+	    .i_mstatus_en (csr_mstatus_en),
+	    .i_mie_en     (csr_mie_en    ),
+	    .i_mcause_en  (csr_mcause_en ),
+	    .i_csr_source (csr_source),
+	    .i_mret       (mret),
+	    .i_csr_d_sel  (csr_d_sel),
+	    //Data
+	    .i_rf_csr_out (rf_csr_out),
+	    .o_csr_in     (csr_in),
+	    .i_csr_imm    (csr_imm),
+	    .i_rs1        (rs1),
+	    .o_q          (csr_rd));
+      end else begin : gen_no_csr
+	 assign csr_in = 1'b0;
+	 assign csr_rd = 1'b0;
+	 assign new_irq = 1'b0;
+      end
+   endgenerate
+
+
+`ifdef RISCV_FORMAL
+   reg [31:0] 	 pc = RESET_PC;
+
+   wire rs_en = two_stage_op ? init : ctrl_pc_en;
+
+   always @(posedge clk) begin
+      /* End of instruction */
+      rvfi_valid <= cnt_done & ctrl_pc_en & !i_rst;
+      rvfi_order <= rvfi_order + {63'd0,rvfi_valid};
+
+      /* Get instruction word when it's fetched from ibus */
+      if (wb_ibus_cyc & wb_ibus_ack)
+	rvfi_insn <= i_wb_rdt;
+
+      /* Store data written to rd */
+      if (o_wen0)
+        rvfi_rd_wdata <= {o_wdata0,rvfi_rd_wdata[31:1]};
+
+      if (cnt_done & ctrl_pc_en) begin
+         rvfi_pc_rdata <= pc;
+	 if (!(rd_en & (|rd_addr))) begin
+	   rvfi_rd_addr <= 5'd0;
+	   rvfi_rd_wdata <= 32'd0;
+	 end
+      end
+      rvfi_trap <= trap;
+      if (rvfi_valid) begin
+         rvfi_trap <= 1'b0;
+         pc <= rvfi_pc_wdata;
+      end
+
+      /* Not used */
+      rvfi_halt <= 1'b0;
+      rvfi_intr <= 1'b0;
+      rvfi_mode <= 2'd3;
+      rvfi_ixl = 2'd1;
+
+      /* RS1 not valid during J, U instructions (immdec_en[1]) */
+      /* RS2 not valid during I, J, U instructions (immdec_en[2]) */
+      if (i_rf_ready) begin
+	 rvfi_rs1_addr <= !immdec_en[1] ? rs1_addr : 5'd0;
+         rvfi_rs2_addr <= !immdec_en[2] /*rs2_valid*/ ? rs2_addr : 5'd0;
+	 rvfi_rd_addr  <= rd_addr;
+      end
+      if (rs_en) begin
+         rvfi_rs1_rdata <= {!immdec_en[1] & rs1,rvfi_rs1_rdata[31:1]};
+         rvfi_rs2_rdata <= {!immdec_en[2] & rs2,rvfi_rs2_rdata[31:1]};
+      end
+
+      if (i_dbus_ack) begin
+         rvfi_mem_addr <= o_dbus_adr;
+         rvfi_mem_rmask <= o_dbus_we ? 4'b0000 : o_dbus_sel;
+         rvfi_mem_wmask <= o_dbus_we ? o_dbus_sel : 4'b0000;
+         rvfi_mem_rdata <= i_dbus_rdt;
+         rvfi_mem_wdata <= o_dbus_dat;
+      end
+      if (wb_ibus_ack) begin
+         rvfi_mem_rmask <= 4'b0000;
+         rvfi_mem_wmask <= 4'b0000;
+      end
+   end
+   /* verilator lint_off COMBDLY */
+   always @(wb_ibus_adr)
+     rvfi_pc_wdata <= wb_ibus_adr;
+   /* verilator lint_on COMBDLY */
+
+
+`endif
+
+generate
+  if (MDU) begin: gen_mdu
+    assign dbus_rdt = i_ext_ready ? i_ext_rd:i_dbus_rdt;
+    assign dbus_ack = i_dbus_ack | i_ext_ready;
+  end else begin : gen_no_mdu
+    assign dbus_rdt = i_dbus_rdt;
+    assign dbus_ack = i_dbus_ack;
+  end
+  assign o_ext_rs2 = o_dbus_dat;
+endgenerate
+
+endmodule
+`default_nettype wire

fw/rtl/top.sv

@@ -14,6 +14,9 @@ module top (
     input n64_si_clk,
     inout n64_si_dq,
 
+    input n64_cic_clk,
+    inout n64_cic_dq,
+
     input usb_pwrsav,
     output usb_clk,
     output usb_cs,
@@ -48,8 +51,6 @@ module top (
     output mcu_miso,
 
     // Unused I/O
-    output n64_cic_clk,
-    output n64_cic_dq,
     output n64_video_sync
 );
 
@@ -137,7 +138,10 @@ module top (
         .n64_pi_ad(n64_pi_ad),
 
         .n64_si_clk(n64_si_clk),
-        .n64_si_dq(n64_si_dq)
+        .n64_si_dq(n64_si_dq),
+
+        .n64_cic_clk(n64_cic_clk),
+        .n64_cic_dq(n64_cic_dq)
     );
 
 
@@ -272,8 +276,6 @@ module top (
 
     // Unused I/O
 
-    assign n64_cic_clk = 1'bZ;
-    assign n64_cic_dq = 1'bZ;
     assign n64_video_sync = 1'bZ;
 
 endmodule

sw/bootloader/Makefile

@@ -1,9 +1,14 @@
-TOOLCHAIN = mips64-elf-
+N64_BINDIR = $(N64_INST)/bin
+
+TOOLCHAIN = $(N64_BINDIR)/mips64-elf-
 CC = $(TOOLCHAIN)gcc
 CXX = $(TOOLCHAIN)g++
 OBJCOPY = $(TOOLCHAIN)objcopy
 OBJDUMP = $(TOOLCHAIN)objdump
+STRIP = $(TOOLCHAIN)strip
 SIZE = $(TOOLCHAIN)size
+N64_ELFCOMPRESS = $(N64_BINDIR)/n64elfcompress
+N64_TOOL = $(N64_BINDIR)/n64tool
 PYTHON = python3
 
 FLAGS = -march=vr4300 -mtune=vr4300 $(USER_FLAGS)
@@ -19,7 +24,7 @@ BUILD_DIR = build
 SRC_FILES = \
 	startup.S \
 	boot.c \
-	crc32.c \
+	cic.c \
 	display.c \
 	error.c \
 	exception.c \
@@ -69,19 +74,20 @@ $(BUILD_DIR)/%.asset.o: $(BUILD_DIR)/%.asset $(ASSET_DIR)/assets.S
 
 $(BUILD_DIR)/bootloader.elf: $(OBJS) N64.ld
 	$(CXX) $(FLAGS) $(LDFLAGS) -TN64.ld $(OBJS) -o $@
-	@$(OBJDUMP) -S $@ > $(BUILD_DIR)/bootloader.lst
-
-$(BUILD_DIR)/bootloader.bin: $(BUILD_DIR)/bootloader.elf tools/finalize.py
-	@$(OBJCOPY) -O binary $< $@
-	@$(PYTHON) tools/finalize.py $@ > /dev/null
-
-print_size: $(BUILD_DIR)/bootloader.elf
 	@echo 'Size of modules:'
 	@$(SIZE) -B -d -t --common $(OBJS)
 	@echo 'Size of bootloader:'
-	@$(SIZE) -B -d $<
+	@$(SIZE) -B -d $@
+	@$(OBJDUMP) -S $@ > $(BUILD_DIR)/bootloader.lst
 
-all: $(BUILD_DIR)/bootloader.bin print_size
+$(BUILD_DIR)/bootloader.bin: $(BUILD_DIR)/bootloader.elf
+	@cp $< $<.stripped
+	@$(STRIP) -s $<.stripped
+	@cp $<.stripped $<.compressed
+	@$(N64_ELFCOMPRESS) -c 2 -o $(dir $<) $<.compressed
+	@$(N64_TOOL) --title "SC64 bootloader" --output $@ --align 256 $<.compressed
+
+all: $(BUILD_DIR)/bootloader.bin
 
 clean:
 	@rm -rf ./$(BUILD_DIR)/*

sw/bootloader/N64.ld

@@ -1,57 +1,84 @@
 MEMORY {
-    framebuffer (rw)    : org = 0x8026A000, len = 600k
+    exceptions (rx)     : org = 0x80000000, len = 0x400
-    ram (rwx)           : org = 0x80300000, len = 1M
+    ram (rwx)           : org = 0x80000400, len = 1M - 0x400
-    rom (r)             : org = 0xB0000000, len = 1028k
+    framebuffer (rw)    : org = 0x80100000, len = 600k
 }
 
 ENTRY(entry_handler)
 
-__exception_stack_size = 8k;
 __stack_size = 16k;
+__exception_stack_size = 8k;
 
 SECTIONS {
-    .boot : {
+    .exceptions : {
-        KEEP(*(.text.rom_header));
+        KEEP(*(.text.exception_vector));
-        KEEP(*(.text.ipl3));
+        . = ALIGN(8);
-    } > rom
+    } > exceptions
+
+    .text : {
+        *(.text.entry_handler)
+        . = ALIGN(8);
+        *(.text .text.* .gnu.linkonce.t.*)
+        . = ALIGN(8);
+    } > ram
+
+    .assets : {
+        *(.assets .assets.*)
+        . = ALIGN(8);
+    } > ram
+
+    .rodata : {
+        *(.rdata .rodata .rodata.* .gnu.linkonce.r.*)
+        . = ALIGN(8);
+    } > ram
+
+    .data : {
+        *(.data .data.* .gnu.linkonce.d.*)
+        . = ALIGN(8);
+    } > ram
+
+    .sdata : {
+        _gp = . + 0x8000;
+        *(.sdata .sdata.* .gnu.linkonce.s.*)
+        . = ALIGN(8);
+    } > ram
+
+    .lit8 : {
+        *(.lit8)
+        . = ALIGN(8);
+    } > ram
+
+    .lit4 : {
+        *(.lit4)
+        . = ALIGN(8);
+    } > ram
+
+    .sbss : {
+        *(.sbss .sbss.* .gnu.linkonce.sb.*)
+        *(.scommon .scommon.*)
+        . = ALIGN(8);
+    } > ram
+
+    .bss : {
+        *(.bss .bss.* .gnu.linkonce.b.*)
+        *(COMMON)
+        . = ALIGN(8);
+    } > ram
+
+    _sheap = .;
+    . = ORIGIN(ram) + LENGTH(ram) - __stack_size - __exception_stack_size;
+    _eheap = .;
+
+    . += __stack_size;
+    _sp = .;
+
+    . += __exception_stack_size;
+    _esp = .;
 
     .framebuffer (NOLOAD) : SUBALIGN(64) {
         *(.framebuffer .framebuffer.*)
     } > framebuffer
 
-    .text : SUBALIGN(8) {
-        *(.text.entry_handler)
-        *(.text .text.* .gnu.linkonce.t.*)
-        *(.assets .assets.*)
-        *(.rodata .rodata.* .gnu.linkonce.r.*)
-        *(.data .data.* .gnu.linkonce.d.*)
-        . = ALIGN(8);
-        _gp = . + 0x8000;
-        *(.sdata .sdata.* .gnu.linkonce.s.*)
-        *(.lit8 .lit4)
-    } > ram AT > rom
-
-    .bss : {
-        . = ALIGN(8);
-        _sbss = .;
-        *(.sbss .sbss.* .gnu.linkonce.sb.*)
-        *(.scommon .scommon.*)
-        *(.bss .bss.* .gnu.linkonce.b.*)
-        *(COMMON)
-        . = ALIGN(8);
-        _ebss = .;
-    } > ram
-
-    _sheap = .;
-    . = ORIGIN(ram) + LENGTH(ram) - __exception_stack_size - __stack_size;
-    _eheap = .;
-
-    . += __exception_stack_size;
-    _esp = .;
-
-    . += __stack_size;
-    _sp = .;
-
     /DISCARD/ : {
         *(.MIPS.*)
     }

sw/bootloader/assets/assets.S

@@ -1,4 +1,5 @@
 .section .assets.@sym@, "a", %progbits
+.balign 8
 .type assets_@sym@, %object
 .global assets_@sym@
 assets_@sym@:

sw/bootloader/src/boot.c

@@ -1,5 +1,6 @@
 #include "boot.h"
-#include "crc32.h"
+#include "cic.h"
+#include "init.h"
 #include "io.h"
 #include "vr4300.h"
 
@@ -8,28 +9,6 @@ extern uint32_t reboot_start __attribute__((section(".text")));
 extern size_t reboot_size __attribute__((section(".text")));
 
 
-typedef struct {
-    const uint32_t crc32;
-    const uint8_t seed;
-} ipl3_crc32_t;
-
-static const ipl3_crc32_t ipl3_crc32[] = {
-    { .crc32 = 0x587BD543, .seed = 0xAC },  // 5101
-    { .crc32 = 0x6170A4A1, .seed = 0x3F },  // 6101
-    { .crc32 = 0x009E9EA3, .seed = 0x3F },  // 7102
-    { .crc32 = 0x90BB6CB5, .seed = 0x3F },  // 6102/7101
-    { .crc32 = 0x0B050EE0, .seed = 0x78 },  // x103
-    { .crc32 = 0x98BC2C86, .seed = 0x91 },  // x105
-    { .crc32 = 0xACC8580A, .seed = 0x85 },  // x106
-    { .crc32 = 0x0E018159, .seed = 0xDD },  // 5167
-    { .crc32 = 0x10C68B18, .seed = 0xDD },  // NDXJ0
-    { .crc32 = 0xBC605D0A, .seed = 0xDD },  // NDDJ0
-    { .crc32 = 0x502C4466, .seed = 0xDD },  // NDDJ1
-    { .crc32 = 0x0C965795, .seed = 0xDD },  // NDDJ2
-    { .crc32 = 0x8FEBA21E, .seed = 0xDE },  // NDDE0
-};
-
-
 static io32_t *boot_get_device_base (boot_params_t *params) {
     io32_t *device_base_address = ROM_CART;
     if (params->device_type == BOOT_DEVICE_TYPE_64DD) {
@@ -38,37 +17,38 @@ static io32_t *boot_get_device_base (boot_params_t *params) {
     return device_base_address;
 }
 
-static bool boot_detect_cic_seed (boot_params_t *params) {
+static void boot_detect_cic_seed (boot_params_t *params) {
     io32_t *base = boot_get_device_base(params);
 
-    uint32_t ipl3[1008] __attribute__((aligned(8)));
+    uint8_t ipl3[IPL3_LENGTH] __attribute__((aligned(8)));
 
     pi_dma_read(&base[16], ipl3, sizeof(ipl3));
 
-    uint32_t crc32 = crc32_calculate(ipl3, sizeof(ipl3));
+    params->cic_seed = cic_get_seed(cic_detect(ipl3));
-
-    for (int i = 0; i < sizeof(ipl3_crc32) / sizeof(ipl3_crc32_t); i++) {
-        if (ipl3_crc32[i].crc32 == crc32) {
-            params->cic_seed = ipl3_crc32[i].seed;
-            return true;
-        }
-    }
-
-    return false;
 }
 
+
 void boot (boot_params_t *params) {
     if (params->tv_type == BOOT_TV_TYPE_PASSTHROUGH) {
-        params->tv_type = OS_INFO->tv_type;
+        switch (__tv_type) {
-    }
+            case INIT_TV_TYPE_PAL:
-
+                params->tv_type = BOOT_TV_TYPE_PAL;
-    if (params->detect_cic_seed) {
+                break;
-        if (!boot_detect_cic_seed(params)) {
+            case INIT_TV_TYPE_NTSC:
-            params->cic_seed = 0x3F;
+                params->tv_type = BOOT_TV_TYPE_NTSC;
+                break;
+            case INIT_TV_TYPE_MPAL:
+                params->tv_type = BOOT_TV_TYPE_MPAL;
+                break;
+            default:
+                params->tv_type = BOOT_TV_TYPE_NTSC;
+                break;
         }
     }
 
-    OS_INFO->mem_size_6105 = OS_INFO->mem_size;
+    if (params->detect_cic_seed) {
+        boot_detect_cic_seed(params);
+    }
 
     asm volatile (
         "li $t1, %[status] \n"

sw/bootloader/src/cic.c

@@ -0,0 +1,147 @@
+#include "cic.h"
+
+
+static inline uint32_t _get (uint8_t *p, int index) {
+    int i = index * 4;
+    return (p[i] << 24 | p[i + 1] << 16 | p[i + 2] << 8 | p[i + 3]);
+}
+
+static inline uint32_t _add (uint32_t a1, uint32_t a2) {
+    return a1 + a2;
+}
+
+static inline uint32_t _sub (uint32_t a1, uint32_t a2) {
+    return a1 - a2;
+}
+
+static inline uint32_t _mul (uint32_t a1, uint32_t a2) {
+    return a1 * a2;
+}
+
+static inline uint32_t _rol (uint32_t a, uint32_t s) {
+    return ((a) << (s)) | ((a) >> (-(s) & 31));
+}
+
+static inline uint32_t _ror (uint32_t a, uint32_t s) {
+    return ((a) >> (s)) | ((a) << (-(s) & 31));
+}
+
+static uint32_t _sum (uint32_t a0, uint32_t a1, uint32_t a2) {
+    uint64_t prod = ((uint64_t) (a0)) * (a1 == 0 ? a2 : a1);
+    uint32_t hi = (prod >> 32) & 0xFFFFFFFF;
+    uint32_t lo = prod & 0xFFFFFFFF;
+    uint32_t diff = hi - lo;
+    return (diff == 0) ? a0 : diff;
+};
+
+static uint64_t cic_calculate_ipl3_checksum (uint8_t *ipl3, uint8_t seed) {
+    const uint32_t MAGIC = 0x6C078965;
+
+    uint32_t data, prev, next;
+    data = prev = next = _get(ipl3, 0);
+
+    uint32_t init = _add(_mul(MAGIC, seed), 1) ^ data;
+
+    uint32_t buf[16];
+    for (int i = 0; i < 16; i++) {
+        buf[i] = init;
+    }
+
+    for (int i = 1; i <= 1008; i++) {
+        prev = data;
+        data = next;
+
+        buf[0] = _add(buf[0], _sum(_sub(1007, i), data, i));
+        buf[1] = _sum(buf[1], data, i);
+        buf[2] = buf[2] ^ data;
+        buf[3] = _add(buf[3], _sum(_add(data, 5), MAGIC, i));
+        buf[4] = _add(buf[4], _ror(data, prev & 0x1F));
+        buf[5] = _add(buf[5], _rol(data, prev >> 27));
+        buf[6] = (data < buf[6]) ? (_add(buf[3], buf[6]) ^ _add(data, i)) : (_add(buf[4], data) ^ buf[6]);
+        buf[7] = _sum(buf[7], _rol(data, prev & 0x1F), i);
+        buf[8] = _sum(buf[8], _ror(data, prev >> 27), i);
+        buf[9] = (prev < data) ? _sum(buf[9], data, i) : _add(buf[9], data);
+
+        if (i == 1008) {
+            break;
+        }
+
+        next = _get(ipl3, i);
+
+        buf[10] = _sum(_add(buf[10], data), next, i);
+        buf[11] = _sum(buf[11] ^ data, next, i);
+        buf[12] = _add(buf[12], buf[8] ^ data);
+        buf[13] = _add(buf[13], _add(_ror(data, data & 0x1F), _ror(next, next & 0x1F)));
+        buf[14] = _sum(_sum(buf[14], _ror(data, prev & 0x1F), i), _ror(next, data & 0x1F), i);
+        buf[15] = _sum(_sum(buf[15], _rol(data, prev >> 27), i), _rol(next, data >> 27), i);
+    }
+
+    uint32_t final_buf[4];
+    for (int i = 0; i < 4; i++) {
+        final_buf[i] = buf[0];
+    }
+
+    for (int i = 0; i < 16; i++) {
+        uint32_t data = buf[i];
+        final_buf[0] = _add(final_buf[0], _ror(data, data & 0x1F));
+        final_buf[1] = (data < final_buf[0]) ? _add(final_buf[1], data) : _sum(final_buf[1], data, i);
+        final_buf[2] = (((data & 0x02) >> 1) == (data & 0x01)) ? _add(final_buf[2], data) : _sum(final_buf[2], data, i);
+        final_buf[3] = ((data & 0x01) == 0x01) ? (final_buf[3] ^ data) : _sum(final_buf[3], data, i);
+    }
+
+    uint32_t final_sum = _sum(final_buf[0], final_buf[1], 16);
+    uint32_t final_xor = final_buf[3] ^ final_buf[2];
+
+    uint64_t checksum = (((((uint64_t) (final_sum)) & 0xFFFF)) << 32) | (final_xor);
+
+    return checksum;
+}
+
+
+cic_type_t cic_detect (uint8_t *ipl3) {
+    switch (cic_calculate_ipl3_checksum(ipl3, 0x3F)) {
+        case 0x45CC73EE317AULL: return CIC_6101;        // 6101
+        case 0x44160EC5D9AFULL: return CIC_7102;        // 7102
+        case 0xA536C0F1D859ULL: return CIC_6102_7101;   // 6102 / 7101
+    }
+    switch (cic_calculate_ipl3_checksum(ipl3, 0x78)) {
+        case 0x586FD4709867ULL: return CIC_x103;        // 6103 / 7103
+    }
+    switch (cic_calculate_ipl3_checksum(ipl3, 0x85)) {
+        case 0x2BBAD4E6EB74ULL: return CIC_x106;        // 6106 / 7106
+    }
+    switch (cic_calculate_ipl3_checksum(ipl3, 0x91)) {
+        case 0x8618A45BC2D3ULL: return CIC_x105;        // 6105 / 7105
+    }
+    switch (cic_calculate_ipl3_checksum(ipl3, 0xDD)) {
+        case 0x6EE8D9E84970ULL: return CIC_8401;        // NDXJ0
+        case 0x6C216495C8B9ULL: return CIC_8301;        // NDDJ0
+        case 0xE27F43BA93ACULL: return CIC_8302;        // NDDJ1
+        case 0x32B294E2AB90ULL: return CIC_8303;        // NDDJ2
+        case 0x083C6C77E0B1ULL: return CIC_5167;        // 64DD Cartridge conversion
+    }
+    switch (cic_calculate_ipl3_checksum(ipl3, 0xDE)) {
+        case 0x05BA2EF0A5F1ULL: return CIC_8501;        // NDDE0
+    }
+
+    return CIC_UNKNOWN;
+}
+
+uint8_t cic_get_seed (cic_type_t cic_type) {
+    switch (cic_type) {
+        case CIC_5101: return 0xAC;
+        case CIC_5167: return 0xDD;
+        case CIC_6101: return 0x3F;
+        case CIC_7102: return 0x3F;
+        case CIC_6102_7101: return 0x3F;
+        case CIC_x103: return 0x78;
+        case CIC_x105: return 0x91;
+        case CIC_x106: return 0x85;
+        case CIC_8301: return 0xDD;
+        case CIC_8302: return 0xDD;
+        case CIC_8303: return 0xDD;
+        case CIC_8401: return 0xDD;
+        case CIC_8501: return 0xDE;
+        default: return 0x3F;
+    }
+}

sw/bootloader/src/cic.h

@@ -0,0 +1,33 @@
+#ifndef CIC_H__
+#define CIC_H__
+
+
+#include <stdint.h>
+
+
+#define IPL3_LENGTH     (4032)
+
+
+typedef enum {
+    CIC_5101,
+    CIC_5167,
+    CIC_6101,
+    CIC_7102,
+    CIC_6102_7101,
+    CIC_x103,
+    CIC_x105,
+    CIC_x106,
+    CIC_8301,
+    CIC_8302,
+    CIC_8303,
+    CIC_8401,
+    CIC_8501,
+    CIC_UNKNOWN,
+} cic_type_t;
+
+
+cic_type_t cic_detect (uint8_t *ipl3);
+uint8_t cic_get_seed (cic_type_t cic_type);
+
+
+#endif

sw/bootloader/src/crc32.c

@@ -1,50 +0,0 @@
-#include "crc32.h"
-
-
-static const uint32_t crc32_table[256] = {
-    0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA, 0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
-    0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988, 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
-    0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE, 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
-    0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC, 0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
-    0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172, 0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
-    0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940, 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
-    0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116, 0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
-    0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924, 0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
-    0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A, 0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
-    0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818, 0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
-    0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E, 0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
-    0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C, 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
-    0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2, 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
-    0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0, 0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
-    0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086, 0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
-    0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4, 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
-    0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A, 0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
-    0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8, 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
-    0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE, 0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
-    0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC, 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
-    0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252, 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
-    0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60, 0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
-    0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236, 0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
-    0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04, 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
-    0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A, 0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
-    0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38, 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
-    0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E, 0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
-    0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C, 0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
-    0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2, 0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
-    0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0, 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
-    0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6, 0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
-    0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94, 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D,
-};
-
-
-uint32_t crc32_calculate (void *buffer, size_t length) {
-   uint32_t crc32 = 0xFFFFFFFFUL;
-
-   uint8_t *byte_pointer = (uint8_t *) (buffer);
-
-   while (length--) {
-        crc32 = (crc32 >> 8) ^ crc32_table[(crc32 & 0xFF) ^ (*byte_pointer++)];
-   }
-
-   return ~(crc32);
-}

sw/bootloader/src/crc32.h

@@ -1,12 +0,0 @@
-#ifndef CRC32_H__
-#define CRC32_H__
-
-
-#include <stddef.h>
-#include <stdint.h>
-
-
-uint32_t crc32_calculate (void *buffer, size_t length);
-
-
-#endif

sw/bootloader/src/display.c

@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include "display.h"
 #include "font.h"
+#include "init.h"
 #include "io.h"
 
 
@@ -67,24 +68,22 @@ static const vi_regs_t vi_config[] = {{
 
 
 static void display_decompress_background (uint32_t *background) {
-    uint32_t pixel_count = ((*background++) / sizeof(uint32_t));
-    uint32_t pixels_painted = 0;
-    uint8_t *background_data = (uint8_t *) (background);
     uint32_t *framebuffer = (uint32_t *) (display_framebuffer);
 
+    int pixel_count = (int) ((*background++) / 3);
+    int pixels_painted = 0;
+
     while (pixels_painted < pixel_count) {
-        int pixel_repeat = ((background_data[0]) + 1);
+        uint32_t pixel = *background++;
-        uint32_t pixel_value = (
+
-            ((background_data[1]) << 24) |
+        int pixel_repeat = (((pixel >> 24) & 0xFF) + 1);
-            ((background_data[2]) << 16) |
+        uint32_t pixel_value = (((pixel << 8) & 0xFFFFFF00) | 0xFF);
-            ((background_data[3]) << 8) |
+
-            (background_data[4])
-        );
         for (int i = 0; i < pixel_repeat; i++) {
             cpu_io_write(framebuffer++, pixel_value);
         }
+
         pixels_painted += pixel_repeat;
-        background_data += 5;
     }
 }
 
@@ -163,7 +162,7 @@ void display_init (uint32_t *background) {
     if (!vi_configured) {
         vi_configured = true;
 
-        const vi_regs_t *cfg = &vi_config[OS_INFO->tv_type];
+        const vi_regs_t *cfg = &vi_config[__tv_type];
 
         cpu_io_write(&VI->MADDR, (uint32_t) (display_framebuffer));
         cpu_io_write(&VI->H_WIDTH, cfg->H_WIDTH);

sw/bootloader/src/exception.S

@@ -3,10 +3,6 @@
 
 #define WATCHDOG_TIMEOUT            (5 * (93750000UL / 2))
 
-#define VECTOR_LOCATION             (0xA0000000UL)
-#define VECTOR_SIZE                 (0x80)
-#define VECTOR_NUM                  (4)
-
 #define ZR_OFFSET                   (0)
 #define AT_OFFSET                   (8)
 #define V0_OFFSET                   (16)
@@ -46,8 +42,23 @@
 #define SAVE_REGISTERS_SIZE         (280)
 
 
+.section .text.exception_vector
+exception_tlb_miss:
+    .org 0x0000
+    j exception_handler
+
+exception_xtlb_miss:
+    .org 0x0080
+    j exception_handler
+
+exception_other:
+    .org 0x0180
+    j exception_handler
+
+
 .section .text.exception_handler
 exception_handler:
+    .type exception_handler, %function
     .set noat
     la $k0, (_esp - SAVE_REGISTERS_SIZE)
     sd $zero, ZR_OFFSET($k0)
@@ -153,48 +164,9 @@ exception_restore:
     eret
 
 
-.section .text.exception_vector
-exception_vector:
-    .set noreorder
-    la $k0, exception_handler
-    jalr $k1, $k0
-    nop
-    .equ exception_vector_size, (. - exception_vector)
-    .set reorder
-
-
-.section .text.exception_install
-exception_install:
-    .global exception_install
-    la $t0, exception_vector
-    li $t1, VECTOR_LOCATION
-    li $t2, (VECTOR_SIZE * VECTOR_NUM)
-    add $t2, $t2, $t1
-1:
-    move $t3, $t0
-    move $t4, $t1
-    li $t5, exception_vector_size
-    add $t5, $t5, $t4
-2:
-    lw $t6, 0($t3)
-    addiu $t3, 4
-    sw $t6, 0($t4)
-    addiu $t4, 4
-    bne $t4, $t5, 2b
-    addiu $t1, VECTOR_SIZE
-    bne $t1, $t2, 1b
-    li $t0, VECTOR_LOCATION
-    li $t1, (VECTOR_SIZE * VECTOR_NUM)
-    add $t1, $t0, $t1
-3:
-    cache HIT_INVALIDATE_I, 0($t0)
-    addiu $t0, CACHE_LINE_SIZE_I
-    bne $t0, $t1, 3b
-    jr $ra
-
-
 .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
@@ -205,6 +177,7 @@ exception_enable_interrupts:
 
 .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)
@@ -215,6 +188,7 @@ exception_disable_interrupts:
 
 .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
@@ -228,6 +202,7 @@ exception_enable_watchdog:
 
 .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)

sw/bootloader/src/exception.h

@@ -7,7 +7,6 @@
 #define EXCEPTION_TRIGGER(code)     { asm volatile ("syscall %[c]\n" :: [c] "i" (code)); }
 
 
-void exception_install (void);
 void exception_enable_interrupts (void);
 void exception_disable_interrupts (void);
 void exception_enable_watchdog (void);

sw/bootloader/src/init.c

@@ -1,17 +1,20 @@
 #include "error.h"
 #include "exception.h"
+#include "init.h"
 #include "io.h"
 #include "sc64.h"
 #include "test.h"
 
 
-void init (void) {
+init_tv_type_t __tv_type;
+init_reset_type_t __reset_type;
+
+
+void init (init_tv_type_t tv_type, init_reset_type_t reset_type) {
     sc64_error_t error;
 
-    uint32_t pifram = si_io_read((io32_t *) (PIFRAM_STATUS));
+    __tv_type = tv_type;
-    si_io_write((io32_t *) (PIFRAM_STATUS), pifram | PIFRAM_TERMINATE_BOOT);
+    __reset_type = reset_type;
-
-    exception_install();
 
     sc64_unlock();
 
@@ -23,7 +26,7 @@ void init (void) {
     exception_enable_interrupts();
 
     if ((error = sc64_set_config(CFG_ID_BOOTLOADER_SWITCH, false)) != SC64_OK) {
-        error_display("Command SET_CONFIG [BOOTLOADER_SWITCH] failed: %d", error);
+        error_display("Command CONFIG_SET [BOOTLOADER_SWITCH] failed: %d", error);
     }
 
     if (test_check()) {
@@ -33,7 +36,8 @@ void init (void) {
 }
 
 void deinit (void) {
-    sc64_lock();
     exception_disable_interrupts();
     exception_disable_watchdog();
+
+    sc64_lock();
 }

sw/bootloader/src/init.h

@@ -2,7 +2,23 @@
 #define INIT_H__
 
 
-void init (void);
+typedef enum {
+    INIT_TV_TYPE_PAL = 0,
+    INIT_TV_TYPE_NTSC = 1,
+    INIT_TV_TYPE_MPAL = 2,
+} init_tv_type_t;
+
+typedef enum {
+    INIT_RESET_TYPE_COLD = 0,
+    INIT_RESET_TYPE_WARM = 1,
+} init_reset_type_t;
+
+
+extern init_tv_type_t __tv_type;
+extern init_reset_type_t __reset_type;
+
+
+void init (init_tv_type_t tv_type, init_reset_type_t reset_type);
 void deinit (void);
 
 

sw/bootloader/src/io.c

@@ -95,16 +95,3 @@ void pi_dma_write (io32_t *address, void *buffer, size_t length) {
     cpu_io_write(&PI->RDMA, length - 1);
     while (pi_busy());
 }
-
-uint32_t si_busy (void) {
-    return (cpu_io_read(&SI->SR) & (SI_SR_IO_BUSY | SI_SR_DMA_BUSY));
-}
-
-uint32_t si_io_read (io32_t *address) {
-    return cpu_io_read(address);
-}
-
-void si_io_write (io32_t *address, uint32_t value) {
-    cpu_io_write(address, value);
-    while (si_busy());
-}

sw/bootloader/src/io.h

@@ -202,26 +202,6 @@ typedef struct {
 #define PI_SR_CLR_INTR              (1 << 1)
 
 
-typedef struct {
-    io32_t MADDR;
-    io32_t RDMA;
-    io32_t __reserved_1;
-    io32_t __reserved_2;
-    io32_t WDMA;
-    io32_t __reserved_3;
-    io32_t SR;
-} si_regs_t;
-
-#define SI_BASE                     (0x04800000UL)
-#define SI                          ((si_regs_t *) SI_BASE)
-
-#define SI_SR_DMA_BUSY              (1 << 0)
-#define SI_SR_IO_BUSY               (1 << 1)
-#define SI_SR_DMA_ERROR             (1 << 3)
-#define SI_SR_INTERRUPT             (1 << 12)
-#define SI_SR_CLEAR_INTERRUPT       (0)
-
-
 #define ROM_DDIPL_BASE              (0x06000000UL)
 #define ROM_DDIPL                   ((io32_t *) ROM_DDIPL_BASE)
 
@@ -230,34 +210,6 @@ typedef struct {
 #define ROM_CART                    ((io32_t *) ROM_CART_BASE)
 
 
-#define PIFRAM_BASE                 (0x1FC007C0UL)
-#define PIFRAM                      ((io8_t *) PIFRAM_BASE)
-
-#define PIFRAM_STATUS               (&PIFRAM[0x3C])
-
-#define PIFRAM_TERMINATE_BOOT       (1 << 3)
-
-
-typedef struct {
-    uint32_t tv_type;
-    uint32_t device_type;
-    uint32_t device_base;
-    uint32_t reset_type;
-    uint32_t cic_id;
-    uint32_t version;
-    uint32_t mem_size;
-    uint8_t app_nmi_buffer[64];
-    uint32_t __reserved_1[37];
-    uint32_t mem_size_6105;
-} os_info_t;
-
-#define OS_INFO_BASE                (0x80000300UL)
-#define OS_INFO                     ((os_info_t *) OS_INFO_BASE)
-
-#define OS_INFO_RESET_TYPE_COLD     (0)
-#define OS_INFO_RESET_TYPE_NMI      (1)
-
-
 uint32_t c0_count (void);
 void delay_ms (int ms);
 uint32_t cpu_io_read (io32_t *address);
@@ -268,9 +220,6 @@ uint32_t pi_io_read (io32_t *address);
 void pi_io_write (io32_t *address, uint32_t value);
 void pi_dma_read (io32_t *address, void *buffer, size_t length);
 void pi_dma_write (io32_t *address, void *buffer, size_t length);
-uint32_t si_busy (void);
-uint32_t si_io_read (io32_t *address);
-void si_io_write (io32_t *address, uint32_t value);
 void cache_data_hit_writeback_invalidate (void *address, size_t length);
 void cache_data_hit_writeback (void *address, size_t length);
 void cache_inst_hit_invalidate (void *address, size_t length);

sw/bootloader/src/main.c

@@ -16,7 +16,7 @@ void main (void) {
 
     boot_params_t boot_params;
 
-    boot_params.reset_type = OS_INFO->reset_type;
+    boot_params.reset_type = BOOT_RESET_TYPE_COLD;
     boot_params.tv_type = sc64_boot_params.tv_type;
     boot_params.cic_seed = (sc64_boot_params.cic_seed & 0xFF);
     boot_params.detect_cic_seed = (sc64_boot_params.cic_seed == CIC_SEED_AUTO);
@@ -26,6 +26,8 @@ void main (void) {
             menu_load();
             boot_params.device_type = BOOT_DEVICE_TYPE_ROM;
             boot_params.reset_type = BOOT_RESET_TYPE_NMI;
+            boot_params.cic_seed = 0x3F;
+            boot_params.detect_cic_seed = false;
             break;
 
         case BOOT_MODE_ROM:

sw/bootloader/src/menu.c

@@ -6,7 +6,6 @@
 
 
 #define ROM_ADDRESS     (0x10000000)
-#define FILL_SIZE       (0x101000)
 
 
 static const char *fatfs_error_codes[] = {
@@ -45,43 +44,6 @@ static void fix_menu_file_size (FIL *fil) {
     fil->obj.objsize = ALIGN(f_size(fil), FF_MAX_SS);
 }
 
-static void fill_remaining_menu_space (size_t menu_file_size) {
-    if (menu_file_size >= FILL_SIZE) {
-        return;
-    }
-
-    sc64_error_t error;
-    uint32_t rom_write_enable_restore;
-
-    if ((error = sc64_get_config(CFG_ID_ROM_WRITE_ENABLE, &rom_write_enable_restore)) != SC64_OK) {
-        error_display("Command CONFIG_GET [CFG_ID_ROM_WRITE_ENABLE] failed: %d", error);
-    }
-
-    if ((error = sc64_set_config(CFG_ID_ROM_WRITE_ENABLE, true)) != SC64_OK) {
-        error_display("Command CONFIG_SET [CFG_ID_ROM_WRITE_ENABLE] failed: %d", error);
-    }
-
-    uint8_t fill_buffer[4096] __attribute__((aligned(8)));
-
-    for (int i = 0; i < sizeof(fill_buffer); i++) {
-        fill_buffer[i] = 0;
-    }
-
-    uint32_t address = (ROM_ADDRESS + menu_file_size);
-    uint32_t left = (FILL_SIZE - menu_file_size);
-
-    while (left > 0) {
-        size_t block = (left > sizeof(fill_buffer)) ? sizeof(fill_buffer) : left;
-        pi_dma_write((io32_t *) (address), fill_buffer, block);
-        address += block;
-        left -= block;
-    }
-
-    if ((error = sc64_set_config(CFG_ID_ROM_WRITE_ENABLE, rom_write_enable_restore)) != SC64_OK) {
-        error_display("Command CONFIG_SET [CFG_ID_ROM_WRITE_ENABLE] failed: %d", error);
-    }
-}
-
 
 void menu_load (void) {
     sc64_error_t error;
@@ -106,7 +68,6 @@ void menu_load (void) {
     fix_menu_file_size(&fil);
     FF_CHECK(f_read(&fil, (void *) (ROM_ADDRESS), f_size(&fil), &bytes_read), "Could not read menu file");
     FF_CHECK((bytes_read != f_size(&fil)) ? FR_INT_ERR : FR_OK, "Read size is different than expected");
-    fill_remaining_menu_space(f_size(&fil));
     FF_CHECK(f_close(&fil), "Could not close menu file");
     FF_CHECK(f_unmount(""), "Could not unmount drive");
 }

sw/bootloader/src/reboot.S

@@ -4,22 +4,13 @@
 
 #define RI_ADDRESS          0xA4700000
 
-#define RI_MODE             0x00
-#define RI_CONFIG           0x04
-#define RI_CURRENT_LOAD     0x08
 #define RI_SELECT           0x0C
 #define RI_REFRESH          0x10
-#define RI_LATENCY          0x14
 
-#define RI_MODE_RESET       0x00000000
-#define RI_MODE_STANDBY     0x0000000E
-
-#define RDRAM_RESET_DELAY   1024
-#define RDRAM_STANDBY_DELAY 512
 
 .set noat
 .section .text.reboot, "ax", %progbits
-.type reboot, %object
+
 reboot_start:
     .global reboot_start
 
@@ -43,39 +34,33 @@ reboot_entry:
 
     li $sp, STACK_ADDRESS
 
+reset_rdram:
     bnez $s5, reset_rdram_skip
 
-reset_rdram:
     li $t0, RI_ADDRESS
 
-    li $t1, RI_MODE_RESET
-    sw $t1, RI_MODE($t0)
-
-    li $t2, RDRAM_RESET_DELAY
-1:
-    addiu $t2, (-1)
-    bnez $t2, 1b
-
-    sw $zero, RI_CONFIG($t0)
-    sw $zero, RI_CURRENT_LOAD($t0)
-    sw $zero, RI_SELECT($t0)
     sw $zero, RI_REFRESH($t0)
-
+    sw $zero, RI_SELECT($t0)
-    li $t1, RI_MODE_STANDBY
-    sw $t1, RI_MODE($t0)
-
-    li $t2, RDRAM_STANDBY_DELAY
-1:
-    addiu $t2, (-1)
-    bnez $t2, 1b
 reset_rdram_skip:
 
-prepare_registers:
+detect_console_region:
-    la $t0, ra_table
+    li $t0, 1
-    sll $t1, $s4, 2
+    beq $s4, $zero, pal_console
-    add $t0, $t1
+    beq $s4, $t0, ntsc_console
-    lw $ra, ($t0)
+    b mpal_console
 
+pal_console:
+    li $ra, 0xA4001554
+    b prepare_registers
+
+ntsc_console:
+    li $ra, 0xA4001550
+    b prepare_registers
+
+mpal_console:
+    li $ra, 0xA4001554
+
+prepare_registers:
     move $at, $zero
     move $v0, $zero
     move $v1, $zero
@@ -85,7 +70,7 @@ prepare_registers:
     move $a3, $zero
     move $t0, $zero
     move $t1, $zero
-    move $t2, $zero
+    li $t2, 0x40
     move $t4, $zero
     move $t5, $zero
     move $t6, $zero
@@ -104,11 +89,5 @@ run_ipl3:
     li $t3, IPL3_ENTRY
     jr $t3
 
-ra_values:
-    .set ra_table, REBOOT_ADDRESS + (. - reboot_start)
-    .word 0xA4001554
-    .word 0xA4001550
-    .word 0xA4001554
-
     .set reboot_size, (. - reboot_start)
     .global reboot_size

sw/bootloader/src/startup.S

@@ -1,52 +1,15 @@
-#include "vr4300.h"
-
-
-.section .text.rom_header, "a", %progbits
-.type rom_header, %object
-rom_header:
-
-header_pi_config:
-    .word 0x80371240
-
-header_clock_rate:
-    .word 0x0000000F
-
-header_load_addr:
-    .word entry_handler
-
-header_sdk_version:
-    .word 0x00000000
-
-header_crc:
-    .fill 2, 4, 0
-
-    .org 0x20, 0x00
-header_text_info:
-    .ascii "SC64 bootloader"
-    .org 0x40, 0x00
-
-
-.section .text.ipl3, "a", %progbits
-.type ipl3, %object
-ipl3:
-    .incbin "header", 0x40
-
-
 .section .text.entry_handler, "ax", %progbits
-.type entry_handler, %function
+
 entry_handler:
+    .type entry_handler, %function
     .global entry_handler
 
     la $gp, _gp
     la $sp, _sp
 
-bss_init:
+    lui $t0, 0xA400
-    la $a0, _sbss
+    lbu $a0, 9($t0)     # TV type
-    la $a1, _ebss
+    lbu $a1, 10($t0)    # Reset type
-1:
-    sd $zero, 0($a0)
-    addiu $a0, 8
-    bltu $a0, $a1, 1b
 
     la $t0, init
     jalr $t0

sw/bootloader/src/test.c

@@ -2,6 +2,7 @@
 #include <stdlib.h>
 #include "display.h"
 #include "error.h"
+#include "init.h"
 #include "io.h"
 #include "sc64.h"
 #include "test.h"
@@ -224,7 +225,7 @@ bool test_check (void) {
     sc64_error_t error;
     uint32_t button_state;
 
-    if (OS_INFO->reset_type != OS_INFO_RESET_TYPE_COLD) {
+    if (__reset_type == INIT_RESET_TYPE_WARM) {
         return false;
     }
 

sw/bootloader/tools/asset_converter.py

@@ -47,10 +47,10 @@ if __name__ == '__main__':
 
     try:
         source_asset = Image.open(asset_input)
-        converted_asset = source_asset.convert('RGBA').tobytes()
+        converted_asset = source_asset.convert('RGB').tobytes()
 
         if (asset_compress):
-            converted_asset = compress(converted_asset)
+            converted_asset = compress(converted_asset, step_size=3)
 
         final_asset = open(asset_output, 'wb')
         final_asset.write(converted_asset)

sw/bootloader/tools/finalize.py

@@ -1,50 +0,0 @@
-#!/usr/bin/env python3
-
-import subprocess
-import sys
-
-
-
-if __name__ == '__main__':
-    if (len(sys.argv) != 2):
-        print(f'Usage: python {sys.argv[0]} file_path')
-        sys.exit(1)
-
-    ALIGN = 1024
-    CHECKSUM_SIZE = 0x101000
-
-    bin_file = sys.argv[1]
-
-    try:
-        bin_data = b''
-
-        with open(bin_file, 'rb') as f:
-            bin_data = f.read()
-
-        pad_size = CHECKSUM_SIZE - len(bin_data)
-
-        if (pad_size > 0):
-            bin_data += b'\xFF' * pad_size
-            with open(bin_file, 'wb') as f:
-                f.write(bin_data)
-
-        subprocess.run(['chksum64', bin_file])
-
-        with open(bin_file, 'rb') as f:
-            bin_data = f.read()
-
-        bin_data = bin_data.strip(b'\xFF')
-        modulo = len(bin_data) % ALIGN
-        if (modulo > 0):
-            bin_data += b'\xFF' * (ALIGN - modulo)
-
-        with open(bin_file, 'wb') as f:
-            f.write(bin_data)
-
-    except FileNotFoundError as e:
-        print(f'Couldn\'t open file "{bin_file}" {e}')
-        sys.exit(2)
-
-    except Exception as e:
-        print(e)
-        sys.exit(3)

sw/cic/.gitignore

@@ -0,0 +1 @@
+/build

sw/cic/build.sh

@@ -0,0 +1,34 @@
+#!/bin/bash
+
+set -e
+
+TOOLCHAIN="riscv32-unknown-elf-"
+CFLAGS=" \
+    -march=rv32i \
+    -mabi=ilp32 \
+    -Os \
+    -Wl,--gc-sections \
+    -ffunction-sections \
+    -fdata-sections \
+    -ffreestanding \
+    -nostartfiles \
+    -nostdlib \
+    -nodefaultlibs \
+    -fno-builtin \
+    -mcmodel=medany \
+"
+
+case "$1" in
+    all)
+        mkdir -p ./build
+        ${TOOLCHAIN}gcc $CFLAGS -T cic.ld -o ./build/cic.elf startup.S cic.c
+        echo "Size of cic:"
+        ${TOOLCHAIN}size -B -d ./build/cic.elf
+        ${TOOLCHAIN}objdump -S -D ./build/cic.elf > ./build/cic.lst
+        ${TOOLCHAIN}objcopy -O binary ./build/cic.elf ./build/cic.bin
+        python3 ./convert.py ./build/cic.bin ./build/cic.mem
+        ;;
+    clean)
+        rm -rf ./build/*
+        ;;
+esac

sw/cic/cic.c

@@ -0,0 +1,361 @@
+// Original code sourced from https://github.com/jago85/UltraCIC_C
+
+// MIT License
+
+// Copyright (c) 2019 Jan Goldacker
+// Copyright (c) 2022-2023 Mateusz Faderewski
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+
+typedef struct {
+    volatile uint32_t CIC_CONFIG[2];
+    volatile uint32_t GPIO;
+} ext_regs_t;
+
+#define EXT ((ext_regs_t *) (0xC0000000UL))
+
+#define CIC_DQ                      (1 << 0)
+#define CIC_CLK                     (1 << 1)
+#define CIC_RESET                   (1 << 2)
+#define CIC_INVALID_REGION          (1 << 3)
+
+#define CIC_IS_RUNNING()            (EXT->GPIO & CIC_RESET)
+#define CIC_CLK_WAIT_LOW()          { while ((EXT->GPIO & (CIC_RESET | CIC_CLK)) == (CIC_RESET | CIC_CLK)); }
+#define CIC_CLK_WAIT_HIGH()         { while ((EXT->GPIO & (CIC_RESET | CIC_CLK)) == CIC_RESET); }
+#define CIC_DQ_GET()                (EXT->GPIO & CIC_DQ)
+#define CIC_DQ_SET(v)               { EXT->GPIO = ((v) ? CIC_DQ : 0); }
+#define CIC_CLK_GET()               (EXT->GPIO & (CIC_RESET | CIC_CLK))
+#define CIC_NOTIFY_INVALID_REGION() { EXT->GPIO = (CIC_INVALID_REGION | CIC_DQ); }
+
+
+typedef struct {
+    bool cic_disabled;
+    bool cic_64dd_mode;
+    bool cic_region;
+    uint8_t cic_seed;
+    uint8_t cic_checksum[6];
+} cic_config_t;
+
+static cic_config_t config;
+
+static uint8_t cic_ram[32];
+static uint8_t cic_x105_ram[30];
+
+static const uint8_t cic_ram_init[2][32] = {{
+    0xE, 0x0, 0x9, 0xA, 0x1, 0x8, 0x5, 0xA, 0x1, 0x3, 0xE, 0x1, 0x0, 0xD, 0xE, 0xC,
+    0x0, 0xB, 0x1, 0x4, 0xF, 0x8, 0xB, 0x5, 0x7, 0xC, 0xD, 0x6, 0x1, 0xE, 0x9, 0x8
+}, {
+    0xE, 0x0, 0x4, 0xF, 0x5, 0x1, 0x2, 0x1, 0x7, 0x1, 0x9, 0x8, 0x5, 0x7, 0x5, 0xA,
+    0x0, 0xB, 0x1, 0x2, 0x3, 0xF, 0x8, 0x2, 0x7, 0x1, 0x9, 0x8, 0x1, 0x1, 0x5, 0xC
+}};
+
+
+static void cic_die (void) {
+    while (CIC_IS_RUNNING());
+}
+
+static void cic_init (void) {
+    CIC_DQ_SET(1);
+
+    while (!CIC_IS_RUNNING());
+
+    uint32_t cic_config[2];
+
+    cic_config[0] = EXT->CIC_CONFIG[0];
+    cic_config[1] = EXT->CIC_CONFIG[1];
+
+    config.cic_disabled = (cic_config[0] & (1 << 26));
+    config.cic_64dd_mode = (cic_config[0] & (1 << 25));
+    config.cic_region = (cic_config[0] & (1 << 24));
+    config.cic_seed = ((cic_config[0] >> 16) & 0xFF);
+    config.cic_checksum[0] = ((cic_config[0] >> 8) & 0xFF);
+    config.cic_checksum[1] = (cic_config[0] & 0xFF);
+    config.cic_checksum[2] = ((cic_config[1] >> 24) & 0xFF);
+    config.cic_checksum[3] = ((cic_config[1] >> 16) & 0xFF);
+    config.cic_checksum[4] = ((cic_config[1] >> 8) & 0xFF);
+    config.cic_checksum[5] = (cic_config[1] & 0xFF);
+
+    if (config.cic_disabled) {
+        cic_die();
+    }
+}
+
+static uint8_t cic_read (void) {
+    uint8_t value;
+    CIC_CLK_WAIT_LOW();
+    value = CIC_DQ_GET() ? 1 : 0;
+    CIC_CLK_WAIT_HIGH();
+    return value;
+}
+
+static void cic_write (uint8_t value) {
+    CIC_CLK_WAIT_LOW();
+    CIC_DQ_SET(value);
+    CIC_CLK_WAIT_HIGH();
+    CIC_DQ_SET(1);
+}
+
+static uint8_t cic_read_nibble (void) {
+    uint8_t data = 0;
+    for (int i = 0; i < 4; i++) {
+        data = ((data << 1) | cic_read());
+    }
+    return data;
+}
+
+static void cic_write_nibble (uint8_t data) {
+    cic_write(data & 0x08);
+    cic_write(data & 0x04);
+    cic_write(data & 0x02);
+    cic_write(data & 0x01);
+}
+
+static void cic_write_ram_nibbles (uint8_t index) {
+    do {
+        cic_write_nibble(cic_ram[index++]);
+    } while ((index & 0x0F) != 0);
+}
+
+static void cic_encode_round (uint8_t index) {
+    uint8_t data = cic_ram[index++];
+    do {
+        data = ((((data + 1) & 0x0F) + cic_ram[index]) & 0x0F);
+        cic_ram[index++] = data;
+    } while ((index & 0x0F) != 0);
+}
+
+static void cic_write_id (void) {
+    if (config.cic_64dd_mode) {
+        CIC_CLK_WAIT_LOW();
+        while (CIC_CLK_GET() == CIC_RESET) {
+            if (!CIC_DQ_GET()) {
+                cic_die();
+            }
+        }
+    } else {
+        cic_write(0);
+    }
+    cic_write(config.cic_region ? 1 : 0);
+    cic_write(0);
+    cic_write(1);
+}
+
+static void cic_write_seed (void) {
+    cic_ram[0x0A] = 0x0B;
+    cic_ram[0x0B] = 0x05;
+    cic_ram[0x0C] = (config.cic_seed >> 4);
+    cic_ram[0x0D] = config.cic_seed;
+    cic_ram[0x0E] = (config.cic_seed >> 4);
+    cic_ram[0x0F] = config.cic_seed;
+    cic_encode_round(0x0A);
+    cic_encode_round(0x0A);
+
+    uint32_t timeout = 10000;
+    do {
+        if (timeout == 0) {
+            CIC_NOTIFY_INVALID_REGION();
+            cic_die();
+        }
+    } while (timeout-- && (CIC_CLK_GET() == (CIC_RESET | CIC_CLK)));
+
+    cic_write_ram_nibbles(0x0A);
+}
+
+static void cic_write_checksum (void) {
+    for (int i = 0; i < 4; i++) {
+        cic_ram[i] = 0x00;
+    }
+    for (int i = 0; i < 6; i++) {
+        cic_ram[(i * 2) + 4] = ((config.cic_checksum[i] >> 4) & 0x0F);
+        cic_ram[(i * 2) + 5] = (config.cic_checksum[i] & 0x0F);
+    }
+    cic_encode_round(0x00);
+    cic_encode_round(0x00);
+    cic_encode_round(0x00);
+    cic_encode_round(0x00);
+    cic_write(0);
+    cic_write_ram_nibbles(0x00);
+}
+
+static void cic_init_ram (void) {
+    for (int i = 0; i < 32; i++) {
+        cic_ram[i] = cic_ram_init[config.cic_region ? 1 : 0][i];
+    }
+    cic_ram[0x01] = cic_read_nibble();
+    cic_ram[0x11] = cic_read_nibble();
+}
+
+static void cic_exchange_bytes (uint8_t *a, uint8_t *b) {
+    uint8_t tmp = *a;
+    *a = *b;
+    *b = tmp;
+}
+
+static void cic_round (uint8_t *m) {
+    uint8_t a, b, x;
+
+    x = m[15];
+    a = x;
+
+    do {
+        b = 1;
+        a += (m[b] + 1);
+        m[b] = a;
+        b++;
+        a += (m[b] + 1);
+        cic_exchange_bytes(&a, &m[b]);
+        m[b] = ~(m[b]);
+        b++;
+        a &= 0x0F;
+        a += ((m[b] & 0x0F) + 1);
+        if (a < 16) {
+            cic_exchange_bytes(&a, &m[b]);
+            b++;
+        }
+        a += m[b];
+        m[b] = a;
+        b++;
+        a += m[b];
+        cic_exchange_bytes(&a, &m[b]);
+        b++;
+        a &= 0x0F;
+        a += 8;
+        if (a < 16) {
+            a += m[b];
+        }
+        cic_exchange_bytes(&a, &m[b]);
+        b++;
+        do {
+            a += (m[b] + 1);
+            m[b] = a;
+            b++;
+            b &= 0x0F;
+        } while (b != 0);
+        a = (x + 0x0F);
+        x = (a & 0x0F);
+    } while (x != 0x0F);
+}
+
+static void cic_compare_mode (void) {
+    cic_round(&cic_ram[0x10]);
+    cic_round(&cic_ram[0x10]);
+    cic_round(&cic_ram[0x10]);
+
+    uint8_t index = (cic_ram[0x17] & 0x0F);
+    if (index == 0) {
+        index = 1;
+    }
+    index |= 0x10;
+
+    do {
+        cic_read();
+        cic_write(cic_ram[index] & 0x01);
+        if (config.cic_region) {
+            index--;
+        } else {
+            index++;
+        }
+    } while (index & 0x0F);
+}
+
+static void cic_x105_algorithm (void) {
+    uint8_t a = 5;
+    uint8_t carry = 1;
+
+    for (int i = 0; i < 30; ++i) {
+        if (!(cic_x105_ram[i] & 0x01)) {
+            a += 8;
+        }
+        if (!(a & 0x02)) {
+            a += 4;
+        }
+        a = ((a + cic_x105_ram[i]) & 0x0F);
+        cic_x105_ram[i] = a;
+        if (!carry) {
+            a += 7;
+        }
+        a = ((a + cic_x105_ram[i]) & 0x0F);
+        a = (a + cic_x105_ram[i] + carry);
+        if (a >= 0x10) {
+            carry = 1;
+            a -= 0x10;
+        } else {
+            carry = 0;
+        }
+        a = (~(a) & 0x0F);
+        cic_x105_ram[i] = a;
+    }
+}
+
+static void cic_x105_mode (void) {
+    cic_write_nibble(0x0A);
+    cic_write_nibble(0x0A);
+
+    for (int i = 0; i < 30; i++) {
+        cic_x105_ram[i] = cic_read_nibble();
+    }
+
+    cic_x105_algorithm();
+
+    cic_write(0);
+
+    for (int i = 0; i < 30; i++) {
+        cic_write_nibble(cic_x105_ram[i]);
+    }
+}
+
+static void cic_soft_reset (void) {
+    volatile uint32_t timeout = 119050; // ~500 ms delay, measured on real hardware
+    CIC_CLK_WAIT_LOW();
+    while ((timeout--) && CIC_IS_RUNNING());
+    cic_write(0);
+}
+
+
+__attribute__((naked)) void cic_main (void) {
+    while (true) {
+        cic_init();
+
+        cic_write_id();
+        cic_write_seed();
+        cic_write_checksum();
+        cic_init_ram();
+
+        while (CIC_IS_RUNNING()) {
+            uint8_t cmd = 0;
+            cmd |= (cic_read() << 1);
+            cmd |= cic_read();
+
+            if (cmd == 0) {
+                cic_compare_mode();
+            } else if (cmd == 2) {
+                cic_x105_mode();
+            } else if (cmd == 3) {
+                cic_soft_reset();
+            } else {
+                cic_die();
+            }
+        }
+    }
+}

sw/cic/cic.ld

@@ -0,0 +1,46 @@
+MEMORY {
+    ram (rwx) : org = 0x80000000, len = 2k
+}
+
+ENTRY(entry_handler)
+
+SECTIONS {
+    .text : {
+        *(.text.entry_handler)
+        . = ALIGN(4);
+        *(.text .text.* .gnu.linkonce.t.*)
+        . = ALIGN(4);
+    } > ram
+
+    .rodata : {
+        *(.rdata .rodata .rodata.* .gnu.linkonce.r.*)
+        . = ALIGN(4);
+    } > ram
+
+    .data : {
+        *(.data .data.* .gnu.linkonce.d.*)
+        . = ALIGN(4);
+    } > ram
+
+    .sdata : {
+        _gp = . + 0x800;
+        *(.sdata .sdata.* .gnu.linkonce.s.*)
+        . = ALIGN(4);
+    } > ram
+
+    .sbss : {
+        _sbss = .;
+        *(.sbss .sbss.* .gnu.linkonce.sb.*)
+        *(.scommon .scommon.*)
+        . = ALIGN(4);
+    } > ram
+
+    .bss : {
+        *(.bss .bss.* .gnu.linkonce.b.*)
+        *(COMMON)
+        . = ALIGN(4);
+        _ebss = .;
+    } > ram
+
+    _sp = ORIGIN(ram) + LENGTH(ram);
+}

sw/cic/convert.py

@@ -0,0 +1,21 @@
+#!/usr/bin/env python3
+
+import sys
+
+
+
+if __name__ == '__main__':
+    if (len(sys.argv) != 3):
+        print(f'Usage: python {sys.argv[0]} in_file out_file')
+        sys.exit(1)
+
+    with open(sys.argv[1], 'rb') as f:
+        output = ''
+        while True:
+            file_bytes = f.read(4)
+            if len(file_bytes) != 4:
+                break
+            output += f"{int.from_bytes(file_bytes, 'little'):08X}\n"
+
+    with open(sys.argv[2], 'w') as f:
+        f.write(output)

sw/cic/startup.S

@@ -0,0 +1,24 @@
+.option norvc
+.section .text.entry_handler
+entry_handler:
+    .global entry_handler
+
+init_stack_pointer:
+    .option push
+    .option norelax
+    la sp, _sp
+    la gp, _gp
+    .option pop
+
+init_bss:
+    la t5, _sbss
+    la t6, _ebss
+    beq t5, t6, 2f
+1:
+    sw zero, 0(t5)
+    addi t5, t5, 4
+    bltu t5, t6, 1b
+2:
+
+run_main:
+    tail cic_main

sw/controller/common.mk

@@ -6,7 +6,7 @@ OBJDUMP = $(TOOLCHAIN)objdump
 SIZE = $(TOOLCHAIN)size
 
 FLAGS = -mcpu=cortex-m0plus -mthumb -DSTM32G030xx $(USER_FLAGS) -g -ggdb3
-CFLAGS = -Os -Wall -ffunction-sections -fdata-sections -ffreestanding -MMD -MP -I./inc
+CFLAGS = -Os -Wall -ffunction-sections -fdata-sections -ffreestanding -MMD -MP -I. -isystem ./inc
 LDFLAGS = -nostartfiles -Wl,--gc-sections
 
 SRC_DIR = src

sw/controller/src/app.S

@@ -7,7 +7,7 @@
 .section .loader, "a", %progbits
 .type loader, %object
 loader:
-    .incbin "../build/loader/loader.bin"
+    .incbin "build/loader/loader.bin"
 
 
 .section .text.Reset_Handler

sw/controller/src/app.c

@@ -1,5 +1,4 @@
 #include "app.h"
-#include "cic.h"
 #include "gvr.h"
 #include "hw.h"
 #include "led.h"
@@ -7,20 +6,18 @@
 #include "task.h"
 
 
-#define CIC_STACK_SIZE  (256)
 #define RTC_STACK_SIZE  (256)
 #define LED_STACK_SIZE  (256)
 #define GVR_STACK_SIZE  (2048)
 
 
-uint8_t cic_stack[CIC_STACK_SIZE] __attribute__((aligned(8)));
 uint8_t rtc_stack[RTC_STACK_SIZE] __attribute__((aligned(8)));
 uint8_t led_stack[LED_STACK_SIZE] __attribute__((aligned(8)));
 uint8_t gvr_stack[GVR_STACK_SIZE] __attribute__((aligned(8)));
 
 
 void app_get_stack_usage (uint32_t *usage) {
-    *usage++ = task_get_stack_usage(cic_stack, CIC_STACK_SIZE);
+    *usage++ = 0;
     *usage++ = task_get_stack_usage(rtc_stack, RTC_STACK_SIZE);
     *usage++ = task_get_stack_usage(led_stack, LED_STACK_SIZE);
     *usage++ = task_get_stack_usage(gvr_stack, GVR_STACK_SIZE);
@@ -28,9 +25,7 @@ void app_get_stack_usage (uint32_t *usage) {
 
 void app (void) {
     hw_init();
-    cic_hw_init();
 
-    task_create(TASK_ID_CIC, cic_task, cic_stack, CIC_STACK_SIZE);
     task_create(TASK_ID_RTC, rtc_task, rtc_stack, RTC_STACK_SIZE);
     task_create(TASK_ID_LED, led_task, led_stack, LED_STACK_SIZE);
     task_create(TASK_ID_GVR, gvr_task, gvr_stack, GVR_STACK_SIZE);

sw/controller/src/cic.c

@@ -1,34 +1,8 @@
-// Original code sourced from https://github.com/jago85/UltraCIC_C
-
-// MIT License
-
-// Copyright (c) 2019 Jan Goldacker
-// Copyright (c) 2022 Mateusz Faderewski
-
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-
 #include "cic.h"
+#include "fpga.h"
 #include "hw.h"
 #include "led.h"
 #include "rtc.h"
-#include "task.h"
 
 
 typedef enum {
@@ -38,372 +12,86 @@ typedef enum {
 } cic_region_t;
 
 
-static volatile bool cic_enabled = false;
+static bool cic_initialized = false;
-static volatile bool cic_detect_enabled;
-
-static volatile uint8_t cic_next_rd;
-static volatile uint8_t cic_next_wr;
-
-static volatile bool cic_disabled = false;
-static volatile bool cic_dd_mode = false;
-static volatile uint8_t cic_seed = 0x3F;
-static volatile uint8_t cic_checksum[6] = { 0xA5, 0x36, 0xC0, 0xF1, 0xD8, 0x59 };
-
-static uint8_t cic_ram[32];
-static uint8_t cic_x105_ram[30];
-
-static const uint8_t cic_ram_init[2][32] = {{
-    0x0E, 0x00, 0x09, 0x0A, 0x01, 0x08, 0x05, 0x0A, 0x01, 0x03, 0x0E, 0x01, 0x00, 0x0D, 0x0E, 0x0C,
-    0x00, 0x0B, 0x01, 0x04, 0x0F, 0x08, 0x0B, 0x05, 0x07, 0x0C, 0x0D, 0x06, 0x01, 0x0E, 0x09, 0x08
-}, {
-    0x0E, 0x00, 0x04, 0x0F, 0x05, 0x01, 0x02, 0x01, 0x07, 0x01, 0x09, 0x08, 0x05, 0x07, 0x05, 0x0A,
-    0x00, 0x0B, 0x01, 0x02, 0x03, 0x0F, 0x08, 0x02, 0x07, 0x01, 0x09, 0x08, 0x01, 0x01, 0x05, 0x0C
-}};
 
 
 static void cic_irq_reset_falling (void) {
-    cic_enabled = false;
-    hw_gpio_set(GPIO_ID_N64_CIC_DQ);
     led_clear_error(LED_ERROR_CIC);
 }
 
-static void cic_irq_reset_rising (void) {
-    if (!cic_disabled) {
-        cic_enabled = true;
-        task_set_ready_and_reset(TASK_ID_CIC);
-    }
-}
-
-static void cic_irq_clk_falling (void) {
-    if (cic_enabled) {
-        if (!cic_next_wr) {
-            hw_gpio_reset(GPIO_ID_N64_CIC_DQ);
-        }
-        cic_next_rd = hw_gpio_get(GPIO_ID_N64_CIC_DQ) ? 1 : 0;
-        task_set_ready(TASK_ID_CIC);
-    }
-}
-
-static void cic_irq_clk_rising (void) {
-    hw_gpio_set(GPIO_ID_N64_CIC_DQ);
-    if (cic_detect_enabled) {
-        cic_detect_enabled = false;
-        if (!hw_gpio_get(GPIO_ID_N64_CIC_DQ)) {
-            cic_enabled = false;
-        }
-    }
-}
-
-static uint8_t cic_read (void) {
-    cic_next_wr = 1;
-    task_yield();
-    return cic_next_rd;
-}
-
-static void cic_write (uint8_t bit) {
-    cic_next_wr = bit;
-    task_yield();
-}
-
-static void cic_start_detect (void) {
-    cic_detect_enabled = cic_dd_mode;
-}
-
-static uint8_t cic_read_nibble (void) {
-    uint8_t data = 0;
-    for (int i = 0; i < 4; i++) {
-        data = ((data << 1) | cic_read());
-    }
-    return data;
-}
-
-static void cic_write_nibble (uint8_t data) {
-    cic_write(data & 0x08);
-    cic_write(data & 0x04);
-    cic_write(data & 0x02);
-    cic_write(data & 0x01);
-}
-
-static void cic_write_ram_nibbles (uint8_t index) {
-    do {
-        cic_write_nibble(cic_ram[index++]);
-    } while ((index & 0x0F) != 0);
-}
-
-static void cic_encode_round (uint8_t index) {
-    uint8_t data = cic_ram[index++];
-    do {
-        data = ((((data + 1) & 0x0F) + cic_ram[index]) & 0x0F);
-        cic_ram[index++] = data;
-    } while ((index & 0x0F) != 0);
-}
-
-static void cic_write_id (cic_region_t region) {
-    cic_start_detect();
-    cic_write(cic_dd_mode ? 1 : 0);
-    cic_write(region == REGION_PAL ? 1 : 0);
-    cic_write(0);
-    cic_write(1);
-}
-
-static void cic_write_id_failed (void) {
-    uint8_t current_region = rtc_get_region();
-    uint8_t next_region = (current_region == REGION_NTSC) ? REGION_PAL : REGION_NTSC;
-    rtc_set_region(next_region);
-    led_blink_error(LED_ERROR_CIC);
-}
-
-static void cic_write_seed (void) {
-    cic_ram[0x0A] = 0x0B;
-    cic_ram[0x0B] = 0x05;
-    cic_ram[0x0C] = (cic_seed >> 4);
-    cic_ram[0x0D] = cic_seed;
-    cic_ram[0x0E] = (cic_seed >> 4);
-    cic_ram[0x0F] = cic_seed;
-    cic_encode_round(0x0A);
-    cic_encode_round(0x0A);
-    cic_write_ram_nibbles(0x0A);
-}
-
-static void cic_write_checksum (void) {
-    for (int i = 0; i < 4; i++) {
-        cic_ram[i] = 0x00;
-    }
-    for (int i = 0; i < 6; i++) {
-        cic_ram[(i * 2) + 4] = ((cic_checksum[i] >> 4) & 0x0F);
-        cic_ram[(i * 2) + 5] = (cic_checksum[i] & 0x0F);
-    }
-    cic_encode_round(0x00);
-    cic_encode_round(0x00);
-    cic_encode_round(0x00);
-    cic_encode_round(0x00);
-    cic_write(0);
-    cic_write_ram_nibbles(0x00);
-}
-
-static void cic_init_ram (cic_region_t region) {
-    if (region < __REGION_MAX) {
-        for (int i = 0; i < 32; i++) {
-            cic_ram[i] = cic_ram_init[region][i];
-        }
-    }
-    cic_ram[0x01] = cic_read_nibble();
-    cic_ram[0x11] = cic_read_nibble();
-}
-
-static void cic_exchange_bytes (uint8_t *a, uint8_t *b) {
-    uint8_t tmp = *a;
-    *a = *b;
-    *b = tmp;
-}
-
-static void cic_round (uint8_t *m) {
-    uint8_t a, b, x;
-
-    x = m[15];
-    a = x;
-
-    do {
-        b = 1;
-        a += (m[b] + 1);
-        m[b] = a;
-        b++;
-        a += (m[b] + 1);
-        cic_exchange_bytes(&a, &m[b]);
-        m[b] = ~(m[b]);
-        b++;
-        a &= 0x0F;
-        a += ((m[b] & 0x0F) + 1);
-        if (a < 16) {
-            cic_exchange_bytes(&a, &m[b]);
-            b++;
-        }
-        a += m[b];
-        m[b] = a;
-        b++;
-        a += m[b];
-        cic_exchange_bytes(&a, &m[b]);
-        b++;
-        a &= 0x0F;
-        a += 8;
-        if (a < 16) {
-            a += m[b];
-        }
-        cic_exchange_bytes(&a, &m[b]);
-        b++;
-        do {
-            a += (m[b] + 1);
-            m[b] = a;
-            b++;
-            b &= 0x0F;
-        } while (b != 0);
-        a = (x + 0x0F);
-        x = (a & 0x0F);
-    } while (x != 0x0F);
-}
-
-static void cic_compare_mode (cic_region_t region) {
-    cic_round(&cic_ram[0x10]);
-    cic_round(&cic_ram[0x10]);
-    cic_round(&cic_ram[0x10]);
-
-    uint8_t index = (cic_ram[0x17] & 0x0F);
-    if (index == 0) {
-        index = 1;
-    }
-    index |= 0x10;
-
-    do {
-        cic_read();
-        cic_write(cic_ram[index] & 0x01);
-        if (region == REGION_PAL) {
-            index--;
-        } else {
-            index++;
-        }
-    } while (index & 0x0F);
-}
-
-static void cic_x105_algorithm (void) {
-    uint8_t a = 5;
-    uint8_t carry = 1;
-
-    for (int i = 0; i < 30; ++i) {
-        if (!(cic_x105_ram[i] & 0x01)) {
-            a += 8;
-        }
-        if (!(a & 0x02)) {
-            a += 4;
-        }
-        a = ((a + cic_x105_ram[i]) & 0x0F);
-        cic_x105_ram[i] = a;
-        if (!carry) {
-            a += 7;
-        }
-        a = ((a + cic_x105_ram[i]) & 0x0F);
-        a = (a + cic_x105_ram[i] + carry);
-        if (a >= 0x10) {
-            carry = 1;
-            a -= 0x10;
-        } else {
-            carry = 0;
-        }
-        a = (~(a) & 0x0F);
-        cic_x105_ram[i] = a;
-    }
-}
-
-static void cic_x105_mode (void) {
-    cic_write_nibble(0x0A);
-    cic_write_nibble(0x0A);
-
-    for (int i = 0; i < 30; i++) {
-        cic_x105_ram[i] = cic_read_nibble();
-    }
-
-    cic_x105_algorithm();
-
-    cic_write(0);
-
-    for (int i = 0; i < 30; i++) {
-        cic_write_nibble(cic_x105_ram[i]);
-    }
-}
-
-static void cic_soft_reset_timeout (void) {
-    hw_gpio_reset(GPIO_ID_N64_CIC_DQ);
-    task_set_ready(TASK_ID_CIC);
-}
-
-static void cic_soft_reset (void) {
-    cic_read();
-    hw_tim_setup(TIM_ID_CIC, 500, cic_soft_reset_timeout);
-    task_yield();
-}
-
 
 void cic_reset_parameters (void) {
-    cic_disabled = false;
+    cic_region_t region = rtc_get_region();
-    cic_dd_mode = false;
+
-    cic_seed = 0x3F;
+    const uint8_t default_seed = 0x3F;
-    cic_checksum[0] = 0xA5;
+    const uint64_t default_checksum = 0xA536C0F1D859ULL;
-    cic_checksum[1] = 0x36;
+
-    cic_checksum[2] = 0xC0;
+    uint32_t cic_config_0 = (default_seed << CIC_SEED_BIT) | ((default_checksum >> 32) & 0xFFFF);
-    cic_checksum[3] = 0xF1;
+    uint32_t cic_config_1 = (default_checksum & 0xFFFFFFFFUL);
-    cic_checksum[4] = 0xD8;
+
-    cic_checksum[5] = 0x59;
+    if (region == REGION_PAL) {
+        cic_config_0 |= CIC_REGION;
+    }
+    
+    fpga_reg_set(REG_CIC_0, cic_config_0);
+    fpga_reg_set(REG_CIC_1, cic_config_1);
 }
 
 void cic_set_parameters (uint32_t *args) {
-    cic_disabled = (args[0] >> 24) & (1 << 0);
+    uint32_t cic_config_0 = args[0] & (0x00FFFFFF);
-    cic_seed = (args[0] >> 16) & 0xFF;
+    uint32_t cic_config_1 = args[1];
-    cic_checksum[0] = (args[0] >> 8) & 0xFF;
+
-    cic_checksum[1] = args[0] & 0xFF;
+    cic_config_0 |= fpga_reg_get(REG_CIC_0) & (CIC_64DD_MODE | CIC_REGION);
-    cic_checksum[2] = (args[1] >> 24) & 0xFF;
+
-    cic_checksum[3] = (args[1] >> 16) & 0xFF;
+    if (args[0] & (1 << 24)) {
-    cic_checksum[4] = (args[1] >> 8) & 0xFF;
+        cic_config_0 |= CIC_DISABLED;
-    cic_checksum[5] = args[1] & 0xFF;
+    }
+
+    fpga_reg_set(REG_CIC_0, cic_config_0);
+    fpga_reg_set(REG_CIC_1, cic_config_1);
 }
 
 void cic_set_dd_mode (bool enabled) {
-    cic_dd_mode = enabled;
+    uint32_t cic_config_0 = fpga_reg_get(REG_CIC_0);
+
+    if (enabled) {
+        cic_config_0 |= CIC_64DD_MODE;
+    } else {
+        cic_config_0 &= ~(CIC_64DD_MODE);
+    }
+
+    fpga_reg_set(REG_CIC_0, cic_config_0);
 }
 
-void cic_hw_init (void) {
+
+void cic_init (void) {
     hw_gpio_irq_setup(GPIO_ID_N64_RESET, GPIO_IRQ_FALLING, cic_irq_reset_falling);
-    hw_gpio_irq_setup(GPIO_ID_N64_RESET, GPIO_IRQ_RISING, cic_irq_reset_rising);
-    hw_gpio_irq_setup(GPIO_ID_N64_CIC_CLK, GPIO_IRQ_FALLING, cic_irq_clk_falling);
-    hw_gpio_irq_setup(GPIO_ID_N64_CIC_CLK, GPIO_IRQ_RISING, cic_irq_clk_rising);
 }
 
-void cic_task (void) {
-    while (!hw_gpio_get(GPIO_ID_N64_RESET)) {
-        task_yield();
-    }
 
-    cic_region_t region = rtc_get_region();
+void cic_process (void) {
-    if (region >= __REGION_MAX) {
+    if (!cic_initialized) {
-        region = REGION_NTSC;
+        if (rtc_is_initialized()) {
-        rtc_set_region(region);
+            cic_reset_parameters();
-    }
+            cic_initialized = true;
-
+        } else {
-    cic_write_id(region);
+            return;
-
-    hw_tim_setup(TIM_ID_CIC, 500, cic_write_id_failed);
-    cic_write_seed();
-    hw_tim_stop(TIM_ID_CIC);
-
-    cic_write_checksum();
-    cic_init_ram(region);
-
-    while (1) {
-        uint8_t cmd = 0;
-        cmd |= (cic_read() << 1);
-        cmd |= cic_read();
-
-        switch (cmd) {
-            case 0: {
-                cic_compare_mode(region);
-                break;
-            }
-
-            case 2: {
-                cic_x105_mode();
-                break;
-            }
-
-            case 3: {
-                cic_soft_reset();
-                break;
-            }
-
-            case 1:
-            default: {
-                while (1) {
-                    task_yield();
-                }
-                break;
-            }
         }
     }
+
+    uint32_t cic_config_0 = fpga_reg_get(REG_CIC_0);
+
+    if (cic_config_0 & CIC_INVALID_REGION_DETECTED) {
+        cic_config_0 ^= CIC_REGION;
+        cic_config_0 |= CIC_INVALID_REGION_RESET;
+        fpga_reg_set(REG_CIC_0, cic_config_0);
+
+        if (cic_config_0 & CIC_REGION) {
+            rtc_set_region(REGION_PAL);
+        } else {
+            rtc_set_region(REGION_NTSC);
+        }
+
+        led_blink_error(LED_ERROR_CIC);
+    }
 }

sw/controller/src/cic.h

@@ -9,8 +9,8 @@
 void cic_reset_parameters (void);
 void cic_set_parameters (uint32_t *args);
 void cic_set_dd_mode (bool enabled);
-void cic_hw_init (void);
+void cic_init (void);
-void cic_task (void);
+void cic_process (void);
 
 
 #endif

sw/controller/src/fpga.h

@@ -55,6 +55,8 @@ typedef enum {
     REG_VENDOR_DATA,
     REG_DEBUG_0,
     REG_DEBUG_1,
+    REG_CIC_0,
+    REG_CIC_1,
 } fpga_reg_t;
 
 
@@ -186,6 +188,13 @@ typedef enum {
 #define DD_HEAD_TRACK_MASK              (DD_HEAD_MASK | DD_TRACK_MASK)
 #define DD_HEAD_TRACK_INDEX_LOCK        (1 << 13)
 
+#define CIC_SEED_BIT                    (16)
+#define CIC_REGION                      (1 << 24)
+#define CIC_64DD_MODE                   (1 << 25)
+#define CIC_DISABLED                    (1 << 26)
+#define CIC_INVALID_REGION_DETECTED     (1 << 27)
+#define CIC_INVALID_REGION_RESET        (1 << 28)
+
 
 uint8_t fpga_id_get (void);
 uint32_t fpga_reg_get (fpga_reg_t reg);

sw/controller/src/gvr.c

@@ -1,5 +1,6 @@
 #include "button.h"
 #include "cfg.h"
+#include "cic.h"
 #include "dd.h"
 #include "flashram.h"
 #include "fpga.h"
@@ -15,6 +16,7 @@ void gvr_task (void) {
 
     button_init();
     cfg_init();
+    cic_init();
     dd_init();
     flashram_init();
     isv_init();
@@ -25,6 +27,7 @@ void gvr_task (void) {
     while (1) {
         button_process();
         cfg_process();
+        cic_process();
         dd_process();
         flashram_process();
         isv_process();

sw/controller/src/hw.c

@@ -83,6 +83,7 @@ static void hw_gpio_init (gpio_id_t id, gpio_mode_t mode, gpio_ot_t ot, gpio_osp
 void hw_gpio_irq_setup (gpio_id_t id, gpio_irq_t irq, void (*callback)(void)) {
     uint8_t port = ((id >> 4) & 0x07);
     uint8_t pin = (id & 0x0F);
+    __disable_irq();
     if (irq == GPIO_IRQ_FALLING) {
         EXTI->FTSR1 |= (EXTI_FTSR1_FT0 << pin);
         gpio_irq_callbacks[pin].falling = callback;
@@ -92,6 +93,7 @@ void hw_gpio_irq_setup (gpio_id_t id, gpio_irq_t irq, void (*callback)(void)) {
     }
     EXTI->EXTICR[pin / 4] |= (port << (8 * (pin % 4)));
     EXTI->IMR1 |= (EXTI_IMR1_IM0 << pin);
+    __enable_irq();
 }
 
 uint32_t hw_gpio_get (gpio_id_t id) {
@@ -509,7 +511,7 @@ static void hw_init_crc (void) {
 static void hw_init_misc (void) {
     hw_gpio_init(GPIO_ID_N64_RESET, GPIO_INPUT, GPIO_PP, GPIO_SPEED_VLOW, GPIO_PULL_DOWN, GPIO_AF_0, 0);
     hw_gpio_init(GPIO_ID_N64_CIC_CLK, GPIO_INPUT, GPIO_PP, GPIO_SPEED_VLOW, GPIO_PULL_UP, GPIO_AF_0, 0);
-    hw_gpio_init(GPIO_ID_N64_CIC_DQ, GPIO_OUTPUT, GPIO_OD, GPIO_SPEED_VLOW, GPIO_PULL_UP, GPIO_AF_0, 1);
+    hw_gpio_init(GPIO_ID_N64_CIC_DQ, GPIO_INPUT, GPIO_OD, GPIO_SPEED_VLOW, GPIO_PULL_UP, GPIO_AF_0, 1);
     hw_gpio_init(GPIO_ID_FPGA_INT, GPIO_INPUT, GPIO_PP, GPIO_SPEED_VLOW, GPIO_PULL_UP, GPIO_AF_0, 0);
     hw_gpio_init(GPIO_ID_RTC_MFP, GPIO_INPUT, GPIO_PP, GPIO_SPEED_VLOW, GPIO_PULL_UP, GPIO_AF_0, 0);
 }
@@ -528,14 +530,14 @@ void hw_init (void) {
     hw_init_misc();
 
     NVIC_SetPriority(EXTI0_1_IRQn, 0);
-    NVIC_SetPriority(EXTI2_3_IRQn, 1);
+    NVIC_SetPriority(EXTI2_3_IRQn, 0);
-    NVIC_SetPriority(EXTI4_15_IRQn, 2);
+    NVIC_SetPriority(EXTI4_15_IRQn, 0);
-    NVIC_SetPriority(I2C1_IRQn, 1);
+    NVIC_SetPriority(I2C1_IRQn, 0);
     NVIC_SetPriority(TIM14_IRQn, 0);
-    NVIC_SetPriority(TIM16_IRQn, 1);
+    NVIC_SetPriority(TIM16_IRQn, 0);
-    NVIC_SetPriority(TIM17_IRQn, 2);
+    NVIC_SetPriority(TIM17_IRQn, 0);
-    NVIC_SetPriority(TIM3_IRQn, 2);
+    NVIC_SetPriority(TIM3_IRQn, 0);
-    NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 1);
+    NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 0);
 
     NVIC_EnableIRQ(EXTI0_1_IRQn);
     NVIC_EnableIRQ(EXTI2_3_IRQn);

sw/controller/src/rtc.c

@@ -48,6 +48,7 @@ static rtc_settings_t rtc_settings = {
     .led_enabled    = true,
 };
 static volatile bool rtc_settings_pending = false;
+static volatile bool rtc_initialized = false;
 
 static const uint8_t rtc_regs_bit_mask[7] = {
     0b01111111,
@@ -202,6 +203,10 @@ static void rtc_init (void) {
 }
 
 
+bool rtc_is_initialized (void) {
+    return rtc_initialized;
+}
+
 bool rtc_get_time (rtc_time_t *time) {
     bool vaild;
 
@@ -268,6 +273,8 @@ void rtc_task (void) {
     rtc_read_region();
     rtc_read_settings();
 
+    rtc_initialized = true;
+
     while (1) {
         if (rtc_time_pending) {
             rtc_time_pending = false;

sw/controller/src/rtc.h

@@ -21,6 +21,7 @@ typedef struct {
 } rtc_settings_t;
 
 
+bool rtc_is_initialized (void);
 bool rtc_get_time (rtc_time_t *time);
 void rtc_set_time (rtc_time_t *time);
 uint8_t rtc_get_region (void);

sw/controller/src/task.c

@@ -1,3 +1,4 @@
+#include <stdbool.h>
 #include <stdint.h>
 #include <stm32g0xx.h>
 #include "task.h"
@@ -8,18 +9,9 @@
 #define TASK_STACK_FILL_VALUE   (0xDEADBEEF)
 
 
-typedef enum {
-    TASK_FLAG_NONE  = 0,
-    TASK_FLAG_READY = (1 << 0),
-    TASK_FLAG_RESET = (1 << 1),
-} task_flags_t;
-
-
 typedef struct {
-    uint32_t initial_pc;
+    volatile uint32_t sp;
-    uint32_t initial_sp;
+    volatile bool ready;
-    uint32_t sp;
-    task_flags_t flags;
 } task_t;
 
 
@@ -28,42 +20,21 @@ static volatile task_id_t task_current = 0;
 
 
 static void task_exit (void) {
-    task_table[task_current].flags = TASK_FLAG_NONE;
+    while (1) {
-    task_yield();
+        task_yield();
-    while (1);
-}
-
-static void task_initialize (task_id_t id) {
-    task_t *task = &task_table[id];
-    uint32_t *sp = ((uint32_t *) (task->initial_sp));
-    *--sp = TASK_INITIAL_XPSR;
-    *--sp = task->initial_pc;
-    *--sp = ((uint32_t) (task_exit));
-    for (int i = 0; i < 13; i++) {
-        *--sp = 0;
     }
-    task->sp = ((uint32_t) (sp));
-}
-
-static void task_reset (task_id_t id) {
-    task_table[id].flags &= ~(TASK_FLAG_RESET);
-    task_initialize(id);
 }
 
 static uint32_t task_switch_context (uint32_t sp) {
     task_table[task_current].sp = sp;
 
     for (task_id_t id = 0; id < __TASK_ID_MAX; id++) {
-        if (task_table[id].flags & TASK_FLAG_READY) {
+        if (task_table[id].ready) {
             task_current = id;
             break;
         }
     }
 
-    if (task_table[task_current].flags & TASK_FLAG_RESET) {
-        task_reset(task_current);
-    }
-
     return task_table[task_current].sp;
 }
 
@@ -73,26 +44,30 @@ void task_create (task_id_t id, void (*code)(void), void *stack, size_t stack_si
         for (size_t i = 0; i < stack_size; i += sizeof(uint32_t)) {
             (*(uint32_t *) (stack + i)) = TASK_STACK_FILL_VALUE;
         }
+        uint32_t *sp =  ((uint32_t *) ((uint32_t) (stack) + stack_size));
+        *--sp = TASK_INITIAL_XPSR;
+        *--sp = (uint32_t) (code);
+        *--sp = ((uint32_t) (task_exit));
+        for (int i = 0; i < 13; i++) {
+            *--sp = 0;
+        }
         task_t *task = &task_table[id];
-        task->initial_pc = (uint32_t) (code);
+        task->sp = ((uint32_t) (sp));
-        task->initial_sp = (((uint32_t) (stack)) + stack_size);
+        task->ready = true;
-        task->flags = TASK_FLAG_READY;
-        task_initialize(id);
     }
 }
 
 void task_yield (void) {
-    task_table[task_current].flags &= ~(TASK_FLAG_READY);
+    __disable_irq();
+    task_table[task_current].ready = false;
+    __enable_irq();
     TASK_CONTEXT_SWITCH();
 }
 
 void task_set_ready (task_id_t id) {
-    task_table[id].flags |= TASK_FLAG_READY;
+    __disable_irq();
-    TASK_CONTEXT_SWITCH();
+    task_table[id].ready = true;
-}
+    __enable_irq();
-
-void task_set_ready_and_reset (task_id_t id) {
-    task_table[id].flags |= (TASK_FLAG_RESET | TASK_FLAG_READY);
     TASK_CONTEXT_SWITCH();
 }
 

sw/controller/src/task.h

@@ -6,7 +6,6 @@
 
 
 typedef enum {
-    TASK_ID_CIC,
     TASK_ID_RTC,
     TASK_ID_LED,
     TASK_ID_GVR,
@@ -17,7 +16,6 @@ typedef enum {
 void task_create (task_id_t id, void (*code)(void), void *stack, size_t stack_size);
 void task_yield (void);
 void task_set_ready (task_id_t id);
-void task_set_ready_and_reset (task_id_t id);
 size_t task_get_stack_usage (void *stack, size_t stack_size);
 void task_scheduler_start (void);
 

sw/deployer/src/sc64/cic.rs

@@ -3,74 +3,7 @@ use super::Error;
 pub const IPL3_OFFSET: u32 = 0x40;
 pub const IPL3_LENGTH: usize = 0xFC0;
 
-enum CicType {
+fn calculate_ipl3_checksum(ipl3: &[u8], seed: u8) -> Result<u64, Error> {
-    _5101,
-    _6101,
-    _7102,
-    X102,
-    X103,
-    X105,
-    X106,
-    _5167,
-    NDXJ0,
-    NDDJ0,
-    NDDJ1,
-    NDDJ2,
-    NDDE0,
-}
-
-impl From<u32> for CicType {
-    fn from(value: u32) -> Self {
-        match value {
-            0x587BD543 => CicType::_5101,
-            0x6170A4A1 => CicType::_6101,
-            0x009E9EA3 => CicType::_7102,
-            0x90BB6CB5 => CicType::X102,
-            0x0B050EE0 => CicType::X103,
-            0x98BC2C86 => CicType::X105,
-            0xACC8580A => CicType::X106,
-            0x0E018159 => CicType::_5167,
-            0x10C68B18 => CicType::NDXJ0,
-            0xBC605D0A => CicType::NDDJ0,
-            0x502C4466 => CicType::NDDJ1,
-            0x0C965795 => CicType::NDDJ2,
-            0x8FEBA21E => CicType::NDDE0,
-            _ => CicType::X102,
-        }
-    }
-}
-
-impl From<CicType> for u8 {
-    fn from(value: CicType) -> Self {
-        match value {
-            CicType::_5101 => 0xAC,
-            CicType::_6101 => 0x3F,
-            CicType::_7102 => 0x3F,
-            CicType::X102 => 0x3F,
-            CicType::X103 => 0x78,
-            CicType::X105 => 0x91,
-            CicType::X106 => 0x85,
-            CicType::_5167 => 0xDD,
-            CicType::NDXJ0 => 0xDD,
-            CicType::NDDJ0 => 0xDD,
-            CicType::NDDJ1 => 0xDD,
-            CicType::NDDJ2 => 0xDD,
-            CicType::NDDE0 => 0xDE,
-        }
-    }
-}
-
-pub fn guess_ipl3_seed(ipl3: &[u8]) -> Result<u8, Error> {
-    if ipl3.len() < IPL3_LENGTH {
-        return Err(Error::new("Invalid IPL3 length provided"));
-    }
-
-    let cic_type: CicType = crc32fast::hash(ipl3).into();
-
-    Ok(cic_type.into())
-}
-
-pub fn calculate_ipl3_checksum(ipl3: &[u8], seed: u8) -> Result<[u8; 6], Error> {
     if ipl3.len() < IPL3_LENGTH {
         return Err(Error::new("Invalid IPL3 length provided"));
     }
@@ -87,9 +20,9 @@ pub fn calculate_ipl3_checksum(ipl3: &[u8], seed: u8) -> Result<[u8; 6], Error>
     let add = |a1: u32, a2: u32| u32::wrapping_add(a1, a2);
     let sub = |a1: u32, a2: u32| u32::wrapping_sub(a1, a2);
     let mul = |a1: u32, a2: u32| u32::wrapping_mul(a1, a2);
-    let lsh = |a: u32, s: u32| if s >= 32 { 0 } else { u32::wrapping_shl(a, s) };
+    let rol = |a: u32, s: u32| u32::rotate_left(a, s);
-    let rsh = |a: u32, s: u32| if s >= 32 { 0 } else { u32::wrapping_shr(a, s) };
+    let ror = |a: u32, s: u32| u32::rotate_right(a, s);
-    let checksum = |a0: u32, a1: u32, a2: u32| {
+    let sum = |a0: u32, a1: u32, a2: u32| {
         let prod = (a0 as u64).wrapping_mul(if a1 == 0 { a2 as u64 } else { a1 as u64 });
         let hi = ((prod >> 32) & 0xFFFFFFFF) as u32;
         let lo = (prod & 0xFFFFFFFF) as u32;
@@ -99,122 +32,109 @@ pub fn calculate_ipl3_checksum(ipl3: &[u8], seed: u8) -> Result<[u8; 6], Error>
 
     let init = add(mul(MAGIC, seed as u32), 1) ^ get(0);
 
-    let mut buffer = vec![init; 16];
+    let mut buf = vec![init; 16];
 
     for i in 1..=1008 as u32 {
-        let data_prev = get(i.saturating_sub(2));
+        let prev = get(i.saturating_sub(2));
-        let data_curr = get(i - 1);
+        let data = get(i - 1);
 
-        buffer[0] = add(buffer[0], checksum(sub(1007, i), data_curr, i));
+        buf[0] = add(buf[0], sum(sub(1007, i), data, i));
-        buffer[1] = checksum(buffer[1], data_curr, i);
+        buf[1] = sum(buf[1], data, i);
-        buffer[2] = buffer[2] ^ data_curr;
+        buf[2] = buf[2] ^ data;
-        buffer[3] = add(buffer[3], checksum(add(data_curr, 5), MAGIC, i));
+        buf[3] = add(buf[3], sum(add(data, 5), MAGIC, i));
-
+        buf[4] = add(buf[4], ror(data, prev & 0x1F));
-        let shift = data_prev & 0x1F;
+        buf[5] = add(buf[5], rol(data, prev >> 27));
-        let data_left = lsh(data_curr, 32 - shift);
+        buf[6] = if data < buf[6] {
-        let data_right = rsh(data_curr, shift);
+            add(buf[3], buf[6]) ^ add(data, i)
-        let b4_shifted = data_left | data_right;
-        buffer[4] = add(buffer[4], b4_shifted);
-
-        let shift = rsh(data_prev, 27);
-        let data_left = lsh(data_curr, shift);
-        let data_right = rsh(data_curr, 32 - shift);
-        let b5_shifted = data_left | data_right;
-        buffer[5] = add(buffer[5], b5_shifted);
-
-        if data_curr < buffer[6] {
-            buffer[6] = add(buffer[3], buffer[6]) ^ add(data_curr, i);
         } else {
-            buffer[6] = add(buffer[4], data_curr) ^ buffer[6];
+            add(buf[4], data) ^ buf[6]
-        }
+        };
-
+        buf[7] = sum(buf[7], rol(data, prev & 0x1F), i);
-        let shift = data_prev & 0x1F;
+        buf[8] = sum(buf[8], ror(data, prev >> 27), i);
-        let data_left = lsh(data_curr, shift);
+        buf[9] = if prev < data {
-        let data_right = rsh(data_curr, 32 - shift);
+            sum(buf[9], data, i)
-        buffer[7] = checksum(buffer[7], data_left | data_right, i);
-
-        let shift = rsh(data_prev, 27);
-        let data_left = lsh(data_curr, 32 - shift);
-        let data_right = rsh(data_curr, shift);
-        buffer[8] = checksum(buffer[8], data_left | data_right, i);
-
-        if data_prev < data_curr {
-            buffer[9] = checksum(buffer[9], data_curr, i)
         } else {
-            buffer[9] = add(buffer[9], data_curr);
+            add(buf[9], data)
-        }
+        };
 
         if i == 1008 {
             break;
         }
 
-        let data_next = get(i);
+        let next = get(i);
 
-        buffer[10] = checksum(add(buffer[10], data_curr), data_next, i);
+        buf[10] = sum(add(buf[10], data), next, i);
-        buffer[11] = checksum(buffer[11] ^ data_curr, data_next, i);
+        buf[11] = sum(buf[11] ^ data, next, i);
-        buffer[12] = add(buffer[12], buffer[8] ^ data_curr);
+        buf[12] = add(buf[12], buf[8] ^ data);
-
+        buf[13] = add(buf[13], add(ror(data, data & 0x1F), ror(next, next & 0x1F)));
-        let shift = data_curr & 0x1F;
+        buf[14] = sum(
-        let data_left = lsh(data_curr, 32 - shift);
+            sum(buf[14], ror(data, prev & 0x1F), i),
-        let data_right = rsh(data_curr, shift);
+            ror(next, data & 0x1F),
-        let tmp = data_left | data_right;
+            i,
-        let shift = data_next & 0x1F;
+        );
-        let data_left = lsh(data_next, 32 - shift);
+        buf[15] = sum(
-        let data_right = rsh(data_next, shift);
+            sum(buf[15], rol(data, prev >> 27), i),
-        buffer[13] = add(buffer[13], add(tmp, data_left | data_right));
+            rol(next, data >> 27),
-
+            i,
-        let shift = data_curr & 0x1F;
+        );
-        let data_left = lsh(data_next, 32 - shift);
-        let data_right = rsh(data_next, shift);
-        let sum = checksum(buffer[14], b4_shifted, i);
-        buffer[14] = checksum(sum, data_left | data_right, i);
-
-        let shift = rsh(data_curr, 27);
-        let data_left = lsh(data_next, shift);
-        let data_right = rsh(data_next, 32 - shift);
-        let sum = checksum(buffer[15], b5_shifted, i);
-        buffer[15] = checksum(sum, data_left | data_right, i);
     }
 
-    let mut final_buffer = vec![buffer[0]; 4];
+    let mut final_buf = vec![buf[0]; 4];
 
     for i in 0..16 as u32 {
-        let data = buffer[i as usize];
+        let data = buf[i as usize];
 
-        let shift = data & 0x1F;
+        final_buf[0] = add(final_buf[0], ror(data, data & 0x1F));
-        let data_left = lsh(data, 32 - shift);
+        final_buf[1] = if data < final_buf[0] {
-        let data_right = rsh(data, shift);
+            add(final_buf[1], data)
-        let b0_shifted = add(final_buffer[0], data_left | data_right);
-        final_buffer[0] = b0_shifted;
-
-        if data < b0_shifted {
-            final_buffer[1] = add(final_buffer[1], data);
         } else {
-            final_buffer[1] = checksum(final_buffer[1], data, i);
+            sum(final_buf[1], data, i)
-        }
+        };
-
+        final_buf[2] = if ((data & 0x02) >> 1) == (data & 0x01) {
-        if rsh(data & 0x02, 1) == (data & 0x01) {
+            add(final_buf[2], data)
-            final_buffer[2] = add(final_buffer[2], data);
         } else {
-            final_buffer[2] = checksum(final_buffer[2], data, i);
+            sum(final_buf[2], data, i)
-        }
+        };
-
+        final_buf[3] = if (data & 0x01) == 0x01 {
-        if (data & 0x01) == 0x01 {
+            final_buf[3] ^ data
-            final_buffer[3] = final_buffer[3] ^ data;
         } else {
-            final_buffer[3] = checksum(final_buffer[3], data, i);
+            sum(final_buf[3], data, i)
+        };
+    }
+
+    let final_sum = sum(final_buf[0], final_buf[1], 16);
+    let final_xor = final_buf[3] ^ final_buf[2];
+
+    let checksum = (((final_sum & 0xFFFF) as u64) << 32) | (final_xor as u64);
+
+    Ok(checksum)
+}
+
+pub fn sign_ipl3(ipl3: &[u8]) -> Result<(u8, u64), Error> {
+    let known_seed_checksum_pairs = [
+        (0xDD, 0x083C6C77E0B1u64), // 5167
+        (0x3F, 0x45CC73EE317Au64), // 6101
+        (0x3F, 0x44160EC5D9AFu64), // 7102
+        (0x3F, 0xA536C0F1D859u64), // 6102/7102
+        (0x78, 0x586FD4709867u64), // 6103/7103
+        (0x91, 0x8618A45BC2D3u64), // 6105/7105
+        (0x85, 0x2BBAD4E6EB74u64), // 6106/7106
+        (0xDD, 0x6EE8D9E84970u64), // NDXJ0
+        (0xDD, 0x6C216495C8B9u64), // NDDJ0
+        (0xDD, 0xE27F43BA93ACu64), // NDDJ1
+        (0xDD, 0x32B294E2AB90u64), // NDDJ2
+        (0xDE, 0x05BA2EF0A5F1u64), // NDDE0
+    ];
+
+    for (seed, checksum) in known_seed_checksum_pairs {
+        if calculate_ipl3_checksum(ipl3, seed)? == checksum {
+            return Ok((seed, checksum));
         }
     }
 
-    let sum = checksum(final_buffer[0], final_buffer[1], 16);
+    // Unknown IPL3 detected, sign it with arbitrary seed (CIC6102/7101 value is used here)
-    let xor = final_buffer[3] ^ final_buffer[2];
+    const DEFAULT_SEED: u8 = 0x3F;
+    let checksum = calculate_ipl3_checksum(ipl3, DEFAULT_SEED)?;
 
-    Ok([
+    Ok((DEFAULT_SEED, checksum))
-        (sum >> 8) as u8,
-        (sum >> 0) as u8,
-        (xor >> 24) as u8,
-        (xor >> 16) as u8,
-        (xor >> 8) as u8,
-        (xor >> 0) as u8,
-    ])
 }

sw/deployer/src/sc64/link.rs

@@ -193,6 +193,7 @@ trait Backend {
         data_type: DataType,
         packets: &mut VecDeque<Packet>,
     ) -> Result<Option<Response>, Error>;
+    fn close(&self) {}
 }
 
 struct SerialBackend {
@@ -221,6 +222,7 @@ fn new_serial_backend(port: &str) -> Result<SerialBackend, Error> {
 }
 
 struct TcpBackend {
+    stream: TcpStream,
     reader: BufReader<TcpStream>,
     writer: BufWriter<TcpStream>,
 }
@@ -332,6 +334,10 @@ impl Backend for TcpBackend {
 
         Ok(None)
     }
+
+    fn close(&self) {
+        self.stream.shutdown(std::net::Shutdown::Both).ok();
+    }
 }
 
 fn new_tcp_backend(address: &str) -> Result<TcpBackend, Error> {
@@ -349,7 +355,11 @@ fn new_tcp_backend(address: &str) -> Result<TcpBackend, Error> {
     };
     let reader = BufReader::new(stream.try_clone()?);
     let writer = BufWriter::new(stream.try_clone()?);
-    Ok(TcpBackend { reader, writer })
+    Ok(TcpBackend {
+        stream,
+        reader,
+        writer,
+    })
 }
 
 pub struct Link {
@@ -410,6 +420,12 @@ impl Link {
     }
 }
 
+impl Drop for Link {
+    fn drop(&mut self) {
+        self.backend.close();
+    }
+}
+
 pub fn new_local(port: &str) -> Result<Link, Error> {
     Ok(Link {
         backend: Box::new(new_serial_backend(port)?),

sw/deployer/src/sc64/mod.rs

@@ -18,7 +18,7 @@ pub use self::{
 };
 
 use self::{
-    cic::{calculate_ipl3_checksum, guess_ipl3_seed, IPL3_LENGTH, IPL3_OFFSET},
+    cic::{sign_ipl3, IPL3_LENGTH, IPL3_OFFSET},
     link::{Command, Link},
     time::{convert_from_datetime, convert_to_datetime},
     types::{
@@ -143,11 +143,13 @@ impl SC64 {
         &mut self,
         disable: bool,
         seed: u8,
-        checksum: &[u8; 6],
+        checksum: u64,
     ) -> Result<(), Error> {
+        let checksum_high = ((checksum >> 32) & 0xFFFF) as u32;
+        let checksum_low = (checksum & 0xFFFFFFFF) as u32;
         let args = [
-            u32::from_be_bytes([(disable as u8) << 0, seed, checksum[0], checksum[1]]),
+            ((if disable { 1 } else { 0 }) << 24) | ((seed as u32) << 16) | checksum_high,
-            u32::from_be_bytes([checksum[2], checksum[3], checksum[4], checksum[5]]),
+            checksum_low,
         ];
         self.link.execute_command(&Command {
             id: b'B',
@@ -503,8 +505,7 @@ impl SC64 {
             _ => BOOTLOADER_ADDRESS,
         };
         let ipl3 = self.command_memory_read(address + IPL3_OFFSET, IPL3_LENGTH)?;
-        let seed = guess_ipl3_seed(&ipl3)?;
+        let (seed, checksum) = sign_ipl3(&ipl3)?;
-        let checksum = &calculate_ipl3_checksum(&ipl3, seed)?;
         self.command_cic_params_set(false, seed, checksum)
     }
 

sw/deployer/src/sc64/types.rs

@@ -852,9 +852,12 @@ impl TryFrom<Vec<u8>> for McuStackUsage {
 
 impl Display for McuStackUsage {
     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        if self.cic > 0 {
+            f.write_fmt(format_args!("CIC: {}, ", self.cic))?;
+        }
         f.write_fmt(format_args!(
-            "CIC: {}, RTC: {}, LED: {}, GVR: {}",
+            "RTC: {}, LED: {}, GVR: {}",
-            self.cic, self.rtc, self.led, self.gvr
+            self.rtc, self.led, self.gvr
         ))
     }
 }