From 2b7779d3b62fcb80cc1a8143bae6123f30951605 Mon Sep 17 00:00:00 2001
From: Florent Kermarrec <florent@enjoy-digital.fr>
Date: Wed, 12 Nov 2014 18:20:34 +0100
Subject: [PATCH] link: wip bfm

---
 lib/sata/link/scrambler.py    |  7 ++---
 lib/sata/link/test/bfm.py     | 58 +++++++++++++++++++++++++++++++++--
 lib/sata/link/test/link_tb.py | 10 +++---
 3 files changed, 64 insertions(+), 11 deletions(-)

diff --git a/lib/sata/link/scrambler.py b/lib/sata/link/scrambler.py
index df599fec0..640e96f44 100644
--- a/lib/sata/link/scrambler.py
+++ b/lib/sata/link/scrambler.py
@@ -77,7 +77,7 @@ class SATAScrambler(Module):
 
 		self.submodules.scrambler = Scrambler()
 		ongoing = Signal()
-		self.sync += [
+		self.sync += \
 			If(sink.stb & sink.ack,
 				If(sink.eop,
 					ongoing.eq(0)
@@ -85,10 +85,9 @@ class SATAScrambler(Module):
 					ongoing.eq(1)
 				)
 			)
-		]
 		self.comb += [
-			self.scrambler.ce.eq(sink.stb & (sink.sop | ongoing)),
-			self.scrambler.reset.eq(~ongoing),
+			self.scrambler.ce.eq(sink.stb & sink.ack & (sink.sop | ongoing)),
+			self.scrambler.reset.eq(~(sink.sop | ongoing)),
 			Record.connect(sink, source),
 			source.d.eq(sink.d ^ self.scrambler.value)
 		]
diff --git a/lib/sata/link/test/bfm.py b/lib/sata/link/test/bfm.py
index 10c87e429..577d15cce 100644
--- a/lib/sata/link/test/bfm.py
+++ b/lib/sata/link/test/bfm.py
@@ -1,3 +1,5 @@
+import subprocess
+
 from migen.fhdl.std import *
 
 from lib.sata.std import *
@@ -32,7 +34,7 @@ class BFMSource(Module):
 				if v == self.dword.dat:
 					selfp.source.charisk = 0b0001
 			selfp.source.data = self.dword.dat
-		elif selfp.source.stb == 1 and selfp.source.ack == 1:
+		if selfp.source.stb == 1 and selfp.source.ack == 1:
 				self.dword.done = 1
 				selfp.source.stb = 0
 
@@ -81,8 +83,60 @@ class BFM(Module):
 		###
 
 		self.submodules.phy = BFMPHY(dw)
+		self.get_scrambler_ref()
+
+		self.rx_packet_ongoing = False
+		self.rx_packet = []
+
+	def get_scrambler_ref(self):
+		p = subprocess.Popen(["./scrambler"], stdout=subprocess.PIPE)
+		out, err = p.communicate()
+		self.scrambler_ref = [int(e, 16) for e in out.decode("utf-8").split("\n")[:-1]]
+
+	def descramble(self, packet):
+		p = []
+		for i in range(len(packet)):
+			v = packet[i] ^ self.scrambler_ref[i]
+			p.append(v)
+		return p
+
+	def check_crc(self, packet):
+		# Todo from C Code or Python Code
+		return packet[:-1]
+
+	def packet_callback(self, packet):
+		packet = self.descramble(packet)
+		packet = self.check_crc(packet)
+		for v in packet:
+			print("%08x" %v)
+
+	def dword_callback(self, dword):
+		print("%08x " %dword, end="")
+		for k, v in primitives.items():
+			if dword == v:
+				print(k, end="")
+		print("")
+
+		# X_RDY / WTRM response
+		if dword == primitives["X_RDY"]:
+			self.phy.bfm_source.dwords.append(BFMDword(primitives["R_RDY"]))
+		if dword == primitives["WTRM"]:
+			self.phy.bfm_source.dwords.append(BFMDword(primitives["R_OK"]))
+
+		# packet capture
+		if dword == primitives["EOF"]:
+			self.rx_packet_ongoing = False
+			self.packet_callback(self.rx_packet)
+
+		if self.rx_packet_ongoing:
+			self.rx_packet.append(dword)
+
+		if dword == primitives["SOF"]:
+			self.rx_packet_ongoing = True
+			self.rx_packet = []
 
 	def gen_simulation(self, selfp):
+		self.phy.bfm_source.dwords.append(BFMDword(primitives["SYNC"]))
 		while True:
 			yield from self.phy.receive()
-			print("%08x" %(self.phy.rx_dword))
+			self.dword_callback(self.phy.rx_dword)
diff --git a/lib/sata/link/test/link_tb.py b/lib/sata/link/test/link_tb.py
index 7753e5b0f..50dc4404b 100644
--- a/lib/sata/link/test/link_tb.py
+++ b/lib/sata/link/test/link_tb.py
@@ -76,15 +76,15 @@ class TB(Module):
 		self.submodules.streamer = LinkStreamer(32)
 		self.submodules.logger = LinkLogger(32)
 		self.comb += [
-			self.link_layer.sink.eq(self.streamer.source),
-			self.logger.sink.eq(self.link_layer.source)
+			Record.connect(self.streamer.source, self.link_layer.sink),
+			Record.connect(self.link_layer.source, self.logger.sink)
 		]
 
 	def gen_simulation(self, selfp):
 		for i in range(200):
 			yield
-		yield from self.bfm.phy.send(BFMDword(primitives["R_RDY"]), False)
-		yield from self.streamer.send(LinkPacket([0, 1, 2, 3]))
+		for i in range(8):
+			yield from self.streamer.send(LinkPacket([0, 1, 2, 3]))
 
 if __name__ == "__main__":
-	run_simulation(TB(), ncycles=5000, vcd_name="my.vcd", keep_files=True)
+	run_simulation(TB(), ncycles=512, vcd_name="my.vcd", keep_files=True)