Operand code update

4 files changed, 95 insertions, 74 deletions

diff --git a/src/Commands.hs b/src/Commands.hs
index e12cfdf..ea0a50d 100644
--- a/src/Commands.hs
+++ b/src/Commands.hs
@@ -112,18 +112,18 @@ doTRAP _ = return ()
 doLINK :: Int -> Emulator ()
 doLINK a = do
     incPC
-    addr <- readA a
-    sp <- readA 7
-    writeA 7 (sp-4)
-    setLong (sp-4) addr
+    addr <- readA a 4
+    sp <- readA 7 4
+    writeA 7 4 (sp - 4)
+    setLong (sp - 4) addr
 
 doUNLK :: Int -> Emulator ()
 doUNLK a = do
     incPC
-    addr <- readA a
+    addr <- readA a 4
     val <- getLong addr
-    writeA a val
-    writeA 7 (addr + 4)
+    writeA a 4 val
+    writeA 7 4 (addr + 4)
 
 doMOVEUSP :: Int -> Int -> Emulator ()
 doMOVEUSP _ _ = return ()
diff --git a/src/Machine.hs b/src/Machine.hs
index 53a0905..e6162e4 100644
--- a/src/Machine.hs
+++ b/src/Machine.hs
@@ -55,13 +55,13 @@ with field f = do
 -------------------------------------------------------------------------------
 -- Data and Address Registers Access
 
-readD :: Int -> Int -> Emulator Long
+readD :: Int -> Long -> Emulator Long
 readD 0 s = with drs $ \rs -> do
     (r,_,_,_,_,_,_,_) <- readIORef rs
     return $ convertLong r s
 readD 1 s = with drs $ \rs -> do
     (_,r,_,_,_,_,_,_) <- readIORef rs
-    return r
+    return $ convertLong r s
 readD 2 s = with drs $ \rs -> do
     (_,_,r,_,_,_,_,_) <- readIORef rs
     return $ convertLong r s
@@ -82,35 +82,35 @@ readD 7 s = with drs $ \rs -> do
     return $ convertLong r s
 readD _ _ = return $ error "Incorrect Data register read"
 
-readA :: Int -> Emulator Long
-readA 0 = with ars $ \rs -> do
+readA :: Int -> Long -> Emulator Long
+readA 0 s = with ars $ \rs -> do
     (r,_,_,_,_,_,_) <- readIORef rs
-    return r
-readA 1 = with ars $ \rs -> do
+    return $ convertLong r s
+readA 1 s = with ars $ \rs -> do
     (_,r,_,_,_,_,_) <- readIORef rs
-    return r
-readA 2 = with ars $ \rs -> do
+    return $ convertLong r s
+readA 2 s = with ars $ \rs -> do
     (_,_,r,_,_,_,_) <- readIORef rs
-    return r
-readA 3 = with ars $ \rs -> do
+    return $ convertLong r s
+readA 3 s = with ars $ \rs -> do
     (_,_,_,r,_,_,_) <- readIORef rs
-    return r
-readA 4 = with ars $ \rs -> do
+    return $ convertLong r s
+readA 4 s = with ars $ \rs -> do
     (_,_,_,_,r,_,_) <- readIORef rs
-    return r
-readA 5 = with ars $ \rs -> do
+    return $ convertLong r s
+readA 5 s = with ars $ \rs -> do
     (_,_,_,_,_,r,_) <- readIORef rs
-    return r
-readA 6 = with ars $ \rs -> do
+    return $ convertLong r s
+readA 6 s = with ars $ \rs -> do
     (_,_,_,_,_,_,r) <- readIORef rs
-    return r
-readA 7 = isSupervisor >>= \sup -> if sup
+    return $ convertLong r s
+readA 7 s = isSupervisor >>= \sup -> if sup
     then with ssp $ \sp -> readIORef sp
     else with usp $ \sp -> readIORef sp
-readA _ = return $ error "Incorrect Address register read"
+readA _ _ = return $ error "Incorrect Address register read"
 
 
-writeD :: Int -> Int -> Long -> Emulator ()
+writeD :: Int -> Long -> 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)
@@ -137,32 +137,32 @@ writeD 7 s r = with drs $ \rs -> do
     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
