From b89bec430c94042ce0fce7527aad91a42af9f00b Mon Sep 17 00:00:00 2001
From: Rutherther <rutherther@proton.me>
Date: Sat, 16 Dec 2023 08:45:23 +0100
Subject: [PATCH] feat: add misaligned memory access support

---
 src/stages/memory_access.sv | 109 +++++++++++++++++++++++++++++-------
 1 file changed, 88 insertions(+), 21 deletions(-)

diff --git a/src/stages/memory_access.sv b/src/stages/memory_access.sv
index 054fc63..bf6a5f4 100644
--- a/src/stages/memory_access.sv
+++ b/src/stages/memory_access.sv
@@ -1,16 +1,16 @@
 import cpu_types::*;
 
 module memory_access(
-  input         clk,
+  input             clk,
 
-  input [31:0]  memory_out,
-  output [3:0]  memory_byte_enable,
-  output [31:0] memory_write,
-  output        memory_we,
-  output [31:0] memory_address,
+  input [31:0]      memory_out,
+  output reg [3:0]  memory_byte_enable,
+  output reg [31:0] memory_write,
+  output            memory_we,
+  output [31:0]     memory_address,
 
-  input         stage_status_t stage_in,
-  output        stage_status_t stage_out
+  input             stage_status_t stage_in,
+  output            stage_status_t stage_out
 );
 
   function bit[31:0] mem_sext_maybe;
@@ -39,27 +39,94 @@ module memory_access(
     end
   endfunction
 
-  assign memory_byte_enable = mask_to_mask_bytes(.mask(stage_in.instruction.memory_mask)) << memory_address[1:0];
-  assign memory_write = stage_in.reg_rd2 << (8*memory_address[1:0]);
-  assign memory_address = stage_in.data.data;
-  assign memory_we = stage_in.valid ? stage_in.instruction.memory_we : 0;
+  reg [31:0]  read_data;
+  reg [31:0]  stored_read_data;
+  reg         misaligned_access; // signals that two addresses will have to be accessed
+  reg         offset_position;
+  wire        is_read;
+  wire        is_write;
+  wire [1:0]  bit_position;
+  memory_mask_t       memory_mask;
+
+  assign memory_mask = stage_in.instruction.memory_mask;
+  assign is_read = stage_in.valid && stage_in.instruction.reg_rd_src == RD_MEMORY;
+  assign is_write = stage_in.valid && stage_in.instruction.memory_we;
+  assign bit_position = memory_address[1:0];
+
+  assign misaligned_access = (is_read || is_write) &&
+                          ((bit_position == 2'b11 && memory_mask == MEM_HALFWORD) ||
+                          (bit_position != 0 && memory_mask == MEM_WORD)); // for MEM_BYTE, cannot happen
+
+  always_ff @ (posedge clk) begin
+    stored_read_data = read_data;
+
+    if (misaligned_access) begin
+      if (offset_position == 1'b1) begin
+        offset_position = 1'b0;
+      end
+      else begin
+        offset_position = offset_position + 1;
+      end
+    end
+    else begin
+      offset_position = 1'b0;
+    end
+  end
+
+  always_comb begin
+    read_data = 32'bX;
+    memory_write = 32'bX;
+    memory_byte_enable = 4'bX;
+    // regular access (or not access at all)
+    if (offset_position == 1'b0) begin
+      memory_byte_enable = mask_to_mask_bytes(.mask(memory_mask)) << bit_position;
+      memory_write = stage_in.reg_rd2 << (8*bit_position);
+      read_data = mem_sext_maybe(
+          .num(memory_out >> (8 * bit_position)),
+          .mask(memory_mask),
+          .sext(stage_in.instruction.memory_sign_extension)
+      );
+    end // misaligned access:
+    else if (offset_position == 1'b0) begin
+      memory_byte_enable = mask_to_mask_bytes(.mask(memory_mask)) << bit_position;
+      memory_write = stage_in.reg_rd2 << (8*bit_position);
+      read_data = memory_out >> (8 * bit_position);
+    end // second stage of misaligned access:
+    else if (offset_position == 1'b1) begin
+      memory_byte_enable = mask_to_mask_bytes(.mask(memory_mask)) >> (4 - {2'b0, bit_position});
+      memory_write = stage_in.reg_rd2 >> (4 - {2'b0, bit_position})*8;
+      read_data = mem_sext_maybe(
+        .num((memory_out << (4 - {2'b0, bit_position}) * 8) | stored_read_data),
+        .mask(memory_mask),
+        .sext(stage_in.instruction.memory_sign_extension)
+      );
+    end
+
+  end
+
+  assign memory_address = stage_in.data.data + {29'b0, offset_position, 2'b0};
+
+  // 1. figure out if two addresses will have to be read
+  // if yes, set ready to 0
+  // 2. read from the first address, shift it accordingly
+  // 3. read from second address, or it to the result
+  //   this way only one 32-bit register should be needed for the storage
+  //   alternative would be to have two registers, and them, shift them. get last 32 bits.
+
+  assign memory_we = is_write;
 
   assign stage_out.instruction = stage_in.instruction;
   assign stage_out.pc = stage_in.pc;
   assign stage_out.reg_rd1 = stage_in.reg_rd1;
   assign stage_out.reg_rd2 = stage_in.reg_rd2;
 
-  assign stage_out.data.valid = stage_in.valid;
+  assign stage_out.data.valid = stage_in.valid && (offset_position == 1'b1 || !misaligned_access);
   assign stage_out.data.address = stage_in.valid ? stage_in.data.address : 0;
   assign stage_out.data.data =
-    stage_in.instruction.reg_rd_src == RD_MEMORY ?
-        mem_sext_maybe(
-            .num(memory_out >> (8*memory_address[1:0])),
-            .mask(stage_in.instruction.memory_mask),
-            .sext(stage_in.instruction.memory_sign_extension)
-        ) :
+    is_read ?
+        read_data :
         stage_in.data.data;
 
-  assign stage_out.valid = stage_in.valid;
-  assign stage_out.ready = 1;
+  assign stage_out.valid = stage_in.valid && (offset_position == 1'b1 || !misaligned_access);
+  assign stage_out.ready = offset_position == 1'b1 || !misaligned_access;
 endmodule
-- 
2.48.1