static unsigned long __calc_values(struct alpha_pll_clk *pll,
unsigned long rate, int *l_val, u64 *a_val, bool round_up)
{
u32 parent_rate;
u64 remainder;
u64 quotient;
unsigned long freq_hz;
int alpha_bw = ALPHA_BITWIDTH;
parent_rate = clk_get_rate(pll->c.parent);
quotient = rate;
remainder = do_div(quotient, parent_rate);
*l_val = quotient;
if (!remainder) {
*a_val = 0;
return rate;
}
if (pll->is_fabia)
alpha_bw = FABIA_ALPHA_BITWIDTH;
/* Upper ALPHA_BITWIDTH bits of Alpha */
quotient = remainder << alpha_bw;
remainder = do_div(quotient, parent_rate);
if (remainder && round_up)
quotient++;
*a_val = quotient;
freq_hz = compute_rate(pll, *l_val, *a_val);
return freq_hz;
}
static enum handoff alpha_pll_handoff(struct clk *c)
{
struct alpha_pll_clk *pll = to_alpha_pll_clk(c);
struct alpha_pll_masks *masks = pll->masks;
u64 a_val;
u32 alpha_en, l_val, regval;
/* Set the PLL_HW_UPDATE_LOGIC_BYPASS bit before continuing */
if (pll->dynamic_update) {
regval = readl_relaxed(MODE_REG(pll));
regval |= ALPHA_PLL_HW_UPDATE_LOGIC_BYPASS;
writel_relaxed(regval, MODE_REG(pll));
}
update_vco_tbl(pll);
if (!is_locked(pll)) {
if (pll->slew) {
if (c->rate && dyna_alpha_pll_set_rate(c, c->rate))
WARN(1, "%s: Failed to configure rate\n",
c->dbg_name);
} else {
if (c->rate && alpha_pll_set_rate(c, c->rate))
WARN(1, "%s: Failed to configure rate\n",
c->dbg_name);
}
__init_alpha_pll(c);
return HANDOFF_DISABLED_CLK;
} else if (pll->fsm_en_mask && !is_fsm_mode(MODE_REG(pll))) {
WARN(1, "%s should be in FSM mode but is not\n", c->dbg_name);
}
l_val = readl_relaxed(L_REG(pll));
/* read u64 in two steps to satisfy alignment constraint */
a_val = readl_relaxed(A_REG(pll) + 0x4);
a_val = a_val << 32 | readl_relaxed(A_REG(pll));
/* get upper 32 bits */
a_val = a_val >> (ALPHA_REG_BITWIDTH - ALPHA_BITWIDTH);
alpha_en = readl_relaxed(ALPHA_EN_REG(pll));
alpha_en &= masks->alpha_en_mask;
if (!alpha_en)
a_val = 0;
c->rate = compute_rate(pll, l_val, a_val);
/*
* Unconditionally vote for the PLL; it might be on because of
* another master's vote.
*/
if (pll->fsm_en_mask)
__alpha_pll_vote_enable(pll);
return HANDOFF_ENABLED_CLK;
}
void init(u8 spi_int_priority, bool fast_spi_irq, u8 aux_int_priority, bool fast_aux_irq)
{
// deselect all devices
select_device(select::none);
regs.master_control.spi_1_clock_enable = true;
regs.master_control.spi_1_pin_control = true;
regs.global_control.enable = true;
regs.global_control.reset = true; // reset must be done after controller is enabled
regs.global_control.reset = false;
// Aux controller supports up to 16.6 mbps. If the other devices cannot go as high, we may need to reprogram the rate for each transfer depending on the target
//regs.control.rate = compute_rate(16100000); // effective rate will be 12.5Mbps, this divider is applied directly on the h_clock, so it's not very precise for high rates
regs.control.rate = compute_rate(8000000);
regs.control.master = true;
regs.control.bitnum = 0xF; // 16 bits
regs.control.shift_off = false; // enable the clock output
regs.control.thr = 0; // select no threshold (interrupt as soon as 1 entry in FIFO)
regs.control.mode = 1; // clock starts low, data sampled on falling edge
regs.control.lsb_first = false; // send most significant bit first
regs.control.bhalt = false; // don't use the busy signal
regs.control.unidir = true; // bidirectional
regs.control.rxtx = 1; // output at all times, we don't mux the pin
regs.frame_count = 0; // single frame sent, no block transfers
regs.interrupt.intthr = true;
regs.interrupt.inteot = true;
regs.timer_control.mode = 0;
regs.timer_control.pirqe = 1;
regs.timer_control.tirqe = 0;
// register the SPI interrupt handler - we don't need it yet, our only spi device is the aux controller which sends us an interrupt on its own. for the IMU, this may change.
//get_int_ctrl().install_service_routine(interrupt::id::spi_1, spi_int_priority, fast_spi_irq, interrupt::trigger::high_level, static_spi_isr);
//get_int_ctrl().enable_interrupt(interrupt::id::spi_1);
// register the auxiliary controller interrupt handler (tells us when data is ready)
get_int_ctrl().install_service_routine(interrupt::id::aux_ctrl_spi, aux_int_priority, fast_aux_irq, interrupt::trigger::positive_edge, static_aux_isr);
get_int_ctrl().enable_interrupt(interrupt::id::aux_ctrl_spi);
}
static enum handoff fabia_alpha_pll_handoff(struct clk *c)
{
struct alpha_pll_clk *pll = to_alpha_pll_clk(c);
u64 a_val;
u32 l_val, regval;
/* Set the PLL_HW_UPDATE_LOGIC_BYPASS bit before continuing */
regval = readl_relaxed(MODE_REG(pll));
regval |= ALPHA_PLL_HW_UPDATE_LOGIC_BYPASS;
writel_relaxed(regval, MODE_REG(pll));
if (!is_locked(pll)) {
if (c->rate && fabia_alpha_pll_set_rate(c, c->rate))
WARN(1, "%s: Failed to configure rate\n", c->dbg_name);
__init_alpha_pll(c);
return HANDOFF_DISABLED_CLK;
} else if (pll->fsm_en_mask && !is_fsm_mode(MODE_REG(pll))) {
WARN(1, "%s should be in FSM mode but is not\n", c->dbg_name);
}
l_val = readl_relaxed(FABIA_L_REG(pll));
if (pll->fabia_frac_offset)
a_val = readl_relaxed(FABIA_FRAC_OFF(pll));
else
a_val = readl_relaxed(FABIA_FRAC_REG(pll));
c->rate = compute_rate(pll, l_val, a_val);
/*
* Unconditionally vote for the PLL; it might be on because of
* another master's vote.
*/
if (pll->fsm_en_mask)
__alpha_pll_vote_enable(pll);
return HANDOFF_ENABLED_CLK;
}
void compute_estimation(aodv_neigh *tmp_neigh) {
int recv_delay;
//Nominal rate computation
//Add last own transmission delay measured by our neighbour
tmp_neigh->ett.count_rcv++;
if (tmp_neigh->ett.count_rcv == tmp_neigh->ett.ett_window) { //Let's compute the estimation!
recv_delay = compute_delay(tmp_neigh);
if (recv_delay == 0 && tmp_neigh->ett.ett_window == ETT_PROBES_MAX) {
if (tmp_neigh->ett.reset) { //Second consectutive round without an estimation!
#ifdef DEBUG
printk(
"Capacity Estimation with %s is not possible - Second: Resetting\n",
inet_ntoa(tmp_neigh->ip));
#endif
reset_ett(tmp_neigh);
} else { //First round without an estimation!
#ifdef DEBUG
printk(
"Capacity Estimation with %s is not possible - First: Using last measured values\n",
inet_ntoa(tmp_neigh->ip));
#endif
//lets start a new measurements round
delay_vector_init(&(tmp_neigh->ett.recv_delays[0]));
tmp_neigh->ett.count_rcv = 0;
tmp_neigh->ett.ett_window = ETT_PROBES_MIN;
tmp_neigh->ett.reset = 1;
}
send_ett_info(tmp_neigh, tmp_neigh->send_rate, FALSE);
return;
}
if (recv_delay == 0) {
//Increase the size of the estimation window
tmp_neigh->ett.ett_window += 2; //lets do two more measurements
if (tmp_neigh->ett.ett_window > ETT_PROBES_MAX)
tmp_neigh->ett.ett_window=ETT_PROBES_MAX;
return;
}
if (recv_delay == DEFAULT_ETT_METRIC) {
#ifdef DEBUG
printk(
"%s does not receive my ETT_PROBES: Capacity Estimation is not possible - Resetting ETT...\n",
inet_ntoa(tmp_neigh->ip));
#endif
reset_ett(tmp_neigh);
return;
}
//lets start a new measurement round
delay_vector_init(&(tmp_neigh->ett.recv_delays[0]));
tmp_neigh->ett.count_rcv = 0;
tmp_neigh->ett.ett_window = ETT_PROBES_MIN;
tmp_neigh->ett.reset = 0;
tmp_neigh->send_rate = compute_rate(recv_delay);
send_ett_info(tmp_neigh, tmp_neigh->send_rate, FALSE);
}
}
