From 4617721c502b2ddaa4e324e86da4997edf738fa5 Mon Sep 17 00:00:00 2001 From: Steven Rostedt Date: Wed, 2 Sep 2026 09:55:01 -0400 Subject: ftrace: Synchronize the initialization of ftrace_ops There's some internal state that ftrace_ops needs to have set, but since it can be declared outside of the ftrace.c code, it calls ftrace_ops_init() on the ops in every global function. The issue is that if two tasks call it on the same ops at the same time it is possible to have the initialization of one corrupt the initialization of the other call. Create a ops_mutex to use to synchronize every initialization of the ftrace_ops. The mutex is taken within checking the ftrace_ops flag that states it was initializied but the flag is checked again after the mutex has been taken. Checking first outside the mutex allows it to shortcut having to take the mutex. But then the check needs to be done again after the mute is taken in case of races. Cc: stable@vger.kernel.org Link: https://patch.msgid.link/20260902095501.6b59af20@gandalf.local.home Fixes: f04f24fb7e48d ("ftrace, kprobes: Fix a deadlock on ftrace_regex_lock") Reported-by: sashiko-bot@kernel.org Close: https://lore.kernel.org/all/20260829025528.49A831F000E9@smtp.kernel.org/ Signed-off-by: Steven Rostedt --- net/6lowpan/6lowpan_i.h | 34 + net/6lowpan/Kconfig | 105 ++++ net/6lowpan/Makefile | 22 + net/6lowpan/core.c | 183 ++++++ net/6lowpan/debugfs.c | 278 +++++++++ net/6lowpan/iphc.c | 1313 +++++++++++++++++++++++++++++++++++++++ net/6lowpan/ndisc.c | 223 +++++++ net/6lowpan/nhc.c | 169 +++++ net/6lowpan/nhc.h | 133 ++++ net/6lowpan/nhc_dest.c | 17 + net/6lowpan/nhc_fragment.c | 16 + net/6lowpan/nhc_ghc_ext_dest.c | 16 + net/6lowpan/nhc_ghc_ext_frag.c | 17 + net/6lowpan/nhc_ghc_ext_hop.c | 16 + net/6lowpan/nhc_ghc_ext_route.c | 16 + net/6lowpan/nhc_ghc_icmpv6.c | 16 + net/6lowpan/nhc_ghc_udp.c | 16 + net/6lowpan/nhc_hop.c | 16 + net/6lowpan/nhc_ipv6.c | 16 + net/6lowpan/nhc_mobility.c | 16 + net/6lowpan/nhc_routing.c | 16 + net/6lowpan/nhc_udp.c | 176 ++++++ 22 files changed, 2830 insertions(+) create mode 100644 net/6lowpan/6lowpan_i.h create mode 100644 net/6lowpan/Kconfig create mode 100644 net/6lowpan/Makefile create mode 100644 net/6lowpan/core.c create mode 100644 net/6lowpan/debugfs.c create mode 100644 net/6lowpan/iphc.c create mode 100644 net/6lowpan/ndisc.c create mode 100644 net/6lowpan/nhc.c create mode 100644 net/6lowpan/nhc.h create mode 100644 net/6lowpan/nhc_dest.c create mode 100644 net/6lowpan/nhc_fragment.c create mode 100644 net/6lowpan/nhc_ghc_ext_dest.c create mode 100644 net/6lowpan/nhc_ghc_ext_frag.c create mode 100644 net/6lowpan/nhc_ghc_ext_hop.c create mode 100644 net/6lowpan/nhc_ghc_ext_route.c create mode 100644 net/6lowpan/nhc_ghc_icmpv6.c create mode 100644 net/6lowpan/nhc_ghc_udp.c create mode 100644 net/6lowpan/nhc_hop.c create mode 100644 net/6lowpan/nhc_ipv6.c create mode 100644 net/6lowpan/nhc_mobility.c create mode 100644 net/6lowpan/nhc_routing.c create mode 100644 net/6lowpan/nhc_udp.c (limited to 'net/6lowpan') diff --git a/net/6lowpan/6lowpan_i.h b/net/6lowpan/6lowpan_i.h new file mode 100644 index 000000000..01853cec0 --- /dev/null +++ b/net/6lowpan/6lowpan_i.h @@ -0,0 +1,34 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +#ifndef __6LOWPAN_I_H +#define __6LOWPAN_I_H + +#include + +#include + +/* caller need to be sure it's dev->type is ARPHRD_6LOWPAN */ +static inline bool lowpan_is_ll(const struct net_device *dev, + enum lowpan_lltypes lltype) +{ + return lowpan_dev(dev)->lltype == lltype; +} + +extern const struct ndisc_ops lowpan_ndisc_ops; + +int addrconf_ifid_802154_6lowpan(u8 *eui, struct net_device *dev); + +#ifdef CONFIG_6LOWPAN_DEBUGFS +void lowpan_dev_debugfs_init(struct net_device *dev); +void lowpan_dev_debugfs_exit(struct net_device *dev); + +void __init lowpan_debugfs_init(void); +void lowpan_debugfs_exit(void); +#else +static inline void lowpan_dev_debugfs_init(struct net_device *dev) { } +static inline void lowpan_dev_debugfs_exit(struct net_device *dev) { } + +static inline void __init lowpan_debugfs_init(void) { } +static inline void lowpan_debugfs_exit(void) { } +#endif /* CONFIG_6LOWPAN_DEBUGFS */ + +#endif /* __6LOWPAN_I_H */ diff --git a/net/6lowpan/Kconfig b/net/6lowpan/Kconfig new file mode 100644 index 000000000..d8fc45949 --- /dev/null +++ b/net/6lowpan/Kconfig @@ -0,0 +1,105 @@ +# SPDX-License-Identifier: GPL-2.0-only +menuconfig 6LOWPAN + tristate "6LoWPAN Support" + depends on IPV6 + help + This enables IPv6 over Low power Wireless Personal Area Network - + "6LoWPAN" which is supported by IEEE 802.15.4 or Bluetooth stacks. + +config 6LOWPAN_DEBUGFS + bool "6LoWPAN debugfs support" + depends on 6LOWPAN + depends on DEBUG_FS + help + This enables 6LoWPAN debugfs support. For example to manipulate + IPHC context information at runtime. + +menuconfig 6LOWPAN_NHC + tristate "Next Header and Generic Header Compression Support" + depends on 6LOWPAN + default y + help + Support for next header and generic header compression defined in + RFC6282 and RFC7400. + +if 6LOWPAN_NHC + +config 6LOWPAN_NHC_DEST + tristate "Destination Options Header Support" + default y + help + 6LoWPAN IPv6 Destination Options Header compression according to + RFC6282. + +config 6LOWPAN_NHC_FRAGMENT + tristate "Fragment Header Support" + default y + help + 6LoWPAN IPv6 Fragment Header compression according to RFC6282. + +config 6LOWPAN_NHC_HOP + tristate "Hop-by-Hop Options Header Support" + default y + help + 6LoWPAN IPv6 Hop-by-Hop Options Header compression according to + RFC6282. + +config 6LOWPAN_NHC_IPV6 + tristate "IPv6 Header Support" + default y + help + 6LoWPAN IPv6 Header compression according to RFC6282. + +config 6LOWPAN_NHC_MOBILITY + tristate "Mobility Header Support" + default y + help + 6LoWPAN IPv6 Mobility Header compression according to RFC6282. + +config 6LOWPAN_NHC_ROUTING + tristate "Routing Header Support" + default y + help + 6LoWPAN IPv6 Routing Header compression according to RFC6282. + +config 6LOWPAN_NHC_UDP + tristate "UDP Header Support" + default y + help + 6LoWPAN IPv6 UDP Header compression according to RFC6282. + +config 6LOWPAN_GHC_EXT_HDR_HOP + tristate "GHC Hop-by-Hop Options Header Support" + help + 6LoWPAN IPv6 Hop-by-Hop option generic header compression according + to RFC7400. + +config 6LOWPAN_GHC_UDP + tristate "GHC UDP Support" + help + 6LoWPAN IPv6 UDP generic header compression according to RFC7400. + +config 6LOWPAN_GHC_ICMPV6 + tristate "GHC ICMPv6 Support" + help + 6LoWPAN IPv6 ICMPv6 generic header compression according to RFC7400. + +config 6LOWPAN_GHC_EXT_HDR_DEST + tristate "GHC Destination Options Header Support" + help + 6LoWPAN IPv6 destination option generic header compression according + to RFC7400. + +config 6LOWPAN_GHC_EXT_HDR_FRAG + tristate "GHC Fragmentation Options Header Support" + help + 6LoWPAN IPv6 fragmentation option generic header compression + according to RFC7400. + +config 6LOWPAN_GHC_EXT_HDR_ROUTE + tristate "GHC Routing Options Header Support" + help + 6LoWPAN IPv6 routing option generic header compression according + to RFC7400. + +endif diff --git a/net/6lowpan/Makefile b/net/6lowpan/Makefile new file mode 100644 index 000000000..2247b96db --- /dev/null +++ b/net/6lowpan/Makefile @@ -0,0 +1,22 @@ +# SPDX-License-Identifier: GPL-2.0 +obj-$(CONFIG_6LOWPAN) += 6lowpan.o + +6lowpan-y := core.o iphc.o nhc.o ndisc.o +6lowpan-$(CONFIG_6LOWPAN_DEBUGFS) += debugfs.o + +#rfc6282 nhcs +obj-$(CONFIG_6LOWPAN_NHC_DEST) += nhc_dest.o +obj-$(CONFIG_6LOWPAN_NHC_FRAGMENT) += nhc_fragment.o +obj-$(CONFIG_6LOWPAN_NHC_HOP) += nhc_hop.o +obj-$(CONFIG_6LOWPAN_NHC_IPV6) += nhc_ipv6.o +obj-$(CONFIG_6LOWPAN_NHC_MOBILITY) += nhc_mobility.o +obj-$(CONFIG_6LOWPAN_NHC_ROUTING) += nhc_routing.o +obj-$(CONFIG_6LOWPAN_NHC_UDP) += nhc_udp.o + +#rfc7400 ghcs +obj-$(CONFIG_6LOWPAN_GHC_EXT_HDR_HOP) += nhc_ghc_ext_hop.o +obj-$(CONFIG_6LOWPAN_GHC_UDP) += nhc_ghc_udp.o +obj-$(CONFIG_6LOWPAN_GHC_ICMPV6) += nhc_ghc_icmpv6.o +obj-$(CONFIG_6LOWPAN_GHC_EXT_HDR_DEST) += nhc_ghc_ext_dest.o +obj-$(CONFIG_6LOWPAN_GHC_EXT_HDR_FRAG) += nhc_ghc_ext_frag.o +obj-$(CONFIG_6LOWPAN_GHC_EXT_HDR_ROUTE) += nhc_ghc_ext_route.o diff --git a/net/6lowpan/core.c b/net/6lowpan/core.c new file mode 100644 index 000000000..850d4a185 --- /dev/null +++ b/net/6lowpan/core.c @@ -0,0 +1,183 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * + * Authors: + * (C) 2015 Pengutronix, Alexander Aring + */ + +#include +#include + +#include +#include + +#include "6lowpan_i.h" + +int lowpan_register_netdevice(struct net_device *dev, + enum lowpan_lltypes lltype) +{ + int i, ret; + + switch (lltype) { + case LOWPAN_LLTYPE_IEEE802154: + dev->addr_len = EUI64_ADDR_LEN; + break; + + case LOWPAN_LLTYPE_BTLE: + dev->addr_len = ETH_ALEN; + break; + } + + dev->type = ARPHRD_6LOWPAN; + dev->mtu = IPV6_MIN_MTU; + + lowpan_dev(dev)->lltype = lltype; + + spin_lock_init(&lowpan_dev(dev)->ctx.lock); + for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) + lowpan_dev(dev)->ctx.table[i].id = i; + + dev->ndisc_ops = &lowpan_ndisc_ops; + + ret = register_netdevice(dev); + if (ret < 0) + return ret; + + lowpan_dev_debugfs_init(dev); + + return ret; +} +EXPORT_SYMBOL(lowpan_register_netdevice); + +int lowpan_register_netdev(struct net_device *dev, + enum lowpan_lltypes lltype) +{ + int ret; + + rtnl_lock(); + ret = lowpan_register_netdevice(dev, lltype); + rtnl_unlock(); + return ret; +} +EXPORT_SYMBOL(lowpan_register_netdev); + +void lowpan_unregister_netdevice(struct net_device *dev) +{ + unregister_netdevice(dev); + lowpan_dev_debugfs_exit(dev); +} +EXPORT_SYMBOL(lowpan_unregister_netdevice); + +void lowpan_unregister_netdev(struct net_device *dev) +{ + rtnl_lock(); + lowpan_unregister_netdevice(dev); + rtnl_unlock(); +} +EXPORT_SYMBOL(lowpan_unregister_netdev); + +int addrconf_ifid_802154_6lowpan(u8 *eui, struct net_device *dev) +{ + struct wpan_dev *wpan_dev = lowpan_802154_dev(dev)->wdev->ieee802154_ptr; + + /* Set short_addr autoconfiguration if short_addr is present only */ + if (!lowpan_802154_is_valid_src_short_addr(wpan_dev->short_addr)) + return -1; + + /* For either address format, all zero addresses MUST NOT be used */ + if (wpan_dev->pan_id == cpu_to_le16(0x0000) && + wpan_dev->short_addr == cpu_to_le16(0x0000)) + return -1; + + /* Alternatively, if no PAN ID is known, 16 zero bits may be used */ + if (wpan_dev->pan_id == cpu_to_le16(IEEE802154_PAN_ID_BROADCAST)) + memset(eui, 0, 2); + else + ieee802154_le16_to_be16(eui, &wpan_dev->pan_id); + + /* The "Universal/Local" (U/L) bit shall be set to zero */ + eui[0] &= ~2; + eui[2] = 0; + eui[3] = 0xFF; + eui[4] = 0xFE; + eui[5] = 0; + ieee802154_le16_to_be16(&eui[6], &wpan_dev->short_addr); + return 0; +} + +static int lowpan_event(struct notifier_block *unused, + unsigned long event, void *ptr) +{ + struct net_device *dev = netdev_notifier_info_to_dev(ptr); + struct inet6_dev *idev; + struct in6_addr addr; + int i; + + if (dev->type != ARPHRD_6LOWPAN) + return NOTIFY_DONE; + + idev = __in6_dev_get(dev); + if (!idev) + return NOTIFY_DONE; + + switch (event) { + case NETDEV_UP: + case NETDEV_CHANGE: + /* (802.15.4 6LoWPAN short address slaac handling */ + if (lowpan_is_ll(dev, LOWPAN_LLTYPE_IEEE802154) && + addrconf_ifid_802154_6lowpan(addr.s6_addr + 8, dev) == 0) { + __ipv6_addr_set_half(&addr.s6_addr32[0], + htonl(0xFE800000), 0); + addrconf_add_linklocal(idev, &addr, 0); + } + break; + case NETDEV_DOWN: + for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) + clear_bit(LOWPAN_IPHC_CTX_FLAG_ACTIVE, + &lowpan_dev(dev)->ctx.table[i].flags); + break; + default: + return NOTIFY_DONE; + } + + return NOTIFY_OK; +} + +static struct notifier_block lowpan_notifier = { + .notifier_call = lowpan_event, +}; + +static int __init lowpan_module_init(void) +{ + int ret; + + lowpan_debugfs_init(); + + ret = register_netdevice_notifier(&lowpan_notifier); + if (ret < 0) { + lowpan_debugfs_exit(); + return ret; + } + + request_module_nowait("nhc_dest"); + request_module_nowait("nhc_fragment"); + request_module_nowait("nhc_hop"); + request_module_nowait("nhc_ipv6"); + request_module_nowait("nhc_mobility"); + request_module_nowait("nhc_routing"); + request_module_nowait("nhc_udp"); + + return 0; +} + +static void __exit lowpan_module_exit(void) +{ + lowpan_debugfs_exit(); + unregister_netdevice_notifier(&lowpan_notifier); +} + +module_init(lowpan_module_init); +module_exit(lowpan_module_exit); + +MODULE_DESCRIPTION("IPv6 over Low-Power Wireless Personal Area Network core module"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/debugfs.c b/net/6lowpan/debugfs.c new file mode 100644 index 000000000..600b9563b --- /dev/null +++ b/net/6lowpan/debugfs.c @@ -0,0 +1,278 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * + * Authors: + * (C) 2015 Pengutronix, Alexander Aring + * Copyright (c) 2015 Nordic Semiconductor. All Rights Reserved. + */ + +#include + +#include "6lowpan_i.h" + +#define LOWPAN_DEBUGFS_CTX_PFX_NUM_ARGS 8 + +static struct dentry *lowpan_debugfs; + +static int lowpan_ctx_flag_active_set(void *data, u64 val) +{ + struct lowpan_iphc_ctx *ctx = data; + + if (val != 0 && val != 1) + return -EINVAL; + + if (val) + set_bit(LOWPAN_IPHC_CTX_FLAG_ACTIVE, &ctx->flags); + else + clear_bit(LOWPAN_IPHC_CTX_FLAG_ACTIVE, &ctx->flags); + + return 0; +} + +static int lowpan_ctx_flag_active_get(void *data, u64 *val) +{ + *val = lowpan_iphc_ctx_is_active(data); + return 0; +} + +DEFINE_DEBUGFS_ATTRIBUTE(lowpan_ctx_flag_active_fops, + lowpan_ctx_flag_active_get, + lowpan_ctx_flag_active_set, "%llu\n"); + +static int lowpan_ctx_flag_c_set(void *data, u64 val) +{ + struct lowpan_iphc_ctx *ctx = data; + + if (val != 0 && val != 1) + return -EINVAL; + + if (val) + set_bit(LOWPAN_IPHC_CTX_FLAG_COMPRESSION, &ctx->flags); + else + clear_bit(LOWPAN_IPHC_CTX_FLAG_COMPRESSION, &ctx->flags); + + return 0; +} + +static int lowpan_ctx_flag_c_get(void *data, u64 *val) +{ + *val = lowpan_iphc_ctx_is_compression(data); + return 0; +} + +DEFINE_DEBUGFS_ATTRIBUTE(lowpan_ctx_flag_c_fops, lowpan_ctx_flag_c_get, + lowpan_ctx_flag_c_set, "%llu\n"); + +static int lowpan_ctx_plen_set(void *data, u64 val) +{ + struct lowpan_iphc_ctx *ctx = data; + struct lowpan_iphc_ctx_table *t = + container_of(ctx, struct lowpan_iphc_ctx_table, table[ctx->id]); + + if (val > 128) + return -EINVAL; + + spin_lock_bh(&t->lock); + ctx->plen = val; + spin_unlock_bh(&t->lock); + + return 0; +} + +static int lowpan_ctx_plen_get(void *data, u64 *val) +{ + struct lowpan_iphc_ctx *ctx = data; + struct lowpan_iphc_ctx_table *t = + container_of(ctx, struct lowpan_iphc_ctx_table, table[ctx->id]); + + spin_lock_bh(&t->lock); + *val = ctx->plen; + spin_unlock_bh(&t->lock); + return 0; +} + +DEFINE_DEBUGFS_ATTRIBUTE(lowpan_ctx_plen_fops, lowpan_ctx_plen_get, + lowpan_ctx_plen_set, "%llu\n"); + +static int lowpan_ctx_pfx_show(struct seq_file *file, void *offset) +{ + struct lowpan_iphc_ctx *ctx = file->private; + struct lowpan_iphc_ctx_table *t = + container_of(ctx, struct lowpan_iphc_ctx_table, table[ctx->id]); + + spin_lock_bh(&t->lock); + seq_printf(file, "%04x:%04x:%04x:%04x:%04x:%04x:%04x:%04x\n", + be16_to_cpu(ctx->pfx.s6_addr16[0]), + be16_to_cpu(ctx->pfx.s6_addr16[1]), + be16_to_cpu(ctx->pfx.s6_addr16[2]), + be16_to_cpu(ctx->pfx.s6_addr16[3]), + be16_to_cpu(ctx->pfx.s6_addr16[4]), + be16_to_cpu(ctx->pfx.s6_addr16[5]), + be16_to_cpu(ctx->pfx.s6_addr16[6]), + be16_to_cpu(ctx->pfx.s6_addr16[7])); + spin_unlock_bh(&t->lock); + + return 0; +} + +static int lowpan_ctx_pfx_open(struct inode *inode, struct file *file) +{ + return single_open(file, lowpan_ctx_pfx_show, inode->i_private); +} + +static ssize_t lowpan_ctx_pfx_write(struct file *fp, + const char __user *user_buf, size_t count, + loff_t *ppos) +{ + char buf[128] = {}; + struct seq_file *file = fp->private_data; + struct lowpan_iphc_ctx *ctx = file->private; + struct lowpan_iphc_ctx_table *t = + container_of(ctx, struct lowpan_iphc_ctx_table, table[ctx->id]); + int status = count, n, i; + unsigned int addr[8]; + + if (copy_from_user(&buf, user_buf, min_t(size_t, sizeof(buf) - 1, + count))) { + status = -EFAULT; + goto out; + } + + n = sscanf(buf, "%04x:%04x:%04x:%04x:%04x:%04x:%04x:%04x", + &addr[0], &addr[1], &addr[2], &addr[3], &addr[4], + &addr[5], &addr[6], &addr[7]); + if (n != LOWPAN_DEBUGFS_CTX_PFX_NUM_ARGS) { + status = -EINVAL; + goto out; + } + + spin_lock_bh(&t->lock); + for (i = 0; i < 8; i++) + ctx->pfx.s6_addr16[i] = cpu_to_be16(addr[i] & 0xffff); + spin_unlock_bh(&t->lock); + +out: + return status; +} + +static const struct file_operations lowpan_ctx_pfx_fops = { + .open = lowpan_ctx_pfx_open, + .read = seq_read, + .write = lowpan_ctx_pfx_write, + .llseek = seq_lseek, + .release = single_release, +}; + +static void lowpan_dev_debugfs_ctx_init(struct net_device *dev, + struct dentry *ctx, u8 id) +{ + struct lowpan_dev *ldev = lowpan_dev(dev); + struct dentry *root; + char buf[32]; + + if (WARN_ON_ONCE(id >= LOWPAN_IPHC_CTX_TABLE_SIZE)) + return; + + sprintf(buf, "%d", id); + + root = debugfs_create_dir(buf, ctx); + + debugfs_create_file("active", 0644, root, &ldev->ctx.table[id], + &lowpan_ctx_flag_active_fops); + + debugfs_create_file("compression", 0644, root, &ldev->ctx.table[id], + &lowpan_ctx_flag_c_fops); + + debugfs_create_file("prefix", 0644, root, &ldev->ctx.table[id], + &lowpan_ctx_pfx_fops); + + debugfs_create_file("prefix_len", 0644, root, &ldev->ctx.table[id], + &lowpan_ctx_plen_fops); +} + +static int lowpan_context_show(struct seq_file *file, void *offset) +{ + struct lowpan_iphc_ctx_table *t = file->private; + int i; + + seq_printf(file, "%3s|%-43s|%c\n", "cid", "prefix", 'C'); + seq_puts(file, "-------------------------------------------------\n"); + + spin_lock_bh(&t->lock); + for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) { + if (!lowpan_iphc_ctx_is_active(&t->table[i])) + continue; + + seq_printf(file, "%3d|%39pI6c/%-3d|%d\n", t->table[i].id, + &t->table[i].pfx, t->table[i].plen, + lowpan_iphc_ctx_is_compression(&t->table[i])); + } + spin_unlock_bh(&t->lock); + + return 0; +} +DEFINE_SHOW_ATTRIBUTE(lowpan_context); + +static int lowpan_short_addr_get(void *data, u64 *val) +{ + struct wpan_dev *wdev = data; + + rtnl_lock(); + *val = le16_to_cpu(wdev->short_addr); + rtnl_unlock(); + + return 0; +} + +DEFINE_DEBUGFS_ATTRIBUTE(lowpan_short_addr_fops, lowpan_short_addr_get, NULL, + "0x%04llx\n"); + +static void lowpan_dev_debugfs_802154_init(const struct net_device *dev, + struct lowpan_dev *ldev) +{ + struct dentry *root; + + if (!lowpan_is_ll(dev, LOWPAN_LLTYPE_IEEE802154)) + return; + + root = debugfs_create_dir("ieee802154", ldev->iface_debugfs); + + debugfs_create_file("short_addr", 0444, root, + lowpan_802154_dev(dev)->wdev->ieee802154_ptr, + &lowpan_short_addr_fops); +} + +void lowpan_dev_debugfs_init(struct net_device *dev) +{ + struct lowpan_dev *ldev = lowpan_dev(dev); + struct dentry *contexts; + int i; + + /* creating the root */ + ldev->iface_debugfs = debugfs_create_dir(dev->name, lowpan_debugfs); + + contexts = debugfs_create_dir("contexts", ldev->iface_debugfs); + + debugfs_create_file("show", 0644, contexts, &lowpan_dev(dev)->ctx, + &lowpan_context_fops); + + for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) + lowpan_dev_debugfs_ctx_init(dev, contexts, i); + + lowpan_dev_debugfs_802154_init(dev, ldev); +} + +void lowpan_dev_debugfs_exit(struct net_device *dev) +{ + debugfs_remove_recursive(lowpan_dev(dev)->iface_debugfs); +} + +void __init lowpan_debugfs_init(void) +{ + lowpan_debugfs = debugfs_create_dir("6lowpan", NULL); +} + +void lowpan_debugfs_exit(void) +{ + debugfs_remove_recursive(lowpan_debugfs); +} diff --git a/net/6lowpan/iphc.c b/net/6lowpan/iphc.c new file mode 100644 index 000000000..37eaff3f7 --- /dev/null +++ b/net/6lowpan/iphc.c @@ -0,0 +1,1313 @@ +/* + * Copyright 2011, Siemens AG + * written by Alexander Smirnov + */ + +/* Based on patches from Jon Smirl + * Copyright (c) 2011 Jon Smirl + * + * 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. + * + */ + +/* Jon's code is based on 6lowpan implementation for Contiki which is: + * 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 +#include +#include + +#include +#include + +#include "6lowpan_i.h" +#include "nhc.h" + +/* Values of fields within the IPHC encoding first byte */ +#define LOWPAN_IPHC_TF_MASK 0x18 +#define LOWPAN_IPHC_TF_00 0x00 +#define LOWPAN_IPHC_TF_01 0x08 +#define LOWPAN_IPHC_TF_10 0x10 +#define LOWPAN_IPHC_TF_11 0x18 + +#define LOWPAN_IPHC_NH 0x04 + +#define LOWPAN_IPHC_HLIM_MASK 0x03 +#define LOWPAN_IPHC_HLIM_00 0x00 +#define LOWPAN_IPHC_HLIM_01 0x01 +#define LOWPAN_IPHC_HLIM_10 0x02 +#define LOWPAN_IPHC_HLIM_11 0x03 + +/* Values of fields within the IPHC encoding second byte */ +#define LOWPAN_IPHC_CID 0x80 + +#define LOWPAN_IPHC_SAC 0x40 + +#define LOWPAN_IPHC_SAM_MASK 0x30 +#define LOWPAN_IPHC_SAM_00 0x00 +#define LOWPAN_IPHC_SAM_01 0x10 +#define LOWPAN_IPHC_SAM_10 0x20 +#define LOWPAN_IPHC_SAM_11 0x30 + +#define LOWPAN_IPHC_M 0x08 + +#define LOWPAN_IPHC_DAC 0x04 + +#define LOWPAN_IPHC_DAM_MASK 0x03 +#define LOWPAN_IPHC_DAM_00 0x00 +#define LOWPAN_IPHC_DAM_01 0x01 +#define LOWPAN_IPHC_DAM_10 0x02 +#define LOWPAN_IPHC_DAM_11 0x03 + +/* ipv6 address based on mac + * second bit-flip (Universe/Local) is done according RFC2464 + */ +#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])) + +/* check whether we can compress the IID to 16 bits, + * it's possible for unicast addresses with first 49 bits are zero only. + */ +#define lowpan_is_iid_16_bit_compressable(a) \ + ((((a)->s6_addr16[4]) == 0) && \ + (((a)->s6_addr[10]) == 0) && \ + (((a)->s6_addr[11]) == 0xff) && \ + (((a)->s6_addr[12]) == 0xfe) && \ + (((a)->s6_addr[13]) == 0)) + +/* check whether the 112-bit gid of the multicast address is mappable to: */ + +/* 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_linklocal_zero_padded(a) \ + (!(hdr->saddr.s6_addr[1] & 0x3f) && \ + !hdr->saddr.s6_addr16[1] && \ + !hdr->saddr.s6_addr32[1]) + +#define LOWPAN_IPHC_CID_DCI(cid) (cid & 0x0f) +#define LOWPAN_IPHC_CID_SCI(cid) ((cid & 0xf0) >> 4) + +static inline void +lowpan_iphc_uncompress_802154_lladdr(struct in6_addr *ipaddr, + const void *lladdr) +{ + const struct ieee802154_addr *addr = lladdr; + u8 eui64[EUI64_ADDR_LEN]; + + switch (addr->mode) { + case IEEE802154_ADDR_LONG: + ieee802154_le64_to_be64(eui64, &addr->extended_addr); + lowpan_iphc_uncompress_eui64_lladdr(ipaddr, eui64); + break; + case IEEE802154_ADDR_SHORT: + /* fe:80::ff:fe00:XXXX + * \__/ + * short_addr + * + * Universe/Local bit is zero. + */ + ipaddr->s6_addr[0] = 0xFE; + ipaddr->s6_addr[1] = 0x80; + ipaddr->s6_addr[11] = 0xFF; + ipaddr->s6_addr[12] = 0xFE; + ieee802154_le16_to_be16(&ipaddr->s6_addr16[7], + &addr->short_addr); + break; + default: + /* should never handled and filtered by 802154 6lowpan */ + WARN_ON_ONCE(1); + break; + } +} + +static struct lowpan_iphc_ctx * +lowpan_iphc_ctx_get_by_id(const struct net_device *dev, u8 id) +{ + struct lowpan_iphc_ctx *ret = &lowpan_dev(dev)->ctx.table[id]; + + if (!lowpan_iphc_ctx_is_active(ret)) + return NULL; + + return ret; +} + +static struct lowpan_iphc_ctx * +lowpan_iphc_ctx_get_by_addr(const struct net_device *dev, + const struct in6_addr *addr) +{ + struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table; + struct lowpan_iphc_ctx *ret = NULL; + struct in6_addr addr_pfx; + u8 addr_plen; + int i; + + for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) { + /* Check if context is valid. A context that is not valid + * MUST NOT be used for compression. + */ + if (!lowpan_iphc_ctx_is_active(&table[i]) || + !lowpan_iphc_ctx_is_compression(&table[i])) + continue; + + ipv6_addr_prefix(&addr_pfx, addr, table[i].plen); + + /* if prefix len < 64, the remaining bits until 64th bit is + * zero. Otherwise we use table[i]->plen. + */ + if (table[i].plen < 64) + addr_plen = 64; + else + addr_plen = table[i].plen; + + if (ipv6_prefix_equal(&addr_pfx, &table[i].pfx, addr_plen)) { + /* remember first match */ + if (!ret) { + ret = &table[i]; + continue; + } + + /* get the context with longest prefix len */ + if (table[i].plen > ret->plen) + ret = &table[i]; + } + } + + return ret; +} + +static struct lowpan_iphc_ctx * +lowpan_iphc_ctx_get_by_mcast_addr(const struct net_device *dev, + const struct in6_addr *addr) +{ + struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table; + struct lowpan_iphc_ctx *ret = NULL; + struct in6_addr addr_mcast, network_pfx = {}; + int i; + + /* init mcast address with */ + memcpy(&addr_mcast, addr, sizeof(*addr)); + + for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) { + /* Check if context is valid. A context that is not valid + * MUST NOT be used for compression. + */ + if (!lowpan_iphc_ctx_is_active(&table[i]) || + !lowpan_iphc_ctx_is_compression(&table[i])) + continue; + + /* setting plen */ + addr_mcast.s6_addr[3] = table[i].plen; + /* get network prefix to copy into multicast address */ + ipv6_addr_prefix(&network_pfx, &table[i].pfx, + table[i].plen); + /* setting network prefix */ + memcpy(&addr_mcast.s6_addr[4], &network_pfx, 8); + + if (ipv6_addr_equal(addr, &addr_mcast)) { + ret = &table[i]; + break; + } + } + + return ret; +} + +static void lowpan_iphc_uncompress_lladdr(const struct net_device *dev, + struct in6_addr *ipaddr, + const void *lladdr) +{ + switch (dev->addr_len) { + case ETH_ALEN: + lowpan_iphc_uncompress_eui48_lladdr(ipaddr, lladdr); + break; + case EUI64_ADDR_LEN: + lowpan_iphc_uncompress_eui64_lladdr(ipaddr, lladdr); + break; + default: + WARN_ON_ONCE(1); + break; + } +} + +/* Uncompress address function for source and + * destination address(non-multicast). + * + * address_mode is the masked value for sam or dam value + */ +static int lowpan_iphc_uncompress_addr(struct sk_buff *skb, + const struct net_device *dev, + struct in6_addr *ipaddr, + u8 address_mode, const void *lladdr) +{ + bool fail; + + switch (address_mode) { + /* SAM and DAM are the same here */ + case LOWPAN_IPHC_DAM_00: + /* for global link addresses */ + fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); + break; + case LOWPAN_IPHC_SAM_01: + case LOWPAN_IPHC_DAM_01: + /* fe:80::XXXX:XXXX:XXXX:XXXX */ + ipaddr->s6_addr[0] = 0xFE; + ipaddr->s6_addr[1] = 0x80; + fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); + break; + case LOWPAN_IPHC_SAM_10: + case LOWPAN_IPHC_DAM_10: + /* fe:80::ff:fe00:XXXX */ + ipaddr->s6_addr[0] = 0xFE; + ipaddr->s6_addr[1] = 0x80; + ipaddr->s6_addr[11] = 0xFF; + ipaddr->s6_addr[12] = 0xFE; + fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); + break; + case LOWPAN_IPHC_SAM_11: + case LOWPAN_IPHC_DAM_11: + fail = false; + switch (lowpan_dev(dev)->lltype) { + case LOWPAN_LLTYPE_IEEE802154: + lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr); + break; + default: + lowpan_iphc_uncompress_lladdr(dev, ipaddr, lladdr); + break; + } + break; + default: + pr_debug("Invalid address mode value: 0x%x\n", address_mode); + return -EINVAL; + } + + if (fail) { + pr_debug("Failed to fetch skb data\n"); + return -EIO; + } + + raw_dump_inline(NULL, "Reconstructed ipv6 addr is", + ipaddr->s6_addr, 16); + + return 0; +} + +/* Uncompress address function for source context + * based address(non-multicast). + */ +static int lowpan_iphc_uncompress_ctx_addr(struct sk_buff *skb, + const struct net_device *dev, + const struct lowpan_iphc_ctx *ctx, + struct in6_addr *ipaddr, + u8 address_mode, const void *lladdr) +{ + bool fail; + + switch (address_mode) { + /* SAM and DAM are the same here */ + case LOWPAN_IPHC_DAM_00: + fail = false; + /* SAM_00 -> unspec address :: + * Do nothing, address is already :: + * + * DAM 00 -> reserved should never occur. + */ + break; + case LOWPAN_IPHC_SAM_01: + case LOWPAN_IPHC_DAM_01: + fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); + ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); + break; + case LOWPAN_IPHC_SAM_10: + case LOWPAN_IPHC_DAM_10: + ipaddr->s6_addr[11] = 0xFF; + ipaddr->s6_addr[12] = 0xFE; + fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); + ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); + break; + case LOWPAN_IPHC_SAM_11: + case LOWPAN_IPHC_DAM_11: + fail = false; + switch (lowpan_dev(dev)->lltype) { + case LOWPAN_LLTYPE_IEEE802154: + lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr); + break; + default: + lowpan_iphc_uncompress_lladdr(dev, ipaddr, lladdr); + break; + } + ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); + break; + default: + pr_debug("Invalid sam value: 0x%x\n", address_mode); + return -EINVAL; + } + + if (fail) { + pr_debug("Failed to fetch skb data\n"); + return -EIO; + } + + raw_dump_inline(NULL, + "Reconstructed context based ipv6 src addr is", + ipaddr->s6_addr, 16); + + return 0; +} + +/* Uncompress function for multicast destination address, + * when M bit is set. + */ +static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb, + struct in6_addr *ipaddr, + u8 address_mode) +{ + bool fail; + + switch (address_mode) { + case LOWPAN_IPHC_DAM_00: + /* 00: 128 bits. The full address + * is carried in-line. + */ + fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); + break; + case LOWPAN_IPHC_DAM_01: + /* 01: 48 bits. The address takes + * the form ffXX::00XX:XXXX:XXXX. + */ + ipaddr->s6_addr[0] = 0xFF; + fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); + fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); + break; + case LOWPAN_IPHC_DAM_10: + /* 10: 32 bits. The address takes + * the form ffXX::00XX:XXXX. + */ + ipaddr->s6_addr[0] = 0xFF; + fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); + fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); + break; + case LOWPAN_IPHC_DAM_11: + /* 11: 8 bits. The address takes + * the form ff02::00XX. + */ + ipaddr->s6_addr[0] = 0xFF; + ipaddr->s6_addr[1] = 0x02; + fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); + break; + default: + pr_debug("DAM value has a wrong value: 0x%x\n", address_mode); + return -EINVAL; + } + + if (fail) { + pr_debug("Failed to fetch skb data\n"); + return -EIO; + } + + raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", + ipaddr->s6_addr, 16); + + return 0; +} + +static int lowpan_uncompress_multicast_ctx_daddr(struct sk_buff *skb, + struct lowpan_iphc_ctx *ctx, + struct in6_addr *ipaddr, + u8 address_mode) +{ + struct in6_addr network_pfx = {}; + bool fail; + + ipaddr->s6_addr[0] = 0xFF; + fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 2); + fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[12], 4); + if (fail) + return -EIO; + + /* take prefix_len and network prefix from the context */ + ipaddr->s6_addr[3] = ctx->plen; + /* get network prefix to copy into multicast address */ + ipv6_addr_prefix(&network_pfx, &ctx->pfx, ctx->plen); + /* setting network prefix */ + memcpy(&ipaddr->s6_addr[4], &network_pfx, 8); + + return 0; +} + +/* get the ecn values from iphc tf format and set it to ipv6hdr */ +static inline void lowpan_iphc_tf_set_ecn(struct ipv6hdr *hdr, const u8 *tf) +{ + /* get the two higher bits which is ecn */ + u8 ecn = tf[0] & 0xc0; + + /* ECN takes 0x30 in hdr->flow_lbl[0] */ + hdr->flow_lbl[0] |= (ecn >> 2); +} + +/* get the dscp values from iphc tf format and set it to ipv6hdr */ +static inline void lowpan_iphc_tf_set_dscp(struct ipv6hdr *hdr, const u8 *tf) +{ + /* DSCP is at place after ECN */ + u8 dscp = tf[0] & 0x3f; + + /* The four highest bits need to be set at hdr->priority */ + hdr->priority |= ((dscp & 0x3c) >> 2); + /* The two lower bits is part of hdr->flow_lbl[0] */ + hdr->flow_lbl[0] |= ((dscp & 0x03) << 6); +} + +/* get the flow label values from iphc tf format and set it to ipv6hdr */ +static inline void lowpan_iphc_tf_set_lbl(struct ipv6hdr *hdr, const u8 *lbl) +{ + /* flow label is always some array started with lower nibble of + * flow_lbl[0] and followed with two bytes afterwards. Inside inline + * data the flow_lbl position can be different, which will be handled + * by lbl pointer. E.g. case "01" vs "00" the traffic class is 8 bit + * shifted, the different lbl pointer will handle that. + * + * The flow label will started at lower nibble of flow_lbl[0], the + * higher nibbles are part of DSCP + ECN. + */ + hdr->flow_lbl[0] |= lbl[0] & 0x0f; + memcpy(&hdr->flow_lbl[1], &lbl[1], 2); +} + +/* lowpan_iphc_tf_decompress - decompress the traffic class. + * This function will return zero on success, a value lower than zero if + * failed. + */ +static int lowpan_iphc_tf_decompress(struct sk_buff *skb, struct ipv6hdr *hdr, + u8 val) +{ + u8 tf[4]; + + /* Traffic Class and Flow Label */ + switch (val) { + case LOWPAN_IPHC_TF_00: + /* ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) */ + if (lowpan_fetch_skb(skb, tf, 4)) + return -EINVAL; + + /* 1 2 3 + * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |ECN| DSCP | rsv | Flow Label | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + lowpan_iphc_tf_set_ecn(hdr, tf); + lowpan_iphc_tf_set_dscp(hdr, tf); + lowpan_iphc_tf_set_lbl(hdr, &tf[1]); + break; + case LOWPAN_IPHC_TF_01: + /* ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided. */ + if (lowpan_fetch_skb(skb, tf, 3)) + return -EINVAL; + + /* 1 2 + * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |ECN|rsv| Flow Label | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + lowpan_iphc_tf_set_ecn(hdr, tf); + lowpan_iphc_tf_set_lbl(hdr, &tf[0]); + break; + case LOWPAN_IPHC_TF_10: + /* ECN + DSCP (1 byte), Flow Label is elided. */ + if (lowpan_fetch_skb(skb, tf, 1)) + return -EINVAL; + + /* 0 1 2 3 4 5 6 7 + * +-+-+-+-+-+-+-+-+ + * |ECN| DSCP | + * +-+-+-+-+-+-+-+-+ + */ + lowpan_iphc_tf_set_ecn(hdr, tf); + lowpan_iphc_tf_set_dscp(hdr, tf); + break; + case LOWPAN_IPHC_TF_11: + /* Traffic Class and Flow Label are elided */ + break; + default: + WARN_ON_ONCE(1); + return -EINVAL; + } + + return 0; +} + +/* TTL uncompression values */ +static const u8 lowpan_ttl_values[] = { + [LOWPAN_IPHC_HLIM_01] = 1, + [LOWPAN_IPHC_HLIM_10] = 64, + [LOWPAN_IPHC_HLIM_11] = 255, +}; + +int lowpan_header_decompress(struct sk_buff *skb, const struct net_device *dev, + const void *daddr, const void *saddr) +{ + struct ipv6hdr hdr = {}; + struct lowpan_iphc_ctx *ci; + u8 iphc0, iphc1, cid = 0; + int err; + + raw_dump_table(__func__, "raw skb data dump uncompressed", + skb->data, skb->len); + + if (lowpan_fetch_skb(skb, &iphc0, sizeof(iphc0)) || + lowpan_fetch_skb(skb, &iphc1, sizeof(iphc1))) + return -EINVAL; + + hdr.version = 6; + + /* default CID = 0, another if the CID flag is set */ + if (iphc1 & LOWPAN_IPHC_CID) { + if (lowpan_fetch_skb(skb, &cid, sizeof(cid))) + return -EINVAL; + } + + err = lowpan_iphc_tf_decompress(skb, &hdr, + iphc0 & LOWPAN_IPHC_TF_MASK); + if (err < 0) + return err; + + /* Next Header */ + if (!(iphc0 & LOWPAN_IPHC_NH)) { + /* Next header is carried inline */ + if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr))) + return -EINVAL; + + pr_debug("NH flag is set, next header carried inline: %02x\n", + hdr.nexthdr); + } + + /* Hop Limit */ + if ((iphc0 & LOWPAN_IPHC_HLIM_MASK) != LOWPAN_IPHC_HLIM_00) { + hdr.hop_limit = lowpan_ttl_values[iphc0 & LOWPAN_IPHC_HLIM_MASK]; + } else { + if (lowpan_fetch_skb(skb, &hdr.hop_limit, + sizeof(hdr.hop_limit))) + return -EINVAL; + } + + if (iphc1 & LOWPAN_IPHC_SAC) { + spin_lock_bh(&lowpan_dev(dev)->ctx.lock); + ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_SCI(cid)); + if (!ci) { + spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); + return -EINVAL; + } + + pr_debug("SAC bit is set. Handle context based source address.\n"); + err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.saddr, + iphc1 & LOWPAN_IPHC_SAM_MASK, + saddr); + spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); + } else { + /* Source address uncompression */ + pr_debug("source address stateless compression\n"); + err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.saddr, + iphc1 & LOWPAN_IPHC_SAM_MASK, + saddr); + } + + /* Check on error of previous branch */ + if (err) + return -EINVAL; + + switch (iphc1 & (LOWPAN_IPHC_M | LOWPAN_IPHC_DAC)) { + case LOWPAN_IPHC_M | LOWPAN_IPHC_DAC: + skb->pkt_type = PACKET_BROADCAST; + + spin_lock_bh(&lowpan_dev(dev)->ctx.lock); + ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid)); + if (!ci) { + spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); + return -EINVAL; + } + + /* multicast with context */ + pr_debug("dest: context-based mcast compression\n"); + err = lowpan_uncompress_multicast_ctx_daddr(skb, ci, + &hdr.daddr, + iphc1 & LOWPAN_IPHC_DAM_MASK); + spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); + break; + case LOWPAN_IPHC_M: + skb->pkt_type = PACKET_BROADCAST; + + /* multicast */ + err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr, + iphc1 & LOWPAN_IPHC_DAM_MASK); + break; + case LOWPAN_IPHC_DAC: + skb->pkt_type = PACKET_HOST; + + spin_lock_bh(&lowpan_dev(dev)->ctx.lock); + ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid)); + if (!ci) { + spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); + return -EINVAL; + } + + /* Destination address context based uncompression */ + pr_debug("DAC bit is set. Handle context based destination address.\n"); + err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.daddr, + iphc1 & LOWPAN_IPHC_DAM_MASK, + daddr); + spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); + break; + default: + skb->pkt_type = PACKET_HOST; + + err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.daddr, + iphc1 & LOWPAN_IPHC_DAM_MASK, + daddr); + pr_debug("dest: stateless compression mode %d dest %pI6c\n", + iphc1 & LOWPAN_IPHC_DAM_MASK, &hdr.daddr); + break; + } + + if (err) + return -EINVAL; + + /* Next header data uncompression */ + if (iphc0 & LOWPAN_IPHC_NH) { + err = lowpan_nhc_do_uncompression(skb, dev, &hdr); + if (err < 0) + return err; + } else { + err = skb_cow(skb, sizeof(hdr)); + if (unlikely(err)) + return err; + } + + switch (lowpan_dev(dev)->lltype) { + case LOWPAN_LLTYPE_IEEE802154: + if (lowpan_802154_cb(skb)->d_size) + hdr.payload_len = htons(lowpan_802154_cb(skb)->d_size - + sizeof(struct ipv6hdr)); + else + hdr.payload_len = htons(skb->len); + break; + default: + hdr.payload_len = htons(skb->len); + break; + } + + pr_debug("skb headroom size = %d, data length = %d\n", + skb_headroom(skb), skb->len); + + pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" + "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", + hdr.version, ntohs(hdr.payload_len), hdr.nexthdr, + hdr.hop_limit, &hdr.daddr); + + skb_push(skb, sizeof(hdr)); + skb_reset_mac_header(skb); + skb_reset_network_header(skb); + skb_copy_to_linear_data(skb, &hdr, sizeof(hdr)); + + raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr)); + + return 0; +} +EXPORT_SYMBOL_GPL(lowpan_header_decompress); + +static const u8 lowpan_iphc_dam_to_sam_value[] = { + [LOWPAN_IPHC_DAM_00] = LOWPAN_IPHC_SAM_00, + [LOWPAN_IPHC_DAM_01] = LOWPAN_IPHC_SAM_01, + [LOWPAN_IPHC_DAM_10] = LOWPAN_IPHC_SAM_10, + [LOWPAN_IPHC_DAM_11] = LOWPAN_IPHC_SAM_11, +}; + +static inline bool +lowpan_iphc_compress_ctx_802154_lladdr(const struct in6_addr *ipaddr, + const struct lowpan_iphc_ctx *ctx, + const void *lladdr) +{ + const struct ieee802154_addr *addr = lladdr; + unsigned char extended_addr[EUI64_ADDR_LEN]; + bool lladdr_compress = false; + struct in6_addr tmp = {}; + + switch (addr->mode) { + case IEEE802154_ADDR_LONG: + ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr); + /* check for SAM/DAM = 11 */ + memcpy(&tmp.s6_addr[8], &extended_addr, EUI64_ADDR_LEN); + /* second bit-flip (Universe/Local) is done according RFC2464 */ + tmp.s6_addr[8] ^= 0x02; + /* context information are always used */ + ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); + if (ipv6_addr_equal(&tmp, ipaddr)) + lladdr_compress = true; + break; + case IEEE802154_ADDR_SHORT: + tmp.s6_addr[11] = 0xFF; + tmp.s6_addr[12] = 0xFE; + ieee802154_le16_to_be16(&tmp.s6_addr16[7], + &addr->short_addr); + /* context information are always used */ + ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); + if (ipv6_addr_equal(&tmp, ipaddr)) + lladdr_compress = true; + break; + default: + /* should never handled and filtered by 802154 6lowpan */ + WARN_ON_ONCE(1); + break; + } + + return lladdr_compress; +} + +static bool lowpan_iphc_addr_equal(const struct net_device *dev, + const struct lowpan_iphc_ctx *ctx, + const struct in6_addr *ipaddr, + const void *lladdr) +{ + struct in6_addr tmp = {}; + + lowpan_iphc_uncompress_lladdr(dev, &tmp, lladdr); + + if (ctx) + ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); + + return ipv6_addr_equal(&tmp, ipaddr); +} + +static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct net_device *dev, + const struct in6_addr *ipaddr, + const struct lowpan_iphc_ctx *ctx, + const unsigned char *lladdr, bool sam) +{ + struct in6_addr tmp; + u8 dam; + + switch (lowpan_dev(dev)->lltype) { + case LOWPAN_LLTYPE_IEEE802154: + if (lowpan_iphc_compress_ctx_802154_lladdr(ipaddr, ctx, + lladdr)) { + dam = LOWPAN_IPHC_DAM_11; + goto out; + } + break; + default: + if (lowpan_iphc_addr_equal(dev, ctx, ipaddr, lladdr)) { + dam = LOWPAN_IPHC_DAM_11; + goto out; + } + break; + } + + memset(&tmp, 0, sizeof(tmp)); + /* check for SAM/DAM = 10 */ + tmp.s6_addr[11] = 0xFF; + tmp.s6_addr[12] = 0xFE; + memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2); + /* context information are always used */ + ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); + if (ipv6_addr_equal(&tmp, ipaddr)) { + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2); + dam = LOWPAN_IPHC_DAM_10; + goto out; + } + + memset(&tmp, 0, sizeof(tmp)); + /* check for SAM/DAM = 01, should always match */ + memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8); + /* context information are always used */ + ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); + if (ipv6_addr_equal(&tmp, ipaddr)) { + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8); + dam = LOWPAN_IPHC_DAM_01; + goto out; + } + + WARN_ONCE(1, "context found but no address mode matched\n"); + return LOWPAN_IPHC_DAM_00; +out: + + if (sam) + return lowpan_iphc_dam_to_sam_value[dam]; + else + return dam; +} + +static inline bool +lowpan_iphc_compress_802154_lladdr(const struct in6_addr *ipaddr, + const void *lladdr) +{ + const struct ieee802154_addr *addr = lladdr; + unsigned char extended_addr[EUI64_ADDR_LEN]; + bool lladdr_compress = false; + struct in6_addr tmp = {}; + + switch (addr->mode) { + case IEEE802154_ADDR_LONG: + ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr); + if (is_addr_mac_addr_based(ipaddr, extended_addr)) + lladdr_compress = true; + break; + case IEEE802154_ADDR_SHORT: + /* fe:80::ff:fe00:XXXX + * \__/ + * short_addr + * + * Universe/Local bit is zero. + */ + tmp.s6_addr[0] = 0xFE; + tmp.s6_addr[1] = 0x80; + tmp.s6_addr[11] = 0xFF; + tmp.s6_addr[12] = 0xFE; + ieee802154_le16_to_be16(&tmp.s6_addr16[7], + &addr->short_addr); + if (ipv6_addr_equal(&tmp, ipaddr)) + lladdr_compress = true; + break; + default: + /* should never handled and filtered by 802154 6lowpan */ + WARN_ON_ONCE(1); + break; + } + + return lladdr_compress; +} + +static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct net_device *dev, + const struct in6_addr *ipaddr, + const unsigned char *lladdr, bool sam) +{ + u8 dam = LOWPAN_IPHC_DAM_01; + + switch (lowpan_dev(dev)->lltype) { + case LOWPAN_LLTYPE_IEEE802154: + if (lowpan_iphc_compress_802154_lladdr(ipaddr, lladdr)) { + dam = LOWPAN_IPHC_DAM_11; /* 0-bits */ + pr_debug("address compression 0 bits\n"); + goto out; + } + break; + default: + if (lowpan_iphc_addr_equal(dev, NULL, ipaddr, lladdr)) { + dam = LOWPAN_IPHC_DAM_11; + pr_debug("address compression 0 bits\n"); + goto out; + } + + break; + } + + if (lowpan_is_iid_16_bit_compressable(ipaddr)) { + /* compress IID to 16 bits xxxx::XXXX */ + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2); + dam = LOWPAN_IPHC_DAM_10; /* 16-bits */ + raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)", + *hc_ptr - 2, 2); + goto out; + } + + /* do not compress IID => xxxx::IID */ + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8); + raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)", + *hc_ptr - 8, 8); + +out: + + if (sam) + return lowpan_iphc_dam_to_sam_value[dam]; + else + return dam; +} + +/* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */ +static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr) +{ + u8 dscp, ecn; + + /* hdr->priority contains the higher bits of dscp, lower are part of + * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr. + */ + dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6); + /* ECN is at the two lower bits from first nibble of flow_lbl[0] */ + ecn = (hdr->flow_lbl[0] & 0x30); + /* for pretty debug output, also shift ecn to get the ecn value */ + pr_debug("ecn 0x%02x dscp 0x%02x\n", ecn >> 4, dscp); + /* ECN is at 0x30 now, shift it to have ECN + DCSP */ + return (ecn << 2) | dscp; +} + +/* lowpan_iphc_is_flow_lbl_zero - check if flow label is zero */ +static inline bool lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr *hdr) +{ + return ((!(hdr->flow_lbl[0] & 0x0f)) && + !hdr->flow_lbl[1] && !hdr->flow_lbl[2]); +} + +/* lowpan_iphc_tf_compress - compress the traffic class which is set by + * ipv6hdr. Return the corresponding format identifier which is used. + */ +static u8 lowpan_iphc_tf_compress(u8 **hc_ptr, const struct ipv6hdr *hdr) +{ + /* get ecn dscp data in a byteformat as: ECN(hi) + DSCP(lo) */ + u8 tc = lowpan_iphc_get_tc(hdr), tf[4], val; + + /* printout the traffic class in hc format */ + pr_debug("tc 0x%02x\n", tc); + + if (lowpan_iphc_is_flow_lbl_zero(hdr)) { + if (!tc) { + /* 11: Traffic Class and Flow Label are elided. */ + val = LOWPAN_IPHC_TF_11; + } else { + /* 10: ECN + DSCP (1 byte), Flow Label is elided. + * + * 0 1 2 3 4 5 6 7 + * +-+-+-+-+-+-+-+-+ + * |ECN| DSCP | + * +-+-+-+-+-+-+-+-+ + */ + lowpan_push_hc_data(hc_ptr, &tc, sizeof(tc)); + val = LOWPAN_IPHC_TF_10; + } + } else { + /* check if dscp is zero, it's after the first two bit */ + if (!(tc & 0x3f)) { + /* 01: ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided + * + * 1 2 + * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |ECN|rsv| Flow Label | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + memcpy(&tf[0], &hdr->flow_lbl[0], 3); + /* zero the highest 4-bits, contains DCSP + ECN */ + tf[0] &= ~0xf0; + /* set ECN */ + tf[0] |= (tc & 0xc0); + + lowpan_push_hc_data(hc_ptr, tf, 3); + val = LOWPAN_IPHC_TF_01; + } else { + /* 00: ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) + * + * 1 2 3 + * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |ECN| DSCP | rsv | Flow Label | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + memcpy(&tf[0], &tc, sizeof(tc)); + /* highest nibble of flow_lbl[0] is part of DSCP + ECN + * which will be the 4-bit pad and will be filled with + * zeros afterwards. + */ + memcpy(&tf[1], &hdr->flow_lbl[0], 3); + /* zero the 4-bit pad, which is reserved */ + tf[1] &= ~0xf0; + + lowpan_push_hc_data(hc_ptr, tf, 4); + val = LOWPAN_IPHC_TF_00; + } + } + + return val; +} + +static u8 lowpan_iphc_mcast_ctx_addr_compress(u8 **hc_ptr, + const struct lowpan_iphc_ctx *ctx, + const struct in6_addr *ipaddr) +{ + u8 data[6] = {}; + + /* flags/scope, reserved (RIID) */ + memcpy(data, &ipaddr->s6_addr[1], 2); + /* group ID */ + memcpy(&data[2], &ipaddr->s6_addr[12], 4); + lowpan_push_hc_data(hc_ptr, data, 6); + + return LOWPAN_IPHC_DAM_00; +} + +static u8 lowpan_iphc_mcast_addr_compress(u8 **hc_ptr, + const struct in6_addr *ipaddr) +{ + u8 val; + + if (lowpan_is_mcast_addr_compressable8(ipaddr)) { + pr_debug("compressed to 1 octet\n"); + /* use last byte */ + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[15], 1); + val = LOWPAN_IPHC_DAM_11; + } else if (lowpan_is_mcast_addr_compressable32(ipaddr)) { + pr_debug("compressed to 4 octets\n"); + /* second byte + the last three */ + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1); + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[13], 3); + val = LOWPAN_IPHC_DAM_10; + } else if (lowpan_is_mcast_addr_compressable48(ipaddr)) { + pr_debug("compressed to 6 octets\n"); + /* second byte + the last five */ + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1); + lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[11], 5); + val = LOWPAN_IPHC_DAM_01; + } else { + pr_debug("using full address\n"); + lowpan_push_hc_data(hc_ptr, ipaddr->s6_addr, 16); + val = LOWPAN_IPHC_DAM_00; + } + + return val; +} + +int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev, + const void *daddr, const void *saddr) +{ + u8 iphc0, iphc1, *hc_ptr, cid = 0; + struct ipv6hdr *hdr; + u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {}; + struct lowpan_iphc_ctx *dci, *sci, dci_entry, sci_entry; + int ret, ipv6_daddr_type, ipv6_saddr_type; + + if (skb->protocol != htons(ETH_P_IPV6)) + return -EINVAL; + + hdr = ipv6_hdr(skb); + hc_ptr = head + 2; + + pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" + "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", + hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, + hdr->hop_limit, &hdr->daddr); + + raw_dump_table(__func__, "raw skb network header dump", + skb_network_header(skb), sizeof(struct ipv6hdr)); + + /* 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; + + raw_dump_table(__func__, "sending raw skb network uncompressed packet", + skb->data, skb->len); + + ipv6_daddr_type = ipv6_addr_type(&hdr->daddr); + spin_lock_bh(&lowpan_dev(dev)->ctx.lock); + if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) + dci = lowpan_iphc_ctx_get_by_mcast_addr(dev, &hdr->daddr); + else + dci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->daddr); + if (dci) { + memcpy(&dci_entry, dci, sizeof(*dci)); + cid |= dci->id; + } + spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); + + spin_lock_bh(&lowpan_dev(dev)->ctx.lock); + sci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->saddr); + if (sci) { + memcpy(&sci_entry, sci, sizeof(*sci)); + cid |= (sci->id << 4); + } + spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); + + /* if cid is zero it will be compressed */ + if (cid) { + iphc1 |= LOWPAN_IPHC_CID; + lowpan_push_hc_data(&hc_ptr, &cid, sizeof(cid)); + } + + /* Traffic Class, Flow Label compression */ + iphc0 |= lowpan_iphc_tf_compress(&hc_ptr, hdr); + + /* NOTE: payload length is always compressed */ + + /* Check if we provide the nhc format for nexthdr and compression + * functionality. If not nexthdr is handled inline and not compressed. + */ + ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr); + if (ret == -ENOENT) + lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr, + sizeof(hdr->nexthdr)); + else + iphc0 |= LOWPAN_IPHC_NH; + + /* 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_HLIM_01; + break; + case 64: + iphc0 |= LOWPAN_IPHC_HLIM_10; + break; + case 255: + iphc0 |= LOWPAN_IPHC_HLIM_11; + break; + default: + lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit, + sizeof(hdr->hop_limit)); + } + + ipv6_saddr_type = ipv6_addr_type(&hdr->saddr); + /* source address compression */ + if (ipv6_saddr_type == IPV6_ADDR_ANY) { + pr_debug("source address is unspecified, setting SAC\n"); + iphc1 |= LOWPAN_IPHC_SAC; + } else { + if (sci) { + iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev, + &hdr->saddr, + &sci_entry, saddr, + true); + iphc1 |= LOWPAN_IPHC_SAC; + } else { + if (ipv6_saddr_type & IPV6_ADDR_LINKLOCAL && + lowpan_is_linklocal_zero_padded(hdr->saddr)) { + iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev, + &hdr->saddr, + saddr, true); + pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n", + &hdr->saddr, iphc1); + } else { + pr_debug("send the full source address\n"); + lowpan_push_hc_data(&hc_ptr, + hdr->saddr.s6_addr, 16); + } + } + } + + /* destination address compression */ + if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) { + pr_debug("destination address is multicast: "); + iphc1 |= LOWPAN_IPHC_M; + if (dci) { + iphc1 |= lowpan_iphc_mcast_ctx_addr_compress(&hc_ptr, + &dci_entry, + &hdr->daddr); + iphc1 |= LOWPAN_IPHC_DAC; + } else { + iphc1 |= lowpan_iphc_mcast_addr_compress(&hc_ptr, + &hdr->daddr); + } + } else { + if (dci) { + iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev, + &hdr->daddr, + &dci_entry, daddr, + false); + iphc1 |= LOWPAN_IPHC_DAC; + } else { + if (ipv6_daddr_type & IPV6_ADDR_LINKLOCAL && + lowpan_is_linklocal_zero_padded(hdr->daddr)) { + iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev, + &hdr->daddr, + daddr, false); + pr_debug("dest address unicast link-local %pI6c iphc1 0x%02x\n", + &hdr->daddr, iphc1); + } else { + pr_debug("dest address unicast %pI6c\n", + &hdr->daddr); + lowpan_push_hc_data(&hc_ptr, + hdr->daddr.s6_addr, 16); + } + } + } + + /* next header compression */ + if (iphc0 & LOWPAN_IPHC_NH) { + ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr); + if (ret < 0) + return ret; + } + + head[0] = iphc0; + head[1] = iphc1; + + skb_pull(skb, sizeof(struct ipv6hdr)); + skb_reset_transport_header(skb); + memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head); + skb_reset_network_header(skb); + + pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len); + + raw_dump_table(__func__, "raw skb data dump compressed", + skb->data, skb->len); + return 0; +} +EXPORT_SYMBOL_GPL(lowpan_header_compress); diff --git a/net/6lowpan/ndisc.c b/net/6lowpan/ndisc.c new file mode 100644 index 000000000..868d28583 --- /dev/null +++ b/net/6lowpan/ndisc.c @@ -0,0 +1,223 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * + * Authors: + * (C) 2016 Pengutronix, Alexander Aring + */ + +#include +#include +#include + +#include "6lowpan_i.h" + +#if IS_ENABLED(CONFIG_IEEE802154_6LOWPAN) +#define NDISC_802154_SHORT_ADDR_LENGTH 1 +static int lowpan_ndisc_parse_802154_options(const struct net_device *dev, + struct nd_opt_hdr *nd_opt, + struct ndisc_options *ndopts) +{ + switch (nd_opt->nd_opt_len) { + case NDISC_802154_SHORT_ADDR_LENGTH: + if (ndopts->nd_802154_opt_array[nd_opt->nd_opt_type]) + net_dbg_ratelimited("%s: duplicated short addr ND6 option found: type=%d\n", + __func__, nd_opt->nd_opt_type); + else + ndopts->nd_802154_opt_array[nd_opt->nd_opt_type] = nd_opt; + return 1; + default: + /* all others will be handled by ndisc IPv6 option parsing */ + return 0; + } +} + +static int lowpan_ndisc_parse_options(const struct net_device *dev, + struct nd_opt_hdr *nd_opt, + struct ndisc_options *ndopts) +{ + if (!lowpan_is_ll(dev, LOWPAN_LLTYPE_IEEE802154)) + return 0; + + switch (nd_opt->nd_opt_type) { + case ND_OPT_SOURCE_LL_ADDR: + case ND_OPT_TARGET_LL_ADDR: + return lowpan_ndisc_parse_802154_options(dev, nd_opt, ndopts); + default: + return 0; + } +} + +static void lowpan_ndisc_802154_update(struct neighbour *n, u32 flags, + u8 icmp6_type, + const struct ndisc_options *ndopts) +{ + struct lowpan_802154_neigh *neigh = lowpan_802154_neigh(neighbour_priv(n)); + u8 *lladdr_short = NULL; + + switch (icmp6_type) { + case NDISC_ROUTER_SOLICITATION: + case NDISC_ROUTER_ADVERTISEMENT: + case NDISC_NEIGHBOUR_SOLICITATION: + if (ndopts->nd_802154_opts_src_lladdr) { + lladdr_short = __ndisc_opt_addr_data(ndopts->nd_802154_opts_src_lladdr, + IEEE802154_SHORT_ADDR_LEN, 0); + if (!lladdr_short) { + net_dbg_ratelimited("NA: invalid short link-layer address length\n"); + return; + } + } + break; + case NDISC_REDIRECT: + case NDISC_NEIGHBOUR_ADVERTISEMENT: + if (ndopts->nd_802154_opts_tgt_lladdr) { + lladdr_short = __ndisc_opt_addr_data(ndopts->nd_802154_opts_tgt_lladdr, + IEEE802154_SHORT_ADDR_LEN, 0); + if (!lladdr_short) { + net_dbg_ratelimited("NA: invalid short link-layer address length\n"); + return; + } + } + break; + default: + break; + } + + write_lock_bh(&n->lock); + if (lladdr_short) { + ieee802154_be16_to_le16(&neigh->short_addr, lladdr_short); + if (!lowpan_802154_is_valid_src_short_addr(neigh->short_addr)) + neigh->short_addr = cpu_to_le16(IEEE802154_ADDR_SHORT_UNSPEC); + } + write_unlock_bh(&n->lock); +} + +static void lowpan_ndisc_update(const struct net_device *dev, + struct neighbour *n, u32 flags, u8 icmp6_type, + const struct ndisc_options *ndopts) +{ + if (!lowpan_is_ll(dev, LOWPAN_LLTYPE_IEEE802154)) + return; + + /* react on overrides only. TODO check if this is really right. */ + if (flags & NEIGH_UPDATE_F_OVERRIDE) + lowpan_ndisc_802154_update(n, flags, icmp6_type, ndopts); +} + +static int lowpan_ndisc_opt_addr_space(const struct net_device *dev, + u8 icmp6_type, struct neighbour *neigh, + u8 *ha_buf, u8 **ha) +{ + struct lowpan_802154_neigh *n; + struct wpan_dev *wpan_dev; + int addr_space = 0; + + if (!lowpan_is_ll(dev, LOWPAN_LLTYPE_IEEE802154)) + return 0; + + switch (icmp6_type) { + case NDISC_REDIRECT: + n = lowpan_802154_neigh(neighbour_priv(neigh)); + + read_lock_bh(&neigh->lock); + if (lowpan_802154_is_valid_src_short_addr(n->short_addr)) { + memcpy(ha_buf, &n->short_addr, + IEEE802154_SHORT_ADDR_LEN); + read_unlock_bh(&neigh->lock); + addr_space += __ndisc_opt_addr_space(IEEE802154_SHORT_ADDR_LEN, 0); + *ha = ha_buf; + } else { + read_unlock_bh(&neigh->lock); + } + break; + case NDISC_NEIGHBOUR_ADVERTISEMENT: + case NDISC_NEIGHBOUR_SOLICITATION: + case NDISC_ROUTER_SOLICITATION: + wpan_dev = lowpan_802154_dev(dev)->wdev->ieee802154_ptr; + + if (lowpan_802154_is_valid_src_short_addr(wpan_dev->short_addr)) + addr_space = __ndisc_opt_addr_space(IEEE802154_SHORT_ADDR_LEN, 0); + break; + default: + break; + } + + return addr_space; +} + +static void lowpan_ndisc_fill_addr_option(const struct net_device *dev, + struct sk_buff *skb, u8 icmp6_type, + const u8 *ha) +{ + struct wpan_dev *wpan_dev; + __be16 short_addr; + u8 opt_type; + + if (!lowpan_is_ll(dev, LOWPAN_LLTYPE_IEEE802154)) + return; + + switch (icmp6_type) { + case NDISC_REDIRECT: + if (ha) { + ieee802154_le16_to_be16(&short_addr, ha); + __ndisc_fill_addr_option(skb, ND_OPT_TARGET_LL_ADDR, + &short_addr, + IEEE802154_SHORT_ADDR_LEN, 0); + } + return; + case NDISC_NEIGHBOUR_ADVERTISEMENT: + opt_type = ND_OPT_TARGET_LL_ADDR; + break; + case NDISC_ROUTER_SOLICITATION: + case NDISC_NEIGHBOUR_SOLICITATION: + opt_type = ND_OPT_SOURCE_LL_ADDR; + break; + default: + return; + } + + wpan_dev = lowpan_802154_dev(dev)->wdev->ieee802154_ptr; + + if (lowpan_802154_is_valid_src_short_addr(wpan_dev->short_addr)) { + ieee802154_le16_to_be16(&short_addr, + &wpan_dev->short_addr); + __ndisc_fill_addr_option(skb, opt_type, &short_addr, + IEEE802154_SHORT_ADDR_LEN, 0); + } +} + +static void lowpan_ndisc_prefix_rcv_add_addr(struct net *net, + struct net_device *dev, + const struct prefix_info *pinfo, + struct inet6_dev *in6_dev, + struct in6_addr *addr, + int addr_type, u32 addr_flags, + bool sllao, bool tokenized, + __u32 valid_lft, + u32 prefered_lft, + bool dev_addr_generated) +{ + int err; + + /* generates short based address for RA PIO's */ + if (lowpan_is_ll(dev, LOWPAN_LLTYPE_IEEE802154) && dev_addr_generated && + !addrconf_ifid_802154_6lowpan(addr->s6_addr + 8, dev)) { + err = addrconf_prefix_rcv_add_addr(net, dev, pinfo, in6_dev, + addr, addr_type, addr_flags, + sllao, tokenized, valid_lft, + prefered_lft); + if (err) + net_dbg_ratelimited("RA: could not add a short address based address for prefix: %pI6c\n", + &pinfo->prefix); + } +} +#endif + +const struct ndisc_ops lowpan_ndisc_ops = { +#if IS_ENABLED(CONFIG_IEEE802154_6LOWPAN) + .parse_options = lowpan_ndisc_parse_options, + .update = lowpan_ndisc_update, + .opt_addr_space = lowpan_ndisc_opt_addr_space, + .fill_addr_option = lowpan_ndisc_fill_addr_option, + .prefix_rcv_add_addr = lowpan_ndisc_prefix_rcv_add_addr, +#endif +}; diff --git a/net/6lowpan/nhc.c b/net/6lowpan/nhc.c new file mode 100644 index 000000000..a4dde8566 --- /dev/null +++ b/net/6lowpan/nhc.c @@ -0,0 +1,169 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN next header compression + * + * Authors: + * Alexander Aring + */ + +#include + +#include + +#include "nhc.h" + +static const struct lowpan_nhc *lowpan_nexthdr_nhcs[NEXTHDR_MAX + 1]; +static DEFINE_SPINLOCK(lowpan_nhc_lock); + +static const struct lowpan_nhc *lowpan_nhc_by_nhcid(struct sk_buff *skb) +{ + const struct lowpan_nhc *nhc; + int i; + u8 id; + + if (!pskb_may_pull(skb, 1)) + return NULL; + + id = *skb->data; + + for (i = 0; i < NEXTHDR_MAX + 1; i++) { + nhc = lowpan_nexthdr_nhcs[i]; + if (!nhc) + continue; + + if ((id & nhc->idmask) == nhc->id) + return nhc; + } + + return NULL; +} + +int lowpan_nhc_check_compression(struct sk_buff *skb, + const struct ipv6hdr *hdr, u8 **hc_ptr) +{ + const struct lowpan_nhc *nhc; + int ret = 0; + + spin_lock_bh(&lowpan_nhc_lock); + + nhc = lowpan_nexthdr_nhcs[hdr->nexthdr]; + if (!(nhc && nhc->compress)) + ret = -ENOENT; + + spin_unlock_bh(&lowpan_nhc_lock); + + return ret; +} + +int lowpan_nhc_do_compression(struct sk_buff *skb, const struct ipv6hdr *hdr, + u8 **hc_ptr) +{ + int ret; + const struct lowpan_nhc *nhc; + + spin_lock_bh(&lowpan_nhc_lock); + + nhc = lowpan_nexthdr_nhcs[hdr->nexthdr]; + /* check if the nhc module was removed in unlocked part. + * TODO: this is a workaround we should prevent unloading + * of nhc modules while unlocked part, this will always drop + * the lowpan packet but it's very unlikely. + * + * Solution isn't easy because we need to decide at + * lowpan_nhc_check_compression if we do a compression or not. + * Because the inline data which is added to skb, we can't move this + * handling. + */ + if (unlikely(!nhc || !nhc->compress)) { + ret = -EINVAL; + goto out; + } + + /* In the case of RAW sockets the transport header is not set by + * the ip6 stack so we must set it ourselves + */ + if (skb->transport_header == skb->network_header) + skb_set_transport_header(skb, sizeof(struct ipv6hdr)); + + ret = nhc->compress(skb, hc_ptr); + if (ret < 0) + goto out; + + /* skip the transport header */ + skb_pull(skb, nhc->nexthdrlen); + +out: + spin_unlock_bh(&lowpan_nhc_lock); + + return ret; +} + +int lowpan_nhc_do_uncompression(struct sk_buff *skb, + const struct net_device *dev, + struct ipv6hdr *hdr) +{ + const struct lowpan_nhc *nhc; + int ret; + + spin_lock_bh(&lowpan_nhc_lock); + + nhc = lowpan_nhc_by_nhcid(skb); + if (nhc) { + if (nhc->uncompress) { + ret = nhc->uncompress(skb, sizeof(struct ipv6hdr) + + nhc->nexthdrlen); + if (ret < 0) { + spin_unlock_bh(&lowpan_nhc_lock); + return ret; + } + } else { + netdev_warn(dev, "received nhc id for %s which is not implemented.\n", + nhc->name); + spin_unlock_bh(&lowpan_nhc_lock); + return -ENOTSUPP; + } + } else { + spin_unlock_bh(&lowpan_nhc_lock); + netdev_warn(dev, "received unknown nhc id which was not found.\n"); + return -ENOENT; + } + + hdr->nexthdr = nhc->nexthdr; + skb_reset_transport_header(skb); + raw_dump_table(__func__, "raw transport header dump", + skb_transport_header(skb), nhc->nexthdrlen); + + spin_unlock_bh(&lowpan_nhc_lock); + + return 0; +} + +int lowpan_nhc_add(const struct lowpan_nhc *nhc) +{ + int ret = 0; + + spin_lock_bh(&lowpan_nhc_lock); + + if (lowpan_nexthdr_nhcs[nhc->nexthdr]) { + ret = -EEXIST; + goto out; + } + + lowpan_nexthdr_nhcs[nhc->nexthdr] = nhc; +out: + spin_unlock_bh(&lowpan_nhc_lock); + return ret; +} +EXPORT_SYMBOL(lowpan_nhc_add); + +void lowpan_nhc_del(const struct lowpan_nhc *nhc) +{ + spin_lock_bh(&lowpan_nhc_lock); + + lowpan_nexthdr_nhcs[nhc->nexthdr] = NULL; + + spin_unlock_bh(&lowpan_nhc_lock); + + synchronize_net(); +} +EXPORT_SYMBOL(lowpan_nhc_del); diff --git a/net/6lowpan/nhc.h b/net/6lowpan/nhc.h new file mode 100644 index 000000000..ab7b4977c --- /dev/null +++ b/net/6lowpan/nhc.h @@ -0,0 +1,133 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +#ifndef __6LOWPAN_NHC_H +#define __6LOWPAN_NHC_H + +#include +#include +#include + +#include +#include + +/** + * LOWPAN_NHC - helper macro to generate nh id fields and lowpan_nhc struct + * + * @__nhc: variable name of the lowpan_nhc struct. + * @_name: const char * of common header compression name. + * @_nexthdr: ipv6 nexthdr field for the header compression. + * @_nexthdrlen: ipv6 nexthdr len for the reserved space. + * @_id: one byte nhc id value. + * @_idmask: one byte nhc id mask value. + * @_uncompress: callback for uncompression call. + * @_compress: callback for compression call. + */ +#define LOWPAN_NHC(__nhc, _name, _nexthdr, \ + _hdrlen, _id, _idmask, \ + _uncompress, _compress) \ +static const struct lowpan_nhc __nhc = { \ + .name = _name, \ + .nexthdr = _nexthdr, \ + .nexthdrlen = _hdrlen, \ + .id = _id, \ + .idmask = _idmask, \ + .uncompress = _uncompress, \ + .compress = _compress, \ +} + +#define module_lowpan_nhc(__nhc) \ +static int __init __nhc##_init(void) \ +{ \ + return lowpan_nhc_add(&(__nhc)); \ +} \ +module_init(__nhc##_init); \ +static void __exit __nhc##_exit(void) \ +{ \ + lowpan_nhc_del(&(__nhc)); \ +} \ +module_exit(__nhc##_exit); + +/** + * struct lowpan_nhc - hold 6lowpan next hdr compression ifnformation + * + * @name: name of the specific next header compression + * @nexthdr: next header value of the protocol which should be compressed. + * @nexthdrlen: ipv6 nexthdr len for the reserved space. + * @id: one byte nhc id value. + * @idmask: one byte nhc id mask value. + * @compress: callback to do the header compression. + * @uncompress: callback to do the header uncompression. + */ +struct lowpan_nhc { + const char *name; + u8 nexthdr; + size_t nexthdrlen; + u8 id; + u8 idmask; + + int (*uncompress)(struct sk_buff *skb, size_t needed); + int (*compress)(struct sk_buff *skb, u8 **hc_ptr); +}; + +/** + * lowpan_nhc_by_nexthdr - return the 6lowpan nhc by ipv6 nexthdr. + * + * @nexthdr: ipv6 nexthdr value. + */ +struct lowpan_nhc *lowpan_nhc_by_nexthdr(u8 nexthdr); + +/** + * lowpan_nhc_check_compression - checks if we support compression format. If + * we support the nhc by nexthdr field, the function will return 0. If we + * don't support the nhc by nexthdr this function will return -ENOENT. + * + * @skb: skb of 6LoWPAN header to read nhc and replace header. + * @hdr: ipv6hdr to check the nexthdr value + * @hc_ptr: pointer for 6LoWPAN header which should increment at the end of + * replaced header. + */ +int lowpan_nhc_check_compression(struct sk_buff *skb, + const struct ipv6hdr *hdr, u8 **hc_ptr); + +/** + * lowpan_nhc_do_compression - calling compress callback for nhc + * + * @skb: skb of 6LoWPAN header to read nhc and replace header. + * @hdr: ipv6hdr to set the nexthdr value + * @hc_ptr: pointer for 6LoWPAN header which should increment at the end of + * replaced header. + */ +int lowpan_nhc_do_compression(struct sk_buff *skb, const struct ipv6hdr *hdr, + u8 **hc_ptr); + +/** + * lowpan_nhc_do_uncompression - calling uncompress callback for nhc + * + * @nhc: 6LoWPAN nhc context, get by lowpan_nhc_by_ functions. + * @skb: skb of 6LoWPAN header, skb->data should be pointed to nhc id value. + * @dev: netdevice for print logging information. + * @hdr: ipv6hdr for setting nexthdr value. + */ +int lowpan_nhc_do_uncompression(struct sk_buff *skb, + const struct net_device *dev, + struct ipv6hdr *hdr); + +/** + * lowpan_nhc_add - register a next header compression to framework + * + * @nhc: nhc which should be add. + */ +int lowpan_nhc_add(const struct lowpan_nhc *nhc); + +/** + * lowpan_nhc_del - delete a next header compression from framework + * + * @nhc: nhc which should be delete. + */ +void lowpan_nhc_del(const struct lowpan_nhc *nhc); + +/** + * lowpan_nhc_init - adding all default nhcs + */ +void lowpan_nhc_init(void); + +#endif /* __6LOWPAN_NHC_H */ diff --git a/net/6lowpan/nhc_dest.c b/net/6lowpan/nhc_dest.c new file mode 100644 index 000000000..0cbcc7806 --- /dev/null +++ b/net/6lowpan/nhc_dest.c @@ -0,0 +1,17 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN IPv6 Destination Options Header compression according to + * RFC6282 + */ + +#include "nhc.h" + +#define LOWPAN_NHC_DEST_ID_0 0xe6 +#define LOWPAN_NHC_DEST_MASK_0 0xfe + +LOWPAN_NHC(nhc_dest, "RFC6282 Destination Options", NEXTHDR_DEST, 0, + LOWPAN_NHC_DEST_ID_0, LOWPAN_NHC_DEST_MASK_0, NULL, NULL); + +module_lowpan_nhc(nhc_dest); +MODULE_DESCRIPTION("6LoWPAN next header RFC6282 Destination Options compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_fragment.c b/net/6lowpan/nhc_fragment.c new file mode 100644 index 000000000..9414552df --- /dev/null +++ b/net/6lowpan/nhc_fragment.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN IPv6 Fragment Header compression according to RFC6282 + */ + +#include "nhc.h" + +#define LOWPAN_NHC_FRAGMENT_ID_0 0xe4 +#define LOWPAN_NHC_FRAGMENT_MASK_0 0xfe + +LOWPAN_NHC(nhc_fragment, "RFC6282 Fragment", NEXTHDR_FRAGMENT, 0, + LOWPAN_NHC_FRAGMENT_ID_0, LOWPAN_NHC_FRAGMENT_MASK_0, NULL, NULL); + +module_lowpan_nhc(nhc_fragment); +MODULE_DESCRIPTION("6LoWPAN next header RFC6282 Fragment compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_ghc_ext_dest.c b/net/6lowpan/nhc_ghc_ext_dest.c new file mode 100644 index 000000000..e4745ddd1 --- /dev/null +++ b/net/6lowpan/nhc_ghc_ext_dest.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN Extension Header compression according to RFC7400 + */ + +#include "nhc.h" + +#define LOWPAN_GHC_EXT_DEST_ID_0 0xb6 +#define LOWPAN_GHC_EXT_DEST_MASK_0 0xfe + +LOWPAN_NHC(ghc_ext_dest, "RFC7400 Destination Extension Header", NEXTHDR_DEST, + 0, LOWPAN_GHC_EXT_DEST_ID_0, LOWPAN_GHC_EXT_DEST_MASK_0, NULL, NULL); + +module_lowpan_nhc(ghc_ext_dest); +MODULE_DESCRIPTION("6LoWPAN generic header destination extension compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_ghc_ext_frag.c b/net/6lowpan/nhc_ghc_ext_frag.c new file mode 100644 index 000000000..220e5abfa --- /dev/null +++ b/net/6lowpan/nhc_ghc_ext_frag.c @@ -0,0 +1,17 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN Extension Header compression according to RFC7400 + */ + +#include "nhc.h" + +#define LOWPAN_GHC_EXT_FRAG_ID_0 0xb4 +#define LOWPAN_GHC_EXT_FRAG_MASK_0 0xfe + +LOWPAN_NHC(ghc_ext_frag, "RFC7400 Fragmentation Extension Header", + NEXTHDR_FRAGMENT, 0, LOWPAN_GHC_EXT_FRAG_ID_0, + LOWPAN_GHC_EXT_FRAG_MASK_0, NULL, NULL); + +module_lowpan_nhc(ghc_ext_frag); +MODULE_DESCRIPTION("6LoWPAN generic header fragmentation extension compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_ghc_ext_hop.c b/net/6lowpan/nhc_ghc_ext_hop.c new file mode 100644 index 000000000..9b0de4da7 --- /dev/null +++ b/net/6lowpan/nhc_ghc_ext_hop.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN Extension Header compression according to RFC7400 + */ + +#include "nhc.h" + +#define LOWPAN_GHC_EXT_HOP_ID_0 0xb0 +#define LOWPAN_GHC_EXT_HOP_MASK_0 0xfe + +LOWPAN_NHC(ghc_ext_hop, "RFC7400 Hop-by-Hop Extension Header", NEXTHDR_HOP, 0, + LOWPAN_GHC_EXT_HOP_ID_0, LOWPAN_GHC_EXT_HOP_MASK_0, NULL, NULL); + +module_lowpan_nhc(ghc_ext_hop); +MODULE_DESCRIPTION("6LoWPAN generic header hop-by-hop extension compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_ghc_ext_route.c b/net/6lowpan/nhc_ghc_ext_route.c new file mode 100644 index 000000000..3e86faec5 --- /dev/null +++ b/net/6lowpan/nhc_ghc_ext_route.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN Extension Header compression according to RFC7400 + */ + +#include "nhc.h" + +#define LOWPAN_GHC_EXT_ROUTE_ID_0 0xb2 +#define LOWPAN_GHC_EXT_ROUTE_MASK_0 0xfe + +LOWPAN_NHC(ghc_ext_route, "RFC7400 Routing Extension Header", NEXTHDR_ROUTING, + 0, LOWPAN_GHC_EXT_ROUTE_ID_0, LOWPAN_GHC_EXT_ROUTE_MASK_0, NULL, NULL); + +module_lowpan_nhc(ghc_ext_route); +MODULE_DESCRIPTION("6LoWPAN generic header routing extension compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_ghc_icmpv6.c b/net/6lowpan/nhc_ghc_icmpv6.c new file mode 100644 index 000000000..1634f3eb0 --- /dev/null +++ b/net/6lowpan/nhc_ghc_icmpv6.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN ICMPv6 compression according to RFC7400 + */ + +#include "nhc.h" + +#define LOWPAN_GHC_ICMPV6_ID_0 0xdf +#define LOWPAN_GHC_ICMPV6_MASK_0 0xff + +LOWPAN_NHC(ghc_icmpv6, "RFC7400 ICMPv6", NEXTHDR_ICMP, 0, + LOWPAN_GHC_ICMPV6_ID_0, LOWPAN_GHC_ICMPV6_MASK_0, NULL, NULL); + +module_lowpan_nhc(ghc_icmpv6); +MODULE_DESCRIPTION("6LoWPAN generic header ICMPv6 compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_ghc_udp.c b/net/6lowpan/nhc_ghc_udp.c new file mode 100644 index 000000000..4ac4813b7 --- /dev/null +++ b/net/6lowpan/nhc_ghc_udp.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN UDP compression according to RFC7400 + */ + +#include "nhc.h" + +#define LOWPAN_GHC_UDP_ID_0 0xd0 +#define LOWPAN_GHC_UDP_MASK_0 0xf8 + +LOWPAN_NHC(ghc_udp, "RFC7400 UDP", NEXTHDR_UDP, 0, + LOWPAN_GHC_UDP_ID_0, LOWPAN_GHC_UDP_MASK_0, NULL, NULL); + +module_lowpan_nhc(ghc_udp); +MODULE_DESCRIPTION("6LoWPAN generic header UDP compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_hop.c b/net/6lowpan/nhc_hop.c new file mode 100644 index 000000000..182087dfd --- /dev/null +++ b/net/6lowpan/nhc_hop.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN IPv6 Hop-by-Hop Options Header compression according to RFC6282 + */ + +#include "nhc.h" + +#define LOWPAN_NHC_HOP_ID_0 0xe0 +#define LOWPAN_NHC_HOP_MASK_0 0xfe + +LOWPAN_NHC(nhc_hop, "RFC6282 Hop-by-Hop Options", NEXTHDR_HOP, 0, + LOWPAN_NHC_HOP_ID_0, LOWPAN_NHC_HOP_MASK_0, NULL, NULL); + +module_lowpan_nhc(nhc_hop); +MODULE_DESCRIPTION("6LoWPAN next header RFC6282 Hop-by-Hop Options compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_ipv6.c b/net/6lowpan/nhc_ipv6.c new file mode 100644 index 000000000..20242360b --- /dev/null +++ b/net/6lowpan/nhc_ipv6.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN IPv6 Header compression according to RFC6282 + */ + +#include "nhc.h" + +#define LOWPAN_NHC_IPV6_ID_0 0xee +#define LOWPAN_NHC_IPV6_MASK_0 0xfe + +LOWPAN_NHC(nhc_ipv6, "RFC6282 IPv6", NEXTHDR_IPV6, 0, LOWPAN_NHC_IPV6_ID_0, + LOWPAN_NHC_IPV6_MASK_0, NULL, NULL); + +module_lowpan_nhc(nhc_ipv6); +MODULE_DESCRIPTION("6LoWPAN next header RFC6282 IPv6 compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_mobility.c b/net/6lowpan/nhc_mobility.c new file mode 100644 index 000000000..1c31d872c --- /dev/null +++ b/net/6lowpan/nhc_mobility.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN IPv6 Mobility Header compression according to RFC6282 + */ + +#include "nhc.h" + +#define LOWPAN_NHC_MOBILITY_ID_0 0xe8 +#define LOWPAN_NHC_MOBILITY_MASK_0 0xfe + +LOWPAN_NHC(nhc_mobility, "RFC6282 Mobility", NEXTHDR_MOBILITY, 0, + LOWPAN_NHC_MOBILITY_ID_0, LOWPAN_NHC_MOBILITY_MASK_0, NULL, NULL); + +module_lowpan_nhc(nhc_mobility); +MODULE_DESCRIPTION("6LoWPAN next header RFC6282 Mobility compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_routing.c b/net/6lowpan/nhc_routing.c new file mode 100644 index 000000000..dae03ebf7 --- /dev/null +++ b/net/6lowpan/nhc_routing.c @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN IPv6 Routing Header compression according to RFC6282 + */ + +#include "nhc.h" + +#define LOWPAN_NHC_ROUTING_ID_0 0xe2 +#define LOWPAN_NHC_ROUTING_MASK_0 0xfe + +LOWPAN_NHC(nhc_routing, "RFC6282 Routing", NEXTHDR_ROUTING, 0, + LOWPAN_NHC_ROUTING_ID_0, LOWPAN_NHC_ROUTING_MASK_0, NULL, NULL); + +module_lowpan_nhc(nhc_routing); +MODULE_DESCRIPTION("6LoWPAN next header RFC6282 Routing compression"); +MODULE_LICENSE("GPL"); diff --git a/net/6lowpan/nhc_udp.c b/net/6lowpan/nhc_udp.c new file mode 100644 index 000000000..ed4227e6d --- /dev/null +++ b/net/6lowpan/nhc_udp.c @@ -0,0 +1,176 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * 6LoWPAN IPv6 UDP compression according to RFC6282 + * + * Authors: + * Alexander Aring + * + * Original written by: + * Alexander Smirnov + * Jon Smirl + */ + +#include "nhc.h" + +#define LOWPAN_NHC_UDP_MASK 0xF8 +#define LOWPAN_NHC_UDP_ID 0xF0 + +#define LOWPAN_NHC_UDP_4BIT_PORT 0xF0B0 +#define LOWPAN_NHC_UDP_4BIT_MASK 0xFFF0 +#define LOWPAN_NHC_UDP_8BIT_PORT 0xF000 +#define LOWPAN_NHC_UDP_8BIT_MASK 0xFF00 + +/* values for port compression, _with checksum_ ie bit 5 set to 0 */ + +/* all inline */ +#define LOWPAN_NHC_UDP_CS_P_00 0xF0 +/* source 16bit inline, dest = 0xF0 + 8 bit inline */ +#define LOWPAN_NHC_UDP_CS_P_01 0xF1 +/* source = 0xF0 + 8bit inline, dest = 16 bit inline */ +#define LOWPAN_NHC_UDP_CS_P_10 0xF2 +/* source & dest = 0xF0B + 4bit inline */ +#define LOWPAN_NHC_UDP_CS_P_11 0xF3 +/* checksum elided */ +#define LOWPAN_NHC_UDP_CS_C 0x04 + +static int udp_uncompress(struct sk_buff *skb, size_t needed) +{ + u8 tmp = 0, val = 0; + struct udphdr uh; + bool fail; + int err; + + fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp)); + + pr_debug("UDP header uncompression\n"); + switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { + case LOWPAN_NHC_UDP_CS_P_00: + fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); + fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); + break; + case LOWPAN_NHC_UDP_CS_P_01: + fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); + fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); + uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); + break; + case LOWPAN_NHC_UDP_CS_P_10: + fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); + uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); + fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); + break; + case LOWPAN_NHC_UDP_CS_P_11: + fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); + uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4)); + uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f)); + break; + default: + BUG(); + } + + pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", + ntohs(uh.source), ntohs(uh.dest)); + + /* checksum */ + if (tmp & LOWPAN_NHC_UDP_CS_C) { + pr_debug_ratelimited("checksum elided currently not supported\n"); + fail = true; + } else { + fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check)); + } + + if (fail) + return -EINVAL; + + /* UDP length needs to be inferred from the lower layers + * here, we obtain the hint from the remaining size of the + * frame + */ + switch (lowpan_dev(skb->dev)->lltype) { + case LOWPAN_LLTYPE_IEEE802154: + if (lowpan_802154_cb(skb)->d_size) + udp_set_len_short(&uh, lowpan_802154_cb(skb)->d_size - + sizeof(struct ipv6hdr)); + else + udp_set_len_short(&uh, skb->len + sizeof(struct udphdr)); + break; + default: + udp_set_len_short(&uh, skb->len + sizeof(struct udphdr)); + break; + } + pr_debug("uncompressed UDP length: src = %d", udp_get_len_short(&uh)); + + /* replace the compressed UDP head by the uncompressed UDP + * header + */ + err = skb_cow(skb, needed); + if (unlikely(err)) + return err; + + skb_push(skb, sizeof(struct udphdr)); + skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); + + return 0; +} + +static int udp_compress(struct sk_buff *skb, u8 **hc_ptr) +{ + const struct udphdr *uh = udp_hdr(skb); + u8 tmp; + + if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) == + LOWPAN_NHC_UDP_4BIT_PORT) && + ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) == + LOWPAN_NHC_UDP_4BIT_PORT)) { + pr_debug("UDP header: both ports compression to 4 bits\n"); + /* compression value */ + tmp = LOWPAN_NHC_UDP_CS_P_11; + lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); + /* source and destination port */ + tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT + + ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4); + lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); + } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) == + LOWPAN_NHC_UDP_8BIT_PORT) { + pr_debug("UDP header: remove 8 bits of dest\n"); + /* compression value */ + tmp = LOWPAN_NHC_UDP_CS_P_01; + lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); + /* source port */ + lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); + /* destination port */ + tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT; + lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); + } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) == + LOWPAN_NHC_UDP_8BIT_PORT) { + pr_debug("UDP header: remove 8 bits of source\n"); + /* compression value */ + tmp = LOWPAN_NHC_UDP_CS_P_10; + lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); + /* source port */ + tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT; + lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); + /* destination port */ + lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); + } else { + pr_debug("UDP header: can't compress\n"); + /* compression value */ + tmp = LOWPAN_NHC_UDP_CS_P_00; + lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); + /* source port */ + lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); + /* destination port */ + lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); + } + + /* checksum is always inline */ + lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check)); + + return 0; +} + +LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr), + LOWPAN_NHC_UDP_ID, LOWPAN_NHC_UDP_MASK, udp_uncompress, udp_compress); + +module_lowpan_nhc(nhc_udp); +MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression"); +MODULE_LICENSE("GPL"); -- cgit v1.3.1