Merge pull request #19 from quartiq/nk/servo

Nk/servo

adc.py

@@ -0,0 +1,133 @@
+from collections import namedtuple
+from migen.genlib.io import DifferentialInput, DifferentialOutput, DDROutput
+from migen import *
+
+
+# all times in cycles
+AdcParams = namedtuple(
+    "AdcParams",
+    [
+        "channels",  # number of channels per lane
+        "lanes",  # number of SDO? data lanes
+        # lanes need to be named alphabetically and contiguous
+        # (e.g. [sdoa, sdob, sdoc, sdoc] or [sdoa, sdob])
+        "width",  # bits to transfer per channel
+        "t_cnvh",  # CNVH duration (minimum)
+        "t_conv",  # CONV duration (minimum)
+        "t_rtt",  # upper estimate for clock round trip time from
+        # sck at the FPGA to clkout at the FPGA (cycles)
+        # this avoids having synchronizers and another counter
+        # to signal end-of transfer
+        # and it ensures fixed latency early in the pipeline
+    ],
+)
+
+
+class Adc(Module):
+    """Multi-lane, multi-channel, triggered, source-synchronous, serial
+    ADC interface.
+    * Supports ADCs like the LTC2320-16.
+    * Hardcoded timings.
+    """
+
+    def __init__(self, pins, params):
+        self.params = p = params  # ADCParams
+        self.data = [
+            Signal((p.width, True), reset_less=True) for _ in range(p.channels)
+        ]  # retrieved ADC data
+        self.start = Signal()  # start conversion and reading
+        self.reading = Signal()  # data is being read (outputs are invalid)
+        self.done = Signal()  # data is valid and a new conversion can be started
+
+        ###
+
+        self.sck = sck = Signal()
+        self.sck_en = sck_en = Signal()
+        # self.clkout_in = clkout_in = Signal()
+        self.clkout = clkout = Signal()
+        self.cnvn = cnvn = Signal()
+        self.sdo = sdo = [Signal(), Signal()]
+        self.sdo2n = sdo2n = Signal()  # inverted input
+        self.ddr_clk_synth = ddr_clk_synth = Signal(
+            6, reset=0b000111
+        )  # signal to generate a 10ns period clock
+
+        if pins != None:
+            self.specials += [
+                DifferentialOutput(sck, pins.sck_n, pins.sck_p),  # swapped
+                DifferentialInput(pins.clkout_p, pins.clkout_n, clkout),
+                DifferentialOutput(~cnvn, pins.cnvn_n, pins.cnvn_p),  # swapped
+                DifferentialInput(pins.sdo_p[0], pins.sdo_n[0], sdo[0]),
+                DifferentialInput(pins.sdo_n[1], pins.sdo_p[1], sdo2n),  # swapped
+                DDROutput(ddr_clk_synth[1], ddr_clk_synth[0], sck, ClockSignal("sys")),
+            ]
+
+        self.comb += [
+            sdo[1].eq(~sdo2n),  # invert due to swapped input
+        ]
+
+        # set up counters for the four states CNVH, CONV, READ, RTT
+        t_read = 3 * p.width * p.channels // p.lanes  # SDR
+        assert p.lanes * t_read == p.width * p.channels * 3
+        assert all(_ > 0 for _ in (p.t_cnvh, p.t_conv, p.t_rtt))
+        assert p.t_conv > 1
+        count = Signal(max=max(p.t_cnvh, p.t_conv, t_read, p.t_rtt), reset_less=True)
+        count_load = Signal.like(count)
+        count_done = Signal()
+        update = Signal()
+
+        self.comb += count_done.eq(count == 0)
+        self.sync += [
+            count.eq(count - 1),
+            If(
+                count_done,
+                count.eq(count_load),
+            ),
+            # 6 bit barrel shifter that shifts by two each cycle
+            If(sck_en, Cat(ddr_clk_synth).eq(Cat(ddr_clk_synth[-2:], ddr_clk_synth))),
+        ]
+
+        self.submodules.fsm = fsm = FSM("IDLE")
+        fsm.act(
+            "IDLE",
+            self.done.eq(1),
+            If(self.start, count_load.eq(p.t_cnvh - 1), NextState("CNVH")),
+        )
+        fsm.act(
+            "CNVH",
+            count_load.eq(p.t_conv - 1),
+            cnvn.eq(1),
+            If(count_done, NextState("CONV")),
+        )
+        fsm.act("CONV", count_load.eq(t_read - 1), If(count_done, NextState("READ")))
+        fsm.act(
+            "READ",
+            self.reading.eq(1),
+            count_load.eq(p.t_rtt - 1),
+            sck_en.eq(1),
+            If(count_done, NextState("RTT")),
+        )
+        fsm.act(
+            "RTT",  # account for sck->clkout round trip time
+            self.reading.eq(1),
+            If(count_done, update.eq(1), NextState("IDLE")),
+        )
+
+        self.clock_domains.cd_ret = ClockDomain("ret", reset_less=True)
+        self.comb += self.cd_ret.clk.eq(clkout)
+
+        k = p.channels // p.lanes
+        assert t_read == k * p.width * 3
+        # flip sdos because the inputs on the ADC are flipped on schematic
+        for i, sdo in enumerate(reversed(sdo)):
+            sdo_sr = Signal(2 * t_read)
+            self.sync.ret += [
+                sdo_sr[1:].eq(sdo_sr),
+                sdo_sr[0].eq(sdo),
+            ]
+            self.sync += [
+                If(
+                    update,
+                    Cat(reversed([self.data[i * k + j] for j in range(k)])).eq(sdo_sr),
+                )
+            ]

