Show send_dump.c syntax highlighted
/* -*- Mode: C; tab-width: 8; c-basic-indent: 4; indent-tabs-mode: nil -*- */
/*
*
* Copyright (c) 2003 The Regents of the University of California. All
* rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* - Neither the name of the University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
/* note:
this is based on thanos' ctrl, with modifications, which have tended to mess it up.
in short, this is ugly and poorly written and not advised to even touch it with a 1 foot pole.
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <ctype.h>
#include <sys/time.h>
#include <arpa/inet.h>
#include "emrun/emrun.h"
#include "link/link.h"
#include <link/link_headers.h>
#include "libmisc/misc.h"
#include "send_dump.h"
#include "msr_misc.h"
#define GOOD_PKT 1
#define BAD_PKT 0
#define SENT 0
#define RCVD 1
#define SENDREPORT 2
#define RECEIPT_TIMEOUT 60000
#define PREAMBLELEN 5
#define AM_TYPE 0xaa
#define SEND_DUMP_MODE 0 // in this mode we repeatedly send a packet from one node
#define SPEW_MODE 1 // in this mode we just listen on all nodes - should
// be used in conjunction with a modified CC1000 where one
// sender continuously spews 0x55, and other nodes continuously listen
#define LISTEN_MODE 2 // listen continuosly and dump out all packets
#define BLAST_MODE 3 // like send_dump except that packets are sent directly from the mote (but with same bit pattern)
void ctrl_shutdown(void *data);
static int good_pkt_rcvd(lu_context_t *link,
link_pkt_t *link_pkt, int datalen);
static int error_pkt_rcvd(void *pkt, ssize_t len,
pd_client_context_t *pd_client);
int send_timer_expired(void *data, int interval, g_event_t *ev);
int receipt_timeout(void *data, int interval, g_event_t *event);
void log_pkt(node_element_t *nelement, link_pkt_t *link_pkt,
int datalen, int type);
uint8_t simgroup=0x7d;// TOS_DEFAULT_AM_GROUP
int payloadlen = 16;
int sender = -1;
int allsenders = 0; // if 1, we will send 1 pkt per node round robin, else always send from same node
int destination = LINK_BROADCAST;
int send_period = 200;
int dump;
int npkts = 1000;
int packets_sent = 0;
int mode = SEND_DUMP_MODE;
int receipts=1;
int lossypre=0; // if 1, expect packets which contain the preamble byte in the last PREAMBLELEN data bytes
ctrl_state_t *cstate;
int skip_nodes[] = {11};
int skipnode(int node) {
int i;
for (i=0; i < sizeof(skip_nodes); i++) {
if (node == skip_nodes[i]) return 1;
}
return 0;
}
void next_sender() {
while(1) {
sender++;
if (sender > cstate->num_nodes) sender = 0;
if (!skipnode(sender)) break;
}
}
uint8_t guess_phase(link_pkt_t *link_pkt) {
// We get a stream of 0x55, but we don't know the receiver's offset,
// ie he could be legitately receiving 0xaa.
// So we make a first pass to 'guess' which phase the receiver has,
// and once we have this guess, a second pass to actually compute the errors.
int nerrs_55=0;
int nerrs_aa=0;
int i;
int offset =0; // we need to pass an in to compare bytes (not used here)
// TOS_Msg.addr
nerrs_55 += comparebytes(link_pkt->dst.id & 0xFF, 0x55, 0, &offset);
nerrs_55 += comparebytes((link_pkt->dst.id >> 8) & 0xFF, 0x55, 0, &offset);
// TOS_Msg.type
nerrs_55 += comparebytes(link_pkt->ext_type, 0x55, 0, &offset);
// TOS_Msg.group
nerrs_55 += comparebytes((link_pkt->ext_group & 0xFF), 0x55, 0, &offset);
// skip length
for (i=0; i<payloadlen; i++) {
nerrs_55 += comparebytes(link_pkt->data[i], 0x55, 0, &offset);
}
// TOS_Msg.addr
nerrs_aa += comparebytes(link_pkt->dst.id & 0xFF, 0xaa, 0, &offset);
nerrs_aa += comparebytes((link_pkt->dst.id >> 8) & 0xFF, 0xaa, 0, &offset);
// TOS_Msg.type
nerrs_aa += comparebytes(link_pkt->ext_type, 0xaa, 0, &offset);
// TOS_Msg.group
nerrs_aa += comparebytes((link_pkt->ext_group & 0xFF), 0xaa, 0, &offset);
// skip length (see above)
for (i=0; i<payloadlen; i++) {
nerrs_aa += comparebytes(link_pkt->data[i], 0xaa, 0, &offset);
}
// if (nerrs_aa < nerrs_55) return 0xaa; else return 0x55;
return 0;
}
// this is for finding errors on packets send in SPEW_MODE, where one mote continuously
// emits the byte 0x55, and the others continuously read in bytes and stick them in packets
// that are sent back to us.
// In these received packets, all fields except length and crc are filled with the bytes
// received off the air. Length and crc are set by the mote after reception and we do not
// look at those here.
int find_errors_spew(link_pkt_t *link_pkt, uint8_t datalen, int8_t dumperrs) {
int nerrs=0;
int i;
int offset=0;
uint8_t byte = guess_phase(link_pkt);
// TOS_Msg.addr
nerrs += comparebytes(link_pkt->dst.id & 0xFF, byte, 0, &offset);
nerrs += comparebytes((link_pkt->dst.id >> 8) & 0xFF, byte, 0, &offset);
// TOS_Msg.type
nerrs += comparebytes(link_pkt->ext_type, byte, 0, &offset);
// TOS_Msg.group
nerrs += comparebytes((link_pkt->ext_group & 0xFF), byte, 0, &offset);
// skip length (see above)
for (i=0; i<payloadlen; i++) {
nerrs += comparebytes(link_pkt->data[i], byte, 0, &offset);
}
return nerrs;
}
int find_errors_sd(link_pkt_t *link_pkt, uint8_t datalen, int8_t dumperrs) {
node_element_t *selement = cstate->node_element[sender - 1];
uint16_t crc=selement->crc;
// printf("find_errors_sd thinks the sender is %d\n", sender - 1);
int nerrs=0;
int i;
int offset=0;
int preamblelen= lossypre ? PREAMBLELEN : 0;
// This would be so much simpler if we simply got a real unadulterated TOSMsg rather than
// a link_pkt with bits of TOSMsg sprinkled in different places...
// TOS_Msg.addr
// nerrs += comparebytes(link_pkt->dst.id & 0xFF, destination & 0xFF, dumperrs, &offset);
// nerrs += comparebytes((link_pkt->dst.id >> 8) & 0xFF, (destination >> 8) & 0xFF, dumperrs, &offset);
// TOS_Msg.type
nerrs += comparebytes(link_pkt->ext_type,AM_TYPE,dumperrs, &offset);
// TOS_Msg.group
// nerrs += comparebytes((link_pkt->ext_group & 0xFF),simgroup,dumperrs, &offset);
// length
nerrs += comparebytes(datalen - preamblelen, payloadlen,dumperrs, &offset);
for (i=0; i<payloadlen; i++) {
nerrs += comparebytes(link_pkt->data[i], data_payload[i], dumperrs, &offset);
}
// errors on crc compute crc , compare with data[datalen-2, datalen-1]
// nerrs += comparebytes(link_pkt->data[datalen], (crc >> 0) & 0xff, dumperrs, &offset);
// nerrs += comparebytes(link_pkt->data[datalen+1], (crc >> 8) & 0xff, dumperrs, &offset);
crc =0;
return nerrs;
}
int receipt_timeout(void *data, int interval, g_event_t *event)
{
// node_element_t *nelement = (node_element_t *)data;
elog(LOG_ERR, "Timed out waiting for receipt!");
// For now, we exit when this happens, because it's BAD
exit(1);
return TIMER_DONE;
}
int send_timer_expired(void *data, int interval, g_event_t *ev)
{
ctrl_state_t *cstate=(ctrl_state_t *)data;
int i;
uint16_t crc=0;
if (allsenders) next_sender();
i = sender - 1;
int j=0;
node_element_t *nelement = cstate->node_element[i];
char *buf=NULL;
link_pkt_t *pkt=NULL;
pkt_payload_t *payload=NULL;
buf = (char *)malloc(sizeof(link_pkt_t) +
sizeof(pkt_payload_t));
memset(buf, 0, (sizeof(link_pkt_t) + sizeof(pkt_payload_t)));
pkt = (link_pkt_t *)buf;
payload = (pkt_payload_t *)pkt->data;
// set the header
pkt->dst.id = destination;
pkt->src.id = nelement->id;
pkt->ext_group = simgroup; // not strictly necessary (it gets set somewere after lu_send())
// but we set it here so that the group shows up in sent packets
pkt->type = PKT_TYPE_TOS; // so we don't get emstar encapsulation (in which case real underlying packet
// would have additional fields which we don't want).
pkt->ext_type = AM_TYPE; // some random type - this is the real type that goes into the tos_msg
// increment seqno
nelement->seqno++;
// update crc
crc=crcByte(crc,destination & 0xff); // addr
crc=crcByte(crc,(destination >> 8) & 0xFF);
crc=crcByte(crc,AM_TYPE); // type
crc=crcByte(crc, simgroup); // group
crc=crcByte(crc, payloadlen); // length
for (j=0; j<payloadlen; j++) {
payload->data[j] = data_payload[j];
crc=crcByte(crc, data_payload[j]);
}
// store the crc for comparison at reception
nelement->crc=crc;
// pkt is good to go
// First, check if the outbound pointer is not null
// if so, the receipt for the previous packet has probably
// not arrived yet, so we just return TIMER_RENEW
if (receipts) {
if (nelement->outbound_pkt != NULL) {
if (g_timer_isset(nelement->receipt_timer_ev)) {
elog(LOG_ERR, "Outbound pkt pointer NOT NULL"
" but receipt timer is set."
" Deferring transmission");
nelement->seqno--;
goto done;
} else {
elog(LOG_ERR, "Outbound pkt pointer NOT NULL!");
exit(1);
}
}
// Request a receipt. We'll use it to mark the 'send time'
// which will actually be the 'send done' time
link_receipt_request(pkt, NULL);
}
if (lu_send(nelement->link, pkt, payloadlen) < 0) {
elog(LOG_ERR, "Node [%d]: Can't send on %s: %m",
nelement->id, lu_name(nelement->link, NULL));
free(buf);
// exit(1);
} else {
elog(LOG_DEBUG(2), "Node [%d]: Sending pkt with seqno %u",
nelement->id, nelement->seqno);
packets_sent++;
if (packets_sent > npkts)
ctrl_shutdown(NULL);
if (receipts) {
// Store the pointer, we'll free the memory when we get
// the receipt
nelement->outbound_pkt = pkt;
nelement->pkt_len = payloadlen;
// now add the receipt timer. This timer should not
// be triggered unless something goes badly wrong
g_timer_add(RECEIPT_TIMEOUT, receipt_timeout, nelement,
NULL, &(nelement->receipt_timer_ev));
}
}
done:
return TIMER_RENEW;
}
// datalen should be the actual data length for SENT messages,
// and the actual message length for RECV messages (which is not equal
// to the datalen in the link_pkt, because we have to stuff the crc in the
// data field of the link_pkt)
void log_pkt(node_element_t *nelement, link_pkt_t *link_pkt,
int datalen, int type)
{
node_element_t *selement = cstate->node_element[sender-1];
char c;
int preamblelen= lossypre ? PREAMBLELEN : 0;
if (type==RCVD) {
c='R';
} else {
c='S';
}
// Format for logfile is:
// Tmestamp NodeID Sender/Receiver Contents
// NOTE: contents not in yet
printf("%s %02u %1c ",
misc_tv_to_str(&link_pkt->rcv_time),
nelement->id,
c);
if (nelement->ctrl_ref==NULL) {
elog(LOG_ERR, "NULL ctrl struct reference!");
exit(1);
}
if (type == RCVD) {
if (mode == SEND_DUMP_MODE )
printf ("%02d ", find_errors_sd(link_pkt, datalen, 0));
else if (mode == SPEW_MODE)
printf ("%02d ", find_errors_spew(link_pkt, datalen, 0));
else if (mode == LISTEN_MODE)
printf ("xx ");
else if (mode == BLAST_MODE) {
// printf ("%02d ", find_errors_sd(link_pkt, datalen, 0));
if (link_pkt->crc_ok) printf("00 "); else printf("xx ");
}
} else
printf ("%02d ", link_pkt->n_xmits);
// If dump is enabled, dump the packet contents
if (dump == 1) {
int j=0;
// Dumping the ENTIRE link_hdr would not be a very good idea
// since it's huge and 90% of it is just 0s.
// The right way of doing this is to convert the link_pkt BACK
// to a tos-msg, since we're using motes after all
// OR, have a special tos device that the motenic provides
//
// For now, I dump the following:
// destinationID sourceID type ext_type length PAYLOAD
//
// Link header first
printf("%02X %02X %02X %02X %02X ",
(link_pkt->dst.id & 0xFF),
((link_pkt->dst.id >> 8) & 0xFF),
(link_pkt->ext_type & 0xFF),
(link_pkt->ext_group & 0xFF),
((datalen - preamblelen) & 0xFF)); // datalen includes crc length, so correct for that
// Now the payload (including preamble if it was dumped)
for (j=0; j<datalen; j++) {
printf("%02X ", (link_pkt->data[j] & 0xFF));
}
// If this is a send event, we grab the crc that we have stored in the selement struct
if (type == SENT) {
printf("%02X ", ((nelement->crc) >> 8) & 0xFF);
printf("%02X ", (nelement->crc) & 0xFF);
} else {
printf("%02X ", (link_pkt->data[datalen+1] & 0xFF));
printf("%02X ", (link_pkt->data[datalen] & 0xFF));
}
printf(" ");
// RSSI
printf("%03d ", link_pkt->rssi);
if (mode == SEND_DUMP_MODE)
printf("%05u", selement->seqno);// Real Seqno
// else printf("no_sn");
}
// it would be more efficient to do a single pass of find_errors() and record the
// errors in some buffer that can then be retrieved here...
if (type == RCVD) {
printf(" ");
if (mode == SEND_DUMP_MODE)
printf ("%02d ", find_errors_sd(link_pkt, datalen, dump));
else if (mode == SPEW_MODE)
printf ("%02d ", find_errors_spew(link_pkt, datalen, 0));
else if (mode == LISTEN_MODE)
printf ("xx ");
else if (mode == BLAST_MODE) {
// printf ("%02d ", find_errors_sd(link_pkt, datalen, dump));
if (link_pkt->crc_ok) printf("00 "); else printf("xx ");
}
}
printf("\n");
fflush(stdout);
}
static int good_pkt_rcvd(lu_context_t *link, link_pkt_t *link_pkt,
int datalen)
{
node_element_t *nelement = (node_element_t *)lu_data(link);
if (nelement == NULL) {
elog(LOG_ERR, "NULL pointer");
goto done;
}
if (receipts) {
// Check for receipts
if (link_pkt->type == PKT_TYPE_MAC_CTRL &&
link_pkt->ext_type == MAC_CTRL_RECEIPT) {
// copy the link pkt header into the outbound pkt
// receipts return ONLY the header (with a retval)
// so we need to hold on to the payload until we
// get the receipt
// What we need from the receipt is the timestamp and the retval
nelement->outbound_pkt->rcv_time = link_pkt->rcv_time;
nelement->outbound_pkt->retval = link_pkt->retval;
nelement->outbound_pkt->n_xmits = link_pkt->n_xmits;
// this is a receipt, log the pkt as sent
log_pkt(nelement, nelement->outbound_pkt,
payloadlen, SENT);
// free the outbound pkt and set the pointer to null
free(nelement->outbound_pkt);
nelement->outbound_pkt=NULL;
if (g_timer_isset(nelement->receipt_timer_ev)) {
g_event_destroy(nelement->receipt_timer_ev);
}
goto done;
}
}
// normal reception
elog(LOG_DEBUG(2), "Node [%d]: Good pkt received, n_xmits %d",
nelement->id, link_pkt->n_xmits);
/* elog(LOG_ERR, "Node [%d]: Good pkt received, n_xmits %d", */
/* nelement->id, link_pkt->n_xmits); */
nelement->good_pkts_rcvd++;
if ( link_pkt->n_xmits >= 250)
log_pkt(nelement, link_pkt, datalen - 2, RCVD);
else
log_pkt(nelement, link_pkt, datalen - 2, SENDREPORT);
done:
free(link_pkt);
return EVENT_RENEW;
}
static int error_pkt_rcvd(void *pkt, ssize_t len,
pd_client_context_t *pd_client)
{
#if 0
pd_client_opts_t *pd_opts;
node_element_t *nelement;
link_pkt_t *link_pkt = (link_pkt_t *)pkt;
pd_opts=pd_client_opts(pd_client);
nelement=(node_element_t *)(pd_opts->data);
if (nelement==NULL) {
elog(LOG_ERR, "NULL pointer");
goto done;
}
elog(LOG_DEBUG(2), "Node [%d]: Bad pkt received",
nelement->id);
nelement->bad_pkts_rcvd++;
log_pkt(nelement, link_pkt, len, RCVD);
done:
#endif
free(pkt);
return EVENT_RENEW;
}
void ctrl_shutdown(void *data)
{
elog(LOG_NOTICE, "Controller shutting down");
exit(0);
}
void usage(char *name)
{
misc_print_usage
(name,
" -n <num nodes> [-d] [-e] -s <sender>",
" --num-nodes <num nodes>: specify number of nodes\n"
" --sender (or -s) <sender>: sending node (round robin if not specified)\n"
" --dump (or -d): dump packet contents\n"
" --period (or -p) <period>: inter-packet period [ms]\n"
" --pkts (or -k) <pkts>: number of packets to send\n"
" --pktlen (or -l) <len>: length of payload\n"
" --receipts (or -r) : ACK receipts\n"
" --mode (or -m) [spew | send_dump | listen]: what to do \n"
" --lossypre (or -L) data payload will have an extra PREAMBLELEN bytes with preamble \n"
);
exit(1);
}
int main(int argc, char **argv)
{
int i=0;
char motename[10];
emrun_opts_t emrun_opts = {
shutdown: ctrl_shutdown,
silent: 1
};
char *mode_opt;
// we need to convince emrun that we're in the simulator
in_sim = 0;
cstate = malloc(sizeof(ctrl_state_t));
memset(cstate, 0, sizeof(ctrl_state_t));
misc_init(&argc, argv, CVSTAG);
// assign the number of nodes to the struct
misc_parse_option_as_int(&argc, argv, "num-nodes", 'n',
&cstate->num_nodes);
misc_parse_option_as_int(&argc, argv, "period", 'p',
&send_period);
misc_parse_option_as_int(&argc, argv, "sender", 's',
&sender);
misc_parse_option_as_int(&argc, argv, "pkts", 'k',
&npkts);
misc_parse_option_as_int(&argc, argv, "pktlen", 'l',
&payloadlen);
lossypre = misc_parse_out_switch(&argc, argv, "lossypre", 'L');
dump = misc_parse_out_switch(&argc, argv, "dump", ' ');
misc_parse_option_as_int(&argc, argv, "dest", 'd', &destination);
receipts = misc_parse_out_switch(&argc, argv,
"receipts", 'r');
mode_opt = misc_parse_out_option(&argc, argv,
"mode", 'm');
if (misc_args_remain(&argc, argv)) {
elog(LOG_CRIT, "Addidional unparsed arguments!");
usage(argv[0]);
}
if (cstate->num_nodes == 0) {
elog(LOG_CRIT, "Number of nodes is zero!");
usage(argv[0]);
}
if (cstate->num_nodes > MAX_NUM_NODES) {
elog(LOG_CRIT, "Too many nodes, max is %d", MAX_NUM_NODES);
exit(1);
}
if (skipnode(sender)) {
elog (LOG_CRIT, "Node %d is in the skipped nodes list!\n", sender);
exit(1);
}
if (sender == -1) {
allsenders = 1;
sender = 1;
}
if (sender > cstate->num_nodes) {
elog(LOG_CRIT, "Sender out of range!");
usage(argv[0]);
}
if (destination > cstate->num_nodes && destination != LINK_BROADCAST) {
elog(LOG_CRIT, "Destination out of range!");
usage(argv[0]);
}
if (payloadlen > MAX_DATA_LEN) {
elog(LOG_CRIT, "Packet Length too big!");
usage(argv[0]);
}
if (strcmp(mode_opt, "spew") == 0) {
mode = SPEW_MODE;
} else if (strcmp(mode_opt, "send_dump") == 0) {
mode = SEND_DUMP_MODE;
} else if (strcmp(mode_opt, "listen") == 0) {
mode = LISTEN_MODE;
} else if (strcmp(mode_opt, "blast") == 0) {
mode = BLAST_MODE;
} else {
elog(LOG_CRIT, "Unknown mode!");
usage(argv[0]);
}
elog(LOG_NOTICE, "Running with %d nodes", cstate->num_nodes);
// open each link/pkt device in sequence
for (i=0; i<cstate->num_nodes; i++) {
// allocate memory and place it in the array
node_element_t *nelement = malloc(sizeof(node_element_t));
// init lu/pd opts
lu_opts_t lu_opts = {
opts: {
// data: (void *)nelement,
},
receive: good_pkt_rcvd,
};
pd_client_opts_t pd_opts = {
receive: error_pkt_rcvd,
// data: (void *)nelement,
};
memset(nelement, 0, sizeof(node_element_t));
cstate->node_element[i]=nelement;
// set the backwards reference
nelement->ctrl_ref = cstate;
nelement->id = i+1; // node ids start at 1
my_node_id=nelement->id;
// reassign the pointers; this is necessary for callbacks
// to work correcty with different node IDs, since the nelement
// pointer is not set at the beginnig
lu_opts.opts.data = (void *)nelement;
pd_opts.data=(void *)nelement;
sprintf(motename, "mote%d", nelement->id);
// get the name
lu_opts.opts.name = link_parse_uses(&argc, argv, motename);
pd_opts.devname = link_name_s(lu_opts.opts.name, "errors");
if (lu_opts.opts.name == NULL) {
elog(LOG_CRIT, "Please specify --uses!");
exit(1);
}
nelement->link_name = lu_opts.opts.name;
// open the ld and the pd
if (lu_open(&lu_opts, &(nelement->link)) < 0) {
elog(LOG_CRIT, "can't open %s: %m",
link_name(&lu_opts.opts, NULL));
// exit(1);
} else {
elog(LOG_NOTICE, "Using link device %s",
lu_name(nelement->link, NULL));
}
if (pd_client_open(&pd_opts, &(nelement->errors)) < 0) {
elog(LOG_CRIT, "can't open %s: %m", pd_opts.devname);
// exit(1);
} else {
elog(LOG_NOTICE, "Using errors device %s", pd_opts.devname);
}
}
if ((mode != LISTEN_MODE) && (mode != BLAST_MODE)) {
// Start the sender timer
g_timer_add(send_period, send_timer_expired, cstate, NULL,
&(cstate->send_timer_ev));
}
// Ok now it's time to start emrun
emrun_init(&emrun_opts);
g_main();
elog(LOG_CRIT, "event loop terminated abnormally!");
return 0;
}
See more files for this project here