stepgen.py

#!/usr/bin/env python3

import itertools

from litex.gen import *
from litex.soc.interconnect.csr import *
from litex.soc.interconnect.stream import SyncFIFO

from migen.fhdl import verilog

class Delay(Module):
    def __init__(self, sig, hold_cycles):
        stages = 2**hold_cycles.nbits

        self.cancel = Signal(reset=0)

        sigs = Array([Signal() for _ in range(stages)])
        self.comb += sigs[0].eq(sig)
        for i in range(1, stages):
            self.sync += sigs[i].eq(sigs[i-1])
        self.out = Signal()
        self.comb += self.out.eq(sigs[hold_cycles])

        self.sync += [
            If(self.cancel, [sigs[i].eq(self.out) for i in range(1, stages)]),
        ]

class HoldTimeStepOutput(Module):
    def __init__(self, hold_cycles):
        fsm = FSM(reset_state="IDLE")
        self.submodules += fsm

        remaining_cycles = Signal(hold_cycles.nbits)
        self.input = Signal()
        self.out = Signal()
        self.force_low = Signal()
        self.keep_high = Signal()

        fsm.act("IDLE",
            self.out.eq(0),
            If(self.input, [NextValue(remaining_cycles, hold_cycles), NextState("HIGH")]),
        )
        fsm.act("HIGH",
            self.out.eq(1),
            If(remaining_cycles != 1, NextValue(remaining_cycles, remaining_cycles - 1)),
            If(~self.keep_high & (self.force_low | (remaining_cycles == 1)), NextState("IDLE")),
        )

class Stepgen(Module, AutoCSR):
    def __init__(self, num_outputs, queue_depth=512, counter=None):
        assert num_outputs <= 32, "The maximum supported number of stepper outputs is 32"
        assert queue_depth == 2**bits_for(queue_depth-1), "The queue must have a power-of-two number of items"
        self._num_outputs = CSRConstant(num_outputs)
        self.num_outputs = num_outputs
        self._queue_depth = CSRConstant(queue_depth)
        self.queue_depth = queue_depth

        self.counter = counter if counter is not None else Signal(32)

        self.failure = CSRStatus(
            size=num_outputs,
            description="Failure states of all stepgens",
            fields=[CSRField(name=f"output{idx}",
                             description=f"Failure state for output {idx}")
                    for idx in range(num_outputs)],
        )

        self.flush = CSRStorage(
            size=num_outputs,
            description="Flush one or more stepgens",
            fields=[CSRField(name=f"output{idx}",
                             description=f"Write to flush output {idx}",
                             pulse=True)
                    for idx in range(num_outputs)],
        )
        self.sync += self.flush.storage.eq(0)

        self.outputs = CSRCluster(description="Step outputs")
        self.outputs_step = Signal(num_outputs)
        self.outputs_dir = Signal(num_outputs)
        for idx in range(num_outputs):
            output = StepgenOutput(self, idx)
            self.outputs.append(output)
            self.comb += [
                self.outputs_step[idx].eq(output.output_step),
                self.outputs_dir[idx].eq(output.output_dir),
                getattr(self.failure.fields, f"output{idx}").eq(output.has_error),
                If(self.flush.re & getattr(self.flush.fields, f"output{idx}"), output.flush.eq(1)),
                # output.flush.eq(getattr(self.flush.fields, f"output{idx}")),
            ]
            self.submodules += output

STEP_SEQUENCE = [
    ("interval", 32, DIR_NONE),
    ("count", 16, DIR_NONE),
    ("add", (16, True), DIR_NONE),
    ("dir", 1, DIR_NONE),
]

