From c8100093874c734a2fc3c28f1bb92233bc909ee1 Mon Sep 17 00:00:00 2001
From: Florent Kermarrec <florent@enjoy-digital.fr>
Date: Tue, 4 Nov 2014 11:40:43 +0100
Subject: [PATCH] link: add Scrambler and testbench

---
 lib/sata/link/crc.py               |  8 ++--
 lib/sata/link/scrambler.py         | 67 ++++++++++++++++++++++++++++++
 lib/sata/link/test/Makefile        |  1 +
 lib/sata/link/test/scrambler.c     | 46 ++++++++++++++++++++
 lib/sata/link/test/scrambler_tb.py | 54 ++++++++++++++++++++++++
 5 files changed, 172 insertions(+), 4 deletions(-)
 create mode 100644 lib/sata/link/scrambler.py
 create mode 100644 lib/sata/link/test/scrambler_tb.py

diff --git a/lib/sata/link/crc.py b/lib/sata/link/crc.py
index 8f483005..59c650a1 100644
--- a/lib/sata/link/crc.py
+++ b/lib/sata/link/crc.py
@@ -54,12 +54,12 @@ class CRCEngine(Module):
 		# compute and optimize CRC's LFSR
 		curval = [[("new", i)] for i in range(width)]
 		for i in range(width):
-			feedback = curval.pop()
-			curval.insert(0, feedback)
-			for j in range(1, width-1):
-				if (polynom&(1<<j)):
+			feedback = curval.pop()		
+			for j in range(width-1):
+				if (polynom & (1<<(j+1))):
 					curval[j] += feedback
 				curval[j] = _optimize_eq(curval[j])
+			curval.insert(0, feedback)
 
 		# implement logic
 		for i in range(width):
diff --git a/lib/sata/link/scrambler.py b/lib/sata/link/scrambler.py
new file mode 100644
index 00000000..e2aaa54b
--- /dev/null
+++ b/lib/sata/link/scrambler.py
@@ -0,0 +1,67 @@
+from migen.fhdl.std import *
+from migen.genlib.misc import optree
+
+@DecorateModule(InsertReset)
+@DecorateModule(InsertCE)
+class SATAScrambler(Module):
+	"""SATA Scrambler
+
+	Implement a SATA Scrambler
+
+	Attributes
+	----------
+	value : out
+		Scrambled value.
+	"""
+	def __init__(self):
+		self.value = Signal(32)
+
+		###
+
+		context = Signal(16, reset=0xf0f6)
+		next_value = Signal(32)
+		self.sync += context.eq(next_value[16:32])
+
+		# from SATA specification, if possible replace it
+		# with a generic implementation using polynoms.
+		lfsr_coefs = (
+			(15, 13, 4, 0), #0
+			(15, 14, 13, 5, 4, 1, 0),
+			(14, 13, 6, 5, 4, 2,1, 0),
+			(15, 14, 7, 6, 5, 3,2, 1),
+			(13, 8, 7, 6, 3, 2, 0),
+			(14, 9, 8, 7, 4, 3, 1),
+			(15, 10, 9, 8, 5, 4, 2),
+			(15, 13, 11, 10, 9, 6, 5, 4, 3, 0),
+			(15, 14, 13, 12, 11, 10,7, 6, 5, 1, 0),
+			(14, 12, 11, 8, 7, 6, 4, 2, 1, 0),
+			(15, 13, 12, 9, 8, 7, 5, 3, 2, 1),
+			(15, 14, 10, 9, 8, 6, 3, 2, 0),
+			(13, 11, 10, 9, 7, 3, 1, 0),
+			(14, 12, 11, 10, 8, 4, 2, 1),
+			(15, 13, 12, 11, 9, 5, 3, 2),
+			(15, 14, 12, 10, 6, 3, 0),
+
+			(11, 7, 1, 0), #16
+			(12, 8, 2, 1),
+			(13, 9, 3, 2),
+			(14, 10, 4, 3),
+			(15, 11, 5, 4),
+			(15, 13, 12, 6, 5, 4, 0),
+			(15, 14, 7, 6, 5, 4, 1, 0),
+			(13, 8, 7, 6, 5, 4, 2, 1, 0),
+			(14, 9, 8,7, 6, 5, 3, 2, 1),
+			(15, 10, 9, 8, 7, 6, 4, 3, 2),
+			(15, 13, 11, 10, 9, 8, 7, 5, 3, 0),
+			(15, 14, 13, 12, 11, 10, 9, 8, 6, 1, 0),
+			(14, 12, 11, 10, 9, 7, 4, 2, 1, 0),
+			(15, 13, 12, 11, 10, 8, 5, 3, 2, 1),
+			(15, 14, 12, 11, 9, 6, 3, 2, 0),
+			(12, 10, 7, 3, 1, 0),
+		)
+
+		for n, coefs in enumerate(lfsr_coefs):
+			eq = [context[i] for i in coefs]
+			self.comb += next_value[n].eq(optree("^", eq))
+
+		self.comb += self.value.eq(next_value)
diff --git a/lib/sata/link/test/Makefile b/lib/sata/link/test/Makefile
index 92c9db9c..d21efd7b 100644
--- a/lib/sata/link/test/Makefile
+++ b/lib/sata/link/test/Makefile
@@ -14,6 +14,7 @@ crc_tb:
 scrambler_tb:
 	$(CC) $(CFLAGS) $(INC) -o scrambler scrambler.c
 	./scrambler /> scrambler_ref
