From c2d09b5b6b278ed15b95e9c098eed2fa38ab4b21 Mon Sep 17 00:00:00 2001
From: Michael Nolan <mtnolan2640@gmail.com>
Date: Mon, 4 May 2020 15:56:24 -0400
Subject: [PATCH] Extend CLZ to work over even, non powers of 2

---
 src/ieee754/cordic/clz.py              | 51 +++++++++++++++++---------
 src/ieee754/cordic/formal/proof_clz.py |  2 +-
 src/ieee754/cordic/test/test_clz.py    |  4 ++
 3 files changed, 38 insertions(+), 19 deletions(-)

diff --git a/src/ieee754/cordic/clz.py b/src/ieee754/cordic/clz.py
index a6dec14c..51239dc2 100644
--- a/src/ieee754/cordic/clz.py
+++ b/src/ieee754/cordic/clz.py
@@ -24,31 +24,45 @@ class CLZ(Elaboratable):
                     comb += pair_cnt.eq(1)
                 with m.Default():
                     comb += pair_cnt.eq(0)
-            pairs.append(pair_cnt)
+            pairs.append((pair_cnt, 2))  # append pair, max_value
         return pairs
 
     def combine_pairs(self, m, iteration, pairs):
         comb = m.d.comb
         length = len(pairs)
-        assert length % 2 == 0  # TODO handle non powers of 2
         ret = []
         for i in range(0, length, 2):
-            left = pairs[i+1]
-            right = pairs[i]
-            width = left.width + 1
-            print(left)
-            print(f"pair({i}, {i+1}) - cnt_{iteration}_{i}")
-            new_pair = Signal(left.width + 1, name="cnt_%d_%d" %
-                              (iteration, i))
-            with m.If(left[-1] == 1):
-                with m.If(right[-1] == 1):
-                    comb += new_pair.eq(Cat(Repl(0, width-1), 1))
-                with m.Else():
-                    comb += new_pair.eq(Cat(right[0:-1], 0b01))
-            with m.Else():
-                comb += new_pair.eq(Cat(left, 0))
+            if i+1 >= length:
+                right, mv = pairs[i]
+                width = right.width
+                print(f"single({i}) - cnt_{iteration}_{i}")
+                new_pair = Signal(width, name="cnt_%d_%d" % (iteration, i))
+                comb += new_pair.eq(Cat(right, 0))
+                ret.append((new_pair, mv))
+            else:
+                left, lv = pairs[i+1]
+                right, rv = pairs[i]
+                width = right.width + 1
+                print(left)
+                print(f"pair({left}, {right}) - cnt_{iteration}_{i}")
+                new_pair = Signal(width, name="cnt_%d_%d" %
+                                  (iteration, i))
+                if rv == lv:
+                    with m.If(left[-1] == 1):
+                        with m.If(right[-1] == 1):
+                            comb += new_pair.eq(Cat(Repl(0, width-1), 1))
+                        with m.Else():
+                            comb += new_pair.eq(Cat(right[0:-1], 0b01))
+                    with m.Else():
+                        comb += new_pair.eq(Cat(left, 0))
+                else:
+                    with m.If(left == lv):
+                        comb += new_pair.eq(right + left)
+                    with m.Else():
+                        comb += new_pair.eq(left)
+                        
 
-            ret.append(new_pair)
+                ret.append((new_pair, lv+rv))
         return ret
 
     def elaborate(self, platform):
@@ -59,9 +73,10 @@ class CLZ(Elaboratable):
         i = 2
         while len(pairs) > 1:
             pairs = self.combine_pairs(m, i, pairs)
+            print(pairs)
             i += 1
 
-        comb += self.lz.eq(pairs[0])
+        comb += self.lz.eq(pairs[0][0])
 
         return m
 
diff --git a/src/ieee754/cordic/formal/proof_clz.py b/src/ieee754/cordic/formal/proof_clz.py
index 3eb4f5e4..273fc61f 100644
--- a/src/ieee754/cordic/formal/proof_clz.py
+++ b/src/ieee754/cordic/formal/proof_clz.py
@@ -17,7 +17,7 @@ class Driver(Elaboratable):
     def elaborate(self, platform):
         m = Module()
         comb = m.d.comb
-        width = 32
+        width = 10
 
         m.submodules.dut = dut = CLZ(width)
         sig_in = Signal.like(dut.sig_in)
diff --git a/src/ieee754/cordic/test/test_clz.py b/src/ieee754/cordic/test/test_clz.py
index 78d43e31..c3051007 100644
--- a/src/ieee754/cordic/test/test_clz.py
+++ b/src/ieee754/cordic/test/test_clz.py
@@ -37,6 +37,10 @@ class CLZTestCase(FHDLTestCase):
         inputs = [0, 15, 10, 127]
         self.run_test(iter(inputs), width=8)
 
+    def test_non_power_2(self):
+        inputs = [0, 128, 512]
+        self.run_test(iter(inputs), width=10)
+
 
 if __name__ == "__main__":
     unittest.main()
-- 
2.30.2