From 20fbac086c95122047c49adacae95ff1310db8e9 Mon Sep 17 00:00:00 2001
From: Romain Dolbeau <romain@dolbeau.org>
Date: Sat, 9 Jan 2021 09:02:43 -0500
Subject: [PATCH] PIO decrypt of CBC

---
 NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.c | 49 +++++++++++++++++++++++-
 NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.h |  1 +
 sbus-to-ztex-gateware/sbus_fsm.vhd       |  9 +++--
 3 files changed, 54 insertions(+), 5 deletions(-)

diff --git a/NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.c b/NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.c
index f6ff185..c14b251 100644
--- a/NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.c
+++ b/NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.c
@@ -704,7 +704,54 @@ static void
 rdfpga_rijndael128_decrypt(void *key, u_int8_t *blk)
 {
   /* ugly cast */
-	rijndael_decrypt((rijndael_ctx *) key, (u_char *) blk, (u_char *) blk);
+  //	rijndael_decrypt((rijndael_ctx *) key, (u_char *) blk, (u_char *) blk);
+    u_int32_t ctrl;
+  u_int64_t data[2];
+  u_int64_t *ptr;
+  int i;
+  rdfpga_rijndael_ctx* ctx;
+  struct rdfpga_softc *sc;
+  
+  ctx = key;
+  sc = ctx->sc;
+
+  /* alignment constraint */
+  if (!(((u_int32_t)blk) & 0x7)) {
+    ptr = (u_int64_t*)blk;
+  } else {
+    memcpy(data, blk, 16);
+    ptr = data;
+  }
+  
+  if (rdfpga_wait_aes_ready(sc)) {
+    aprint_error_dev(sc->sc_dev, "rdfpga_rijndael128_crypt: stuck\n");
+    return;
+  }
+  
+  /* aprint_normal_dev(sc->sc_dev, "rdfpga_rijndael128_crypt: write data & start\n"); */
+  
+  for (i = 0 ; i < 2 ; i++)
+    bus_space_write_8(sc->sc_bustag, sc->sc_bhregs, (RDFPGA_REG_AES128_DATA + (i*8)), ptr[i] );
+  ctrl = RDFPGA_MASK_AES128_START | RDFPGA_MASK_AES128_DEC;
+  //  if (ctx->cbc)
+  //    ctrl |= RDFPGA_MASK_AES128_CBCMOD;
+  bus_space_write_4(sc->sc_bustag, sc->sc_bhregs, RDFPGA_REG_AES128_CTRL, ctrl);
+
+  if (ctx->readback) {
+    if (rdfpga_wait_aes_ready(sc)) {
+      aprint_error_dev(sc->sc_dev, "rdfpga_rijndael128_crypt: stuck\n");
+      return;
+    }
+    
+    for (i = 0 ; i < 2 ; i++)
+      ptr[i] = bus_space_read_8(sc->sc_bustag, sc->sc_bhregs, (RDFPGA_REG_AES128_OUT + (i*8)));
+    
+    if (!(((u_int32_t)blk) & 0x7)) {
+      /* nothing */
+    } else {
+      memcpy(blk, data, 16);
+    }
+  }
 }
 
 static int
diff --git a/NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.h b/NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.h
index 4f44314..81b5589 100644
--- a/NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.h
+++ b/NetBSD/9.0/usr/src/sys/dev/sbus/rdfpga.h
@@ -97,5 +97,6 @@ struct rdfpga_softc {
 #define RDFPGA_MASK_AES128_NEWKEY 0x10000000
 #define RDFPGA_MASK_AES128_CBCMOD 0x08000000
 #define RDFPGA_MASK_AES128_AES256 0x04000000
+#define RDFPGA_MASK_AES128_DEC    0x02000000
 
 #endif /* _RDFPGA_H_ */
diff --git a/sbus-to-ztex-gateware/sbus_fsm.vhd b/sbus-to-ztex-gateware/sbus_fsm.vhd
index 0430245..4579950 100644
--- a/sbus-to-ztex-gateware/sbus_fsm.vhd
+++ b/sbus-to-ztex-gateware/sbus_fsm.vhd
@@ -127,6 +127,7 @@ ENTITY SBusFSM is
   constant AES128_CTRL_NEWKEY_IDX : integer := 28;
   constant AES128_CTRL_CBCMOD_IDX : integer := 27;
   constant AES128_CTRL_AES256_IDX : integer := 26;
+  constant AES128_CTRL_DEC_IDX    : integer := 25;
 
   -- OFFSET to REGS; (8 downto 0) so 9 bits
   CONSTANT REG_OFFSET_LED        : std_logic_vector(8 downto 0) := conv_std_logic_vector(REG_INDEX_LED   *4, 9);
@@ -1133,7 +1134,7 @@ BEGIN
               fifo_toaes_din <=
                 '0' & -- !iskey
                 '0' & -- keylen, ignored
-                '1' & -- encdec
+                '1' & -- encdec, enc for now
                 '0' & -- cbc
                 '1' & -- internal cbc
                 x"00000000000000000000000000000000" &
@@ -1144,7 +1145,7 @@ BEGIN
               fifo_toaes_din <=
                 '0' & -- !iskey
                 '0' & -- keylen, ignored
-                '1' & -- encdec
+                '1' & -- encdec, enc for now
                 '0' & -- cbc
                 '0' & -- internal cbc
                 x"00000000000000000000000000000000" &
@@ -1287,7 +1288,7 @@ BEGIN
             fifo_toaes_din <=
               '1' & -- iskey
               REGISTERS(REG_INDEX_AES128_CTRL)(AES128_CTRL_AES256_IDX) & -- keylen
-              '1' & -- encdec
+              (NOT REGISTERS(REG_INDEX_AES128_CTRL)(AES128_CTRL_DEC_IDX)) & -- encdec
               REGISTERS(REG_INDEX_AES128_CTRL)(AES128_CTRL_CBCMOD_IDX) & -- cbc
               '0' & -- internal cbc
               REGISTERS(REG_INDEX_AES128_KEY1) & REGISTERS(REG_INDEX_AES128_KEY2) &
@@ -1300,7 +1301,7 @@ BEGIN
             fifo_toaes_din <=
               '0' & -- !iskey
               REGISTERS(REG_INDEX_AES128_CTRL)(AES128_CTRL_AES256_IDX) & -- keylen
-              '1' & -- encdec
+              (NOT REGISTERS(REG_INDEX_AES128_CTRL)(AES128_CTRL_DEC_IDX)) & -- encdec
               REGISTERS(REG_INDEX_AES128_CTRL)(AES128_CTRL_CBCMOD_IDX) & -- cbc
               '0' & -- internal cbc
               REGISTERS(REG_INDEX_AES128_OUT1) & REGISTERS(REG_INDEX_AES128_OUT2) &