From 3d84a7a9de47201a1dbe06860a57e54800a70fde Mon Sep 17 00:00:00 2001
From: Robert Jordens <jordens@gmail.com>
Date: Sun, 7 Sep 2014 00:18:04 -0600
Subject: [PATCH] cordic: round() constants if not power of two bitwidth,
 cleanup, simplify some logic

---
 examples/sim/cordic_err.py | 23 ++++++++++++----------
 migen/genlib/cordic.py     | 40 +++++++++++++++++---------------------
 migen/test/test_cordic.py  |  2 +-
 3 files changed, 32 insertions(+), 33 deletions(-)

diff --git a/examples/sim/cordic_err.py b/examples/sim/cordic_err.py
index bd22f250..d7b89b1c 100644
--- a/examples/sim/cordic_err.py
+++ b/examples/sim/cordic_err.py
@@ -15,30 +15,30 @@ class TestBench(Module):
 			n = 1<<flen(self.cordic.xi)
 		self.c = c = 2**(flen(self.cordic.xi) - 1)
 		self.cz = cz = 2**(flen(self.cordic.zi) - 1)
+		x = int(xmax*c/self.cordic.gain)
 		if i is None:
-			i = [(int(xmax*c/self.cordic.gain), 0, int(cz*(i/n - .5)))
-					for i in range(n)]
+			i = [(x, 0, int(cz*(2.*ii/n - 1))) for ii in range(n)]
 		self.i = i
 		random.shuffle(self.i)
 		self.ii = iter(self.i)
 		self.o = []
 
 	def do_simulation(self, selfp):
+		if selfp.cordic.new_out:
+			self.o.append((selfp.cordic.xo, selfp.cordic.yo, selfp.cordic.zo))
 		if selfp.cordic.new_in:
 			try:
 				selfp.cordic.xi, selfp.cordic.yi, selfp.cordic.zi = next(self.ii)
 			except StopIteration:
 				raise StopSimulation
-		if selfp.cordic.new_out:
-			self.o.append((selfp.cordic.xo, selfp.cordic.yo, selfp.cordic.zo))
 
 	def run_io(self):
 		run_simulation(self)
-		del self.i[-1], self.o[0]
+		del self.o[0]
 		if self.i[0] != (0, 0, 0):
 			assert self.o[0] != (0, 0, 0)
-		if self.i[-1] != self.i[-2]:
-			assert self.o[-1] != self.o[-2], self.o[-2:]
+		#if self.i[-1] != self.i[-2]:
+		#	assert self.o[-1] != self.o[-2], self.o[-2:]
 
 def rms_err(width, guard=None, stages=None, n=None):
 	tb = TestBench(width=width, guard=guard, stages=stages,
@@ -87,9 +87,10 @@ def plot_function(**kwargs):
 	xi, yi, zi = np.array(tb.i).T
 	xo, yo, zo = np.array(tb.o).T
 	fig, ax = plt.subplots()
+	#ax.plot(zi, xo-np.around(xi[0]*g*np.cos(zi/cz*np.pi)), "k-")
 	ax.plot(zi, xo, "r,")
 	ax.plot(zi, yo, "g,")
-	ax.plot(zi, zo, "g,")
+	ax.plot(zi, zo, "b,")
 
 
 if __name__ == "__main__":
@@ -101,6 +102,8 @@ if __name__ == "__main__":
 	#plot_function(func_mode="hyperbolic", xmax=.3, width=16, n=333)
 	#plot_function(func_mode="circular", width=16, n=333)
 	#plot_function(func_mode="hyperbolic", cordic_mode="vector",
-    #        xmax=.3, width=16, n=333)
-	#plot_function(func_mode="circular", width=16, n=333)
+	#        xmax=.3, width=16, n=333)
+	plot_function(func_mode="circular",
+			width=16, stages=15, guard=0,
+			n=1000, xmax=.98)
 	plt.show()
diff --git a/migen/genlib/cordic.py b/migen/genlib/cordic.py
index 0b930665..c6d134dd 100644
--- a/migen/genlib/cordic.py
+++ b/migen/genlib/cordic.py
@@ -1,4 +1,4 @@
-from math import atan, atanh, log, sqrt, pi, ceil
+from math import atan, atanh, log, sqrt, pi
 
 from migen.fhdl.std import *
 
@@ -276,9 +276,12 @@ class TwoQuadrantCordic(Module):
 			a = [atanh(2**-i) for i in s]
 			g = [sqrt(1 - 2**(-2*i)) for i in s]
 			zmax = sum(a)*2
-		a = [int(ai*2**(bits - 1)/zmax) for ai in a]
 		# round here helps the width=2**i - 1 case but hurts the
 		# important width=2**i case
+		cast = int
+		if log(bits)/log(2) % 1:
+			cast = round
+		a = [cast(ai*2**(bits - 1)/zmax) for ai in a]
 		gain = 1.
 		for gi in g:
 			gain *= gi
@@ -316,30 +319,23 @@ class Cordic(TwoQuadrantCordic):
 		if self.func_mode != "circular":
 			return # no need to remap quadrants
 
-		width = flen(self.xi)
-		widthz = flen(self.zi)
 		cxi, cyi, czi = self.xi, self.yi, self.zi
-		self.xi = Signal((width, True))
-		self.yi = Signal((width, True))
-		self.zi = Signal((widthz, True))
+		self.xi = xi = Signal.like(cxi)
+		self.yi = yi = Signal.like(cyi)
+		self.zi = zi = Signal.like(czi)
 
 		###
 
-		pi2 = 1<<(widthz - 2)
+		q = Signal()
 		if self.cordic_mode == "rotate":
-			#rot = self.zi + pi2 < 0
-			rot = self.zi[-1] ^ self.zi[-2]
+			self.comb += q.eq(zi[-2] ^ zi[-1])
 		else: # vector
-			rot = self.xi < 0
-			#rot = self.xi[-1]
+			self.comb += q.eq(xi < 0)
 		self.comb += [
-				cxi.eq(self.xi),
-				cyi.eq(self.yi),
-				czi.eq(self.zi),
-				If(rot,
-					cxi.eq(-self.xi),
-					cyi.eq(-self.yi),
-					czi.eq(self.zi + 2*pi2),
-					#czi.eq(self.zi ^ (2*pi2)),
-				),
-				]
+				If(q,
+					Cat(cxi, cyi, czi).eq(Cat(-xi, -yi,
+						zi + (1 << flen(zi) - 1)))
+				).Else(
+					Cat(cxi, cyi, czi).eq(Cat(xi, yi, zi))
+				)
+		]
diff --git a/migen/test/test_cordic.py b/migen/test/test_cordic.py
index 7a715aae..082ad133 100644
--- a/migen/test/test_cordic.py
+++ b/migen/test/test_cordic.py
@@ -121,7 +121,7 @@ class CordicCase(SimCase, unittest.TestCase):
 			return xi, yi, 0
 		def proc(xi, yi, zi):
 			return sqrt(xi**2 - yi**2), 0, atanh(yi/xi)
-		self._run_io(50, gen, proc)
+		self._run_io(50, gen, proc, deltaz=2)
 
 	def test_vec_hyp(self):
 		self.setUp(cordic_mode="vector", func_mode="hyperbolic")
-- 
2.30.2