//-----------------------------------------------------------------------------
// send a route request
void send_route_request(CnetAddr destaddr) {
if (pending_route_requests[destaddr] == NULLTIMER) {
ROUTE_PACKET req_packet;
req_packet.type = RREQ;
req_packet.source = nodeinfo.address;
req_packet.dest = destaddr;
req_packet.hop_count = 0;
req_packet.total_delay = 0;
req_packet.req_id = route_req_id;
req_packet.min_mtu = 0;
route_req_id++;
uint16_t request_size = ROUTE_PACKET_SIZE(req_packet);
DATAGRAM r_packet = alloc_datagram(__ROUTING__, nodeinfo.address,
destaddr, (char*) &req_packet,
request_size);
// start timer for pending route request
pending_route_requests[destaddr] =
CNET_start_timer(EV_ROUTE_PENDING_TIMER, ROUTE_REQUEST_TIMEOUT, destaddr);
route_notified[destaddr] = 0;
// insert into local history table
insert_local_history(req_packet.source,req_packet.dest, req_packet.req_id);
broadcast_packet(r_packet, -1);
}
}
void packet_loop(void) {
sensor_data_t packet;
char c;
while (1) {
memset(&packet, '\0', sizeof(sensor_data_t));
#ifndef TESTMODE
read(serial_port, &c, 1);
if (c != 'A')
continue;
fprintf(stderr, "GOT START\n");
// RTC
myread(serial_port, &packet.rtc.tm_hour, 2);
myread(serial_port, &packet.rtc.tm_min, 2);
myread(serial_port, &packet.rtc.tm_sec, 2);
// IMU
myread(serial_port, &packet.imu.gx, 2);
myread(serial_port, &packet.imu.gy, 2);
myread(serial_port, &packet.imu.ax, 2);
myread(serial_port, &packet.imu.ay, 2);
myread(serial_port, &packet.imu.az, 2);
// SCP
myread(serial_port, &packet.bmp.raw_pressure, 4);
myread(serial_port, &packet.bmp.raw_temperature, 2);
// GPS
myread(serial_port, &packet.gps.f_latitude, 4);
myread(serial_port, &packet.gps.f_longitude, 4);
myread(serial_port, &packet.gps.u_altitude, 2);
myread(serial_port, &packet.gps.u_valid, 1);
myread(serial_port, &packet.gps.u_satellites, 1);
myread(serial_port, &packet.humidity, 2);
myread(serial_port, &packet.bearing, 2);
myread(serial_port, &packet.light, 2);
myread(serial_port, &packet.internal_temp, 2);
myread(serial_port, &packet.extern_temp, 2);
myread(serial_port, &packet.gsm_registered, 1);
myread(serial_port, &packet.gsm_ready, 1);
myread(serial_port, &c, 1);
if (c != 'Z')
continue;
#endif
// push packet to telemetry topic
char *xml = (char *)calloc(10000, sizeof(char));
packet_to_xml(&packet, xml);
int retval = broadcast_packet(xml, strlen(xml));
free(xml);
fprintf(stderr, "Published packet: %u\n", retval);
sleep(1);
}
}
/**
* Forward packets to correct output buffers
*
* Packets, whose destination MAC address is already known from previously
* received packets, are forwarded to the matching switch ports. Packets
* destined to unknown addresses are broadcasted to all switch ports (except
* the ingress port).
*
* @param pkt_tbl Array of packets
* @param num Number of packets in the array
* @param thr_arg Thread arguments
* @param port_in Input port index
*/
static inline void forward_packets(odp_packet_t pkt_tbl[], unsigned num,
thread_args_t *thr_arg, uint8_t port_in)
{
odp_packet_t pkt;
odph_ethhdr_t *eth;
unsigned i;
unsigned buf_id;
int ret;
uint8_t port_out = 0;
for (i = 0; i < num; i++) {
pkt = pkt_tbl[i];
if (!odp_packet_has_eth(pkt)) {
odp_packet_free(pkt);
continue;
}
eth = (odph_ethhdr_t *)odp_packet_l2_ptr(pkt, NULL);
/* Lookup source MAC address */
ret = mac_table_get(ð->src, &port_out);
/* Update for address table if necessary */
if (ret < 0 || port_out != port_in)
mac_table_put(ð->src, port_in);
/* Lookup destination MAC address */
ret = mac_table_get(ð->dst, &port_out);
if (ret < 0) {
/* If address was not found, broadcast packet */
broadcast_packet(pkt, thr_arg, port_in);
continue;
}
buf_id = thr_arg->tx_pktio[port_out].buf.len;
thr_arg->tx_pktio[port_out].buf.pkt[buf_id] = pkt;
thr_arg->tx_pktio[port_out].buf.len++;
}
}
/*
* RCrecv_callback()
*
* The receive packet callback routine. This is called by
* RCProcMsgQ() after the adapter posts buffers which have been
* filled (one ethernet packet per buffer).
*/
static void
RCrecv_callback(U32 Status,
U8 PktCount,
U32 BucketsRemain,
PU32 PacketDescBlock,
U16 AdapterID)
{
U32 len, count;
PDPA pDpa;
struct sk_buff *skb;
struct device *dev;
singleTCB tcb;
psingleTCB ptcb = &tcb;
pDpa = PCIAdapters[AdapterID];
dev = pDpa->dev;
ptcb->bcount = 1;
#ifdef RCDEBUG
printk("rc: RCrecv_callback: 0x%x, 0x%x, 0x%x\n",
(uint)PktCount, (uint)BucketsRemain, (uint)PacketDescBlock);
#endif
#ifdef RCDEBUG
if ((pDpa->shutdown || pDpa->reboot) && !Status)
printk("shutdown||reboot && !Status: PktCount = %d\n",PktCount);
#endif
if ( (Status != RC_REPLY_STATUS_SUCCESS) || pDpa->shutdown)
{
/*
* Free whatever buffers the adapter returned, but don't
* pass them to the kernel.
*/
if (!pDpa->shutdown && !pDpa->reboot)
printk("rc: RCrecv error: status = 0x%x\n", (uint)Status);
else
printk("rc: Returning %d buffers, status = 0x%x\n",
PktCount, (uint)Status);
/*
* TO DO: check the nature of the failure and put the adapter in
* failed mode if it's a hard failure. Send a reset to the adapter
* and free all outstanding memory.
*/
if (Status == RC_REPLY_STATUS_ABORT_NO_DATA_TRANSFER)
{
#ifdef RCDEBUG
printk("RCrecv status ABORT NO DATA TRANSFER\n");
#endif
}
/* check for reset status: RC_REPLY_STATUS_ABORT_NO_DATA_TRANSFER */
if (PacketDescBlock)
{
while(PktCount--)
{
skb = (struct sk_buff *)PacketDescBlock[0];
skb->free = 1;
skb->lock = 0;
#ifdef RCDEBUG
printk("free skb 0x%p\n", skb);
#endif
dev_kfree_skb(skb, FREE_READ);
pDpa->numOutRcvBuffers--;
PacketDescBlock += BD_SIZE; /* point to next context field */
}
}
return;
}
else
{
while(PktCount--)
{
skb = (struct sk_buff *)PacketDescBlock[0];
#ifdef RCDEBUG
if (pDpa->shutdown)
printk("shutdown: skb=0x%x\n", (uint)skb);
printk("skb = 0x%x: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n", (uint)skb,
(uint)skb->data[0], (uint)skb->data[1], (uint)skb->data[2],
(uint)skb->data[3], (uint)skb->data[4], (uint)skb->data[5]);
#endif
if ( (memcmp(dev->dev_addr, skb->data, 6)) &&
(!broadcast_packet(skb->data)))
{
/*
* Re-post the buffer to the adapter. Since the adapter usually
* return 1 to 2 receive buffers at a time, it's not too inefficient
* post one buffer at a time but ... may be that should be
* optimized at some point.
*/
ptcb->b.context = (U32)skb;
ptcb->b.scount = 1;
ptcb->b.size = MAX_ETHER_SIZE;
ptcb->b.addr = (U32)skb->data;
if ( RCPostRecvBuffers(pDpa->id, (PRCTCB)ptcb ) != RC_RTN_NO_ERROR)
{
printk("rc: RCrecv_callback: post buffer failed!\n");
skb->free = 1;
dev_kfree_skb(skb, FREE_READ);
}
else
{
pDpa->numOutRcvBuffers++;
}
}
else
{
len = PacketDescBlock[2];
skb->dev = dev;
skb_put( skb, len ); /* adjust length and tail */
skb->protocol = eth_type_trans(skb, dev);
netif_rx(skb); /* send the packet to the kernel */
dev->last_rx = jiffies;
}
pDpa->numOutRcvBuffers--;
PacketDescBlock += BD_SIZE; /* point to next context field */
}
}
/*
* Replenish the posted receive buffers.
* DO NOT replenish buffers if the driver has already
* initiated a reboot or shutdown!
*/
if (!pDpa->shutdown && !pDpa->reboot)
{
count = RC_allocate_and_post_buffers(dev,
MAX_NMBR_RCV_BUFFERS-pDpa->numOutRcvBuffers);
pDpa->numOutRcvBuffers += count;
}
}
//-----------------------------------------------------------------------------
// process a routing datagram
void do_routing(int link, DATAGRAM datagram) {
ROUTE_PACKET r_packet;
size_t datagram_length = datagram.length;
memcpy(&r_packet, &datagram.payload, datagram_length);
switch (r_packet.type) {
case RREQ:
// check local history if we already processed this RREQ
if (find_local_history(r_packet.source,r_packet.dest, r_packet.req_id) != -1) {
break;
}
// insert into local history table
insert_local_history(r_packet.source, r_packet.dest, r_packet.req_id);
// set up mtu and delays
if (r_packet.min_mtu == 0) {
r_packet.min_mtu = linkinfo[link].mtu;
} else {
if (r_packet.min_mtu > linkinfo[link].mtu) {
r_packet.min_mtu = linkinfo[link].mtu;
}
}
if (r_packet.total_delay == 0) {
r_packet.total_delay = linkinfo[link].propagationdelay;
} else {
r_packet.total_delay += linkinfo[link].propagationdelay;
}
r_packet.hop_count = r_packet.hop_count + 1;
// the current node is not the destination
if (nodeinfo.address != r_packet.dest) {
// if a route exist to this destination
if (is_neighbour(r_packet.dest)) {
int next_link = get_next_link_for_dest(r_packet.dest);
uint16_t request_size = ROUTE_PACKET_SIZE(r_packet);
DATAGRAM rreq_datagram = alloc_datagram(__ROUTING__, r_packet.source,
r_packet.dest, (char*) &r_packet,
request_size);
insert_reverse_route(r_packet.source,r_packet.dest,link,r_packet.req_id);
send_packet_to_link(next_link,rreq_datagram);
} else {
// if no route rebroadcast
uint16_t request_size = ROUTE_PACKET_SIZE(r_packet);
DATAGRAM rreq_datagram =
alloc_datagram(__ROUTING__, r_packet.source,
r_packet.dest, (char*) &r_packet, request_size);
insert_reverse_route(r_packet.source, r_packet.dest,link, r_packet.req_id);
broadcast_packet(rreq_datagram,link);
}
} else {
// learn the route table
insert_reverse_route(r_packet.source, r_packet.dest, link, r_packet.req_id);
// send RREP
ROUTE_PACKET rrep_packet;
rrep_packet.source = r_packet.source;
rrep_packet.dest = r_packet.dest;
rrep_packet.type = RREP;
rrep_packet.req_id = r_packet.req_id;
rrep_packet.forward_min_mtu = r_packet.min_mtu;
rrep_packet.min_mtu = linkinfo[link].mtu;
rrep_packet.total_delay = linkinfo[link].propagationdelay;
rrep_packet.hop_count = 0;
uint16_t request_size = ROUTE_PACKET_SIZE(rrep_packet);
DATAGRAM rrep_datagram =
alloc_datagram(__ROUTING__, r_packet.dest, r_packet.source,
(char*) &rrep_packet, request_size);
send_packet_to_link(link,rrep_datagram);
}
break;
case RREP:
r_packet.hop_count = r_packet.hop_count + 1;
if (nodeinfo.address != r_packet.source) {
// insert into the forward table
insert_forward_route(r_packet.source, r_packet.dest, link, r_packet.req_id);
// find a reverse link
int reverse_link = find_reverse_route(r_packet.source, r_packet.dest,
r_packet.req_id);
if (reverse_link == -1) {
abort();
}
if (r_packet.min_mtu > linkinfo[reverse_link].mtu) {
r_packet.min_mtu = linkinfo[reverse_link].mtu;
}
r_packet.total_delay += linkinfo[reverse_link].propagationdelay;
uint16_t request_size = ROUTE_PACKET_SIZE(r_packet);
DATAGRAM rrep_datagram =
alloc_datagram(__ROUTING__, r_packet.dest, r_packet.source,
(char*) &r_packet, request_size);
send_packet_to_link(reverse_link,rrep_datagram);
} else {
learn_route_table(r_packet.dest, r_packet.hop_count, link, r_packet.min_mtu,
r_packet.total_delay, r_packet.req_id);
}
break;
}
}
