From 951620286dfb2fc7da57914088bacab0cd9b05a5 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Fri, 13 Dec 2024 11:40:23 -0500
Subject: [PATCH] can2040: Optimize unstuf_pull_bits() on rp2350

It is possible to use the ARM clz instruction to optimize bit
unstuffing.  (This instruction is not present on cortex-m0+ chips so
can't be used on the rp2040.)

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 scripts/bitstuf.py | 46 +++++++++++++++++++++++++++++++++++++++++++++-
 src/can2040.c      | 45 ++++++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 89 insertions(+), 2 deletions(-)

diff --git a/scripts/bitstuf.py b/scripts/bitstuf.py
index e75106b..3daac26 100755
--- a/scripts/bitstuf.py
+++ b/scripts/bitstuf.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 # Tool for testing bitstuffing implementations
 #
-# Copyright (C) 2022  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2022-2024  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import sys
@@ -11,6 +11,10 @@
 def report(msg):
     sys.stderr.write("\n" + msg + "\n")
 
+def clz(v):
+    v &= 0xffffffff
+    return 32 - v.bit_length()
+
 LOOP = UNLOOP = 0
 
 def bitstuf(b, num_bits):
@@ -174,6 +178,46 @@ def bitunstuf_batch_pass4(sb, num_bits):
         else:
             try_cnt //= 2
 
+def bitunstuf_clz(sb, num_bits):
+    global UNLOOP
+    edges = sb ^ (sb >> 1)
+    e2 = edges | (edges >> 1)
+    e4 = e2 | (e2 >> 2)
+    rm_bits = ~e4
+    unstuffed_bits = 0
+    cu = TESTBITS
+    cs = num_bits
+    while 1:
+        UNLOOP += 1
+        if not cs:
+            # Need more data
+            return -999
+        try_cnt = cu if cu < cs else cs
+        try_mask = ((1 << try_cnt) - 1) << (cs + 1 - try_cnt)
+        rm_masked_bits = rm_bits & try_mask
+        if not rm_masked_bits:
+            # No stuff bits in try_cnt bits - copy into unstuffed_bits
+            cu -= try_cnt
+            cs -= try_cnt
+            unstuffed_bits |= ((sb >> cs) & ((1 << try_cnt) - 1)) << cu
+            if not cu:
+                # Extracted desired bits
+                return unstuffed_bits
+            # Need more data
+            return -999
+        # Copy any leading bits prior to stuff bit (may be zero)
+        copy_cnt = cs - (31 - clz(rm_masked_bits))
+        cs -= copy_cnt
+        cu -= copy_cnt
+        unstuffed_bits |= ((sb >> cs) & ((1 << copy_cnt) - 1)) << cu
+        # High bit is now a stuff bit - remove it
+        cs -= 1
+        if rm_bits & (1 << cs):
+            # Six consecutive bits - a bitstuff error
+            if (sb >> cs) & 1:
+                return -cs
+            return -cs
+
 def main():
     stuf_func = bitstuf_batch
     unstuf_func = bitunstuf_batch
diff --git a/src/can2040.c b/src/can2040.c
index c195db9..13f2f11 100644
--- a/src/can2040.c
+++ b/src/can2040.c
@@ -506,7 +506,7 @@ unstuf_restore_state(struct can2040_bitunstuffer *bu, uint32_t data)
 
 // Pull bits from unstuffer (as specified in unstuf_set_count() )
 static int
-unstuf_pull_bits(struct can2040_bitunstuffer *bu)
+unstuf_pull_bits_rp2040(struct can2040_bitunstuffer *bu)
 {
     uint32_t sb = bu->stuffed_bits, edges = sb ^ (sb >> 1);
     uint32_t e2 = edges | (edges >> 1), e4 = e2 | (e2 >> 2), rm_bits = ~e4;
@@ -550,6 +550,49 @@ unstuf_pull_bits(struct can2040_bitunstuffer *bu)
     }
 }
 
+// Pull bits from unstuffer (optimized for rp2350)
+static int
+unstuf_pull_bits(struct can2040_bitunstuffer *bu)
+{
+    if (!IS_RP2350)
+        return unstuf_pull_bits_rp2040(bu);
+    uint32_t sb = bu->stuffed_bits, edges = sb ^ (sb >> 1);
+    uint32_t e2 = edges | (edges >> 1), e4 = e2 | (e2 >> 2), rm_bits = ~e4;
+    uint32_t cs = bu->count_stuff, cu = bu->count_unstuff;
+    for (;;) {
+        if (!cs)
+            // Need more data
+            return 1;
+        uint32_t try_cnt = cs > cu ? cu : cs;
+        uint32_t try_mask = ((1 << try_cnt) - 1) << (cs + 1 - try_cnt);
+        uint32_t rm_masked_bits = rm_bits & try_mask;
+        if (likely(!rm_masked_bits)) {
+            // No stuff bits in try_cnt bits - copy into unstuffed_bits
+            bu->count_unstuff = cu = cu - try_cnt;
+            bu->count_stuff = cs = cs - try_cnt;
+            bu->unstuffed_bits |= ((sb >> cs) & ((1 << try_cnt) - 1)) << cu;
+            if (! cu)
+                // Extracted desired bits
+                return 0;
+            // Need more data
+            return 1;
+        }
+        // Copy any leading bits prior to stuff bit (may be zero)
+        uint32_t copy_cnt = cs - (31 - __builtin_clz(rm_masked_bits));
+        cs -= copy_cnt;
+        bu->count_unstuff = cu = cu - copy_cnt;
+        bu->unstuffed_bits |= ((sb >> cs) & ((1 << copy_cnt) - 1)) << cu;
+        // High bit is now a stuff bit - remove it
+        bu->count_stuff = cs = cs - 1;
+        if (unlikely(rm_bits & (1 << cs))) {
+            // Six consecutive bits - a bitstuff error
+            if (sb & (1 << cs))
+                return -1;
+            return -2;
+        }
+    }
+}
+
 // Return most recent raw (still stuffed) bits
 static uint32_t
 unstuf_get_raw(struct can2040_bitunstuffer *bu)