Puf.

3 files changed, 77 insertions, 51 deletions

diff --git a/src/Machine.hs b/src/Machine.hs
index eebbe91..53a0905 100644
--- a/src/Machine.hs
+++ b/src/Machine.hs
@@ -55,32 +55,32 @@ with field f = do
 -------------------------------------------------------------------------------
 -- Data and Address Registers Access
 
-readD :: Int -> Emulator Long
-readD 0 = with drs $ \rs -> do
+readD :: Int -> Int -> Emulator Long
+readD 0 s = with drs $ \rs -> do
     (r,_,_,_,_,_,_,_) <- readIORef rs
-    return r
-readD 1 = with drs $ \rs -> do
+    return $ convertLong r s
+readD 1 s = with drs $ \rs -> do
     (_,r,_,_,_,_,_,_) <- readIORef rs
     return r
-readD 2 = with drs $ \rs -> do
+readD 2 s = with drs $ \rs -> do
     (_,_,r,_,_,_,_,_) <- readIORef rs
-    return r
-readD 3 = with drs $ \rs -> do
+    return $ convertLong r s
+readD 3 s = with drs $ \rs -> do
     (_,_,_,r,_,_,_,_) <- readIORef rs
-    return r
-readD 4 = with drs $ \rs -> do
+    return $ convertLong r s
+readD 4 s = with drs $ \rs -> do
     (_,_,_,_,r,_,_,_) <- readIORef rs
-    return r
-readD 5 = with drs $ \rs -> do
+    return $ convertLong r s
+readD 5 s = with drs $ \rs -> do
     (_,_,_,_,_,r,_,_) <- readIORef rs
-    return r
-readD 6 = with drs $ \rs -> do
+    return $ convertLong r s
+readD 6 s = with drs $ \rs -> do
     (_,_,_,_,_,_,r,_) <- readIORef rs
-    return r
-readD 7 = with drs $ \rs -> do
+    return $ convertLong r s
+readD 7 s = with drs $ \rs -> do
     (_,_,_,_,_,_,_,r) <- readIORef rs
-    return r
-readD _ = return $ error "Incorrect Data register read"
+    return $ convertLong r s
+readD _ _ = return $ error "Incorrect Data register read"
 
 readA :: Int -> Emulator Long
 readA 0 = with ars $ \rs -> do
@@ -110,32 +110,32 @@ readA 7 = isSupervisor >>= \sup -> if sup
 readA _ = return $ error "Incorrect Address register read"
 
 
-writeD :: Int -> Long -> Emulator ()
-writeD 0 r = with drs $ \rs -> do
-    (_,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
-    writeIORef rs (r,r1,r2,r3,r4,r5,r6,r7)
-writeD 1 r = with drs $ \rs -> do
-    (r0,_,r2,r3,r4,r5,r6,r7) <- readIORef rs
-    writeIORef rs (r0,r,r2,r3,r4,r5,r6,r7)
-writeD 2 r = with drs $ \rs -> do
-    (r0,r1,_,r3,r4,r5,r6,r7) <- readIORef rs
-    writeIORef rs (r0,r1,r,r3,r4,r5,r6,r7)
-writeD 3 r = with drs $ \rs -> do
-    (r0,r1,r2,_,r4,r5,r6,r7) <- readIORef rs
-    writeIORef rs (r0,r1,r2,r,r4,r5,r6,r7)
-writeD 4 r = with drs $ \rs -> do
-    (r0,r1,r2,r3,_,r5,r6,r7) <- readIORef rs
-    writeIORef rs (r0,r1,r2,r3,r,r5,r6,r7)
-writeD 5 r = with drs $ \rs -> do
-    (r0,r1,r2,r3,r4,_,r6,r7) <- readIORef rs
-    writeIORef rs (r0,r0,r2,r3,r4,r,r6,r7)
-writeD 6 r = with drs $ \rs -> do
-    (r0,r1,r2,r3,r4,r5,_,r7) <- readIORef rs
-    writeIORef rs (r0,r1,r2,r3,r4,r5,r,r7)
-writeD 7 r = with drs $ \rs -> do
-    (r0,r1,r2,r3,r4,r5,r6,_) <- readIORef rs
-    writeIORef rs (r0,r1,r2,r3,r4,r5,r6,r)
-writeD _ _ = return $ error "Incorrect Data register write"
+writeD :: Int -> Int -> Long -> Emulator ()
+writeD 0 s r = with drs $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
+    writeIORef rs (combineLong r r0 s,r1,r2,r3,r4,r5,r6,r7)
+writeD 1 s r = with drs $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
+    writeIORef rs (r0,combineLong r r1 s,r2,r3,r4,r5,r6,r7)
+writeD 2 s r = with drs $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
+    writeIORef rs (r0,r1,combineLong r r2 s,r3,r4,r5,r6,r7)
+writeD 3 s r = with drs $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
+    writeIORef rs (r0,r1,r2,combineLong r r3 s,r4,r5,r6,r7)
+writeD 4 s r = with drs $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
+    writeIORef rs (r0,r1,r2,r3,combineLong r r4 s,r5,r6,r7)
+writeD 5 s r = with drs $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
+    writeIORef rs (r0,r0,r2,r3,r4,combineLong r r5 s,r6,r7)
+writeD 6 s r = with drs $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
+    writeIORef rs (r0,r1,r2,r3,r4,r5,combineLong r r6 s,r7)
+writeD 7 s r = with drs $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6,r7) <- readIORef rs
+    writeIORef rs (r0,r1,r2,r3,r4,r5,r6,combineLong r r7 s)
+writeD _ _ _ = return $ error "Incorrect Data register write"
 
 writeA :: Int -> Long -> Emulator ()
 writeA 0 r = with ars $ \rs -> do