class StepgenOutput(Module, AutoCSR):
    def __init__(self, shared, idx):
        self.idx = idx
        queue_depth = shared.queue_depth

        self.error_state = CSRStatus(
            description="Current error state for the output. Write to clear.",
            fields=[
                CSRField("steps_too_close"),
                CSRField("deadline_missed"),
            ]
        )
        self.next_dir = CSRStorage(description="Direction of next queued step sequence")
        self.next_count = CSRStorage(description="Length of next queued step sequence in steps", size=16)
        self.next_add = CSRStorage(description="Per-step interval addition value for the next step sequence", size=16)
        self.push_interval = CSRStorage(
            size=32,
            description="Writing an interval will queue the next step sequence",
        )
        self.last_step = CSRStatus(
            size=32,
            description="Returns the clock value of the last step. Write to set new last step.",
            read_only=False,
        )
        self.current_step = CSRStatus(
            size=32,
            description="Current step position. Write to override.",
            read_only=False,
        )
        self.cmd_start = CSRStatus(
            size=32,
            description="Clock when the current command was scheduled",
        )
        self.cmd_steps_remaining = CSRStatus(
            size=32,
            description="Number of steps left in current step sequence",
        )
        self.status = CSRStatus(
            description="Status information",
            fields=[
                CSRField("active"),
            ]
        )
        self._flush = CSRStorage(
            description="Writing to this register will flush the step queue and cancel the current move sequence",
            fields=[
                CSRField("flush", pulse=True, access=CSRAccess.WriteOnly),
            ],
        )
        self.config = CSRStorage(
            description="Output settings",
            fields=[
                CSRField("step_both_edges"),
                CSRField("invert_step"),
                CSRField("hold_delay", size=6, reset=3),
            ]
        )
        self.command_queue_len = CSRStatus(
            size=bits_for(queue_depth),
            description="Number of currently queued items in the command queue",
        )
        self.command_queue_full = CSRStatus(
            description="Indicates if the command queue is currently full",
        )

        self.counter_at_error = CSRStatus(
            size=32,
            description="Counter value when error occured",
            reset=0,
        )

        dedge_state_step = Signal()
        sedge_state_step = HoldTimeStepOutput(self.config.fields.hold_delay)
        self.submodules += sedge_state_step
        state_dir = Signal()

        output_step_noninverted = Signal()

        self.output_step = Signal()
        self.output_dir = Signal()
        self.comb += [
            self.output_dir.eq(state_dir),
            output_step_noninverted.eq(Mux(self.config.fields.step_both_edges, dedge_state_step, sedge_state_step.out)),
            self.output_step.eq(Mux(self.config.fields.invert_step, ~output_step_noninverted, output_step_noninverted)),
        ]

        last_step = self.last_step.status
        step_count = self.current_step.status
        queue = ResetInserter()(SyncFIFO(STEP_SEQUENCE, shared.queue_depth, buffered=True))
        self.submodules += queue
        self.flush = Signal(reset=0)
        do_flush = Signal()
        self.comb += [
            do_flush.eq(self._flush.re | self.flush),
            queue.reset.eq(do_flush),
        ]
        self.sync += self._flush.storage.eq(0),

        # Config
        step_both_edges = self.config.fields.step_both_edges

        # Error handling
        self.has_error = Signal()
        self.comb += self.has_error.eq(Reduce("OR", self.error_state.fields.fields))

        def set_error(field):
            return [
                getattr(self.error_state.fields, field).eq(1),
                self.counter_at_error.status.eq(shared.counter),
            ]

        # Push command
        self.comb += [
            queue.sink.payload.interval.eq(self.push_interval.storage),
            queue.sink.payload.count.eq(self.next_count.storage),
            queue.sink.payload.add.eq(self.next_add.storage),
            queue.sink.payload.dir.eq(self.next_dir.storage),

            queue.sink.valid.eq(self.push_interval.re & ~self.has_error),
        ]

        # Command execution
        current_command = Record(STEP_SEQUENCE)
        has_next_step = Signal()
        next_step = Signal(32)
        is_last_step = Signal()
        step_now = Signal()
        next_interval = Signal(32)
        self.comb += [
            has_next_step.eq(~self.has_error & (current_command.count != 0)),
            next_step.eq(last_step + current_command.interval),
            is_last_step.eq(current_command.count == 1),
            step_now.eq(has_next_step & (next_step == shared.counter)),
            next_interval.eq(current_command.interval + current_command.add),
            sedge_state_step.input.eq(step_now),
        ]
        self.sync += If(step_now, [
            If(step_both_edges | (~step_both_edges & (sedge_state_step.out == 0)), [
                If(current_command.dir,
                   step_count.eq(step_count - 1)
                ).Else(
                   step_count.eq(step_count + 1)
                ),
                last_step.eq(next_step),
                dedge_state_step.eq(~dedge_state_step),
                current_command.count.eq(current_command.count - 1),
                current_command.interval.eq(next_interval),
            ]).Else([
                # Go to error state: software put steps so close together that we didn't get to unstep
                set_error("steps_too_close")
            ])
        ])

        # The direction signal follows the current command, delayed by a fixed number of cycles. This
        # ensures dir-after-step hold time requirements are met. The dir-to-step setup time is not
        # enforced.
        delayed_direction = Delay(current_command.dir, self.config.fields.hold_delay)
        self.submodules += delayed_direction
        self.comb += [
            state_dir.eq(delayed_direction.out),
            delayed_direction.cancel.eq(do_flush | self.has_error),
        ]

        # When we have no next step, or we are taking our last step. We can load the next.
        load_now = Signal()
        self.comb += [
            load_now.eq(~self.has_error & (~has_next_step | (step_now & is_last_step))),
            queue.source.ready.eq(load_now),
        ]
        self.sync += If(load_now & queue.source.valid, [
            current_command.eq(queue.source.payload),
            self.cmd_start.status.eq(shared.counter),
        ])

        # In single-edge mode we need to unstep. Unstepping happens a fixed number of cycles after the
        # step when no new step is queued. When a new step is queued, we instead wait until half way.
        # This gives 50% duty cycle.
        half_next_step = Signal(32)
        unstep_now = Signal()
        self.comb += [
            half_next_step.eq(last_step + (current_command.interval >> 1)),
            unstep_now.eq(has_next_step & (half_next_step == shared.counter)),
            sedge_state_step.force_low.eq(unstep_now),
            sedge_state_step.keep_high.eq(has_next_step & (half_next_step > shared.counter)),
        ]

        # If ever the next_step time is smaller than the counter, we missed the deadline and
        # need to report an error. To easily handled the counters being unsigned, we can rewrite the
        # logic as follows:
        #   next_step < shared_counter
        #   next_step - shared_counter < 0
        # The < 0 can be tested by just looking a the msb, as negative numbers will have this bit set
        missed_deadline = Signal()
        deadline_counter_diff = Signal(32)
        self.comb += [
            deadline_counter_diff.eq(next_step - shared.counter),
            missed_deadline.eq(has_next_step & deadline_counter_diff[-1]),
        ]
        self.sync += If(~self.has_error & missed_deadline, set_error("deadline_missed"))

        # Direct command handling and status registers
        self.comb += [
            self.command_queue_len.status.eq(queue.level),
            self.command_queue_full.status.eq(~queue.fifo.writable),
            self.cmd_steps_remaining.status.eq(current_command.count),
            self.status.fields.active.eq(~self.has_error & (has_next_step | queue.source.valid)),
        ]

        self.sync += [
            If(self.last_step.re, self.last_step.status.eq(self.last_step.r)),
            If(self.current_step.re, self.current_step.status.eq(self.current_step.r)),
            # Flush current command and queue. This must be last, to ensure the resets win.
            # Note that we don't do anything to the step and dir signals. They need to keep
            # running to ensure hold times are not violated.
            If(do_flush, [
                current_command.count.eq(0),
                current_command.dir.eq(state_dir),
                *[field.eq(0) for field in self.error_state.fields.fields],
            ]),
        ]

        self._debug = [
            shared.counter,
            has_next_step,
            is_last_step,
            step_now,
            next_interval,
            unstep_now,
            self.has_error,
            missed_deadline,
        ]

