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

This commit is contained in:
Florent Kermarrec 2014-12-14 15:32:00 +01:00
parent f01a8934f2
commit 0959f5b979
3 changed files with 136 additions and 48 deletions

View File

@ -3,7 +3,13 @@ from migen.genlib.fsm import FSM, NextState
from lib.sata.common import * from lib.sata.common import *
from_rx = [ tx_to_rx = [
("write", 1),
("read", 1),
("identify", 1)
]
rx_to_tx = [
("dma_activate", 1), ("dma_activate", 1),
("data", 1), ("data", 1),
("reg_d2h", 1) ("reg_d2h", 1)
@ -12,7 +18,8 @@ from_rx = [
class SATACommandTX(Module): class SATACommandTX(Module):
def __init__(self, transport): def __init__(self, transport):
self.sink = sink = Sink(command_tx_description(32)) 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") NextState("WAIT_DMA_ACTIVATE")
) )
) )
# XXX: split when length > 2048 dwords
fsm.act("WAIT_DMA_ACTIVATE", fsm.act("WAIT_DMA_ACTIVATE",
If(self.from_rx.dma_activate, If(from_rx.dma_activate,
NextState("SEND_DATA") NextState("SEND_DATA")
) )
) )
@ -68,7 +76,7 @@ class SATACommandTX(Module):
transport.sink.data.eq(sink.data), transport.sink.data.eq(sink.data),
sink.ack.eq(transport.sink.ack), sink.ack.eq(transport.sink.ack),
If(sink.stb & sink.ack & sink.eop, If(sink.stb & sink.ack & sink.eop,
NextState("WAIT_REG_D2H") NextState("IDLE")
) )
) )
fsm.act("SEND_READ_DMA_CMD", fsm.act("SEND_READ_DMA_CMD",
@ -80,7 +88,7 @@ class SATACommandTX(Module):
transport.sink.command.eq(regs["READ_DMA_EXT"]), transport.sink.command.eq(regs["READ_DMA_EXT"]),
sink.ack.eq(transport.sink.ack), sink.ack.eq(transport.sink.ack),
If(sink.stb & sink.ack, If(sink.stb & sink.ack,
NextState("WAIT_DATA") NextState("IDLE")
) )
) )
fsm.act("SEND_IDENTIFY_CMD", fsm.act("SEND_IDENTIFY_CMD",
@ -92,25 +100,23 @@ class SATACommandTX(Module):
transport.sink.command.eq(regs["IDENTIFY_DEVICE_DMA"]), transport.sink.command.eq(regs["IDENTIFY_DEVICE_DMA"]),
sink.ack.eq(transport.sink.ack), sink.ack.eq(transport.sink.ack),
If(sink.stb & 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") 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): class SATACommandRX(Module):
def __init__(self, transport): def __init__(self, transport):
self.source = source = Source(command_rx_description(32)) 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] return transport.source.type == fis_types[name]
dma_activate = Signal() 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("DMA_ACTIVATE_D2H"),
dma_activate.eq(1),
NextState("WAIT_WRITE_REG_D2H")
)
)
)
fsm.act("WAIT_WRITE_REG_D2H",
transport.source.ack.eq(1),
If(transport.source.stb, If(transport.source.stb,
If(test_type("REG_D2H"), If(test_type("REG_D2H"),
# XXX add checks NextState("PRESENT_WRITE_RESPONSE")
reg_d2h.eq(1), )
transport.source.ack.eq(1) )
).Elif(test_type("DMA_ACTIVATE_D2H"), )
# XXX add checks fsm.act("PRESENT_WRITE_RESPONSE",
dma_activate.eq(1),
transport.source.ack.eq(1)
).Elif(test_type("DATA"),
source.stb.eq(1), 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.sop.eq(transport.source.sop),
source.eop.eq(transport.source.eop), source.eop.eq(transport.source.eop),
source.data.eq(transport.source.data), source.data.eq(transport.source.data),
data.eq(source.eop & source.ack), transport.source.ack.eq(source.ack),
transport.source.ack.eq(source.ack) If(source.stb & source.eop & source.ack,
).Else( NextState("WAIT_READ_REG_D2H")
transport.source.ack.eq(1) )
)
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.comb += [
self.to_tx.dma_activate.eq(dma_activate), to_tx.dma_activate.eq(dma_activate),
self.to_tx.data.eq(data),
self.to_tx.reg_d2h.eq(reg_d2h)
] ]
class SATACommand(Module): class SATACommand(Module):
def __init__(self, transport): def __init__(self, transport):
self.submodules.tx = SATACommandTX(transport) self.submodules.tx = SATACommandTX(transport)
self.submodules.rx = SATACommandRX(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 self.sink, self.source = self.tx.sink, self.rx.source

View File

@ -223,6 +223,7 @@ class LinkLayer(Module):
self.tx_packet.done = True self.tx_packet.done = True
elif dword == primitives["R_ERR"]: elif dword == primitives["R_ERR"]:
self.tx_packet.done = True self.tx_packet.done = True
self.phy.send(primitives["SYNC"])
def insert_cont(self): def insert_cont(self):
self.tx_lasts.pop(0) self.tx_lasts.pop(0)
@ -388,15 +389,20 @@ class CommandLayer(Module):
def callback(self, fis): def callback(self, fis):
# XXX manage maximum of 2048 DWORDS per DMA # XXX manage maximum of 2048 DWORDS per DMA
resp = None
if isinstance(fis, FIS_REG_H2D): if isinstance(fis, FIS_REG_H2D):
if fis.command == regs["WRITE_DMA_EXT"]: 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"]: 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"]: 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): 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 # HDD model
class HDDMemRegion: class HDDMemRegion:
@ -412,23 +418,32 @@ class HDD(Module):
self.mem = None self.mem = None
self.wr_address = 0 self.wr_address = 0
self.wr_length = 0
self.wr_cnt = 0
def write_dma_cmd(self, fis): def write_dma_cmd(self, fis):
self.wr_address = fis.lba_lsb 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): def read_dma_cmd(self, fis):
packet = self.read_mem(fis.lba_lsb, fis.count*4) packet = self.read_mem(fis.lba_lsb, fis.count*4)
packet.insert(0, 0) packet.insert(0, 0)
return FIS_DATA(packet) return [FIS_DATA(packet), FIS_REG_D2H()]
def identify_dma_cmd(self, fis): def identify_dma_cmd(self, fis):
packet = [i for i in range(256)] packet = [i for i in range(256)]
packet.insert(0, 0) packet.insert(0, 0)
return FIS_DATA(packet) return [FIS_DATA(packet), FIS_REG_D2H()]
def data_cmd(self, fis): def data_cmd(self, fis):
self.write_mem(self.wr_address, fis.packet[1:]) 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): def allocate_mem(self, base, length):
# XXX add support for multiple memory regions # XXX add support for multiple memory regions

View File

@ -126,6 +126,9 @@ class TB(Module):
yield yield
streamer_packet = CommandTXPacket(write=1, address=1024, length=32, data=[i for i in range(32)]) 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.streamer.send(streamer_packet)
yield from self.logger.receive()
for d in self.logger.packet:
print("%08x" %d)
for i in range(32): for i in range(32):
yield yield
streamer_packet = CommandTXPacket(read=1, address=1024, length=32) streamer_packet = CommandTXPacket(read=1, address=1024, length=32)
@ -133,6 +136,9 @@ class TB(Module):
yield from self.logger.receive() yield from self.logger.receive()
for d in self.logger.packet: for d in self.logger.packet:
print("%08x" %d) print("%08x" %d)
yield from self.logger.receive()
for d in self.logger.packet:
print("%08x" %d)
if __name__ == "__main__": if __name__ == "__main__":
run_simulation(TB(), ncycles=512, vcd_name="my.vcd", keep_files=True) run_simulation(TB(), ncycles=512, vcd_name="my.vcd", keep_files=True)