add F extension

2 files changed, 263 insertions, 0 deletions

diff --git a/src/core.rs b/src/core.rs
index ff6bb52..251336e 100644
--- a/src/core.rs
+++ b/src/core.rs
@@ -1,3 +1,5 @@
+use std::num::FpCategory;
+
 pub fn add(registers: &mut [u32; 32], rd: usize, rs1: usize, rs2: usize) {
     registers[rd] = registers[rs1].wrapping_add(registers[rs2]);
 }
@@ -243,3 +245,174 @@ pub fn lui(registers: &mut [u32; 32], rd: usize, imm: u32) {
 pub fn auipc(registers: &mut [u32; 32], pc: u32, rd: usize, imm: u32) {
     registers[rd] = pc + (imm << 12);
 }
+
+pub fn fmadd_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize, rs3: usize) {
+    registers[rd] = registers[rs1] * registers[rs2] + registers[rs3];
+}
+
+pub fn fmsub_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize, rs3: usize) {
+    registers[rd] = registers[rs1] * registers[rs2] - registers[rs3];
+}
+
+pub fn fnmadd_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize, rs3: usize) {
+    registers[rd] = -registers[rs1] * registers[rs2] + registers[rs3];
+}
+
+pub fn fnmsub_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize, rs3: usize) {
+    registers[rd] = -registers[rs1] * registers[rs2] - registers[rs3];
+}
+
+pub fn fadd_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    registers[rd] = registers[rs1] + registers[rs2];
+}
+
+pub fn fsub_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    registers[rd] = registers[rs1] - registers[rs2];
+}
+
+pub fn fmul_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    registers[rd] = registers[rs1] * registers[rs2];
+}
+
+pub fn fdiv_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    let a = registers[rs1];
+    let b = registers[rs2];
+
+    registers[rd] = if b == 0.0 {
+        f32::NAN
+        // todo: raise division by zero in csr
+    } else {
+        a / b
+    }
+}
+
+pub fn fsqrt_s(registers: &mut [f32; 32], rd: usize, rs1: usize) {
+    registers[rd] = registers[rs1].sqrt();
+}
+
+pub fn fsgnj_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    registers[rd] = registers[rs1].abs() * registers[rs2].signum();
+}
+pub fn fsgnjn_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    registers[rd] = registers[rs1].abs() * -registers[rs2].signum();
+}
+
+pub fn fsgnjx_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    registers[rd] = registers[rs1] * registers[rs2].signum();
+}
+pub fn fmin_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    registers[rd] = registers[rs1].min(registers[rs2]);
+}
+pub fn fmax_s(registers: &mut [f32; 32], rd: usize, rs1: usize, rs2: usize) {
+    registers[rd] = registers[rs1].max(registers[rs2]);
+}
+
+pub fn fcvt_s_w(registers: &mut [u32; 32], f_registers: &mut [f32; 32], rd: usize, rs1: usize) {
+    f_registers[rd] = registers[rs1] as i32 as f32;
+}
+
+pub fn fcvt_s_wu(registers: &mut [u32; 32], f_registers: &mut [f32; 32], rd: usize, rs1: usize) {
+    f_registers[rd] = registers[rs1] as f32;
+}
+
+pub fn fcvt_w_s(registers: &mut [u32; 32], f_registers: &mut [f32; 32], rd: usize, rs1: usize) {
+    registers[rd] = f_registers[rs1] as i32 as u32;
+}
+
+pub fn fcvt_wu_s(registers: &mut [u32; 32], f_registers: &mut [f32; 32], rd: usize, rs1: usize) {
+    registers[rd] = f_registers[rs1] as u32;
+}
+
+pub fn fmv_x_w(registers: &mut [u32; 32], f_registers: &mut [f32; 32], rd: usize, rs1: usize) {
+    registers[rd] = u32::from_le_bytes(f_registers[rs1].to_le_bytes());
+}
+
+pub fn fmv_w_x(registers: &mut [u32; 32], f_registers: &mut [f32; 32], rd: usize, rs1: usize) {
+    f_registers[rd] = f32::from_le_bytes(registers[rs1].to_le_bytes());
+}
+
+pub fn feq_s(
+    registers: &mut [u32; 32],
+    f_registers: &mut [f32; 32],
+    rd: usize,
+    rs1: usize,
+    rs2: usize,
+) {
+    registers[rd] = if f_registers[rs1] == f_registers[rs2] {
+        1
+    } else {
+        0
+    }
+}
+
+pub fn flt_s(
+    registers: &mut [u32; 32],
+    f_registers: &mut [f32; 32],
+    rd: usize,
+    rs1: usize,
+    rs2: usize,
+) {
+    registers[rd] = if f_registers[rs1] < f_registers[rs2] {
+        1
+    } else {
+        0
+    }
+}
+
+pub fn fle_s(
+    registers: &mut [u32; 32],
+    f_registers: &mut [f32; 32],
+    rd: usize,
+    rs1: usize,
+    rs2: usize,
+) {
+    registers[rd] = if f_registers[rs1] <= f_registers[rs2] {
+        1
+    } else {
+        0
+    }
+}
+
+pub fn fclass_s(registers: &mut [u32; 32], f_registers: &mut [f32; 32], rd: usize, rs1: usize) {
+    let number = f_registers[rs1];
+    let class = number.classify();
+    let frac_msb = (number.to_bits() >> 22) & 0x01;
+
+    registers[rd] = match class {
+        FpCategory::Nan => {
+            if frac_msb == 0 {
+                0b1
+            } else {
+                0b10
+            }
+        }
+        FpCategory::Infinite => {
+            if number.is_sign_negative() {
+                0b100
+            } else {
+                0b100000000
+            }
+        }
+        FpCategory::Zero => {
+            if number.is_sign_negative() {
+                0b10000
+            } else {
+                0b100000
+            }
+        }
+        FpCategory::Subnormal => {
+            if number.is_sign_negative() {
+                0b1000
+            } else {
+                0b1000000
+            }
+        }
+        FpCategory::Normal => {
+            if number.is_sign_negative() {
+                0b100
+            } else {
+                0b10000000
+            }
+        }
+    }
+}
diff --git a/src/main.rs b/src/main.rs
index 4ccd0a1..9ec507b 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -17,6 +17,7 @@ static MEMORY: LazyLock<Mutex<Box<[u8]>>> =
 struct CpuState<'a> {
     memory: &'a mut Box<[u8]>,
     registers: &'a mut [u32; 32],