iir.py

@@ -0,0 +1,141 @@
+# First order IIR filter for multiple channels and profiles with one DSP and no blockram.
+# DSP block with MSB aligned inputs and "round half down" rounding.
+#
+#
+# Note: Migen translates the "out of range" pc mux selector to the last vaid mux input.
+
+from ast import Constant
+from migen import *
+
+N_COEFF = 3  # [b0, b1, a0] number of coefficients for a first order iir
+
+
+class Dsp(Module):
+    def __init__(self):
+        # xilinx dsp architecture (subset)
+        self.a = a = Signal((25, True), reset_less=True)
+        self.b = b = Signal((18, True), reset_less=True)
+        self.c = c = Signal((48, True), reset_less=True)
+        self.mux_p = mux_p = Signal()  # accumulator mux
+        self.m = m = Signal((48, True), reset_less=True)
+        self.p = p = Signal((48, True), reset_less=True)
+        self.sync += [m.eq(a * b), p.eq(m + c), If(mux_p, p.eq(m + p))]
+
+
+class Iir(Module):
+    def __init__(self, w_coeff, w_data, log2_a0, n_profiles, n_channels):
+        # input strobe signal (start processing all channels)
+        self.stb_in = stb_in = Signal()
+        self.stb_out = stb_out = Signal()  # output strobe signal (all channels done)
+        self.inp = inp = Array(Signal((w_data, True)) for _ in range(n_channels))
+        self.outp = outp = Array(Signal((w_data, True)) for _ in range(n_channels))
+        # coeff registers for all channels and profiles
+        self.coeff = coeff = Array(
+            Array(
+                Array(Signal((w_coeff, True)) for _ in range(n_channels))
+                for _ in range(n_profiles)
+            )
+            for _ in range(N_COEFF)
+        )
+        self.offset = offset = Array(
+            Array(Signal((w_data, True)) for _ in range(n_channels))
+            for _ in range(n_profiles)
+        )
+        # registers for selected profile for channel
+        self.ch_profile = ch_profile = Array(
+            Signal(max=n_profiles + 1) for _ in range(n_channels)
+        )
+        # output hold signal for each channel
+        self.hold = hold = Array(Signal() for _ in range(n_channels))
+
+        ###
+
+        # Making these registers reset less results in worsend timing.
+        # y1 register unique for each profile
+        y1 = Array(
+            Array(Signal((w_data, True)) for _ in range(n_channels))
+            for _ in range(n_profiles)
+        )
+        # x0, x1 registers shared for all profiles
+        x = Array(
+            Array(Signal((w_data, True)) for _ in range(n_channels))
+            for _ in range(N_COEFF - 1)
+        )
+        y0_clipped = Signal((w_data, True))
+        profile_index = Signal(max=n_profiles + 1)
+        channel_index = Signal(max=n_channels + 1)
+        busy = Signal()
+        # computation steps/pipeline:
+        # 0    -> load coeff[0],xy[0]
+        # 1    -> load coeff[1],xy[1], m0=coeff[0]*xy[0]
+        # 2    -> load coeff[2],xy[2], m1=coeff[1]*xy[1], p0=offset+m0
+        # 1(3) ->                      m2=coeff[2]*xy[2], p1=p0+m1
+        # 2(4) ->                                         p2=p1+m2
+        # 3(5) ->                                                      retrieve data y0=clip(p2)?hold
+        step = Signal(2)  # computation step
+        ch_profile_last_ch = Signal(max=n_profiles + 1)  # auxillary signal for muxing
+        self.submodules.dsp = dsp = Dsp()
+        assert w_data <= len(dsp.b)
+        assert w_coeff <= len(dsp.a)
+        shift_c = len(dsp.a) + len(dsp.b) - w_data - (w_data - log2_a0)
+        shift_a = len(dsp.a) - w_coeff
+        shift_b = len(dsp.b) - w_data
+        # +1 from standard sign bit
+        n_sign = len(dsp.p) - len(dsp.a) - len(dsp.b) + w_data - log2_a0 + 1
+        c_rounding_offset = Constant((1 << shift_c - 1) - 1, shift_c)
+
+        self.sync += [
+            # default to 0 and set to 1 further down if computation done in this cycle
+            stb_out.eq(0),
+            dsp.a.eq(coeff[step][profile_index][channel_index] << shift_a),
+            dsp.b.eq(
+                x[channel_index][step] << shift_b
+            ),  # overwritten later if at step==2
+            dsp.c.eq(Cat(c_rounding_offset, offset[profile_index][channel_index])),
+            If(
+                stb_in & ~busy,
+                busy.eq(1),
+                profile_index.eq(ch_profile[channel_index]),
+                [xi[0].eq(i) for xi, i in zip(x, inp)],
+            ),
+            If(
+                busy,
+                step.eq(step + 1),
+                If(step == 1, dsp.mux_p.eq(0)),
+                If(
+                    step == 2,
+                    dsp.mux_p.eq(1),
+                    step.eq(0),
+                    channel_index.eq(channel_index + 1),
+                    profile_index.eq(ch_profile[channel_index + 1]),
+                    dsp.b.eq(y1[profile_index][channel_index] << shift_b),
+                    If(
+                        (channel_index != 0)
+                        & (channel_index != n_channels + 1)
+                        & ~hold[channel_index - 1],
+                        y1[ch_profile_last_ch][channel_index - 1].eq(y0_clipped),
+                    ),
+                ),
+            ),
+            # if done with all channels and last data is done
+            If(
+                (channel_index == n_channels) & (step == 2),
+                channel_index.eq(0),
+                profile_index.eq(ch_profile[0]),
+                busy.eq(0),
+                stb_out.eq(1),
+                [xi[1].eq(xi[0]) for xi in x],
+            ),
+        ]
+        self.comb += [
+            # assign extra signal for xy adressing
+            ch_profile_last_ch.eq(ch_profile[channel_index - 1]),
+            [o.eq(y1[ch_profile[ch]][ch]) for ch, o in enumerate(outp)],
+            # clipping to positive output range
+            y0_clipped.eq(dsp.p >> shift_c),
+            If(
+                dsp.p[-n_sign:] != 0,  # if out of output range
+                y0_clipped.eq((1 << w_data - 1) - 1),
+            ),
+            If(dsp.p[-1] != 0, y0_clipped.eq(0)),  # if negative
+        ]