int spawnNetSender(struct fifo *fifo,
int sock,
struct net_config *config,
participantsDb_t db,
sender_stats_t stats)
{
int i;
sender_state_t sendst = MALLOC(struct senderState);
sendst->fifo = fifo;
sendst->socket = sock;
sendst->config = config;
sendst->stats = stats;
#ifdef BB_FEATURE_UDPCAST_FEC
if(sendst->config->flags & FLAG_FEC)
sendst->fec_data = xmalloc(NR_SLICES *
config->fec_stripes *
config->fec_redundancy *
config->blockSize);
#endif
sendst->rc.participantsDb = db;
initReturnChannel(&sendst->rc, sendst->config, sendst->socket);
sendst->free_slices_pc = pc_makeProduconsum(NR_SLICES, "free slices");
pc_produce(sendst->free_slices_pc, NR_SLICES);
for(i = 0; i <NR_SLICES; i++)
sendst->slices[i].state = SLICE_FREE;
#ifdef BB_FEATURE_UDPCAST_FEC
if(sendst->config->flags & FLAG_FEC) {
/* Free memory queue is initially full */
fec_init();
sendst->fec_data_pc = pc_makeProduconsum(NR_SLICES, "fec data");
pthread_create(&sendst->fec_thread, NULL, fecMain, sendst);
}
#endif
pthread_create(&fifo->thread, NULL, netSenderMain, sendst);
return 0;
}
static int
ethif_probe(struct device *dev)
{
u_long base_addr = dev->base_addr;
if ((base_addr == 0xffe0) || (base_addr == 1))
return 1; /* ENXIO */
if (1
/* All PCI probes are safe, and thus should be first. */
#ifdef CONFIG_DE4X5 /* DEC DE425, DE434, DE435 adapters */
&& de4x5_probe(dev)
#endif
#ifdef CONFIG_UCCS8900
&& cs89x0_probe(dev)
#endif
#ifdef CONFIG_M68EN302_ETHERNET
&& en302_probe(dev)
#endif
#ifdef CONFIG_M68EN360_ETHERNET
&& m68360_enet_probe(dev)
#endif
#ifdef CONFIG_DGRS
&& dgrs_probe(dev)
#endif
#ifdef CONFIG_EEXPRESS_PRO100B /* Intel EtherExpress Pro100B */
&& eepro100_probe(dev)
#endif
#ifdef CONFIG_EPIC
&& epic100_probe(dev)
#endif
#if defined(CONFIG_HP100)
&& hp100_probe(dev)
#endif
#if defined(CONFIG_NE2K_PCI)
&& ne2k_pci_probe(dev)
#endif
#ifdef CONFIG_PCNET32
&& pcnet32_probe(dev)
#endif
#ifdef CONFIG_RTL8139
&& rtl8139_probe(dev)
#endif
#if defined(CONFIG_VIA_RHINE)
&& via_rhine_probe(dev)
#endif
#if defined(CONFIG_VORTEX)
&& tc59x_probe(dev)
#endif
#if defined(CONFIG_DEC_ELCP)
&& tulip_probe(dev)
#endif
#ifdef CONFIG_YELLOWFIN
&& yellowfin_probe(dev)
#endif
/* Next mostly-safe EISA-only drivers. */
#ifdef CONFIG_AC3200 /* Ansel Communications EISA 3200. */
&& ac3200_probe(dev)
#endif
#if defined(CONFIG_ULTRA32)
&& ultra32_probe(dev)
#endif
/* Third, sensitive ISA boards. */
#ifdef CONFIG_AT1700
&& at1700_probe(dev)
#endif
#if defined(CONFIG_ULTRA)
&& ultra_probe(dev)
#endif
#if defined(CONFIG_SMC9194)
&& smc_init(dev)
#endif
#if defined(CONFIG_SMC91111)
&& smc_init_91C111(dev)
#endif
#if defined(CONFIG_FEC)
&& fec_init(dev)
#endif
#if defined(CONFIG_WD80x3)
&& wd_probe(dev)
#endif
#if defined(CONFIG_EL2) /* 3c503 */
&& el2_probe(dev)
#endif
#if defined(CONFIG_HPLAN)
&& hp_probe(dev)
#endif
#if defined(CONFIG_HPLAN_PLUS)
&& hp_plus_probe(dev)
#endif
#if defined(CONFIG_SEEQ8005)
&& seeq8005_probe(dev)
#endif
#ifdef CONFIG_E2100 /* Cabletron E21xx series. */
&& e2100_probe(dev)
#endif
#if defined(CONFIG_NE2000)
&& ne_probe(dev)
#endif
#ifdef CONFIG_AT1500
&& at1500_probe(dev)
#endif
#ifdef CONFIG_FMV18X /* Fujitsu FMV-181/182 */
&& fmv18x_probe(dev)
#endif
#ifdef CONFIG_ETH16I
&& eth16i_probe(dev) /* ICL EtherTeam 16i/32 */
#endif
#ifdef CONFIG_EL3 /* 3c509 */
&& el3_probe(dev)
#endif
#ifdef CONFIG_3C515 /* 3c515 */
&& tc515_probe(dev)
#endif
#ifdef CONFIG_ZNET /* Zenith Z-Note and some IBM Thinkpads. */
&& znet_probe(dev)
#endif
#ifdef CONFIG_EEXPRESS /* Intel EtherExpress */
&& express_probe(dev)
#endif
#ifdef CONFIG_EEXPRESS_PRO /* Intel EtherExpress Pro/10 */
&& eepro_probe(dev)
#endif
#ifdef CONFIG_DEPCA /* DEC DEPCA */
&& depca_probe(dev)
#endif
#ifdef CONFIG_EWRK3 /* DEC EtherWORKS 3 */
&& ewrk3_probe(dev)
#endif
#ifdef CONFIG_APRICOT /* Apricot I82596 */
&& apricot_probe(dev)
#endif
#ifdef CONFIG_EL1 /* 3c501 */
&& el1_probe(dev)
#endif
#if defined(CONFIG_WAVELAN) /* WaveLAN */
&& wavelan_probe(dev)
#endif /* defined(CONFIG_WAVELAN) */
#ifdef CONFIG_EL16 /* 3c507 */
&& el16_probe(dev)
#endif
#ifdef CONFIG_ELPLUS /* 3c505 */
&& elplus_probe(dev)
#endif
#ifdef CONFIG_DE600 /* D-Link DE-600 adapter */
&& de600_probe(dev)
#endif
#ifdef CONFIG_DE620 /* D-Link DE-620 adapter */
&& de620_probe(dev)
#endif
#if defined(CONFIG_SK_G16)
&& SK_init(dev)
#endif
#ifdef CONFIG_NI52
&& ni52_probe(dev)
#endif
#ifdef CONFIG_NI65
&& ni65_probe(dev)
#endif
#ifdef CONFIG_LANCE /* ISA LANCE boards */
&& lance_probe(dev)
#endif
#ifdef CONFIG_ATARILANCE /* Lance-based Atari ethernet boards */
&& atarilance_probe(dev)
#endif
#ifdef CONFIG_A2065 /* Commodore/Ameristar A2065 Ethernet Board */
&& a2065_probe(dev)
#endif
#ifdef CONFIG_ARIADNE /* Village Tronic Ariadne Ethernet Board */
&& ariadne_probe(dev)
#endif
#ifdef CONFIG_HYDRA /* Hydra Systems Amiganet Ethernet board */
&& hydra_probe(dev)
#endif
#ifdef CONFIG_SUNLANCE
&& sparc_lance_probe(dev)
#endif
#ifdef CONFIG_TLAN
&& tlan_probe(dev)
#endif
#ifdef CONFIG_LANCE
&& lance_probe(dev)
#endif
#ifdef CONFIG_OPEN_ETH
#ifdef CONFIG_LEON_2
&& oeth_probe(dev)
#endif
#endif
#ifdef CONFIG_GRLIB_OPENCORES_ETHERNET
#ifdef CONFIG_LEON_3
&& opencores_ethermac_probe(dev)
#endif
#endif
#ifdef CONFIG_GRLIB_GAISLER_GRETH
#ifdef CONFIG_LEON_3
&& gaisler_greth_probe(dev)
#endif
#endif
&& 1 ) {
return 1; /* -ENODEV or -EAGAIN would be more accurate. */
}
return 0;
}
/**
* Pull one frame from the card
* @param[in] dev Our ethernet device to handle
* @return Length of packet read
*/
static int fec_recv(struct eth_device *dev)
{
struct fec_priv *fec = (struct fec_priv *)dev->priv;
struct fec_bd *rbd = &fec->rbd_base[fec->rbd_index];
unsigned long ievent;
int frame_length, len = 0;
struct nbuf *frame;
uint16_t bd_status;
uchar buff[FEC_MAX_PKT_SIZE];
/*
* Check if any critical events have happened
*/
ievent = readl(&fec->eth->ievent);
writel(ievent, &fec->eth->ievent);
debug("fec_recv: ievent 0x%lx\n", ievent);
if (ievent & FEC_IEVENT_BABR) {
fec_halt(dev);
fec_init(dev, fec->bd);
printf("some error: 0x%08lx\n", ievent);
return 0;
}
if (ievent & FEC_IEVENT_HBERR) {
/* Heartbeat error */
writel(0x00000001 | readl(&fec->eth->x_cntrl),
&fec->eth->x_cntrl);
}
if (ievent & FEC_IEVENT_GRA) {
/* Graceful stop complete */
if (readl(&fec->eth->x_cntrl) & 0x00000001) {
fec_halt(dev);
writel(~0x00000001 & readl(&fec->eth->x_cntrl),
&fec->eth->x_cntrl);
fec_init(dev, fec->bd);
}
}
/*
* ensure reading the right buffer status
*/
bd_status = readw(&rbd->status);
debug("fec_recv: status 0x%x\n", bd_status);
if (!(bd_status & FEC_RBD_EMPTY)) {
if ((bd_status & FEC_RBD_LAST) && !(bd_status & FEC_RBD_ERR) &&
((readw(&rbd->data_length) - 4) > 14)) {
/*
* Get buffer address and size
*/
frame = (struct nbuf *)readl(&rbd->data_pointer);
frame_length = readw(&rbd->data_length) - 4;
/*
* Fill the buffer and pass it to upper layers
*/
#ifdef CONFIG_FEC_MXC_SWAP_PACKET
swap_packet((uint32_t *)frame->data, frame_length);
#endif
memcpy(buff, frame->data, frame_length);
NetReceive(buff, frame_length);
len = frame_length;
} else {
if (bd_status & FEC_RBD_ERR)
printf("error frame: 0x%08lx 0x%08x\n",
(ulong)rbd->data_pointer,
bd_status);
}
/*
* free the current buffer, restart the engine
* and move forward to the next buffer
*/
fec_rbd_clean(fec->rbd_index == (FEC_RBD_NUM - 1) ? 1 : 0, rbd);
fec_rx_task_enable(fec);
fec->rbd_index = (fec->rbd_index + 1) % FEC_RBD_NUM;
}
debug("fec_recv: stop\n");
return len;
}
/**
* Pull one frame from the card
* @param[in] dev Our ethernet device to handle
* @return Length of packet read
*/
static int fec_recv(struct eth_device *dev)
{
struct fec_priv *fec = (struct fec_priv *)dev->priv;
struct fec_bd *rbd = &fec->rbd_base[fec->rbd_index];
unsigned long ievent;
int frame_length, len = 0;
uint16_t bd_status;
uint32_t addr, size, end;
int i;
ALLOC_CACHE_ALIGN_BUFFER(uchar, buff, FEC_MAX_PKT_SIZE);
/*
* Check if any critical events have happened
*/
ievent = readl(&fec->eth->ievent);
writel(ievent, &fec->eth->ievent);
debug("fec_recv: ievent 0x%lx\n", ievent);
if (ievent & FEC_IEVENT_BABR) {
fec_halt(dev);
fec_init(dev, fec->bd);
printf("some error: 0x%08lx\n", ievent);
return 0;
}
if (ievent & FEC_IEVENT_HBERR) {
/* Heartbeat error */
writel(0x00000001 | readl(&fec->eth->x_cntrl),
&fec->eth->x_cntrl);
}
if (ievent & FEC_IEVENT_GRA) {
/* Graceful stop complete */
if (readl(&fec->eth->x_cntrl) & 0x00000001) {
fec_halt(dev);
writel(~0x00000001 & readl(&fec->eth->x_cntrl),
&fec->eth->x_cntrl);
fec_init(dev, fec->bd);
}
}
/*
* Read the buffer status. Before the status can be read, the data cache
* must be invalidated, because the data in RAM might have been changed
* by DMA. The descriptors are properly aligned to cachelines so there's
* no need to worry they'd overlap.
*
* WARNING: By invalidating the descriptor here, we also invalidate
* the descriptors surrounding this one. Therefore we can NOT change the
* contents of this descriptor nor the surrounding ones. The problem is
* that in order to mark the descriptor as processed, we need to change
* the descriptor. The solution is to mark the whole cache line when all
* descriptors in the cache line are processed.
*/
addr = (uint32_t)rbd;
addr &= ~(ARCH_DMA_MINALIGN - 1);
size = roundup(sizeof(struct fec_bd), ARCH_DMA_MINALIGN);
invalidate_dcache_range(addr, addr + size);
bd_status = readw(&rbd->status);
debug("fec_recv: status 0x%x\n", bd_status);
if (!(bd_status & FEC_RBD_EMPTY)) {
if ((bd_status & FEC_RBD_LAST) && !(bd_status & FEC_RBD_ERR) &&
((readw(&rbd->data_length) - 4) > 14)) {
/*
* Get buffer address and size
*/
addr = readl(&rbd->data_pointer);
frame_length = readw(&rbd->data_length) - 4;
/*
* Invalidate data cache over the buffer
*/
end = roundup(addr + frame_length, ARCH_DMA_MINALIGN);
addr &= ~(ARCH_DMA_MINALIGN - 1);
invalidate_dcache_range(addr, end);
/*
* Fill the buffer and pass it to upper layers
*/
#ifdef CONFIG_FEC_MXC_SWAP_PACKET
swap_packet((uint32_t *)addr, frame_length);
#endif
memcpy(buff, (char *)addr, frame_length);
net_process_received_packet(buff, frame_length);
len = frame_length;
} else {
if (bd_status & FEC_RBD_ERR)
printf("error frame: 0x%08x 0x%08x\n",
addr, bd_status);
}
/*
* Free the current buffer, restart the engine and move forward
* to the next buffer. Here we check if the whole cacheline of
* descriptors was already processed and if so, we mark it free
* as whole.
*/
size = RXDESC_PER_CACHELINE - 1;
if ((fec->rbd_index & size) == size) {
i = fec->rbd_index - size;
addr = (uint32_t)&fec->rbd_base[i];
for (; i <= fec->rbd_index ; i++) {
fec_rbd_clean(i == (FEC_RBD_NUM - 1),
&fec->rbd_base[i]);
}
flush_dcache_range(addr,
addr + ARCH_DMA_MINALIGN);
}
fec_rx_task_enable(fec);
fec->rbd_index = (fec->rbd_index + 1) % FEC_RBD_NUM;
}
debug("fec_recv: stop\n");
return len;
}
struct eth_driver*
ethif_plat_init(int dev_id, struct ethif_os_interface interface) {
struct enet * enet;
struct ocotp * ocotp;
struct clock* arm_clk;
struct imx6_eth_data *eth_data;
struct ldesc* ldesc;
(void)dev_id;
os_iface_init(interface);
eth_data = (struct imx6_eth_data*)os_malloc(sizeof(struct imx6_eth_data));
if (eth_data == NULL) {
cprintf(COL_IMP, "Failed to allocate dma buffers\n");
return NULL;
}
ldesc = ldesc_init(RX_DESC_COUNT, RXBUF_SIZE, TX_DESC_COUNT, TXBUF_SIZE);
assert(ldesc);
/*
* We scale up the CPU to improve benchmarking performance
* It is not the right place so should be moved later
*/
arm_clk = clk_get_clock(CLK_ARM);
clk_set_freq(arm_clk, CPU_FREQ);
CLK_DEBUG(printf("ARM clock frequency: %9d HZ\n", clk_get_freq(arm_clk)));
/* initialise the eFuse controller so we can get a MAC address */
ocotp = ocotp_init();
/* Initialise ethernet pins */
gpio_init();
setup_iomux_enet();
/* Initialise the phy library */
miiphy_init();
/* Initialise the phy */
phy_micrel_init();
/* Initialise the RGMII interface */
enet = enet_init(ldesc);
assert(enet);
/* Fetch and set the MAC address */
if(ocotp == NULL || ocotp_get_mac(ocotp, eth_data->ethaddr)){
memcpy(eth_data->ethaddr, DEFAULT_MAC, 6);
}
enet_set_mac(enet, eth_data->ethaddr);
/* Connect the phy to the ethernet controller */
if(fec_init(CONFIG_FEC_MXC_PHYMASK, enet)){
return NULL;
}
/* Start the controller */
enet_enable(enet);
/* Update book keeping */
eth_data->irq_enabled = 0;
eth_data->enet = enet;
eth_data->ldesc = ldesc;
eth_driver_set_data(&imx6_eth_driver, eth_data);
/* done */
return &imx6_eth_driver;
}
int main(int argc, char *argv[]) {
char szErrbuf[PCAP_ERRBUF_SIZE];
int i;
pcap_t *ppcap = NULL;
char fBrokenSocket = 0;
int pcnt = 0;
time_t start_time;
uint8_t packet_transmit_buffer[MAX_PACKET_LENGTH];
size_t packet_header_length = 0;
fd_set fifo_set;
int max_fifo_fd = -1;
fifo_t fifo[MAX_FIFOS];
int param_transmission_count = 1;
int param_data_packets_per_block = 8;
int param_fec_packets_per_block = 4;
int param_packet_length = MAX_USER_PACKET_LENGTH;
int param_port = 0;
int param_min_packet_length = 0;
int param_fifo_count = 1;
strcpy(param_serial_port, "/dev/ttyUSB0");
running = 1;
printf("Raw data transmitter (c) 2015 befinitiv GPL2\n");
while (1) {
int nOptionIndex;
static const struct option optiona[] = {
{ "help", no_argument, &flagHelp, 1 },
{ 0, 0, 0, 0 }
};
int c = getopt_long(argc, argv, "r:hf:p:b:m:s:x:P:B:", optiona, &nOptionIndex);
if (c == -1) {
break;
}
switch (c) {
case 0: // long option
break;
case 'h': // help
usage();
case 'r': // retransmissions
param_fec_packets_per_block = atoi(optarg);
break;
case 'f': // MTU
param_packet_length = atoi(optarg);
break;
case 'p': //port
param_port = atoi(optarg);
break;
case 'b': //retransmission block size
param_data_packets_per_block = atoi(optarg);
break;
case 'm'://minimum packet length
param_min_packet_length = atoi(optarg);
break;
case 's': //how many streams (fifos) do we have in parallel
param_fifo_count = atoi(optarg);
break;
case 'x': //how often is a block transmitted
param_transmission_count = atoi(optarg);
break;
case 'P': //Serial-Port
strcpy(param_serial_port, optarg);
break;
case 'B': //Serial-Baud
param_serial_baud = atoi(optarg);
break;
default:
printf("unknown switch %c\n", c);
usage();
break;
}
}
if (optind >= argc) {
usage();
}
mavlink_init(0, param_serial_port, param_serial_baud);
#ifdef SDL2
wifibc_thread_telemetry = SDL_CreateThread(wifibc_update_telemetry, NULL, NULL);
#else
wifibc_thread_telemetry = SDL_CreateThread(wifibc_update_telemetry, NULL);
#endif
if (param_packet_length > MAX_USER_PACKET_LENGTH) {
printf("Packet length is limited to %d bytes (you requested %d bytes)\n", MAX_USER_PACKET_LENGTH, param_packet_length);
return (1);
}
if (param_min_packet_length > param_packet_length) {
printf("Your minimum packet length is higher that your maximum packet length (%d > %d)\n", param_min_packet_length, param_packet_length);
return (1);
}
if (param_fifo_count > MAX_FIFOS) {
printf("The maximum number of streams (FIFOS) is %d (you requested %d)\n", MAX_FIFOS, param_fifo_count);
return (1);
}
if (param_data_packets_per_block > MAX_DATA_OR_FEC_PACKETS_PER_BLOCK || param_fec_packets_per_block > MAX_DATA_OR_FEC_PACKETS_PER_BLOCK) {
printf("Data and FEC packets per block are limited to %d (you requested %d data, %d FEC)\n", MAX_DATA_OR_FEC_PACKETS_PER_BLOCK,
param_data_packets_per_block, param_fec_packets_per_block);
return (1);
}
packet_header_length = packet_header_init(packet_transmit_buffer);
fifo_init(fifo, param_fifo_count, param_data_packets_per_block);
fifo_open(fifo, param_fifo_count);
fifo_create_select_set(fifo, param_fifo_count, &fifo_set, &max_fifo_fd);
//initialize forward error correction
fec_init();
// open the interface in pcap
szErrbuf[0] = '\0';
ppcap = pcap_open_live(argv[optind], 800, 1, 20, szErrbuf);
if (ppcap == NULL) {
printf("Unable to open interface %s in pcap: %s\n",
argv[optind], szErrbuf);
return (1);
}
pcap_setnonblock(ppcap, 1, szErrbuf);
start_time = time(NULL);
while (!fBrokenSocket) {
fd_set rdfs;
int ret;
rdfs = fifo_set;
//wait for new data on the fifos
ret = select(max_fifo_fd + 1, &rdfs, NULL, NULL, NULL);
if (ret < 0) {
perror("select");
return (1);
}
//cycle through all fifos and look for new data
for (i = 0; i < param_fifo_count && ret; ++i) {
if (!FD_ISSET(fifo[i].fd, &rdfs)) {
continue;
}
ret--;
packet_buffer_t *pb = fifo[i].pbl + fifo[i].curr_pb;
//if the buffer is fresh we add a payload header
if (pb->len == 0) {
pb->len += sizeof(payload_header_t); //make space for a length field (will be filled later)
}
//read the data
int inl = read(fifo[i].fd, pb->data + pb->len, param_packet_length - pb->len - sizeof(ModelDataMinimal));
if (inl < 0 || inl > param_packet_length - pb->len - sizeof(ModelDataMinimal)) {
perror("reading stdin");
return 1;
}
if (inl == 0) {
//EOF
printf("Warning: Lost connection to fifo %d. Please make sure that a data source is connected\n", i);
usleep(1e5);
continue;
}
pb->len += inl;
// add telemtry-data
ModelDataMinimal.p_lat = ModelData[0].p_lat;
ModelDataMinimal.p_long = ModelData[0].p_long;
ModelDataMinimal.p_alt = ModelData[0].p_alt;
ModelDataMinimal.pitch = ModelData[0].pitch;
ModelDataMinimal.roll = ModelData[0].roll;
ModelDataMinimal.yaw = ModelData[0].yaw;
ModelDataMinimal.speed = ModelData[0].speed;
ModelDataMinimal.voltage = ModelData[0].voltage;
ModelDataMinimal.ampere = ModelData[0].ampere;
ModelDataMinimal.gpsfix = ModelData[0].gpsfix;
ModelDataMinimal.numSat = ModelData[0].numSat;
memcpy(pb->data + pb->len + param_packet_length, &ModelDataMinimal, sizeof(ModelDataMinimal));
pb->len += sizeof(ModelDataMinimal);
//check if this packet is finished
if (pb->len >= param_min_packet_length) {
payload_header_t *ph = (payload_header_t *)pb->data;
ph->data_length = pb->len - sizeof(
payload_header_t); //write the length into the packet. this is needed since with fec we cannot use the wifi packet lentgh anymore. We could also set the user payload to a fixed size but this would introduce additional latency since tx would need to wait until that amount of data has been received
pcnt++;
//check if this block is finished
if (fifo[i].curr_pb == param_data_packets_per_block - 1) {
pb_transmit_block(fifo[i].pbl, ppcap, &(fifo[i].seq_nr), i + param_port, param_packet_length, packet_transmit_buffer, packet_header_length,
param_data_packets_per_block, param_fec_packets_per_block, param_transmission_count);
fifo[i].curr_pb = 0;
} else {
fifo[i].curr_pb++;
}
}
}
if (pcnt % 64 == 0) {
printf("%d data packets sent (interface rate: %.3f)\n", pcnt, 1.0 * pcnt / param_data_packets_per_block * (param_data_packets_per_block + param_fec_packets_per_block) / (time(NULL) - start_time));
}
}
running = 0;
SDL_WaitThread(wifibc_thread_telemetry, NULL);
mavlink_exit(0);
printf("Broken socket\n");
return (0);
}
