void gps_dgps_message() {
#if 0
unsigned int dgps_source = gps_payload[1];
debug("DGPS source: "); debug_int(dgps_source); debug(", PRN:");
for (int i = 16; i < 52 ; i += 3) {
debug(" ");
debug_int((int)(gps_payload[i]));
}
debug("\n");
#endif
}
void gps_geodetic_message() {
unsigned int year = gps_payload[11] << 8 | gps_payload[12];
unsigned int month = gps_payload[13];
unsigned int day = gps_payload[14];
unsigned int hour = gps_payload[15];
unsigned int minute = gps_payload[16];
unsigned int rawsecond = (gps_payload[17] << 8 | gps_payload[18]);
unsigned int second = rawsecond / 1000;
unsigned int millis = rawsecond % 1000;
unsigned int gps_week = gps_payload[5] << 8 | gps_payload[6];
unsigned int gps_tow = (unsigned int)gps_payload[7] << 24
| (unsigned int)gps_payload[8] << 16
| (unsigned int)gps_payload[9] << 8
| (unsigned int)gps_payload[10];
unsigned int gps_tow_sec = gps_tow / 1000L;
unsigned int numsvs = gps_payload[88];
debug_int(year); debug("-"); debug_int(month); debug("-"); debug_int(day);
debug(" ");
debug_int(hour); debug(":"); debug_int(minute); debug(":");
debug_int(second); debug("."); debug_int(millis);
debug(", ");
debug_int(numsvs);
debug(" SVs\r\n");
int utc_offset = gps_utc_offset(hour, minute, second, gps_tow_sec);
time_set_date(gps_week, gps_tow_sec, utc_offset);
health_set_gps_status(GPS_OK);
health_reset_gps_watchdog();
}
void gps_navdata_message() {
#if 0
unsigned int mode1 = gps_payload[19];
unsigned int mode2 = gps_payload[21];
unsigned int svs = gps_payload[28];
unsigned int pmode = mode1 & 0x7;
debug("DGPS: ");
if (mode1 & 1<<7)
debug("OK");
else
debug("NO");
debug(" DOP: ");
if (mode1 & 1<<6)
debug("NO");
else
debug("OK");
debug(" VAL: ");
if (mode2 & 1<<1)
debug("OK");
else
debug("NO");
debug(" PMODE: ");
debug_int(pmode);
debug("\n");
#endif
}
void gps_satvisible_message() {
#if 0
unsigned short num_visible = gps_payload[1];
debug("Visible SVs:");
for (int i = 0 ; i < num_visible; i++) {
debug(" ");
debug_int((int)(gps_payload[2 + 5 * i]));
}
debug("\n");
#endif
}
void gps_handle_message() {
unsigned char message_type = gps_payload[0];
switch(message_type) {
case 2:
gps_navdata_message();
break;
case 4:
gps_tracking_data_message();
break;
case 7:
gps_clockstatus_message();
break;
case 11:
gps_ack_message();
break;
case 12:
gps_nak_message();
break;
case 13:
gps_satvisible_message();
break;
case 27:
gps_dgps_message();
break;
case 41:
gps_geodetic_message();
break;
case 9:
break;
default:
debug("Got "); debug_int(gps_payload_len);
debug(" byte message, type "); debug_int((int)message_type);
debug("\r\n");
break;
}
}
void gps_tracking_data_message() {
#if 0
unsigned char chans = gps_payload[7];
unsigned char ch;
unsigned char first = 1;
debug("SVs in view: ");
for (ch = 0 ; ch < chans ; ch++) {
unsigned char svid = gps_payload[8 + ch * 15];
if (svid != 0) {
unsigned char raw_az = gps_payload[9 + ch * 15];
unsigned char raw_el = gps_payload[10 + ch * 15];
unsigned int state = (unsigned int) gps_payload[11 + ch * 15] << 8
| (unsigned int) gps_payload[12 + ch * 15];
float az = raw_az * 3.0 / 2.0;
float el = raw_el / 2.0;
float snr_avg = 0;
unsigned char meas;
for (meas = 0 ; meas < 2 ; meas++) {
snr_avg += gps_payload[15 + ch * 15 + meas];
}
snr_avg /= 2.0;
if (first)
first = 0;
else
debug(" ");
debug_int((unsigned int)svid);
debug(" ["); debug_float(az);
debug(" "); debug_float(el);
debug(" "); debug_float(snr_avg);
debug("]");
}
}
debug("\n");
#endif
}
/* Create an OTR Data message. Pass the plaintext as msg, and an
* optional chain of TLVs. A newly-allocated string will be returned in
* *encmessagep. */
gcry_error_t otrl_proto_create_data(char **encmessagep, ConnContext *context,
const char *msg, const OtrlTLV *tlvs, unsigned char flags)
{
size_t justmsglen = strlen(msg);
size_t msglen = justmsglen + 1 + otrl_tlv_seriallen(tlvs);
size_t buflen;
size_t pubkeylen;
unsigned char *buf = NULL;
unsigned char *bufp;
size_t lenp;
DH_sesskeys *sess = &(context->sesskeys[1][0]);
gcry_error_t err;
size_t reveallen = 20 * context->numsavedkeys;
size_t base64len;
char *base64buf = NULL;
unsigned char *msgbuf = NULL;
enum gcry_mpi_format format = GCRYMPI_FMT_USG;
char *msgdup;
int version = context->protocol_version;
/* Make sure we're actually supposed to be able to encrypt */
if (context->msgstate != OTRL_MSGSTATE_ENCRYPTED ||
context->their_keyid == 0) {
return gcry_error(GPG_ERR_CONFLICT);
}
/* We need to copy the incoming msg, since it might be an alias for
* context->lastmessage, which we'll be freeing soon. */
msgdup = gcry_malloc_secure(justmsglen + 1);
if (msgdup == NULL) {
return gcry_error(GPG_ERR_ENOMEM);
}
strcpy(msgdup, msg);
*encmessagep = NULL;
/* Header, send keyid, recv keyid, counter, msg len, msg
* len of revealed mac keys, revealed mac keys, MAC */
buflen = 3 + (version == 2 ? 1 : 0) + 4 + 4 + 8 + 4 + msglen +
4 + reveallen + 20;
gcry_mpi_print(format, NULL, 0, &pubkeylen, context->our_dh_key.pub);
buflen += pubkeylen + 4;
buf = malloc(buflen);
msgbuf = gcry_malloc_secure(msglen);
if (buf == NULL || msgbuf == NULL) {
free(buf);
gcry_free(msgbuf);
gcry_free(msgdup);
return gcry_error(GPG_ERR_ENOMEM);
}
memmove(msgbuf, msgdup, justmsglen);
msgbuf[justmsglen] = '\0';
otrl_tlv_serialize(msgbuf + justmsglen + 1, tlvs);
bufp = buf;
lenp = buflen;
if (version == 1) {
memmove(bufp, "\x00\x01\x03", 3); /* header */
} else {
memmove(bufp, "\x00\x02\x03", 3); /* header */
}
debug_data("Header", bufp, 3);
bufp += 3; lenp -= 3;
if (version == 2) {
bufp[0] = flags;
bufp += 1; lenp -= 1;
}
write_int(context->our_keyid-1); /* sender keyid */
debug_int("Sender keyid", bufp-4);
write_int(context->their_keyid); /* recipient keyid */
debug_int("Recipient keyid", bufp-4);
write_mpi(context->our_dh_key.pub, pubkeylen, "Y"); /* Y */
otrl_dh_incctr(sess->sendctr);
memmove(bufp, sess->sendctr, 8); /* Counter (top 8 bytes only) */
debug_data("Counter", bufp, 8);
bufp += 8; lenp -= 8;
write_int(msglen); /* length of encrypted data */
debug_int("Msg len", bufp-4);
err = gcry_cipher_reset(sess->sendenc);
if (err) goto err;
err = gcry_cipher_setctr(sess->sendenc, sess->sendctr, 16);
if (err) goto err;
err = gcry_cipher_encrypt(sess->sendenc, bufp, msglen, msgbuf, msglen);
if (err) goto err; /* encrypted data */
debug_data("Enc data", bufp, msglen);
bufp += msglen;
lenp -= msglen;
gcry_md_reset(sess->sendmac);
gcry_md_write(sess->sendmac, buf, bufp-buf);
memmove(bufp, gcry_md_read(sess->sendmac, GCRY_MD_SHA1), 20);
debug_data("MAC", bufp, 20);
bufp += 20; /* MAC */
lenp -= 20;
write_int(reveallen); /* length of revealed MAC keys */
debug_int("Revealed MAC length", bufp-4);
if (reveallen > 0) {
memmove(bufp, context->saved_mac_keys, reveallen);
debug_data("Revealed MAC data", bufp, reveallen);
bufp += reveallen; lenp -= reveallen;
free(context->saved_mac_keys);
context->saved_mac_keys = NULL;
context->numsavedkeys = 0;
}
assert(lenp == 0);
/* Make the base64-encoding. */
base64len = ((buflen + 2) / 3) * 4;
base64buf = malloc(5 + base64len + 1 + 1);
if (base64buf == NULL) {
err = gcry_error(GPG_ERR_ENOMEM);
goto err;
}
memmove(base64buf, "?OTR:", 5);
otrl_base64_encode(base64buf+5, buf, buflen);
base64buf[5 + base64len] = '.';
base64buf[5 + base64len + 1] = '\0';
free(buf);
gcry_free(msgbuf);
*encmessagep = base64buf;
gcry_free(context->lastmessage);
context->lastmessage = NULL;
context->may_retransmit = 0;
if (msglen > 0) {
const char *prefix = "[resent] ";
size_t prefixlen = strlen(prefix);
if (!strncmp(prefix, msgdup, prefixlen)) {
/* The prefix is already there. Don't add it again. */
prefix = "";
prefixlen = 0;
}
context->lastmessage = gcry_malloc_secure(prefixlen + justmsglen + 1);
if (context->lastmessage) {
strcpy(context->lastmessage, prefix);
strcat(context->lastmessage, msgdup);
}
}
gcry_free(msgdup);
return gcry_error(GPG_ERR_NO_ERROR);
err:
free(buf);
gcry_free(msgbuf);
gcry_free(msgdup);
*encmessagep = NULL;
return err;
}
void debug_nmi(interrupt_cpu_status_t *cs, CLOCK iclk)
{
debug_int(cs, "*** NMI", IK_NMI, iclk);
}
void debug_irq(interrupt_cpu_status_t *cs, CLOCK iclk)
{
debug_int(cs, "*** IRQ", IK_IRQ, iclk);
}
/**
* 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*/
}
void gps_nak_message() {
debug("Got NAK for message ");
debug_int((int)gps_payload[1]);
debug("\r\n");
}
void gps_poll() {
while (gps_can_read()) {
int ch = gps_read();
switch (decoder_state) {
case GPS_HEADER1:
if (ch == 0xa0)
decoder_state = GPS_HEADER2;
else {
debug("Got garbage char "); debug_int(ch); debug(" from GPS\r\n");
}
break;
case GPS_HEADER2:
if (ch == 0xa2)
decoder_state = GPS_LENGTH1;
else {
debug("Got garbage char "); debug_int(ch); debug(" from GPS (expecting 2nd header byte)\r\n");
decoder_state = GPS_HEADER1;
}
break;
case GPS_LENGTH1:
gps_payload_len = (ch & 0x7f) << 8;
decoder_state = GPS_LENGTH2;
break;
case GPS_LENGTH2:
gps_payload_len += ch;
gps_payload_remain = gps_payload_len;
if (gps_payload_len > GPS_BUFFER_SIZE - 1) {
debug_int(gps_payload_len); debug(" byte payload too big\r\n");
gps_ignore_payload = 1;
} else {
gps_message_valid = 1;
gps_ignore_payload = 0;
gps_payload_checksum = 0;
gps_payload_ptr = gps_payload;
}
decoder_state = GPS_PAYLOAD;
break;
case GPS_PAYLOAD:
if (!gps_ignore_payload) {
*(gps_payload_ptr++) = ch;
gps_payload_checksum += ch;
}
if (--gps_payload_remain == 0)
decoder_state = GPS_CHECKSUM1;
break;
case GPS_CHECKSUM1:
msg_checksum = ch << 8;
decoder_state = GPS_CHECKSUM2;
break;
case GPS_CHECKSUM2:
msg_checksum += ch;
if (!gps_ignore_payload) {
gps_payload_checksum &= 0x7FFF;
if (gps_payload_checksum != msg_checksum) {
debug("Received checksum "); debug_int(msg_checksum);
debug(" != calculated "); debug_int(gps_payload_checksum);
debug(", discarding message\r\n");
gps_message_valid = 0;
}
}
decoder_state = GPS_TRAILER1;
break;
case GPS_TRAILER1:
if (ch == 0xb0)
decoder_state = GPS_TRAILER2;
else {
debug("GPS got garbage "); debug_int(ch); debug(" (expecting 1st trailer)\r\n");
decoder_state = GPS_HEADER1;
}
break;
case GPS_TRAILER2:
if (ch == 0xb3) {
if (gps_message_valid)
gps_handle_message();
} else {
debug("GPS Got garbage "); debug_int(ch); debug(" (expecting 2nd trailer\r\n");
}
decoder_state = GPS_HEADER1;
break;
default:
debug("GPS decoder in unknown state?\r\n");
decoder_state = GPS_HEADER1;
}
}
}
/**
* The task for generating button events, also does terminal and ping sending
*/
static void vButtonTask( int8_t *pvParameters )
{
uint8_t event;
uint8_t buttons = 0;
uint8_t s1_count = 0;
uint8_t s2_count = 0;
int16_t byte;
uint8_t i;
uint8_t channel;
pvParameters;
N710_BUTTON_INIT();
vTaskDelay( 200 / portTICK_RATE_MS );
stack_event = stack_service_init(NULL); /* Open socket for stack status message */
channel = mac_current_channel();
while (1)
{
if(gw_assoc_state == 0 && scan_active == 0)
{
LED1_OFF();
scan_active=1;
scan_start = xTaskGetTickCount();
gw_discover();
}
/* Sleep for 10 ms or received from UART */
byte = debug_read_blocking(10 / portTICK_RATE_MS);
if (byte != -1)
{
switch(byte)
{
case '1': /*send a button 1 event*/
event = 1;
xQueueSend( button_events, ( void * ) &event, (portTickType) 0 );
break;
case '2': /*send a button 2 event*/
event = 2;
xQueueSend( button_events, ( void * ) &event, (portTickType) 0 );
break;
case '\r':
debug("\r\n");
break;
case 'm':
{
sockaddr_t mac;
rf_mac_get(&mac);
debug("MAC: ");
debug_address(&mac);
debug("\r\n");
}
break;
case 'p':
if(ping_active == 0)
{
echo_result.count=0;
if(ping(NULL, &echo_result) == pdTRUE) /* Broadcast */
{
ping_start = xTaskGetTickCount();
ping_active = 2;
debug("Ping\r\n");
}
else
debug("No buffer.\r\n");
}
break;
case 'u':
if(ping_active == 0)
{
echo_result.count=0;
if(udp_echo(NULL, &echo_result) == pdTRUE)
{
ping_start = xTaskGetTickCount();
ping_active = 1;
debug("udp echo_req()\r\n");
}
else
debug("No buffer.\r\n");
}
break;
/*case 'C':
if (channel < 26) channel++;
channel++;
case 'c':
if (channel > 11) channel--;
mac_set_channel(channel);
debug("Channel: ");
debug_int(channel);
debug("\r\n");
break;*/
default:
debug_put(byte);
}
}
/* Read button (S1 and S2) status */
if (N710_BUTTON_1 == 1)
{
if (s1_count > 5)
{
event = 1;
if (s1_count >= 100) event |= 0x80; /*long keypress*/
xQueueSend( button_events, ( void * ) &event, (portTickType) 0 );
}
s1_count = 0;
}
else
{
if (s1_count < 100)
{
s1_count++;
}
}
if (N710_BUTTON_2 == 1)
{
if (s2_count > 5)
{
event = 2;
if (s2_count >= 100) event |= 0x80; /*long keypress*/
xQueueSend( button_events, ( void * ) &event, (portTickType) 0 );
}
s2_count = 0;
}
else
{
if (s2_count < 100)
{
s2_count++;
}
}
/*LED blinkers*/
if (led1_count)
{
led1_count--;
LED1_ON();
}
else
{
LED1_OFF();
}
if (led2_count)
{
led2_count--;
LED2_ON();
}
else
{
LED2_OFF();
}
buttons = 0;
if (LED1())
{
buttons |= 1;
}
if (LED2())
{
buttons |= 2;
}
ssi_sensor_update(3, (uint32_t) buttons);
/* ping response handling */
if ((xTaskGetTickCount() - ping_start)*portTICK_RATE_MS > 1000 && ping_active)
{
debug("Ping timeout.\r\n");
stop_ping();
if(echo_result.count)
{
debug("Response: \r\n");
for(i=0; i<echo_result.count; i++)
{
debug_address(&(echo_result.result[i].src));
debug(" ");
debug_int(echo_result.result[i].rssi);
debug(" dbm, ");
debug_int(echo_result.result[i].time);
debug(" ms\r\n");
vTaskDelay(4);
}
echo_result.count=0;
}
else
{
debug("No response.\r\n");
}
ping_active = 0;
}
/* stack events */
if(stack_event)
{
buffer_t *buffer = waiting_stack_event(10);
if(buffer)
{
switch (parse_event_message(buffer))
{
case BROKEN_LINK:
buffer->dst_sa.port = datasensor_address.port;
if(memcmp(&datasensor_address.address, &(buffer->dst_sa.address), 8) == 0)
{
gw_assoc_state = 0;
datasensor_address.addr_type = ADDR_NONE;
LED1_OFF();
}
break;
case ROUTER_DISCOVER_RESPONSE:
scan_active=0;
if(gw_assoc_state==0)
{
memcpy(datasensor_address.address, buffer->src_sa.address, 8);
datasensor_address.addr_type = buffer->src_sa.addr_type ;
gw_assoc_state=1;
LED1_ON();
}
break;
default:
break;
}
if(buffer)
{
socket_buffer_free(buffer);
buffer = 0;
}
}
} /*end stack events*/
if (scan_active == 1 && (xTaskGetTickCount() - scan_start)*portTICK_RATE_MS > 1000)
{
mac_gw_discover();
scan_start = xTaskGetTickCount();
}
} /*end main loop*/
}
/**
* The application task reading ADC values and using the LEDs.
*/
static void vAppTask( int8_t *pvParameters )
{
uint8_t buttons = 0;
uint16_t U1_value = 0;
uint16_t U2_value = 0;
uint8_t count = 0;
pvParameters;
led1_count = 50;
led2_count = 100;
vTaskDelay( 50 / portTICK_RATE_MS );
vTaskDelay( 100 / portTICK_RATE_MS );
debug("NanoSensor N710 Example Application.\r\n");
/* Start an endless task loop, we must sleep most of the time allowing execution of other tasks. */
for (;;)
{
/* Sleep for 500 ms or until button event */
if (xQueueReceive(button_events, &buttons, 500 / portTICK_RATE_MS) == pdTRUE)
{
switch(buttons)
{
case 0x81:
case 0x01:
/* Read the LM60 illumination sensor (U1) */
/* P0.2 */
debug("Reading illumination (U1). ");
if (get_adc_value(N710_LIGHT, &U1_value) == 0)
{
ssi_sensor_update(0, (uint32_t) U1_value);
debug_int(U1_value);
led1_count = 30;
}
else
{
debug("failed.");
led1_count = 300;
}
debug("\r\n");
break;
case 0x82:
case 0x02:
/* Read the EL7900 temperature sensor (U2) */
/* P0.3 */
debug("Reading temp (U2): ");
if (get_adc_value(N710_TEMP, &U2_value) == 0)
{
int32_t calc_int = (int32_t) U2_value;
calc_int /= 4; /* drop insignificant bits */
//calc_int *= 58608; /* 3*160.039*1000*1000/8192 (value with 6 decimals)*/
calc_int *= 24420; /* 1.25*160.039*1000*1000/8192 (value with 6 decimals)*/
calc_int /= 10000; /* adjust to 2 decimals */
calc_int -= 6785;
U2_value = (int16_t) calc_int;
debug_int(U2_value/100);
debug_put('.');
if ((U2_value%100) >= 10)
{
debug_integer(2, 10, (U2_value%100));
}
else
{
debug_hex(U2_value%100);
}
led1_count = 30;
led2_count = 30;
ssi_sensor_update(1, (int32_t) U2_value);
}
else
{
debug("failed.");
led1_count = 300;
led2_count = 300;
}
debug("\r\n");
break;
default:
debug_hex(buttons);
break;
}
count = 0;
}
else
{
if (count++ >= 2)
{
count = 0;
buttons = 0x01;
xQueueSend( button_events, ( void * ) &buttons, (portTickType) 0 );
buttons = 0x02;
xQueueSend( button_events, ( void * ) &buttons, (portTickType) 0 );
}
}
}
}
void print_table_information(void)
{
neighbor_info_t *b;
#ifdef HAVE_ROUTING
uint8_t addres_length=0;
route_info_t *ptr;
#endif
if( xSemaphoreTake( table_lock, ( portTickType ) 10 ) == pdTRUE )
{
uint8_t i, j;
if(neighbor_table.count)
{
debug("Neighbor Info count:");
debug_hex(neighbor_table.count);
debug("\r\n");
debug("Child count:");
debug_hex(neighbor_table.child_count);
debug("\r\n");
for(i=0; i < MAX_NEIGHBOR_COUNT; i++)
{
b=&(neighbor_table.neighbor_info[i]);
if(b->type==ADDR_NONE)
b=0;
if(b)
{
if(b->type== ADDR_802_15_4_PAN_LONG)
{
debug("Long: ");
for(j=0; j < 2 ; j++)
{
if (j) debug_put(':');
debug_hex( b->address[9-j]);
}
debug(" ");
for(j=0; j < 8 ; j++)
{
if (j) debug_put(':');
debug_hex( b->address[7-j]);
}
}
if(b->type == ADDR_802_15_4_PAN_SHORT)
{
debug("Short: ");
for(j=0; j < 2 ; j++)
{
if (j) debug_put(':');
debug_hex( b->address[3-j]);
}
debug(" ");
for(j=0; j < 2 ; j++)
{
if (j) debug_put(':');
debug_hex( b->address[1-j]);
}
}
debug("\r\nrssi: ");
debug_int(b->last_rssi);
debug("\r\nTTL: ");
debug_hex(b->ttl);
debug("\r\n");
pause_us(200);
}
}
}
else
{
debug("No Neighbor info\r\n");
}
#ifdef HAVE_ROUTING
if(routing_table.count)
{
debug("\r\nroute Info count:");
debug_hex(routing_table.count);
debug("\r\n");
for(i=0; i < MAX_ROUTE_INFO_COUNT; i++)
{
ptr = &(routing_table.route_info[i]);
if(ptr->dest_addr_type==ADDR_NONE)
ptr=0;
if(ptr)
{
debug("Dest: ");
if(ptr->dest_addr_type==ADDR_802_15_4_PAN_LONG)
addres_length=8;
else
addres_length=2;
for(j=0; j < addres_length ; j++)
{
if (j) debug_put(':');
debug_hex(ptr->destination[(addres_length-1)-j]);
}
debug("\r\nNext hop: ");
if(ptr->next_hop_addr_type==ADDR_802_15_4_PAN_LONG)
addres_length=10;
else
addres_length=4;
for(j=0; j < addres_length ; j++)
{
if (j) debug_put(':');
debug_hex(ptr->next_hop[(addres_length-1)-j]);
}
debug("\r\nrssi: ");
debug_int(ptr->last_rssi);
debug("\r\nHop count: ");
debug_hex(ptr->hop_count);
debug("\r\nTTL: ");
debug_hex(ptr->ttl);
debug("\r\n");
}
}
}
else
{
debug("No route info\r\n");
}
#else
debug("Routing disable\r\n");
#endif
xSemaphoreGive( table_lock ); /*free lock*/
}
}
static void vbeacon_rfd( void *pvParameters )
{
buffer_t *buffer=0;
uint8_t send=0, *ptr, i, sqn=0;
int16_t byte;
uint16_t count = 0;
pause(200);
debug_init(115200);
pause(300);
stack_event = open_stack_event_bus(); /* Open socket for stack status message */
stack_service_init( stack_event,NULL, 0 , NULL );
if(stack_start(NULL)==START_SUCCESS)
{
debug("Start Ok\r\n");
}
/*****************************************************************************************************/
/****************************************************************************************************/
for (;;)
{
byte = debug_read_blocking(10);
if(byte != -1)
{
switch(byte)
{
case 'm':
ptr=mac_get_mac_pib_parameter(MAC_IEEE_ADDRESS);
if(ptr)
{
ptr +=7;
debug("Devices mac-address: ");
for(i=0; i< 8 ;i++)
{
if(i)
debug(":");
debug_hex(*ptr--);
}
debug("\r\n");
}
break;
case 'b':
debug_int(uxQueueMessagesWaiting(buffers));
debug(" buffers available.\r\n");
break;
case 'c':
ptr=mac_get_mac_pib_parameter(MAC_CURRENT_CHANNEL);
if(ptr)
{
debug("Current channel: ");
debug_int(*ptr);
debug("\r\n");
}
break;
case '\r':
debug("\r\n");
break;
default:
debug_put(byte);
break;
}
}
if(send && data)
{
buffer_t *buf = socket_buffer_get(data);
if (buf)
{
/* Create data packet */
/*buf->buf[buf->buf_end++] = 'S';
buf->buf[buf->buf_end++] = 'e';
buf->buf[buf->buf_end++] = 'n';
buf->buf[buf->buf_end++] = 's';
buf->buf[buf->buf_end++] = 'o';
buf->buf[buf->buf_end++] = 'r';
buf->buf[buf->buf_end++] = '_';
buf->buf[buf->buf_end++] = mac_long.address[1];
buf->buf[buf->buf_end++] = mac_long.address[0];*/
buf->buf[buf->buf_end++] = sqn;
buf->buf[buf->buf_end++] = 0xc2;
buf->buf[buf->buf_end++] = 0x00;
buf->buf[buf->buf_end++] = 0x08;
buf->buf[buf->buf_end++] = 0x00;
buf->buf[buf->buf_end++] = 0x08;
buf->buf[buf->buf_end++] = 0x00;
buf->buf[buf->buf_end++] = 0x10;
buf->buf[buf->buf_end++] = 0x23;
buf->buf[buf->buf_end++] = 0x16;
if (socket_sendto(data, &cord_address, buf) != pdTRUE)
{
debug("Data send failed.\r\n");
socket_buffer_free(buf);
buf=0;
}
if(sqn==0x0f)
sqn=0;
else
sqn++;
send=0;
}
}
if (count++ >= 400)
{
send=1;
count = 0;
}
if(stack_event)
{
buffer=0;
buffer = waiting_stack_event(10);
if(buffer)
{
switch (parse_event_message(buffer))
{
case BROKEN_LINK:
debug("Route broken to ");
debug("\r\n");
debug_address(&(buffer->dst_sa));
debug("\r\n");
break;
case ASSOCIATION_WITH_COORDINATOR:
debug("Assoc ok ");
if(get_coord_address(&cord_address) == pdTRUE)
{
data = socket(MODULE_CUDP, 0); /* Socket for response data from port number 61619 */
if (data)
{
if (socket_bind(data, &cord_address) != pdTRUE)
{
debug("Bind failed.\r\n");
}
}
}
break;
case NOT_ASSOCIATED:
debug("Application design error:\r\n");
debug("RFD try send data before association\r\n");
break;
case ASSOCIATION_LOST:
if(socket_close(data) == pdTRUE)
{
data=0;
scan_network();
}
break;
case TOO_LONG_PACKET:
debug("Payload Too Length, to ");
debug("\r\n");
break;
default:
break;
}
if(buffer)
{
socket_buffer_free(buffer);
buffer = 0;
}
}
}
LED1_ON();
LED1_OFF();
}
}
