forked from M-Labs/artiq
parent
4fe09fddd5
commit
934c41b90a
|
|
@ -0,0 +1,85 @@
|
|||
from migen import *
|
||||
|
||||
from artiq.gateware.rtio import rtlink
|
||||
|
||||
|
||||
class RTServoCtrl(Module):
|
||||
"""Per channel RTIO control interface"""
|
||||
def __init__(self, ctrl):
|
||||
self.rtlink = rtlink.Interface(
|
||||
rtlink.OInterface(len(ctrl)))
|
||||
|
||||
# # #
|
||||
|
||||
self.sync.rio += [
|
||||
If(self.rtlink.o.stb,
|
||||
Cat(ctrl.profile, ctrl.en_out, ctrl.en_iir).eq(
|
||||
self.rtlink.o.data),
|
||||
)
|
||||
]
|
||||
self.comb += [
|
||||
ctrl.stb.eq(self.rtlink.o.stb)
|
||||
]
|
||||
|
||||
|
||||
class RTServoMem(Module):
|
||||
"""All-channel all-profile coefficient and state RTIO control
|
||||
interface."""
|
||||
def __init__(self, w, servo):
|
||||
m_coeff = servo.m_coeff.get_port(write_capable=True,
|
||||
we_granularity=w.coeff)
|
||||
assert len(m_coeff.we) == 2
|
||||
m_state = servo.m_state.get_port(write_capable=True)
|
||||
self.specials += m_state, m_coeff
|
||||
|
||||
assert w.coeff >= w.state
|
||||
assert w.coeff >= w.word
|
||||
|
||||
self.rtlink = rtlink.Interface(
|
||||
rtlink.OInterface(
|
||||
w.coeff,
|
||||
# coeff, profile, channel, 2 mems, rw
|
||||
3 + w.profile + w.channel + 1 + 1,
|
||||
enable_replace=False),
|
||||
rtlink.IInterface(
|
||||
w.coeff,
|
||||
timestamped=False)
|
||||
)
|
||||
|
||||
# # #
|
||||
|
||||
we = self.rtlink.o.address[-1]
|
||||
state_sel = self.rtlink.o.address[-2]
|
||||
high_coeff = self.rtlink.o.address[0]
|
||||
self.comb += [
|
||||
self.rtlink.o.busy.eq(active),
|
||||
m_coeff.adr.eq(self.rtlink.o.address[1:]),
|
||||
m_coeff.dat_w.eq(Cat(self.rtlink.o.data, self.rtlink.o.data)),
|
||||
m_coeff.we[0].eq(self.rtlink.o.stb & ~high_coeff &
|
||||
we & ~state_sel),
|
||||
m_coeff.we[1].eq(self.rtlink.o.stb & high_coeff &
|
||||
we & ~state_sel),
|
||||
m_state.adr.eq(self.rtlink.o.address),
|
||||
m_state.dat_w.eq(self.rtlink.o.data << w.state - w.coeff),
|
||||
m_state.we.eq(self.rtlink.o.stb & we & state_sel),
|
||||
]
|
||||
read = Signal()
|
||||
read_sel = Signal()
|
||||
read_high = Signal()
|
||||
self.sync.rio += [
|
||||
If(read,
|
||||
read.eq(0)
|
||||
),
|
||||
If(self.rtlink.o.stb & ~we,
|
||||
read.eq(1),
|
||||
read_sel.eq(state_sel),
|
||||
read_high.eq(high_coeff),
|
||||
)
|
||||
]
|
||||
self.comb += [
|
||||
self.rtlink.i.stb.eq(read),
|
||||
self.rtlink.i.data.eq(Mux(state_sel,
|
||||
m_state.dat_r >> w.state - w.coeff,
|
||||
Mux(read_high, m_coeff.dat_r[w.coeff:],
|
||||
m_coeff.dat_r)))
|
||||
]
|
||||
|
|
@ -0,0 +1,139 @@
|
|||
import logging
|
||||
import string
|
||||
from collections import namedtuple
|
||||
|
||||
from migen import *
|
||||
from migen.genlib import io
|
||||
|
||||
from .tools import DiffMixin
|
||||
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
# all times in cycles
|
||||
ADCParams = namedtuple("ADCParams", [
|
||||
"channels", # number of channels
|
||||
"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.
|
||||
# this avoids having synchronizers and another counter
|
||||
# to signal end-of transfer (CLKOUT cycles)
|
||||
# and it ensures fixed latency early in the pipeline
|
||||
])
|
||||
|
||||
|
||||
class ADC(Module, DiffMixin):
|
||||
"""Multi-lane, multi-channel, triggered, source-synchronous, serial
|
||||
ADC interface.
|
||||
|
||||
* Supports ADCs like the LTC2320-16.
|
||||
* Hardcoded timings.
|
||||
"""
|
||||
def __init__(self, pads, params):
|
||||
self.params = p = params # ADCParams
|
||||
self.data = [Signal((p.width, True), reset_less=True)
|
||||
for i 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
|
||||
|
||||
###
|
||||
|
||||
# collect sdo lines
|
||||
sdo = []
|
||||
for i in string.ascii_lowercase[:p.lanes]:
|
||||
sdo.append(self._diff(pads, "sdo" + i))
|
||||
assert p.lanes == len(sdo)
|
||||
|
||||
# set up counters for the four states CNVH, CONV, READ, RTT
|
||||
t_read = p.width*p.channels//p.lanes//2 # DDR
|
||||
assert 2*p.lanes*t_read == p.width*p.channels
|
||||
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) - 1,
|
||||
reset_less=True)
|
||||
count_load = Signal.like(count)
|
||||
count_done = Signal()
|
||||
self.comb += [
|
||||
count_done.eq(count == 0),
|
||||
]
|
||||
self.sync += [
|
||||
count.eq(count - 1),
|
||||
If(count_done,
|
||||
count.eq(count_load),
|
||||
)
|
||||
]
|
||||
|
||||
sck_en = Signal()
|
||||
if hasattr(pads, "sck_en"):
|
||||
self.sync += pads.sck_en.eq(sck_en) # ODDR delay
|
||||
self.specials += io.DDROutput(0, sck_en,
|
||||
self._diff(pads, "sck", output=True))
|
||||
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 - 2), # account for sck ODDR delay
|
||||
pads.cnv_b.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), # account for sck ODDR delay
|
||||
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,
|
||||
NextState("IDLE")
|
||||
)
|
||||
)
|
||||
|
||||
try:
|
||||
sck_en_ret = pads.sck_en_ret
|
||||
except AttributeError:
|
||||
sck_en_ret = 1
|
||||
self.clock_domains.cd_ret = ClockDomain("ret", reset_less=True)
|
||||
self.comb += [
|
||||
# falling clkout makes two bits available
|
||||
self.cd_ret.clk.eq(~self._diff(pads, "clkout")),
|
||||
]
|
||||
k = p.channels//p.lanes
|
||||
assert 2*t_read == k*p.width
|
||||
for i, sdo in enumerate(sdo):
|
||||
sdo_sr = Signal(2*t_read - 2)
|
||||
sdo_ddr = Signal(2)
|
||||
self.specials += io.DDRInput(sdo, sdo_ddr[1], sdo_ddr[0],
|
||||
self.cd_ret.clk)
|
||||
self.sync.ret += [
|
||||
If(self.reading & sck_en_ret,
|
||||
sdo_sr.eq(Cat(sdo_ddr, sdo_sr))
|
||||
)
|
||||
]
|
||||
self.comb += [
|
||||
Cat(reversed([self.data[i*k + j] for j in range(k)])).eq(
|
||||
Cat(sdo_ddr, sdo_sr))
|
||||
]
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
import logging
|
||||
|
||||
from migen import *
|
||||
|
||||
from . import spi
|
||||
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
DDSParams = spi.SPIParams
|
||||
|
||||
|
||||
class DDS(spi.SPISimple):
|
||||
"""Multi-DDS SPI interface.
|
||||
|
||||
* Supports SPI DDS chips like the AD9910.
|
||||
* Shifts data out to multiple DDS in parallel with a shared CLK and shared
|
||||
CS_N line.
|
||||
* Supports a single hardcoded command.
|
||||
* Configuration and setup must be done over a different channel.
|
||||
* Asserts IO_UPDATE for one clock cycle immediately after the SPI transfer.
|
||||
"""
|
||||
def __init__(self, pads, params):
|
||||
super().__init__(pads, params)
|
||||
|
||||
self.profile = [Signal(32 + 16 + 16, reset_less=True)
|
||||
for i in range(params.channels)]
|
||||
cmd = Signal(8, reset=0x0e) # write to single tone profile 0
|
||||
assert params.width == len(cmd) + len(self.profile[0])
|
||||
|
||||
self.sync += [
|
||||
If(self.start,
|
||||
[d.eq(Cat(p, cmd))
|
||||
for d, p in zip(self.data, self.profile)]
|
||||
)
|
||||
]
|
||||
|
||||
io_update = self._diff(pads, "io_update", output=True)
|
||||
# this assumes that the cycle time (1/125 MHz = 8 ns) is >1 SYNC_CLK
|
||||
# cycle (1/250 MHz = 4ns)
|
||||
done = Signal()
|
||||
self.sync += [
|
||||
done.eq(self.done)
|
||||
]
|
||||
self.comb += [
|
||||
io_update.eq(self.done & ~done)
|
||||
]
|
||||
|
|
@ -0,0 +1,677 @@
|
|||
from collections import namedtuple
|
||||
import logging
|
||||
|
||||
from migen import *
|
||||
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
# all these are number of bits!
|
||||
IIRWidths = namedtuple("IIRWidths", [
|
||||
"state", # the signed x and y states of the IIR filter
|
||||
# DSP A input, x state is one bit smaller
|
||||
# due to AD pre-adder, y has full width (25)
|
||||
"coeff", # signed IIR filter coefficients a1, b0, b1 (18)
|
||||
"accu", # IIR accumulator width (48)
|
||||
"adc", # signed ADC data (16)
|
||||
"word", # "word" size to break up DDS profile data (16)
|
||||
"asf", # unsigned amplitude scale factor for DDS (14)
|
||||
"shift", # fixed point scaling coefficient for a1, b0, b1 (log2!) (11)
|
||||
"channel", # channels (log2!) (3)
|
||||
"profile", # profiles per channel (log2!) (5)
|
||||
])
|
||||
|
||||
|
||||
def signed(v, w):
|
||||
"""Convert an unsigned integer ``v`` to it's signed value assuming ``w``
|
||||
bits"""
|
||||
assert 0 <= v < (1 << w)
|
||||
if v & (1 << w - 1):
|
||||
v -= 1 << w
|
||||
return v
|
||||
|
||||
|
||||
class DSP(Module):
|
||||
"""Thin abstraction of DSP functionality used here, commonly present,
|
||||
and inferrable in FPGAs: multiplier with pre-adder and post-accumulator
|
||||
and pipeline registers at every stage."""
|
||||
def __init__(self, w, signed_output=False):
|
||||
self.state = Signal((w.state, True))
|
||||
# NOTE:
|
||||
# If offset is non-zero, care must be taken to ensure that the
|
||||
# offset-state difference does not overflow the width of the ad factor
|
||||
# which is also w.state.
|
||||
self.offset = Signal((w.state, True))
|
||||
self.coeff = Signal((w.coeff, True))
|
||||
self.output = Signal((w.state, True))
|
||||
self.accu_clr = Signal()
|
||||
self.offset_load = Signal()
|
||||
self.clip = Signal()
|
||||
|
||||
a = Signal((w.state, True), reset_less=True)
|
||||
d = Signal((w.state, True), reset_less=True)
|
||||
ad = Signal((w.state, True), reset_less=True)
|
||||
b = Signal((w.coeff, True), reset_less=True)
|
||||
m = Signal((w.accu, True), reset_less=True)
|
||||
p = Signal((w.accu, True), reset_less=True)
|
||||
|
||||
self.sync += [
|
||||
a.eq(self.state),
|
||||
If(self.offset_load,
|
||||
d.eq(self.offset)
|
||||
),
|
||||
ad.eq(d - a),
|
||||
b.eq(self.coeff),
|
||||
m.eq(ad*b),
|
||||
p.eq(p + m),
|
||||
If(self.accu_clr,
|
||||
# inject symmetric rouding constant
|
||||
# p.eq(1 << (w.shift - 1))
|
||||
# but that won't infer P reg, so we just clear
|
||||
# and round down
|
||||
p.eq(0),
|
||||
)
|
||||
]
|
||||
# Bit layout (LSB-MSB): w.shift | w.state - 1 | n_sign - 1 | 1 (sign)
|
||||
n_sign = w.accu - w.state - w.shift + 1
|
||||
assert n_sign > 1
|
||||
|
||||
# clipping
|
||||
if signed_output:
|
||||
self.comb += [
|
||||
self.clip.eq(p[-n_sign:] != Replicate(p[-1], n_sign)),
|
||||
self.output.eq(Mux(self.clip,
|
||||
Cat(Replicate(~p[-1], w.state - 1), p[-1]),
|
||||
p[w.shift:]))
|
||||
]
|
||||
else:
|
||||
self.comb += [
|
||||
self.clip.eq(p[-n_sign:] != 0),
|
||||
self.output.eq(Mux(self.clip,
|
||||
Replicate(~p[-1], w.state - 1),
|
||||
p[w.shift:]))
|
||||
]
|
||||
|
||||
|
||||
class IIR(Module):
|
||||
"""Pipelined IIR processor.
|
||||
|
||||
This module implements a multi-channel IIR (infinite impulse response)
|
||||
filter processor optimized for synthesis on FPGAs.
|
||||
|
||||
The module is parametrized by passing a ``IIRWidths()`` object which
|
||||
will be abbreviated W here.
|
||||
|
||||
It reads 1 << W.channels input channels (typically from an ADC)
|
||||
and on each iteration processes the data on using a first-order IIR filter.
|
||||
At the end of the cycle each the output of the filter together with
|
||||
additional data (typically frequency tunning word and phase offset word
|
||||
for a DDS) are presented at the 1 << W.channels outputs of the module.
|
||||
|
||||
Profile memory
|
||||
==============
|
||||
|
||||
Each channel can operate using any of its 1 << W.profile profiles.
|
||||
The profile data consists of the input ADC channel index (SEL), a delay
|
||||
(DLY) for delayed activation of the IIR updates, the three IIR
|
||||
coefficients (A1, B0, B1), the input offset (OFFSET), and additional data
|
||||
(FTW0, FTW1, and POW). Profile data is stored in a dual-port block RAM that
|
||||
can be accessed externally.
|
||||
|
||||
Memory Layout
|
||||
-------------
|
||||
|
||||
The profile data is stored sequentially for each channel.
|
||||
Each channel has 1 << W.profile profiles available.
|
||||
Each profile stores 8 values, each up to W.coeff bits wide, arranged as:
|
||||
[FTW1, B1, POW, CFG, OFFSET, A1, FTW0, B0]
|
||||
The lower 8 bits of CFG hold the ADC input channel index SEL.
|
||||
The bits from bit 8 up hold the IIR activation delay DLY.
|
||||
The back memory is 2*W.coeff bits wide and each value pair
|
||||
(even and odd address)
|
||||
are stored in a single location with the odd address value occupying the
|
||||
high bits.
|
||||
|
||||
State memory
|
||||
============
|
||||
|
||||
The filter state consists of the previous ADC input values X1,
|
||||
the current ADC input values X0 and the previous output values
|
||||
of the IIR filter (Y1). The filter
|
||||
state is stored in a dual-port block RAM that can be accessed
|
||||
externally.
|
||||
|
||||
Memory Layout
|
||||
-------------
|
||||
|
||||
The state memory holds all Y1 values (IIR processor outputs) for all
|
||||
profiles of all channels in the lower half (1 << W.profile + W.channel
|
||||
addresses) and the pairs of old and new ADC input values X1, and X0,
|
||||
in the upper half (1 << W.channel addresses). Each memory location is
|
||||
W.state bits wide.
|
||||
|
||||
Real-time control
|
||||
=================
|
||||
|
||||
Signals are exposed for each channel:
|
||||
|
||||
* The active profile, PROFILE
|
||||
* Whether to perform IIR filter iterations, EN_IIR
|
||||
* The RF switch state enabling output from the channel, EN_OUT
|
||||
|
||||
Delayed IIR processing
|
||||
======================
|
||||
|
||||
The IIR filter iterations on a given channel are only performed all of the
|
||||
following are true:
|
||||
|
||||
* PROFILE, EN_IIR, EN_OUT have not been updated in the within the
|
||||
last DLY cycles
|
||||
* EN_IIR is asserted
|
||||
* EN_OUT is asserted
|
||||
|
||||
DSP design
|
||||
==========
|
||||
|
||||
Typical design at the DSP level. This does not include the description of
|
||||
the pipelining or the overall latency involved.
|
||||
|
||||
IIRWidths(state=25, coeff=18, adc=16,
|
||||
asf=14, word=16, accu=48, shift=11,
|
||||
channel=3, profile=5)
|
||||
|
||||
X0 = ADC * 2^(25 - 1 - 16)
|
||||
X1 = X0 delayed by one cycle
|
||||
A0 = 2^11
|
||||
A0*Y0 = A1*Y1 - B0*(X0 - OFFSET) - B1*(X1 - OFFSET)
|
||||
Y1 = Y0 delayed by one cycle
|
||||
ASF = Y0 / 2^(25 - 14 - 1)
|
||||
|
||||
ADC: input value from the ADC
|
||||
ASF: output amplitude scale factor to DDS
|
||||
OFFSET: setpoint
|
||||
A0: fixed factor
|
||||
A1/B0/B1: coefficients
|
||||
|
||||
B0 --/- A0: 2^11
|
||||
18 | |
|
||||
ADC -/-[<<]-/-(-)-/---(x)-(+)-/-[>>]-/-[_/^]-/---[>>]-/- ASF
|
||||
16 8 24 | 25 | | 48 11 37 25 | 10 15
|
||||
OFFSET --/- [z^-1] ^ [z^-1]
|
||||
24 | | |
|
||||
-(x)-(+)-<-(x)-----<------
|
||||
| |
|
||||
B1 --/- A1 --/-
|
||||
18 18
|
||||
|
||||
[<<]: left shift, multiply by 2^n
|
||||
[>>]: right shift, divide by 2^n
|
||||
(x): multiplication
|
||||
(+), (-): addition, subtraction
|
||||
[_/^]: clip
|
||||
[z^-1]: register, delay by one processing cycle (~1.1 µs)
|
||||
--/--: signal with a given bit width always includes a sign bit
|
||||
-->--: flow is to the right and down unless otherwise indicated
|
||||
"""
|
||||
def __init__(self, w):
|
||||
self.widths = w
|
||||
for i, j in enumerate(w):
|
||||
assert j > 0, (i, j, w)
|
||||
assert w.word <= w.coeff # same memory
|
||||
assert w.state + w.coeff + 3 <= w.accu
|
||||
|
||||
# m_coeff of active profiles should only be accessed during
|
||||
# ~processing
|
||||
self.specials.m_coeff = Memory(
|
||||
width=2*w.coeff, # Cat(pow/ftw/offset, cfg/a/b)
|
||||
depth=4 << w.profile + w.channel)
|
||||
# m_state[x] should only be read during ~(shifting |
|
||||
# loading)
|
||||
# m_state[y] of active profiles should only be read during
|
||||
# ~processing
|
||||
self.specials.m_state = Memory(
|
||||
width=w.state, # y1,x0,x1
|
||||
depth=(1 << w.profile + w.channel) + (2 << w.channel))
|
||||
# ctrl should only be updated synchronously
|
||||
self.ctrl = [Record([
|
||||
("profile", w.profile),
|
||||
("en_out", 1),
|
||||
("en_iir", 1),
|
||||
("stb", 1)])
|
||||
for i in range(1 << w.channel)]
|
||||
# only update during ~loading
|
||||
self.adc = [Signal((w.adc, True), reset_less=True)
|
||||
for i in range(1 << w.channel)]
|
||||
# Cat(ftw0, ftw1, pow, asf)
|
||||
# only read during ~processing
|
||||
self.dds = [Signal(4*w.word, reset_less=True)
|
||||
for i in range(1 << w.channel)]
|
||||
# perform one IIR iteration, start with loading,
|
||||
# then processing, then shifting, end with done
|
||||
self.start = Signal()
|
||||
# adc inputs being loaded into RAM (becoming x0)
|
||||
self.loading = Signal()
|
||||
# processing state data (extracting ftw0/ftw1/pow,
|
||||
# computing asf/y0, and storing as y1)
|
||||
self.processing = Signal()
|
||||
# shifting input state values around (x0 becomes x1)
|
||||
self.shifting = Signal()
|
||||
# iteration done, the next iteration can be started
|
||||
self.done = Signal()
|
||||
|
||||
###
|
||||
|
||||
# pivot arrays for muxing
|
||||
profiles = Array([ch.profile for ch in self.ctrl])
|
||||
en_outs = Array([ch.en_out for ch in self.ctrl])
|
||||
en_iirs = Array([ch.en_iir for ch in self.ctrl])
|
||||
|
||||
# state counter
|
||||
state = Signal(w.channel + 2)
|
||||
# pipeline group activity flags (SR)
|
||||
stage = Signal(3)
|
||||
self.submodules.fsm = fsm = FSM("IDLE")
|
||||
state_clr = Signal()
|
||||
stage_en = Signal()
|
||||
fsm.act("IDLE",
|
||||
self.done.eq(1),
|
||||
state_clr.eq(1),
|
||||
If(self.start,
|
||||
NextState("LOAD")
|
||||
)
|
||||
)
|
||||
fsm.act("LOAD",
|
||||
self.loading.eq(1),
|
||||
If(state == (1 << w.channel) - 1,
|
||||
state_clr.eq(1),
|
||||
stage_en.eq(1),
|
||||
NextState("PROCESS")
|
||||
)
|
||||
)
|
||||
fsm.act("PROCESS",
|
||||
self.processing.eq(1),
|
||||
# this is technically wasting three cycles
|
||||
# (one for setting stage, and phase=2,3 with stage[2])
|
||||
If(stage == 0,
|
||||
state_clr.eq(1),
|
||||
NextState("SHIFT")
|
||||
)
|
||||
)
|
||||
fsm.act("SHIFT",
|
||||
self.shifting.eq(1),
|
||||
If(state == (2 << w.channel) - 1,
|
||||
NextState("IDLE")
|
||||
)
|
||||
)
|
||||
|
||||
self.sync += [
|
||||
state.eq(state + 1),
|
||||
If(state_clr,
|
||||
state.eq(0),
|
||||
),
|
||||
If(stage_en,
|
||||
stage[0].eq(1)
|
||||
)
|
||||
]
|
||||
|
||||
# pipeline group channel pointer
|
||||
# for each pipeline stage, this is the channel currently being
|
||||
# processed
|
||||
channel = [Signal(w.channel, reset_less=True) for i in range(3)]
|
||||
# pipeline group profile pointer (SR)
|
||||
# for each pipeline stage, this is the profile currently being
|
||||
# processed
|
||||
profile = [Signal(w.profile, reset_less=True) for i in range(2)]
|
||||
# pipeline phase (lower two bits of state)
|
||||
phase = Signal(2, reset_less=True)
|
||||
|
||||
self.comb += Cat(phase, channel[0]).eq(state)
|
||||
self.sync += [
|
||||
Case(phase, {
|
||||
0: [
|
||||
profile[0].eq(profiles[channel[0]]),
|
||||
profile[1].eq(profile[0])
|
||||
],
|
||||
3: [
|
||||
Cat(channel[1:]).eq(Cat(channel[:-1])),
|
||||
stage[1:].eq(stage[:-1]),
|
||||
If(channel[0] == (1 << w.channel) - 1,
|
||||
stage[0].eq(0)
|
||||
)
|
||||
]
|
||||
})
|
||||
]
|
||||
|
||||
m_coeff = self.m_coeff.get_port()
|
||||
m_state = self.m_state.get_port(write_capable=True)
|
||||
self.specials += m_state, m_coeff
|
||||
|
||||
dsp = DSP(w)
|
||||
self.submodules += dsp
|
||||
|
||||
offset_clr = Signal()
|
||||
|
||||
self.comb += [
|
||||
m_coeff.adr.eq(Cat(phase, profile[0],
|
||||
Mux(phase==0, channel[1], channel[0]))),
|
||||
dsp.offset[-w.coeff - 1:].eq(Mux(offset_clr, 0,
|
||||
Cat(m_coeff.dat_r[:w.coeff], m_coeff.dat_r[w.coeff - 1])
|
||||
)),
|
||||
dsp.coeff.eq(m_coeff.dat_r[w.coeff:]),
|
||||
dsp.state.eq(m_state.dat_r),
|
||||
Case(phase, {
|
||||
0: dsp.accu_clr.eq(1),
|
||||
2: [
|
||||
offset_clr.eq(1),
|
||||
dsp.offset_load.eq(1)
|
||||
],
|
||||
3: dsp.offset_load.eq(1)
|
||||
})
|
||||
]
|
||||
|
||||
# selected adc (combinatorial from dat_r)
|
||||
sel_profile = Signal(w.channel)
|
||||
# profile delay (combinatorial from dat_r)
|
||||
dly_profile = Signal(8)
|
||||
|
||||
# latched adc selection
|
||||
sel = Signal(w.channel, reset_less=True)
|
||||
# iir enable SR
|
||||
en = Signal(2, reset_less=True)
|
||||
|
||||
assert w.channel <= 8
|
||||
assert w.profile <= len(dly_profile)
|
||||
assert w.profile + 8 <= len(m_coeff.dat_r)
|
||||
|
||||
self.comb += [
|
||||
sel_profile.eq(m_coeff.dat_r[w.coeff:]),
|
||||
dly_profile.eq(m_coeff.dat_r[w.coeff + 8:]),
|
||||
If(self.shifting,
|
||||
m_state.adr.eq(state | (1 << w.profile + w.channel)),
|
||||
m_state.dat_w.eq(m_state.dat_r),
|
||||
m_state.we.eq(state[0])
|
||||
),
|
||||
If(self.loading,
|
||||
m_state.adr.eq((state << 1) | (1 << w.profile + w.channel)),
|
||||
m_state.dat_w[-w.adc - 1:-1].eq(Array(self.adc)[state]),
|
||||
m_state.dat_w[-1].eq(m_state.dat_w[-2]),
|
||||
m_state.we.eq(1)
|
||||
),
|
||||
If(self.processing,
|
||||
m_state.adr.eq(Array([
|
||||
# write back new y
|
||||
Cat(profile[1], channel[2]),
|
||||
# read old y
|
||||
Cat(profile[0], channel[0]),
|
||||
# x0 (recent)
|
||||
0 | (sel_profile << 1) | (1 << w.profile + w.channel),
|
||||
# x1 (old)
|
||||
1 | (sel << 1) | (1 << w.profile + w.channel),
|
||||
])[phase]),
|
||||
m_state.dat_w.eq(dsp.output),
|
||||
m_state.we.eq((phase == 0) & stage[2] & en[1]),
|
||||
)
|
||||
]
|
||||
|
||||
# internal channel delay counters
|
||||
dlys = Array([Signal(len(dly_profile))
|
||||
for i in range(1 << w.channel)])
|
||||
self._dlys = dlys # expose for debugging only
|
||||
|
||||
for i in range(1 << w.channel):
|
||||
self.sync += [
|
||||
# (profile != profile_old) | ~en_out
|
||||
If(self.ctrl[i].stb,
|
||||
dlys[i].eq(0),
|
||||
)
|
||||
]
|
||||
|
||||
# latched channel delay
|
||||
dly = Signal(len(dly_profile), reset_less=True)
|
||||
# latched channel en_out
|
||||
en_out = Signal(reset_less=True)
|
||||
# latched channel en_iir
|
||||
en_iir = Signal(reset_less=True)
|
||||
# muxing
|
||||
ddss = Array(self.dds)
|
||||
|
||||
self.sync += [
|
||||
Case(phase, {
|
||||
0: [
|
||||
dly.eq(dlys[channel[0]]),
|
||||
en_out.eq(en_outs[channel[0]]),
|
||||
en_iir.eq(en_iirs[channel[0]]),
|
||||
If(stage[1],
|
||||
ddss[channel[1]][:w.word].eq(
|
||||
m_coeff.dat_r),
|
||||
)
|
||||
],
|
||||
1: [
|
||||
If(stage[1],
|
||||
ddss[channel[1]][w.word:2*w.word].eq(
|
||||
m_coeff.dat_r),
|
||||
),
|
||||
If(stage[2],
|
||||
ddss[channel[2]][3*w.word:].eq(
|
||||
m_state.dat_r[w.state - w.asf - 1:w.state - 1])
|
||||
)
|
||||
],
|
||||
2: [
|
||||
en[0].eq(0),
|
||||
en[1].eq(en[0]),
|
||||
sel.eq(sel_profile),
|
||||
If(stage[0],
|
||||
ddss[channel[0]][2*w.word:3*w.word].eq(
|
||||
m_coeff.dat_r),
|
||||
If(en_out,
|
||||
If(dly != dly_profile,
|
||||
dlys[channel[0]].eq(dly + 1)
|
||||
).Elif(en_iir,
|
||||
en[0].eq(1)
|
||||
)
|
||||
)
|
||||
)
|
||||
],
|
||||
3: [
|
||||
],
|
||||
}),
|
||||
]
|
||||
|
||||
def _coeff(self, channel, profile, coeff):
|
||||
"""Return ``high_word``, ``address`` and bit ``mask`` for the
|
||||
storage of coefficient name ``coeff`` in profile ``profile``
|
||||
of channel ``channel``.
|
||||
|
||||
``high_word`` determines whether the coefficient is stored in the high
|
||||
or low part of the memory location.
|
||||
"""
|
||||
w = self.widths
|
||||
addr = "ftw1 b1 pow cfg offset a1 ftw0 b0".split().index(coeff)
|
||||
coeff_addr = ((channel << w.profile + 2) | (profile << 2) |
|
||||
(addr >> 1))
|
||||
mask = (1 << w.coeff) - 1
|
||||
return addr & 1, coeff_addr, mask
|
||||
|
||||
def set_coeff(self, channel, profile, coeff, value):
|
||||
"""Set the coefficient value.
|
||||
|
||||
Note that due to two coefficiddents sharing a single memory
|
||||
location, only one coefficient update can be effected to a given memory
|
||||
location per simulation clock cycle.
|
||||
"""
|
||||
w = self.widths
|
||||
word, addr, mask = self._coeff(channel, profile, coeff)
|
||||
val = yield self.m_coeff[addr]
|
||||
if word:
|
||||
val = (val & mask) | ((value & mask) << w.coeff)
|
||||
else:
|
||||
val = (value & mask) | (val & (mask << w.coeff))
|
||||
yield self.m_coeff[addr].eq(val)
|
||||
|
||||
def get_coeff(self, channel, profile, coeff):
|
||||
"""Get a coefficient value."""
|
||||
w = self.widths
|
||||
word, addr, mask = self._coeff(channel, profile, coeff)
|
||||
val = yield self.m_coeff[addr]
|
||||
if word:
|
||||
return val >> w.coeff
|
||||
else:
|
||||
return val & mask
|
||||
if val in "offset a1 b0 b1".split():
|
||||
val = signed(val, w.coeff)
|
||||
return val
|
||||
|
||||
def set_state(self, channel, val, profile=None, coeff="y1"):
|
||||
"""Set a state value."""
|
||||
w = self.widths
|
||||
if coeff == "y1":
|
||||
assert profile is not None
|
||||
yield self.m_state[profile | (channel << w.profile)].eq(val)
|
||||
elif coeff == "x0":
|
||||
assert profile is None
|
||||
yield self.m_state[(channel << 1) |
|
||||
(1 << w.profile + w.channel)].eq(val)
|
||||
elif coeff == "x1":
|
||||
assert profile is None
|
||||
yield self.m_state[1 | (channel << 1) |
|
||||
(1 << w.profile + w.channel)].eq(val)
|
||||
else:
|
||||
raise ValueError("no such state", coeff)
|
||||
|
||||
def get_state(self, channel, profile=None, coeff="y1"):
|
||||
"""Get a state value."""
|
||||
w = self.widths
|
||||
if coeff == "y1":
|
||||
val = yield self.m_state[profile | (channel << w.profile)]
|
||||
elif coeff == "x0":
|
||||
val = yield self.m_state[(channel << 1) |
|
||||
(1 << w.profile + w.channel)]
|
||||
elif coeff == "x1":
|
||||
val = yield self.m_state[1 | (channel << 1) |
|
||||
(1 << w.profile + w.channel)]
|
||||
else:
|
||||
raise ValueError("no such state", coeff)
|
||||
return signed(val, w.state)
|
||||
|
||||
def fast_iter(self):
|
||||
"""Perform a single processing iteration."""
|
||||
assert (yield self.done)
|
||||
yield self.start.eq(1)
|
||||
yield
|
||||
yield self.start.eq(0)
|
||||
yield
|
||||
while not (yield self.done):
|
||||
yield
|
||||
|
||||
def check_iter(self):
|
||||
"""Perform a single processing iteration while verifying
|
||||
the behavior."""
|
||||
w = self.widths
|
||||
|
||||
while not (yield self.done):
|
||||
yield
|
||||
|
||||
yield self.start.eq(1)
|
||||
yield
|
||||
yield self.start.eq(0)
|
||||
yield
|
||||
assert not (yield self.done)
|
||||
assert (yield self.loading)
|
||||
while (yield self.loading):
|
||||
yield
|
||||
|
||||
x0s = []
|
||||
# check adc loading
|
||||
for i in range(1 << w.channel):
|
||||
v_adc = signed((yield self.adc[i]), w.adc)
|
||||
x0 = yield from self.get_state(i, coeff="x0")
|
||||
x0s.append(x0)
|
||||
assert v_adc << (w.state - w.adc - 1) == x0, (hex(v_adc), hex(x0))
|
||||
logger.debug("adc[%d] adc=%x x0=%x", i, v_adc, x0)
|
||||
|
||||
data = []
|
||||
# predict output
|
||||
for i in range(1 << w.channel):
|
||||
j = yield self.ctrl[i].profile
|
||||
en_iir = yield self.ctrl[i].en_iir
|
||||
en_out = yield self.ctrl[i].en_out
|
||||
dly_i = yield self._dlys[i]
|
||||
logger.debug("ctrl[%d] profile=%d en_iir=%d en_out=%d dly=%d",
|
||||
i, j, en_iir, en_out, dly_i)
|
||||
|
||||
cfg = yield from self.get_coeff(i, j, "cfg")
|
||||
k_j = cfg & ((1 << w.channel) - 1)
|
||||
dly_j = (cfg >> 8) & 0xff
|
||||
logger.debug("cfg[%d,%d] sel=%d dly=%d", i, j, k_j, dly_j)
|
||||
|
||||
en = en_iir & en_out & (dly_i >= dly_j)
|
||||
logger.debug("en[%d,%d] %d", i, j, en)
|
||||
|
||||
offset = yield from self.get_coeff(i, j, "offset")
|
||||
offset <<= w.state - w.coeff - 1
|
||||
a1 = yield from self.get_coeff(i, j, "a1")
|
||||
b0 = yield from self.get_coeff(i, j, "b0")
|
||||
b1 = yield from self.get_coeff(i, j, "b1")
|
||||
logger.debug("coeff[%d,%d] offset=%#x a1=%#x b0=%#x b1=%#x",
|
||||
i, j, offset, a1, b0, b1)
|
||||
|
||||
ftw0 = yield from self.get_coeff(i, j, "ftw0")
|
||||
ftw1 = yield from self.get_coeff(i, j, "ftw1")
|
||||
pow = yield from self.get_coeff(i, j, "pow")
|
||||
logger.debug("dds[%d,%d] ftw0=%#x ftw1=%#x pow=%#x",
|
||||
i, j, ftw0, ftw1, pow)
|
||||
|
||||
y1 = yield from self.get_state(i, j, "y1")
|
||||
x1 = yield from self.get_state(k_j, coeff="x1")
|
||||
x0 = yield from self.get_state(k_j, coeff="x0")
|
||||
logger.debug("state y1[%d,%d]=%#x x0[%d]=%#x x1[%d]=%#x",
|
||||
i, j, y1, k_j, x0, k_j, x1)
|
||||
|
||||
p = (0*(1 << w.shift - 1) + a1*(0 - y1) +
|
||||
b0*(offset - x0) + b1*(offset - x1))
|
||||
out = p >> w.shift
|
||||
y0 = min(max(0, out), (1 << w.state - 1) - 1)
|
||||
logger.debug("dsp[%d,%d] p=%#x out=%#x y0=%#x",
|
||||
i, j, p, out, y0)
|
||||
|
||||
if not en:
|
||||
y0 = y1
|
||||
data.append((ftw0, ftw1, pow, y0, x1, x0))
|
||||
|
||||
# wait for output
|
||||
assert (yield self.processing)
|
||||
while (yield self.processing):
|
||||
yield
|
||||
|
||||
assert (yield self.shifting)
|
||||
while (yield self.shifting):
|
||||
yield
|
||||
|
||||
# check x shifting
|
||||
for i, x0 in enumerate(x0s):
|
||||
x1 = yield from self.get_state(i, coeff="x1")
|
||||
assert x1 == x0, (hex(x1), hex(x0))
|
||||
logger.debug("adc[%d] x0=%x x1=%x", i, x0, x1)
|
||||
|
||||
# check new state
|
||||
for i in range(1 << w.channel):
|
||||
j = yield self.ctrl[i].profile
|
||||
logger.debug("ch[%d] profile=%d", i, j)
|
||||
y1 = yield from self.get_state(i, j, "y1")
|
||||
ftw0, ftw1, pow, y0, x1, x0 = data[i]
|
||||
assert y1 == y0, (hex(y1), hex(y0))
|
||||
|
||||
# check dds output
|
||||
for i in range(1 << w.channel):
|
||||
ftw0, ftw1, pow, y0, x1, x0 = data[i]
|
||||
asf = y0 >> (w.state - w.asf - 1)
|
||||
dds = (ftw0 | (ftw1 << w.word) |
|
||||
(pow << 2*w.word) | (asf << 3*w.word))
|
||||
dds_state = yield self.dds[i]
|
||||
logger.debug("ch[%d] dds_state=%#x dds=%#x", i, dds_state, dds)
|
||||
assert dds_state == dds, [hex(_) for _ in
|
||||
(dds_state, asf, pow, ftw1, ftw0)]
|
||||
|
||||
assert (yield self.done)
|
||||
return data
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
from migen import *
|
||||
|
||||
from .adc_ser import ADC, ADCParams
|
||||
from .iir import IIR, IIRWidths
|
||||
from .dds_ser import DDS, DDSParams
|
||||
|
||||
|
||||
class Servo(Module):
|
||||
def __init__(self, adc_pads, dds_pads, adc_p, iir_p, dds_p):
|
||||
self.submodules.adc = ADC(adc_pads, adc_p)
|
||||
self.submodules.iir = IIR(iir_p)
|
||||
self.submodules.dds = DDS(dds_pads, dds_p)
|
||||
|
||||
for i, j, k, l in zip(self.adc.data, self.iir.adc,
|
||||
self.iir.dds, self.dds.profile):
|
||||
self.comb += j.eq(i), l.eq(k)
|
||||
|
||||
t_adc = (adc_p.t_cnvh + adc_p.t_conv + adc_p.t_rtt +
|
||||
adc_p.channels*adc_p.width//2//adc_p.lanes) + 1
|
||||
t_iir = ((1 + 4 + 1) << iir_p.channel) + 1
|
||||
t_dds = (dds_p.width*2 + 1)*dds_p.clk + 1
|
||||
t_cycle = max(t_adc, t_iir, t_dds)
|
||||
assert t_iir + (2 << iir_p.channel) < t_cycle, "need shifting time"
|
||||
|
||||
self.start = Signal()
|
||||
t_restart = t_cycle - t_iir - t_adc
|
||||
assert t_restart > 0
|
||||
cnt = Signal(max=t_restart)
|
||||
cnt_done = Signal()
|
||||
token = Signal(2)
|
||||
self.done = Signal()
|
||||
iir_done = Signal()
|
||||
self.comb += [
|
||||
cnt_done.eq(cnt == 0),
|
||||
iir_done.eq(self.iir.shifting | self.iir.done),
|
||||
self.adc.start.eq(self.start & cnt_done),
|
||||
self.iir.start.eq(token[0] & self.adc.done),
|
||||
self.dds.start.eq(token[1] & iir_done),
|
||||
self.done.eq(self.dds.done),
|
||||
]
|
||||
self.sync += [
|
||||
If(iir_done & ~cnt_done & ~token[0],
|
||||
cnt.eq(cnt - 1),
|
||||
),
|
||||
If(self.adc.start,
|
||||
cnt.eq(t_restart - 1),
|
||||
),
|
||||
If(self.adc.done,
|
||||
token[0].eq(0)
|
||||
),
|
||||
If(self.adc.start,
|
||||
token[0].eq(1)
|
||||
),
|
||||
If(iir_done,
|
||||
token[1].eq(0)
|
||||
),
|
||||
If(self.iir.start,
|
||||
token[1].eq(1)
|
||||
)
|
||||
]
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
import logging
|
||||
from collections import namedtuple
|
||||
|
||||
from migen import *
|
||||
from migen.genlib.fsm import FSM, NextState
|
||||
from migen.genlib import io
|
||||
|
||||
from .tools import DiffMixin
|
||||
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
# all times in cycles
|
||||
SPIParams = namedtuple("SPIParams", [
|
||||
"channels", # number of MOSI? data lanes
|
||||
"width", # transfer width
|
||||
"clk", # CLK half cycle width (in cycles)
|
||||
])
|
||||
|
||||
|
||||
class SPISimple(Module, DiffMixin):
|
||||
"""Simple reduced SPI interface.
|
||||
|
||||
* Multiple MOSI lines
|
||||
* Supports differential CLK/CS_N/MOSI
|
||||
* Fixed CLK timing
|
||||
* SPI MODE 0 (CPHA=0, CPOL=0)
|
||||
"""
|
||||
def __init__(self, pads, params):
|
||||
self.params = p = params
|
||||
self.data = [Signal(p.width, reset_less=True)
|
||||
for i in range(p.channels)] # data to be output, MSB first
|
||||
self.start = Signal() # start transfer
|
||||
self.done = Signal() # transfer complete, next transfer can be
|
||||
# started
|
||||
|
||||
###
|
||||
|
||||
assert p.clk >= 1
|
||||
|
||||
cs_n = self._diff(pads, "cs_n", output=True)
|
||||
|
||||
clk = self._diff(pads, "clk", output=True)
|
||||
cnt = Signal(max=max(2, p.clk), reset_less=True)
|
||||
cnt_done = Signal()
|
||||
cnt_next = Signal()
|
||||
self.comb += cnt_done.eq(cnt == 0)
|
||||
self.sync += [
|
||||
If(cnt_done,
|
||||
If(cnt_next,
|
||||
cnt.eq(p.clk - 1)
|
||||
)
|
||||
).Else(
|
||||
cnt.eq(cnt - 1)
|
||||
)
|
||||
]
|
||||
|
||||
for i, d in enumerate(self.data):
|
||||
self.comb += [
|
||||
self._diff(pads, "mosi{}".format(i), output=True).eq(d[-1])
|
||||
]
|
||||
|
||||
bits = Signal(max=p.width + 1, reset_less=True)
|
||||
|
||||
self.submodules.fsm = fsm = CEInserter()(FSM("IDLE"))
|
||||
|
||||
self.comb += [
|
||||
fsm.ce.eq(cnt_done)
|
||||
]
|
||||
|
||||
fsm.act("IDLE",
|
||||
self.done.eq(1),
|
||||
cs_n.eq(1),
|
||||
If(self.start,
|
||||
cnt_next.eq(1),
|
||||
NextState("SETUP")
|
||||
)
|
||||
)
|
||||
fsm.act("SETUP",
|
||||
cnt_next.eq(1),
|
||||
If(bits == 0,
|
||||
NextState("IDLE")
|
||||
).Else(
|
||||
NextState("HOLD")
|
||||
)
|
||||
)
|
||||
fsm.act("HOLD",
|
||||
cnt_next.eq(1),
|
||||
clk.eq(1),
|
||||
NextState("SETUP")
|
||||
)
|
||||
|
||||
self.sync += [
|
||||
If(fsm.ce,
|
||||
If(fsm.before_leaving("HOLD"),
|
||||
bits.eq(bits - 1),
|
||||
[d[1:].eq(d) for d in self.data]
|
||||
),
|
||||
If(fsm.ongoing("IDLE"),
|
||||
bits.eq(p.width)
|
||||
)
|
||||
)
|
||||
]
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
from migen import *
|
||||
from migen.genlib import io
|
||||
|
||||
|
||||
class DiffMixin:
|
||||
def _diff(self, pads, name, output=False):
|
||||
"""Retrieve the single-ended ``Signal()`` ``name`` from
|
||||
``pads`` and in its absence retrieve the differential signal with the
|
||||
pin pairs ``name_p`` and ``name_n``. Do so as an output if ``output``,
|
||||
otherwise make a differential input."""
|
||||
if hasattr(pads, name):
|
||||
return getattr(pads, name)
|
||||
sig = Signal()
|
||||
p, n = (getattr(pads, name + "_" + s) for s in "pn")
|
||||
if output:
|
||||
self.specials += io.DifferentialOutput(sig, p, n)
|
||||
else:
|
||||
self.specials += io.DifferentialInput(p, n, sig)
|
||||
return sig
|
||||
Loading…
Reference in New Issue