math: Added 16 bit sub

diff --git a/src/math.s b/src/math.s
index 22e0f6474d9314a763962045a81bfb848ffba08d..6a612eed5bd551627f499c38a0b1ec906bbb921c 100644 (file)
--- a/src/math.s
+++ b/src/math.s
@@ -52,4 +52,19 @@ m16_add:
        ;       returns:
        ;               m16_a: m16_a - m16_b
 m16_sub:
+       ld a, [m16_b]
+       ld b, a
+       ld a, [m16_a]
+       
+       ; low is stored
+       sub a, b
+       ld [m16_a], a
+
+       ld a, [m16_b+1]
+       ld b, a
+       ld a, [m16_a+1]
+
+       ; high + carry stored
+       sbc a, b
+       ld [m16_a+1], a
        ret