tosnic_framer.c from EmStar at Krugle
Show tosnic_framer.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"
/*
* tosnic_framer.c
*
* frames and deframes packets; demuxes packets to clients
*/
static void tosnic_framer_run_output(tosnic_state_t *s);
static void tosnic_framer_run_input(tosnic_state_t *s);
static int tosnic_framer_ack_timeout(void *data, int interval, g_event_t *ev);
static void tosnic_framer_output_ack(tosnic_state_t *s, uint8_t seqno, int acked);
/*
* The framing code
*/
#define SYNC_BYTE 0x7e
#define ESCAPE_BYTE 0x7d
#define ESCAPE_BIT 0x20
static
int tosnic_framer_resync(tosnic_state_t *s)
{
if (s->current_buffer) {
int i;
for (i=0; i<s->current_buffer->len; i++) {
if (s->current_buffer->buf[i] == SYNC_BYTE) {
if (i != 0) {
elog(LOG_NOTICE, "Dropping %d inter-frame bytes", i);
buf_remove(s->current_buffer, 0, i);
}
return 0;
}
}
}
return -1;
}
static
uint16_t tosnic_framer_crc_byte(uint16_t crc, uint8_t b)
{
uint8_t i;
crc = crc ^ b << 8;
i = 8;
do {
if (crc & 0x8000)
crc = crc << 1 ^ 0x1021;
else
crc = crc << 1;
} while (--i);
return crc;
}
static
uint16_t tosnic_framer_crc(uint16_t start_crc, char *packet, int length)
{
uint16_t crc = start_crc;
int i;
for (i=0; i<length; i++)
crc = tosnic_framer_crc_byte(crc, packet[i]);
return crc;
}
static
void tosnic_framer_push_escaped(buf_t *framed, uint8_t *buf, int len)
{
int i;
for (i=0; i<len; i++) {
if (buf[i] == SYNC_BYTE || buf[i] == ESCAPE_BYTE) {
uint8_t byte = ESCAPE_BYTE;
bufcpy(framed, &byte, 1);
byte = buf[i] ^ ESCAPE_BIT;
bufcpy(framed, &byte, 1);
}
else
bufcpy(framed, &(buf[i]), 1);
}
}
static
buf_t *tosnic_framer_frame_to_buf(uint8_t type, uint8_t seqno, buf_t *payload)
{
buf_t *buf = buf_new();
/* sync */
uint8_t sync_byte = SYNC_BYTE;
bufcpy(buf, &sync_byte, sizeof(sync_byte));
/* push the serial header */
struct tosnic_serial_encap s_hdr = {
type: type,
seqno: seqno
};
tosnic_framer_push_escaped(buf, (uint8_t*)&s_hdr, sizeof(s_hdr));
/* push the payload */
if (payload)
tosnic_framer_push_escaped(buf, (uint8_t*)payload->buf, payload->len);
/* compute crc */
uint16_t crc = tosnic_framer_crc(0, (char*)&s_hdr, sizeof(s_hdr));
crc = tosnic_framer_crc(crc, payload->buf, payload->len);
tosnic_framer_push_escaped(buf, (uint8_t*)&crc, sizeof(crc));
/* trailing sync */
bufcpy(buf, &sync_byte, sizeof(sync_byte));
elog(LOG_DEBUG(5), "Framing packet, type=%u, seqno=%u, payload:", type, seqno);
elog_raw(LOG_DEBUG(5), payload->buf, payload->len);
elog(LOG_DEBUG(5), "Output frame:");
elog_raw(LOG_DEBUG(5), buf->buf, buf->len);
return buf;
}
static
void tosnic_framer_send_current(tosnic_state_t *s)
{
/* set the ack timer */
g_timer_add(s->ack_timeout_length, tosnic_framer_ack_timeout, s, NULL, &(s->ack_timeout));
/* frame the packet */
s->framed =
tosnic_framer_frame_to_buf(s->demux[s->curr_client_index].type_on_deck, s->serial_seqno,
s->demux[s->curr_client_index].on_deck);
s->type_of_framed = s->demux[s->curr_client_index].type_on_deck;
/* send it down now */
tosnic_framer_ack_timeout(s, 0, NULL);
}
static
void tosnic_framer_run_input(tosnic_state_t *s)
{
int escaped;
buf_t *deframed = NULL;
int i;
loop:
/* resync to frame */
if (tosnic_framer_resync(s) < 0)
goto incomplete;
/* debugging */
elog(LOG_DEBUG(5), "Synced to frame:");
elog_raw(LOG_DEBUG(5), s->current_buffer->buf, s->current_buffer->len);
/* start packet */
deframed = buf_new();
escaped = 0;
/* unescape until next sync byte */
for (i=1; i<s->current_buffer->len; i++) {
uint8_t byte = s->current_buffer->buf[i];
/* escape mode */
if (escaped) {
if (byte == SYNC_BYTE) {
elog(LOG_NOTICE, "Illegal sync after escape byte.. resyncing");
s->current_buffer->buf[0] = 0;
goto loop;
}
escaped = 0;
byte ^= ESCAPE_BIT;
bufcpy(deframed, &byte, 1);
}
/* regular mode */
else {
if (byte == ESCAPE_BYTE) {
escaped = 1;
}
else if (byte == SYNC_BYTE) {
buf_remove(s->current_buffer, 0, i);
goto process_frame;
}
else
bufcpy(deframed, &byte, 1);
}
}
/* incomplete frame.. */
goto incomplete;
process_frame:
/* debugging */
elog(LOG_DEBUG(5), "Got frame!");
elog_raw(LOG_DEBUG(5), deframed->buf, deframed->len);
int payload_len = deframed->len - (sizeof(struct tosnic_serial_encap) +
sizeof(uint16_t));
if (payload_len >= 0) {
uint16_t computed_crc = tosnic_framer_crc(0, deframed->buf, deframed->len-sizeof(int16_t));
uint16_t read_crc;
memmove(&read_crc, deframed->buf + deframed->len - sizeof(int16_t), sizeof(int16_t));
if (read_crc == computed_crc) {
struct tosnic_serial_encap s_hdr = {};
/* grab type/seqno from serial encapsulation */
memmove(&s_hdr, deframed->buf, sizeof(s_hdr));
uint8_t type_index = TOSNIC_TYPE(s_hdr.type);
/* note whether we have a sequence gap in packets from mote->pc..
* this tells us whether we dropped one */
if (s_hdr.seqno == s->last_mote_seqno) {
if (s->last_mote_seqno)
elog(LOG_DEBUG(3), "Same seqno as last message..?? %u", s_hdr.seqno);
}
else if (s_hdr.seqno != (uint8_t)(s->last_mote_seqno+1)) {
if (s->last_mote_seqno)
elog(LOG_NOTICE, "Dropped message from mote->pc? %u/%u",
s_hdr.seqno, s->last_mote_seqno);
s->mote_to_pc_drops++;
}
s->last_mote_seqno = s_hdr.seqno;
/* got at least one packet */
s->serial_packet_recd = 1;
if (s->mote_fail_warn) {
elog(LOG_WARNING, "...Mote OK!");
s->mote_fail_warn = 0;
}
/* process returning acks for pc->mote traffic */
if (type_index == TOSNIC_ACK_PACKET) {
if (payload_len == 2)
tosnic_framer_output_ack(s, (uint8_t)deframed->buf[sizeof(s_hdr)],
(uint8_t)deframed->buf[sizeof(s_hdr)+1]);
else
elog(LOG_NOTICE, "Got ack with wrong size: payload size=%d", payload_len);
}
/* otherwise try to deliver it to a handler */
else {
int i;
for (i=0; i<TOSNIC_MAX_CLIENTS; i++) {
if (s->demux[i].in_use && s->demux[i].handler &&
(s->demux[i].type == 0 || s->demux[i].type == type_index)) {
/* strip off serial header and crc */
buf_t *payload = buf_new();
bufcpy(payload, deframed->buf+sizeof(s_hdr),
deframed->len - sizeof(s_hdr) - sizeof(int16_t));
s->demux[i].handler(s, i, type_index, payload);
}
}
/* generate ACK */
if (TOSNIC_ACK_REQ(s_hdr.type)) {
buf_t *ack =
tosnic_framer_frame_to_buf(TOSNIC_ACK_PACKET, s_hdr.seqno, NULL);
tosnic_lower_write(s, ack);
buf_free(ack);
}
}
}
else {
elog(LOG_NOTICE, "Serial CRC failed: %u != %u", read_crc, computed_crc);
s->serial_crc_fail++;
}
}
else {
if (deframed->len > 0) {
s->small_packet_fail++;
elog(LOG_NOTICE, "Dropping small frame of length %d", deframed->len);
}
}
/* extract next frame */
if (deframed)
buf_free(deframed);
deframed = NULL;
goto loop;
incomplete:
if (deframed)
buf_free(deframed);
return;
}
/*
* serial ack mechanism and glue
*/
void tosnic_framer_reset(tosnic_state_t *s)
{
/* $$ randomize seqno? */
g_event_destroy(s->ack_timeout);
if (s->framed) buf_free(s->framed);
s->framed = NULL;
s->sequential_ack_fail = 0;
s->serial_data_recd = 0;
s->serial_packet_recd = 0;
s->reset_count++;
tosnic_framer_run_output(s);
}
static
int tosnic_framer_ack_timeout(void *data, int interval, g_event_t *ev)
{
tosnic_state_t *s = (tosnic_state_t *)data;
if (ev) {
s->retrans_count++;
s->sequential_ack_fail++;
if (s->serial_packet_recd) {
elog(s->sequential_ack_fail < 3 ? LOG_DEBUG(0) : LOG_WARNING,
"Ack timeout on seqno %u[%d].. resending",
s->serial_seqno, s->sequential_ack_fail);
}
if (s->sequential_ack_fail > TOSNIC_MAX_ACK_FAIL) {
tosnic_reset(s);
return TIMER_DONE;
}
}
elog(LOG_DEBUG(2), "Sending packet, proto %d, seqno %u",
s->framed->buf[1], s->serial_seqno);
s->msg_out = 1;
tosnic_lower_write(s, s->framed);
return TIMER_RENEW;
}
static
void tosnic_framer_output_ack(tosnic_state_t *s, uint8_t seqno, int acked)
{
/* check for seqno mismatch */
if (seqno != s->serial_seqno) {
elog(seqno+1 == s->serial_seqno ? LOG_DEBUG(0) : LOG_WARNING,
"Seqno mismatch: ack for %u, current is %u", seqno, s->serial_seqno);
s->retrans_ack_count++;
return;
}
if (!s->msg_out) {
elog(LOG_WARNING, "Duplicate ACK, seqno %u?", seqno);
return;
}
/* if this is a nack, force resend now */
if (acked == TOSNIC_ACK_NACK) {
elog(LOG_DEBUG(0), "NACK received on seqno %u", seqno);
tosnic_framer_ack_timeout(s, 0, NULL);
return;
}
if (acked == TOSNIC_ACK_NO_HANDLER) {
elog(LOG_WARNING, "No handler on mote for type %d.. packet to mote was dropped.",
s->type_of_framed);
s->mote_no_handler_drops++;
}
/*
* otherwise this is a valid ack..
*/
elog(LOG_DEBUG(2), "ACK received on seqno %u", seqno);
/* free framed buffer */
buf_free(s->framed);
s->framed = NULL;
s->msg_out = 0;
/* kill ack timer */
g_event_destroy(s->ack_timeout);
/* increment curr_client for round robin */
int curr = s->curr_client_index;
s->curr_client_index = (s->curr_client_index + 1) % TOSNIC_MAX_CLIENTS;
/* call send_done, then free the buffer on-deck */
buf_t *tmp_buf = s->demux[curr].on_deck;
s->demux[curr].on_deck = NULL;
if (s->demux[curr].send_done)
s->demux[curr].send_done(s, curr, s->demux[curr].type_on_deck, tmp_buf, acked);
else
elog(LOG_WARNING, "No send_done callback for client %s[%d]",
s->demux[curr].description, curr);
buf_free(tmp_buf);
/* run queue */
tosnic_framer_run_output(s);
}
static
void tosnic_framer_run_output(tosnic_state_t *s)
{
/* if not waiting for an ack from the last packet */
if (s->ack_timeout == NULL) {
/* loop through demux and send the next packet that is "on_deck" */
int i;
for (i=0; i<TOSNIC_MAX_CLIENTS; i++) {
int index = (s->curr_client_index + i) % TOSNIC_MAX_CLIENTS;
if (s->demux[index].on_deck) {
elog(LOG_DEBUG(2), "Starting send for client %s", s->demux[index].description);
s->curr_client_index = index;
s->serial_seqno++;
s->sequential_ack_fail = 0;
tosnic_framer_send_current(s);
return;
}
}
}
}
/*
* Demux interface
*/
void tosnic_framer_enqueue_from_upper(tosnic_state_t *s, int client_index, int type, buf_t *pkt)
{
if (s->demux[client_index].on_deck) {
elog(LOG_WARNING, "Data already on deck.. dropping!");
buf_free(pkt);
return;
}
s->demux[client_index].on_deck = pkt;
s->demux[client_index].type_on_deck = type;
tosnic_framer_run_output(s);
}
void tosnic_framer_data_from_lower(tosnic_state_t *s, buf_t *pkt)
{
s->serial_rx_count += pkt->len;
if (s->current_buffer == NULL)
s->current_buffer = buf_new();
bufcat(s->current_buffer, pkt);
elog(LOG_DEBUG(5), "Input from lower:");
elog_raw(LOG_DEBUG(5), pkt->buf, pkt->len);
buf_free(pkt);
s->serial_data_recd = 1;
tosnic_framer_run_input(s);
}
/* returns index */
int tosnic_framer_register(tosnic_state_t *s, tosnic_client_t *client_spec)
{
if (s->clients_count >= TOSNIC_MAX_CLIENTS) {
elog(LOG_WARNING, "Can't register client %s.. no more room!", client_spec->description);
return -1;
}
s->demux[s->clients_count] = *client_spec;
s->demux[s->clients_count].in_use = 1;
s->demux[s->clients_count].index = s->clients_count;
elog(LOG_NOTICE, "Registered type handler %s[%d] for %d...",
client_spec->description, s->clients_count, client_spec->type);
return s->clients_count++;
}
See more files for this project here