@@ -312,3 +312,11 @@ setLong :: Long -> Long -> Emulator ()
 setLong a l = do
     setWord a       (fromIntegral (rem (fromIntegral l) 256 * 256))
     setWord (a + 2) (fromIntegral (div (fromIntegral l) 256 * 256))
+
+
+-------------------------------------------------------------------------------
+-- Operand Access
+
+getOp :: Int -> Int -> Int
+      -> (Emulator Long, Long -> Emulator ())
+getOp 0 dr s = (readD dr s, writeD dr s)
diff --git a/src/Suem.hs b/src/Suem.hs
index 8ac1c32..ca994a8 100644
--- a/src/Suem.hs
+++ b/src/Suem.hs
@@ -374,14 +374,14 @@ doCommand cmd
     | otherwise = do
                 pc <- readPC
                 sr <- readSR
-                d0 <- readD 0
-                d1 <- readD 1
-                d2 <- readD 2
-                d3 <- readD 3
-                d4 <- readD 4
-                d5 <- readD 5
-                d6 <- readD 6
-                d7 <- readD 7
+                d0 <- readD 0 3
+                d1 <- readD 1 3
+                d2 <- readD 2 3
+                d3 <- readD 3 3
+                d4 <- readD 4 3
+                d5 <- readD 5 3
+                d6 <- readD 6 3
+                d7 <- readD 7 3
                 a0 <- readA 0
                 a1 <- readA 1
                 a2 <- readA 2
diff --git a/src/Utils.hs b/src/Utils.hs
index 03d6db3..82b31ca 100644
--- a/src/Utils.hs
+++ b/src/Utils.hs
@@ -2,6 +2,7 @@
 module Utils where
 
 import Data.Bits
+import Data.Word (Word8, Word16, Word32)
 
 
 -------------------------------------------------------------------------------
@@ -19,3 +20,20 @@ fromBits = foldl (\a b -> 2 * a + b) 0 . reverse
 
 extractBits :: Bits a => a -> [Int] -> Int
 extractBits x r = fromBits $ toBits x r
+
+
+-------------------------------------------------------------------------------
+-- Size Convertion
+
+convertLong :: Word32 -> Int -> Word32
+convertLong x 1 = x .&. 0x000000FF
+convertLong x 2 = x .&. 0x0000FFFF
+convertLong x 3 = x
+convertLong _ s = error $ "Wrong size (" ++ show s ++ ") of convertLong"
+
+combineLong :: Word32 -> Word32 -> Int -> Word32
+combineLong update base 1 = base .&. 0xFFFFFF00 .|. (convertLong update 1)
+combineLong update base 2 = base .&. 0xFFFF0000 .|. (convertLong update 2)
+combineLong update _    3 = update
+combineLong _      _    s = error $
+    "Wrong size (" ++ show s ++ ") of combineLong"