diff --git a/lib/sata/command/__init__.py b/lib/sata/command/__init__.py
index c68b8beac..6eea41f19 100644
--- a/lib/sata/command/__init__.py
+++ b/lib/sata/command/__init__.py
@@ -11,7 +11,9 @@ regs = {
 }
 
 from_rx = [
-	("dma_activate", 1)
+	("dma_activate", 1),
+	("data", 1),
+	("reg_d2h", 1)
 ]
 
 class SATACommandTX(Module):
@@ -73,7 +75,7 @@ class SATACommandTX(Module):
 			transport.sink.data.eq(sink.data),
 			sink.ack.eq(transport.sink.ack),
 			If(sink.stb & sink.ack & sink.eop,
-				NextState("IDLE")
+				NextState("WAIT_REG_D2H")
 			)
 		)
 		fsm.act("SEND_READ_DMA_CMD",
@@ -85,7 +87,7 @@ class SATACommandTX(Module):
 			transport.sink.command.eq(regs["READ_DMA_EXT"]),
 			sink.ack.eq(transport.sink.ack),
 			If(sink.stb & sink.ack,
-				NextState("IDLE")
+				NextState("WAIT_DATA")
 			)
 		)
 		fsm.act("SEND_IDENTIFY_CMD",
@@ -97,6 +99,17 @@ 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")
 			)
 		)
@@ -108,23 +121,44 @@ class SATACommandRX(Module):
 
 		###
 
-		self.comb += [
-			transport.source.ack.eq(1),
-			# XXX for test
-			If(transport.source.stb & (transport.source.type == fis_types["DMA_ACTIVATE_D2H"]),
-				self.to_tx.dma_activate.eq(1)
-			),
-			If(transport.source.stb & (transport.source.type == fis_types["DATA"]),
-				source.stb.eq(1),
-				source.sop.eq(transport.source.sop),
-				source.eop.eq(transport.source.eop),
-				source.data.eq(transport.source.data),
+		def test_type(name):
+			return transport.source.type == fis_types[name]
+
+		dma_activate = Signal()
+		data = Signal()
+		reg_d2h = Signal()
+
+		self.comb += \
+			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
+					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)
+				)
 			)
+
+		self.comb += [
+			self.to_tx.dma_activate.eq(dma_activate),
+			self.to_tx.data.eq(data),
+			self.to_tx.reg_d2h.eq(reg_d2h)
 		]
 
 class SATACommand(Module):
 	def __init__(self, transport):
 		self.submodules.tx = SATACommandTX(transport)
 		self.submodules.rx = SATACommandRX(transport)
-		self.sink, self.source = self.tx.sink, self.rx.source
 		self.comb += self.rx.to_tx.connect(self.tx.from_rx)
+		self.sink, self.source = self.tx.sink, self.rx.source
diff --git a/lib/sata/test/command_tb.py b/lib/sata/test/command_tb.py
index 636d27f75..4849d961a 100644
--- a/lib/sata/test/command_tb.py
+++ b/lib/sata/test/command_tb.py
@@ -23,6 +23,7 @@ class TB(Module):
 	def gen_simulation(self, selfp):
 		self.bfm.command.allocate_dma(0x00000000, 64*1024*1024)
 		self.bfm.command.enable_dma()
+		selfp.command.source.ack = 1
 		for i in range(100):
 			yield
 		for i in range(32):