This patch adds basic support for 6lowpan. Signed-off-by: Alexander Smirnov <[email protected]> --- net/ieee802154/6lowpan.c | 945 ++++++++++++++++++++++++++++++++++++++++++++++ net/ieee802154/6lowpan.h | 232 ++++++++++++ net/ieee802154/Kconfig | 6 + net/ieee802154/Makefile | 8 +- 4 files changed, 1188 insertions(+), 3 deletions(-) create mode 100644 net/ieee802154/6lowpan.c create mode 100644 net/ieee802154/6lowpan.h
diff --git a/net/ieee802154/6lowpan.c b/net/ieee802154/6lowpan.c new file mode 100644 index 0000000..9aa013b --- /dev/null +++ b/net/ieee802154/6lowpan.c @@ -0,0 +1,945 @@ +/* + * Copyright 2011, Siemens AG + * written by Alexander Smirnov <[email protected]> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 + * as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +/* + * Based on patches from Jon Smirl <[email protected]> who derived code + * from Contiki - http://www.sics.se/contiki + * + * Please find below the Contiki license: + * + * Copyright (c) 2008, Swedish Institute of Computer Science. + * All rights reserved. + * + * 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 of the Institute 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 INSTITUTE 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 INSTITUTE 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 <linux/module.h> +#include <linux/kernel.h> +#include <linux/netdevice.h> +#include <linux/etherdevice.h> +#include <linux/init.h> +#include <linux/moduleparam.h> +#include <linux/rtnetlink.h> +#include <linux/if_arp.h> +#include <linux/u64_stats_sync.h> +#include <linux/if_ieee802154.h> +#include <net/af_ieee802154.h> +#include <net/ieee802154_netdev.h> +#include <net/ieee802154.h> +#include <net/ipv6.h> +#include <net/rtnetlink.h> + +#include "6lowpan.h" + +#ifdef DEBUG + +/* this macro allows raw data dump */ +#define LOWPAN_DEBUG 1 + +#undef pr_debug +#define pr_debug(fmt, ...) \ + printk(pr_fmt(fmt), ##__VA_ARGS__) + +#endif /* DEBUG */ + +/* 802.15.4 interface name */ +static char *real_dev_name = "wpan0"; + +/* TTL uncompression values */ +static const u8 ttl_values[] = {0, 1, 64, 255}; + +/* + * Uncompression of linklocal: + * 0 -> 16 bytes from packet + * 1 -> 2 bytes from prefix - bunch of zeroes and 8 from packet + * 2 -> 2 bytes from prefix - zeroes + 2 from packet + * 3 -> 2 bytes from prefix - infer 8 bytes from lladdr + * + * NOTE: => the uncompress function does change 0xf to 0x10 + * NOTE: 0x00 => no-autoconfig => unspecified + */ +const u8 unc_llconf[] = {0x0f,0x28,0x22,0x20}; + +/* + * Uncompression of ctx-based: + * 0 -> 0 bits from packet [unspecified / reserved] + * 1 -> 8 bytes from prefix - bunch of zeroes and 8 from packet + * 2 -> 8 bytes from prefix - zeroes + 2 from packet + * 3 -> 8 bytes from prefix - infer 8 bytes from lladdr + */ +const u8 unc_ctxconf[] = {0x00,0x88,0x82,0x80}; + +/* + * Uncompression of ctx-base + * 0 -> 0 bits from packet + * 1 -> 2 bytes from prefix - bunch of zeroes 5 from packet + * 2 -> 2 bytes from prefix - zeroes + 3 from packet + * 3 -> 2 bytes from prefix - infer 1 bytes from lladdr + */ +const u8 unc_mxconf[] = {0x0f, 0x25, 0x23, 0x21}; + +/* Link local prefix */ +const u8 llprefix[] = {0xfe, 0x80}; + +/* TODO: lists instead of static allocation? */ +#define LOWPAN_CONF_MAX_ADDR_CONTEXTS 8 +static struct lowpan_addr_context + addr_contexts[LOWPAN_CONF_MAX_ADDR_CONTEXTS]; + +/* Find the context corresponding to prefix ipaddr */ +static struct lowpan_addr_context* +addr_context_lookup_by_prefix(const struct in6_addr *ipaddr) +{ + int i; + + for(i = 0; i < LOWPAN_CONF_MAX_ADDR_CONTEXTS; i++) { + if((addr_contexts[i].used == 1) && + ipaddr_prefixcmp(&addr_contexts[i].prefix, ipaddr->s6_addr, 64)) + return &addr_contexts[i]; + } + return NULL; +} + +/* Find the context with the given number */ +static struct lowpan_addr_context* +addr_context_lookup_by_number(u8 number) +{ + int i; + + for(i = 0; i < LOWPAN_CONF_MAX_ADDR_CONTEXTS; i++) { + if((addr_contexts[i].used == 1) && + addr_contexts[i].number == number) + return &addr_contexts[i]; + } + return NULL; +} + +static u8 +compress_addr_64(u8 **hc06_ptr, u8 bitpos, const struct in6_addr *ipaddr, + const unsigned char lladdr[IEEE802154_ALEN]) +{ + if (is_addr_mac_addr_based(ipaddr, lladdr)) + return 3 << bitpos; /* 0-bits */ + + else if (lowpan_is_iid_16_bit_compressable(ipaddr)) { + /* compress IID to 16 bits xxxx::XXXX */ + memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2); + *hc06_ptr += 2; + return 2 << bitpos; /* 16-bits */ + } + /* do not compress IID => xxxx::IID */ + memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8); + *hc06_ptr += 8; + return 1 << bitpos; /* 64-bits */ +} + +void +uip_ds6_set_addr_iid(struct in6_addr *ipaddr, unsigned char *lladdr) +{ + memcpy(&ipaddr->s6_addr[8], lladdr, IEEE802154_ALEN); + ipaddr->s6_addr[8] ^= 0x02; +} + +/* + * Uncompress addresses based on a prefix and a postfix with zeroes in + * between. If the postfix is zero in length it will use the link address + * to configure the IP address (autoconf style). + * pref_post_count takes a byte where the first nibble specify prefix count + * and the second postfix count (NOTE: 15/0xf => 16 bytes copy). + */ +static void +uncompress_addr(struct sk_buff *skb, struct in6_addr *ipaddr, u8 const *prefix, + u8 pref_post_count, unsigned char *lladdr) +{ + int tmp; + u8 prefcount = pref_post_count >> 4; + u8 postcount = pref_post_count & 0x0f; + + /* full nibble 15 => 16 */ + prefcount = prefcount == 15 ? 16 : prefcount; + postcount = postcount == 15 ? 16 : postcount; + +#ifdef LOWPAN_DEBUG + if (lladdr) { + pr_debug("(%s): link local address: ", __func__); + for (tmp = 0; tmp < IEEE802154_ALEN; tmp++) + pr_debug("%02x", lladdr[tmp]); + pr_debug("\n"); + } + pr_debug("(%s): uncompressing %d + %d => ", __func__, prefcount, postcount); +#endif /* LOWPAN_DEBUG */ + + if (prefcount > 0) + memcpy(ipaddr, prefix, prefcount); + + if (prefcount + postcount < 16) + memset(&ipaddr->s6_addr[prefcount], 0, 16 - (prefcount + postcount)); + + if (postcount > 0) { + memcpy(&ipaddr->s6_addr[16 - postcount], skb->data, postcount); + skb_pull(skb, postcount); + } else + if (prefcount > 0) + /* no IID based configuration if no prefix and no data => unspec */ + uip_ds6_set_addr_iid(ipaddr, lladdr); + +#ifdef LOWPAN_DEBUG + for (tmp = 0; tmp < 16; tmp++) + pr_debug("%02x", ipaddr->s6_addr[tmp]); + pr_debug("\n"); +#endif /* LOWPAN_DEBUG */ +} + +static u8 fetch_skb_u8(struct sk_buff *skb) +{ + u8 ret; + + BUG_ON(skb->len < 1); + + ret = skb->data[0]; + skb_pull(skb, 1); + + return ret; +} + +/* + * currently the long address only is used + */ +int lowpan_header_create_finish(struct sk_buff *skb, + struct net_device *dev, + unsigned short type, const void *_daddr, + const void *_saddr, unsigned len) +{ + u8 head[24] = {0,}; + u16 fc; + int pos = 2; /* leave room for FC */ + const u8 *saddr = _saddr; + const u8 *daddr = _daddr; + + head[pos++] = mac_cb(skb)->seq; /* DSN/BSN */ + + fc = IEEE802154_FC_TYPE_DATA; + if (mac_cb_is_ackreq(skb)) + fc |= IEEE802154_FC_ACK_REQ; + + if (!daddr) return -EINVAL; + if (!saddr) saddr = dev->dev_addr; + + /*TODO: default panid, do we need that? */ + head[pos++] = 0xCD, head[pos++] = 0xAB; + + if (lowpan_is_addr_broadcast(daddr)) { + fc |= (IEEE802154_ADDR_SHORT << IEEE802154_FC_DAMODE_SHIFT); + head[pos++] = 0xff, head[pos++] = 0xff; + } else { + /* force long */ + fc |= (IEEE802154_ADDR_LONG << IEEE802154_FC_DAMODE_SHIFT); + data_swap(daddr, head + pos); + pos += IEEE802154_ADDR_LEN; + } + + fc |= (IEEE802154_ADDR_LONG << IEEE802154_FC_SAMODE_SHIFT); + fc |= IEEE802154_FC_INTRA_PAN; + + data_swap(saddr, head + pos); + pos += IEEE802154_ADDR_LEN; + + head[0] = fc, head[1] = fc >> 8; + + memcpy(skb_push(skb, pos), head, pos); + + return pos; +} + +static int lowpan_header_create(struct sk_buff *skb, + struct net_device *dev, + unsigned short type, const void *_daddr, + const void *_saddr, unsigned len) +{ + u8 tmp, iphc0, iphc1, *hc06_ptr; + struct ipv6hdr* hdr; + struct lowpan_addr_context *context; + const u8 *saddr = _saddr; + const u8 *daddr = _daddr; + u8 head[100] = {}; + u8 daddr_buf[IEEE802154_ALEN]; + + if (type != ETH_P_IPV6) + return 0; + /* TODO: + * if this package isn't ipv6 one, where should it be routed? + * return lowpan_header_create_finish(skb, dev, type, _daddr, _saddr, skb->len); + */ + + hdr = ipv6_hdr(skb); + hc06_ptr = head + 2; + +#ifdef LOWPAN_DEBUG + pr_debug("(%s): IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\tnexthdr = 0x%02x\n\thop_lim = %d\n", + __func__, hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit); + + pr_debug("(%s): raw skb network header dump:", __func__); + for (tmp = 0; tmp < sizeof(skb_network_header(skb)); tmp++) { + if (tmp % 8 == 0) + pr_debug("\n\t0x%04x: ", tmp); + pr_debug("%02x ", skb_network_header(skb)[tmp]); + } + pr_debug("\n"); +#endif /* LOWPAN_DEBUG */ + + if (!saddr) + saddr = dev->dev_addr; + +#ifdef LOWPAN_DEBUG + pr_debug("(%s): saddr: ", __func__); + for (tmp = 0; tmp < 8; tmp++) + pr_debug("%02x ", saddr[tmp]); + pr_debug("\n"); +#endif /* LOWPAN_DEBUG */ + + /* + * As we copy some bit-length fields, in the IPHC encoding bytes, + * we sometimes use |= + * If the field is 0, and the current bit value in memory is 1, + * this does not work. We therefore reset the IPHC encoding here + */ + iphc0 = LOWPAN_DISPATCH_IPHC; + iphc1 = 0; + + /* check if dest context exists (for allocating third byte); */ + if ((context = addr_context_lookup_by_prefix(&hdr->daddr)) != NULL || + addr_context_lookup_by_prefix(&hdr->saddr) != NULL) + { + pr_debug("(%s): compressing dest or src ipaddr - setting CID\n", __func__); + iphc1 |= LOWPAN_IPHC_CID; + hc06_ptr++; + } + + if (context && (memcmp(daddr, saddr, IEEE802154_ALEN) == 0)) { + memcpy(&daddr_buf, &hdr->daddr.s6_addr[8], IEEE802154_ALEN); + daddr_buf[0] &= 0xFD; + daddr = &daddr_buf[0]; + } + +#ifdef LOWPAN_DEBUG + pr_debug("(%s): daddr: ", __func__); + for (tmp = 0; tmp < 8; tmp++) + pr_debug("%02x ", daddr[tmp]); + pr_debug("\n"); +#endif /* LOWPAN_DEBUG */ + + /* + * Traffic class, flow label + * If flow label is 0, compress it. If traffic class is 0, compress it + * We have to process both in the same time as the offset of traffic class + * depends on the presence of version and flow label + */ + + /* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */ + tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4); + tmp = ((tmp & 0x03) << 6) | (tmp >> 2); + + if(((hdr->flow_lbl[0] & 0x0F) == 0) && + (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) { + /* flow label can be compressed */ + iphc0 |= LOWPAN_IPHC_FL_C; + if((hdr->priority == 0) && + ((hdr->flow_lbl[0] & 0xF0) == 0)) { + /* compress (elide) all */ + iphc0 |= LOWPAN_IPHC_TC_C; + } else { + /* compress only the flow label */ + *hc06_ptr = tmp; + hc06_ptr += 1; + } + } else { + /* Flow label cannot be compressed */ + if((hdr->priority == 0) && + ((hdr->flow_lbl[0] & 0xF0) == 0)) { + /* compress only traffic class */ + iphc0 |= LOWPAN_IPHC_TC_C; + *hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F); + memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2); + hc06_ptr += 3; + } else { + /* compress nothing */ + memcpy(hc06_ptr, &hdr, 4); + /* but replace the top byte with the new ECN | DSCP format*/ + *hc06_ptr = tmp; + hc06_ptr += 4; + } + } + + /* NOTE: payload length is always compressed */ + + /* Next Header is compress if UDP */ + if(hdr->nexthdr == UIP_PROTO_UDP) + iphc0 |= LOWPAN_IPHC_NH_C; + +/* TODO: next header compression */ +#if 0 + if(LOWPAN_NH_COMPRESSOR.is_compressable(hdr->proto)) { + iphc0 |= LOWPAN_IPHC_NH_C; + } +#endif + + if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { + *hc06_ptr = hdr->nexthdr; + hc06_ptr += 1; + } + + /* + * Hop limit + * if 1: compress, encoding is 01 + * if 64: compress, encoding is 10 + * if 255: compress, encoding is 11 + * else do not compress + */ + switch(hdr->hop_limit) { + case 1: + iphc0 |= LOWPAN_IPHC_TTL_1; + break; + case 64: + iphc0 |= LOWPAN_IPHC_TTL_64; + break; + case 255: + iphc0 |= LOWPAN_IPHC_TTL_255; + break; + default: + *hc06_ptr = hdr->hop_limit; + break; + } + + /* source address compression */ + if (is_addr_unspecified(&hdr->saddr)) { + pr_debug("(%s): source address is unspecified, setting SAC\n", __func__); + iphc1 |= LOWPAN_IPHC_SAC; + } else if ((context = addr_context_lookup_by_prefix(&hdr->saddr)) != NULL) { + /* elide the prefix - indicate by CID and set context + SAC */ + pr_debug("(%s): compressing source address with context, setting CID & SAC ctx: %d\n", + __func__, context->number); + iphc1 |= LOWPAN_IPHC_CID | LOWPAN_IPHC_SAC; + head[2] |= context->number << 4; + + /* compession compare with this nodes address (source) */ + iphc1 |= compress_addr_64(&hc06_ptr, LOWPAN_IPHC_SAM_BIT, + &hdr->saddr, saddr); + } else if (is_addr_link_local(&hdr->saddr)) { + pr_debug("(%s): source address is link-local\n", __func__); + iphc1 |= compress_addr_64(&hc06_ptr, LOWPAN_IPHC_SAM_BIT, + &hdr->saddr, saddr); + } else { + pr_debug("(%s): send the full source address\n", __func__); + memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16); + hc06_ptr += 16; + } + + /* destination address compression */ + if (is_addr_mcast(&hdr->daddr)) { + pr_debug("(%s): destination address is multicast: ", __func__); + iphc1 |= LOWPAN_IPHC_M; + if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) { + pr_debug("compressed to 1 octet\n"); + iphc1 |= LOWPAN_IPHC_DAM_11; + /* use last byte */ + *hc06_ptr = hdr->daddr.s6_addr[15]; + hc06_ptr += 1; + } else + if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) { + pr_debug("compressed to 4 octets\n"); + iphc1 |= LOWPAN_IPHC_DAM_10; + /* second byte + the last three */ + *hc06_ptr = hdr->daddr.s6_addr[1]; + memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3); + hc06_ptr += 4; + } else + if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) { + pr_debug("compressed to 6 octets\n"); + iphc1 |= LOWPAN_IPHC_DAM_01; + /* second byte + the last five */ + *hc06_ptr = hdr->daddr.s6_addr[1]; + memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5); + hc06_ptr += 6; + } else { + pr_debug("using full address\n"); + iphc1 |= LOWPAN_IPHC_DAM_00; + memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16); + hc06_ptr += 16; + } + } else { + pr_debug("(%s): destination address is unicast: ", __func__); + if((context = addr_context_lookup_by_prefix(&hdr->daddr)) != NULL) { + pr_debug("compressing with context, setting CID & DAC ctx: %d\n", + context->number); + iphc1 |= LOWPAN_IPHC_CID | LOWPAN_IPHC_DAC; + head[2] |= context->number; + /* compession compare with link adress (destination) */ + iphc1 |= compress_addr_64(&hc06_ptr, LOWPAN_IPHC_DAM_BIT, + &hdr->daddr, daddr); + } else if(is_addr_link_local(&hdr->daddr)) { + pr_debug("destination address is link-local\n"); + iphc1 |= compress_addr_64(&hc06_ptr, LOWPAN_IPHC_DAM_BIT, + &hdr->daddr, daddr); + } else { + pr_debug("using full address\n"); + memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16); + hc06_ptr += 16; + } + } + + /* TODO: UDP header compression */ + /* TODO: Next Header compression */ + + head[0] = iphc0; + head[1] = iphc1; + + skb_pull(skb, sizeof(struct ipv6hdr)); + memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head); + +#ifdef LOWPAN_DEBUG + pr_debug("(%s): raw skb data dump:", __func__); + for (tmp = 0; tmp < skb->len; tmp++) { + if (tmp % 16 == 0) + pr_debug("\n\t0x%04x: ", tmp); + pr_debug("%02x ", skb->data[tmp]); + } + pr_debug("\n"); +#endif /* LOWPAN_DEBUG */ + + return lowpan_header_create_finish(skb, dev, type, daddr, saddr, skb->len); +} + +static int +lowpan_process_data(struct net_device *dev, struct sk_buff *skb) +{ + struct ipv6hdr hdr; + u8 tmp, iphc0, iphc1, num_context = 0; + u8 *_saddr, *_daddr; + struct lowpan_addr_context *context = NULL; + +#ifdef LOWPAN_DEBUG + pr_debug("(%s): raw skb data dump:", __func__); + for (tmp = 0; tmp < skb->len; tmp++) { + if (tmp % 16 == 0) + pr_debug("\n\t0x%04x: ", tmp); + pr_debug("%02x ", skb->data[tmp]); + } + pr_debug("\n"); +#endif /* LOWPAN_DEBUG */ + + /* at least two byte will be used for the encoding */ + iphc0 = fetch_skb_u8(skb); + iphc1 = fetch_skb_u8(skb); + + _saddr = mac_cb(skb)->sa.hwaddr; + _daddr = mac_cb(skb)->da.hwaddr; + + pr_debug("(%s): iphc0 = %02x, iphc1 = %02x\n", __func__, iphc0, iphc1); + + /* another if the CID flag is set */ + if(iphc1 & LOWPAN_IPHC_CID) { + pr_debug("(%s): CID flag is set, increase header with one\n", __func__); + num_context = fetch_skb_u8(skb); + } + + hdr.version = 6; + + /* Traffic Class and Flow Label */ + switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) { + /* + * Traffic Class and FLow Label carried in-line + * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) + */ + case 0: /* 00b */ + tmp = fetch_skb_u8(skb); + memcpy(&hdr.flow_lbl, &skb->data[0], 3); + skb_pull(skb, 3); + hdr.priority = ((tmp >> 2) & 0x0f); + hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) | (hdr.flow_lbl[0] & 0x0f); + break; + /* + * Traffic class carried in-line + * ECN + DSCP (1 byte), Flow Label is elided + */ + case 1: /* 10b */ + tmp = fetch_skb_u8(skb); + hdr.priority = ((tmp >> 2) & 0x0f); + hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30); + hdr.flow_lbl[1] = 0; + hdr.flow_lbl[2] = 0; + break; + /* + * Flow Label carried in-line + * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided + */ + case 2: /* 01b */ + tmp = fetch_skb_u8(skb); + hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30); + memcpy(&hdr.flow_lbl[1], &skb->data[0], 2); + skb_pull(skb, 2); + break; + /* Traffic Class and Flow Label are elided */ + case 3: /* 11b */ + hdr.priority = 0; + hdr.flow_lbl[0] = 0; + hdr.flow_lbl[1] = 0; + hdr.flow_lbl[2] = 0; + break; + default: + break; + } + + /* Next Header */ + if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { + /* Next header is carried inline */ + hdr.nexthdr = fetch_skb_u8(skb); + pr_debug("(%s): NH flag is set, next header is carried inline: %02x\n", __func__, hdr.nexthdr); + } + + /* Hop Limit */ + if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I) + hdr.hop_limit = ttl_values[iphc0 & 0x03]; + else + hdr.hop_limit = fetch_skb_u8(skb); + + /* Extract SAM to the tmp variable */ + tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03; + + /* Source address uncompression */ + if (iphc1 & LOWPAN_IPHC_SAC) { + u8 sci = (iphc1 & LOWPAN_IPHC_CID) ? num_context >> 4 : 0; + + pr_debug("(%s): SAC flag is set, using context-bsed compression\n", __func__); + + if((iphc1 & LOWPAN_IPHC_SAM) != 0) { + context = addr_context_lookup_by_number(sci); + pr_debug("(%s): source address context %p, SCI %d\n", __func__, context, sci); + + if(context == NULL) + return pr_debug("(%s): ERROR: context not found\n", __func__), -ENXIO; + else + printk("(%s): found compressed source context for SCI = %d\n", __func__, sci); + + uncompress_addr(skb, &hdr.saddr, context->prefix, unc_ctxconf[tmp], skb->data); + } else { + pr_debug("(%s): the full address is carried in-line\n", __func__); + uncompress_addr(skb, &hdr.saddr, (u8 *)llprefix, unc_ctxconf[tmp], _saddr); + } + } else { + pr_debug("(%s): source address stateless compression\n", __func__); + uncompress_addr(skb, &hdr.saddr, llprefix, unc_llconf[tmp], skb->data); + } + + /* Extract DAM to the tmp variable */ + tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03; + + /* check for Multicast Compression */ + if (iphc1 & LOWPAN_IPHC_M) { + if (iphc1 & LOWPAN_IPHC_DAC) { + pr_debug("(%s): destination address context-based multicast compression\n", __func__); + /* TODO: implement this */ + } else { + u8 prefix[] = {0xff, 0x02}; + + pr_debug("(%s): destination address non-context-based multicast compression\n", __func__); + if (0 < tmp && tmp < 3) + prefix[1] = fetch_skb_u8(skb); + + uncompress_addr(skb, &hdr.daddr, prefix, unc_mxconf[tmp], NULL); + } + } else { + if(iphc1 & LOWPAN_IPHC_DAC) { + u8 dci = (iphc1 & LOWPAN_IPHC_CID) ? num_context & 0x0f : 0; + context = addr_context_lookup_by_number(dci); + + pr_debug("(%s): destination address context-based compression\n", __func__); + + if (context == NULL) + return pr_debug("(%s): ERROR: context not found\n", __func__), -ENXIO; + + uncompress_addr(skb, &hdr.daddr, context->prefix, unc_ctxconf[tmp], skb->data); + } else { + pr_debug("(%s): destination address stateless compression\n", __func__); + uncompress_addr(skb, &hdr.daddr, llprefix, unc_llconf[tmp], skb->data); + } + } + + /* TODO: UDP header parse */ + + /* Not fragmented package */ + hdr.payload_len = htons(skb->len); + + pr_debug("(%s): skb headroom size = %d, data length = %d\n", __func__, skb_headroom(skb), skb->len); + + skb_push(skb, sizeof(struct ipv6hdr)); + /* TODO: do we need that? + * skb_copy_to_linear_data(skb, &hdr, sizeof(struct ipv6hdr)); + */ + skb->protocol = htons(ETH_P_IPV6); + skb->pkt_type = PACKET_HOST; + + pr_debug("(%s): IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\tnexthdr = 0x%02x\n\thop_lim = %d\n", + __func__, hdr.version, ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit); + +#ifdef LOWPAN_DEBUG + pr_debug("(%s): raw header dump:", __func__); + for (tmp = 0; tmp < sizeof(hdr); tmp++) { + if (tmp % 8 == 0) + pr_debug("\n\t0x%04x: ", tmp); + pr_debug("%02x ", ((u8 *)&hdr)[tmp]); + } + pr_debug("\n"); +#endif /* LOWPAN_DEBUG */ + + if (in_interrupt()) + return netif_rx(skb); + else + return netif_rx_ni(skb); +} + +/* TODO: do we need that? */ +static int lowpan_header_parse(const struct sk_buff *skb, + unsigned char *haddr) +{ + const u8 *hdr = skb_mac_header(skb), *tail = skb_tail_pointer(skb); + + pr_debug("(%s)\n", __func__); + + if (hdr + IEEE802154_ADDR_LEN <= tail) + data_swap(hdr, haddr); + + return 0; +} + +static int lowpan_set_address(struct net_device *dev, void *p) +{ + struct sockaddr *sa = p; + + if (netif_running(dev)) + return -EBUSY; + + /* TODO: validate addr */ + memcpy(dev->dev_addr, sa->sa_data, dev->addr_len); + + return 0; +} + +static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev) +{ + int err = 0; + + pr_debug("(%s): package xmit\n", __func__); + + if ((skb->dev = lowpan_dev_info(dev)->real_dev) == NULL) { + pr_debug("(%s) ERROR: no real wpan device found\n", __func__); + dev_kfree_skb(skb); + } else + err = dev_queue_xmit(skb); + + return (err < 0 ? NETDEV_TX_BUSY : NETDEV_TX_OK); +} + +static int lowpan_dev_init(struct net_device *dev) +{ + return 0; +} + +static void lowpan_dev_free(struct net_device *dev) +{ + dev_put(lowpan_dev_info(dev)->real_dev); + free_netdev(dev); +} + +static struct header_ops lowpan_header_ops = { + .create = lowpan_header_create, + .parse = lowpan_header_parse, +}; + +static const struct net_device_ops lowpan_netdev_ops = { + .ndo_init = lowpan_dev_init, + .ndo_start_xmit = lowpan_xmit, + .ndo_do_ioctl = NULL, /* TODO: do we need that? */ + .ndo_set_mac_address = lowpan_set_address, +}; + +static void lowpan_setup(struct net_device *dev) +{ + pr_debug("(%s)\n", __func__); + + dev->addr_len = IEEE802154_ADDR_LEN; + memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN); + dev->type = ARPHRD_IEEE802154; + dev->features = NETIF_F_NO_CSUM; + dev->hard_header_len = 2 + 1 + 20 + 14; + dev->needed_tailroom = 2; /* FCS */ + dev->mtu = 1280; + dev->tx_queue_len = 10; + dev->flags = IFF_NOARP | IFF_BROADCAST; + dev->watchdog_timeo = 0; + + dev->netdev_ops = &lowpan_netdev_ops; + dev->header_ops = &lowpan_header_ops; + dev->destructor = lowpan_dev_free; +} + +static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[]) +{ + pr_debug("(%s)\n", __func__); + + if (tb[IFLA_ADDRESS]) { + if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN) + return -EINVAL; + } + return 0; +} + +static struct rtnl_link_ops lowpan_link_ops __read_mostly = { + .kind = "lowpan", + .setup = lowpan_setup, + .validate = lowpan_validate, +}; + +module_param(real_dev_name, charp, 0); +MODULE_PARM_DESC(real_dev_name, "802.15.4 interface name to which 6lowpan will be assigned"); +static int __init lowpan_init_one(void) +{ + struct net_device *dev_lowpan, *dev_wpan; + int err; + + pr_debug("(%s)\n", __func__); + + /* allocate lowpan device */ + dev_lowpan = alloc_netdev(sizeof(struct lowpan_dev_info), "lowpan%d", lowpan_setup); + if (!dev_lowpan) + return -ENOMEM; + + err = dev_alloc_name(dev_lowpan, dev_lowpan->name); + if (err < 0) + goto err; + + /* set rtnl operations for lowpan device */ + dev_lowpan->rtnl_link_ops = &lowpan_link_ops; + + /* find original wpan device (using init_net) */ + dev_wpan = dev_get_by_name(dev_net(dev_lowpan), real_dev_name); + if (dev_wpan == NULL) { + pr_debug("(%s) ERROR: no real device found\n", __func__); + err = -ENODEV; + goto err; + } + + /* TODO: dev_net_set(dev_lowpan, dev_net(dev_wpan));*/ + + lowpan_dev_info(dev_lowpan)->id = 1; /* ??? */ + lowpan_dev_info(dev_lowpan)->real_dev = dev_wpan; + + err = register_netdevice(dev_lowpan); + if (err < 0) + goto err1; + return 0; + +err1: + dev_put(dev_wpan); +err: + free_netdev(dev_lowpan); + return err; +} + +static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev, + struct packet_type *pt, struct net_device *orig_dev) +{ + if (!netif_running(dev)) + goto drop; + + if (dev->type != ARPHRD_IEEE802154) + goto drop; + + /* TODO: is it enough? */ + if ((skb->data[0] & 0xe0) == 0x60) + lowpan_process_data(dev, skb); + return NET_RX_SUCCESS; + +drop: + kfree_skb(skb); + return NET_RX_DROP; +} + +static struct packet_type lowpan_packet_type = { + .type = __constant_htons(ETH_P_IEEE802154), + .func = lowpan_rcv, +}; + +static int __init lowpan_init_module(void) +{ + int err = 0; + + pr_debug("(%s)\n", __func__); + + rtnl_lock(); + + /* register rtnetlink operations */ + err = __rtnl_link_register(&lowpan_link_ops); + + /* register and setup net device */ + err = lowpan_init_one(); + + /* add new packet type for rcv ability */ + dev_add_pack(&lowpan_packet_type); + + if (err < 0) + __rtnl_link_unregister(&lowpan_link_ops); + + rtnl_unlock(); + + return err; +} + +static void __exit lowpan_cleanup_module(void) +{ + pr_debug("(%s)\n", __func__); + + dev_remove_pack(&lowpan_packet_type); + rtnl_link_unregister(&lowpan_link_ops); +} + +module_init(lowpan_init_module); +module_exit(lowpan_cleanup_module); +MODULE_LICENSE("GPL"); +MODULE_ALIAS_RTNL_LINK("lowpan"); diff --git a/net/ieee802154/6lowpan.h b/net/ieee802154/6lowpan.h new file mode 100644 index 0000000..0e35b7b --- /dev/null +++ b/net/ieee802154/6lowpan.h @@ -0,0 +1,232 @@ +/* + * Copyright 2011, Siemens AG + * written by Alexander Smirnov <[email protected]> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 + * as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +/* + * Based on patches from Jon Smirl <[email protected]> who derived code + * from Contiki - http://www.sics.se/contiki + * + * Please find below the Contiki license: + * + * Copyright (c) 2008, Swedish Institute of Computer Science. + * All rights reserved. + * + * 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 of the Institute 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 INSTITUTE 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 INSTITUTE 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. + */ + +#ifndef __6LOWPAN_H__ +#define __6LOWPAN_H__ + +struct lowpan_dev_info { + u16 flags; /* for future using */ + u16 id; /* for future using */ + + struct net_device *real_dev; +}; + +static inline struct lowpan_dev_info *lowpan_dev_info(const struct net_device *dev) +{ + return netdev_priv(dev); +} + +#define UIP_802154_SHORTADDR_LEN 2 /* compressed ipv6 address length */ +#define UIP_IPH_LEN 40 /* ipv6 fixed header size */ +#define UIP_PROTO_UDP 17 /* ipv6 next header value for UDP */ +#define UIP_FRAGH_LEN 8 /* ipv6 fragment header size */ + +/* ipv6 address based on mac */ +#define is_addr_mac_addr_based(a, m) \ + ((((a)->s6_addr[8]) == (((m)[0]) ^ 0x02)) && \ + (((a)->s6_addr[9]) == (m)[1]) && \ + (((a)->s6_addr[10]) == (m)[2]) && \ + (((a)->s6_addr[11]) == (m)[3]) && \ + (((a)->s6_addr[12]) == (m)[4]) && \ + (((a)->s6_addr[13]) == (m)[5]) && \ + (((a)->s6_addr[14]) == (m)[6]) && \ + (((a)->s6_addr[15]) == (m)[7])) + +/* ipv6 address is unspecified */ +#define is_addr_unspecified(a) \ + ((((a)->s6_addr32[0]) == 0) && \ + (((a)->s6_addr32[1]) == 0) && \ + (((a)->s6_addr32[2]) == 0) && \ + (((a)->s6_addr32[3]) == 0)) + +/* compare ipv6 addresses prefixes */ +#define ipaddr_prefixcmp(addr1, addr2, length) (memcmp(addr1, addr2, length>>3) == 0) + +/* local link, i.e. FE80::/10 */ +#define is_addr_link_local(a) (((a)->s6_addr16[0]) == 0x80FE) + +/* + * check whether we can compress the IID to 16 bits, + * it's possible for unicast adresses with first 49 bits are zero only. + */ +#define lowpan_is_iid_16_bit_compressable(a) \ + ((((a)->s6_addr16[4]) == 0) && \ + (((a)->s6_addr16[5]) == 0) && \ + (((a)->s6_addr16[6]) == 0) && \ + ((((a)->s6_addr[14]) & 0x80) == 0)) + +/* multicast address */ +#define is_addr_mcast(a) (((a)->s6_addr[0]) == 0xFF) + +/* check whether the 112-bit group-id of the multicast address is mappable to: */ + +/* 9 bits, for FF02::1 (all nodes) and FF02::2 (all routers) addresses only. */ +#define lowpan_is_mcast_addr_compressable(a) \ + ((((a)->s6_addr16[1]) == 0) && \ + (((a)->s6_addr16[2]) == 0) && \ + (((a)->s6_addr16[3]) == 0) && \ + (((a)->s6_addr16[4]) == 0) && \ + (((a)->s6_addr16[5]) == 0) && \ + (((a)->s6_addr16[6]) == 0) && \ + (((a)->s6_addr[14]) == 0) && \ + ((((a)->s6_addr[15]) == 1) || (((a)->s6_addr[15]) == 2))) + +/* 48 bits, FFXX::00XX:XXXX:XXXX */ +#define lowpan_is_mcast_addr_compressable48(a) \ + ((((a)->s6_addr16[1]) == 0) && \ + (((a)->s6_addr16[2]) == 0) && \ + (((a)->s6_addr16[3]) == 0) && \ + (((a)->s6_addr16[4]) == 0) && \ + (((a)->s6_addr[10]) == 0)) + +/* 32 bits, FFXX::00XX:XXXX */ +#define lowpan_is_mcast_addr_compressable32(a) \ + ((((a)->s6_addr16[1]) == 0) && \ + (((a)->s6_addr16[2]) == 0) && \ + (((a)->s6_addr16[3]) == 0) && \ + (((a)->s6_addr16[4]) == 0) && \ + (((a)->s6_addr16[5]) == 0) && \ + (((a)->s6_addr[12]) == 0)) + +/* 8 bits, FF02::00XX */ +#define lowpan_is_mcast_addr_compressable8(a) \ + ((((a)->s6_addr[1]) == 2) && \ + (((a)->s6_addr16[1]) == 0) && \ + (((a)->s6_addr16[2]) == 0) && \ + (((a)->s6_addr16[3]) == 0) && \ + (((a)->s6_addr16[4]) == 0) && \ + (((a)->s6_addr16[5]) == 0) && \ + (((a)->s6_addr16[6]) == 0) && \ + (((a)->s6_addr[14]) == 0)) + +#define lowpan_is_addr_broadcast(a) \ + ((((a)[0]) == 0xFF) && \ + (((a)[1]) == 0xFF) && \ + (((a)[2]) == 0xFF) && \ + (((a)[3]) == 0xFF) && \ + (((a)[4]) == 0xFF) && \ + (((a)[5]) == 0xFF) && \ + (((a)[6]) == 0xFF) && \ + (((a)[7]) == 0xFF)) + +/* + * An address context for IPHC address compression, each context can + * have upto 8 bytes + */ +struct lowpan_addr_context { + u8 used; /* possibly use as prefix-length */ + u8 number; + u8 prefix[8]; +}; + +#define LOWPAN_DISPATCH_IPV6 0x41 /* 01000001 = 65 */ +#define LOWPAN_DISPATCH_HC1 0x42 /* 01000010 = 66 */ +#define LOWPAN_DISPATCH_IPHC 0x60 /* 011xxxxx = ... */ +#define LOWPAN_DISPATCH_FRAG1 0xc0 /* 11000xxx */ +#define LOWPAN_DISPATCH_FRAGN 0xe0 /* 11100xxx */ + +/* + * Values of fields within the IPHC encoding first byte + * (C stands for compressed and I for inline) + */ +#define LOWPAN_IPHC_TF 0x18 + +#define LOWPAN_IPHC_FL_C 0x10 +#define LOWPAN_IPHC_TC_C 0x08 +#define LOWPAN_IPHC_NH_C 0x04 +#define LOWPAN_IPHC_TTL_1 0x01 +#define LOWPAN_IPHC_TTL_64 0x02 +#define LOWPAN_IPHC_TTL_255 0x03 +#define LOWPAN_IPHC_TTL_I 0x00 + + +/* Values of fields within the IPHC encoding second byte */ +#define LOWPAN_IPHC_CID 0x80 + +#define LOWPAN_IPHC_SAC 0x40 +#define LOWPAN_IPHC_SAM_00 0x00 +#define LOWPAN_IPHC_SAM_01 0x10 +#define LOWPAN_IPHC_SAM_10 0x20 +#define LOWPAN_IPHC_SAM 0x30 + +#define LOWPAN_IPHC_SAM_BIT 4 + +#define LOWPAN_IPHC_M 0x08 +#define LOWPAN_IPHC_DAC 0x04 +#define LOWPAN_IPHC_DAM_00 0x00 +#define LOWPAN_IPHC_DAM_01 0x01 +#define LOWPAN_IPHC_DAM_10 0x02 +#define LOWPAN_IPHC_DAM_11 0x03 + +#define LOWPAN_IPHC_DAM_BIT 0 +/* + * LOWPAN_UDP encoding (works together with IPHC) + */ +#define LOWPAN_NHC_UDP_MASK 0xF8 +#define LOWPAN_NHC_UDP_ID 0xF0 +#define LOWPAN_NHC_UDP_CHECKSUMC 0x04 +#define LOWPAN_NHC_UDP_CHECKSUMI 0x00 + +/* values for port compression, _with checksum_ ie bit 5 set to 0 */ +#define LOWPAN_NHC_UDP_CS_P_00 0xF0 /* all inline */ +#define LOWPAN_NHC_UDP_CS_P_01 0xF1 /* source 16bit inline, dest = 0xF0 + 8 bit inline */ +#define LOWPAN_NHC_UDP_CS_P_10 0xF2 /* source = 0xF0 + 8bit inline, dest = 16 bit inline */ +#define LOWPAN_NHC_UDP_CS_P_11 0xF3 /* source & dest = 0xF0B + 4bit inline */ + +#define data_swap(src, dest) \ +{ \ + int i; \ + for (i = 0; i < IEEE802154_ADDR_LEN; i++) \ + (dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i]; \ +} + +#endif /* __6LOWPAN_H__ */ diff --git a/net/ieee802154/Kconfig b/net/ieee802154/Kconfig index 1c1de97..7dee650 100644 --- a/net/ieee802154/Kconfig +++ b/net/ieee802154/Kconfig @@ -10,3 +10,9 @@ config IEEE802154 Say Y here to compile LR-WPAN support into the kernel or say M to compile it as modules. + +config IEEE802154_6LOWPAN + tristate "6lowpan support over IEEE 802.15.4" + depends on IEEE802154 && IPV6 + ---help--- + IPv6 compression over IEEE 802.15.4. diff --git a/net/ieee802154/Makefile b/net/ieee802154/Makefile index ce2d335..c8e6cda 100644 --- a/net/ieee802154/Makefile +++ b/net/ieee802154/Makefile @@ -1,5 +1,7 @@ -obj-$(CONFIG_IEEE802154) += ieee802154.o af_802154.o -ieee802154-y := netlink.o nl-mac.o nl-phy.o nl_policy.o wpan-class.o -af_802154-y := af_ieee802154.o raw.o dgram.o +obj-$(CONFIG_IEEE802154) += ieee802154.o af_802154.o +obj-$(CONFIG_IEEE802154_6LOWPAN) += 6lowpan.o + +ieee802154-y := netlink.o nl-mac.o nl-phy.o nl_policy.o wpan-class.o +af_802154-y := af_ieee802154.o raw.o dgram.o ccflags-y += -Wall -DDEBUG -- 1.7.2.3 ------------------------------------------------------------------------------ All of the data generated in your IT infrastructure is seriously valuable. Why? It contains a definitive record of application performance, security threats, fraudulent activity, and more. Splunk takes this data and makes sense of it. IT sense. And common sense. http://p.sf.net/sfu/splunk-d2d-c2 _______________________________________________ Linux-zigbee-devel mailing list [email protected] https://lists.sourceforge.net/lists/listinfo/linux-zigbee-devel
