//
// Copyright 2010 Ettus Research LLC
//
/* -*- c++ -*- */
/*
* Copyright 2007,2008,2009 Free Software Foundation, Inc.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see .
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "app_common_v2.h"
#include "buffer_pool.h"
#include "memcpy_wa.h"
#include "ethernet.h"
#include "nonstdio.h"
#include "print_rmon_regs.h"
#include "db.h"
#include "clocks.h"
#include "u2_init.h"
#include
#include
#include "usrp2_fw_common.h"
volatile bool link_is_up = false; // eth handler sets this
int cpu_tx_buf_dest_port = PORT_ETH;
// If this is non-zero, this dbsm could be writing to the ethernet
dbsm_t *ac_could_be_sending_to_eth;
static unsigned char exp_seqno __attribute__((unused)) = 0;
static bool
set_time(const op_set_time_t *p)
{
//printf("Setting time: secs %u, ticks %u\n", p->time_secs, p->time_ticks);
//sr_time64->secs = p->time_secs; //set below...
sr_time64->ticks = p->time_ticks;
switch (p->type){
case OP_SET_TIME_TYPE_NOW:
sr_time64->imm = 1;
break;
case OP_SET_TIME_TYPE_PPS:
sr_time64->imm = 0;
break;
}
sr_time64->secs = p->time_secs; //set this last to latch the regs
return true;
}
static inline bool
config_clock_cmd(const op_config_clock_t *p)
{
//handle the 10 mhz ref source
clocks_mimo_config(p->flags & MC_REF_CLK_MASK);
//handle the pps config
uint32_t pps_flags = 0;
if (p->flags & MC_PPS_POLARITY_NEG) pps_flags |= 0x00 << 0;
if (p->flags & MC_PPS_POLARITY_POS) pps_flags |= 0x01 << 0;
if (p->flags & MC_PPS_SOURCE_SMA) pps_flags |= 0x00 << 1;
if (p->flags & MC_PPS_SOURCE_MIMO) pps_flags |= 0x01 << 1;
sr_time64->flags = pps_flags;
return true;
}
static size_t
op_id_cmd(const op_generic_t *p,
void *reply_payload, size_t reply_payload_space)
{
op_id_reply_t *r = (op_id_reply_t *) reply_payload;
if (reply_payload_space < sizeof(*r)) // no room
return 0;
// Build reply subpacket
r->opcode = OP_ID_REPLY;
r->len = sizeof(op_id_reply_t);
r->rid = p->rid;
r->addr = *ethernet_mac_addr();
r->hw_rev = (u2_hw_rev_major << 8) | u2_hw_rev_minor;
// r->fpga_md5sum = ; // FIXME
// r->sw_md5sum = ; // FIXME
return r->len;
}
static size_t
config_tx_v2_cmd(const op_config_tx_v2_t *p,
void *reply_payload, size_t reply_payload_space)
{
op_config_tx_reply_v2_t *r = (op_config_tx_reply_v2_t *) reply_payload;
if (reply_payload_space < sizeof(*r))
return 0; // no room
struct tune_result tune_result;
memset(&tune_result, 0, sizeof(tune_result));
bool ok = true;
if (p->valid & CFGV_GAIN){
//ok &= db_set_gain(tx_dboard, p->gain);
}
if (p->valid & CFGV_FREQ){
bool was_streaming = is_streaming();
if (was_streaming)
stop_rx_cmd();
u2_fxpt_freq_t f = u2_fxpt_freq_from_hilo(p->freq_hi, p->freq_lo);
bool tune_ok = false;//db_tune(tx_dboard, f, &tune_result);
ok &= tune_ok;
print_tune_result("Tx", tune_ok, f, &tune_result);
if (was_streaming)
restart_streaming();
}
if (p->valid & CFGV_INTERP_DECIM){
int interp = p->interp;
int hb1 = 0;
int hb2 = 0;
if (!(interp & 1)){
hb2 = 1;
interp = interp >> 1;
}
if (!(interp & 1)){
hb1 = 1;
interp = interp >> 1;
}
if (interp < MIN_CIC_INTERP || interp > MAX_CIC_INTERP)
ok = false;
else {
dsp_tx_regs->interp_rate = (hb1<<9) | (hb2<<8) | interp;
// printf("Interp: %d, register %d\n", p->interp, (hb1<<9) | (hb2<<8) | interp);
}
}
if (p->valid & CFGV_SCALE_IQ){
dsp_tx_regs->scale_iq = p->scale_iq;
}
// Build reply subpacket
r->opcode = OP_CONFIG_TX_REPLY_V2;
r->len = sizeof(*r);
r->rid = p->rid;
r->ok = ok;
r->inverted = tune_result.inverted;
r->baseband_freq_hi = u2_fxpt_freq_hi(tune_result.baseband_freq);
r->baseband_freq_lo = u2_fxpt_freq_lo(tune_result.baseband_freq);
r->duc_freq_hi = u2_fxpt_freq_hi(tune_result.dxc_freq);
r->duc_freq_lo = u2_fxpt_freq_lo(tune_result.dxc_freq);
r->residual_freq_hi = u2_fxpt_freq_hi(tune_result.residual_freq);
r->residual_freq_lo = u2_fxpt_freq_lo(tune_result.residual_freq);
return r->len;
}
static size_t
config_rx_v2_cmd(const op_config_rx_v2_t *p,
void *reply_payload, size_t reply_payload_space)
{
op_config_rx_reply_v2_t *r = (op_config_rx_reply_v2_t *) reply_payload;
if (reply_payload_space < sizeof(*r))
return 0; // no room
struct tune_result tune_result;
memset(&tune_result, 0, sizeof(tune_result));
bool ok = true;
if (p->valid & CFGV_GAIN){
//ok &= db_set_gain(rx_dboard, p->gain);
}
if (p->valid & CFGV_FREQ){
bool was_streaming = is_streaming();
if (was_streaming)
stop_rx_cmd();
u2_fxpt_freq_t f = u2_fxpt_freq_from_hilo(p->freq_hi, p->freq_lo);
bool tune_ok = false;//db_tune(rx_dboard, f, &tune_result);
ok &= tune_ok;
print_tune_result("Rx", tune_ok, f, &tune_result);
if (was_streaming)
restart_streaming();
}
if (p->valid & CFGV_INTERP_DECIM){
int decim = p->decim;
int hb1 = 0;
int hb2 = 0;
if(!(decim & 1)) {
hb2 = 1;
decim = decim >> 1;
}
if(!(decim & 1)) {
hb1 = 1;
decim = decim >> 1;
}
if (decim < MIN_CIC_DECIM || decim > MAX_CIC_DECIM)
ok = false;
else {
dsp_rx_regs->decim_rate = (hb1<<9) | (hb2<<8) | decim;
// printf("Decim: %d, register %d\n", p->decim, (hb1<<9) | (hb2<<8) | decim);
}
}
if (p->valid & CFGV_SCALE_IQ){
dsp_rx_regs->scale_iq = p->scale_iq;
}
// Build reply subpacket
r->opcode = OP_CONFIG_RX_REPLY_V2;
r->len = sizeof(*r);
r->rid = p->rid;
r->ok = ok;
r->inverted = tune_result.inverted;
r->baseband_freq_hi = u2_fxpt_freq_hi(tune_result.baseband_freq);
r->baseband_freq_lo = u2_fxpt_freq_lo(tune_result.baseband_freq);
r->ddc_freq_hi = u2_fxpt_freq_hi(tune_result.dxc_freq);
r->ddc_freq_lo = u2_fxpt_freq_lo(tune_result.dxc_freq);
r->residual_freq_hi = u2_fxpt_freq_hi(tune_result.residual_freq);
r->residual_freq_lo = u2_fxpt_freq_lo(tune_result.residual_freq);
return r->len;
}
static void
fill_db_info(u2_db_info_t *p, const struct db_base *db)
{
/*p->dbid = db->dbid;
p->freq_min_hi = u2_fxpt_freq_hi(db->freq_min);
p->freq_min_lo = u2_fxpt_freq_lo(db->freq_min);
p->freq_max_hi = u2_fxpt_freq_hi(db->freq_max);
p->freq_max_lo = u2_fxpt_freq_lo(db->freq_max);
p->gain_min = db->gain_min;
p->gain_max = db->gain_max;
p->gain_step_size = db->gain_step_size;*/
}
static size_t
dboard_info_cmd(const op_generic_t *p,
void *reply_payload, size_t reply_payload_space)
{
op_dboard_info_reply_t *r = (op_dboard_info_reply_t *) reply_payload;
if (reply_payload_space < sizeof(*r))
return 0; // no room
r->opcode = OP_DBOARD_INFO_REPLY;
r->len = sizeof(*r);
r->rid = p->rid;
r->ok = true;
//fill_db_info(&r->tx_db_info, tx_dboard);
//fill_db_info(&r->rx_db_info, rx_dboard);
return r->len;
}
static size_t
peek_cmd(const op_peek_t *p,
void *reply_payload, size_t reply_payload_space)
{
op_generic_t *r = (op_generic_t *) reply_payload;
//putstr("peek: addr="); puthex32(p->addr);
//printf(" bytes=%u\n", p->bytes);
if ((reply_payload_space < (sizeof(*r) + p->bytes)) ||
p->bytes > MAX_SUBPKT_LEN - sizeof(op_generic_t)) {
putstr("peek: insufficient reply packet space\n");
return 0; // FIXME do partial read?
}
r->opcode = OP_PEEK_REPLY;
r->len = sizeof(*r)+p->bytes;
r->rid = p->rid;
r->ok = true;
memcpy_wa(reply_payload+sizeof(*r), (void *)p->addr, p->bytes);
return r->len;
}
static bool
poke_cmd(const op_poke_t *p)
{
int bytes = p->len - sizeof(*p);
//putstr("poke: addr="); puthex32(p->addr);
//printf(" bytes=%u\n", bytes);
uint8_t *src = (uint8_t *)p + sizeof(*p);
memcpy_wa((void *)p->addr, src, bytes);
return true;
}
static bool
set_lo_offset_cmd(const op_freq_t *p)
{
/*u2_fxpt_freq_t f = u2_fxpt_freq_from_hilo(p->freq_hi, p->freq_lo);
if (p->opcode == OP_SET_TX_LO_OFFSET)
return db_set_lo_offset(tx_dboard, f);
else
return db_set_lo_offset(rx_dboard, f);
*/return false;
}
static size_t
gpio_read_cmd(const op_gpio_t *p,
void *reply_payload, size_t reply_payload_space)
{
op_gpio_read_reply_t *r = (op_gpio_read_reply_t *) reply_payload;
if (reply_payload_space < sizeof(*r)) // no room
return 0;
// Build reply subpacket
r->opcode = OP_GPIO_READ_REPLY;
r->len = sizeof(op_gpio_read_reply_t);
r->rid = p->rid;
r->ok = true;
r->mbz = 0;
r->value = hal_gpio_read(p->bank);
return r->len;
}
static size_t
generic_reply(const op_generic_t *p,
void *reply_payload, size_t reply_payload_space,
bool ok)
{
op_generic_t *r = (op_generic_t *) reply_payload;
if (reply_payload_space < sizeof(*r))
return 0; // no room
r->opcode = p->opcode | OP_REPLY_BIT;
r->len = sizeof(*r);
r->rid = p->rid;
r->ok = ok;
return r->len;
}
static size_t
add_eop(void *reply_payload, size_t reply_payload_space)
{
op_generic_t *r = (op_generic_t *) reply_payload;
if (reply_payload_space < sizeof(*r))
return 0; // no room
r->opcode = OP_EOP;
r->len = sizeof(*r);
r->rid = 0;
r->ok = 0;
return r->len;
}
/***********************************************************************
* Global vars to make the control replies
**********************************************************************/
#define REPLY_PAYLOAD_MAX_LEN (4 * sizeof(u2_subpkt_t))
uint16_t host_dst_udp_port;
uint16_t host_src_udp_port;
struct in_addr host_dst_ip_addr;
struct in_addr host_src_ip_addr;
eth_mac_addr_t host_dst_mac_addr;
eth_mac_addr_t host_src_mac_addr;
/***********************************************************************
* Handle input control data and produce output control data
**********************************************************************/
static size_t handle_control_packets(
const void *data_in, size_t len_in, void *data_out
){
// point to the begining of outgoing payload (subpackets)
uint8_t *reply_payload = data_out;
size_t reply_payload_space = REPLY_PAYLOAD_MAX_LEN;
// point to beginning of incoming payload (subpackets)
uint8_t *payload = (uint8_t *)data_in;
size_t payload_len = len_in;
size_t subpktlen = 0;
bool ok = false;
while (payload_len >= sizeof(op_generic_t)){
const op_generic_t *gp = (const op_generic_t *) payload;
subpktlen = 0;
// printf("\nopcode = %d\n", gp->opcode);
switch(gp->opcode){
case OP_EOP: // end of subpackets
goto end_of_subpackets;
case OP_ID:
subpktlen = op_id_cmd(gp, reply_payload, reply_payload_space);
break;
case OP_CONFIG_TX_V2:
subpktlen = config_tx_v2_cmd((op_config_tx_v2_t *) payload, reply_payload, reply_payload_space);
break;
case OP_CONFIG_RX_V2:
subpktlen = config_rx_v2_cmd((op_config_rx_v2_t *) payload, reply_payload, reply_payload_space);
break;
case OP_START_RX_STREAMING:
start_rx_streaming_cmd((op_start_rx_streaming_t *) payload);
ok = true;
goto generic_reply;
case OP_STOP_RX:
stop_rx_cmd();
ok = true;
goto generic_reply;
case OP_BURN_MAC_ADDR:
ok = ethernet_set_mac_addr(&((op_burn_mac_addr_t *)payload)->addr);
goto generic_reply;
case OP_CONFIG_CLOCK:
ok = config_clock_cmd((op_config_clock_t *) payload);
goto generic_reply;
case OP_DBOARD_INFO:
subpktlen = dboard_info_cmd(gp, reply_payload, reply_payload_space);
break;
case OP_SET_TIME:
ok = set_time((op_set_time_t *) payload);
goto generic_reply;
case OP_PEEK:
subpktlen = peek_cmd((op_peek_t *)payload, reply_payload, reply_payload_space);
break;
case OP_POKE:
ok = poke_cmd((op_poke_t *)payload);
goto generic_reply;
case OP_SET_TX_LO_OFFSET:
case OP_SET_RX_LO_OFFSET:
ok = set_lo_offset_cmd((op_freq_t *)payload);
goto generic_reply;
case OP_RESET_DB:
db_init();
ok = true;
goto generic_reply;
case OP_GPIO_SET_DDR:
ok = true;
hal_gpio_set_ddr(((op_gpio_t *)payload)->bank,
((op_gpio_t *)payload)->value,
((op_gpio_t *)payload)->mask);
goto generic_reply;
case OP_GPIO_SET_SELS:
ok = true;
hal_gpio_set_sels(((op_gpio_set_sels_t *)payload)->bank,
(char *)(&((op_gpio_set_sels_t *)payload)->sels));
goto generic_reply;
case OP_GPIO_READ:
subpktlen = gpio_read_cmd((op_gpio_t *) payload, reply_payload, reply_payload_space);
break;
case OP_GPIO_WRITE:
ok = true;
hal_gpio_write(((op_gpio_t *)payload)->bank,
((op_gpio_t *)payload)->value,
((op_gpio_t *)payload)->mask);
goto generic_reply;
case OP_GPIO_STREAM:
ok = true;
dsp_rx_regs->gpio_stream_enable = (uint32_t)((op_gpio_t *)payload)->value;
goto generic_reply;
// Add new opcode handlers here
generic_reply:
subpktlen = generic_reply(gp, reply_payload, reply_payload_space, ok);
break;
default:
printf("app_common_v2: unhandled opcode = %d\n", gp->opcode);
break;
}
int t = (gp->len + 3) & ~3; // bump to a multiple of 4
payload += t;
payload_len -= t;
subpktlen = (subpktlen + 3) & ~3; // bump to a multiple of 4
reply_payload += subpktlen;
reply_payload_space -= subpktlen;
}
end_of_subpackets:
// add the EOP marker
subpktlen = add_eop(reply_payload, reply_payload_space);
subpktlen = (subpktlen + 3) & ~3; // bump to a multiple of 4
reply_payload += subpktlen;
reply_payload_space -= subpktlen;
// how big the payload?
return REPLY_PAYLOAD_MAX_LEN - reply_payload_space;
}
static void handle_control_packet(
const usrp2_ctrl_data_t *data_in, usrp2_ctrl_data_t *data_out
){
}
static void
send_reply(void *reply, size_t reply_len)
{
if (reply_len < 64) reply_len = 64;
// wait for buffer to become idle
hal_set_leds(0x4, 0x4);
while((buffer_pool_status->status & BPS_IDLE(CPU_TX_BUF)) == 0)
;
hal_set_leds(0x0, 0x4);
// copy reply into CPU_TX_BUF
memcpy_wa(buffer_ram(CPU_TX_BUF), reply, reply_len);
// wait until nobody else is sending to the ethernet
if (ac_could_be_sending_to_eth){
hal_set_leds(0x8, 0x8);
dbsm_wait_for_opening(ac_could_be_sending_to_eth);
hal_set_leds(0x0, 0x8);
}
if (0){
printf("sending_reply to port %d, len = %d\n", cpu_tx_buf_dest_port, (int)reply_len);
print_buffer(buffer_ram(CPU_TX_BUF), reply_len/4);
}
// fire it off
bp_send_from_buf(CPU_TX_BUF, cpu_tx_buf_dest_port, 1, 0, reply_len/4);
// wait for it to complete (not long, it's a small pkt)
while((buffer_pool_status->status & (BPS_DONE(CPU_TX_BUF) | BPS_ERROR(CPU_TX_BUF))) == 0)
;
bp_clear_buf(CPU_TX_BUF);
}
static uint16_t
chksum_buffer(void *buff_, size_t len)
{
uint16_t *buff = (uint16_t *) buff_;
// sum the individual 16 bit words
uint32_t sum = 0;
for (size_t i = 0; i < len/sizeof(uint16_t); i++){
sum += buff[i];
}
// take only 16 bits out of the 32 bit sum and add up the carries
while (sum >> 16){
sum = (sum & 0xffff) + (sum >> 16);
}
// one's complement the result
return ~sum;
}
static struct in_addr
create_ip_from_host(struct in_addr addr){
//get an address that looks like the hosts
uint8_t low_byte = addr.s_addr & 0xff;
low_byte += 1;
if (low_byte == 0xff) low_byte = 0;
if (low_byte == 0x00) low_byte = 1;
addr.s_addr = (addr.s_addr & ~0xff) | low_byte;
return addr;
}
static void
handle_control_chan_frame(u2_eth_ip_udp_t *pkt, size_t len)
{
// setup reply
struct {
uint32_t ctrl_word;
u2_eth_ip_udp_t hdr;
usrp2_ctrl_data_t data;
} reply _AL4;
memset(&reply, 0, sizeof(reply));
// process the control data
handle_control_packet(
(usrp2_ctrl_data_t*)((uint8_t*)pkt + sizeof(u2_eth_ip_udp_t)),
&reply.data
);
size_t total_len = sizeof(reply);
reply.ctrl_word = total_len;
// load the ethernet header
reply.hdr.eth.dst = host_dst_mac_addr;
reply.hdr.eth.src = host_src_mac_addr;
reply.hdr.eth.ethertype = ETHERTYPE_IPV4;
// load the ip header
reply.hdr.ip.ip_hl = sizeof(u2_ipv4_hdr_t)/sizeof(uint32_t);
reply.hdr.ip.ip_v = 4;
reply.hdr.ip.ip_tos = 0;
reply.hdr.ip.ip_len = sizeof(u2_ipv4_hdr_t) + sizeof(u2_udp_hdr_t) + sizeof(usrp2_ctrl_data_t);
reply.hdr.ip.ip_id = 0;
reply.hdr.ip.ip_off = IP_DF;
reply.hdr.ip.ip_ttl = 255;
reply.hdr.ip.ip_p = IP_PROTO_UDP;
reply.hdr.ip.ip_sum = 0;
reply.hdr.ip.ip_src = host_src_ip_addr;
reply.hdr.ip.ip_dst = host_dst_ip_addr;
reply.hdr.ip.ip_sum = chksum_buffer(&reply.hdr.ip, sizeof(u2_ipv4_hdr_t));
// load the udp header
reply.hdr.udp.src_port = pkt->udp.dst_port;
reply.hdr.udp.dst_port = pkt->udp.src_port;
reply.hdr.udp.length = sizeof(u2_udp_hdr_t) + sizeof(usrp2_ctrl_data_t);
reply.hdr.udp.checksum = 0;
//send the reply
send_reply(&reply, total_len);
}
/*
* Called when an ethernet packet is received.
* Return true if we handled it here, otherwise
* it'll be passed on to the DSP Tx pipe
*/
bool
eth_pkt_inspector(dbsm_t *sm, int bufno)
{
u2_eth_ip_udp_t *pkt = (u2_eth_ip_udp_t *) buffer_ram(bufno);
size_t byte_len = (buffer_pool_status->last_line[bufno] - 3) * 4;
printf("Got an eth packet of len %d\n", (int)byte_len);
if (pkt->eth.ethertype != ETHERTYPE_IPV4)
return true; // ignore, probably bogus PAUSE frame from MAC
// inspect rcvd frame and figure out what do do.
switch (pkt->udp.dst_port){
case USRP2_UDP_CTRL_PORT:
//record the ip and mac addrs (used when setting up data init)
host_dst_ip_addr = pkt->ip.ip_src;
host_src_ip_addr = create_ip_from_host(pkt->ip.ip_src);
host_dst_mac_addr = pkt->eth.src;
host_src_mac_addr = *ethernet_mac_addr();
//handle_control_chan_frame(pkt, byte_len);
return true;
case USRP2_UDP_DATA_PORT:
//record the udp data ports (used when setting up data init)
host_dst_udp_port = pkt->udp.src_port;
host_src_udp_port = pkt->udp.dst_port;
return false; // pass it on to Tx DSP
}
return true; // ignore, whatever
}
/*
* Called when eth phy state changes (w/ interrupts disabled)
*/
void
link_changed_callback(int speed)
{
link_is_up = speed != 0;
hal_set_leds(link_is_up ? LED_RJ45 : 0x0, LED_RJ45);
printf("\neth link changed: speed = %d\n", speed);
}
void
print_tune_result(char *msg, bool tune_ok,
u2_fxpt_freq_t target_freq, struct tune_result *r)
{
#if 0
printf("db_tune %s %s\n", msg, tune_ok ? "true" : "false");
putstr(" target_freq "); print_fxpt_freq(target_freq); newline();
putstr(" baseband_freq "); print_fxpt_freq(r->baseband_freq); newline();
putstr(" dxc_freq "); print_fxpt_freq(r->dxc_freq); newline();
putstr(" residual_freq "); print_fxpt_freq(r->residual_freq); newline();
printf(" inverted %s\n", r->inverted ? "true" : "false");
#endif
}