+	$(CMD) scrambler_tb.py
 
 clean:
 	rm crc crc_ref scrambler scrambler_ref
diff --git a/lib/sata/link/test/scrambler.c b/lib/sata/link/test/scrambler.c
index f1711f15..cc96459b 100644
--- a/lib/sata/link/test/scrambler.c
+++ b/lib/sata/link/test/scrambler.c
@@ -1,4 +1,50 @@
 // Adapted from SATA specification
+/****************************************************************************/
+/*                                                                          */
+/* scramble.c                                                               */
+/*                                                                          */
+/* This sample code generates the entire sequence of 65535 Dwords produced  */
+/* by the scrambler defined in the Serial ATA specification. The            */
+/* specification calls for an LFSR to generate a string of bits that will   */
+/* be packaged into 32 bit Dwords to be XORed with the data Dwords. The     */
+/* generator polynomial specified is:                                       */
+/*         16  15  13  4                                                    */
+/* G(x) = x + x + x + x + 1                                                 */
+/*                                                                          */
+/* Parallelized versions of the scrambler are initialized to a value        */
+/* derived from the initialization value of 0xFFFF defined in the           */
+/* specification. This implementation is initialized to 0xF0F6. Other       */
+/* parallel implementations will have different initial values. The         */
+/* important point is that the first Dword output of any implementation     */
+/* must equal 0xC2D2768D.                                                   */
+/* This code does not represent an elegant solution for a C implementation, */
+/* but it does demonstrate a method of generating the sequence that can be  */
+/* easily implemented in hardware. A block diagram of the circuit emulated  */
+/* by this code is shown below.                                             */
+/*                                                                          */
+/* +-----------------------------------+                                    */
+/* |                                   |                                    */
+/* |                                   |                                    */
+/* |     +---+                +---+    |                                    */
+/* |     | R |                | * |    |                                    */
+/* +---->| e |----------+---->| M |----+----> Output(31 downto 16)          */
+/*       | g |          |     | 1 |                                         */
+/*       +---+          |     +---+                                         */
+/*                      |                                                   */
+/*                      |     +---+                                         */
+/*                      |     | * |                                         */
+/*                      +---->| M |---------> Output(15 downto 0)           */
+/*                            | 2 |                                         */
+/*                            +---+                                         */
+/*                                                                          */
+/* The register shown in the block diagram is a 16 bit register. The two    */
+/* boxes, *M1 and *M2, each represent a multiply by a 16 by 16 binary       */
+/* matrix. A 16 by 16 matrix times a 16 bit vector yields a 16 bit vector.  */
+/* The two vectors are the two halves of the 32 bit scrambler value. The    */
+/* upper half of the scrambler value is stored back into the context        */
+/* register to be used to generate the next value in the scrambler          */
+/*                                                                          */
+/****************************************************************************/
 #include <stdlib.h>
 #include <stdio.h>
 int main(int argc, char *argv[])
diff --git a/lib/sata/link/test/scrambler_tb.py b/lib/sata/link/test/scrambler_tb.py
new file mode 100644
index 00000000..301b1251
--- /dev/null
+++ b/lib/sata/link/test/scrambler_tb.py
@@ -0,0 +1,54 @@
+from subprocess import check_output
+
+from migen.fhdl.std import *
+
+from lib.sata.std import *
+from lib.sata.link.scrambler import *
+
+def check(ref, res):
+	shift = 0
+	while((ref[0] != res[0]) and (len(res)>1)):
+		res.pop(0)
+		shift += 1
+	length = min(len(ref), len(res))
+	errors = 0
+	for i in range(length):
+		if ref.pop(0) != res.pop(0):
+			errors += 1
+	return shift, length, errors
+
+class TB(Module):
+	def __init__(self):
+		self.submodules.scrambler = SATAScrambler()
+
+	def gen_simulation(self, selfp):
+		
+	# init CRC
+		selfp.scrambler.ce = 1
+		selfp.scrambler.reset = 1
+		yield
+		selfp.scrambler.reset = 0
+
+	# get C code results
+		ref = []
+		f = open("scrambler_ref", "r")
+		for l in f:
+			ref.append(int(l, 16))
+		f.close()
+
+	# log results
+		yield
+		res = []
+		for i in range(256):
+			res.append(selfp.scrambler.value)
+			yield
+		for e in res:
+			print("%08x" %e)
+
+	# check results
+		s, l, e = check(ref, res)
+		print("shift "+ str(s) + " / length " + str(l) + " / errors " + str(e))
+
+if __name__ == "__main__":
+	from migen.sim.generic import run_simulation
+	run_simulation(TB(), ncycles=1000, vcd_name="my.vcd", keep_files=True)
-- 
2.30.2