From f74471d027cf29fdbe3fc588c42b30e7ccb42a55 Mon Sep 17 00:00:00 2001
From: Florent Kermarrec <florent@enjoy-digital.fr>
Date: Wed, 24 Sep 2014 11:37:28 +0200
Subject: [PATCH] add device ctrl skeleton (we will use it for simulation with
 the host)

---
 lib/sata/k7sataphy/ctrl.py | 105 ++++++++++++++++++++++++++++++++++++-
 1 file changed, 104 insertions(+), 1 deletion(-)

diff --git a/lib/sata/k7sataphy/ctrl.py b/lib/sata/k7sataphy/ctrl.py
index 19260df6..e960981f 100644
--- a/lib/sata/k7sataphy/ctrl.py
+++ b/lib/sata/k7sataphy/ctrl.py
@@ -126,11 +126,12 @@ class K7SATAPHYHostCtrl(Module):
 		]
 
 		txcominit_d = Signal()
-		txcomwake_d = Signa()
+		txcomwake_d = Signal()
 		self.sync += [
 			gtx.txcominit.eq(txcominit & ~txcominit_d),
 			gtx.txcomwake.eq(txcomwake & ~txcomwake),
 		]
+		self.comb +=  align_detect.eq(gtx.rxdata == ALIGN_VAL);
 
 		align_timeout_cnt = Signal(16)
 		self.sync += \
@@ -169,6 +170,108 @@ class K7SATAPHYHostCtrl(Module):
 				).Else(
 					non_align_cnt.eq(0)
 				)
+			)
 
 class K7SATAPHYDeviceCtrl(Module):
 	def __init__(self, gtx):
+		self.link_up = Signal()
+		self.speed = Signal(3)
+
+		align_timeout = Signal()
+		align_detect = Signal()
+
+		txcominit = Signal()
+		txcomwake = Signa()
+		txdata = Signal(32)
+		txcharisk = Signal(4)
+
+		fsm = FSM(reset_state="IDLE")
+		self.submodules += fsm
+
+		fsm.act("RESET",
+			gtx.txelecidle.eq(1),
+			If(gtx.rxcominitdet,
+				NextState("AWAIT_COMINIT")
+			)
+		)
+		fsm.act("COMINIT",
+			gtx.txelecidle.eq(1),
+			If(gtx.txcomfinish,
+				NextState("AWAIT_COMWAKE")
+			)
+		)
+		fsm.act("AWAIT_COMWAKE",
+			gtx.txelecidle.eq(1),
+			If(gtx.rxcomwake,
+				NextState("AWAIT_NO_COMWAKE")
+			).Else(
+				If(retry_cnt == 0,
+					NextState("RESET")
+				)
+			)
+		)
+		fsm.act("AWAIT_NO_COMWAKE",
+			gtx.txelecidle.eq(1),
+			If(~gtx.rxcomwake,
+				NextState("CALIBRATE")
+			)
+		)
+		fsm.act("CALIBRATE",
+			gtx.txelecidle.eq(1),
+			NextState("COMWAKE")
+		)
+		fsm.act("COMWAKE",
+			gtx.txelecidle.eq(1),
+			If(gtx.txcomfinish,
+				NextState("SEND_ALIGN")
+			).Elif(align_timeout,
+				NextState("ERROR")
+			)
+		)
+		fsm.act("SEND_ALIGN",
+			gtx.txelecidle.eq(0),
+			txdata.eq(ALIGN_VAL),
+			txcharisk.eq(0b0001),
+			If(align_detect,
+				NextState("READY")
+			).Elsif(align_timeout,
+				NextState("ERROR")
+			)
+		)
+		fsm.act("READY",
+			txdata.eq(SYNC_VAL),
+			txcharisk.eq(0b0001),
+			gtx.txelecidle.eq(0),
+			NextState("READY"),
+			If(gtx.rxelecidle,
+				NextState("RESET")
+			),
+			self.link_up.eq(1)
+		)
+		fsm.act("ERROR",
+			gtx.txelecidle.eq(1),
+			NextState("RESET")
+		)
+
+		txcominit_d = Signal()
+		txcomwake_d = Signal()
+		self.sync += [
+			gtx.txcominit.eq(txcominit & ~txcominit_d),
+			gtx.txcomwake.eq(txcomwake & ~txcomwake),
+		]
+		self.comb +=  align_detect.eq(gtx.rxdata == ALIGN_VAL);
+
+		align_timeout_cnt = Signal(16)
+		self.sync += \
+			If(fsm.ongoing("RESET"),
+				If(speed == 0b100,
+					align_timeout_cnt.eq(us(55, "SATA3"))
+				).Elif(speed == 0b010,
+					align_timeout_cnt.eq(us(55, "SATA2"))
+				).Else(
+					align_timeout_cnt.eq(us(55, "SATA1"))
+				)
+			).Elif(fsm.ongoing("AWAIT_ALIGN"),
+				align_timeout_cnt.eq(align_timeout_cnt-1)
+			)
+		self.comb += align_timeout.eq(align_timeout_cnt == 0)
\ No newline at end of file
-- 
2.30.2