From 5219887ad7812e5b3c174999b2dc66942639fa41 Mon Sep 17 00:00:00 2001
From: Virgil Dupras <hsoft@hardcoded.net>
Date: Fri, 24 Apr 2020 20:46:01 -0400
Subject: [PATCH] sdc: implement _crc16 in z80

---
 blk/226                 |  1 +
 blk/373                 | 23 +++++++++++++++--------
 blk/377                 | 12 ------------
 tests/forth/test_sdc.fs |  6 +++---
 4 files changed, 19 insertions(+), 23 deletions(-)

diff --git a/blk/226 b/blk/226
index 0077e26..9ecca53 100644
--- a/blk/226
+++ b/blk/226
@@ -10,4 +10,5 @@
 0xe6 OP2n ANDn,
 0xf6 OP2n ORn,
 0xd6 OP2n SUBn,
+0xee OP2n XORn,
 
diff --git a/blk/373 b/blk/373
index 4d81491..90912b4 100644
--- a/blk/373
+++ b/blk/373
@@ -1,9 +1,16 @@
-CODE _sdcSel
-    SDC_CSLOW OUTnA,
+CODE _sdcSel SDC_CSLOW OUTnA, ;CODE
+CODE _sdcDesel SDC_CSHIGH OUTnA, ;CODE
+( Computes n into crc c with polynomial 0x1021 )
+CODE _crc16  ( c n -- c )
+    HL POPqq, ( n ) DE POPqq, ( c )
+    A L LDrr, D XORr, D A LDrr,
+    B 8 LDrn,
+    BEGIN,
+        E SLAr, D RLr,
+        IFC, ( msb is set, apply polynomial )
+            A D LDrr, 0x10 XORn, D A LDrr,
+            A E LDrr, 0x21 XORn, E A LDrr,
+        THEN,
+    DJNZ, AGAIN,
+    DE PUSHqq,
 ;CODE
-
-CODE _sdcDesel
-    SDC_CSHIGH OUTnA,
-;CODE
-
-
diff --git a/blk/377 b/blk/377
index 18facb2..a1a7c9f 100644
--- a/blk/377
+++ b/blk/377
@@ -1,15 +1,3 @@
-( c n -- c )
-( Computes n into crc c with polynomial 0x1021 )
-: _crc16
-    SWAP DUP 256 /    ( n c c>>8 )
-    ROT XOR           ( c x )
-    DUP 16 / XOR      ( c x^x>>4 )
-    SWAP 256 *        ( x c<<8 )
-    OVER 4096 * XOR   ( x c^x<<12 )
-    OVER 32 * XOR     ( x c^x<<5 )
-    XOR               ( c )
-;
-
 ( send-and-crc7 )
 ( n c -- c )
 : _s+crc SWAP DUP _sdcSR DROP _crc7 ;
diff --git a/tests/forth/test_sdc.fs b/tests/forth/test_sdc.fs
index a35d13d..db5cb1b 100644
--- a/tests/forth/test_sdc.fs
+++ b/tests/forth/test_sdc.fs
@@ -5,6 +5,6 @@
 372 LOAD ( sdc.z80 )
 374 LOAD ( sdc.fs )
 
-0x0000 0x0000 _crc16 0x0000 #eq
-0x0000 0x0001 _crc16 0x1021 #eq
-0x5678 0x1234 _crc16 0x43c4 #eq
+0x0000 0x00 _crc16 0x0000 #eq
+0x0000 0x01 _crc16 0x1021 #eq
+0x5678 0x34 _crc16 0x34e4 #eq