command: add fsm for RX Path and manage D2H response in bfm

diff --git a/lib/sata/command/__init__.py b/lib/sata/command/__init__.py
index 3adc889d0db81a5670cbafb9f3ac4abd047e8e83..88dd700e644fef1ffdca19c129aaefb2cee4ad2f 100644 (file)
--- 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 97fe1174c166d9fe566a5ed07b5fcf561b9b2b5b..1c1ac49a1d7f295be513cf37669a1e6e1046c3e7 100644 (file)
--- 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 00a7aafc9d0309d737a6a743ff77e3740ee01b7b..28c2e5a87023e45e36ed8c0bbb0d84b2365125db 100644 (file)
--- 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)