Show tosnic_upper.c syntax highlighted
/*
*
* Copyright (c) 2005 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.
*
*/
#include "tosnic_i.h"
#include "link/libmacinfo.h"
/*
* tosnic_upper.c
*
* Provides an emstar link interface
*/
static void tosnic_upper_note_send(tosnic_state_t *s, link_pkt_t *pkt, uint8_t seqno);
static
int tosnic_upper_watchdog_timeout(void *data, int interval, g_event_t *ev)
{
tosnic_state_t *s = (tosnic_state_t*)data;
elog(LOG_CRIT, "Transmit timeout!");
s->tx_watchdog_fires++;
tosnic_reset(s);
return EVENT_DONE;
}
static
void tosnic_upper_set_watchdog(tosnic_state_t *s)
{
if (s->tx_watchdog == NULL)
g_timer_add(TOSNIC_TX_WATCHDOG, tosnic_upper_watchdog_timeout,
s, NULL, &(s->tx_watchdog));
}
/*****************************************************************************
*
* link provider functions
*/
static
int tosnic_command(lp_context_t *lp, parser_state_t *cmd_input)
{
tosnic_state_t *s = (tosnic_state_t*)lp_data(lp);
int retval = EVENT_RENEW;
/* parse the command */
while (misc_parse_next_kvp(cmd_input) >= 0) {
if (strcmp(cmd_input->key, "acks") == 0) {
if (strncmp(cmd_input->value, "on", 2) == 0 ||
strncmp(cmd_input->value, "1", 1) == 0) {
elog(LOG_WARNING, "set acks.. NOT IMPLEMENTED");
retval = EVENT_ERROR(EINVAL);
}
}
else if (strcmp(cmd_input->key, "reset") == 0) {
tosnic_reset(s);
}
else if (strcmp(cmd_input->key, "hard_reset") == 0) {
elog(LOG_WARNING, "hard reset not implemented");
tosnic_reset(s);
}
else if (strcmp(cmd_input->key, "if_id") == 0) {
if (parse_if_id(cmd_input->value, &(s->if_id)) < 0) {
elog(LOG_WARNING, "error parsing ifid %s", cmd_input->value);
retval = EVENT_ERROR(EINVAL);
}
}
else if (strcmp(cmd_input->key, "group") == 0) {
uint group;
if (strncasecmp(cmd_input->value, "0x", 2)) {
if (1 == sscanf(cmd_input->value+2, "%x", &group))
s->default_group = group;
else goto group_err;
}
else {
if (1 == sscanf(cmd_input->value+2, "%d", &group))
s->default_group = group;
else {
group_err:
elog(LOG_WARNING, "error parsing group %s", cmd_input->value);
retval = EVENT_ERROR(EINVAL);
}
}
}
}
return retval;
}
static
void tosnic_usage(lp_context_t *lp, buf_t *fill_usage)
{
tosnic_state_t *s = (tosnic_state_t*)lp_data(lp);
bufprintf(fill_usage,
" acks=[on,off]: enables link layer acks\n"
" reset: resets emstar side\n"
" hard_reset: resets device and emstar side\n"
" if_id=<value>: sets interface address to dotted value notation \n"
" group=<value>: sets the default group (currently 0x%x)\n"
"\n",
s->default_group);
}
static
int tosnic_send(lp_context_t *lp, link_pkt_t *link_pkt,
int data_len, int loop_needed)
{
tosnic_state_t *s = (tosnic_state_t*)lp_data(lp);
/* fill in header */
link_pkt->src.id = s->if_id;
gettimeofday(&(link_pkt->rcv_time), NULL);
/* loopback */
if (loop_needed)
lp_loop_receive(lp, link_pkt, data_len);
/* OK, now we convert it to tos msg, frame it, and send it */
/* start packet */
buf_t *pkt = buf_new();
/* push header */
TOS_Msg_emstar_hdr_t hdr = {
type: (link_pkt->type == PKT_TYPE_TOS) ? link_pkt->ext_type : PKT_TYPE_TOS,
group: (link_pkt->ext_group ? link_pkt->ext_group : s->default_group),
length: (link_pkt->type == PKT_TYPE_TOS) ? data_len : data_len + sizeof(struct tosnic_emstar_encap),
addr: link_pkt->dst.id,
seq_num: s->data_seqno++,
retx_req: link_pkt->retx
};
bufcpy(pkt, &hdr, sizeof(hdr));
/* if this is an emstar packet, we add an emstar encapsulation header */
if (link_pkt->type != PKT_TYPE_TOS) {
struct tosnic_emstar_encap em = {
emstar_type: link_pkt->type,
src_if: link_pkt->src.id
};
bufcpy(pkt, &em, sizeof(em));
}
/* copy the data */
bufcpy(pkt, link_pkt->data, data_len);
/* pump to framer, framer will free buf */
s->packet_pending = 1;
tosnic_framer_enqueue_from_upper(s, s->data_client_index, TOSNIC_DATA_PACKET, pkt);
tosnic_upper_note_send(s, link_pkt, hdr.seq_num);
/* block until framer send is acked */
return PD_BLOCKED;
}
static
int tosnic_enqueue(lp_context_t *lp, link_pkt_t *link_pkt,
int data_len)
{
/* always OK */
return 0;
}
static
int tosnic_check_status(lp_context_t *lp)
{
tosnic_state_t *s = (tosnic_state_t*)lp_data(lp);
link_status_t stat = {
if_id: s->if_id,
MTU: (s->MTU > sizeof(struct tosnic_emstar_encap) ?
s->MTU - sizeof(struct tosnic_emstar_encap) : 0),
POT: s->power
};
static char buf[128];
sprintf(buf, "TOSBase on %s", s->mote_type_detected);
if (s->description) free(s->description);
s->description = strdup(buf);
lp_update_root_trace(s->lp, s->serial_devname, s->description);
lp_push_status(lp, &stat);
return 0;
}
/*
* SEND_DONE protocol and receipt generation
*/
static
void tosnic_upper_send_receipt(tosnic_state_t *s, link_pkt_t *link_pkt, int retval)
{
if (link_receipt_is_requested(link_pkt)) {
link_pkt->type = PKT_TYPE_MAC_CTRL;
link_pkt->ext_type = MAC_CTRL_RECEIPT;
link_pkt->retval = retval;
lp_receive(s->lp, link_pkt, 0);
}
}
static
void tosnic_upper_maybe_unblock(tosnic_state_t *s)
{
if (s->pending_count < s->pending_queue_length && s->packet_pending == 0) {
g_event_destroy(s->tx_watchdog);
lp_unblock(s->lp, 0);
}
else {
tosnic_upper_set_watchdog(s);
}
}
static
void tosnic_upper_process_send_done(tosnic_state_t *s, int index, int errno_value)
{
tosnic_upper_send_receipt(s, &(s->pending[index].hdr), errno_value);
s->pending_count--;
memmove(&(s->pending[index]), &(s->pending[index+1]),
sizeof(s->pending[0]) * (s->pending_count-index));
if (errno_value) lp_report_tx_error(s->lp);
tosnic_upper_maybe_unblock(s);
}
static
void tosnic_upper_note_send(tosnic_state_t *s, link_pkt_t *pkt, uint8_t seqno)
{
while (s->pending_count >= s->pending_queue_length) {
elog(LOG_WARNING, "Pending queue length exceeded!");
tosnic_upper_process_send_done(s, 0, ETIMEDOUT);
}
tosnic_upper_set_watchdog(s);
s->pending[s->pending_count].hdr = *pkt;
s->pending[s->pending_count].seqno = seqno;
elog(LOG_DEBUG(2), "Sending data packet, SENDDONE seqno %u",
s->pending[s->pending_count].seqno);
s->pending_count++;
}
static
void tosnic_upper_send_done_proto_data_ready(tosnic_state_t *s, int index, int type, buf_t *pkt)
{
int i=0,j;
/* verify length */
if (pkt->len != sizeof(send_done_msg_t)) {
elog(LOG_WARNING, "Wrong size SENDDONE message.. %d", pkt->len);
goto done;
}
send_done_msg_t *sd_hdr = (send_done_msg_t *)pkt->buf;
/* search for this packet and process it.. */
for (i=0; i<s->pending_count; i++) {
if (s->pending[i].seqno == sd_hdr->seqno) {
int retval = 0;
switch (sd_hdr->result) {
case SEND_DONE_SUCCESS:
retval = 0;
break;
case SEND_DONE_FAILED_TRANSMISSION:
if (s->pending[i].hdr.dst.id == LINK_BROADCAST)
retval = 0;
else
retval = ECOMM;
break;
default:
case SEND_DONE_UNKNOWN_SEQUENCE_NUMBER:
retval = EBADMSG;
break;
}
elog(LOG_DEBUG(2), "Firing SENDDONE for seqno %u, errno %s",
s->pending[i].seqno, strerror(retval));
tosnic_upper_process_send_done(s, i, retval);
s->last_send_done = sd_hdr->seqno;
goto done;
}
}
/* not found.. give up.. */
if (sd_hdr->seqno == s->last_send_done) {
s->dup_send_done_count++;
elog(LOG_DEBUG(0), "Ignoring duplicate send done for seqno %u", sd_hdr->seqno);
}
else {
s->spurious_send_done_count++;
elog(LOG_WARNING, "Can't find matching SENDDONE for seqno %u", sd_hdr->seqno);
}
goto free_pkt;
done:
/* clear out all prior queued send dones.. */
for (j=0; j<i; j++) {
/* clear out any pending stuff that's not our returning packet.. */
elog(LOG_WARNING, "Firing missing SENDDONE for seqno %u (we are %u)",
s->pending[0].seqno, sd_hdr->seqno);
s->dropped_send_done_count++;
tosnic_upper_process_send_done(s, 0, ETIMEDOUT);
}
free_pkt:
buf_free(pkt);
}
/*
* interface to framer
*/
static
if_id_t bcast_extend(uint16_t addr)
{
return (addr == 0xFFFF) ? LINK_BROADCAST : addr;
}
static
void tosnic_upper_data_proto_data_ready(tosnic_state_t *s, int index, int type, buf_t *pkt)
{
char *reason = NULL;
if (pkt->len < sizeof(TOS_Msg_emstar_hdr_t)) {
reason = "incomplete tos_hdr";
goto fail;
}
TOS_Msg_emstar_hdr_t *tos_hdr = (TOS_Msg_emstar_hdr_t *)pkt->buf;
/* convert back to link_pkt format */
link_pkt_t link_hdr = {
type: tos_hdr->type,
ext_group: tos_hdr->group,
dst: {
id: bcast_extend(tos_hdr->addr)
},
#if 0
src: {
id: bcast_extend(tos_hdr->s_addr)
},
#endif
rssi: tos_hdr->strength,
lqi: tos_hdr->lqi,
seqno: tos_hdr->seq_num,
retx: tos_hdr->retx_req
};
/* $$$ implement macinfo interface here..!! */
/* set bogus receive time */
struct timeval rcv_time;
gettimeofday(&rcv_time, NULL);
macinfo_set_rcv_time(&link_hdr, &rcv_time);
char *payload = (char *)tos_hdr->data;
int data_len = pkt->len - sizeof(*tos_hdr);
/* de-encapsulate if emstar type */
if (link_hdr.type == PKT_TYPE_TOS) {
struct tosnic_emstar_encap *em = (struct tosnic_emstar_encap *)tos_hdr->data;
if (pkt->len < (sizeof(struct tosnic_emstar_encap)+sizeof(TOS_Msg_emstar_hdr_t))) {
reason = "incomplete emstar encapsulation hdr";
goto fail;
}
link_hdr.type = em->emstar_type;
link_hdr.src.id = bcast_extend(em->src_if);
payload += sizeof(struct tosnic_emstar_encap);
data_len -= sizeof(struct tosnic_emstar_encap);
}
else {
link_hdr.ext_type = link_hdr.type;
link_hdr.type = PKT_TYPE_TOS;
}
buf_t *link_pkt = buf_new();
bufcpy(link_pkt, &link_hdr, sizeof(link_hdr));
bufcpy(link_pkt, payload, data_len);
/* push to clients */
lp_receive(s->lp, (link_pkt_t*)(link_pkt->buf), data_len);
/* free */
buf_free(link_pkt);
buf_free(pkt);
return;
fail:
lp_report_rx_error(s->lp);
elog(LOG_WARNING, "Bad message format: %s, length %d", reason, pkt->len);
buf_free(pkt);
}
static
void tosnic_upper_data_proto_send_done(tosnic_state_t *s, int index, int type, buf_t *pkt, int acked)
{
if (acked != TOSNIC_ACK_ACK)
elog(LOG_WARNING, "message acked with error.. no handler on mote?");
/* unblock provider to send next message */
s->packet_pending = 0;
tosnic_upper_maybe_unblock(s);
}
/*
* raw access to mote serial
*/
static
void tosnic_upper_raw_data_ready(tosnic_state_t *s, int index, int type, buf_t *pkt)
{
if (pkt->len > 65535) {
elog(LOG_WARNING, "got huge packet (%d), dropping", pkt->len);
goto free_it;
}
char buf[65536];
buf[0] = type;
memmove(buf+1, pkt->buf, pkt->len);
pd_receive(s->mote_raw_access, buf, pkt->len+1);
free_it:
buf_free(pkt);
}
static
void tosnic_upper_raw_send_done(tosnic_state_t *s, int index, int type, buf_t *pkt, int acked)
{
/* unblock provider to send next message */
pd_unblock(s->mote_raw_access, 0);
}
static
int tosnic_upper_mote_raw_filter(pd_context_t *pd, const void *packet, int
packetlen, const pd_filter_t *filter)
{
if (filter->len >= PD_MAX_FILTER_LEN) {
elog(LOG_WARNING, "Illegal filter!");
return 0;
}
if (packetlen <= 0) {
elog(LOG_CRIT, "Illegal packet.. length %d!", packetlen);
return 0;
}
char type = *(char*)packet;
int i;
for (i=0; i<filter->len; i++)
if (type == filter->data[i]) {
return 1;
}
return 0;
}
static
int tosnic_upper_mote_raw_send(pd_context_t *pd, const void *packet, int
packetlen, int loop_needed)
{
tosnic_state_t *s = (tosnic_state_t *)pd_data(pd);
buf_t *pkt = buf_new();
bufcpy(pkt, packet+1, packetlen-1);
tosnic_framer_enqueue_from_upper(s, s->raw_client_index,
((uint8_t*)packet)[0], pkt);
if (loop_needed)
pd_loop_receive(pd, packet, packetlen);
return PD_BLOCKED;
}
static
int tosnic_upper_mote_raw_enqueue(pd_context_t *pd, const void *packet, int
packetlen)
{
if (packetlen > 0) return 0;
return -EMSGSIZE;
}
/*
* status/config interface
*/
void tosnic_upper_check_config(tosnic_state_t *s)
{
/* $$$ if reqeust != curr then config? */
}
void tosnic_upper_process_status_power(tosnic_state_t *s, uint8_t power)
{
s->power = power;
tosnic_upper_check_config(s);
}
void tosnic_upper_process_status_channel(tosnic_state_t *s, uint8_t channel)
{
s->channel = channel;
tosnic_upper_check_config(s);
}
void tosnic_upper_process_status_stats(tosnic_state_t *s, radio_stats_t *stats)
{
s->MTU = stats->MTU;
s->tosbase_version = stats->tosbase_version;
if (s->mote_type_detected) free(s->mote_type_detected);
stats->mote_rev[sizeof(stats->mote_rev)-1] = 0;
s->mote_type_detected = strdup(stats->mote_rev);
/* compare and add.. */
s->mote_serial_crc_fail +=
(((int16_t)stats->serial_crc_fail) - ((int16_t)s->last_radio_stats.serial_crc_fail));
s->mote_radio_crc_fail +=
(((int16_t)stats->radio_crc_fail) - ((int16_t)s->last_radio_stats.radio_crc_fail));
s->mote_radio_queue_drops +=
(((int16_t)stats->radio_queue_drops) - ((int16_t)s->last_radio_stats.radio_queue_drops));
}
/*
* protocol status dev
*/
static
int tosnic_upper_mote_proto_status_print(status_context_t *info, buf_t *buf)
{
tosnic_state_t *s = (tosnic_state_t *) sd_data(info);
bufprintf(buf, "Mote protocol status (%s), node %s\n", s->dev_name, print_if_id(my_node_id));
bufprintf(buf, "=====================================\n");
bufprintf(buf,
"Hardware status:\n"
" Mote type configured: %s\n"
" Mote type detected: %s\n"
" Firmware version: %d\n"
"\n"
"Mote->PC serial comm stats:\n"
" CRC errors: %d\n"
" Small packets: %d\n"
" Sequence gaps: %d\n"
" Serial RX: %u\n"
"\n"
"PC->Mote serial comm stats:\n"
" Retrans count: %d\n"
" Serial TX: %u\n"
" Serial CRC err: %u\n"
"\n"
"Mote-side comm stats\n"
" Radio CRC err: %u\n"
" Queue drops: %u\n"
"\n"
"Reset counter: %d\n"
"Watchdog counter: %d\n"
"\n"
"Default group: %d\n"
"\n"
"Queue state:\n"
" Pending data packet: %d\n"
" Next expected send done: %d\n"
" Last submit send done: %d\n"
" Mote side queue length: %d (max=%d)\n"
" Duplicate send dones: %d\n"
" Dropped send dones: %d\n"
" Spurious send dones: %d\n"
"\n"
,
s->mote_type_str,
s->mote_type_detected,
s->tosbase_version,
s->serial_crc_fail, s->small_packet_fail, s->mote_to_pc_drops,
s->serial_rx_count, s->retrans_count, s->serial_tx_count,
s->mote_serial_crc_fail, s->mote_radio_crc_fail, s->mote_radio_queue_drops,
s->reset_count, s->tx_watchdog_fires,
s->default_group,
s->packet_pending,
s->pending_count ? s->pending[0].seqno : -1,
s->pending_count ? s->pending[s->pending_count-1].seqno : -1,
s->pending_count, s->pending_queue_length,
s->dup_send_done_count,
s->dropped_send_done_count,
s->spurious_send_done_count
);
return STATUS_MSG_COMPLETE;
}
void tosnic_upper_reset(tosnic_state_t *s)
{
s->last_send_done = 0;
/* clear at least one slot in the queue */
while (s->pending_count >= s->pending_queue_length) {
tosnic_upper_process_send_done(s, 0, ETIMEDOUT);
}
/* reset watchdog and maybe unblock now */
g_event_destroy(s->tx_watchdog);
tosnic_upper_maybe_unblock(s);
}
void tosnic_upper_init(tosnic_state_t *s, int *argc, char **argv)
{
lp_opts_t lp_opts = {
description: s->description,
root_trace: s->trace,
send: tosnic_send,
enqueue: tosnic_enqueue,
command_request: tosnic_command,
usage: tosnic_usage,
status_request: tosnic_check_status,
opts: {
name: s->dev_name,
if_class: s->class_name,
data: s
},
we_are_root: 1
};
if (lp_register(&lp_opts, &(s->lp)) < 0) {
elog(LOG_CRIT, "can't create link dev %s: %m", lp_opts.opts.name);
exit(1);
}
/* init default pending queue length */
s->pending_queue_length = 1;
/* push status now */
tosnic_check_status(s->lp);
/* register to get deframed data */
tosnic_client_t spec = {
type: TOSNIC_DATA_PACKET,
handler: tosnic_upper_data_proto_data_ready,
send_done: tosnic_upper_data_proto_send_done,
description: "Packet Data"
};
s->data_client_index = tosnic_framer_register(s, &spec);
/* register for SEND_DONE protocol */
tosnic_client_t spec_ack = {
type: TOSNIC_SEND_DONE_PACKET,
handler: tosnic_upper_send_done_proto_data_ready,
description: "Packet SendDone"
};
tosnic_framer_register(s, &spec_ack);
/* mote protocol status */
status_dev_opts_t st_opts = {
device: {
devname: link_name_s(s->dev_name, "mote/proto"),
device_info: s
},
printable: tosnic_upper_mote_proto_status_print
};
if (g_status_dev(&st_opts, &(s->mote_proto_status)) < 0) {
elog(LOG_CRIT, "Unable to create status device %s: %m",
st_opts.device.devname);
exit(1);
}
/* mote raw access */
packet_dev_opts_t raw_opts = {
device: {
devname: link_name_s(s->dev_name, "mote/raw"),
device_info: s
},
send: tosnic_upper_mote_raw_send,
filter: tosnic_upper_mote_raw_filter,
enqueue: tosnic_upper_mote_raw_enqueue
};
if (g_packet_dev(&raw_opts, &(s->mote_raw_access)) < 0) {
elog(LOG_CRIT, "Unable to create raw access device %s: %m",
raw_opts.device.devname);
exit(1);
}
/* register for raw access */
tosnic_client_t spec_raw = {
type: 0, /* all */
handler: tosnic_upper_raw_data_ready,
send_done: tosnic_upper_raw_send_done,
description: "Mote Raw Access"
};
s->raw_client_index = tosnic_framer_register(s, &spec_raw);
}
See more files for this project here