+    f_registers: &'a mut [f32; 32],
     pc: &'a mut u32,
     reservations: &'a mut HashSet<usize>,
 }
@@ -43,6 +44,7 @@ fn main() {
     }
 
     let mut registers: [u32; 32] = [0; 32];
+    let mut f_registers: [f32; 32] = [0f32; 32];
     let mut pc: u32 = 0;
     let mut reservations: HashSet<usize> = HashSet::new();
 
@@ -64,6 +66,7 @@ fn main() {
             handle_instruction(CpuState {
                 memory: &mut *memory,
                 registers: &mut registers,
+                f_registers: &mut f_registers,
                 pc: &mut pc,
                 reservations: &mut reservations,
             });
@@ -75,6 +78,7 @@ fn handle_instruction(
     CpuState {
         memory,
         registers,
+        f_registers,
         pc,
         reservations,
     }: CpuState,
@@ -352,6 +356,92 @@ fn handle_instruction(
                 _ => panic!("Illegal instruction: {:x}", word),
             }
         }
+        0b1000011 => {
+            // todo: handle rm bit
+            let rd = ((word >> 7) & REGISTER_NAME_WIDTH) as usize;
+            let rs1 = ((word >> 15) & REGISTER_NAME_WIDTH) as usize;
+            let rs2 = ((word >> 20) & REGISTER_NAME_WIDTH) as usize;
+            let rs3 = ((word >> 27) & REGISTER_NAME_WIDTH) as usize;
+
+            fmadd_s(f_registers, rd, rs1, rs2, rs3);
+        }
+        0b1000111 => {
+            // todo: handle rm bit
+            let rd = ((word >> 7) & REGISTER_NAME_WIDTH) as usize;
+            let rs1 = ((word >> 15) & REGISTER_NAME_WIDTH) as usize;
+            let rs2 = ((word >> 20) & REGISTER_NAME_WIDTH) as usize;
+            let rs3 = ((word >> 27) & REGISTER_NAME_WIDTH) as usize;
+
+            fmsub_s(f_registers, rd, rs1, rs2, rs3);
+        }
+        0b1001011 => {
+            // todo: handle rm bit
+            let rd = ((word >> 7) & REGISTER_NAME_WIDTH) as usize;
+            let rs1 = ((word >> 15) & REGISTER_NAME_WIDTH) as usize;
+            let rs2 = ((word >> 20) & REGISTER_NAME_WIDTH) as usize;
+            let rs3 = ((word >> 27) & REGISTER_NAME_WIDTH) as usize;
+
+            fnmsub_s(f_registers, rd, rs1, rs2, rs3);
+        }
+        0b1001111 => {
+            // todo: handle rm bit
+            let rd = ((word >> 7) & REGISTER_NAME_WIDTH) as usize;
+            let rs1 = ((word >> 15) & REGISTER_NAME_WIDTH) as usize;
+            let rs2 = ((word >> 20) & REGISTER_NAME_WIDTH) as usize;
+            let rs3 = ((word >> 27) & REGISTER_NAME_WIDTH) as usize;
+
+            fnmadd_s(f_registers, rd, rs1, rs2, rs3);
+        }
+        0b1010011 => {
+            // todo: handle rm bit
+            let rd = ((word >> 7) & REGISTER_NAME_WIDTH) as usize;
+            let rs1 = ((word >> 15) & REGISTER_NAME_WIDTH) as usize;
+            let rs2 = ((word >> 20) & REGISTER_NAME_WIDTH) as usize;
+            let f5 = word >> 27;
+            let rm = (word >> 12) & 0b111;
+
+            match f5 {
+                0 => fadd_s(f_registers, rd, rs1, rs2),
+                1 => fsub_s(f_registers, rd, rs1, rs2),
+                2 => fmul_s(f_registers, rd, rs1, rs2),
+                3 => fdiv_s(f_registers, rd, rs1, rs2),
+                11 => fsqrt_s(f_registers, rd, rs1),
+                4 => match rm {
+                    0 => fsgnj_s(f_registers, rd, rs1, rs2),
+                    1 => fsgnjn_s(f_registers, rd, rs1, rs2),
+                    2 => fsgnjx_s(f_registers, rd, rs1, rs2),
+                    _ => panic!("Illegal instruction: {:x}", rm),
+                },
+                5 => match rm {
+                    0 => fmin_s(f_registers, rd, rs1, rs2),
+                    1 => fmax_s(f_registers, rd, rs1, rs2),
+                    _ => panic!("Illegal instruction: {:x}", rm),
+                },
+                24 => {
+                    fcvt_wu_s(registers, f_registers, rd, rs1);
+                }
+                28 => {
+                    match rm {
+                        0 => fmv_x_w(registers, f_registers, rd, rs1),
+                        1 => fclass_s(registers, f_registers, rd, rs1),
+                        _ => panic!("Illegal instruction: {:x}", rm),
+                    };
+                }
+                20 => match rm {
+                    0 => feq_s(registers, f_registers, rd, rs1, rs2),
+                    1 => flt_s(registers, f_registers, rd, rs1, rs2),
+                    2 => fle_s(registers, f_registers, rd, rs1, rs2),
+                    _ => panic!("Illegal instruction: {:x}", rm),
+                },
+
+                _ => panic!("Illegal instruction: {:x}", word),
+            }
+        }
+        0b1010101 => {
+            let rd = ((word >> 7) & REGISTER_NAME_WIDTH) as usize;
+            let rs1 = ((word >> 15) & REGISTER_NAME_WIDTH) as usize;
+            fcvt_w_s(registers, f_registers, rd, rs1);
+        }
         _ => panic!("Illegal instruction or todo: {:x}", word),
     }