BSR and RTS added

4 files changed, 39 insertions, 21 deletions

diff --git a/src/Instructions.hs b/src/Instructions.hs
index 320c93a..98e4337 100644
--- a/src/Instructions.hs
+++ b/src/Instructions.hs
@@ -152,7 +152,11 @@ doRTE :: Emulator ()
 doRTE = error "RTE"
 
 doRTS :: Emulator ()
-doRTS = error "RTS"
+doRTS = do
+    sp <- readA 7 4
+    writeA 7 4 (sp + 4)
+    addr <- getMemory sp 4
+    writePC addr
 
 doTRAPV :: Emulator ()
 doTRAPV = error "TRAPV"
@@ -187,21 +191,24 @@ doADDQ _ _ _ _ = error "ADDQ"
 doSUBQ :: Int -> Int -> Int -> Int -> Emulator ()
 doSUBQ _ _ _ _ = error "SUBQ"
 
-doBRA :: Int -> Emulator ()
-doBRA 0 = do
-    incPC
-    pc <- readPC
-    disp <- getMemory pc 2
-    writePC (pc + disp)
-doBRA disp = do
+doBSR :: Int -> Emulator ()
+doBSR disp = do
     incPC
     pc <- readPC
-    writePC (pc + (fromIntegral disp))
-
-doBSR :: Int -> Emulator ()
-doBSR _ = error "BSR"
+    tmp_disp <- if disp == 0
+        then getMemory pc 2
+        else return $ fromIntegral disp
+    let final_disp = signExtend tmp_disp (if disp == 0 then 2 else 1)
+    writePC $ pc + final_disp
+    let return_address = if disp == 0 then pc + 2 else pc
+    sp <- readA 7 4
+    writeA 7 4 (sp - 4)
+    setMemory (sp - 4) 4 (fromIntegral return_address)
 
 checkBccCondition :: Int -> Emulator Bool
+-- BRA
+checkBccCondition 0 = do
+    return True
 -- BNE
 checkBccCondition 6 = do
     zf <- isZero
@@ -214,10 +221,11 @@ doBcc cc disp = do
     incPC
     pc <- readPC
     check <- checkBccCondition cc
-    the_disp <- if disp == 0
+    tmp_disp <- if disp == 0
         then if check then getMemory pc 2 else return 2
         else if check then return $ fromIntegral disp else return 0
-    writePC $ pc + the_disp
+    let final_disp = signExtend tmp_disp (if disp == 0 then 2 else 1)
+    writePC $ pc + final_disp
 
 doMOVEQ :: Int -> Int -> Emulator ()
 doMOVEQ _ _ = error "MOVEQ"
diff --git a/src/Machine.hs b/src/Machine.hs
index 1f0b6e5..91bcca0 100644
--- a/src/Machine.hs
+++ b/src/Machine.hs
@@ -312,14 +312,14 @@ setByte a b | a < 0x8 = return ()
  -- TODO: only even addresses are allowed
 setWord :: Long -> Word -> Emulator ()
 setWord a w = do
-    setByte a       (fromIntegral (rem (fromIntegral w) 256))
-    setByte (a + 1) (fromIntegral (div (fromIntegral w) 256))
+    setByte a       (fromIntegral (div (fromIntegral w) 256))
+    setByte (a + 1) (fromIntegral (rem (fromIntegral w) 256))
 
  -- TODO: only even addresses are allowed
 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))
+    setWord a       (fromIntegral (div (fromIntegral l) (256 * 256)))
+    setWord (a + 2) (fromIntegral (rem (fromIntegral l) (256 * 256)))
 
 
 getMemory :: Long -> Int -> Emulator Long
diff --git a/src/Suem.hs b/src/Suem.hs
index 63e52d2..eeb743d 100644
--- a/src/Suem.hs
+++ b/src/Suem.hs
@@ -201,9 +201,7 @@ doInstruction opcode
                (extractBits opcode [8..9])
                (extractBits opcode [10..12])
                (extractBits opcode [13..15])
-    | (extractBits opcode [0..7]) == 0b01100000 =
-        doBRA (extractBits opcode [8..15])
-    | (extractBits opcode [0..7]) == 0b01100000 =
+    | (extractBits opcode [0..7]) == 0b01100001 =
         doBSR (extractBits opcode [8..15])
     | (extractBits opcode [0..3]) == 0b0110 =
         doBcc (extractBits opcode [4..7])
diff --git a/src/Utils.hs b/src/Utils.hs
index ee1cd7a..cd2f1e3 100644
--- a/src/Utils.hs
+++ b/src/Utils.hs
@@ -64,3 +64,15 @@ checkZero 0 = True
 checkZero _ = False
 
 -- TODO: carry & overflow checkers
+
+-------------------------------------------------------------------------------
+-- Sign extender
+
+signExtend :: Word32 -> Int -> Word32
+signExtend x 1
+    | x < 0x80 = x
+    | otherwise = x + 0xffffff00
+signExtend x 2
+    | x < 0x8000 = x
+    | otherwise = x + 0xffff0000
+signExtend x 4 = x