diff --git a/ieee802154/common/Makefile b/ieee802154/common/Makefile index 045d2b1f5..22fdbdad8 100644 --- a/ieee802154/common/Makefile +++ b/ieee802154/common/Makefile @@ -43,6 +43,7 @@ ASRCS = CSRCS = ieee802154_setchan.c ieee802154_setpanid.c CSRCS += ieee802154_seteaddr.c ieee802154_setsaddr.c CSRCS += ieee802154_setpromisc.c +CSRCS += ieee802154_addrparse.c ieee802154_addrtostr.c # diff --git a/ieee802154/common/ieee802154_addrparse.c b/ieee802154/common/ieee802154_addrparse.c new file mode 100644 index 000000000..02b15c16a --- /dev/null +++ b/ieee802154/common/ieee802154_addrparse.c @@ -0,0 +1,126 @@ +/**************************************************************************** + * ieee802154/common/ieee802154_addrparse.c + * + * Copyright (C) 2015 Sebastien Lorquet. All rights reserved. + * Author: Sebastien Lorquet + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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 COPYRIGHT HOLDERS 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 + * COPYRIGHT OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +int ieee802154_addrparse(FAR struct ieee802154_packet_s *packet, + FAR struct ieee802154_addr_s *dest, + FAR struct ieee802154_addr_s *src) +{ + uint8_t fc1, fc2, daddr, saddr; + int index=3; + + /* read fc */ + fc1 = packet->data[0]; + fc2 = packet->data[1]; + daddr = fc2 & IEEE802154_FC2_DADDR; + saddr = fc2 & IEEE802154_FC2_SADDR; + + /* decode dest addr */ + + if(daddr == IEEE802154_DADDR_SHORT) + { + memcpy(&dest->ia_panid, packet->data+index, 2); + index += 2; /* skip dest pan id */ + memcpy(&dest->ia_saddr, packet->data+index, 2); + index += 2; /* skip dest addr */ + dest->ia_len = 2; + } + else if(daddr == IEEE802154_DADDR_EXT) + { + memcpy(&dest->ia_panid, packet->data+index, 2); + index += 2; /* skip dest pan id */ + memcpy(dest->ia_eaddr, packet->data+index, 8); + index += 8; /* skip dest addr */ + dest->ia_len = 8; + } + else if(daddr == IEEE802154_DADDR_NONE) + { + dest->ia_len = 0; + } + else + { + return -EINVAL; + } + + /* decode source pan id according to compression */ + if( (saddr == IEEE802154_SADDR_SHORT) || (saddr == IEEE802154_SADDR_EXT) ) + { + if(fc1 & IEEE802154_FC1_INTRA) + { + src->ia_panid = dest->ia_panid; + } + else + { + memcpy(&src->ia_panid, packet->data+index, 2); + index += 2; /*skip dest pan id*/ + } + } + + /* decode source addr */ + + if(saddr == IEEE802154_SADDR_SHORT) + { + memcpy(&src->ia_saddr, packet->data+index, 2); + index += 2; /* skip dest addr */ + src->ia_len = 2; + } + if(saddr == IEEE802154_SADDR_EXT) + { + memcpy(src->ia_eaddr, packet->data+index, 8); + index += 8; /* skip dest addr */ + src->ia_len = 8; + } + else if(saddr == IEEE802154_SADDR_NONE) + { + src->ia_len = 0; + } + else + { + return -EINVAL; + } + + return index; +} + diff --git a/ieee802154/common/ieee802154_addrtostr.c b/ieee802154/common/ieee802154_addrtostr.c new file mode 100644 index 000000000..743b2fd55 --- /dev/null +++ b/ieee802154/common/ieee802154_addrtostr.c @@ -0,0 +1,83 @@ +/**************************************************************************** + * ieee802154/common/ieee802154_addrtostr.c + * + * Copyright (C) 2015 Sebastien Lorquet. All rights reserved. + * Author: Sebastien Lorquet + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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 COPYRIGHT HOLDERS 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 + * COPYRIGHT OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include + +int ieee802154_addrtostr(FAR char *buf, int len, FAR struct ieee802154_addr_s *addr) +{ +#ifndef CONFIG_BIG_ENDIAN + uint16_t panid = ((addr->ia_panid & 0xff)<<8) | ((addr->ia_panid>>8) & 0xff); +#else + uint16_t panid = addr->ia_panid; +#endif + + if(addr->ia_len == 0) + { + return snprintf(buf, len, "none"); + } + else if(addr->ia_len == 2) + { +#ifndef CONFIG_BIG_ENDIAN + uint16_t saddr = ((addr->ia_saddr & 0xff)<<8) | ((addr->ia_saddr>>8) & 0xff); +#else + uint16_t saddr = addr->ia_saddr; +#endif + return snprintf(buf, len, "%04X/%04X", panid, saddr); + } + else if(addr->ia_len == 8) + { + int i; + int off = snprintf(buf, len, "%04X/", panid); + for(i=0; i<8; i++) + { + off += snprintf(buf+off, len-off, "%02X", addr->ia_eaddr[i]); + } + return off; + } + else + { + return snprintf(buf,len,""); + } + return -1; +} + diff --git a/ieee802154/coord/coord_main.c b/ieee802154/coord/coord_main.c index 0ac396555..89e582f11 100644 --- a/ieee802154/coord/coord_main.c +++ b/ieee802154/coord/coord_main.c @@ -90,25 +90,13 @@ struct ieee_client_s struct ieee802154_packet_s pending; /* pending packet for device */ }; -struct ieee_addr -{ - uint8_t len; - uint16_t panid; - union - { - uint16_t saddr; - uint8_t eaddr[8]; - } addr; -}; - struct ieee_frame_s { struct ieee802154_packet_s packet; - uint8_t fc1; - uint8_t fc2; + struct ieee802154_addr_s dest; + struct ieee802154_addr_s src; uint8_t seq; - struct ieee_addr daddr; - struct ieee_addr saddr; + uint8_t plen; uint8_t *payload; }; @@ -207,84 +195,37 @@ static int coord_command(FAR struct ieee_coord_s *coord) static int coord_manage(FAR struct ieee_coord_s *coord) { /* Decode frame type */ - uint8_t ftype, saddr, daddr; - int index; + uint8_t fc1, ftype; + int hlen; + char buf[IEEE802154_ADDRSTRLEN+1]; FAR struct ieee_frame_s *rx = &coord->rxbuf; int i; - for(i=0; ipacket.len; i++) printf("%02X", rx->packet.data[i]); - printf("\n"); - rx->fc1 = rx->packet.data[0]; - rx->fc2 = rx->packet.data[1]; + fc1 = rx->packet.data[0]; rx->seq = rx->packet.data[2]; - ftype = rx->fc1 & IEEE802154_FC1_FTYPE; - daddr = rx->fc2 & IEEE802154_FC2_DADDR; - saddr = rx->fc2 & IEEE802154_FC2_SADDR; - - index = 3; - - if(daddr == IEEE802154_DADDR_SHORT) + hlen = ieee802154_addrparse(&rx->packet, &rx->dest, &rx->src); + if(hlen<0) { - memcpy(&rx->daddr.panid, rx->packet.data+index, 2); - index += 2; /* skip dest pan id */ - memcpy(&rx->daddr.addr.saddr, rx->packet.data+index, 2); - index += 2; /* skip dest addr */ - rx->daddr.len = 2; - } - else if(daddr == IEEE802154_DADDR_EXT) - { - memcpy(&rx->daddr.panid, rx->packet.data+index, 2); - index += 2; /* skip dest pan id */ - memcpy(&rx->daddr.addr.eaddr, rx->packet.data+index, 8); - index += 8; /* skip dest addr */ - rx->daddr.len = 8; - } - else - { - rx->daddr.len = 0; + printf("invalid packet\n"); + return 1; } - if(saddr == IEEE802154_SADDR_SHORT) - { - if(rx->fc1 & IEEE802154_FC1_INTRA) - { - rx->saddr.panid = rx->daddr.panid; - } - else - { - memcpy(&rx->saddr.panid, rx->packet.data+index, 2); - index += 2; /*skip dest pan id*/ - } - memcpy(&rx->saddr.addr.saddr, rx->packet.data+index, 2); - index += 2; /* skip dest addr */ - rx->saddr.len = 2; - } - else if(saddr == IEEE802154_SADDR_EXT) - { - if(rx->fc1 & IEEE802154_FC1_INTRA) - { - rx->saddr.panid = rx->daddr.panid; - } - else - { - memcpy(&rx->saddr.panid, rx->packet.data+index, 2); - index += 2; /*skip dest pan id*/ - } - memcpy(&rx->saddr.addr.eaddr, rx->packet.data+index, 8); - index += 8; /* skip dest addr */ - rx->saddr.len = 8; - } - else - { - rx->saddr.len = 0; - } + rx->payload = rx->packet.data + hlen; + rx->plen = rx->packet.len - hlen; - rx->payload = rx->packet.data + index; + ftype = fc1 & IEEE802154_FC1_FTYPE; + + ieee802154_addrtostr(buf,sizeof(buf),&rx->src); + printf("[%s -> ", buf); + ieee802154_addrtostr(buf,sizeof(buf),&rx->dest); + printf("%s] ", buf); + + for(i=0; iplen; i++) printf("%02X", rx->payload[i]); + printf("\n"); - printf("SADDR len %d DADDR len %d\n", rx->saddr.len, rx->daddr.len); switch(ftype) { case IEEE802154_FRAME_BEACON : coord_beacon (coord); break; diff --git a/ieee802154/i8sak/i8sak_main.c b/ieee802154/i8sak/i8sak_main.c index 55b26b23e..42fc6501d 100644 --- a/ieee802154/i8sak/i8sak_main.c +++ b/ieee802154/i8sak/i8sak_main.c @@ -529,6 +529,7 @@ data_error: gTxPacket.data[gTxPacket.len++] = 0xFF; gTxPacket.data[gTxPacket.len++] = 0xFF; //saddr gTxPacket.data[gTxPacket.len++] = 0xFF; + gTxPacket.data[gTxPacket.len++] = 0x07; //BEACON_REQ // for(i=1;i<3;i++) while(1) { diff --git a/include/ieee802154/ieee802154.h b/include/ieee802154/ieee802154.h index 6d4602db6..60ae08677 100644 --- a/include/ieee802154/ieee802154.h +++ b/include/ieee802154/ieee802154.h @@ -39,10 +39,26 @@ #include #include -int ieee802154_setchan (int fd, uint8_t chan); +struct ieee802154_addr_s +{ + uint8_t ia_len; /* structure length NOT including panid, so 2/8*/ + uint16_t ia_panid; + union { + uint16_t _ia_saddr; + uint8_t _ia_eaddr[8]; + } ia_addr; +#define ia_saddr ia_addr._ia_saddr +#define ia_eaddr ia_addr._ia_eaddr +}; +#define IEEE802154_ADDRSTRLEN 22 /* (4+1+8*2) */ + +int ieee802154_setchan (int fd, uint8_t chan); int ieee802154_setpanid (int fd, uint16_t panid); int ieee802154_setsaddr (int fd, uint16_t saddr); -int ieee802154_seteaddr (int fd, uint8_t *chan); -int ieee802154_setpromisc(int fd, bool promisc); +int ieee802154_seteaddr (int fd, FAR uint8_t *chan); +int ieee802154_setpromisc(int fd, bool promisc); + +int ieee802154_addrparse(FAR struct ieee802154_packet_s *inPacket, FAR struct ieee802154_addr_s *dest, FAR struct ieee802154_addr_s *src); +int ieee802154_addrtostr(FAR char *buf, int len, FAR struct ieee802154_addr_s *addr); #endif /*__APPS_INCLUDE_IEEE802154_IEEE802154_H */