def output_core():
    stepgen = Stepgen(1)
    print(verilog.convert(stepgen))

def run_sims():

    # def sim():
    #     yield dut.counter.eq(0)
    #     # yield from init()
    #     # for i in range(len(dut.outputs)):
    #     #     yield from push(i, 100, 1, 0, 0)
    #     #     yield from push(i, 1, 60000, 0, 0)
    #     #     yield from push(i, 3000, 1, 0, 1)
    #     # yield from advance(140)
    #     # yield
    #     # yield from set_both_edges(0, False)
    #     # yield from set_last_step(0, 0)
    #     # yield from push(0, 150, 1, 0, 0)
    #     # yield from push(0, 20, 9, 0, 0)
    #     # yield from push(0, 20, 10, 0, 1)
    #     # yield from push(0, 20, 10, 0, 0)
    #     # yield from advance(1000)
    #     # yield from flush(0)
    #     # yield from advance(100)
    #     # yield from set_last_step(0, (yield dut.counter))
    #     # yield from push(0, 100, 1, 0, 0)
    #     # yield from push(0, 1, 60000, 0, 0)
    #     # yield from advance(1000)
    #     yield from advance(50)
    #     yield from set_last_step(0, (yield dut.counter) + 100)
    #     yield from push(0, 0, 1, 0, 0)
    #     yield from push(0, 2, 9, 0, 1)
    #     yield from advance(10)
    #     yield from advance(130)
    #     yield from push(0, 1, 9, 0, 1)
    #     yield from push(0, 1, 9, 0, 1)
    #     yield from push(0, 1, 9, 0, 1)
    #     yield from flush(0)
    #     yield from advance(100)
    #     yield from push(0, 1, 9, 0, 1)
    #     yield from advance(100)
    #     yield from flush(0)
    #     yield from advance(10)
    #     yield from set_last_step(0, (yield dut.counter) + 10)
    #     yield from push(0, 0, 0, 0, 0)
    #     yield from push(0, 2, 9, 0, 0)
    #     yield from advance(300)
    #     # yield from push(0, 1, 9, 0, 1)
    #     # for i in range(10):
    #     #     yield from push(0, 10, 10, 0, i % 2)
    #     # yield from advance(1000)

    def init():
        for (idx, output) in enumerate(dut.outputs):
            yield from set_both_edges(idx, False)
        yield

    def set_last_step(dut, idx, last_step):
        yield dut.outputs[idx].last_step.r.eq(last_step)
        yield dut.outputs[idx].last_step.re.eq(1)
        yield from advance(dut)
        yield dut.outputs[idx].last_step.re.eq(0)

    def set_both_edges(dut, idx, enable):
        yield dut.outputs[idx].config.fields.step_both_edges.eq(1 if enable else 0)

    def flush(dut, idx):
        yield dut.outputs[idx]._flush.re.eq(1)
        yield from advance(dut)
        yield dut.outputs[idx]._flush.re.eq(0)

    def push(dut, idx, interval, count, add, direction):
        yield dut.outputs[idx].push_interval.storage.eq(interval)
        yield dut.outputs[idx].next_count.storage.eq(count)
        yield dut.outputs[idx].next_add.storage.eq(add)
        yield dut.outputs[idx].next_dir.storage.eq(direction)
        yield dut.outputs[idx].push_interval.re.eq(1)
        yield from advance(dut)
        yield dut.outputs[idx].push_interval.re.eq(0)

    def advance(dut, n=1):
        for i in range(n):
            yield dut.counter.eq(dut.counter + 1)
            yield

    def testcase_0(dut):
        yield from set_last_step(dut, 0, 0)
        yield from push(dut, 0, 0x00000010, 1, 0, 0)
        yield from push(dut, 0, 0x0000102b, 9, -328, 0)
        yield from push(dut, 0, 0x00000623, 28, -29, 0)
        yield from push(dut, 0, 0x00000377, 39, -8, 0)
        yield from push(dut, 0, 0x0000026d, 59, -3, 0)
        yield from push(dut, 0, 0x000001e0, 50, -2, 0)
        yield from push(dut, 0, 0x00000193, 64, -1, 0)
        yield from push(dut, 0, 0x00000164, 427, 0, 0)
        yield from push(dut, 0, 0x00000163, 44, 0, 0)
        yield from push(dut, 0, 0x00000177, 83, 1, 0)
        yield from push(dut, 0, 0x000001e3, 54, 2, 0)
        yield from push(dut, 0, 0x00000268, 45, 6, 0)
        yield from push(dut, 0, 0x000003b1, 24, 27, 0)
        yield from push(dut, 0, 0x00000908, 25, -37, 0)
        yield from push(dut, 0, 0x0000045b, 43, -12, 0)
        yield from push(dut, 0, 0x0000029a, 52, -4, 0)
        yield from push(dut, 0, 0x000001ee, 59, -2, 0)
        yield from push(dut, 0, 0x0000018c, 81, -1, 0)
        yield from push(dut, 0, 0x00000144, 35, 0, 0)
        yield from push(dut, 0, 0x0000013e, 250, 0, 0)
        yield from push(dut, 0, 0x0000013f, 263, 0, 0)
        yield from push(dut, 0, 0x00000158, 39, 0, 0)
        yield from push(dut, 0, 0x0000016b, 74, 1, 0)
        yield from push(dut, 0, 0x000001ca, 57, 2, 0)
        yield from push(dut, 0, 0x00000259, 43, 6, 0)
        yield from push(dut, 0, 0x00000393, 23, 29, 0)
        yield from push(dut, 0, 0x0000081b, 16, 71, 0)
        yield from push(dut, 0, 0x000007c0, 30, -34, 0)
        yield from push(dut, 0, 0x00000438, 50, -8, 0)
        yield from push(dut, 0, 0x000002da, 65, -3, 0)
        yield from push(dut, 0, 0x0000023b, 286, 0, 0)
        yield from push(dut, 0, 0x0000024f, 54, 2, 0)
        yield from push(dut, 0, 0x000002dc, 50, 6, 0)
        yield from push(dut, 0, 0x0000044c, 27, 25, 0)
        yield from push(dut, 0, 0x00000763, 10, 237, 0)
        yield from push(dut, 0, 0x00002022, 1, 0, 0)

        yield from advance(dut, 300000)

    def testcase_1(dut):
        yield from set_last_step(dut, 0, 0)
        yield from push(dut, 0, 100, 1, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)

        yield from advance(dut, 490)
        yield from flush(dut, 0)
        yield from set_last_step(dut, 0, (yield dut.counter))
        yield from push(dut, 0, 5, 1000, 0, 1)
        yield from advance(dut, 1000)

        assert (yield dut.outputs[0].error_state.status) == 0, "Must be no errors"

    def testcase_2(dut):
        yield from set_last_step(dut, 0, 0)
        yield from push(dut, 0, 100, 1, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)
        yield from push(dut, 0, 30, 1000, 0, 1)

        yield from advance(dut, 483)
        yield from flush(dut, 0)
        yield from advance(dut, 30)
        yield from set_last_step(dut, 0, (yield dut.counter)+10)
        yield from push(dut, 0, 4, 10, 0, 1)
        yield from advance(dut, 1000)

        assert (yield dut.outputs[0].error_state.status) == 0, "Must be no errors"

    # def testcase_3(dut):
    #     yield from set_last_step(dut, 0, 0)
    #     yield from push(dut, 0, 100, 1, 0, 1)
    #     yield from push(dut, 0, 10, 100, 1/8, 1)
    #     yield from advance(dut, 3000)

    # def testcase_4(dut):
    #     yield dut.timebase.counter.eq(0xfffffe0a)
    #     yield
    #     yield from set_last_step(dut, 0, 0xfffffe0a)
    #     yield from push(dut, 0, 0xf4bd, 0x0003, 0x4f51, 1, raw=True)
    #     yield from advance(dut, 2000)

    # def testcase_5(dut):
    #     target = 0x79078240
    #     yield dut.timebase.counter.eq(target-4*30)
    #     yield
    #     yield from set_last_step(dut, 0, target-100)
    #     # yield from advance(dut, 50)
    #     yield from push(dut, 0, 100, 1, 0, 1, raw=True)
    #     yield from push(dut, 0, 0x24a3, 0x5, 0xbfe, 1, raw=True)
    #     yield from advance(dut, 2000)

    def run_case(name, testcase, num_gens=1):
        print(f"Test case: {name}")
        dut = Stepgen(num_gens)
        dut.counter.eq(0)
        testcase(dut)
        run_simulation(dut, testcase(dut), vcd_name=f"build/{name}.vcd")

    # run_case("testcase_0", testcase_0)
    run_case("testcase_1", testcase_1)
    run_case("testcase_2", testcase_2)
    # run_case("testcase_3", testcase_3)
    # run_case("testcase_4", testcase_4)
    # run_case("testcase_5", testcase_5)

if __name__ == "__main__":
    run_sims()