link: manage CONT on RX path

lib/sata/link/__init__.py

@@ -4,10 +4,11 @@ from migen.genlib.fsm import FSM, NextState
 from lib.sata.std import *
 from lib.sata.link.crc import SATACRCInserter, SATACRCChecker
 from lib.sata.link.scrambler import SATAScrambler
-from lib.sata.link.cont import SATACONTInserter
+from lib.sata.link.cont import SATACONTInserter, SATACONTRemover
 
-# Todo:
+# TODO:
-# - RX: manage CONT
+# - Test D2H
+# - Do more tests
 
 class SATALinkLayer(Module):
 	def __init__(self, phy):
@@ -19,67 +20,75 @@ class SATALinkLayer(Module):
 
 	# TX
 		# insert CRC
-		crc = SATACRCInserter(link_layout(32))
+		tx_crc = SATACRCInserter(link_layout(32))
-		self.submodules += crc
+		self.submodules += tx_crc
 
 		# scramble
-		scrambler = SATAScrambler(link_layout(32))
+		tx_scrambler = SATAScrambler(link_layout(32))
-		self.submodules += scrambler
+		self.submodules += tx_scrambler
 
 		# graph
 		self.comb += [
-			Record.connect(self.sink, crc.sink),
+			Record.connect(self.sink, tx_crc.sink),
-			Record.connect(crc.source, scrambler.sink)
+			Record.connect(tx_crc.source, tx_scrambler.sink)
 		]
 
 		# inserter CONT and scrambled data between
 		# CONT and next primitive
-		cont  = SATACONTInserter(phy_layout(32))
+		tx_cont  = SATACONTInserter(phy_layout(32))
-		self.submodules += cont
+		self.submodules += tx_cont
 
 		# datas / primitives mux
 		tx_insert = Signal(32)
 		self.comb += [
 			If(tx_insert != 0,
-				cont.sink.stb.eq(1),
+				tx_cont.sink.stb.eq(1),
-				cont.sink.data.eq(tx_insert),
+				tx_cont.sink.data.eq(tx_insert),
-				cont.sink.charisk.eq(0x0001),
+				tx_cont.sink.charisk.eq(0x0001),
 			).Elif(fsm.ongoing("H2D_COPY"),
-				cont.sink.stb.eq(scrambler.source.stb),
+				tx_cont.sink.stb.eq(tx_scrambler.source.stb),
-				cont.sink.data.eq(scrambler.source.d),
+				tx_cont.sink.data.eq(tx_scrambler.source.d),
-				scrambler.source.ack.eq(cont.sink.ack),
+				tx_scrambler.source.ack.eq(tx_cont.sink.ack),
-				cont.sink.charisk.eq(0)
+				tx_cont.sink.charisk.eq(0)
 			)
 		]
 
 		# graph
-		self.comb += Record.connect(cont.source, phy.sink)
+		self.comb += Record.connect(tx_cont.source, phy.sink)
 
 	# RX
+
+		# CONT remover
+		rx_cont = SATACONTRemover(phy_layout(32))
+		self.submodules += rx_cont
+
+		# graph
+		self.comb += Record.connect(phy.source, rx_cont.sink)
+
 		# datas / primitives detection
 		rx_det = Signal(32)
 		self.comb += \
