from migen import *
from sync_serdes import *
from migen.genlib.fifo import SyncFIFO
from migen.build.platforms.sinara import kasli
from migen.genlib.misc import WaitTimer
from kasli_crg import TransceiverCRG
from eem_helpers import generate_pads
from uart import UART
from io_loopback import SingleIOLoopback


class SingleSerDesLoopBack(Module):
    def __init__(self, io_pad, sys_clk_freq, debug):
        self.uart_rx = Signal()
        self.uart_tx = Signal()

        self.submodules.uart = UART(round((115200/sys_clk_freq)*2**32))
        self.comb += [
            self.uart.phy_rx.eq(self.uart_rx),
            self.uart_tx.eq(self.uart.phy_tx),
        ]
        
        self.submodules.tx = SingleLineTX()
        self.submodules.rx = SingleLineRX()

        # Debugging reader
        self.submodules.bitslip_reader = BitSlipReader()
        self.submodules.postslip_reader = BitSlipReader()

        # Alignment modules
        self.submodules.slave_aligner = SlaveAligner()

        # The actual channel
        self.submodules.channel = SingleIOLoopback(io_pad)

        # Attach FIFO to UART TX, send rate is too slow w.r.t sysclk
        self.submodules.tx_fifo = SyncFIFO(8, 64)

        self.comb += [
            # Repetitively send 0b00100
            self.tx.txdata.eq(0b00100),

            # Loopback channel
            self.channel.i.eq(self.tx.ser_out),
            self.rx.ser_in_no_dly.eq(self.channel.o),
            self.channel.t.eq(self.tx.t_out),

            # TX path
            self.uart.tx_data.eq(self.tx_fifo.dout),
            self.uart.tx_stb.eq(self.tx_fifo.readable),
            self.tx_fifo.re.eq(self.uart.tx_ack),
        ]

        # Debugging logics
        self.comb += [
            self.bitslip_reader.loopback_rxdata.eq(self.rx.rxdata),
            self.postslip_reader.loopback_rxdata.eq(self.rx.rxdata),
            self.slave_aligner.loopback_rxdata.eq(self.rx.rxdata),

            self.rx.master_bitslip.eq(
                self.bitslip_reader.bitslip
                | self.slave_aligner.master_bitslip
                | self.postslip_reader.bitslip
            ),
            self.rx.slave_bitslip.eq(
                self.bitslip_reader.bitslip
                | self.slave_aligner.master_bitslip
                | self.postslip_reader.bitslip
            ),
        ]

        bitslip_count = Signal(3)

        fsm = FSM(reset_state="WAIT_DONE")
        self.submodules += fsm

        fsm.act("WAIT_DONE",
            self.bitslip_reader.start.eq(1),
            If(self.bitslip_reader.done,
                NextValue(bitslip_count, 0),
                NextState("WRITE_UPPER"),
            ),
        )

        fsm.act("WRITE_UPPER",
            If(bitslip_count == 5,
                NextState("WAIT_BITSLIP_ALIGNED"),
            ).Elif(self.tx_fifo.writable,
                self.tx_fifo.we.eq(1),
                self.tx_fifo.din.eq(self.bitslip_reader.data_result[bitslip_count][8:]),
                NextState("WRITE_LOWER"),
            ),
        )

        fsm.act("WRITE_LOWER",
            If(self.tx_fifo.writable,
                self.tx_fifo.we.eq(1),
                self.tx_fifo.din.eq(self.bitslip_reader.data_result[bitslip_count][:8]),
                NextValue(bitslip_count, bitslip_count + 1),
                NextState("WRITE_UPPER"),
            )
        )

        fsm.act("WAIT_BITSLIP_ALIGNED",
            self.slave_aligner.start.eq(1),
            If(self.slave_aligner.done,
                NextState("BARRIER_UPPER"),
            ),
        )

        fsm.act("BARRIER_UPPER",
            If(self.tx_fifo.writable,
                self.tx_fifo.we.eq(1),
                self.tx_fifo.din.eq(0xFF),
                NextState("BARRIER_LOWER"),
            ),
        )

        fsm.act("BARRIER_LOWER",
            If(self.tx_fifo.writable,
                self.tx_fifo.we.eq(1),
                self.tx_fifo.din.eq(0xFF),
                NextState("REWAIT_BITSLIP_READ_DONE"),
            ),
        )

        fsm.act("REWAIT_BITSLIP_READ_DONE",
            self.postslip_reader.start.eq(1),
            If(self.postslip_reader.done,
                NextValue(bitslip_count, 0),
                NextState("REWRITE_UPPER"),
            ),
        )

        fsm.act("REWRITE_UPPER",
            If(bitslip_count == 5,
                NextState("TERMINATE"),
            ).Elif(self.tx_fifo.writable,
                self.tx_fifo.we.eq(1),
                self.tx_fifo.din.eq(self.postslip_reader.data_result[bitslip_count][8:]),
                NextState("REWRITE_LOWER"),
            ),
        )

        fsm.act("REWRITE_LOWER",
            If(self.tx_fifo.writable,
                self.tx_fifo.we.eq(1),
                self.tx_fifo.din.eq(self.postslip_reader.data_result[bitslip_count][:8]),
                NextValue(bitslip_count, bitslip_count + 1),
                NextState("REWRITE_UPPER"),
            )
        )

        fsm.act("TERMINATE",
            NextState("TERMINATE"),
        )

        self.comb += self.tx.txdata.eq(0b00100)


if __name__ == "__main__":
    platform = kasli.Platform(hw_rev="v2.0")

    # Generate pads for the I/O blocks
    eem = 3
    generate_pads(platform, eem)
    pad = platform.request("dio{}".format(eem), 0)

    crg = TransceiverCRG(platform, platform.request("clk125_gtp"))
    top = SingleSerDesLoopBack(pad, crg.sys_clk_freq, True)

    # Wire up UART core to the pads
    uart_pads = platform.request("serial")
    top.comb += [
        top.uart_rx.eq(uart_pads.rx),
        uart_pads.tx.eq(top.uart_tx),
    ]

    top.submodules += crg
    platform.build(top)