void run_equalizer(FloatBuffer *fbin, FloatBuffer *fbout, EqualizerData *data)
{
int i, rpos;
float lpf_out[EQUALIZER_BANDS + 1];
float sum = 0.0;
/* Save the input read location; we can reuse the same input data on all
* of the LPFs. */
rpos = fbin->rpos;
/* Run the child filters. */
for (i = 0; i < EQUALIZER_BANDS + 1; i++)
{
fbin->rpos = rpos;
run_lpf(fbin, &data->fb[i], &data->lpf[i]);
lpf_out[i] = data->fb[i].buff[data->fb[i].rpos++];
}
/* Now process the results of the filters. Remember that each band is
* output(hi)-output(lo). */
for (i = 0; i < EQUALIZER_BANDS; i++)
sum += (lpf_out[i+1] - lpf_out[i]) * data->gain[i];
/* Write that result. */
fb_ensure_writable(fbout, 1);
fbout->buff[fbout->rlen++] = sum;
}
void begin(void)
{
int i;
FloatBuffer fb1, fb2, fb3, fb4;
LPFData lpf_data;
EqualizerData eq_data;
fb1.rpos = fb1.rlen = 0;
fb2.rpos = fb2.rlen = 0;
fb3.rpos = fb3.rlen = 0;
fb4.rpos = fb4.rlen = 0;
init_lpf_data(&lpf_data, CUTOFF_FREQUENCY, NUM_TAPS, DECIMATION);
init_equalizer(&eq_data);
/* Startup: */
get_floats(&fb1);
/* LPF needs at least NUM_TAPS+1 inputs; get_floats is fine. */
run_lpf(&fb1, &fb2, &lpf_data);
/* run_demod needs 1 input, OK here. */
/* run_equalizer needs 51 inputs (same reason as for LPF). This means
* running the pipeline up to demod 50 times in advance: */
for (i = 0; i < 64; i++)
{
if (fb1.rlen - fb1.rpos < NUM_TAPS + 1)
get_floats(&fb1);
run_lpf(&fb1, &fb2, &lpf_data);
run_demod(&fb2, &fb3);
}
/* Main loop: */
while (numiters == -1 || numiters-- > 0)
{
/* The low-pass filter will need NUM_TAPS+1 items; read them if we
* need to. */
if (fb1.rlen - fb1.rpos < NUM_TAPS + 1)
get_floats(&fb1);
run_lpf(&fb1, &fb2, &lpf_data);
run_demod(&fb2, &fb3);
run_equalizer(&fb3, &fb4, &eq_data);
write_floats(&fb4);
}
}
void
startup(void)
{
/* Monitor code is @ 0x8001000 */
void (*fptr)() = (void *)0x8001000;
int frames_rcvd = 0, addrchars, i;
uchar demod_dst[16+1];
LPFData lpf_data;
/* If we are the clone of someone, my_id is already set, keep it, for timeslot stuff */
if (my_id == -1)
{
addrchars = strlen((char *)NIC_OUI);
my_id = 0;
for (i = addrchars-1; i >= 0; i--)
{
my_id += (*(NIC_OUI+i) - '0') * pow10(addrchars - 1 - i);
}
}
sprint(&demod_dst[0], "%d", my_id+1);
hdlr_install();
/* Power up both RX interfaces */
devnet_ctl(NIC_NCR_WRITE, NIC_CMD_POWERUP, 0);
devnet_ctl(NIC_NCR_WRITE, NIC_CMD_POWERUP, 1);
print("swradio-lpf [%d] installing vector code...\n", my_id);
/* Make sure we're talking on regular segment */
/* uncommenmt if doing segmented network */
sample_flag = 0;
syn_sent = 0;
fb1.rpos = fb1.rlen = 0;
fb2.rpos = fb2.rlen = 0;
init_lpf_data(&lpf_data, CUTOFF_FREQUENCY, NUM_TAPS, DECIMATION);
/* LPF needs at least NUM_TAPS+1 inputs; get_floats is fine. */
while (!sample_flag)
{
sleep();
}
run_lpf((void *)&fb1, (void *)&fb2, &lpf_data);
sample_flag = 0;
/* */
/* run_demod needs 1 input, OK here. run_equalizer needs 51 */
/* inputs (same reason as for LPF). This means running the */
/* pipeline up to demod 50 times in advance: */
/* */
while (frames_rcvd < NUM_TAPS)
{
while (!sample_flag)
{
sleep();
}
frames_rcvd++;
sample_flag = 0;
run_lpf((void *)&fb1, (void *)&fb2, &lpf_data);
/*
print("swradio-lpf (node [%d]) sending [%d] bytes to DEMOD (node [%s])\n",
my_id, sizeof(fb1), demod_dst);
print("fb2.rpos = [%d], fb2.rlen = [%d]\n", fb2.rpos, fb2.rlen);
*/
devnet_xmit((uchar *)&demod_dst, 0, (uchar *)&fb2, sizeof(fb2), 1);
/* */
/* Since we are doing Call-By-Value, we must make this change */
/* here. The corresponding calculation in run_demod is a No-op */
/* */
fb2.rpos++;
}
/* Wait till intr_hdlr is done */
done_flag = 0;
while (!done_flag)
{
if (sample_flag)
{
run_lpf((void *)&fb1, (void *)&fb2, &lpf_data);
/*
print("swradio-lpf (node [%d]) sending [%d] bytes to DEMOD (node [%s])\n",
my_id, sizeof(fb1), demod_dst);
print("fb2.rpos = [%d], fb2.rlen = [%d]\n", fb2.rpos, fb2.rlen);
*/
/* Now send fb2 to be Demod'd. We dont re-xmit */
devnet_xmit((uchar *)&demod_dst, 0, (uchar *)&fb2, sizeof(fb2), 1);
/* */
/* Since we are doing Call-By-Value, we must make this change */
/* here. The corresponding calculation in run_demod is a No-op */
/* */
fb2.rpos++;
sample_flag = 0;
}
sleep();
}
/* Forcefully go to monitor */
fptr();
return;
}