-			If(phy.source.stb & (phy.source.charisk == 0b0001),
+			If(rx_cont.source.stb & (rx_cont.source.charisk == 0b0001),
-				rx_det.eq(phy.source.data)
+				rx_det.eq(rx_cont.source.data)
 			)
 
 		# descrambler
-		descrambler = SATAScrambler(link_layout(32))
+		rx_scrambler = SATAScrambler(link_layout(32))
-		self.submodules += descrambler
+		self.submodules += rx_scrambler
 
 		# check CRC
-		crc_checker = SATACRCChecker(link_layout(32))
+		rx_crc = SATACRCChecker(link_layout(32))
-		self.submodules += crc_checker
+		self.submodules += rx_crc
 
 		# graph
 		self.comb += [
 			If(fsm.ongoing("D2H_COPY") & (rx_det == 0),
-				descrambler.sink.stb.eq(phy.source.stb & (phy.source.charisk == 0)),
+				rx_scrambler.sink.stb.eq(rx_cont.source.stb & (rx_cont.source.charisk == 0)),
-				descrambler.sink.d.eq(phy.source.data),
+				rx_scrambler.sink.d.eq(rx_cont.source.data),
 			),
-			phy.source.ack.eq(1),
+			rx_cont.source.ack.eq(1),
-			Record.connect(descrambler.source, crc_checker.sink),
+			Record.connect(rx_scrambler.source, rx_crc.sink),
-			Record.connect(crc_checker.source, self.source)
+			Record.connect(rx_crc.source, self.source)
 		]
 
 	# FSM
@@ -87,7 +96,7 @@ class SATALinkLayer(Module):
 			tx_insert.eq(primitives["SYNC"]),
 			If(rx_det == primitives["X_RDY"],
 				NextState("D2H_RDY")
-			).Elif(scrambler.source.stb & scrambler.source.sop,
+			).Elif(tx_scrambler.source.stb & tx_scrambler.source.sop,
 				NextState("H2D_RDY")
 			)
 		)
@@ -108,9 +117,9 @@ class SATALinkLayer(Module):
 		fsm.act("H2D_COPY",
 			If(rx_det == primitives["HOLD"],
 				tx_insert.eq(primitives["HOLDA"]),
-			).Elif(~scrambler.source.stb,
+			).Elif(~tx_scrambler.source.stb,
 				tx_insert.eq(primitives["HOLD"]),
-			).Elif(scrambler.source.stb & scrambler.source.eop & scrambler.source.ack,
+			).Elif(tx_scrambler.source.stb & tx_scrambler.source.eop & tx_scrambler.source.ack,
 				NextState("H2D_EOF")
 			)
 		)

lib/sata/link/cont.py

@@ -12,6 +12,7 @@ class SATACONTInserter(Module):
 		###
 
 		# Detect consecutive primitives
+		# tn insert CONT
 		cnt = Signal(2)
 		is_primitive = Signal()
 		last_was_primitive = Signal()
@@ -46,7 +47,7 @@ class SATACONTInserter(Module):
 				)
 			)
 
-		# Repeated primitives scranbler
+		# scranbler (between CONT and next primitive)
 		scrambler = Scrambler()
 		self.submodules += scrambler
 		self.comb += [
@@ -72,3 +73,43 @@ class SATACONTInserter(Module):
 				)
 			)
 		]
+
+class SATACONTRemover(Module):
+	def __init__(self, layout):
+		self.sink = sink = Sink(layout)
+		self.source = source = Source(layout)
+
+		###
+
+		# Detect CONT
+		is_primitive = Signal()
+		is_cont = Signal()
+		in_cont = Signal()
+		cont_ongoing = Signal()
+
+		self.comb += [
+			is_primitive.eq(sink.charisk != 0),
+			is_cont.eq(is_primitive & sink.data == primitives["CONT"])
+		]
+		self.sync += \
+			If(is_cont,
+				in_cont.eq(1)
+			).Elif(is_primitive,
+				in_cont.eq(0)
+			)
+		self.comb += cont_ongoing.eq(is_cont | (in_cont & ~is_primitive))
+
+		# Datapath
+		last_primitive = Signal()
+		self.sync += [
+			If(is_primitive & ~is_cont,
+				last_primitive.eq(sink.data)
+			)
+		]
+		self.comb += [
+			Record.connect(sink, source),
+			If(cont_ongoing,
+				self.source.charisk.eq(0b0001),
+				self.source.data.eq(last_primitive)
+			)
+		]

lib/sata/link/test/bfm.py

@@ -132,6 +132,7 @@ class BFM(Module):
 		print("----")
 
 	def dword_callback(self, dword):
+		# CONT detection
 		if dword == primitives["CONT"]:
 			self.rx_cont_ongoing = True
 		elif is_primitive(dword):
@@ -148,7 +149,7 @@ class BFM(Module):
 		elif dword == primitives["HOLD"]:
 			self.phy.send(primitives["HOLDA"])
 
-		# packet capture
+		# Packet capture
 		elif dword == primitives["EOF"]:
 			self.rx_packet_ongoing = False
 			self.packet_callback(self.rx_packet)