Skip to content

W
White Rabbit Trigger Distribution
Commit f0b1c2b4 authored by Tristan Gingold's avatar Tristan Gingold
Browse files
Options

wrtd: modularize fd

parent a50e4e06
Hide whitespace changes
Inline Side-by-side
Showing with 339 additions and 341 deletions
software/firmware/fd/acam.c
View file @ f0b1c2b4
......@@ -18,9 +18,6 @@
#include "fine-delay-init.h"
static int fd_acam_addr = -1;
int fd_bin_size = -1;
/*
* Calculation is fixed point: picoseconds and 16 decimals (i.e. ps << 16).
* We know the bin is small, but the Tref is several nanos so we need 64 bits
......@@ -87,72 +84,72 @@ static int acam_calc_pll(uint64_t tref, int bin, int *hsdiv_out,
return (bin + 1); /* We always return the bin size in the I mode. Other modes should scale it appropriately. */
}
static void acam_set_address(int addr)
static void acam_set_address(struct wrtd_fd_dev *fd, int addr)
{
if (addr == fd_acam_addr)
if (fd->fd_acam_addr == addr)
return;
if (fd_acam_addr == -1) {
if (fd->fd_acam_addr == -1) {
/* first time */
fd_gpio_dir(0xf00, FD_GPIO_OUT);
fd_gpio_dir(fd, 0xf00, FD_GPIO_OUT);
}
fd_gpio_val(0xf00, addr << 8);
fd_acam_addr = addr;
fd_gpio_val(fd, 0xf00, addr << 8);
fd->fd_acam_addr = addr;
}
/* Warning: acam_readl and acam_writel only work if GCR.BYPASS is set */
uint32_t acam_readl(int reg)
uint32_t acam_readl(struct wrtd_fd_dev *fd, int reg)
{
acam_set_address(reg);
fd_writel(FD_TDCSR_READ, FD_REG_TDCSR);
return fd_readl(FD_REG_TDR) & ACAM_MASK;
acam_set_address(fd, reg);
fd_writel(fd, FD_TDCSR_READ, FD_REG_TDCSR);
return fd_readl(fd, FD_REG_TDR) & ACAM_MASK;
}
void acam_writel(int val, int reg)
void acam_writel(struct wrtd_fd_dev *fd, int val, int reg)
{
acam_set_address(reg);
fd_writel(val, FD_REG_TDR);
fd_writel(FD_TDCSR_WRITE, FD_REG_TDCSR);
acam_set_address(fd, reg);
fd_writel(fd, val, FD_REG_TDR);
fd_writel(fd, FD_TDCSR_WRITE, FD_REG_TDCSR);
}
static void acam_set_bypass(int on)
static void acam_set_bypass(struct wrtd_fd_dev *fd, int on)
{
/* warning: this clears the "input enable" bit: call at init only */
fd_writel(on ? FD_GCR_BYPASS : 0, FD_REG_GCR);
fd_writel(fd, on ? FD_GCR_BYPASS : 0, FD_REG_GCR);
}
static inline int acam_is_pll_locked()
static inline int acam_is_pll_locked(struct wrtd_fd_dev *fd)
{
return !(acam_readl(12) &AR12_NotLocked);
return !(acam_readl(fd, 12) & AR12_NotLocked);
}
/* Two test functions to verify the bus is working -- Tom */
static int acam_test_addr_bit(int base, int bit,
int data)
static int acam_test_addr_bit(struct wrtd_fd_dev *fd,
int base, int bit, int data)
{
int addr1 = base;
int addr2 = base + (1<<bit);
int reg;
reg = acam_readl(addr1) & ~data;
acam_writel(reg, addr1); /* zero the data mask */
reg = acam_readl(addr2) | data;
acam_writel(reg, addr2); /* set the data mask */
reg = acam_readl(fd, addr1) & ~data;
acam_writel(fd, reg, addr1); /* zero the data mask */
reg = acam_readl(fd, addr2) | data;
acam_writel(fd, reg, addr2); /* set the data mask */
if ((acam_readl(addr1) & data) != 0)
if ((acam_readl(fd, addr1) & data) != 0)
goto out;
if ((acam_readl(addr2) & data) != data)
if ((acam_readl(fd, addr2) & data) != data)
goto out;
/* the other way around */
reg = acam_readl(addr2) & ~data;
acam_writel(reg, addr2); /* zero the data mask */
reg = acam_readl(addr1) | data;
acam_writel(reg, addr1); /* set the data mask */
reg = acam_readl(fd, addr2) & ~data;
acam_writel(fd, reg, addr2); /* zero the data mask */
reg = acam_readl(fd, addr1) | data;
acam_writel(fd, reg, addr1); /* set the data mask */
if ((acam_readl(addr2) & data) != 0)
if ((acam_readl(fd, addr2) & data) != 0)
goto out;
if ((acam_readl(addr1) & data) != data)
if ((acam_readl(fd, addr1) & data) != data)
goto out;
return 0;
......@@ -161,28 +158,28 @@ out:
return -EIO;
}
static int acam_test_bus()
static int acam_test_bus(struct wrtd_fd_dev *fd)
{
int err = 0, i, v;
/* Use register 5 to checke the data bits */
for(i = 0; i & ACAM_MASK; i <<= 1) {
acam_writel(i, 5);
acam_readl(0);
v = acam_readl(5);
acam_writel(fd, i, 5);
acam_readl(fd, 0);
v = acam_readl(fd, 5);
if (v != i)
goto out;
acam_writel(~i & ACAM_MASK, 5);
acam_readl( 0);
v = acam_readl( 5);
acam_writel(fd, ~i & ACAM_MASK, 5);
acam_readl(fd, 0);
v = acam_readl(fd, 5);
if (v != (~i & ACAM_MASK))
goto out;
}
err += acam_test_addr_bit(0, 0, 0x000001);
err += acam_test_addr_bit(1, 1, 0x000008);
err += acam_test_addr_bit(0, 2, 0x000001);
err += acam_test_addr_bit(3, 3, 0x010000);
err += acam_test_addr_bit(fd, 0, 0, 0x000001);
err += acam_test_addr_bit(fd, 1, 1, 0x000008);
err += acam_test_addr_bit(fd, 0, 2, 0x000001);
err += acam_test_addr_bit(fd, 3, 3, 0x010000);
if (err)
return -EIO;
return 0;
......@@ -239,24 +236,27 @@ static struct acam_init_data acam_init_regs_imode[] = {
};
static int acam_configure( enum fd_acam_modes mode, struct acam_init_data *regs, int n_regs )
static int acam_configure(struct wrtd_fd_dev *fd, enum fd_acam_modes mode,
const struct acam_init_data *regs, int n_regs )
{
int i, hsdiv, refdiv, reg7val;
struct acam_init_data *p;
const struct acam_init_data *p;
uint32_t regval;
int locked = 0;
fd_bin_size = acam_calc_pll(ACAM_FP_TREF, ACAM_FP_BIN, &hsdiv, &refdiv);
fd->fd_bin_size = acam_calc_pll(ACAM_FP_TREF, ACAM_FP_BIN,
&hsdiv, &refdiv);
reg7val = AR7_HSDiv(hsdiv) | AR7_RefClkDiv(refdiv);
pr_debug("ACAM config: mode %d bin 0x%x, hsdiv %i, refdiv %i\n", mode, fd_bin_size, hsdiv, refdiv);
pr_debug("ACAM config: mode %d bin 0x%x, hsdiv %i, refdiv %i\n",
mode, fd->fd_bin_size, hsdiv, refdiv);
/* Disable TDC inputs prior to configuring */
fd_writel(FD_TDCSR_STOP_DIS | FD_TDCSR_START_DIS, FD_REG_TDCSR);
fd_writel(fd, FD_TDCSR_STOP_DIS | FD_TDCSR_START_DIS, FD_REG_TDCSR);
/* Disable the ACAM PLL for a while to make sure it is reset */
acam_writel(0, 0);
acam_writel(7, 0);
acam_writel(fd, 0, 0);
acam_writel(fd, 7, 0);
mdelay(100);
......@@ -269,12 +269,12 @@ static int acam_configure( enum fd_acam_modes mode, struct acam_init_data *regs,
if (p->addr == 6 && mode == ACAM_GMODE )
regval |= AR6_StartOff2(ACAM_GMODE_START_OFFSET);
acam_writel(regval, p->addr);
acam_writel(fd, regval, p->addr);
}
for (i = 0; i < 20; i++)
{
if (acam_is_pll_locked())
if (acam_is_pll_locked(fd))
{
locked = 1;
break;
......@@ -289,34 +289,34 @@ static int acam_configure( enum fd_acam_modes mode, struct acam_init_data *regs,
}
/* after config, set the FIFO address for further reads */
acam_set_address(8);
acam_set_address(fd, 8);
return 0;
}
int fd_acam_init()
int fd_acam_init(struct wrtd_fd_dev *fd)
{
int ret;
fd_acam_addr = -1; /* First time must be activated */
fd->fd_acam_addr = -1; /* First time must be activated */
acam_set_bypass(1); /* Driven by host, not core */
acam_set_bypass(fd, 1); /* Driven by host, not core */
if ( (ret = acam_test_bus()) )
if ( (ret = acam_test_bus(fd)) )
return ret;
if ( (ret = acam_configure(ACAM_IMODE, acam_init_regs_imode, ARRAY_SIZE(acam_init_regs_imode))) )
if ( (ret = acam_configure(fd, ACAM_IMODE, acam_init_regs_imode, ARRAY_SIZE(acam_init_regs_imode))) )
return ret;
if ( (ret = fd_calibrate_outputs()) )
if ( (ret = fd_calibrate_outputs(fd)) )
return ret;
if ( (ret = acam_configure(ACAM_GMODE, acam_init_regs_gmode, ARRAY_SIZE(acam_init_regs_gmode))) )
if ( (ret = acam_configure(fd, ACAM_GMODE, acam_init_regs_gmode, ARRAY_SIZE(acam_init_regs_gmode))) )
return ret;
acam_set_bypass(0); /* Driven by core, not host */
acam_set_bypass(fd, 0); /* Driven by core, not host */
/* Clear and disable the timestamp readout buffer */
fd_writel(FD_TSBCR_PURGE | FD_TSBCR_RST_SEQ, FD_REG_TSBCR);
fd_writel(fd, FD_TSBCR_PURGE | FD_TSBCR_RST_SEQ, FD_REG_TSBCR);
/*
* Program the ACAM-specific TS registers w pre-defined calib values:
......@@ -326,9 +326,9 @@ int fd_acam_init()
* GMode fix: we no longer use the values from the EEPROM (they are fixed anyway)
*/
fd_writel( ACAM_GMODE_ADSFR, FD_REG_ADSFR);
fd_writel( ACAM_GMODE_ASOR, FD_REG_ASOR);
fd_writel( ACAM_GMODE_ATMCR, FD_REG_ATMCR);
fd_writel(fd, ACAM_GMODE_ADSFR, FD_REG_ADSFR);
fd_writel(fd, ACAM_GMODE_ASOR, FD_REG_ASOR);
fd_writel(fd, ACAM_GMODE_ATMCR, FD_REG_ATMCR);
return 0;
}
software/firmware/fd/calibrate.c
View file @ f0b1c2b4
......@@ -19,9 +19,9 @@
#include "hw/fd_channel_regs.h"
/* This is the same as in ./acam.c: use only at init time */
static void acam_set_bypass( int on)
static void acam_set_bypass(struct wrtd_fd_dev *fd, int on)
{
fd_writel( on ? FD_GCR_BYPASS : 0, FD_REG_GCR);
fd_writel(fd, on ? FD_GCR_BYPASS : 0, FD_REG_GCR);
}
......@@ -34,23 +34,23 @@ static void acam_set_bypass( int on)
*/
/* Note: channel is the "internal" one: 0..3 */
static uint64_t output_delay_ps(int ch, int fine, int n)
static uint64_t output_delay_ps(struct wrtd_fd_dev *fd, int ch, int fine, int n)
{
int i;
uint64_t acc = 0;
/* Disable the output for the channel being calibrated */
fd_gpio_clr( FD_GPIO_OUTPUT_EN(FD_CH_EXT(ch)));
fd_gpio_clr(fd, FD_GPIO_OUTPUT_EN(FD_CH_EXT(ch)));
/* Enable the stop input in ACAM for the channel being calibrated */
acam_writel( AR0_TRiseEn(0) | AR0_TRiseEn(FD_CH_EXT(ch))
acam_writel(fd, AR0_TRiseEn(0) | AR0_TRiseEn(FD_CH_EXT(ch))
| AR0_HQSel | AR0_ROsc, 0);
/* Program the output delay line setpoint */
fd_drv_ch_writel( ch, fine, FD_REG_FRR);
fd_drv_ch_writel( ch, FD_DCR_ENABLE | FD_DCR_MODE | FD_DCR_UPDATE,
FD_REG_DCR);
fd_drv_ch_writel( ch, FD_DCR_FORCE_DLY | FD_DCR_ENABLE, FD_REG_DCR);
fd_drv_ch_writel(fd, ch, fine, FD_REG_FRR);
fd_drv_ch_writel(fd, ch, FD_DCR_ENABLE | FD_DCR_MODE | FD_DCR_UPDATE,
FD_REG_DCR);
fd_drv_ch_writel(fd, ch, FD_DCR_FORCE_DLY | FD_DCR_ENABLE, FD_REG_DCR);
/*
* Set the calibration pulse mask to genrate calibration
......@@ -58,27 +58,27 @@ static uint64_t output_delay_ps(int ch, int fine, int n)
* crosstalk in the output buffer which can severely decrease
* the accuracy of calibration measurements
*/
fd_writel( FD_CALR_PSEL_W(1 << ch), FD_REG_CALR);
fd_writel(fd, FD_CALR_PSEL_W(1 << ch), FD_REG_CALR);
udelay(1);
/* Do n_avgs single measurements and average */
for (i = 0; i < n; i++) {
uint32_t fr;
/* Re-arm the ACAM (it's working in a single-shot mode) */
fd_writel( FD_TDCSR_ALUTRIG, FD_REG_TDCSR);
fd_writel(fd, FD_TDCSR_ALUTRIG, FD_REG_TDCSR);
udelay(1);
/* Produce a calib pulse on the TDC start and the output ch */
fd_writel( FD_CALR_CAL_PULSE |
fd_writel(fd, FD_CALR_CAL_PULSE |
FD_CALR_PSEL_W(1 << ch), FD_REG_CALR);
udelay(1);
/* read the tag, convert to picoseconds (fixed point: 16.16) */
fr = acam_readl( 8 /* fifo */) & 0x1ffff;
fr = acam_readl(fd, 8 /* fifo */) & 0x1ffff;
//pp_printf("i %d fr %x\n\r", i, fr);
acc += fr * fd_bin_size;
acc += fr * fd->fd_bin_size;
}
fd_drv_ch_writel(ch, 0, FD_REG_DCR);
fd_drv_ch_writel(fd, ch, 0, FD_REG_DCR);
/* Calculate avg, min max */
acc = div_u64((acc + n / 2), n );
......@@ -88,7 +88,7 @@ static uint64_t output_delay_ps(int ch, int fine, int n)
return acc;
}
static int fd_find_8ns_tap(int ch)
static int fd_find_8ns_tap(struct wrtd_fd_dev *fd, int ch)
{
int l = 0, mid, r = FD_NUM_TAPS - 1;
uint64_t bias, dly;
......@@ -99,10 +99,10 @@ static int fd_find_8ns_tap(int ch)
* delay line (ingoring the TDC, FPGA and routing delays).
* Use a binary search of the delay value.
*/
bias = output_delay_ps(ch, 0, FD_CAL_STEPS);
bias = output_delay_ps(fd, ch, 0, FD_CAL_STEPS);
while( r - l > 1) {
mid = ( l + r) / 2;
dly = output_delay_ps(ch, mid, FD_CAL_STEPS) - bias;
dly = output_delay_ps(fd, ch, mid, FD_CAL_STEPS) - bias;
if(dly < 8000 << 16)
l = mid;
......@@ -119,17 +119,17 @@ static int fd_find_8ns_tap(int ch)
* for each channel. This is done during ACAM initialization, so on driver
* probe.
*/
int fd_calibrate_outputs()
int fd_calibrate_outputs(struct wrtd_fd_dev *fd)
{
int ch;
int measured;
acam_set_bypass(1); /* not useful */
fd_writel(FD_TDCSR_START_EN | FD_TDCSR_STOP_EN, FD_REG_TDCSR);
acam_set_bypass(fd, 1); /* not useful */
fd_writel(fd, FD_TDCSR_START_EN | FD_TDCSR_STOP_EN, FD_REG_TDCSR);
for (ch = FD_CH_1; ch <= FD_CH_LAST; ch++) {
measured = fd_find_8ns_tap(ch);
fd_drv_ch_writel(ch, measured, FD_REG_FRR);
for (ch = 0; ch < FD_NUM_CHANNELS; ch++) {
measured = fd_find_8ns_tap(fd, ch);
fd_drv_ch_writel(fd, ch, measured, FD_REG_FRR);
pr_debug("Channel %d: 8ns @ %i taps.\n\r", ch+1, measured );
}
......
software/firmware/fd/fine-delay-init.c
View file @ f0b1c2b4
......@@ -18,8 +18,6 @@
#include "fine-delay-init.h"
#include "wrtd-fw-internals.h"
#ifdef SIMULATION
#define UDELAY(X)
#define MDELAY(X)
......@@ -36,90 +34,89 @@
* the fine-delay core (CORE).
* In the reset register 0 means reset, 1 means normal operation.
*/
static void fd_do_reset(int hw_reset)
static void fd_do_reset(struct wrtd_fd_dev *fd, int hw_reset)
{
if (hw_reset) {
/* clear RSTS_RST_FMC bit, set RSTS_RST_CORE bit*/
fd_writel( FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_CORE_MASK,
FD_REG_RSTR);
fd_writel(fd, FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_CORE_MASK,
FD_REG_RSTR);
UDELAY(10000);
fd_writel( FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_CORE_MASK
| FD_RSTR_RST_FMC_MASK, FD_REG_RSTR);
fd_writel(fd, FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_CORE_MASK
| FD_RSTR_RST_FMC_MASK, FD_REG_RSTR);
/* TPS3307 supervisor needs time to de-assert master reset */
MDELAY(600);
return;
}
/* clear RSTS_RST_CORE bit, set RSTS_RST_FMC bit */
fd_writel( FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_FMC_MASK,
fd_writel(fd, FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_FMC_MASK,
FD_REG_RSTR);
UDELAY(1000);
fd_writel( FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_FMC_MASK
fd_writel(fd, FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_FMC_MASK
| FD_RSTR_RST_CORE_MASK, FD_REG_RSTR);
UDELAY(1000);
}
/* Some init procedures to be intermixed with subsystems */
int fd_gpio_defaults()
int fd_gpio_defaults(struct wrtd_fd_dev *fd)
{
fd_gpio_dir( FD_GPIO_TRIG_INTERNAL, FD_GPIO_OUT);
fd_gpio_set( FD_GPIO_TRIG_INTERNAL);
fd_gpio_dir(fd, FD_GPIO_TRIG_INTERNAL, FD_GPIO_OUT);
fd_gpio_set(fd, FD_GPIO_TRIG_INTERNAL);
fd_gpio_set( FD_GPIO_OUTPUT_MASK);
fd_gpio_dir( FD_GPIO_OUTPUT_MASK, FD_GPIO_OUT);
fd_gpio_set(fd, FD_GPIO_OUTPUT_MASK);
fd_gpio_dir(fd, FD_GPIO_OUTPUT_MASK, FD_GPIO_OUT);
fd_gpio_dir( FD_GPIO_TERM_EN, FD_GPIO_OUT);
fd_gpio_clr( FD_GPIO_TERM_EN);
fd_gpio_dir(fd, FD_GPIO_TERM_EN, FD_GPIO_OUT);
fd_gpio_clr(fd, FD_GPIO_TERM_EN);
return 0;
}
int fd_reset_again()
int fd_reset_again(struct wrtd_fd_dev *fd)
{
/* Reset the FD core once we have proper reference/TDC clocks */
fd_do_reset( 0 /* not hw */);
fd_do_reset(fd, 0 /* not hw */);
MDELAY(10);
if (! ( fd_readl(FD_REG_GCR) & FD_GCR_DDR_LOCKED) )
if (! (fd_readl(fd, FD_REG_GCR) & FD_GCR_DDR_LOCKED) )
{
pr_error("timeout waiting for GCR lock bit\n");
return -EIO;
}
fd_do_reset( 0 /* not hw */);
fd_do_reset(fd, 0 /* not hw */);
return 0;
}
/* FIXME missing all calibration */
int fd_init(void)
int fd_init(struct wrtd_fd_dev *fd)
{
int err, ch;
pr_debug("Initializing the Fine Delay board...\n");
fd_do_reset(1);
fd_do_reset(fd, 1);
err = fd_gpio_init();
err = fd_gpio_init(fd);
if (err)
return err;
err = fd_pll_init();
err = fd_pll_init(fd);
if (err)
return err;
fd_gpio_defaults();
fd_gpio_defaults(fd);
err = fd_reset_again();
err = fd_reset_again(fd);
if (err)
return err;
err = fd_acam_init();
err = fd_acam_init(fd);
if (err)
return err;
for (ch = 1; ch <= FD_CH_NUMBER; ch++)
fd_gpio_set( FD_GPIO_OUTPUT_EN(ch));
for (ch = 1; ch <= FD_NUM_CHANNELS; ch++)
fd_gpio_set(fd, FD_GPIO_OUTPUT_EN(ch));
// todo: read offsets from the EEPROM. I2C should be working OK.
......
software/firmware/fd/fine-delay-init.h
View file @ f0b1c2b4
#ifndef __FINE_DELAY_WRAPPER_H
#define __FINE_DELAY_WRAPPER_H
extern int fd_bin_size;
#include "wrtd-common.h"
/* Channels are called 1..4 in all docs. Internally it's 0..3 */
#define FD_CH_1 0
#define FD_CH_LAST 3
#define FD_CH_NUMBER 4
#define FD_CH_INT(i) ((i) - 1)
#define FD_NUM_CHANNELS 4
#define FD_CH_EXT(i) ((i) + 1)
#define FD_NUM_TAPS 1024 /* This is an hardware feature of SY89295U */
......@@ -54,56 +53,85 @@ enum fd_acam_modes {
#define FD_MCP_GPIO 0x12
#define FD_MCP_OLAT 0x14
int fd_spi_xfer(int ss, int num_bits,
uint32_t in, uint32_t *out);
int fd_spi_init();
int fd_pll_init();
#define FD_MAX_QUEUE_PULSES 4
int fd_acam_init();
uint32_t acam_readl(int reg);
void acam_writel(int val, int reg);
/* Pulse FIFO for a single Fine Delay output */
struct lrt_pulse_queue {
struct wrtd_event events[FD_MAX_QUEUE_PULSES];
int head, tail, count;
};
struct wrtd_fd_channel {
uint32_t channel_addr;
struct lrt_pulse_queue queue;
uint8_t idle;
uint32_t width_ns;
/* Last timestamp value written to output config. */
uint32_t last_programmed_sec;
uint32_t last_programmed_ns;
};
struct wrtd_fd_dev {
uint32_t io_addr;
int fd_acam_addr;
int fd_bin_size;
uint32_t fd_mcp_iodir;
uint32_t fd_mcp_olat;
struct wrtd_fd_channel channels[FD_NUM_CHANNELS];
};
int fd_spi_xfer(struct wrtd_fd_dev *fd, int ss, int num_bits,
uint32_t in, uint32_t *out);
int fd_spi_init(struct wrtd_fd_dev *fd);
int fd_pll_init(struct wrtd_fd_dev *fd);
int fd_acam_init(struct wrtd_fd_dev *fd);
uint32_t acam_readl(struct wrtd_fd_dev *fd, int reg);
void acam_writel(struct wrtd_fd_dev *fd, int val, int reg);
/* Functions exported by calibrate.c, called within acam.c */
int fd_calibrate_outputs();
int fd_calibrate_outputs(struct wrtd_fd_dev *fd);
/* Functions exported by gpio.c */
int fd_gpio_init();
void fd_gpio_exit();
void fd_gpio_dir(int pin, int dir);
void fd_gpio_val(int pin, int val);
void fd_gpio_set_clr(int pin, int set);
int fd_gpio_init(struct wrtd_fd_dev *fd);
void fd_gpio_exit(struct wrtd_fd_dev *fd);
void fd_gpio_dir(struct wrtd_fd_dev *fd, int pin, int dir);
void fd_gpio_val(struct wrtd_fd_dev *fd, int pin, int val);
void fd_gpio_set_clr(struct wrtd_fd_dev *fd, int pin, int set);
int fd_init();
int fd_init(struct wrtd_fd_dev *fd);
int fd_sim_init(void);
#define fd_gpio_set(pin) fd_gpio_set_clr((pin), 1)
#define fd_gpio_clr(pin) fd_gpio_set_clr((pin), 0)
#define fd_gpio_set(fd, pin) fd_gpio_set_clr(fd, (pin), 1)
#define fd_gpio_clr(fd, pin) fd_gpio_set_clr(fd, (pin), 0)
#define FD_GPIO_IN 0
#define FD_GPIO_OUT 1
#define DP_BASE_FD_CORE 0x0
static inline void fd_writel(uint32_t val, uint32_t reg)
static inline void fd_writel(struct wrtd_fd_dev *fd, uint32_t val, uint32_t reg)
{
dp_writel( val, reg + DP_BASE_FD_CORE );
dp_writel(val, fd->io_addr + reg);
}
static inline uint32_t fd_readl(uint32_t reg)
static inline uint32_t fd_readl(struct wrtd_fd_dev *fd, uint32_t reg)
{
return dp_readl( reg + DP_BASE_FD_CORE );
return dp_readl(fd->io_addr + reg);
}
static inline uint32_t fd_drv_ch_readl(int ch,
unsigned long reg)
static inline uint32_t fd_drv_ch_readl(struct wrtd_fd_dev *fd, int ch,
unsigned long reg)
{
return fd_readl( 0x100 + ch * 0x100 + reg);
return fd_readl(fd, 0x100 + ch * 0x100 + reg);
}
static inline void fd_drv_ch_writel(int ch,
uint32_t v, unsigned long reg)
static inline void fd_drv_ch_writel(struct wrtd_fd_dev *fd, int ch,
uint32_t v, unsigned long reg)
{
fd_writel( v, 0x100 + ch * 0x100 + reg);
fd_writel(fd, v, 0x100 + ch * 0x100 + reg);
}
// Alessandro, I f*****ing hate you. And all kernel devs. To express my hatred I will use
......
software/firmware/fd/gpio.c
View file @ f0b1c2b4
......@@ -15,25 +15,22 @@
#include "fine-delay-init.h"
static uint32_t fd_mcp_iodir = 0;
static uint32_t fd_mcp_olat = 0;
#define SPI_RETRIES 100
int gpio_writel(int val, int reg)
int gpio_writel(struct wrtd_fd_dev *fd, int val, int reg)
{
int rval = fd_spi_xfer( FD_CS_GPIO, 24,
0x4e0000 | (reg << 8) | val, NULL);
int rval = fd_spi_xfer(fd, FD_CS_GPIO, 24,
0x4e0000 | (reg << 8) | val, NULL);
return rval;
}
static int gpio_readl(int reg)
static int gpio_readl(struct wrtd_fd_dev *fd, int reg)
{
uint32_t ret;
int err;
err = fd_spi_xfer(FD_CS_GPIO, 24,
err = fd_spi_xfer(fd, FD_CS_GPIO, 24,
0x4f0000 | (reg << 8), &ret);
if (err < 0)
......@@ -41,13 +38,13 @@ static int gpio_readl(int reg)
return ret & 0xff;
}
static int gpio_writel_with_retry( int val, int reg)
static int gpio_writel_with_retry(struct wrtd_fd_dev *fd, int val, int reg)
{
int retries = SPI_RETRIES, rv;
while(retries--)
{
gpio_writel( val, reg);
rv = gpio_readl( reg);
gpio_writel(fd, val, reg);
rv = gpio_readl(fd, reg);
if(rv >= 0 && (rv == val))
{
if(SPI_RETRIES-1-retries > 0)
......@@ -59,49 +56,46 @@ static int gpio_writel_with_retry( int val, int reg)
return -EIO;
}
void fd_gpio_dir(int mask, int dir)
void fd_gpio_dir(struct wrtd_fd_dev *fd, int mask, int dir)
{
fd_mcp_iodir &= ~mask;
fd->fd_mcp_iodir &= ~mask;
if (dir == FD_GPIO_IN)
fd_mcp_iodir |= mask;
fd->fd_mcp_iodir |= mask;
gpio_writel_with_retry( (fd_mcp_iodir & 0xff), FD_MCP_IODIR);
gpio_writel_with_retry( (fd_mcp_iodir >> 8), FD_MCP_IODIR+1);
gpio_writel_with_retry(fd, (fd->fd_mcp_iodir & 0xff), FD_MCP_IODIR);
gpio_writel_with_retry(fd, (fd->fd_mcp_iodir >> 8), FD_MCP_IODIR+1);
}
void fd_gpio_val(int mask, int values)
void fd_gpio_val(struct wrtd_fd_dev *fd, int mask, int values)
{
fd_mcp_olat &= ~mask;
fd_mcp_olat |= values;
fd->fd_mcp_olat &= ~mask;
fd->fd_mcp_olat |= values;
gpio_writel_with_retry( (fd_mcp_olat & 0xff), FD_MCP_OLAT);
gpio_writel_with_retry( (fd_mcp_olat >> 8), FD_MCP_OLAT+1);
gpio_writel_with_retry(fd, (fd->fd_mcp_olat & 0xff), FD_MCP_OLAT);
gpio_writel_with_retry(fd, (fd->fd_mcp_olat >> 8), FD_MCP_OLAT+1);
}
void fd_gpio_set_clr(int mask, int set)
void fd_gpio_set_clr(struct wrtd_fd_dev *fd, int mask, int set)
{
if (set)
fd_gpio_val(mask, mask);
else
fd_gpio_val(mask, 0);
fd_gpio_val(fd, mask, set ? mask : 0);
}
int fd_gpio_init()
int fd_gpio_init(struct wrtd_fd_dev *fd)
{
int i, val;
fd_mcp_iodir = 0xffff;
fd_mcp_olat = 0;
fd->fd_mcp_iodir = 0xffff;
fd->fd_mcp_olat = 0;
if (gpio_writel(0x00, FD_MCP_IOCON) < 0)
if (gpio_writel(fd, 0x00, FD_MCP_IOCON) < 0)
goto out;
/* Try to read and write a register to test the SPI connection */
for (val = 0xaa; val >= 0; val -= 0x11) {
if (gpio_writel(val, FD_MCP_IPOL) < 0)
if (gpio_writel(fd, val, FD_MCP_IPOL) < 0)
goto out;
i = gpio_readl( FD_MCP_IPOL);
i = gpio_readl(fd, FD_MCP_IPOL);
if (i < 0)
goto out;
if (i != val) {
......
software/firmware/fd/i2c.c
View file @ f0b1c2b4
......@@ -16,135 +16,135 @@
#include "fine-delay-init.h"
#include "hw/fd_main_regs.h"
static void set_sda(int val)
static void set_sda(struct wrtd_fd_dev *fd, int val)
{
uint32_t reg;
reg = fd_readl(FD_REG_I2CR) & ~FD_I2CR_SDA_OUT;
reg = fd_readl(fd, FD_REG_I2CR) & ~FD_I2CR_SDA_OUT;
if (val)
reg |= FD_I2CR_SDA_OUT;
fd_writel(reg, FD_REG_I2CR);
fd_writel(fd, reg, FD_REG_I2CR);
udelay(3);
}
static void set_scl(int val)
static void set_scl(struct wrtd_fd_dev *fd, int val)
{
uint32_t reg;
reg = fd_readl(FD_REG_I2CR) & ~FD_I2CR_SCL_OUT;
reg = fd_readl(fd, FD_REG_I2CR) & ~FD_I2CR_SCL_OUT;
if (val)
reg |= FD_I2CR_SCL_OUT;
fd_writel(reg, FD_REG_I2CR);
fd_writel(fd, reg, FD_REG_I2CR);
udelay(3);
}
static int get_sda()
static int get_sda(struct wrtd_fd_dev *fd)
{
return fd_readl(FD_REG_I2CR) & FD_I2CR_SDA_IN ? 1 : 0;
};
return fd_readl(fd, FD_REG_I2CR) & FD_I2CR_SDA_IN ? 1 : 0;
}
static void mi2c_start()
static void mi2c_start(struct wrtd_fd_dev *fd)
{
set_sda(0);
set_scl(0);
set_sda(fd, 0);
set_scl(fd, 0);
}
static void mi2c_stop()
static void mi2c_stop(struct wrtd_fd_dev *fd)
{
set_sda(0);
set_scl(1);
set_sda(1);
set_sda(fd, 0);
set_scl(fd, 1);
set_sda(fd, 1);
}
int mi2c_put_byte(int data)
int mi2c_put_byte(struct wrtd_fd_dev *fd, int data)
{
int i;
int ack;
for (i = 0; i < 8; i++, data<<=1) {
set_sda(data & 0x80);
set_scl(1);
set_scl(0);
set_sda(fd, data & 0x80);
set_scl(fd, 1);
set_scl(fd, 0);
}
set_sda(1);
set_scl(1);
set_sda(fd, 1);
set_scl(fd, 1);
ack = get_sda();
ack = get_sda(fd);
set_scl(0);
set_sda(0);
set_scl(fd, 0);
set_sda(fd, 0);
return ack ? -EIO : 0; /* ack low == success */
}
int mi2c_get_byte(unsigned char *data, int sendack)
int mi2c_get_byte(struct wrtd_fd_dev *fd, unsigned char *data, int sendack)
{
int i;
int indata = 0;
/* assert: scl is low */
set_scl(0);
set_sda(1);
set_scl(fd, 0);
set_sda(fd, 1);
for (i = 0; i < 8; i++) {
set_scl(1);
set_scl(fd, 1);
indata <<= 1;
if (get_sda())
if (get_sda(fd))
indata |= 0x01;
set_scl(0);
set_scl(fd, 0);
}
set_sda((sendack ? 0 : 1));
set_scl(1);
set_scl(0);
set_sda(0);
set_sda(fd, (sendack ? 0 : 1));
set_scl(fd, 1);
set_scl(fd, 0);
set_sda(fd, 0);
*data= indata;
return 0;
}
void mi2c_init()
void mi2c_init(struct wrtd_fd_dev *fd)
{
set_scl(1);
set_sda(1);
set_scl(fd, 1);
set_sda(fd, 1);
}
void mi2c_scan()
void mi2c_scan(struct wrtd_fd_dev *fd)
{
int i;
for(i = 0; i < 256; i += 2) {
mi2c_start();
if(!mi2c_put_byte(i))
mi2c_start(fd);
if(!mi2c_put_byte(fd, i))
pr_debug("Found i2c device at 0x%x\n",
i >> 1);
mi2c_stop();
mi2c_stop(fd);
}
}
/* FIXME: this is very inefficient: read several bytes in a row instead */
int fd_eeprom_read(int i2c_addr, uint32_t offset,
void *buf, size_t size)
int fd_eeprom_read(struct wrtd_fd_dev *fd, int i2c_addr, uint32_t offset,
void *buf, size_t size)
{
int i;
uint8_t *buf8 = buf;
unsigned char c;
for(i = 0; i < size; i++) {
mi2c_start();
if(mi2c_put_byte(i2c_addr << 1) < 0) {
mi2c_stop();
mi2c_start(fd);
if(mi2c_put_byte(fd, i2c_addr << 1) < 0) {
mi2c_stop(fd);
return -EIO;
}
mi2c_put_byte((offset >> 8) & 0xff);
mi2c_put_byte(offset & 0xff);
mi2c_put_byte(fd, (offset >> 8) & 0xff);
mi2c_put_byte(fd, offset & 0xff);
offset++;
mi2c_stop();
mi2c_start();
mi2c_put_byte((i2c_addr << 1) | 1);
mi2c_get_byte(&c, 0);
mi2c_stop(fd);
mi2c_start(fd);
mi2c_put_byte(fd, (i2c_addr << 1) | 1);
mi2c_get_byte(fd, &c, 0);
*buf8++ = c;
mi2c_stop();
mi2c_stop(fd);
}
return size;
}
software/firmware/fd/pll.c
View file @ f0b1c2b4
......@@ -104,7 +104,8 @@ static const struct ad9516_reg __9516_regs[] = {
{0x0232, 0x00}, /* Update All registers */
};
int fd_spi_xfer(int ss, int num_bits, uint32_t in, uint32_t *out)
int fd_spi_xfer(struct wrtd_fd_dev *fd,
int ss, int num_bits, uint32_t in, uint32_t *out)
{
uint32_t scr = 0, r;
unsigned long timeout;
......@@ -116,16 +117,16 @@ int fd_spi_xfer(int ss, int num_bits, uint32_t in, uint32_t *out)
else if(ss == FD_CS_GPIO)
scr |= FD_SCR_SEL_GPIO;
fd_writel(scr, FD_REG_SCR);
fd_writel(scr | FD_SCR_START, FD_REG_SCR);
fd_writel(fd, scr, FD_REG_SCR);
fd_writel(fd, scr | FD_SCR_START, FD_REG_SCR);
timeout = trtl_get_runtime_milliseconds() + 1000;
while (!(fd_readl(FD_REG_SCR) & FD_SCR_READY))
while (!(fd_readl(fd, FD_REG_SCR) & FD_SCR_READY))
if (trtl_get_runtime_milliseconds() > timeout)
break;
if (!(fd_readl(FD_REG_SCR) & FD_SCR_READY))
if (!(fd_readl(fd, FD_REG_SCR) & FD_SCR_READY))
return -EIO;
scr = fd_readl(FD_REG_SCR);
scr = fd_readl(fd, FD_REG_SCR);
r = FD_SCR_DATA_R(scr);
if(out)
*out=r;
......@@ -136,17 +137,17 @@ int fd_spi_xfer(int ss, int num_bits, uint32_t in, uint32_t *out)
}
static int fd_pll_writel(int val, int reg)
static int fd_pll_writel(struct wrtd_fd_dev *fd, int val, int reg)
{
return fd_spi_xfer(FD_CS_PLL, 24, (reg << 8) | val, NULL);
return fd_spi_xfer(fd, FD_CS_PLL, 24, (reg << 8) | val, NULL);
}
static int fd_pll_readl(int reg)
static int fd_pll_readl(struct wrtd_fd_dev *fd, int reg)
{
uint32_t ret;
int err;
err = fd_spi_xfer(FD_CS_PLL, 24, (reg << 8) | (1 << 23), &ret);
err = fd_spi_xfer(fd, FD_CS_PLL, 24, (reg << 8) | (1 << 23), &ret);
if (err < 0)
return err;
......@@ -155,11 +156,11 @@ static int fd_pll_readl(int reg)
/* Just check the id register of the pll.
Detect broken SPI link. */
static int fd_pll_check(void)
static int fd_pll_check(struct wrtd_fd_dev *fd)
{
int id;
id = fd_pll_readl(0x003);
id = fd_pll_readl(fd, 0x003);
if (id < 0)
return -EIO;
if (id != 0xc3) {
......@@ -170,38 +171,38 @@ static int fd_pll_check(void)
return 0;
}
int fd_pll_init(void)
int fd_pll_init(struct wrtd_fd_dev *fd)
{
int i, ret;
unsigned long timeout;
const struct ad9516_reg *r;
if (fd_pll_writel(0x99, 0x000) < 0)
if (fd_pll_writel(fd, 0x99, 0x000) < 0)
return -EIO;
if (fd_pll_writel(0x01, 0x232) < 0)
if (fd_pll_writel(fd, 0x01, 0x232) < 0)
return -EIO;
if (fd_pll_check() < 0)
if (fd_pll_check(fd) < 0)
return -EIO;
/* Write the magic config */
for (i = 0, r = __9516_regs; i < ARRAY_SIZE(__9516_regs); i++, r++) {
if (fd_pll_writel(r->val, r->reg) < 0) {
if (fd_pll_writel(fd, r->val, r->reg) < 0) {
pr_error("Error in configuring PLL (step %i)\n", i);
return -EIO;
}
}
/* Enable the new config. */
if (fd_pll_writel(0x01, 0x232) < 0)
if (fd_pll_writel(fd, 0x01, 0x232) < 0)
return -EIO;
if (fd_pll_check() < 0)
if (fd_pll_check(fd) < 0)
return -EIO;
/* Wait (at most 500ms) for it to lock */
timeout = trtl_get_runtime_milliseconds() + 500;
do {
ret = fd_pll_readl(0x1f);
ret = fd_pll_readl(fd, 0x1f);
if (ret < 0)
return -EIO;
if (ret & 0x1)
......@@ -211,7 +212,7 @@ int fd_pll_init(void)
if (!(ret & 0x1))
return -EIO;
if (fd_pll_check() < 0)
if (fd_pll_check(fd) < 0)
return -EIO;
/*
......@@ -219,13 +220,13 @@ int fd_pll_init(void)
* (this is critical for the accuracy!)
*/
if (fd_pll_writel(0x01, 0x230) < 0)
if (fd_pll_writel(fd, 0x01, 0x230) < 0)
return -EIO;
if (fd_pll_writel(0x01, 0x232) < 0)
if (fd_pll_writel(fd, 0x01, 0x232) < 0)
return -EIO;
if (fd_pll_writel(0x00, 0x230) < 0)
if (fd_pll_writel(fd, 0x00, 0x230) < 0)
return -EIO;
if (fd_pll_writel(0x01, 0x232) < 0)
if (fd_pll_writel(fd, 0x01, 0x232) < 0)
return -EIO;
return 0;
......
software/firmware/fd/wrtd-fw-internals.h
View file @ f0b1c2b4
......@@ -5,5 +5,3 @@
*
* SPDX-License-Identifier: LGPL-3.0-or-later
*/
extern int fd_init(void);
software/firmware/fd/wrtd-rt-fd.c
View file @ f0b1c2b4
......@@ -22,8 +22,6 @@
#define NBR_CPUS 2
#define CPU_IDX 0
#define FD_NUM_CHANNELS 4
#define NBR_LOCAL_INPUTS 0
#define NBR_LOCAL_OUTPUTS FD_NUM_CHANNELS
#define NBR_RULES 16
......@@ -37,37 +35,10 @@
#include "wrtd-rt-common.h"
#define FD_MAX_QUEUE_PULSES 4
/* Structure describing a single pulse in the Fine Delay software output
queue. */
struct pulse_queue_entry {
struct wrtd_event ev;
/* Origin timestamp cycles count (for latency statistics) */
int origin_cycles;
};
/* Pulse FIFO for a single Fine Delay output */
struct lrt_pulse_queue {
struct pulse_queue_entry data[FD_MAX_QUEUE_PULSES];
int head, tail, count;
struct wrtd_fd_dev fd0 = {
.io_addr = 0x0
};
struct wrtd_fd_channel {
uint32_t base_addr;
struct lrt_pulse_queue queue;
uint8_t idle;
uint32_t width_ns;
/* Last timestamp value written to output config. */
uint32_t last_programmed_sec;
uint32_t last_programmed_ns;
};
static struct wrtd_fd_channel wrtd_out_channels[FD_NUM_CHANNELS];
static int wr_present(void)
{
return !!(dp_readl(FD_REG_TCR) & FD_TCR_WR_PRESENT);
......@@ -136,7 +107,7 @@ static void wr_update_link(void)
static inline void fd_ch_writel(struct wrtd_fd_channel *out, uint32_t value,
uint32_t reg)
{
dp_writel(value, reg + out->base_addr);
dp_writel(value, reg + out->channel_addr);
}
/**
......@@ -144,7 +115,7 @@ static inline void fd_ch_writel(struct wrtd_fd_channel *out, uint32_t value,
*/
static inline uint32_t fd_ch_readl (struct wrtd_fd_channel *out, uint32_t reg)
{
return dp_readl(reg + out->base_addr);
return dp_readl(reg + out->channel_addr);
}
/**
......@@ -162,21 +133,21 @@ static void pulse_queue_init(struct lrt_pulse_queue *p)
* Requests a new entry in a pulse queue. Returns pointer to the ne
* entry or NULL if the queue is full.
*/
static struct pulse_queue_entry *pulse_queue_push(struct lrt_pulse_queue *p)
static struct wrtd_event *pulse_queue_push(struct lrt_pulse_queue *p)
{
struct pulse_queue_entry *ent;
struct wrtd_event *ev;
if (p->count == FD_MAX_QUEUE_PULSES)
return NULL;
ent = &p->data[p->head];
ev = &p->events[p->head];
p->count++;
p->head++;
if (p->head == FD_MAX_QUEUE_PULSES)
p->head = 0;
return ent;
return ev;
}
......@@ -192,11 +163,11 @@ static inline int pulse_queue_empty(struct lrt_pulse_queue *p)
/**
* Returns the oldest entry in the pulse queue (or NULL if empty).
*/
static struct pulse_queue_entry* pulse_queue_front(struct lrt_pulse_queue *p)
static struct wrtd_event *pulse_queue_front(struct lrt_pulse_queue *p)
{
if (!p->count)
return NULL;
return &p->data[p->tail];
return &p->events[p->tail];
}
......@@ -248,7 +219,7 @@ static int check_output_timeout (struct wrtd_fd_channel *out)
* Drop the given enqueued trigger
*/
static void drop_trigger(struct wrtd_fd_channel *out,
struct pulse_queue_entry *pq_ent,
struct wrtd_event *ev,
struct lrt_pulse_queue *q, unsigned reason)
{
out->idle = 1;
......@@ -262,7 +233,7 @@ static void drop_trigger(struct wrtd_fd_channel *out,
/* Disarm the FD output */
fd_ch_writel(out, FD_DCR_MODE, FD_REG_DCR);
wrtd_log(WRTD_LOG_MSG_EV_DISCARDED, reason, &pq_ent->ev, NULL);
wrtd_log(WRTD_LOG_MSG_EV_DISCARDED, reason, ev, NULL);
}
......@@ -270,10 +241,11 @@ static void drop_trigger(struct wrtd_fd_channel *out,
* Output driving function. Reads pulses from the output queue,
* programs the output and updates the output statistics.
*/
static void do_output (struct wrtd_fd_channel *out)
static void do_output (struct wrtd_fd_dev *fd, unsigned channel)
{
struct wrtd_fd_channel *out = &fd->channels[channel];
struct lrt_pulse_queue *q = &out->queue;
struct pulse_queue_entry *pq_ent = pulse_queue_front(q);
struct wrtd_event *ev = pulse_queue_front(q);
uint32_t dcr = fd_ch_readl(out, FD_REG_DCR);
struct wrtd_tstamp *ts;
......@@ -281,21 +253,20 @@ static void do_output (struct wrtd_fd_channel *out)
if (!out->idle) {
if (!wr_is_timing_ok()) {
/* Timing has been lost. */
drop_trigger(out, pq_ent, q, WRTD_LOG_DISCARD_NO_SYNC);
drop_trigger(out, ev, q, WRTD_LOG_DISCARD_NO_SYNC);
return;
}
if (!(dcr & FD_DCR_PG_TRIG)) {
/* Armed but still waiting for trigger */
if (check_output_timeout (out)) {
/* Will never trigger. Missed. */
drop_trigger(out, pq_ent, q,
drop_trigger(out, ev, q,
WRTD_LOG_DISCARD_TIMEOUT);
}
} else {
/* Has been triggered. */
wrtd_log(WRTD_LOG_MSG_EV_CONSUMED,
WRTD_LOG_CONSUMED_DONE,
&pq_ent->ev, NULL);
WRTD_LOG_CONSUMED_DONE, ev, NULL);
pulse_queue_pop(q);
out->idle = 1;
......@@ -307,11 +278,11 @@ static void do_output (struct wrtd_fd_channel *out)
if (pulse_queue_empty(q))
return;
pq_ent = pulse_queue_front(q);
ts = &pq_ent->ev.ts;
ev = pulse_queue_front(q);
ts = &ev->ts;
if (!wr_is_timing_ok()) {
drop_trigger(out, pq_ent, q, WRTD_LOG_DISCARD_NO_SYNC);
drop_trigger(out, ev, q, WRTD_LOG_DISCARD_NO_SYNC);
return;
}
......@@ -334,8 +305,7 @@ static void do_output (struct wrtd_fd_channel *out)
fd_ch_writel(out, FD_DCR_MODE | FD_DCR_PG_ARM | FD_DCR_ENABLE,
FD_REG_DCR);
wrtd_log(WRTD_LOG_MSG_EV_CONSUMED, WRTD_LOG_CONSUMED_START,
&pq_ent->ev, NULL);
wrtd_log(WRTD_LOG_MSG_EV_CONSUMED, WRTD_LOG_CONSUMED_START, ev, NULL);
ts_add2_ns (ts, 8000);
......@@ -348,13 +318,14 @@ static void do_output (struct wrtd_fd_channel *out)
out->idle = 0;
}
static void wrtd_local_output(struct wrtd_event *ev, unsigned ch)
static void fd_local_output(struct wrtd_fd_dev *fd,
struct wrtd_event *ev, unsigned ch)
{
struct wrtd_fd_channel *out = &wrtd_out_channels[ch];
struct pulse_queue_entry *pq_ent;
struct wrtd_fd_channel *out = &fd->channels[ch];
struct wrtd_event *pq_ev;
pq_ent = pulse_queue_push(&out->queue);
if (!pq_ent) {
pq_ev = pulse_queue_push(&out->queue);
if (!pq_ev) {
/* overflow.
FIXME: stats ? */
wrtd_log(WRTD_LOG_MSG_EV_DISCARDED, WRTD_LOG_DISCARD_OVERFLOW,
......@@ -362,16 +333,20 @@ static void wrtd_local_output(struct wrtd_event *ev, unsigned ch)
return;
}
pq_ent->ev = *ev;
pq_ent->origin_cycles = 0;
*pq_ev = *ev;
}
static void do_outputs(void)
static void wrtd_local_output(struct wrtd_event *ev, unsigned ch)
{
fd_local_output(&fd0, ev, ch);
}
static void do_outputs(struct wrtd_fd_dev *fd)
{
int i;
for (i = 0;i < FD_NUM_CHANNELS; i++)
do_output(&wrtd_out_channels[i]);
do_output(fd, i);
}
/*.
......@@ -426,13 +401,27 @@ static int wrtd_o_sim_init(void)
#endif
static void wrtd_fd_data_init(struct wrtd_fd_dev *fd)
{
unsigned i;
/* Channels */
for (i = 0; i < FD_NUM_CHANNELS; i++) {
memset(&fd->channels[i], 0, sizeof(struct wrtd_fd_channel));
fd->channels[i].channel_addr = fd->io_addr + 0x100 + i * 0x100;
pulse_queue_init(&fd->channels[i].queue);
fd->channels[i].idle = 1;
fd->channels[i].width_ns = 10000; // 10us
fd->channels[i].last_programmed_sec = 0;
fd->channels[i].last_programmed_ns = 0;
}
}
/**
* Initialize data structures, RT application and variables
*/
static int wrtd_user_init(void)
{
int i;
if (!wr_present()) {
pr_error("WhiteRabbit not found\n\r");
return -EINVAL;
......@@ -441,23 +430,14 @@ static int wrtd_user_init(void)
wr_enable_lock(0);
/* Channels */
for (i = 0; i < FD_NUM_CHANNELS; i++) {
memset(&wrtd_out_channels[i], 0,
sizeof(struct wrtd_fd_channel));
wrtd_out_channels[i].base_addr = 0x100 + i * 0x100;
pulse_queue_init(&wrtd_out_channels[i].queue);
wrtd_out_channels[i].idle = 1;
wrtd_out_channels[i].width_ns = 10000; // 10us
wrtd_out_channels[i].last_programmed_sec = 0;
wrtd_out_channels[i].last_programmed_ns = 0;
}
wrtd_fd_data_init(&fd0);
#ifdef SIMULATION
/* Skip WR sync and automatically generate some events when simulating */
wrtd_o_sim_init();
fd_sim_init();
#else
fd_init();
fd_init(&fd0);
#endif
pr_debug("rt-output firmware initialized.\n\r");
......@@ -467,5 +447,5 @@ static int wrtd_user_init(void)
static void wrtd_io(void)
{
do_outputs();
do_outputs(&fd0);
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment