link: manage CONT on TX path

lib/sata/link/__init__.py

@@ -4,10 +4,10 @@ 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
 
 # Todo:
-# - TX: insert COND and scramble between COND and primitives
-# - RX: manage COND
+# - RX: manage CONT
 
 class SATALinkLayer(Module):
 	def __init__(self, phy):
@@ -19,8 +19,8 @@ class SATALinkLayer(Module):
 
 	# TX
 		# insert CRC
-		crc_inserter = SATACRCInserter(link_layout(32))
-		self.submodules += crc_inserter
+		crc = SATACRCInserter(link_layout(32))
+		self.submodules += crc
 
 		# scramble
 		scrambler = SATAScrambler(link_layout(32))
@@ -28,25 +28,33 @@ class SATALinkLayer(Module):
 
 		# graph
 		self.comb += [
-			Record.connect(self.sink, crc_inserter.sink),
-			Record.connect(crc_inserter.source, scrambler.sink)
+			Record.connect(self.sink, crc.sink),
+			Record.connect(crc.source, scrambler.sink)
 		]
 
+		# inserter CONT and scrambled data between
+		# CONT and next primitive
+		cont  = SATACONTInserter(phy_layout(32))
+		self.submodules += cont
+
 		# datas / primitives mux
 		tx_insert = Signal(32)
 		self.comb += [
 			If(tx_insert != 0,
-				phy.sink.stb.eq(1),
-				phy.sink.data.eq(tx_insert),
-				phy.sink.charisk.eq(0x0001),
+				cont.sink.stb.eq(1),
+				cont.sink.data.eq(tx_insert),
+				cont.sink.charisk.eq(0x0001),
 			).Elif(fsm.ongoing("H2D_COPY"),
-				phy.sink.stb.eq(scrambler.source.stb),
-				phy.sink.data.eq(scrambler.source.d),
-				scrambler.source.ack.eq(phy.source.ack),
-				phy.sink.charisk.eq(0)
+				cont.sink.stb.eq(scrambler.source.stb),
+				cont.sink.data.eq(scrambler.source.d),
+				scrambler.source.ack.eq(cont.sink.ack),
+				cont.sink.charisk.eq(0)
 			)
 		]
 
+		# graph
+		self.comb += Record.connect(cont.source, phy.sink)
+
 	# RX
 		# datas / primitives detection
 		rx_det = Signal(32)

lib/sata/link/cont.py

@@ -0,0 +1,74 @@
+from migen.fhdl.std import *
+from migen.genlib.misc import optree
+
+from lib.sata.std import *
+from lib.sata.link.scrambler import Scrambler
+
+class SATACONTInserter(Module):
+	def __init__(self, layout):
+		self.sink = sink = Sink(layout)
+		self.source = source = Source(layout)
+
+		###
+
+		# Detect consecutive primitives
+		cnt = Signal(2)
+		is_primitive = Signal()
+		last_was_primitive = Signal()
+		last_primitive = Signal(32)
+		change = Signal()
+
+		cont_insert = Signal()
+		scrambler_insert = Signal()
+		last_primitive_insert = Signal()
+
+		self.comb += [
+			is_primitive.eq(sink.charisk != 0),
+			change.eq((sink.data != last_primitive) | ~is_primitive),
+			cont_insert.eq(~change & (cnt==1)),
+			scrambler_insert.eq(~change & (cnt==2)),
+			last_primitive_insert.eq(~is_primitive & last_was_primitive & (cnt==2))
+		]
+		self.sync += \
+			If(sink.stb & source.ack,
+				If(is_primitive,
+					last_primitive.eq(sink.data),
+					last_was_primitive.eq(1)
+				).Else(
+					last_was_primitive.eq(0)
+				),
+				If(change,
+					cnt.eq(0)
+				).Else(
+					If(~scrambler_insert,
+						cnt.eq(cnt+1)
+					)
+				)
+			)
+
+		# Repeated primitives scranbler
+		scrambler = Scrambler()
+		self.submodules += scrambler
+		self.comb += [
+			scrambler.reset.eq(ResetSignal()), #XXX: should be on COMINIT / COMRESET
+			scrambler.ce.eq(scrambler_insert & self.source.stb & self.source.ack)
+		]
+
+		# Datapath
+		self.comb += [
+			Record.connect(sink, source),
+			If(self.sink.stb,
+				If(cont_insert,
+					source.charisk.eq(0b0001),
+					source.data.eq(primitives["CONT"])
+				).Elif(scrambler_insert,
+					source.charisk.eq(0b0000),
+					source.data.eq(scrambler.value)
+				).Elif(last_primitive_insert,
+					source.stb.eq(1),
+					sink.ack.eq(0),
+					source.charisk.eq(0b0001),
+					source.data.eq(last_primitive)
+				)
+			)
+		]

lib/sata/link/test/bfm.py

@@ -69,16 +69,12 @@ class BFMPHY(Module):
 	def __repr__(self):
 		# receive
 		receiving = "%08x " %self.rx_dword
-		for k, v in primitives.items():
-			if self.rx_dword == v:
-				receiving += k
+		receiving += decode_primitive(self.rx_dword)
 		receiving += " "*(16-len(receiving))
 
 		# send
 		sending = "%08x " %self.bfm_source.dword.dat
-		for k, v in primitives.items():
-			if self.bfm_source.dword.dat == v:
-				sending += k
+		sending += decode_primitive(self.bfm_source.dword.dat)
 		sending += " "*(16-len(sending))
 
 		return receiving + sending
@@ -94,6 +90,7 @@ class BFM(Module):
 		self.submodules.phy = BFMPHY(dw)
 		self.get_scrambler_ref()
 
+		self.rx_cont_ongoing = False
 		self.rx_packet_ongoing = False
 		self.rx_packet = []
 
@@ -135,6 +132,11 @@ class BFM(Module):
 		print("----")
 
 	def dword_callback(self, dword):
+		if dword == primitives["CONT"]:
+			self.rx_cont_ongoing = True
+		elif is_primitive(dword):
+			self.rx_cont_ongoing = False
+
 		# X_RDY / WTRM response
 		if dword == primitives["X_RDY"]:
 			self.phy.send(primitives["R_RDY"])
@@ -158,8 +160,9 @@ class BFM(Module):
 					self.phy.send(primitives["HOLD"])
 				else:
 					self.phy.send(primitives["R_RDY"])
-				if dword != primitives["HOLDA"]:
-					self.rx_packet.append(dword)
+				if not is_primitive(dword):
+					if not self.rx_cont_ongoing:
+						self.rx_packet.append(dword)
 
 		elif dword == primitives["SOF"]:
 			self.rx_packet_ongoing = True

lib/sata/std.py

@@ -4,6 +4,7 @@ from migen.flow.actor import EndpointDescription, Sink, Source
 
 primitives = {
 	"ALIGN"	:	0x7B4A4ABC,
+	"CONT"	: 	0X9999AA7C,
 	"SYNC"	:	0xB5B5957C,
 	"R_RDY"	:	0x4A4A957C,
 	"R_OK"	:	0x3535B57C,
@@ -18,6 +19,18 @@ primitives = {
 	"HOLDA"	: 	0X9595AA7C
 }
 
+def is_primitive(dword):
+	for k, v in primitives.items():
+		if dword == v:
+			return True
+	return False
+
+def decode_primitive(dword):
+	for k, v in primitives.items():
+		if dword == v:
+			return k
+	return ""
+
 def ones(width):
 	return 2**width-1