/*************************************************************************
* the callback fnction for reading data from cc2420 *
*************************************************************************/
void _MacRecvCallBack(int16_t timestamp)
{
VMAC_PPDU ppdu;
vhal_data vd;
mac_to_vhal(&ppdu, &vd);
Radio_Disable_Interrupt(); //disable interrupt while reading data
if( !Radio_Recv_Pack(&vd) ) {
Radio_Enable_Interrupt(); //enable interrupt
return;
}
Radio_Enable_Interrupt(); //enable interrupt
vhal_to_mac(&vd, &ppdu);
// Andreas - filter node ID here, even before allocating any new memory
// if you're using sos/config/base you must comment this block out!
if (net_to_host(ppdu.mpdu.daddr) != NODE_ADDR && net_to_host(ppdu.mpdu.daddr) != BCAST_ADDRESS)
{
ker_free(vd.payload);
return;
}
Message *msg = msg_create();
if( msg == NULL ) {
ker_free(vd.payload);
return;
}
mac_to_sosmsg(&ppdu, msg);
// Andreas - start debug
#ifdef ENA_VMAC_UART_DEBUG
uint8_t *payload;
uint8_t msg_len;
msg_len=msg->len;
payload = msg->data;
//post_uart(msg->sid, msg->did, msg->type, msg_len, payload, SOS_MSG_RELEASE, msg->daddr);
// Swap daddr with saddr, because daddr is useless when debugging.
// Of course, if sossrv says "dest addr: 15" that actually means the message SENDER was node 15
post_uart(msg->sid, msg->did, msg->type, msg_len, payload, SOS_MSG_RELEASE, msg->saddr);
#endif
//if (msg->daddr == NODE_ADDR || msg->daddr == BCAST_ADDRESS)
handle_incoming_msg(msg, SOS_MSG_RADIO_IO);
}
/**
* @brief Server receiver side
*/
static void server_recv_thread(int fd)
{
uint8_t d[1024];
int cnt;
//sched_yield();
while( 1 ) {
cnt = read(fd, d, SOS_MSG_HEADER_SIZE);
if (cnt == 0) { // The socket is closed!
interrupt_remove_read_fd(fd);
close(fd);
break;
}
if(cnt != SOS_MSG_HEADER_SIZE)
break;
Message *buf = (Message*)d;
Message *m = msg_create();
if (m == NULL)
break;
uint8_t *data = ker_malloc(buf->len,UART_PID);
if (data == NULL) {
msg_dispose(m);
break;
}
memcpy(m,buf,SOS_MSG_HEADER_SIZE);
m->daddr = entohs(m->daddr);
m->saddr = entohs(m->saddr);
m->data = data;
m->flag = SOS_MSG_RELEASE;
while ((cnt = read(fd, m->data, m->len)) < 0);
if (cnt != m->len) {
msg_dispose(m);
break;
}
handle_incoming_msg(m, SOS_MSG_UART_IO);
}
}
/*
* @brief Process data received from SIO
*/
static inline void process_incoming_data(void)
{
switch (sio_rx_state) {
case UART_RX_STATE_SOT:
sio_rx_ptr = sio_rx_buf;
if (SOT == data[rx_index]) {
sio_rx_state = UART_RX_STATE_LENGTH;
}
break;
case UART_RX_STATE_LENGTH:
sio_rx_length = data[rx_index];
if (sio_rx_length) {
sio_rx_state = UART_RX_PROTOCOL_ID;
*sio_rx_ptr = sio_rx_length;
sio_rx_ptr++;
} else {
/* NULL message */
sio_rx_ptr = sio_rx_buf;
sio_rx_state = UART_RX_STATE_SOT;
}
break;
case UART_RX_PROTOCOL_ID:
protocol_id = data[rx_index];
sio_rx_length--;
sio_rx_state = UART_RX_STATE_DATA;
break;
case UART_RX_STATE_DATA:
*sio_rx_ptr = data[rx_index];
sio_rx_ptr++;
sio_rx_length--;
if (!sio_rx_length) {
sio_rx_state = UART_RX_STATE_EOT;
}
break;
case UART_RX_STATE_EOT:
if (EOT == data[rx_index]) {
if (RF4CONTROL_PID == protocol_id) {
/* Message received successfully */
handle_incoming_msg();
}
}
/* Make rx buffer ready for next reception before handling
* received data. */
sio_rx_ptr = sio_rx_buf;
sio_rx_state = UART_RX_STATE_SOT;
break;
default:
sio_rx_ptr = sio_rx_buf;
sio_rx_state = UART_RX_STATE_SOT;
break;
}
}
/**
* @brief ISR for reception
* This is the writer of rx_queue.
*/
uart_recv_interrupt() {
#ifdef SOS_USE_PREEMPTION
HAS_PREEMPTION_SECTION;
DISABLE_PREEMPTION();
#endif
uint8_t err;
uint8_t byte_in;
static uint16_t crc_in;
static uint8_t saved_state;
SOS_MEASUREMENT_IDLE_END()
LED_DBG(LED_YELLOW_TOGGLE);
//! NOTE that the order has to be this in AVR
err = uart_checkError();
byte_in = uart_getByte();
//DEBUG("uart_recv_interrupt... %d %d %d %d %d\n", byte_in, err,
// state[RX].state, state[RX].msg_state, state[RX].hdlc_state);
switch (state[RX].state) {
case UART_IDLE:
if ((err != 0) || (byte_in != HDLC_FLAG)) {
break;
}
state[RX].state = UART_HDLC_START;
break;
case UART_HDLC_START:
case UART_PROTOCOL:
if (err != 0) {
uart_reset_recv();
break;
}
switch (byte_in) {
//! ignore repeated start symbols
case HDLC_FLAG:
state[RX].state = UART_HDLC_START;
break;
case HDLC_SOS_MSG:
if(state[RX].msgHdr == NULL) {
state[RX].msgHdr = msg_create();
} else {
if((state[RX].msgHdr->data != NULL) &&
(flag_msg_release(state[RX].msgHdr->flag))){
ker_free(state[RX].msgHdr->data);
state[RX].msgHdr->flag &= ~SOS_MSG_RELEASE;
}
}
if(state[RX].msgHdr != NULL) {
state[RX].msg_state = SOS_MSG_RX_HDR;
state[RX].crc = crcByte(0, byte_in);
state[RX].flags |= UART_SOS_MSG_FLAG;
state[RX].idx = 0;
state[RX].state = UART_DATA;
state[RX].hdlc_state = HDLC_DATA;
} else {
// need to generate no mem error
uart_reset_recv();
}
break;
case HDLC_RAW:
if ((state[RX].buff = ker_malloc(UART_MAX_MSG_LEN, UART_PID)) != NULL) {
state[RX].msg_state = SOS_MSG_RX_RAW;
if (state[RX].flags & UART_CRC_FLAG) {
state[RX].crc = crcByte(0, byte_in);
}
state[RX].state = UART_DATA;
state[RX].hdlc_state = HDLC_DATA;
} else {
uart_reset_recv();
}
state[RX].idx = 0;
break;
default:
uart_reset_recv();
break;
}
break;
case UART_DATA:
if (err != 0) {
uart_reset_recv();
break;
}
// recieve an escape byte, wait for next byte
if (byte_in == HDLC_CTR_ESC) {
saved_state = state[RX].hdlc_state;
state[RX].hdlc_state = HDLC_ESCAPE;
break;
}
if (byte_in == HDLC_FLAG) { // got an end of message symbol
/*
if (state[RX].msg_state == SOS_MSG_RX_RAW) {
// end of raw recieve
// should bundle and send off
// trash for now
state[RX].hdlc_state = HDLC_IDLE;
state[RX].state = UART_IDLE;
state[RX].flags |= UART_DATA_RDY_FLAG;
uart_read_done(state[RX].idx, 0);
} else {
// got an end of message symbol early
*/
uart_reset_recv();
//}
break;
}
if (state[RX].hdlc_state == HDLC_ESCAPE) {
byte_in ^= 0x20;
state[RX].hdlc_state = saved_state;
}
switch (state[RX].msg_state) {
case SOS_MSG_RX_HDR:
if (byte_in == HDLC_FLAG) { // got an end of message symbol
uart_reset_recv();
break;
}
uint8_t *tmpPtr = (uint8_t*)(state[RX].msgHdr);
tmpPtr[state[RX].idx++] = byte_in;
state[RX].crc = crcByte(state[RX].crc, byte_in);
if (state[RX].idx == SOS_MSG_HEADER_SIZE) {
// if (state[RX].msgLen != state[RX].msgHdr->len) ????????
state[RX].msgLen = state[RX].msgHdr->len;
if (state[RX].msgLen < UART_MAX_MSG_LEN) {
if (state[RX].msgLen != 0) {
state[RX].buff = (uint8_t*)ker_malloc(state[RX].msgLen, UART_PID);
if (state[RX].buff != NULL) {
state[RX].msgHdr->data = state[RX].buff;
state[RX].msgHdr->flag = SOS_MSG_RELEASE;
state[RX].msg_state = SOS_MSG_RX_DATA;
state[RX].idx = 0;
} else {
uart_reset_recv();
}
} else { // 0 length packet go straight to crc
state[RX].msgHdr->flag &= ~SOS_MSG_RELEASE;
state[RX].msgHdr->data = NULL;
state[RX].msg_state = SOS_MSG_RX_CRC_LOW;
}
} else { // invalid msg length
uart_reset_recv();
}
}
break;
case SOS_MSG_RX_RAW:
case SOS_MSG_RX_DATA:
if (err != 0) {
uart_reset_recv();
return;
}
state[RX].buff[state[RX].idx++] = byte_in;
if (state[RX].flags & UART_CRC_FLAG) {
state[RX].crc = crcByte(state[RX].crc, byte_in);
}
if (state[RX].idx == state[RX].msgLen) {
if (state[RX].flags & UART_SOS_MSG_FLAG) {
state[RX].hdlc_state = HDLC_CRC;
state[RX].msg_state = SOS_MSG_RX_CRC_LOW;
} else {
// rx buffer overflow
uart_reset_recv();
}
}
break;
case SOS_MSG_RX_CRC_LOW:
crc_in = byte_in;
state[RX].msg_state = SOS_MSG_RX_CRC_HIGH;
break;
case SOS_MSG_RX_CRC_HIGH:
crc_in |= ((uint16_t)(byte_in) << 8);
state[RX].hdlc_state = HDLC_PADDING;
state[RX].msg_state = SOS_MSG_RX_END;
state[RX].state = UART_HDLC_STOP;
break;
case SOS_MSG_RX_END: // should never get here
default:
uart_reset_recv();
break;
}
break;
case UART_HDLC_STOP:
if (byte_in != HDLC_FLAG) {
// silently drop until hdlc stop symbol
break;
} else { // sos msg rx done
state[RX].hdlc_state = HDLC_IDLE;
if(crc_in == state[RX].crc) {
#ifndef NO_SOS_UART_MGR
set_uart_address(entohs(state[RX].msgHdr->saddr));
#endif
if(state[RX].msgHdr->type == MSG_TIMESTAMP){
uint32_t timestp = ker_systime32();
memcpy(((uint8_t*)(state[RX].msgHdr->data) + sizeof(uint32_t)),(uint8_t*)(×tp),sizeof(uint32_t));
}
handle_incoming_msg(state[RX].msgHdr, SOS_MSG_UART_IO);
state[RX].msgHdr = NULL;
} else {
msg_dispose(state[RX].msgHdr);
state[RX].msgHdr = NULL;
}
state[RX].state = UART_IDLE;
state[RX].msg_state = SOS_MSG_NO_STATE;
state[RX].hdlc_state = HDLC_IDLE;
//uart_reset_recv();
//state[RX].msg_state = SOS_MSG_NO_STATE;
//state[RX].state = UART_HDLC_START;
}
break;
// XXX fall through
default:
uart_reset_recv();
break;
} // state[RX].state
#ifdef SOS_USE_PREEMPTION
// enable interrupts because
// enabling preemption can cause one to occur
ENABLE_GLOBAL_INTERRUPTS();
// enable preemption
ENABLE_PREEMPTION(NULL);
#endif
}
/**
* Send MSG_I2C_READ_DONE
*/
void i2c_read_done(uint8_t *buff, uint8_t len, uint8_t status) {
uint8_t *bufPtr = NULL;
Message *msgPtr = NULL;
// this is a problem that should be handled
if (buff == NULL) {
return;
}
if ((i2c_sys.calling_mod_id != NULL_PID) && (status & I2C_SYS_ERROR_FLAG)) {
goto post_error_msg;
}
// the bus was reserved the read was of the length we requested
if ((i2c_sys.calling_mod_id != NULL_PID) && (len == i2c_sys.rxLen) &&
// the data was recieved in the correct mode
(((i2c_sys.state == I2C_SYS_MASTER_RX) && (I2C_SYS_MASTER_FLAG & status)) ||
((i2c_sys.state == I2C_SYS_SLAVE_RX) && !(I2C_SYS_MASTER_FLAG & status)))) {
// reserved read done will only be raw reads, wrap in message and send
post_long(
i2c_sys.calling_mod_id,
I2C_PID,
MSG_I2C_READ_DONE,
len,
buff,
SOS_MSG_RELEASE|SOS_MSG_HIGH_PRIORITY);
i2c_sys.state = (i2c_sys.flags & I2C_SYS_MASTER_FLAG)?I2C_SYS_MASTER_WAIT:I2C_SYS_SLAVE_WAIT;
return;
}
// there appers to be a bug in avr-gcc this a work around to make sure
// the cast to message works correctly
// the following code seems to cat the value 0x800008 independent of what
// buff actually ii2c_sys. this has no correlation to the data in buff or the
// value of the pointer the cast (along with many others that should) works
// in gdb but fail to execute correctly when compilied
/* bufPtr = &buff[HDLC_PROTOCOL_SIZE]; */
// DO NOT CHANGE THIS SECTION OF CODE
// start of section
bufPtr = buff+1;
// end of DO NOT CHANGE SECTION
// if it passes sanity checks give it to the scheduler
if (!(I2C_SYS_MASTER_FLAG & status) && (len >= SOS_MSG_HEADER_SIZE) && (buff[0] == HDLC_SOS_MSG)) {
if ((len >= (HDLC_PROTOCOL_SIZE + SOS_MSG_HEADER_SIZE + ((Message*)bufPtr)->len + SOS_MSG_CRC_SIZE)) &&
(len <= I2C_MAX_MSG_LEN)) {
// please do not edit the next line
bufPtr = buff;
// we have enough bytes for it to be a message, lets start the copy out
// XXX msgPtr = (Message*)ker_malloc(sizeof(Message), I2C_PID);
msgPtr = msg_create();
if (msgPtr !=NULL) {
uint8_t i=0;
uint16_t runningCRC=0, crc_in=0;
// extract the protocol field
for (i=0; i<HDLC_PROTOCOL_SIZE; i++) {
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
// extract the header
bufPtr = &buff[HDLC_PROTOCOL_SIZE];
for (i=0; i<SOS_MSG_HEADER_SIZE; i++) {
((uint8_t*)msgPtr)[i] = bufPtr[i];
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
// extract the data if it exists
if (msgPtr->len != 0) {
uint8_t *dataPtr;
dataPtr = ker_malloc(((Message*)msgPtr)->len, I2C_PID);
if (dataPtr != NULL) {
msgPtr->data = dataPtr;
bufPtr = &buff[HDLC_PROTOCOL_SIZE+SOS_MSG_HEADER_SIZE];
for (i=0; i<msgPtr->len; i++) {
msgPtr->data[i] = bufPtr[i];
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
} else { // -ENOMEM
ker_free(msgPtr);
goto post_error_msg;
}
} else {
msgPtr->data = NULL;
}
// get the CRC and check it
bufPtr = &buff[HDLC_PROTOCOL_SIZE+SOS_MSG_HEADER_SIZE+msgPtr->len];
crc_in = bufPtr[0] | (bufPtr[1]<<8);
if (crc_in == runningCRC) {
// message passed all sanity checks including crc
LED_DBG(LED_YELLOW_TOGGLE);
if(msgPtr->data != NULL ) {
msgPtr->flag = SOS_MSG_RELEASE;
}
handle_incoming_msg(msgPtr, SOS_MSG_I2C_IO);
return;
} else { // clean up
ker_free(msgPtr->data);
ker_free(msgPtr);
}
}
}
}
// if we make it to here return error message
post_error_msg:
post_short(
i2c_sys.calling_mod_id,
I2C_PID,
MSG_ERROR,
READ_ERROR,
0,
SOS_MSG_HIGH_PRIORITY);
}
/**
* @brief receiver thread
*/
static void recv_thread(int fd)
{
DEBUG("radio: receiver start\n");
while(1)
{
uint8_t read_buf[SEND_BUF_SIZE];
Message *recv_msg = NULL;
int cnt;
//sched_yield();
DEBUG("Radio: Receiver Idle ... \n");
cnt = read(topo_self.sock, read_buf, SEND_BUF_SIZE);
// XXX Using recvfrom will cause problem on Cygwin
//cnt = recvfrom(topo_self.sock, read_buf, SEND_BUF_SIZE, 0, &fromaddr, &fromaddrlen);
DEBUG("Radio: Read %d bytes\n", cnt);
if(cnt < 0 && cnt != -1) {
DEBUG("Radio: unable to read, exiting...\n");
exit(1);
}
if( cnt == 0 || cnt == -1) {
return;
}
if(cnt < SOS_MSG_HEADER_SIZE) {
DEBUG("Radio: get incomplete header\n");
//! something is wrong...
return;
}
if((((Message*)read_buf)->len) != (cnt - SOS_MSG_HEADER_SIZE)) {
DEBUG("Radio: invalid data payload size\n");
return;
}
recv_msg = msg_create();
if(recv_msg == NULL) {
DEBUG("Radio: no message header\n");
return;
}
memcpy(recv_msg, read_buf, SOS_MSG_HEADER_SIZE);
if(recv_msg->len != 0) {
recv_msg->data = ker_malloc(recv_msg->len, RADIO_PID);
if(recv_msg->data != NULL) {
recv_msg->flag = SOS_MSG_RELEASE;
memcpy(recv_msg->data, read_buf + SOS_MSG_HEADER_SIZE, recv_msg->len);
if(recv_msg->type == MSG_TIMESTAMP)
{
uint32_t timestamp = ker_systime32();
memcpy(&recv_msg->data[4], (uint8_t *)(×tamp), sizeof(uint32_t));
}
if(bTsEnable) {
timestamp_incoming(recv_msg, ker_systime32());
}
DEBUG("handle incoming msg \n");
handle_incoming_msg(recv_msg, SOS_MSG_RADIO_IO);
} else {
msg_dispose(recv_msg);
}
} else {
recv_msg->data = NULL;
recv_msg->flag = 0;
if(bTsEnable) {
timestamp_incoming(recv_msg, ker_systime32());
}
handle_incoming_msg(recv_msg, SOS_MSG_RADIO_IO);
}
}
}
