Skip to content

S
Software for White Rabbit PTP Core
Commit 17b2de51 authored by Tomasz Wlostowski's avatar Tomasz Wlostowski
Browse files
Options

Merge branch 'master' of github.com:twlostow/wr-core-software

parents 2aa0b9af a294935b
Hide whitespace changes
Inline Side-by-side
Showing with 970 additions and 163 deletions
.gitignore 0 → 100644
View file @ 17b2de51
*.o
*.elf
*.ram
*.bin
*.*~
tools/zpu-loader
tools/vuart_console
tools/genraminit
wrc_disasm.S
Makefile
View file @ 17b2de51
......@@ -2,8 +2,29 @@ PLATFORM = lm32
OBJS_WRC = wrc_main.o dev/uart.o dev/endpoint.o dev/minic.o dev/pps_gen.o dev/timer.o dev/softpll.o lib/mprintf.o
D = ptpd-noposix
PTPD_CFLAGS = -ffreestanding -DPTPD_FREESTANDING -DWRPC_EXTRA_SLIM -DPTPD_MSBF -DPTPD_DBG
PTPD_CFLAGS += -Wall -ggdb -I$D/wrsw_hal \
-I$D/libptpnetif -I$D/PTPWRd \
-include $D/compat.h -include $D/libposix/ptpd-wrappers.h
PTPD_CFLAGS += -DPTPD_NO_DAEMON -DNEW_SINGLE_WRFSM #-DPTPD_DBGMSG
OBJS_PTPD = $D/PTPWRd/arith.o
OBJS_PTPD += $D/PTPWRd/bmc.o
OBJS_PTPD += $D/PTPWRd/dep/msg.o
OBJS_PTPD += $D/PTPWRd/dep/net.o
OBJS_PTPD += $D/PTPWRd/dep/servo.o
OBJS_PTPD += $D/PTPWRd/dep/sys.o
OBJS_PTPD += $D/PTPWRd/dep/timer.o
OBJS_PTPD += $D/PTPWRd/dep/wr_servo.o
OBJS_PTPD += $D/PTPWRd/protocol.o
OBJS_PTPD += $D/PTPWRd/wr_protocol.o
OBJS_PTPD_FREE = $D/libposix/freestanding-startup.o
OBJS_PTPD_FREE += $D/libposix/freestanding-display.o
OBJS_PTPD_FREE += $D/libposix/wr_nolibs.o
OBJS_PTPD_FREE += $D/libposix/freestanding-wrapper.o
ifeq ($(PLATFORM), zpu)
CROSS_COMPILE ?= /opt/gcc-zpu/bin/zpu-elf-
CFLAGS_PLATFORM = -abel -Wl,--relax -Wl,--gc-sections
LDFLAGS_PLATFORM = -abel -Wl,--relax -Wl,--gc-sections
OBJS_PLATFORM=
......@@ -11,29 +32,31 @@ else
CROSS_COMPILE ?= /opt/gcc-lm32/bin/lm32-elf-
CFLAGS_PLATFORM = -mmultiply-enabled -mbarrel-shift-enabled
LDFLAGS_PLATFORM = -mmultiply-enabled -mbarrel-shift-enabled -nostdlib -T target/lm32/ram.ld
OBJS_PLATFORM=target/lm32/crt0.o
OBJS_PLATFORM=target/lm32/crt0.o target/lm32/irq.o
endif
CC=$(CROSS_COMPILE)gcc
OBJCOPY=$(CROSS_COMPILE)objcopy
OBJDUMP=$(CROSS_COMPILE)objdump
CFLAGS= $(CFLAGS_PLATFORM) -ffunction-sections -fdata-sections -Os -Iinclude
CFLAGS= $(CFLAGS_PLATFORM) -ffunction-sections -fdata-sections -Os -Iinclude -include include/trace.h $(PTPD_CFLAGS)
LDFLAGS= $(LDFLAGS_PLATFORM) -ffunction-sections -fdata-sections -Os -Iinclude
OBJS=$(OBJS_PLATFORM) $(OBJS_WRC)
SIZE = $(CROSS_COMPILE)size
OBJS=$(OBJS_PLATFORM) $(OBJS_WRC) $(OBJS_PTPD) $(OBJS_PTPD_FREE)
OUTPUT=wrc
all: $(OBJS)
$(SIZE) -t $(OBJS)
${CC} -o $(OUTPUT).elf $(OBJS) $(LDFLAGS)
${OBJCOPY} -O binary $(OUTPUT).elf $(OUTPUT).bin
${OBJDUMP} -d $(OUTPUT).elf > $(OUTPUT)_disasm.S
./tools/genraminit $(OUTPUT).bin 0 > $(OUTPUT).ram
clean:
rm -f $(OBJS) $(OUTPUT).elf $(OUTPUT).bin $(OUTPUT).ram
%.o: %.c
${CC} $(CFLAGS) $(INCLUDE_DIR) $(LIB_DIR) -c $^ -o $@
${CC} $(CFLAGS) $(PTPD_CFLAGS) $(INCLUDE_DIR) $(LIB_DIR) -c $^ -o $@
load: all
./tools/zpu-loader $(OUTPUT).bin
......
dev/endpoint.c
View file @ 17b2de51
#include <stdio.h>
#include "board.h"
#include<endpoint.h>
#include <hw/endpoint_regs.h>
#include <hw/endpoint_mdio.h>
#define DMTD_AVG_SAMPLES 256
#define DMTD_MAX_PHASE 16384
#define UIS_PER_SERIAL_BIT 800
static int autoneg_enabled;
......@@ -34,14 +32,24 @@ static void pcs_write(int location,
}
static void set_mac_addr(uint8_t dev_addr[])
{
EP->MACL = ((uint32_t)dev_addr[3] << 24)
| ((uint32_t)dev_addr[2] << 16)
| ((uint32_t)dev_addr[1] << 8)
| ((uint32_t)dev_addr[0]);
{
EP->MACL = ((uint32_t)dev_addr[2] << 24)
| ((uint32_t)dev_addr[3] << 16)
| ((uint32_t)dev_addr[4] << 8)
| ((uint32_t)dev_addr[5]);
EP->MACH = ((uint32_t)dev_addr[0] << 8)
| ((uint32_t)dev_addr[1]);
}
EP->MACH = ((uint32_t)dev_addr[5] << 8)
| ((uint32_t)dev_addr[4]);
void get_mac_addr(uint8_t dev_addr[])
{
dev_addr[5] = (uint8_t)(EP->MACL & 0x000000ff);
dev_addr[4] = (uint8_t)(EP->MACL & 0x0000ff00) >> 8;
dev_addr[3] = (uint8_t)(EP->MACL & 0x00ff0000) >> 16;
dev_addr[2] = (uint8_t)(EP->MACL & 0xff000000) >> 24;
dev_addr[1] = (uint8_t)(EP->MACH & 0x000000ff);
dev_addr[0] = (uint8_t)(EP->MACH & 0x0000ff00) >> 8;
}
......@@ -71,8 +79,13 @@ int ep_enable(int enabled, int autoneg)
EP->ECR = EP_ECR_TX_EN_FRA | EP_ECR_RX_EN_FRA | EP_ECR_RST_CNT;
autoneg_enabled = autoneg;
pcs_write(MDIO_REG_MCR, MDIO_MCR_RESET); /* reset the PHY */
#if 1
pcs_write(MDIO_REG_MCR, MDIO_MCR_PDOWN); /* reset the PHY */
timer_delay(2000);
pcs_write(MDIO_REG_MCR, 0); /* reset the PHY */
// pcs_write(MDIO_REG_MCR, MDIO_MCR_RESET); /* reset the PHY */
#endif
pcs_write(MDIO_REG_ADVERTISE, 0);
mcr = MDIO_MCR_SPEED1000_MASK | MDIO_MCR_FULLDPLX_MASK;
......@@ -98,6 +111,7 @@ int ep_link_up()
return (msr & flags) == flags ? 1 : 0;
}
int ep_get_deltas(uint32_t *delta_tx, uint32_t *delta_rx)
{
*delta_tx = 0;
......@@ -108,5 +122,36 @@ void ep_show_counters()
{
int i;
for(i=0;i<16;i++)
mprintf("cntr%d = %d\n", i, EP->RMON_RAM[i]);
TRACE_DEV("cntr%d = %d\n", i, EP->RMON_RAM[i]);
}
int ep_get_psval(int32_t *psval)
{
uint32_t val;
val = EP->DMSR;
if(val & EP_DMSR_PS_RDY)
*psval = EP_DMSR_PS_VAL_R(val);
else
*psval = 0;
return val & EP_DMSR_PS_RDY;
}
int ep_cal_pattern_enable()
{
uint32_t val;
val = pcs_read(MDIO_REG_WR_SPEC);
val |= MDIO_WR_SPEC_TX_CAL;
pcs_write(MDIO_REG_WR_SPEC, val);
return 0;
}
int ep_cal_pattern_disable()
{
uint32_t val;
val = pcs_read(MDIO_REG_WR_SPEC);
val &= (~MDIO_WR_SPEC_TX_CAL);
pcs_write(MDIO_REG_WR_SPEC, val);
}
dev/minic.c
View file @ 17b2de51
......@@ -37,8 +37,8 @@
fc = (raw >> 28) & 0xf;
static volatile uint32_t dma_tx_buf[MINIC_DMA_RX_BUF_SIZE / 4];
static volatile uint32_t dma_rx_buf[MINIC_DMA_TX_BUF_SIZE / 4];
static volatile uint32_t dma_tx_buf[MINIC_DMA_TX_BUF_SIZE / 4];
static volatile uint32_t dma_rx_buf[MINIC_DMA_RX_BUF_SIZE / 4];
struct wr_minic {
volatile uint32_t *rx_head, *rx_base;
......@@ -72,6 +72,7 @@ static void minic_new_rx_buffer()
minic_writel(MINIC_REG_MCR, 0);
minic_writel(MINIC_REG_RX_ADDR, (uint32_t) minic.rx_base);
minic_writel(MINIC_REG_RX_AVAIL, (minic.rx_size - MINIC_MTU) >> 2);
// TRACE_DEV("Sizeof: %d Size : %d Avail: %d\n", minic.rx_size, (minic.rx_size - MINIC_MTU) >> 2);
minic_writel(MINIC_REG_MCR, MINIC_MCR_RX_EN);
}
......@@ -87,7 +88,10 @@ static void minic_new_tx_buffer()
void minic_init()
{
minic_writel(MINIC_REG_EIC_IDR, MINIC_EIC_IDR_RX);
minic_writel(MINIC_REG_EIC_ISR, MINIC_EIC_ISR_RX);
minic.rx_base = dma_rx_buf;
minic.rx_size = sizeof(dma_rx_buf);
......@@ -99,13 +103,18 @@ void minic_init()
minic_writel(MINIC_REG_EIC_IER, MINIC_EIC_IER_RX);
}
void minic_disable()
{
minic_writel(MINIC_REG_MCR, 0);
}
int minic_poll_rx()
{
uint32_t isr;
isr = minic_readl(MINIC_REG_EIC_ISR);
return isr;
return (isr & MINIC_EIC_ISR_RX) ? 1 : 0;//>>1;
}
int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_timestamp *hwts)
......@@ -121,10 +130,12 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
return 0;
desc_hdr = *minic.rx_head;
// TRACE_DEV("RX_FRAME_ENTER\n\nRxHead %x buffer at %x\n", minic.rx_head, minic.rx_base);
if(!RX_DESC_VALID(desc_hdr)) /* invalid descriptor? Weird, the RX_ADDR seems to be saying something different. Ignore the packet and purge the RX buffer. */
{
// mprintf("weird, invalid RX descriptor (%x, head %x)\n", desc_hdr, rx_head);
TRACE_DEV("weird, invalid RX descriptor (%x, head %x)\n", desc_hdr, minic.rx_head);
minic_new_rx_buffer();
return 0;
}
......@@ -132,6 +143,8 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
payload_size = RX_DESC_SIZE(desc_hdr);
num_words = ((payload_size + 3) >> 2) + 1;
// TRACE_DEV("NWords %d\n", num_words);
/* valid packet */
if(!RX_DESC_ERROR(desc_hdr))
{
......@@ -161,7 +174,11 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
else
hwts->ahead = 0;
hwts->nsec = counter_r * 8;
TRACE_DEV("TS minic_rx_frame: %d.%d\n", hwts->utc, hwts->nsec);
}
n_recvd = (buf_size < payload_size ? buf_size : payload_size);
......@@ -169,14 +186,23 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
/* FIXME: VLAN support */
memcpy(hdr, (void*)minic.rx_head + 4, ETH_HEADER_SIZE);
//TRACE_DEV("%s: packet: ", __FUNCTION__);
//for(i=0; i<ETH_HEADER_SIZE; i++) TRACE_DEV("%x ", *(hdr+i));
memcpy(payload, (void*)minic.rx_head + 4 + ETH_HEADER_SIZE, n_recvd - ETH_HEADER_SIZE);
//for(i=0; i<n_recvd-ETH_HEADER_SIZE; i++) TRACE_DEV("%x ", *(payload+i));
/* for(i=0;i<n_recvd;i++)
mprintf("%x ", buf[i]);
mprintf("---\n");*/
/* for(i=0;i<n_recvd-14;i++)
TRACE_DEV("%x ", payload[i]);
TRACE_DEV("---\n");
*/
// TRACE_DEV("nwords_avant: %d\n", num_words);
minic.rx_head += num_words;
} else { // RX_DESC_ERROR
// TRACE_DEV("nwords_avant_err: %d\n", num_words);
minic.rx_head += num_words;
}
......@@ -184,6 +210,7 @@ int minic_rx_frame(uint8_t *hdr, uint8_t *payload, uint32_t buf_size, struct hw_
if(rx_addr_cur < (uint32_t)minic.rx_head) /* nothing new in the buffer? */
{
// TRACE_DEV("MoreData? %x, head %x\n", rx_addr_cur, minic.rx_head);
if(minic_readl(MINIC_REG_MCR) & MINIC_MCR_RX_FULL)
minic_new_rx_buffer();
......@@ -201,12 +228,21 @@ int minic_tx_frame(uint8_t *hdr, uint8_t *payload, uint32_t size, struct hw_time
{
uint32_t d_hdr, mcr, nwords;
minic_new_tx_buffer();
nwords = ((size + 1) >> 1) - 1;
memset(minic.tx_head, 0x0, size + 16);
memset((void*)minic.tx_head + 4, 0, size < 60 ? 60 : size);
memcpy((void*)minic.tx_head + 4, hdr, ETH_HEADER_SIZE);
memcpy((void*)minic.tx_head + 4 + ETH_HEADER_SIZE, payload, size - ETH_HEADER_SIZE);
if(size < 60)
size = 60;
nwords = ((size + 1) >> 1) - 1;
if(hwts)
{
......@@ -249,6 +285,9 @@ int minic_tx_frame(uint8_t *hdr, uint8_t *payload, uint32_t size, struct hw_time
hwts->utc = utc;
hwts->ahead = 0;
hwts->nsec = counter_r * 8;
TRACE_DEV("TS minic_tx_frame: %d.%d\n", hwts->utc, hwts->nsec);
}
tx_oob_val++;
......
dev/pps_gen.c
View file @ 17b2de51
......@@ -24,7 +24,7 @@ void pps_gen_init()
ppsg_writel( PPSG_REG_CR, cr);
ppsg_writel( PPSG_REG_ADJ_UTCLO, 1285700840);
ppsg_writel( PPSG_REG_ADJ_UTCLO, 100 );
ppsg_writel( PPSG_REG_ADJ_UTCHI, 0);
ppsg_writel( PPSG_REG_ADJ_NSEC, 0);
......@@ -36,24 +36,30 @@ void pps_gen_adjust_nsec(int32_t how_much)
{
uint32_t cr;
mprintf("AdjustPPS: %d nanoseconds\n", how_much);
TRACE_DEV("ADJ: nsec %d nanoseconds\n", how_much);
#if 1
ppsg_writel( PPSG_REG_ADJ_UTCLO, 0);
ppsg_writel( PPSG_REG_ADJ_UTCHI, 0);
ppsg_writel( PPSG_REG_ADJ_NSEC, ( how_much / 8 ));
ppsg_writel( PPSG_REG_CR, PPSG_CR_CNT_EN | PPSG_CR_PWIDTH_W(PPS_PULSE_WIDTH) | PPSG_CR_CNT_ADJ);
#endif
}
void shw_pps_gen_adjust_utc(int32_t how_much)
void pps_gen_adjust_utc(int32_t how_much)
{
uint32_t cr;
mprintf("AdjustUTC: %d seconds\n", how_much);
#if 1
TRACE_DEV("ADJ: utc %d seconds\n", how_much);
ppsg_writel( PPSG_REG_ADJ_UTCLO, how_much);
ppsg_writel( PPSG_REG_ADJ_UTCHI, 0);
ppsg_writel( PPSG_REG_ADJ_NSEC, 0);
ppsg_writel( PPSG_REG_CR, PPSG_CR_CNT_EN | PPSG_CR_PWIDTH_W(PPS_PULSE_WIDTH) | PPSG_CR_CNT_ADJ);
#endif
}
int pps_gen_busy()
......
dev/softpll.c
View file @ 17b2de51
#include "board.h"
#include "irq.h"
volatile int irq_cnt = 0;
__attribute__((interrupt)) void softpll_irq()
#include <hw/softpll_regs.h>
#define TAG_BITS 17
#define HPLL_N 14
#define HPLL_DAC_BIAS 30000
#define PI_FRACBITS 12
#define CHAN_FB 8
#define CHAN_REF 4
#define CHAN_PERIOD 2
//#define CHAN_HPLL 1
#define READY_FB (8<<4)
#define READY_REF (4<<4)
#define READY_PERIOD (2<<4)
//#define READY_HPLL (1<<4)
static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
struct softpll_config {
int hpll_f_kp;
int hpll_f_ki;
int hpll_f_setpoint;
int hpll_ld_f_samples;
int hpll_ld_f_threshold;
int hpll_p_kp;
int hpll_p_ki;
int hpll_ld_p_samples;
int hpll_ld_p_threshold;
int hpll_delock_threshold;
int hpll_dac_bias;
int dpll_f_kp;
int dpll_f_ki;
int dpll_p_kp;
int dpll_p_ki;
int dpll_ld_samples;
int dpll_ld_threshold;
int dpll_delock_threshold;
int dpll_dac_bias;
int dpll_deglitcher_threshold;
};
const struct softpll_config pll_cfg =
{
/* Helper PLL */
28*32*16, // f_kp
20*32*16, // f_ki
16, // setpoint
1000, // lock detect freq samples
2, // lock detect freq threshold
2.0*32*16, // p_kp
0.05*32*16, // p_ki
1000, // lock detect phase samples
300, // lock detect phase threshold
500, // delock threshold
32000, // HPLL dac bias
/* DMTD PLL */
100, // f_kp
600, // f_ki
1304, // p_kp
10, // p_ki
1000, // lock detect samples
500, // lock detect threshold
500, // delock threshold
32000, // DPLL dac bias
1000 // deglitcher threshold
};
struct softpll_state {
int h_lock_counter;
int h_i;
int h_dac_bias;
int h_p_setpoint;
int h_freq_mode;
int h_locked;
int d_dac_bias;
int d_tag_ref_d0;
int d_tag_fb_d0;
int d_tag_ref_ready;
int d_tag_fb_ready;
int d_period_ref;
int d_period_fb;
int d_freq_mode;
int d_lock_counter;
int d_i;
int d_p_setpoint;
int d_phase_shift;
int d_locked;
};
int eee, dve;
static volatile struct softpll_state pstate;
void _irq_entry()
{
int dv;
int tag_ref_ready = 0;
int tag_fb_ready = 0;
int tag_ref;
int tag_fb;
if(SPLL->CSR & READY_REF)
{
tag_ref = SPLL->TAG_REF;
tag_ref_ready = 1;
}
if(SPLL->CSR & READY_FB)
{
tag_fb = SPLL->TAG_FB;
tag_fb_ready = 1;
}
/* HPLL: active frequency branch */
if(pstate.h_freq_mode)
{
int freq_err = SPLL->PER_HPLL;
if(freq_err & 0x100) freq_err |= 0xffffff00; /* sign-extend */
freq_err += pll_cfg.hpll_f_setpoint;
/* PI control */
pstate.h_i += freq_err;
dv = ((pstate.h_i * pll_cfg.hpll_f_ki + freq_err * pll_cfg.hpll_f_kp) >> PI_FRACBITS) + pll_cfg.hpll_dac_bias;
SPLL->DAC_HPLL = dv; /* update DAC */
/* lock detection */
if(freq_err >= -pll_cfg.hpll_ld_f_threshold && freq_err <= pll_cfg.hpll_ld_f_threshold)
pstate.h_lock_counter++;
else
pstate.h_lock_counter=0;
/* frequency has been stable for quite a while? switch to phase branch */
if(pstate.h_lock_counter == pll_cfg.hpll_ld_f_samples)
{
pstate.h_freq_mode = 0;
pstate.h_dac_bias = dv;
pstate.h_i = 0;
pstate.h_lock_counter = 0;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_REF);
}
/* HPLL: active phase branch */
} else if (tag_ref_ready) {
if(pstate.h_p_setpoint < 0) /* we don't have yet any phase samples? */
pstate.h_p_setpoint = tag_ref & 0x3fff;
else {
int phase_err;
phase_err = (tag_ref & 0x3fff) - pstate.h_p_setpoint;
pstate.h_i += phase_err;
dv = ((pstate.h_i * pll_cfg.hpll_p_ki + phase_err * pll_cfg.hpll_p_kp) >> PI_FRACBITS) + pstate.h_dac_bias;
SPLL->DAC_HPLL = dv; /* Update DAC */
if(abs(phase_err) >= pll_cfg.hpll_delock_threshold && pstate.h_locked)
{
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD); /* fall back to freq mode */
pstate.h_locked = 0;
pstate.h_freq_mode = 1;
}
if(abs(phase_err) <= pll_cfg.hpll_ld_p_threshold && !pstate.h_locked)
pstate.h_lock_counter++;
else
pstate.h_lock_counter = 0;
if(pstate.h_lock_counter == pll_cfg.hpll_ld_p_samples)
{
SPLL->CSR |= SPLL_CSR_TAG_EN_W(CHAN_FB); /* enable feedback channel and start DMPLL */
pstate.h_locked = 1;
pstate.d_tag_ref_d0 = -1;
pstate.d_tag_fb_d0 = -1;
pstate.d_freq_mode = 1;
pstate.d_p_setpoint = 0;
pstate.d_lock_counter = 0;
pstate.d_i = 0;
pstate.d_locked = 0;
}
}
}
/* DMPLL */
if(pstate.h_locked && pstate.d_freq_mode)
{
int freq_err;
if(tag_ref_ready)
{
if(pstate.d_tag_ref_d0 > 0)
{
int tmp = tag_ref - pstate.d_tag_ref_d0;
if(tmp < 0) tmp += (1<<TAG_BITS)-1;
pstate.d_period_ref = tmp;
}
pstate.d_tag_ref_d0 = tag_ref;
}
if(tag_fb_ready)
{
if(pstate.d_tag_fb_d0 > 0)
{
int tmp = tag_fb - pstate.d_tag_fb_d0;
if(tmp < 0) tmp += (1<<TAG_BITS)-1;
pstate.d_period_fb = tmp;
}
pstate.d_tag_fb_d0 = tag_fb;
}
freq_err = - pstate.d_period_fb + pstate.d_period_ref;
pstate.d_i += freq_err;
dv = ((pstate.d_i * pll_cfg.dpll_f_ki + freq_err * pll_cfg.dpll_f_kp) >> PI_FRACBITS) + pll_cfg.dpll_dac_bias;
SPLL->DAC_DMPLL = dv;
if(abs(freq_err) <= pll_cfg.dpll_ld_threshold)
pstate.d_lock_counter++;
else
pstate.d_lock_counter=0;
/* frequency has been stable for quite a while? switch to phase branch */
if(pstate.d_lock_counter == pll_cfg.dpll_ld_samples)
{
pstate.d_freq_mode = 0;
pstate.d_dac_bias = dv;
pstate.d_i = 0;
pstate.d_lock_counter = 0;
pstate.d_tag_ref_ready = 0;
pstate.d_tag_fb_ready = 0;
pstate.d_p_setpoint = 0;
}
}
/* DMPLL phase-lock */
if(pstate.h_locked && !pstate.d_freq_mode)
{
int phase_err = 0;
if(tag_ref_ready)
pstate.d_tag_ref_d0 = tag_ref;
tag_ref = pstate.d_tag_ref_d0 ;
tag_fb += pstate.d_p_setpoint; // was tag_fb
tag_fb &= (1<<TAG_BITS)-1;
//if(tag_ref > (1<<TAG_BITS)) tag_ref -= (1<<TAG_BITS);
//if(tag_ref < 0) tag_ref += (1<<TAG_BITS);
if(tag_fb_ready)
{
while (tag_ref > 16384 ) tag_ref-=16384;
while (tag_fb > 16384 ) tag_fb-=16384;
phase_err = tag_ref-tag_fb;
pstate.d_i += phase_err;
dv = ((pstate.d_i * pll_cfg.dpll_p_ki + phase_err * pll_cfg.dpll_p_kp) >> PI_FRACBITS) + pll_cfg.dpll_dac_bias;
SPLL->DAC_DMPLL = dv;
eee=phase_err;
}
if(pstate.d_p_setpoint < pstate.d_phase_shift)
pstate.d_p_setpoint++;
else if(pstate.d_p_setpoint > pstate.d_phase_shift)
pstate.d_p_setpoint--;
if(abs(phase_err) <= pll_cfg.dpll_ld_threshold && !pstate.d_locked)
pstate.d_lock_counter++;
else
pstate.d_lock_counter = 0;
if(pstate.d_lock_counter == pll_cfg.dpll_ld_samples)
pstate.d_locked = 1;
}
clear_irq();
}
void softpll_enable()
{
SPLL->DAC_HPLL = pll_cfg.hpll_dac_bias;
SPLL->DAC_DMPLL = pll_cfg.dpll_dac_bias;
SPLL->DEGLITCH_THR = pll_cfg.dpll_deglitcher_threshold;
pstate.h_p_setpoint = -1;
pstate.h_i = 0;
pstate.h_freq_mode = 1;
pstate.h_lock_counter = 0;
pstate.h_locked = 0;
SPLL->CSR = SPLL_CSR_TAG_EN_W(CHAN_PERIOD);
SPLL->EIC_IER = 1;
enable_irq();
// softpll_test();
}
int softpll_check_lock()
{
TRACE_DEV("LCK h:f%d l%d d: f%d l%d\n",
pstate.h_freq_mode ,pstate.h_locked,
pstate.d_freq_mode, pstate.d_locked);
return pstate.h_locked && pstate.d_locked;
}
int softpll_busy()
{
return pstate.d_p_setpoint != pstate.d_phase_shift;
}
void softpll_set_phase(int ps)
{
pstate.d_phase_shift = (int32_t) (((int64_t)ps * 16384LL) / 8000LL);
TRACE_DEV("ADJ: phase %d [ps], %d units\n", ps, pstate.d_phase_shift);
}
void softpll_disable()
{
SPLL->CSR = 0;
disable_irq();
}
int softpll_get_setpoint()
{
irq_cnt++;
uart_write_byte('x');
}
\ No newline at end of file
return pstate.d_p_setpoint;
}
include/board.h
View file @ 17b2de51
......@@ -7,6 +7,7 @@
#define BASE_PPSGEN 0x50000
#define BASE_EP 0x20000
#define BASE_MINIC 0x10000
#define BASE_SOFTPLL 0x40000
#define CPU_CLOCK 62500000ULL
......
include/endpoint.h
View file @ 17b2de51
#ifndef __ENDPOINT_H
#define __ENDPOINT_H