diff --git a/lib/sata/command/__init__.py b/lib/sata/command/__init__.py
index 3adc889d0..88dd700e6 100644
--- a/lib/sata/command/__init__.py
+++ b/lib/sata/command/__init__.py
@@ -3,7 +3,13 @@ from migen.genlib.fsm import FSM, NextState
 
 from lib.sata.common import *
 
-from_rx = [
+tx_to_rx = [
+	("write", 1),
+	("read", 1),
+	("identify", 1)
+]
+
+rx_to_tx = [
 	("dma_activate", 1),
 	("data", 1),
 	("reg_d2h", 1)
@@ -12,7 +18,8 @@ from_rx = [
 class SATACommandTX(Module):
 	def __init__(self, transport):
 		self.sink = sink = Sink(command_tx_description(32))
-		self.from_rx = Sink(from_rx)
+		self.to_rx = to_rx = Source(tx_to_rx)
+		self.from_rx = from_rx = Sink(rx_to_tx)
 
 		###
 
@@ -55,8 +62,9 @@ class SATACommandTX(Module):
 				NextState("WAIT_DMA_ACTIVATE")
 			)
 		)
+		# XXX: split when length > 2048 dwords
 		fsm.act("WAIT_DMA_ACTIVATE",
-			If(self.from_rx.dma_activate,
+			If(from_rx.dma_activate,
 				NextState("SEND_DATA")
 			)
 		)
@@ -68,7 +76,7 @@ class SATACommandTX(Module):
 			transport.sink.data.eq(sink.data),
 			sink.ack.eq(transport.sink.ack),
 			If(sink.stb & sink.ack & sink.eop,
-				NextState("WAIT_REG_D2H")
+				NextState("IDLE")
 			)
 		)
 		fsm.act("SEND_READ_DMA_CMD",
@@ -80,7 +88,7 @@ class SATACommandTX(Module):
 			transport.sink.command.eq(regs["READ_DMA_EXT"]),
 			sink.ack.eq(transport.sink.ack),
 			If(sink.stb & sink.ack,
-				NextState("WAIT_DATA")
+				NextState("IDLE")
 			)
 		)
 		fsm.act("SEND_IDENTIFY_CMD",
@@ -92,25 +100,23 @@ class SATACommandTX(Module):
 			transport.sink.command.eq(regs["IDENTIFY_DEVICE_DMA"]),
 			sink.ack.eq(transport.sink.ack),
 			If(sink.stb & sink.ack,
-				NextState("WAIT_DATA")
-			)
-		)
-		fsm.act("WAIT_DATA",
-			If(self.from_rx.data,
-				NextState("WAIT_REG_D2H")
-			)
-		)
-		fsm.act("WAIT_REG_D2H",
-			NextState("IDLE"),
-			If(self.from_rx.reg_d2h,
 				NextState("IDLE")
 			)
 		)
 
+		self.comb += [
+			If(sink.stb,
+				to_rx.write.eq(sink.write),
+				to_rx.read.eq(sink.read),
+				to_rx.identify.eq(sink.identify),
+			)
+		]
+
 class SATACommandRX(Module):
 	def __init__(self, transport):
 		self.source = source = Source(command_rx_description(32))
-		self.to_tx = Source(from_rx)
+		self.to_tx = to_tx = Source(rx_to_tx)
+		self.from_tx = from_tx = Sink(tx_to_rx)
 
 		###
 
@@ -118,40 +124,101 @@ class SATACommandRX(Module):
 			return transport.source.type == fis_types[name]
 
 		dma_activate = Signal()
-		data = Signal()
-		reg_d2h = Signal()
 
-		self.comb += \
+		fsm = FSM(reset_state="IDLE")
+		self.submodules += fsm
+
+		fsm.act("IDLE",
+			transport.source.ack.eq(1),
+			If(from_tx.write,
+				NextState("WAIT_WRITE_ACTIVATE")
+			).Elif(from_tx.read | from_tx.identify,
+				NextState("WAIT_READ_DATA")
+			)
+		)
+		identify = Signal()
+		self.sync += \
+			If(fsm.ongoing("IDLE"),
+				identify.eq(from_tx.identify)
+			)
+		fsm.act("WAIT_WRITE_ACTIVATE",
+			transport.source.ack.eq(1),
 			If(transport.source.stb,
-				If(test_type("REG_D2H"),
-					# XXX add checks
-					reg_d2h.eq(1),
-					transport.source.ack.eq(1)
-				).Elif(test_type("DMA_ACTIVATE_D2H"),
-					# XXX add checks
+				If(test_type("DMA_ACTIVATE_D2H"),
 					dma_activate.eq(1),
-					transport.source.ack.eq(1)
-				).Elif(test_type("DATA"),
-					source.stb.eq(1),
-					source.sop.eq(transport.source.sop),
-					source.eop.eq(transport.source.eop),
-					source.data.eq(transport.source.data),
-					data.eq(source.eop & source.ack),
-					transport.source.ack.eq(source.ack)
-				).Else(
-					transport.source.ack.eq(1)
+					NextState("WAIT_WRITE_REG_D2H")
 				)
 			)
+		)
+		fsm.act("WAIT_WRITE_REG_D2H",
+			transport.source.ack.eq(1),
+			If(transport.source.stb,
+				If(test_type("REG_D2H"),
+					NextState("PRESENT_WRITE_RESPONSE")
+				)
+			)
+		)
+		fsm.act("PRESENT_WRITE_RESPONSE",
+			source.stb.eq(1),
+			source.sop.eq(1),
+			source.eop.eq(1),
+			source.write.eq(1),
+			source.success.eq(1),
+			If(source.ack,
+				NextState("IDLE")
+			)
+		)
+		fsm.act("WAIT_READ_DATA",
+			transport.source.ack.eq(1),
+			If(transport.source.stb,
+				transport.source.ack.eq(0),
+				If(test_type("DATA"),
+					NextState("PRESENT_READ_DATA")
+				)
+			)
+		)
+		fsm.act("PRESENT_READ_DATA",
+			source.stb.eq(transport.source.stb),
+			source.read.eq(~identify),
+			source.identify.eq(identify),
+			source.sop.eq(transport.source.sop),
+			source.eop.eq(transport.source.eop),
+			source.data.eq(transport.source.data),
+			transport.source.ack.eq(source.ack),
+			If(source.stb & source.eop & source.ack,
+				NextState("WAIT_READ_REG_D2H")
+			)
+		)
+		fsm.act("WAIT_READ_REG_D2H",
+			transport.source.ack.eq(1),
+			If(transport.source.stb,
+				If(test_type("REG_D2H"),
+					NextState("PRESENT_READ_RESPONSE")
+				)
+			)
+		)
+		fsm.act("PRESENT_READ_RESPONSE",
+			source.stb.eq(1),
+			source.sop.eq(1),
+			source.eop.eq(1),
+			source.read.eq(~identify),
+			source.identify.eq(identify),
+			source.success.eq(1),
+			If(source.ack,
+				NextState("IDLE")
+			)
+		)
 
 		self.comb += [
-			self.to_tx.dma_activate.eq(dma_activate),
-			self.to_tx.data.eq(data),
-			self.to_tx.reg_d2h.eq(reg_d2h)
+			to_tx.dma_activate.eq(dma_activate),
 		]
 
 class SATACommand(Module):
 	def __init__(self, transport):
 		self.submodules.tx = SATACommandTX(transport)
 		self.submodules.rx = SATACommandRX(transport)
-		self.comb += self.rx.to_tx.connect(self.tx.from_rx)
+		self.comb += [
+			self.rx.to_tx.connect(self.tx.from_rx),
+			self.tx.to_rx.connect(self.rx.from_tx)
+		]
 		self.sink, self.source = self.tx.sink, self.rx.source
diff --git a/lib/sata/test/bfm.py b/lib/sata/test/bfm.py
index 97fe1174c..1c1ac49a1 100644
--- a/lib/sata/test/bfm.py
+++ b/lib/sata/test/bfm.py
@@ -223,6 +223,7 @@ class LinkLayer(Module):
 				self.tx_packet.done = True
 			elif dword == primitives["R_ERR"]:
 				self.tx_packet.done = True
+			self.phy.send(primitives["SYNC"])
 
 	def insert_cont(self):
 		self.tx_lasts.pop(0)
@@ -388,15 +389,20 @@ class CommandLayer(Module):
 
 	def callback(self, fis):
 		# XXX manage maximum of 2048 DWORDS per DMA
+		resp = None
 		if isinstance(fis, FIS_REG_H2D):
 			if fis.command == regs["WRITE_DMA_EXT"]:
-				self.transport.send(self.hdd.write_dma_cmd(fis))
+				resp =  self.hdd.write_dma_cmd(fis)
 			elif fis.command == regs["READ_DMA_EXT"]:
-				self.transport.send(self.hdd.read_dma_cmd(fis))
+				resp = self.hdd.read_dma_cmd(fis)
 			elif fis.command == regs["IDENTIFY_DEVICE_DMA"]:
-				self.transport.send(self.hdd.identify_device_dma_cmd(fis))
+				resp = self.hdd.identify_device_dma_cmd(fis)
 		elif isinstance(fis, FIS_DATA):
-			self.hdd.data_cmd(fis)
+			resp = self.hdd.data_cmd(fis)
+
+		if resp is not None:
+			for packet in resp:
+				self.transport.send(packet)
 
 # HDD model
 class HDDMemRegion:
@@ -412,23 +418,32 @@ class HDD(Module):
 
 		self.mem = None
 		self.wr_address = 0
+		self.wr_length = 0
+		self.wr_cnt = 0
 
 	def write_dma_cmd(self, fis):
 		self.wr_address = fis.lba_lsb
-		return FIS_DMA_ACTIVATE_D2H()
+		self.wr_length = fis.count
+		self.wr_cnt = 0
+		return [FIS_DMA_ACTIVATE_D2H()]
 
 	def read_dma_cmd(self, fis):
 		packet = self.read_mem(fis.lba_lsb, fis.count*4)
 		packet.insert(0, 0)
-		return FIS_DATA(packet)
+		return [FIS_DATA(packet), FIS_REG_D2H()]
 
 	def identify_dma_cmd(self, fis):
 		packet = [i for i in range(256)]
 		packet.insert(0, 0)
-		return FIS_DATA(packet)
+		return [FIS_DATA(packet), FIS_REG_D2H()]
 
 	def data_cmd(self, fis):
 		self.write_mem(self.wr_address, fis.packet[1:])
+		self.wr_cnt += len(fis.packet[1:])
+		if self.wr_length == self.wr_cnt:
+			return [FIS_REG_D2H()]
+		else:
+			return None
 
 	def allocate_mem(self, base, length):
 		# XXX add support for multiple memory regions
diff --git a/lib/sata/test/command_tb.py b/lib/sata/test/command_tb.py
index 00a7aafc9..28c2e5a87 100644
--- a/lib/sata/test/command_tb.py
+++ b/lib/sata/test/command_tb.py
@@ -98,7 +98,7 @@ class CommandLogger(Module):
 			self.packet.append(selfp.sink.data)
 		elif selfp.sink.stb:
 			self.packet.append(selfp.sink.data)
-		if (selfp.sink.stb ==1 and selfp.sink.eop ==1):
+		if (selfp.sink.stb == 1 and selfp.sink.eop == 1):
 			self.packet.done = True
 
 class TB(Module):
@@ -126,6 +126,9 @@ class TB(Module):
 			yield
 		streamer_packet = CommandTXPacket(write=1, address=1024, length=32, data=[i for i in range(32)])
 		yield from self.streamer.send(streamer_packet)
+		yield from self.logger.receive()
+		for d in self.logger.packet:
+			print("%08x" %d)
 		for i in range(32):
 			yield
 		streamer_packet = CommandTXPacket(read=1, address=1024, length=32)
@@ -133,6 +136,9 @@ class TB(Module):
 		yield from self.logger.receive()
 		for d in self.logger.packet:
 			print("%08x" %d)
+		yield from self.logger.receive()
+		for d in self.logger.packet:
+			print("%08x" %d)
 
 if __name__ == "__main__":
 	run_simulation(TB(), ncycles=512, vcd_name="my.vcd", keep_files=True)