-    (_,r1,r2,r3,r4,r5,r6) <- readIORef rs
-    writeIORef rs (r,r1,r2,r3,r4,r5,r6)
-writeA 1 r = with ars $ \rs -> do
-    (r0,_,r2,r3,r4,r5,r6) <- readIORef rs
-    writeIORef rs (r0,r,r2,r3,r4,r5,r6)
-writeA 2 r = with ars $ \rs -> do
-    (r0,r1,_,r3,r4,r5,r6) <- readIORef rs
-    writeIORef rs (r0,r1,r,r3,r4,r5,r6)
-writeA 3 r = with ars $ \rs -> do
-    (r0,r1,r2,_,r4,r5,r6) <- readIORef rs
-    writeIORef rs (r0,r1,r2,r,r4,r5,r6)
-writeA 4 r = with ars $ \rs -> do
-    (r0,r1,r2,r3,_,r5,r6) <- readIORef rs
-    writeIORef rs (r0,r1,r2,r3,r,r5,r6)
-writeA 5 r = with ars $ \rs -> do
-    (r0,r1,r2,r3,r4,_,r6) <- readIORef rs
-    writeIORef rs (r0,r0,r2,r3,r4,r,r6)
-writeA 6 r = with ars $ \rs -> do
-    (r0,r1,r2,r3,r4,r5,_) <- readIORef rs
-    writeIORef rs (r0,r1,r2,r3,r4,r5,r)
-writeA 7 r = isSupervisor >>= \sup -> if sup
+writeA :: Int -> Long -> Long -> Emulator ()
+writeA 0 s r = with ars $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6) <- readIORef rs
+    writeIORef rs (combineLong r r0 s,r1,r2,r3,r4,r5,r6)
+writeA 1 s r = with ars $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6) <- readIORef rs
+    writeIORef rs (r0,combineLong r r1 s,r2,r3,r4,r5,r6)
+writeA 2 s r = with ars $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6) <- readIORef rs
+    writeIORef rs (r0,r1,combineLong r r2 s,r3,r4,r5,r6)
+writeA 3 s r = with ars $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6) <- readIORef rs
+    writeIORef rs (r0,r1,r2,combineLong r r3 s,r4,r5,r6)
+writeA 4 s r = with ars $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6) <- readIORef rs
+    writeIORef rs (r0,r1,r2,r3,combineLong r r4 s,r5,r6)
+writeA 5 s r = with ars $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6) <- readIORef rs
+    writeIORef rs (r0,r0,r2,r3,r4,combineLong r r5 s,r6)
+writeA 6 s r = with ars $ \rs -> do
+    (r0,r1,r2,r3,r4,r5,r6) <- readIORef rs
+    writeIORef rs (r0,r1,r2,r3,r4,r5,combineLong r r6 s)
+writeA 7 s r = isSupervisor >>= \sup -> if sup
     then with ssp $ \sp -> writeIORef sp r
     else with usp $ \sp -> writeIORef sp r
-writeA _ _ = return $ error "Incorrect Address register write"
+writeA _ _ _ = return $ error "Incorrect Address register write"
 
 
 -------------------------------------------------------------------------------
@@ -317,6 +317,27 @@ setLong a l = do
 -------------------------------------------------------------------------------
 -- Operand Access
 
-getOp :: Int -> Int -> Int
-      -> (Emulator Long, Long -> Emulator ())
-getOp 0 dr s = (readD dr s, writeD dr s)
+getOp :: Int -> Int -> Long
+      -> Emulator (Emulator Long, Long -> Emulator ())
+getOp 0 dr s = return (readD dr s, writeD dr s)
+getOp 1 ar s = return (readA ar s, writeA ar s)
+getOp 2 ar _ = do
+    addr <- readA ar 4
+    return (getLong addr, setLong addr)
+getOp 3 ar 4 = do
+    addr <- readA ar 4
+    writeA ar 4 (addr + 4)
+    return (getLong addr, setLong addr)
+getOp 4 ar 4 = do
+    addr <- readA ar 4
+    writeA ar 4 (addr - 4)
+    new_addr <- readA ar 4
+    return (getLong new_addr, setLong new_addr)
+getOp 7 1 4 = do
+    pc <- readPC
+    addr <- getLong pc
+    return (getLong addr, setLong addr)
+getOp 7 6 4 = do
+    addr <- readPC
+    incPC
+    return (getLong addr, setLong addr)
diff --git a/src/Suem.hs b/src/Suem.hs
index ca994a8..5edd041 100644
--- a/src/Suem.hs
+++ b/src/Suem.hs
@@ -374,22 +374,22 @@ doCommand cmd
     | otherwise = do
                 pc <- readPC
                 sr <- readSR
-                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
-                a3 <- readA 3
-                a4 <- readA 4
-                a5 <- readA 5
-                a6 <- readA 6
-                a7 <- readA 7
+                d0 <- readD 0 4
+                d1 <- readD 1 4
+                d2 <- readD 2 4
+                d3 <- readD 3 4
+                d4 <- readD 4 4
+                d5 <- readD 5 4
+                d6 <- readD 6 4
+                d7 <- readD 7 4
+                a0 <- readA 0 4
+                a1 <- readA 1 4
+                a2 <- readA 2 4
+                a3 <- readA 3 4
+                a4 <- readA 4 4
+                a5 <- readA 5 4
+                a6 <- readA 6 4
+                a7 <- readA 7 4
                 error ("Error:\n"
                     ++ "PC:0x" ++ showHex pc "\n"
                     ++ "SR:0x" ++ showHex sr "\n\n"
diff --git a/src/Utils.hs b/src/Utils.hs
index 82b31ca..cffcc11 100644
--- a/src/Utils.hs
+++ b/src/Utils.hs
@@ -25,15 +25,15 @@ extractBits x r = fromBits $ toBits x r
 -------------------------------------------------------------------------------
 -- Size Convertion
 
-convertLong :: Word32 -> Int -> Word32
+convertLong :: Word32 -> Word32 -> Word32
 convertLong x 1 = x .&. 0x000000FF
 convertLong x 2 = x .&. 0x0000FFFF
-convertLong x 3 = x
+convertLong x 4 = x
 convertLong _ s = error $ "Wrong size (" ++ show s ++ ") of convertLong"
 
-combineLong :: Word32 -> Word32 -> Int -> Word32
+combineLong :: Word32 -> Word32 -> Word32 -> Word32
 combineLong update base 1 = base .&. 0xFFFFFF00 .|. (convertLong update 1)
 combineLong update base 2 = base .&. 0xFFFF0000 .|. (convertLong update 2)
-combineLong update _    3 = update
+combineLong update _    4 = update
 combineLong _      _    s = error $
     "Wrong size (" ++ show s ++ ") of combineLong"