Implement halfword access

2 files changed, 58 insertions, 6 deletions

diff --git a/src/lib.rs b/src/lib.rs
index feba5e2..cec9d01 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -143,10 +143,22 @@ mod tests {
                    Err(MemoryError::InvalidAddress));
         assert_eq!(memory.write_byte(2, 0xF0),
                    Err(MemoryError::InvalidAddress));
+        assert_eq!(memory.write_byte(3, 0xF0),
+                   Err(MemoryError::InvalidAddress));
+        assert_eq!(memory.write_halfword(0, 0xF0),
+                   Err(MemoryError::InvalidAddress));
+        assert_eq!(memory.write_halfword(2, 0xF0),
+                   Err(MemoryError::InvalidAddress));
 
         for address in (4..size).step_by(4) {
             assert_eq!(memory.write_word(address, 0xF0), Ok(()));
             assert_eq!(memory.read_word(address), Ok(0xF0));
+            assert_eq!(memory.read_halfword(address), Ok(0xF0));
+            assert_eq!(memory.read_halfword(address + 2), Ok(0x0));
+            assert_eq!(memory.read_byte(address), Ok(0xF0));
+            assert_eq!(memory.read_byte(address + 1), Ok(0x0));
+            assert_eq!(memory.read_byte(address + 2), Ok(0x0));
+            assert_eq!(memory.read_byte(address + 3), Ok(0x0));
         }
 
         assert_eq!(memory.write_word(0x10, 0x01234567), Ok(()));
@@ -155,6 +167,8 @@ mod tests {
         assert_eq!(memory.read_byte(0x11), Ok(0x45));
         assert_eq!(memory.read_byte(0x12), Ok(0x23));
         assert_eq!(memory.read_byte(0x13), Ok(0x01));
+        assert_eq!(memory.read_halfword(0x10), Ok(0x4567));
+        assert_eq!(memory.read_halfword(0x12), Ok(0x0123));
 
         let stall = Err(MemoryError::CacheMiss {
             stall_cycles: memory.latency(),
@@ -183,6 +197,8 @@ mod tests {
         assert_eq!(dm_cache.read_byte(0x13), Ok(0x01));
         assert_eq!(dm_cache.write_word(0x18, 0xBEEFBEEF), Ok(()));
         assert_eq!(dm_cache.read_word(0x18), Ok(0xBEEFBEEF));
+        assert_eq!(dm_cache.read_halfword(0x10), Ok(0x4567));
+        assert_eq!(dm_cache.read_halfword(0x12), Ok(0x0123));
         assert_eq!(memory_ref.borrow_mut().read_word(0x18), Ok(0xBEEFBEEF));
 
         for _ in 0..100 {
@@ -196,5 +212,11 @@ mod tests {
         assert_eq!(dm_cache.read_word(0x14), Ok(0xDEADBEEF));
         assert_eq!(dm_cache.read_word(0x18), Ok(0xBEEFBEEF));
         assert_eq!(dm_cache.read_word(0x1C), Ok(0xF0));
+        assert_eq!(dm_cache.read_halfword(0x10), Ok(0x4567));
+        assert_eq!(dm_cache.read_halfword(0x12), Ok(0x0123));
+
+        assert_eq!(dm_cache.write_byte(0x10, 0x42), Ok(()));
+        assert_eq!(dm_cache.write_halfword(0x12, 0x4242), Ok(()));
+        assert_eq!(memory_ref.borrow_mut().read_word(0x10), Ok(0x42424542));
     }
 }
diff --git a/src/memory.rs b/src/memory.rs
index a0e1a11..5e630b3 100644
--- a/src/memory.rs
+++ b/src/memory.rs
@@ -48,8 +48,39 @@ pub trait MemoryInterface {
         }
     }
 
-    // fn read_halfword(&self, address: isa::Address) -> Result<isa::HalfWord>;
-    // fn write_halfword(&self, address: isa::Address) -> Result<()>;
+    // TODO: check address more thoroughly
+
+    fn read_halfword(&mut self, address: isa::Address) -> Result<isa::HalfWord> {
+        let result = self.read_word(address);
+        let offset = address & 0b10;
+
+        match result {
+            Ok(word) => match offset {
+                0 => Ok((word & 0xFFFF) as isa::HalfWord),
+                2 => Ok(((word & 0xFFFF0000) >> 16) as isa::HalfWord),
+                _ => panic!("Invalid halfword offset: address {:x}", address),
+            },
+            Err(e) => Err(e),
+        }
+    }
+
+    fn write_halfword(&mut self, address: isa::Address, value: isa::HalfWord) -> Result<()> {
+        let result = self.read_word(address);
+        let offset = address & 0b10;
+        let value = value as isa::Word;
+
+        match result {
+            Ok(word) => {
+                let value = match offset {
+                    0 => (word & 0xFFFF0000) | value,
+                    2 => (word & 0x0000FFFF) | (value << 16),
+                    _ => panic!("Invalid halfword offset: address {:x}", address),
+                };
+                self.write_word(address, value)
+            },
+            Err(e) => Err(e),
+        }
+    }
 
     fn read_byte(&mut self, address: isa::Address) -> Result<isa::Byte> {
         let result = self.read_word(address);
@@ -61,16 +92,15 @@ pub trait MemoryInterface {
                 1 => Ok(((word & 0xFF00) >> 8) as isa::Byte),
                 2 => Ok(((word & 0xFF0000) >> 16) as isa::Byte),
                 3 => Ok(((word & 0xFF000000) >> 24) as isa::Byte),
-                _ => panic!(""),
+                _ => panic!("Invalid byte offset: {:x}", address),
             },
             Err(e) => Err(e),
         }
     }
 
     fn write_byte(&mut self, address: isa::Address, value: isa::Byte) -> Result<()> {
-        let offset = address % 4;
-
         let result = self.read_word(address);
+        let offset = address % 4;
         let value = value as isa::Word;
 
         match result {
@@ -80,7 +110,7 @@ pub trait MemoryInterface {
                     1 => (word & !(0xFF00)) | (value << 8),
                     2 => (word & !(0xFF0000)) | (value << 16),
                     3 => (word & !(0xFF000000)) | (value << 24),
-                    _ => panic!(""),
+                    _ => panic!("Invalid byte offset: {:x}", address),
                 };
                 self.write_word(address, value)
             },