/**
* Main cUDP buffer handler.
*
* \param buf pointer to buffer
* \return pdTRUE OK
*/
portCHAR cudp_handle( buffer_t *buf )
{
/* Process the packet */
uint8_t hc_udp,tmp_8, header_length=0;
uint8_t *dptr;
uint8_t portfield=0;
control_message_t *ptr;
uint16_t length=0;
#ifndef NO_FCS
uint16_t fcs;
#endif
debug("cUDP: handler.\r\n");
switch(buf->dir)
{
case BUFFER_DOWN:
debug("down: ");
debug("\r\n ptr: ");
debug_int(buf->buf_ptr);
debug(" end: ");
debug_int(buf->buf_end);
debug("\r\n");
/* Check data payload length */
udp_data_length = (buf->buf_end - buf->buf_ptr);
/* Checksum calculate*/
udp_data_length +=8;
if(stack_buffer_headroom( buf,8)==pdFALSE)
{
ptr = ( control_message_t*) buf->buf;
ptr->message.ip_control.message_id = TOO_LONG_PACKET;
push_to_app(buf);
return pdTRUE;
}
buf->buf_ptr -= 8; /* HC2, SPORT,DPORT and Check Sum */
dptr = buf->buf + buf->buf_ptr;
*dptr++ = (buf->src_sa.port >> 8);
*dptr++ = (uint8_t) buf->src_sa.port;
*dptr++ = (buf->dst_sa.port >> 8);
*dptr++ = (uint8_t)buf->dst_sa.port;
*dptr++ = (udp_data_length >> 8);
*dptr++ = (uint8_t) udp_data_length;
*dptr++ = 0x00;
*dptr++ = 0x00;
#ifndef NO_FCS
fcs = ipv6_fcf(buf,NEXT_HEADER_UDP, 0);
#endif
if( use_compress==0)
{
dptr = buf->buf + (buf->buf_ptr + 6);
}
if ( use_compress )
{
portfield = 0;
tmp_8=2;/* FCS field */
hc_udp=LENGTH_COMPRESSED;
/* Check port that these are supported 6LoWPAN ports 61616 - 61631 */
if((buf->src_sa.port > HC2_PORT_ENCODE && buf->src_sa.port < HC2_ENCODE_MAX_ENCODE) && (buf->dst_sa.port > HC2_PORT_ENCODE && buf->dst_sa.port < HC2_ENCODE_MAX_ENCODE))
{
hc_udp |= S_PORT_COMPRESSED;
portfield = (buf->src_sa.port - HC2_ENCODE_P_VALUE);
hc_udp |= D_PORT_COMPRESSED;
portfield |= ((buf->dst_sa.port - HC2_ENCODE_P_VALUE) << 4);
tmp_8++;
}
else
{
tmp_8 +=4;
}
buf->buf_ptr +=(8 - tmp_8);
dptr = buf->buf + buf->buf_ptr;
buf->options.lowpan_compressed = hc_udp;
if(tmp_8 == 3)
{
*dptr++ = portfield; /* Encoded Source and Destination-port*/
}
else
{
*dptr++ = (buf->src_sa.port >> 8);
*dptr++ = (uint8_t) buf->src_sa.port;
*dptr++ = (buf->dst_sa.port >> 8);
*dptr++ = (uint8_t)buf->dst_sa.port;
}
}
#ifndef NO_FCS
/* FCS */
*dptr++ = (fcs >> 8);
*dptr++ = (uint8_t)fcs;
#else
*dptr++ = 0;
*dptr++ = 0;
#endif
/*Check sum will be soon real, but for beta testing it is always TRUE */
buf->src_sa.addr_type = ADDR_NONE;
buf->from = MODULE_CUDP;
buf->to = MODULE_NONE;
stack_buffer_push(buf);
buf=0;
break;
case BUFFER_UP:
/* Check data payload length */
udp_data_length = (buf->buf_end - buf->buf_ptr);
length=0;
dptr = buf->buf + buf->buf_ptr;
/* Lets check HC_UDP compress options */
switch (buf->options.lowpan_compressed)
{
/*case UNCOMPRESSED_HC_UDP:
// Read Src-port
udp_port = *dptr++;
udp_port <<= 8;
udp_port += *dptr++;
buf->src_sa.port=udp_port;
udp_port = *dptr++;
udp_port <<= 8;
udp_port += *dptr++;
buf->dst_sa.port=udp_port;
length = *dptr++;
length <<= 8;
length += *dptr++;
header_length=8;
udp_data_length = ((buf->buf_end - buf->buf_ptr) - header_length);
break;
*/
case COMPRESSED_HC_UDP:
header_length = 3;
/* Decode Source and Destination port-number */
portfield = *dptr++;
/* Source port */
buf->src_sa.port = ((portfield & S_PORTBM) + HC2_ENCODE_P_VALUE );
/* Destination Port */
buf->dst_sa.port = ((portfield >> 4) + HC2_ENCODE_P_VALUE);
udp_data_length = ((buf->buf_end - buf->buf_ptr) - header_length);
length =(udp_data_length + 8);
break;
case UNCOMPRESSED_HC_UDP:
case LENGTH_COMPRESSED_HC_UDP:
header_length = 6;
/* Read Src-port */
udp_port = *dptr++;
udp_port <<= 8;
udp_port += *dptr++;
buf->src_sa.port=udp_port;
udp_port = *dptr++;
udp_port <<= 8;
udp_port += *dptr++;
buf->dst_sa.port=udp_port;
if(buf->options.lowpan_compressed == UNCOMPRESSED_HC_UDP)
{
length = *dptr++;
length <<= 8;
length += *dptr++;
header_length += 2;
udp_data_length = ((buf->buf_end - buf->buf_ptr) - header_length);
}
else
{
udp_data_length = ((buf->buf_end - buf->buf_ptr) - header_length);
length =(udp_data_length + 8);
}
break;
}
#ifndef NO_FCS
/*fcs*/
fcs=0;
fcs = *dptr++;
fcs <<= 8;
fcs += *dptr++;
#else
dptr += 2;
#endif /*NO_FCS*/
buf-> buf_ptr = (dptr - buf->buf);
if(buf->options.handle_type == HANDLE_NO_ROUTE_TO_DEST)
{
debug("UDP: no del info\r\n");
udp_port = buf->dst_sa.port;
buf->dst_sa.port = buf->src_sa.port;
buf->src_sa.port = udp_port;
buf->from = MODULE_CUDP;
buf->to = MODULE_NONE;
push_to_app(buf);
buf=0;
return pdTRUE;
}
else
{
#ifndef NO_FCS
if(fcs!=0x0000)
#endif
{
/* Calculate and check fcs */
buf->buf_ptr -= 8; /* HC2, SPORT,DPORT and Check Sum */
dptr = buf->buf + buf->buf_ptr;
*dptr++ = (buf->src_sa.port >> 8);
*dptr++ = (uint8_t) buf->src_sa.port;
*dptr++ = (buf->dst_sa.port >> 8);
*dptr++ = (uint8_t)buf->dst_sa.port;
*dptr++ = (length >> 8);
*dptr++ = (uint8_t) length;
#ifndef NO_FCS
*dptr++ = (fcs >> 8);
*dptr++ = (uint8_t)fcs;
if (ipv6_fcf(buf,NEXT_HEADER_UDP, 1) != 0xFFFF)
{
debug("UDP:FCS error\r\n");
stack_buffer_free(buf);
buf=0;
}
#else /*NO_FCS*/
*dptr++ = 0;
*dptr++ = 0;
#endif /*NO_FCS*/
buf->buf_ptr += 8; /* HC2, SPORT,DPORT and Check Sum */
dptr = buf->buf + buf->buf_ptr;
}
#ifndef NO_FCS
else
{
debug("FCS disable\r\n");
}
#endif
#ifndef HAVE_NRP
if (buf && (buf->dst_sa.port == NPING_PORT || buf->dst_sa.port == UDP_ECHO_PORT) )
{ /*Ping*/
if ( ( (buf->src_sa.port == NPING_PORT) && (buf->dst_sa.port == NPING_PORT) )
|| ( (buf->src_sa.port == UDP_ECHO_PORT) && (buf->dst_sa.port == UDP_ECHO_PORT) ) )
{ /*Evil loop ping*/
stack_buffer_free(buf);
buf=0;
}
if (buf)
{ /*respond*/
#ifndef NO_FCS
uint16_t tmp_fcs;
#endif
debug("cUDP: ping ->respond.\r\n");
/* Change Destination and Source-port */
udp_port = buf->dst_sa.port;
buf->dst_sa.port = buf->src_sa.port;
buf->src_sa.port = udp_port;
buf->dst_sa.addr_type = buf->src_sa.addr_type;
buf->src_sa.addr_type = ADDR_NONE;
memcpy(buf->dst_sa.address, buf->src_sa.address, 8);
#ifndef NO_FCS
buf->buf_ptr -= 8; /* HC2, SPORT,DPORT and Check Sum */
dptr = buf->buf + buf->buf_ptr;
*dptr++ = (buf->src_sa.port >> 8);
*dptr++ = (uint8_t) buf->src_sa.port;
*dptr++ = (buf->dst_sa.port >> 8);
*dptr++ = (uint8_t)buf->dst_sa.port;
*dptr++ = (length >> 8);
*dptr++ = (uint8_t) length;
*dptr++ = 0x00;
*dptr++ = 0x00;
tmp_fcs = ipv6_fcf(buf,NEXT_HEADER_UDP, 0);
buf->buf_ptr += 8;
#endif
/* Create header */
if(use_compress)
{
hc_udp=LENGTH_COMPRESSED;
tmp_8=2;
//if(buf->src_sa.port > HC2_PORT_ENCODE && buf->dst_sa.port > HC2_PORT_ENCODE)
if((buf->src_sa.port > HC2_PORT_ENCODE && buf->src_sa.port < HC2_ENCODE_MAX_ENCODE) && (buf->dst_sa.port > HC2_PORT_ENCODE && buf->dst_sa.port < HC2_ENCODE_MAX_ENCODE))
{
hc_udp |= S_PORT_COMPRESSED;
portfield = (buf->src_sa.port - HC2_ENCODE_P_VALUE);
hc_udp |= D_PORT_COMPRESSED;
portfield |= ((buf->src_sa.port - HC2_ENCODE_P_VALUE) << 4);
tmp_8++;
}
else
tmp_8 +=4;
}
else
tmp_8=8;
buf->buf_ptr -= tmp_8;
dptr = buf->buf + buf->buf_ptr;
switch (tmp_8)
{
case 6:
buf->options.lowpan_compressed = LENGTH_COMPRESSED_HC_UDP;
*dptr++ = (buf->src_sa.port >> 8);
*dptr++ = (uint8_t) buf->src_sa.port;
*dptr++ = (buf->dst_sa.port >> 8);
*dptr++ = (uint8_t)buf->dst_sa.port;
break;
case 3:
buf->options.lowpan_compressed = COMPRESSED_HC_UDP;
*dptr++ = portfield;
break;
case 8:
buf->options.lowpan_compressed = 0;
*dptr++ = (buf->src_sa.port >> 8);
*dptr++ = (uint8_t) buf->src_sa.port;
*dptr++ = (buf->dst_sa.port >> 8);
*dptr++ = (uint8_t)buf->dst_sa.port;
*dptr++ = (length >> 8);
*dptr++ = (uint8_t) length;
break;
}
#ifndef NO_FCS
/* Add FCS */
*dptr++ = (tmp_fcs >> 8);
*dptr++ = (uint8_t)tmp_fcs;
#else
*dptr++ = 0;
*dptr++ = 0;
#endif
buf->socket = 0;
buf->from = MODULE_CUDP;
buf->dir = BUFFER_DOWN;
buf->to = MODULE_NONE;
stack_buffer_push(buf);
buf=0;
}
}
#endif /*HAVE_NRP*/
}
/**
*
* \Function stack_msg_sendto
*
* \Brief sends a message
*
* \Params dst: destination
* \Params data: data to send
* \Params msg_length
* \Params add_trace_route
* \Params debugger_msg : it is a debugger message (special treatment required)
*
* \Note normal mode is both parameters at pdFALSE
*
*/
portCHAR stack_msg_sendto (long_addr_t *dst, char *data, uint8_t msg_length, portCHAR add_trace_route, portCHAR debugger_msg)
{
// Useless and should stay commented for router without sync
/* if all is ok to send a message */
if (buffer_left >= STACK_MIN_AVAILABLE_BUFFERS)
{
portCHAR retvalue = pdFALSE;
buffer_t *buf = 0;
uint8_t i;
/**************************************/
/* Get a buffer and fill it with data */
/**************************************/
if ((buf=stack_buffer_get(20)))
{
buf->from = MODULE_APP;
buf->to = MODULE_NONE;
buf->dir = BUFFER_DOWN;
memcpy(&(buf->src_addr), &(lgaddr_mac), sizeof(long_addr_t));
}
else
{
debug_printf("E ! S: mm: %d, bl:%d\r\n", mac_mode, buffer_left);
retvalue = pdFALSE;
return retvalue;
}
if (buf)
{
buf->buf_end=0;
buf->buf_ptr=0;
for( i = 0 ; i < msg_length ; i++)
{
buf->buf[buf->buf_end++] = data[i];
//debug_printf("%d ", data[i]);
}
}
/************************************/
/* Add addresses and buffer options */
/************************************/
if (buf->src_addr.addr_type == ADDR_NONE)
{
memcpy(&(buf->src_addr), &(lgaddr_mac), sizeof(long_addr_t));
}
buf->from = MODULE_APP;
buf->dir = BUFFER_DOWN;
buf->to = MODULE_NONE;
// address_utils_debug_address(dst, ADDR_FULL);
if (dst)
{
retvalue = pdTRUE;
memcpy(&(buf->dst_addr), dst, sizeof(long_addr_t));
}
else
{
debug("Sendto: dst = 0\r\n");
retvalue = pdFALSE; /*No destination*/
}
/* ******* */
/* ADD TTL */
/* ******* */
buf->options.ttl_nb = 0;
/***************/
/* Send buffer */
/***************/
if (retvalue == pdTRUE)
{
// test if dest address has an initialized TL: if no, look at neighbor table if the UID is present
// If uid is present in neighboring table, affect correspondent TL
// if we want to use routing table
debug("nwk handle \r\n");
retvalue = smart_nwk_handle(buf);
return retvalue;
}
else
{
if (buf != 0) {stack_buffer_free(buf);}
return pdFALSE;
}
}
else
{
debug_printf("S: %d buf left\n", buffer_left);
return pdFALSE;
}
}
/**
* Main ICMP buffer handler.
*
* \param buf pointer to buffer
* \return pdTRUE OK
*/
portCHAR icmp_handle( buffer_t *buf )
{
uint8_t *dptr;
control_message_t *ptr;
uint8_t icmp_type=0, icmp_code=0;
#ifdef HAVE_ROUTING
uint8_t i=0;
#endif
debug("ICMP: handler.\r\n");
if(buf->options.type == BUFFER_CONTROL && buf) /* Control message received */
{ /* Control-message */
ptr = ( control_message_t*) buf->buf;
switch(ptr->message.ip_control.message_id)
{
#ifdef HAVE_ROUTING
case BROKEN_LINK:
debug("Control: Broken link mes create & send\r\n");
if(ptr->message.ip_control.message.broken_link_detect.reason == BROKEN_LINK_TYPE)
i = ERROR_CODE_BROKEN_LINK;
if(ptr->message.ip_control.message.broken_link_detect.reason == NO_ROUTE_TO_DESTINATION_TYPE)
i = ERROR_CODE_NO_ROUTE;
memset(buf->buf, 0, 24);
buf->buf_ptr = 0;
buf->buf_end = 24;
dptr = buf->buf;
*dptr++ = ICMP_ERROR_MESSAGE_TYPE;
*dptr++ = i;
dptr += 6;
/* Error packet destination address */
*dptr++ = 0xfe;
*dptr++ = 0x80;
dptr += 6;
if(buf->dst_sa.addr_type == ADDR_802_15_4_PAN_LONG)
{
for(i=0;i<8;i++)
{
*dptr++ = buf->dst_sa.address[7-i];
}
}
else
{
dptr += 3;
*dptr++ = 0xff;
*dptr++ = 0xfe;
dptr += 1;
*dptr++ = buf->dst_sa.address[1];
*dptr++ = buf->dst_sa.address[0];
}
buf->dst_sa.addr_type = buf->src_sa.addr_type;
buf->src_sa.addr_type = ADDR_NONE;
memcpy(buf->dst_sa.address, buf->src_sa.address, 8);
break;
#endif
default:
stack_buffer_free(buf);
buf=0;
break;
}
}
else
{
switch(buf->dir)
{
case BUFFER_DOWN:
stack_buffer_free(buf);
buf=0;
break;
case BUFFER_UP:
debug("IC:UP\r\n");
dptr = (buf->buf + buf->buf_ptr) +2;
icmp_temp_16 = *dptr++;
icmp_temp_16 += ((uint16_t) (*dptr++) << 8);
#ifndef NO_FCS
/* Check FCS first */
if(icmp_temp_16 && (ipv6_fcf(buf, NEXT_HEADER_ICMP6, 1) != 0xffff))
{
debug("ICMP: FCS fail\r\n");
stack_buffer_free(buf);
buf=0;
}
else
#endif
#ifdef HAVE_NRP
{
dptr = (buf->buf + buf->buf_ptr);
icmp_type = *dptr++;
icmp_code = *dptr++;
#ifdef AUTO_GW_RESPONSE
if(buf && icmp_type==ROUTER_SOLICICATION_TYPE)
{
buf->dst_sa.addr_type = buf->src_sa.addr_type;
buf->src_sa.addr_type = ADDR_NONE;
memcpy(buf->dst_sa.address, buf->src_sa.address, 8);
memset(buf->buf, 0, 16);
buf->buf_ptr = 0;
buf->buf_end = 16;
dptr = buf->buf + buf->buf_ptr;
*dptr++ = ROUTER_ADVRT_TYPE;
*dptr++ = ICMP_CODE;
*dptr++ = 0x00;
*dptr++ = 0x00;
*dptr++ = GENERAL_HOPLIMIT;
add_fcf(buf);
stack_buffer_push(buf);
return pdTRUE;
}
else
#endif
{
buf->from = MODULE_ICMP;
buf->to = MODULE_NRP;
stack_buffer_push(buf);
return pdTRUE;
}
}
#else
{
dptr = (buf->buf + buf->buf_ptr);
icmp_type = *dptr++;
icmp_code = *dptr++;
}
if(icmp_type != ICMP_ERROR_MESSAGE_TYPE && icmp_code != 0)
{
stack_buffer_free(buf);
buf=0;
return pdTRUE;
}
if(buf && icmp_type==ECHO_RESPONSE_TYPE)
{
dptr += 4;
buf->buf_ptr = (dptr - buf->buf);
buf->from = MODULE_ICMP;
parse_echo_response(buf);
buf=0;
}
else if(buf && icmp_type==ECHO_REQUEST_TYPE)
{
#ifndef HAVE_NRP
/* Copy source -> Destination field */
buf->dst_sa.addr_type = buf->src_sa.addr_type;
buf->src_sa.addr_type = ADDR_NONE;
memcpy(buf->dst_sa.address, buf->src_sa.address, 8);
dptr = buf->buf + buf->buf_ptr;
*dptr++ = ECHO_RESPONSE_TYPE;
*dptr++ = ICMP_CODE;
*dptr++ = 0x00;
*dptr++ = 0x00;
#else
ptr = ( control_message_t*) buf->buf;
ptr->message.ip_control.message_id = ECHO_REQ;
push_to_app(buf);
buf = 0;
#endif
}
else if(buf && icmp_type==ROUTER_ADVRT_TYPE)
{
//taskENTER_CRITICAL();
//gw_table_update(buf);
//taskEXIT_CRITICAL();
buf->buf_end = 0;
buf->buf_ptr = 0;
ptr = ( control_message_t*) buf->buf;
ptr->message.ip_control.message_id = ROUTER_DISCOVER_RESPONSE;
push_to_app(buf);
buf=0;
}
#ifdef HAVE_ROUTING
else if(buf && icmp_type == ICMP_ERROR_MESSAGE_TYPE)
{
if(icmp_code == ERROR_CODE_BROKEN_LINK || icmp_code == ERROR_CODE_NO_ROUTE)
{
dptr += 6;
for(i=0; i<16; i++)
{
ipv6_address_tmp[i] = *dptr++;
}
if(ipv6_address_tmp[0] == 0xfe && ipv6_address_tmp[1] == 0x80)
{
if((ipv6_address_tmp[12]== 0xfe && ipv6_address_tmp[11] == 0xff) && ipv6_address_tmp[13] == 0x00)
{
buf->dst_sa.addr_type=ADDR_802_15_4_PAN_SHORT;
for(i=0; i<2; i++)
{
buf->dst_sa.address[i] = ipv6_address_tmp[14+i];
}
}
else
{
buf->dst_sa.addr_type=ADDR_802_15_4_PAN_LONG;
for(i=0; i<8; i++)
{
buf->dst_sa.address[7-i] = ipv6_address_tmp[8+i];
}
}
}
memcpy(&(namomesh_info.adr),&(buf->dst_sa), sizeof(sockaddr_t) );
namomesh_info.lqi = 0;
namomesh_info.event = ROUTE_ERR;
update_routing_table(&namomesh_info);
ptr = ( control_message_t*) buf->buf;
ptr->message.ip_control.message_id = BROKEN_LINK;
push_to_app(buf);
//buf = 0;
return pdTRUE;
}
if(buf)
{
stack_buffer_free(buf);
buf=0;
return pdTRUE;
}
}
#endif /*HAVE_ROUTING*/
else if(buf && icmp_type==ROUTER_SOLICICATION_TYPE)
{
if(gateway_features)
{
buf->dst_sa.addr_type = buf->src_sa.addr_type;
buf->src_sa.addr_type = ADDR_NONE;
memcpy(buf->dst_sa.address, buf->src_sa.address, 8);
memset(buf->buf, 0, 16);
buf->buf_ptr = 0;
buf->buf_end = 16;
dptr = buf->buf + buf->buf_ptr;
*dptr++ = ROUTER_ADVRT_TYPE;
*dptr++ = ICMP_CODE;
*dptr++ = 0x00;
*dptr++ = 0x00;
*dptr++ = GENERAL_HOPLIMIT;
}
else
{
stack_buffer_free(buf);
buf=0;
return pdTRUE;
}
}
else
{
stack_buffer_free(buf);
buf=0;
}
#endif /*HAVE_NRP*/
break;
}
}
/* ICMP message build and forward */
if(buf)
{
add_fcf(buf);
stack_buffer_push(buf);
buf=0;
}
return pdTRUE;
}
/**
* \Function cDemo
* \Brief Some Demo
*/
static void vDemo( void *pvParameters )
{
// I am Node 1
uint8_t trial = 0;
uint8_t main_frame_type;
uint8_t src_detection_state;
main_event_t curr_event;
long_addr_t dst_addr= {{{0xff, 0, 0, 0}}, {0, 0xff}, {2, 2},{PAN_ID_LOW, PAN_ID_HIGH}, ADDR_SMG} ;
uint8_t msg[8];
uint8_t msg_length = 8;
msg[0] = MURPHY_MSG_TYPE;
msg[1] = msg_length;
msg[2] = 0x0a;
msg[3] = 0x0b;
clock_pause(1500);
// fill the msg as you wish !
while(1)
{
if(xQueueReceive(queue_app, &(curr_event), 3000 / portTICK_RATE_MS) == pdTRUE)
{
debug("rcv\r\n");
switch (curr_event.main_event_type)
{
case MAIN_BUFFER_UP:
main_frame_type = curr_event.pbuf->buf[curr_event.pbuf->buf_ptr++];
switch(main_frame_type)
{
case MURPHY_MSG_TYPE:
src_detection_state = curr_event.pbuf->buf[curr_event.pbuf->buf_ptr++];
if(src_detection_state == 0)
{
debug_printf("I have received an Empty MSG from :\r\n");
LED1_ON_CHECK_MODE(led_mode); LED0_OFF_CHECK_MODE(led_mode);
}
else
{
debug_printf("I have received a Full MSG from :\r\n");
LED0_ON_CHECK_MODE(led_mode); LED1_OFF_CHECK_MODE(led_mode);
}
address_utils_debug_address(&curr_event.pbuf->src_addr, ADDR_FULL);
break;
default:
break;
}
stack_buffer_free(curr_event.pbuf);
curr_event.pbuf = 0;
break;
case MAIN_BUFFER_DOWN:
/* ********************************************** */
/* send message to nwk, if main_running is pdTRUE */
/* ********************************************** */
smart_nwk_handle(curr_event.pbuf);
break;
default:
stack_buffer_free(curr_event.pbuf);
curr_event.pbuf = 0;
break;
clock_pause(500);
LEDS_OFF_CHECK_MODE(led_mode);
}
}
}
}
