From 9ca8dbcc65cfc63d6f5ef3312a33184e1d726e00 Mon Sep 17 00:00:00 2001 From: Yunhong Jiang Date: Tue, 4 Aug 2015 12:17:53 -0700 Subject: Add the rt linux 4.1.3-rt3 as base Import the rt linux 4.1.3-rt3 as OPNFV kvm base. It's from git://git.kernel.org/pub/scm/linux/kernel/git/rt/linux-rt-devel.git linux-4.1.y-rt and the base is: commit 0917f823c59692d751951bf5ea699a2d1e2f26a2 Author: Sebastian Andrzej Siewior Date: Sat Jul 25 12:13:34 2015 +0200 Prepare v4.1.3-rt3 Signed-off-by: Sebastian Andrzej Siewior We lose all the git history this way and it's not good. We should apply another opnfv project repo in future. Change-Id: I87543d81c9df70d99c5001fbdf646b202c19f423 Signed-off-by: Yunhong Jiang --- kernel/net/mac802154/Kconfig | 20 + kernel/net/mac802154/Makefile | 5 + kernel/net/mac802154/cfg.c | 231 ++++++++ kernel/net/mac802154/cfg.h | 9 + kernel/net/mac802154/driver-ops.h | 223 ++++++++ kernel/net/mac802154/ieee802154_i.h | 189 +++++++ kernel/net/mac802154/iface.c | 651 +++++++++++++++++++++ kernel/net/mac802154/llsec.c | 1056 +++++++++++++++++++++++++++++++++++ kernel/net/mac802154/llsec.h | 108 ++++ kernel/net/mac802154/mac_cmd.c | 162 ++++++ kernel/net/mac802154/main.c | 220 ++++++++ kernel/net/mac802154/mib.c | 286 ++++++++++ kernel/net/mac802154/rx.c | 302 ++++++++++ kernel/net/mac802154/tx.c | 149 +++++ kernel/net/mac802154/util.c | 95 ++++ 15 files changed, 3706 insertions(+) create mode 100644 kernel/net/mac802154/Kconfig create mode 100644 kernel/net/mac802154/Makefile create mode 100644 kernel/net/mac802154/cfg.c create mode 100644 kernel/net/mac802154/cfg.h create mode 100644 kernel/net/mac802154/driver-ops.h create mode 100644 kernel/net/mac802154/ieee802154_i.h create mode 100644 kernel/net/mac802154/iface.c create mode 100644 kernel/net/mac802154/llsec.c create mode 100644 kernel/net/mac802154/llsec.h create mode 100644 kernel/net/mac802154/mac_cmd.c create mode 100644 kernel/net/mac802154/main.c create mode 100644 kernel/net/mac802154/mib.c create mode 100644 kernel/net/mac802154/rx.c create mode 100644 kernel/net/mac802154/tx.c create mode 100644 kernel/net/mac802154/util.c (limited to 'kernel/net/mac802154') diff --git a/kernel/net/mac802154/Kconfig b/kernel/net/mac802154/Kconfig new file mode 100644 index 000000000..aa462b480 --- /dev/null +++ b/kernel/net/mac802154/Kconfig @@ -0,0 +1,20 @@ +config MAC802154 + tristate "Generic IEEE 802.15.4 Soft Networking Stack (mac802154)" + depends on IEEE802154 + select CRC_CCITT + select CRYPTO_AUTHENC + select CRYPTO_CCM + select CRYPTO_CTR + select CRYPTO_AES + ---help--- + This option enables the hardware independent IEEE 802.15.4 + networking stack for SoftMAC devices (the ones implementing + only PHY level of IEEE 802.15.4 standard). + + Note: this implementation is neither certified, nor feature + complete! Compatibility with other implementations hasn't + been tested yet! + + If you plan to use HardMAC IEEE 802.15.4 devices, you can + say N here. Alternatively you can say M to compile it as + module. diff --git a/kernel/net/mac802154/Makefile b/kernel/net/mac802154/Makefile new file mode 100644 index 000000000..702d8b466 --- /dev/null +++ b/kernel/net/mac802154/Makefile @@ -0,0 +1,5 @@ +obj-$(CONFIG_MAC802154) += mac802154.o +mac802154-objs := main.o rx.o tx.o mac_cmd.o mib.o \ + iface.o llsec.o util.o cfg.o + +ccflags-y += -D__CHECK_ENDIAN__ diff --git a/kernel/net/mac802154/cfg.c b/kernel/net/mac802154/cfg.c new file mode 100644 index 000000000..70be9c799 --- /dev/null +++ b/kernel/net/mac802154/cfg.c @@ -0,0 +1,231 @@ +/* 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. + * + * Authors: + * Alexander Aring + * + * Based on: net/mac80211/cfg.c + */ + +#include +#include + +#include "ieee802154_i.h" +#include "driver-ops.h" +#include "cfg.h" + +static struct net_device * +ieee802154_add_iface_deprecated(struct wpan_phy *wpan_phy, + const char *name, + unsigned char name_assign_type, int type) +{ + struct ieee802154_local *local = wpan_phy_priv(wpan_phy); + struct net_device *dev; + + rtnl_lock(); + dev = ieee802154_if_add(local, name, name_assign_type, type, + cpu_to_le64(0x0000000000000000ULL)); + rtnl_unlock(); + + return dev; +} + +static void ieee802154_del_iface_deprecated(struct wpan_phy *wpan_phy, + struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + ieee802154_if_remove(sdata); +} + +static int +ieee802154_add_iface(struct wpan_phy *phy, const char *name, + unsigned char name_assign_type, + enum nl802154_iftype type, __le64 extended_addr) +{ + struct ieee802154_local *local = wpan_phy_priv(phy); + struct net_device *err; + + err = ieee802154_if_add(local, name, name_assign_type, type, + extended_addr); + return PTR_ERR_OR_ZERO(err); +} + +static int +ieee802154_del_iface(struct wpan_phy *wpan_phy, struct wpan_dev *wpan_dev) +{ + ieee802154_if_remove(IEEE802154_WPAN_DEV_TO_SUB_IF(wpan_dev)); + + return 0; +} + +static int +ieee802154_set_channel(struct wpan_phy *wpan_phy, u8 page, u8 channel) +{ + struct ieee802154_local *local = wpan_phy_priv(wpan_phy); + int ret; + + ASSERT_RTNL(); + + /* check if phy support this setting */ + if (!(wpan_phy->channels_supported[page] & BIT(channel))) + return -EINVAL; + + ret = drv_set_channel(local, page, channel); + if (!ret) { + wpan_phy->current_page = page; + wpan_phy->current_channel = channel; + } + + return ret; +} + +static int +ieee802154_set_cca_mode(struct wpan_phy *wpan_phy, + const struct wpan_phy_cca *cca) +{ + struct ieee802154_local *local = wpan_phy_priv(wpan_phy); + int ret; + + ASSERT_RTNL(); + + /* check if phy support this setting */ + if (!(local->hw.flags & IEEE802154_HW_CCA_MODE)) + return -EOPNOTSUPP; + + ret = drv_set_cca_mode(local, cca); + if (!ret) + wpan_phy->cca = *cca; + + return ret; +} + +static int +ieee802154_set_pan_id(struct wpan_phy *wpan_phy, struct wpan_dev *wpan_dev, + __le16 pan_id) +{ + ASSERT_RTNL(); + + /* TODO + * I am not sure about to check here on broadcast pan_id. + * Broadcast is a valid setting, comment from 802.15.4: + * If this value is 0xffff, the device is not associated. + * + * This could useful to simple deassociate an device. + */ + if (pan_id == cpu_to_le16(IEEE802154_PAN_ID_BROADCAST)) + return -EINVAL; + + wpan_dev->pan_id = pan_id; + return 0; +} + +static int +ieee802154_set_backoff_exponent(struct wpan_phy *wpan_phy, + struct wpan_dev *wpan_dev, + u8 min_be, u8 max_be) +{ + struct ieee802154_local *local = wpan_phy_priv(wpan_phy); + + ASSERT_RTNL(); + + if (!(local->hw.flags & IEEE802154_HW_CSMA_PARAMS)) + return -EOPNOTSUPP; + + wpan_dev->min_be = min_be; + wpan_dev->max_be = max_be; + return 0; +} + +static int +ieee802154_set_short_addr(struct wpan_phy *wpan_phy, struct wpan_dev *wpan_dev, + __le16 short_addr) +{ + ASSERT_RTNL(); + + /* TODO + * I am not sure about to check here on broadcast short_addr. + * Broadcast is a valid setting, comment from 802.15.4: + * A value of 0xfffe indicates that the device has + * associated but has not been allocated an address. A + * value of 0xffff indicates that the device does not + * have a short address. + * + * I think we should allow to set these settings but + * don't allow to allow socket communication with it. + */ + if (short_addr == cpu_to_le16(IEEE802154_ADDR_SHORT_UNSPEC) || + short_addr == cpu_to_le16(IEEE802154_ADDR_SHORT_BROADCAST)) + return -EINVAL; + + wpan_dev->short_addr = short_addr; + return 0; +} + +static int +ieee802154_set_max_csma_backoffs(struct wpan_phy *wpan_phy, + struct wpan_dev *wpan_dev, + u8 max_csma_backoffs) +{ + struct ieee802154_local *local = wpan_phy_priv(wpan_phy); + + ASSERT_RTNL(); + + if (!(local->hw.flags & IEEE802154_HW_CSMA_PARAMS)) + return -EOPNOTSUPP; + + wpan_dev->csma_retries = max_csma_backoffs; + return 0; +} + +static int +ieee802154_set_max_frame_retries(struct wpan_phy *wpan_phy, + struct wpan_dev *wpan_dev, + s8 max_frame_retries) +{ + struct ieee802154_local *local = wpan_phy_priv(wpan_phy); + + ASSERT_RTNL(); + + if (!(local->hw.flags & IEEE802154_HW_FRAME_RETRIES)) + return -EOPNOTSUPP; + + wpan_dev->frame_retries = max_frame_retries; + return 0; +} + +static int +ieee802154_set_lbt_mode(struct wpan_phy *wpan_phy, struct wpan_dev *wpan_dev, + bool mode) +{ + struct ieee802154_local *local = wpan_phy_priv(wpan_phy); + + ASSERT_RTNL(); + + if (!(local->hw.flags & IEEE802154_HW_LBT)) + return -EOPNOTSUPP; + + wpan_dev->lbt = mode; + return 0; +} + +const struct cfg802154_ops mac802154_config_ops = { + .add_virtual_intf_deprecated = ieee802154_add_iface_deprecated, + .del_virtual_intf_deprecated = ieee802154_del_iface_deprecated, + .add_virtual_intf = ieee802154_add_iface, + .del_virtual_intf = ieee802154_del_iface, + .set_channel = ieee802154_set_channel, + .set_cca_mode = ieee802154_set_cca_mode, + .set_pan_id = ieee802154_set_pan_id, + .set_short_addr = ieee802154_set_short_addr, + .set_backoff_exponent = ieee802154_set_backoff_exponent, + .set_max_csma_backoffs = ieee802154_set_max_csma_backoffs, + .set_max_frame_retries = ieee802154_set_max_frame_retries, + .set_lbt_mode = ieee802154_set_lbt_mode, +}; diff --git a/kernel/net/mac802154/cfg.h b/kernel/net/mac802154/cfg.h new file mode 100644 index 000000000..e2718f981 --- /dev/null +++ b/kernel/net/mac802154/cfg.h @@ -0,0 +1,9 @@ +/* mac802154 configuration hooks for cfg802154 + */ + +#ifndef __CFG_H +#define __CFG_H + +extern const struct cfg802154_ops mac802154_config_ops; + +#endif /* __CFG_H */ diff --git a/kernel/net/mac802154/driver-ops.h b/kernel/net/mac802154/driver-ops.h new file mode 100644 index 000000000..a0533357b --- /dev/null +++ b/kernel/net/mac802154/driver-ops.h @@ -0,0 +1,223 @@ +#ifndef __MAC802154_DRIVER_OPS +#define __MAC802154_DRIVER_OPS + +#include +#include + +#include + +#include "ieee802154_i.h" + +static inline int +drv_xmit_async(struct ieee802154_local *local, struct sk_buff *skb) +{ + return local->ops->xmit_async(&local->hw, skb); +} + +static inline int +drv_xmit_sync(struct ieee802154_local *local, struct sk_buff *skb) +{ + /* don't allow other operations while sync xmit */ + ASSERT_RTNL(); + + might_sleep(); + + return local->ops->xmit_sync(&local->hw, skb); +} + +static inline int drv_start(struct ieee802154_local *local) +{ + might_sleep(); + + local->started = true; + smp_mb(); + + return local->ops->start(&local->hw); +} + +static inline void drv_stop(struct ieee802154_local *local) +{ + might_sleep(); + + local->ops->stop(&local->hw); + + /* sync away all work on the tasklet before clearing started */ + tasklet_disable(&local->tasklet); + tasklet_enable(&local->tasklet); + + barrier(); + + local->started = false; +} + +static inline int +drv_set_channel(struct ieee802154_local *local, u8 page, u8 channel) +{ + might_sleep(); + + return local->ops->set_channel(&local->hw, page, channel); +} + +static inline int drv_set_tx_power(struct ieee802154_local *local, s8 dbm) +{ + might_sleep(); + + if (!local->ops->set_txpower) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + return local->ops->set_txpower(&local->hw, dbm); +} + +static inline int drv_set_cca_mode(struct ieee802154_local *local, + const struct wpan_phy_cca *cca) +{ + might_sleep(); + + if (!local->ops->set_cca_mode) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + return local->ops->set_cca_mode(&local->hw, cca); +} + +static inline int drv_set_lbt_mode(struct ieee802154_local *local, bool mode) +{ + might_sleep(); + + if (!local->ops->set_lbt) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + return local->ops->set_lbt(&local->hw, mode); +} + +static inline int +drv_set_cca_ed_level(struct ieee802154_local *local, s32 ed_level) +{ + might_sleep(); + + if (!local->ops->set_cca_ed_level) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + return local->ops->set_cca_ed_level(&local->hw, ed_level); +} + +static inline int drv_set_pan_id(struct ieee802154_local *local, __le16 pan_id) +{ + struct ieee802154_hw_addr_filt filt; + + might_sleep(); + + if (!local->ops->set_hw_addr_filt) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + filt.pan_id = pan_id; + + return local->ops->set_hw_addr_filt(&local->hw, &filt, + IEEE802154_AFILT_PANID_CHANGED); +} + +static inline int +drv_set_extended_addr(struct ieee802154_local *local, __le64 extended_addr) +{ + struct ieee802154_hw_addr_filt filt; + + might_sleep(); + + if (!local->ops->set_hw_addr_filt) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + filt.ieee_addr = extended_addr; + + return local->ops->set_hw_addr_filt(&local->hw, &filt, + IEEE802154_AFILT_IEEEADDR_CHANGED); +} + +static inline int +drv_set_short_addr(struct ieee802154_local *local, __le16 short_addr) +{ + struct ieee802154_hw_addr_filt filt; + + might_sleep(); + + if (!local->ops->set_hw_addr_filt) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + filt.short_addr = short_addr; + + return local->ops->set_hw_addr_filt(&local->hw, &filt, + IEEE802154_AFILT_SADDR_CHANGED); +} + +static inline int +drv_set_pan_coord(struct ieee802154_local *local, bool is_coord) +{ + struct ieee802154_hw_addr_filt filt; + + might_sleep(); + + if (!local->ops->set_hw_addr_filt) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + filt.pan_coord = is_coord; + + return local->ops->set_hw_addr_filt(&local->hw, &filt, + IEEE802154_AFILT_PANC_CHANGED); +} + +static inline int +drv_set_csma_params(struct ieee802154_local *local, u8 min_be, u8 max_be, + u8 max_csma_backoffs) +{ + might_sleep(); + + if (!local->ops->set_csma_params) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + return local->ops->set_csma_params(&local->hw, min_be, max_be, + max_csma_backoffs); +} + +static inline int +drv_set_max_frame_retries(struct ieee802154_local *local, s8 max_frame_retries) +{ + might_sleep(); + + if (!local->ops->set_frame_retries) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + return local->ops->set_frame_retries(&local->hw, max_frame_retries); +} + +static inline int +drv_set_promiscuous_mode(struct ieee802154_local *local, bool on) +{ + might_sleep(); + + if (!local->ops->set_promiscuous_mode) { + WARN_ON(1); + return -EOPNOTSUPP; + } + + return local->ops->set_promiscuous_mode(&local->hw, on); +} + +#endif /* __MAC802154_DRIVER_OPS */ diff --git a/kernel/net/mac802154/ieee802154_i.h b/kernel/net/mac802154/ieee802154_i.h new file mode 100644 index 000000000..127ba1838 --- /dev/null +++ b/kernel/net/mac802154/ieee802154_i.h @@ -0,0 +1,189 @@ +/* + * Copyright (C) 2007-2012 Siemens AG + * + * 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. + * + * Written by: + * Pavel Smolenskiy + * Maxim Gorbachyov + * Dmitry Eremin-Solenikov + * Alexander Smirnov + */ +#ifndef __IEEE802154_I_H +#define __IEEE802154_I_H + +#include +#include +#include +#include +#include +#include + +#include "llsec.h" + +/* mac802154 device private data */ +struct ieee802154_local { + struct ieee802154_hw hw; + const struct ieee802154_ops *ops; + + /* ieee802154 phy */ + struct wpan_phy *phy; + + int open_count; + + /* As in mac80211 slaves list is modified: + * 1) under the RTNL + * 2) protected by slaves_mtx; + * 3) in an RCU manner + * + * So atomic readers can use any of this protection methods. + */ + struct list_head interfaces; + struct mutex iflist_mtx; + + /* This one is used for scanning and other jobs not to be interfered + * with serial driver. + */ + struct workqueue_struct *workqueue; + + struct hrtimer ifs_timer; + + bool started; + + struct tasklet_struct tasklet; + struct sk_buff_head skb_queue; +}; + +enum { + IEEE802154_RX_MSG = 1, +}; + +enum ieee802154_sdata_state_bits { + SDATA_STATE_RUNNING, +}; + +/* Slave interface definition. + * + * Slaves represent typical network interfaces available from userspace. + * Each ieee802154 device/transceiver may have several slaves and able + * to be associated with several networks at the same time. + */ +struct ieee802154_sub_if_data { + struct list_head list; /* the ieee802154_priv->slaves list */ + + struct wpan_dev wpan_dev; + + struct ieee802154_local *local; + struct net_device *dev; + + unsigned long state; + char name[IFNAMSIZ]; + + spinlock_t mib_lock; + + /* protects sec from concurrent access by netlink. access by + * encrypt/decrypt/header_create safe without additional protection. + */ + struct mutex sec_mtx; + + struct mac802154_llsec sec; + /* must be last, dynamically sized area in this! */ + struct ieee802154_vif vif; +}; + +#define MAC802154_CHAN_NONE 0xff /* No channel is assigned */ + +/* utility functions/constants */ +extern const void *const mac802154_wpan_phy_privid; /* for wpan_phy privid */ + +static inline struct ieee802154_local * +hw_to_local(struct ieee802154_hw *hw) +{ + return container_of(hw, struct ieee802154_local, hw); +} + +static inline struct ieee802154_sub_if_data * +IEEE802154_DEV_TO_SUB_IF(const struct net_device *dev) +{ + return netdev_priv(dev); +} + +static inline struct ieee802154_sub_if_data * +IEEE802154_WPAN_DEV_TO_SUB_IF(struct wpan_dev *wpan_dev) +{ + return container_of(wpan_dev, struct ieee802154_sub_if_data, wpan_dev); +} + +static inline bool +ieee802154_sdata_running(struct ieee802154_sub_if_data *sdata) +{ + return test_bit(SDATA_STATE_RUNNING, &sdata->state); +} + +extern struct ieee802154_mlme_ops mac802154_mlme_wpan; + +netdev_tx_t +ieee802154_monitor_start_xmit(struct sk_buff *skb, struct net_device *dev); +netdev_tx_t +ieee802154_subif_start_xmit(struct sk_buff *skb, struct net_device *dev); +enum hrtimer_restart ieee802154_xmit_ifs_timer(struct hrtimer *timer); + +/* MIB callbacks */ +void mac802154_dev_set_short_addr(struct net_device *dev, __le16 val); +__le16 mac802154_dev_get_short_addr(const struct net_device *dev); +__le16 mac802154_dev_get_pan_id(const struct net_device *dev); +void mac802154_dev_set_pan_id(struct net_device *dev, __le16 val); +void mac802154_dev_set_page_channel(struct net_device *dev, u8 page, u8 chan); +u8 mac802154_dev_get_dsn(const struct net_device *dev); + +int mac802154_get_params(struct net_device *dev, + struct ieee802154_llsec_params *params); +int mac802154_set_params(struct net_device *dev, + const struct ieee802154_llsec_params *params, + int changed); + +int mac802154_add_key(struct net_device *dev, + const struct ieee802154_llsec_key_id *id, + const struct ieee802154_llsec_key *key); +int mac802154_del_key(struct net_device *dev, + const struct ieee802154_llsec_key_id *id); + +int mac802154_add_dev(struct net_device *dev, + const struct ieee802154_llsec_device *llsec_dev); +int mac802154_del_dev(struct net_device *dev, __le64 dev_addr); + +int mac802154_add_devkey(struct net_device *dev, + __le64 device_addr, + const struct ieee802154_llsec_device_key *key); +int mac802154_del_devkey(struct net_device *dev, + __le64 device_addr, + const struct ieee802154_llsec_device_key *key); + +int mac802154_add_seclevel(struct net_device *dev, + const struct ieee802154_llsec_seclevel *sl); +int mac802154_del_seclevel(struct net_device *dev, + const struct ieee802154_llsec_seclevel *sl); + +void mac802154_lock_table(struct net_device *dev); +void mac802154_get_table(struct net_device *dev, + struct ieee802154_llsec_table **t); +void mac802154_unlock_table(struct net_device *dev); + +/* interface handling */ +int ieee802154_iface_init(void); +void ieee802154_iface_exit(void); +void ieee802154_if_remove(struct ieee802154_sub_if_data *sdata); +struct net_device * +ieee802154_if_add(struct ieee802154_local *local, const char *name, + unsigned char name_assign_type, enum nl802154_iftype type, + __le64 extended_addr); +void ieee802154_remove_interfaces(struct ieee802154_local *local); + +#endif /* __IEEE802154_I_H */ diff --git a/kernel/net/mac802154/iface.c b/kernel/net/mac802154/iface.c new file mode 100644 index 000000000..91b75abbd --- /dev/null +++ b/kernel/net/mac802154/iface.c @@ -0,0 +1,651 @@ +/* + * Copyright 2007-2012 Siemens AG + * + * 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. + * + * Written by: + * Dmitry Eremin-Solenikov + * Sergey Lapin + * Maxim Gorbachyov + * Alexander Smirnov + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "ieee802154_i.h" +#include "driver-ops.h" + +static int mac802154_wpan_update_llsec(struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev); + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + int rc = 0; + + if (ops->llsec) { + struct ieee802154_llsec_params params; + int changed = 0; + + params.pan_id = wpan_dev->pan_id; + changed |= IEEE802154_LLSEC_PARAM_PAN_ID; + + params.hwaddr = wpan_dev->extended_addr; + changed |= IEEE802154_LLSEC_PARAM_HWADDR; + + rc = ops->llsec->set_params(dev, ¶ms, changed); + } + + return rc; +} + +static int +mac802154_wpan_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + struct sockaddr_ieee802154 *sa = + (struct sockaddr_ieee802154 *)&ifr->ifr_addr; + int err = -ENOIOCTLCMD; + + ASSERT_RTNL(); + + spin_lock_bh(&sdata->mib_lock); + + switch (cmd) { + case SIOCGIFADDR: + { + u16 pan_id, short_addr; + + pan_id = le16_to_cpu(wpan_dev->pan_id); + short_addr = le16_to_cpu(wpan_dev->short_addr); + if (pan_id == IEEE802154_PANID_BROADCAST || + short_addr == IEEE802154_ADDR_BROADCAST) { + err = -EADDRNOTAVAIL; + break; + } + + sa->family = AF_IEEE802154; + sa->addr.addr_type = IEEE802154_ADDR_SHORT; + sa->addr.pan_id = pan_id; + sa->addr.short_addr = short_addr; + + err = 0; + break; + } + case SIOCSIFADDR: + if (netif_running(dev)) { + spin_unlock_bh(&sdata->mib_lock); + return -EBUSY; + } + + dev_warn(&dev->dev, + "Using DEBUGing ioctl SIOCSIFADDR isn't recommended!\n"); + if (sa->family != AF_IEEE802154 || + sa->addr.addr_type != IEEE802154_ADDR_SHORT || + sa->addr.pan_id == IEEE802154_PANID_BROADCAST || + sa->addr.short_addr == IEEE802154_ADDR_BROADCAST || + sa->addr.short_addr == IEEE802154_ADDR_UNDEF) { + err = -EINVAL; + break; + } + + wpan_dev->pan_id = cpu_to_le16(sa->addr.pan_id); + wpan_dev->short_addr = cpu_to_le16(sa->addr.short_addr); + + err = mac802154_wpan_update_llsec(dev); + break; + } + + spin_unlock_bh(&sdata->mib_lock); + return err; +} + +static int mac802154_wpan_mac_addr(struct net_device *dev, void *p) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct sockaddr *addr = p; + __le64 extended_addr; + + if (netif_running(dev)) + return -EBUSY; + + ieee802154_be64_to_le64(&extended_addr, addr->sa_data); + if (!ieee802154_is_valid_extended_addr(extended_addr)) + return -EINVAL; + + memcpy(dev->dev_addr, addr->sa_data, dev->addr_len); + sdata->wpan_dev.extended_addr = extended_addr; + + return mac802154_wpan_update_llsec(dev); +} + +static int mac802154_slave_open(struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct ieee802154_local *local = sdata->local; + int res = 0; + + ASSERT_RTNL(); + + set_bit(SDATA_STATE_RUNNING, &sdata->state); + + if (!local->open_count) { + res = drv_start(local); + WARN_ON(res); + if (res) + goto err; + } + + local->open_count++; + netif_start_queue(dev); + return 0; +err: + /* might already be clear but that doesn't matter */ + clear_bit(SDATA_STATE_RUNNING, &sdata->state); + + return res; +} + +static int +ieee802154_check_mac_settings(struct ieee802154_local *local, + struct wpan_dev *wpan_dev, + struct wpan_dev *nwpan_dev) +{ + ASSERT_RTNL(); + + if (local->hw.flags & IEEE802154_HW_PROMISCUOUS) { + if (wpan_dev->promiscuous_mode != nwpan_dev->promiscuous_mode) + return -EBUSY; + } + + if (local->hw.flags & IEEE802154_HW_AFILT) { + if (wpan_dev->pan_id != nwpan_dev->pan_id || + wpan_dev->short_addr != nwpan_dev->short_addr || + wpan_dev->extended_addr != nwpan_dev->extended_addr) + return -EBUSY; + } + + if (local->hw.flags & IEEE802154_HW_CSMA_PARAMS) { + if (wpan_dev->min_be != nwpan_dev->min_be || + wpan_dev->max_be != nwpan_dev->max_be || + wpan_dev->csma_retries != nwpan_dev->csma_retries) + return -EBUSY; + } + + if (local->hw.flags & IEEE802154_HW_FRAME_RETRIES) { + if (wpan_dev->frame_retries != nwpan_dev->frame_retries) + return -EBUSY; + } + + if (local->hw.flags & IEEE802154_HW_LBT) { + if (wpan_dev->lbt != nwpan_dev->lbt) + return -EBUSY; + } + + return 0; +} + +static int +ieee802154_check_concurrent_iface(struct ieee802154_sub_if_data *sdata, + enum nl802154_iftype iftype) +{ + struct ieee802154_local *local = sdata->local; + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + struct ieee802154_sub_if_data *nsdata; + + /* we hold the RTNL here so can safely walk the list */ + list_for_each_entry(nsdata, &local->interfaces, list) { + if (nsdata != sdata && ieee802154_sdata_running(nsdata)) { + int ret; + + /* TODO currently we don't support multiple node types + * we need to run skb_clone at rx path. Check if there + * exist really an use case if we need to support + * multiple node types at the same time. + */ + if (sdata->vif.type == NL802154_IFTYPE_NODE && + nsdata->vif.type == NL802154_IFTYPE_NODE) + return -EBUSY; + + /* check all phy mac sublayer settings are the same. + * We have only one phy, different values makes trouble. + */ + ret = ieee802154_check_mac_settings(local, wpan_dev, + &nsdata->wpan_dev); + if (ret < 0) + return ret; + } + } + + return 0; +} + +static int mac802154_wpan_open(struct net_device *dev) +{ + int rc; + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct ieee802154_local *local = sdata->local; + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + struct wpan_phy *phy = sdata->local->phy; + + rc = ieee802154_check_concurrent_iface(sdata, sdata->vif.type); + if (rc < 0) + return rc; + + rc = mac802154_slave_open(dev); + if (rc < 0) + return rc; + + mutex_lock(&phy->pib_lock); + + if (local->hw.flags & IEEE802154_HW_PROMISCUOUS) { + rc = drv_set_promiscuous_mode(local, + wpan_dev->promiscuous_mode); + if (rc < 0) + goto out; + } + + if (local->hw.flags & IEEE802154_HW_AFILT) { + rc = drv_set_pan_id(local, wpan_dev->pan_id); + if (rc < 0) + goto out; + + rc = drv_set_extended_addr(local, wpan_dev->extended_addr); + if (rc < 0) + goto out; + + rc = drv_set_short_addr(local, wpan_dev->short_addr); + if (rc < 0) + goto out; + } + + if (local->hw.flags & IEEE802154_HW_LBT) { + rc = drv_set_lbt_mode(local, wpan_dev->lbt); + if (rc < 0) + goto out; + } + + if (local->hw.flags & IEEE802154_HW_CSMA_PARAMS) { + rc = drv_set_csma_params(local, wpan_dev->min_be, + wpan_dev->max_be, + wpan_dev->csma_retries); + if (rc < 0) + goto out; + } + + if (local->hw.flags & IEEE802154_HW_FRAME_RETRIES) { + rc = drv_set_max_frame_retries(local, wpan_dev->frame_retries); + if (rc < 0) + goto out; + } + + mutex_unlock(&phy->pib_lock); + return 0; + +out: + mutex_unlock(&phy->pib_lock); + return rc; +} + +static int mac802154_slave_close(struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct ieee802154_local *local = sdata->local; + + ASSERT_RTNL(); + + hrtimer_cancel(&local->ifs_timer); + + netif_stop_queue(dev); + local->open_count--; + + clear_bit(SDATA_STATE_RUNNING, &sdata->state); + + if (!local->open_count) + drv_stop(local); + + return 0; +} + +static int mac802154_set_header_security(struct ieee802154_sub_if_data *sdata, + struct ieee802154_hdr *hdr, + const struct ieee802154_mac_cb *cb) +{ + struct ieee802154_llsec_params params; + u8 level; + + mac802154_llsec_get_params(&sdata->sec, ¶ms); + + if (!params.enabled && cb->secen_override && cb->secen) + return -EINVAL; + if (!params.enabled || + (cb->secen_override && !cb->secen) || + !params.out_level) + return 0; + if (cb->seclevel_override && !cb->seclevel) + return -EINVAL; + + level = cb->seclevel_override ? cb->seclevel : params.out_level; + + hdr->fc.security_enabled = 1; + hdr->sec.level = level; + hdr->sec.key_id_mode = params.out_key.mode; + if (params.out_key.mode == IEEE802154_SCF_KEY_SHORT_INDEX) + hdr->sec.short_src = params.out_key.short_source; + else if (params.out_key.mode == IEEE802154_SCF_KEY_HW_INDEX) + hdr->sec.extended_src = params.out_key.extended_source; + hdr->sec.key_id = params.out_key.id; + + return 0; +} + +static int mac802154_header_create(struct sk_buff *skb, + struct net_device *dev, + unsigned short type, + const void *daddr, + const void *saddr, + unsigned len) +{ + struct ieee802154_hdr hdr; + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + struct ieee802154_mac_cb *cb = mac_cb(skb); + int hlen; + + if (!daddr) + return -EINVAL; + + memset(&hdr.fc, 0, sizeof(hdr.fc)); + hdr.fc.type = cb->type; + hdr.fc.security_enabled = cb->secen; + hdr.fc.ack_request = cb->ackreq; + hdr.seq = ieee802154_mlme_ops(dev)->get_dsn(dev); + + if (mac802154_set_header_security(sdata, &hdr, cb) < 0) + return -EINVAL; + + if (!saddr) { + spin_lock_bh(&sdata->mib_lock); + + if (wpan_dev->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) || + wpan_dev->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) || + wpan_dev->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) { + hdr.source.mode = IEEE802154_ADDR_LONG; + hdr.source.extended_addr = wpan_dev->extended_addr; + } else { + hdr.source.mode = IEEE802154_ADDR_SHORT; + hdr.source.short_addr = wpan_dev->short_addr; + } + + hdr.source.pan_id = wpan_dev->pan_id; + + spin_unlock_bh(&sdata->mib_lock); + } else { + hdr.source = *(const struct ieee802154_addr *)saddr; + } + + hdr.dest = *(const struct ieee802154_addr *)daddr; + + hlen = ieee802154_hdr_push(skb, &hdr); + if (hlen < 0) + return -EINVAL; + + skb_reset_mac_header(skb); + skb->mac_len = hlen; + + if (len > ieee802154_max_payload(&hdr)) + return -EMSGSIZE; + + return hlen; +} + +static int +mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr) +{ + struct ieee802154_hdr hdr; + struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr; + + if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0) { + pr_debug("malformed packet\n"); + return 0; + } + + *addr = hdr.source; + return sizeof(*addr); +} + +static struct header_ops mac802154_header_ops = { + .create = mac802154_header_create, + .parse = mac802154_header_parse, +}; + +static const struct net_device_ops mac802154_wpan_ops = { + .ndo_open = mac802154_wpan_open, + .ndo_stop = mac802154_slave_close, + .ndo_start_xmit = ieee802154_subif_start_xmit, + .ndo_do_ioctl = mac802154_wpan_ioctl, + .ndo_set_mac_address = mac802154_wpan_mac_addr, +}; + +static const struct net_device_ops mac802154_monitor_ops = { + .ndo_open = mac802154_wpan_open, + .ndo_stop = mac802154_slave_close, + .ndo_start_xmit = ieee802154_monitor_start_xmit, +}; + +static void mac802154_wpan_free(struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + mac802154_llsec_destroy(&sdata->sec); + + free_netdev(dev); +} + +static void ieee802154_if_setup(struct net_device *dev) +{ + dev->addr_len = IEEE802154_EXTENDED_ADDR_LEN; + memset(dev->broadcast, 0xff, IEEE802154_EXTENDED_ADDR_LEN); + + dev->hard_header_len = MAC802154_FRAME_HARD_HEADER_LEN; + dev->needed_tailroom = 2 + 16; /* FCS + MIC */ + dev->mtu = IEEE802154_MTU; + dev->tx_queue_len = 300; + dev->flags = IFF_NOARP | IFF_BROADCAST; +} + +static int +ieee802154_setup_sdata(struct ieee802154_sub_if_data *sdata, + enum nl802154_iftype type) +{ + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + + /* set some type-dependent values */ + sdata->vif.type = type; + sdata->wpan_dev.iftype = type; + + get_random_bytes(&wpan_dev->bsn, 1); + get_random_bytes(&wpan_dev->dsn, 1); + + /* defaults per 802.15.4-2011 */ + wpan_dev->min_be = 3; + wpan_dev->max_be = 5; + wpan_dev->csma_retries = 4; + /* for compatibility, actual default is 3 */ + wpan_dev->frame_retries = -1; + + wpan_dev->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST); + wpan_dev->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST); + + switch (type) { + case NL802154_IFTYPE_NODE: + ieee802154_be64_to_le64(&wpan_dev->extended_addr, + sdata->dev->dev_addr); + + sdata->dev->header_ops = &mac802154_header_ops; + sdata->dev->destructor = mac802154_wpan_free; + sdata->dev->netdev_ops = &mac802154_wpan_ops; + sdata->dev->ml_priv = &mac802154_mlme_wpan; + wpan_dev->promiscuous_mode = false; + + spin_lock_init(&sdata->mib_lock); + mutex_init(&sdata->sec_mtx); + + mac802154_llsec_init(&sdata->sec); + break; + case NL802154_IFTYPE_MONITOR: + sdata->dev->destructor = free_netdev; + sdata->dev->netdev_ops = &mac802154_monitor_ops; + wpan_dev->promiscuous_mode = true; + break; + default: + BUG(); + } + + return 0; +} + +struct net_device * +ieee802154_if_add(struct ieee802154_local *local, const char *name, + unsigned char name_assign_type, enum nl802154_iftype type, + __le64 extended_addr) +{ + struct net_device *ndev = NULL; + struct ieee802154_sub_if_data *sdata = NULL; + int ret = -ENOMEM; + + ASSERT_RTNL(); + + ndev = alloc_netdev(sizeof(*sdata) + local->hw.vif_data_size, name, + name_assign_type, ieee802154_if_setup); + if (!ndev) + return ERR_PTR(-ENOMEM); + + ndev->needed_headroom = local->hw.extra_tx_headroom; + + ret = dev_alloc_name(ndev, ndev->name); + if (ret < 0) + goto err; + + ieee802154_le64_to_be64(ndev->perm_addr, + &local->hw.phy->perm_extended_addr); + switch (type) { + case NL802154_IFTYPE_NODE: + ndev->type = ARPHRD_IEEE802154; + if (ieee802154_is_valid_extended_addr(extended_addr)) + ieee802154_le64_to_be64(ndev->dev_addr, &extended_addr); + else + memcpy(ndev->dev_addr, ndev->perm_addr, + IEEE802154_EXTENDED_ADDR_LEN); + break; + case NL802154_IFTYPE_MONITOR: + ndev->type = ARPHRD_IEEE802154_MONITOR; + break; + default: + ret = -EINVAL; + goto err; + } + + /* TODO check this */ + SET_NETDEV_DEV(ndev, &local->phy->dev); + sdata = netdev_priv(ndev); + ndev->ieee802154_ptr = &sdata->wpan_dev; + memcpy(sdata->name, ndev->name, IFNAMSIZ); + sdata->dev = ndev; + sdata->wpan_dev.wpan_phy = local->hw.phy; + sdata->local = local; + + /* setup type-dependent data */ + ret = ieee802154_setup_sdata(sdata, type); + if (ret) + goto err; + + ret = register_netdevice(ndev); + if (ret < 0) + goto err; + + mutex_lock(&local->iflist_mtx); + list_add_tail_rcu(&sdata->list, &local->interfaces); + mutex_unlock(&local->iflist_mtx); + + return ndev; + +err: + free_netdev(ndev); + return ERR_PTR(ret); +} + +void ieee802154_if_remove(struct ieee802154_sub_if_data *sdata) +{ + ASSERT_RTNL(); + + mutex_lock(&sdata->local->iflist_mtx); + list_del_rcu(&sdata->list); + mutex_unlock(&sdata->local->iflist_mtx); + + synchronize_rcu(); + unregister_netdevice(sdata->dev); +} + +void ieee802154_remove_interfaces(struct ieee802154_local *local) +{ + struct ieee802154_sub_if_data *sdata, *tmp; + + mutex_lock(&local->iflist_mtx); + list_for_each_entry_safe(sdata, tmp, &local->interfaces, list) { + list_del(&sdata->list); + + unregister_netdevice(sdata->dev); + } + mutex_unlock(&local->iflist_mtx); +} + +static int netdev_notify(struct notifier_block *nb, + unsigned long state, void *ptr) +{ + struct net_device *dev = netdev_notifier_info_to_dev(ptr); + struct ieee802154_sub_if_data *sdata; + + if (state != NETDEV_CHANGENAME) + return NOTIFY_DONE; + + if (!dev->ieee802154_ptr || !dev->ieee802154_ptr->wpan_phy) + return NOTIFY_DONE; + + if (dev->ieee802154_ptr->wpan_phy->privid != mac802154_wpan_phy_privid) + return NOTIFY_DONE; + + sdata = IEEE802154_DEV_TO_SUB_IF(dev); + memcpy(sdata->name, dev->name, IFNAMSIZ); + + return NOTIFY_OK; +} + +static struct notifier_block mac802154_netdev_notifier = { + .notifier_call = netdev_notify, +}; + +int ieee802154_iface_init(void) +{ + return register_netdevice_notifier(&mac802154_netdev_notifier); +} + +void ieee802154_iface_exit(void) +{ + unregister_netdevice_notifier(&mac802154_netdev_notifier); +} diff --git a/kernel/net/mac802154/llsec.c b/kernel/net/mac802154/llsec.c new file mode 100644 index 000000000..5b2be1283 --- /dev/null +++ b/kernel/net/mac802154/llsec.c @@ -0,0 +1,1056 @@ +/* + * Copyright (C) 2014 Fraunhofer ITWM + * + * 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. + * + * Written by: + * Phoebe Buckheister + */ + +#include +#include +#include +#include +#include + +#include "ieee802154_i.h" +#include "llsec.h" + +static void llsec_key_put(struct mac802154_llsec_key *key); +static bool llsec_key_id_equal(const struct ieee802154_llsec_key_id *a, + const struct ieee802154_llsec_key_id *b); + +static void llsec_dev_free(struct mac802154_llsec_device *dev); + +void mac802154_llsec_init(struct mac802154_llsec *sec) +{ + memset(sec, 0, sizeof(*sec)); + + memset(&sec->params.default_key_source, 0xFF, IEEE802154_ADDR_LEN); + + INIT_LIST_HEAD(&sec->table.security_levels); + INIT_LIST_HEAD(&sec->table.devices); + INIT_LIST_HEAD(&sec->table.keys); + hash_init(sec->devices_short); + hash_init(sec->devices_hw); + rwlock_init(&sec->lock); +} + +void mac802154_llsec_destroy(struct mac802154_llsec *sec) +{ + struct ieee802154_llsec_seclevel *sl, *sn; + struct ieee802154_llsec_device *dev, *dn; + struct ieee802154_llsec_key_entry *key, *kn; + + list_for_each_entry_safe(sl, sn, &sec->table.security_levels, list) { + struct mac802154_llsec_seclevel *msl; + + msl = container_of(sl, struct mac802154_llsec_seclevel, level); + list_del(&sl->list); + kfree(msl); + } + + list_for_each_entry_safe(dev, dn, &sec->table.devices, list) { + struct mac802154_llsec_device *mdev; + + mdev = container_of(dev, struct mac802154_llsec_device, dev); + list_del(&dev->list); + llsec_dev_free(mdev); + } + + list_for_each_entry_safe(key, kn, &sec->table.keys, list) { + struct mac802154_llsec_key *mkey; + + mkey = container_of(key->key, struct mac802154_llsec_key, key); + list_del(&key->list); + llsec_key_put(mkey); + kfree(key); + } +} + +int mac802154_llsec_get_params(struct mac802154_llsec *sec, + struct ieee802154_llsec_params *params) +{ + read_lock_bh(&sec->lock); + *params = sec->params; + read_unlock_bh(&sec->lock); + + return 0; +} + +int mac802154_llsec_set_params(struct mac802154_llsec *sec, + const struct ieee802154_llsec_params *params, + int changed) +{ + write_lock_bh(&sec->lock); + + if (changed & IEEE802154_LLSEC_PARAM_ENABLED) + sec->params.enabled = params->enabled; + if (changed & IEEE802154_LLSEC_PARAM_FRAME_COUNTER) + sec->params.frame_counter = params->frame_counter; + if (changed & IEEE802154_LLSEC_PARAM_OUT_LEVEL) + sec->params.out_level = params->out_level; + if (changed & IEEE802154_LLSEC_PARAM_OUT_KEY) + sec->params.out_key = params->out_key; + if (changed & IEEE802154_LLSEC_PARAM_KEY_SOURCE) + sec->params.default_key_source = params->default_key_source; + if (changed & IEEE802154_LLSEC_PARAM_PAN_ID) + sec->params.pan_id = params->pan_id; + if (changed & IEEE802154_LLSEC_PARAM_HWADDR) + sec->params.hwaddr = params->hwaddr; + if (changed & IEEE802154_LLSEC_PARAM_COORD_HWADDR) + sec->params.coord_hwaddr = params->coord_hwaddr; + if (changed & IEEE802154_LLSEC_PARAM_COORD_SHORTADDR) + sec->params.coord_shortaddr = params->coord_shortaddr; + + write_unlock_bh(&sec->lock); + + return 0; +} + +static struct mac802154_llsec_key* +llsec_key_alloc(const struct ieee802154_llsec_key *template) +{ + const int authsizes[3] = { 4, 8, 16 }; + struct mac802154_llsec_key *key; + int i; + + key = kzalloc(sizeof(*key), GFP_KERNEL); + if (!key) + return NULL; + + kref_init(&key->ref); + key->key = *template; + + BUILD_BUG_ON(ARRAY_SIZE(authsizes) != ARRAY_SIZE(key->tfm)); + + for (i = 0; i < ARRAY_SIZE(key->tfm); i++) { + key->tfm[i] = crypto_alloc_aead("ccm(aes)", 0, + CRYPTO_ALG_ASYNC); + if (IS_ERR(key->tfm[i])) + goto err_tfm; + if (crypto_aead_setkey(key->tfm[i], template->key, + IEEE802154_LLSEC_KEY_SIZE)) + goto err_tfm; + if (crypto_aead_setauthsize(key->tfm[i], authsizes[i])) + goto err_tfm; + } + + key->tfm0 = crypto_alloc_blkcipher("ctr(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(key->tfm0)) + goto err_tfm; + + if (crypto_blkcipher_setkey(key->tfm0, template->key, + IEEE802154_LLSEC_KEY_SIZE)) + goto err_tfm0; + + return key; + +err_tfm0: + crypto_free_blkcipher(key->tfm0); +err_tfm: + for (i = 0; i < ARRAY_SIZE(key->tfm); i++) + if (key->tfm[i]) + crypto_free_aead(key->tfm[i]); + + kfree(key); + return NULL; +} + +static void llsec_key_release(struct kref *ref) +{ + struct mac802154_llsec_key *key; + int i; + + key = container_of(ref, struct mac802154_llsec_key, ref); + + for (i = 0; i < ARRAY_SIZE(key->tfm); i++) + crypto_free_aead(key->tfm[i]); + + crypto_free_blkcipher(key->tfm0); + kfree(key); +} + +static struct mac802154_llsec_key* +llsec_key_get(struct mac802154_llsec_key *key) +{ + kref_get(&key->ref); + return key; +} + +static void llsec_key_put(struct mac802154_llsec_key *key) +{ + kref_put(&key->ref, llsec_key_release); +} + +static bool llsec_key_id_equal(const struct ieee802154_llsec_key_id *a, + const struct ieee802154_llsec_key_id *b) +{ + if (a->mode != b->mode) + return false; + + if (a->mode == IEEE802154_SCF_KEY_IMPLICIT) + return ieee802154_addr_equal(&a->device_addr, &b->device_addr); + + if (a->id != b->id) + return false; + + switch (a->mode) { + case IEEE802154_SCF_KEY_INDEX: + return true; + case IEEE802154_SCF_KEY_SHORT_INDEX: + return a->short_source == b->short_source; + case IEEE802154_SCF_KEY_HW_INDEX: + return a->extended_source == b->extended_source; + } + + return false; +} + +int mac802154_llsec_key_add(struct mac802154_llsec *sec, + const struct ieee802154_llsec_key_id *id, + const struct ieee802154_llsec_key *key) +{ + struct mac802154_llsec_key *mkey = NULL; + struct ieee802154_llsec_key_entry *pos, *new; + + if (!(key->frame_types & (1 << IEEE802154_FC_TYPE_MAC_CMD)) && + key->cmd_frame_ids) + return -EINVAL; + + list_for_each_entry(pos, &sec->table.keys, list) { + if (llsec_key_id_equal(&pos->id, id)) + return -EEXIST; + + if (memcmp(pos->key->key, key->key, + IEEE802154_LLSEC_KEY_SIZE)) + continue; + + mkey = container_of(pos->key, struct mac802154_llsec_key, key); + + /* Don't allow multiple instances of the same AES key to have + * different allowed frame types/command frame ids, as this is + * not possible in the 802.15.4 PIB. + */ + if (pos->key->frame_types != key->frame_types || + pos->key->cmd_frame_ids != key->cmd_frame_ids) + return -EEXIST; + + break; + } + + new = kzalloc(sizeof(*new), GFP_KERNEL); + if (!new) + return -ENOMEM; + + if (!mkey) + mkey = llsec_key_alloc(key); + else + mkey = llsec_key_get(mkey); + + if (!mkey) + goto fail; + + new->id = *id; + new->key = &mkey->key; + + list_add_rcu(&new->list, &sec->table.keys); + + return 0; + +fail: + kfree(new); + return -ENOMEM; +} + +int mac802154_llsec_key_del(struct mac802154_llsec *sec, + const struct ieee802154_llsec_key_id *key) +{ + struct ieee802154_llsec_key_entry *pos; + + list_for_each_entry(pos, &sec->table.keys, list) { + struct mac802154_llsec_key *mkey; + + mkey = container_of(pos->key, struct mac802154_llsec_key, key); + + if (llsec_key_id_equal(&pos->id, key)) { + list_del_rcu(&pos->list); + llsec_key_put(mkey); + return 0; + } + } + + return -ENOENT; +} + +static bool llsec_dev_use_shortaddr(__le16 short_addr) +{ + return short_addr != cpu_to_le16(IEEE802154_ADDR_UNDEF) && + short_addr != cpu_to_le16(0xffff); +} + +static u32 llsec_dev_hash_short(__le16 short_addr, __le16 pan_id) +{ + return ((__force u16)short_addr) << 16 | (__force u16)pan_id; +} + +static u64 llsec_dev_hash_long(__le64 hwaddr) +{ + return (__force u64)hwaddr; +} + +static struct mac802154_llsec_device* +llsec_dev_find_short(struct mac802154_llsec *sec, __le16 short_addr, + __le16 pan_id) +{ + struct mac802154_llsec_device *dev; + u32 key = llsec_dev_hash_short(short_addr, pan_id); + + hash_for_each_possible_rcu(sec->devices_short, dev, bucket_s, key) { + if (dev->dev.short_addr == short_addr && + dev->dev.pan_id == pan_id) + return dev; + } + + return NULL; +} + +static struct mac802154_llsec_device* +llsec_dev_find_long(struct mac802154_llsec *sec, __le64 hwaddr) +{ + struct mac802154_llsec_device *dev; + u64 key = llsec_dev_hash_long(hwaddr); + + hash_for_each_possible_rcu(sec->devices_hw, dev, bucket_hw, key) { + if (dev->dev.hwaddr == hwaddr) + return dev; + } + + return NULL; +} + +static void llsec_dev_free(struct mac802154_llsec_device *dev) +{ + struct ieee802154_llsec_device_key *pos, *pn; + struct mac802154_llsec_device_key *devkey; + + list_for_each_entry_safe(pos, pn, &dev->dev.keys, list) { + devkey = container_of(pos, struct mac802154_llsec_device_key, + devkey); + + list_del(&pos->list); + kfree(devkey); + } + + kfree(dev); +} + +int mac802154_llsec_dev_add(struct mac802154_llsec *sec, + const struct ieee802154_llsec_device *dev) +{ + struct mac802154_llsec_device *entry; + u32 skey = llsec_dev_hash_short(dev->short_addr, dev->pan_id); + u64 hwkey = llsec_dev_hash_long(dev->hwaddr); + + BUILD_BUG_ON(sizeof(hwkey) != IEEE802154_ADDR_LEN); + + if ((llsec_dev_use_shortaddr(dev->short_addr) && + llsec_dev_find_short(sec, dev->short_addr, dev->pan_id)) || + llsec_dev_find_long(sec, dev->hwaddr)) + return -EEXIST; + + entry = kmalloc(sizeof(*entry), GFP_KERNEL); + if (!entry) + return -ENOMEM; + + entry->dev = *dev; + spin_lock_init(&entry->lock); + INIT_LIST_HEAD(&entry->dev.keys); + + if (llsec_dev_use_shortaddr(dev->short_addr)) + hash_add_rcu(sec->devices_short, &entry->bucket_s, skey); + else + INIT_HLIST_NODE(&entry->bucket_s); + + hash_add_rcu(sec->devices_hw, &entry->bucket_hw, hwkey); + list_add_tail_rcu(&entry->dev.list, &sec->table.devices); + + return 0; +} + +static void llsec_dev_free_rcu(struct rcu_head *rcu) +{ + llsec_dev_free(container_of(rcu, struct mac802154_llsec_device, rcu)); +} + +int mac802154_llsec_dev_del(struct mac802154_llsec *sec, __le64 device_addr) +{ + struct mac802154_llsec_device *pos; + + pos = llsec_dev_find_long(sec, device_addr); + if (!pos) + return -ENOENT; + + hash_del_rcu(&pos->bucket_s); + hash_del_rcu(&pos->bucket_hw); + call_rcu(&pos->rcu, llsec_dev_free_rcu); + + return 0; +} + +static struct mac802154_llsec_device_key* +llsec_devkey_find(struct mac802154_llsec_device *dev, + const struct ieee802154_llsec_key_id *key) +{ + struct ieee802154_llsec_device_key *devkey; + + list_for_each_entry_rcu(devkey, &dev->dev.keys, list) { + if (!llsec_key_id_equal(key, &devkey->key_id)) + continue; + + return container_of(devkey, struct mac802154_llsec_device_key, + devkey); + } + + return NULL; +} + +int mac802154_llsec_devkey_add(struct mac802154_llsec *sec, + __le64 dev_addr, + const struct ieee802154_llsec_device_key *key) +{ + struct mac802154_llsec_device *dev; + struct mac802154_llsec_device_key *devkey; + + dev = llsec_dev_find_long(sec, dev_addr); + + if (!dev) + return -ENOENT; + + if (llsec_devkey_find(dev, &key->key_id)) + return -EEXIST; + + devkey = kmalloc(sizeof(*devkey), GFP_KERNEL); + if (!devkey) + return -ENOMEM; + + devkey->devkey = *key; + list_add_tail_rcu(&devkey->devkey.list, &dev->dev.keys); + return 0; +} + +int mac802154_llsec_devkey_del(struct mac802154_llsec *sec, + __le64 dev_addr, + const struct ieee802154_llsec_device_key *key) +{ + struct mac802154_llsec_device *dev; + struct mac802154_llsec_device_key *devkey; + + dev = llsec_dev_find_long(sec, dev_addr); + + if (!dev) + return -ENOENT; + + devkey = llsec_devkey_find(dev, &key->key_id); + if (!devkey) + return -ENOENT; + + list_del_rcu(&devkey->devkey.list); + kfree_rcu(devkey, rcu); + return 0; +} + +static struct mac802154_llsec_seclevel* +llsec_find_seclevel(const struct mac802154_llsec *sec, + const struct ieee802154_llsec_seclevel *sl) +{ + struct ieee802154_llsec_seclevel *pos; + + list_for_each_entry(pos, &sec->table.security_levels, list) { + if (pos->frame_type != sl->frame_type || + (pos->frame_type == IEEE802154_FC_TYPE_MAC_CMD && + pos->cmd_frame_id != sl->cmd_frame_id) || + pos->device_override != sl->device_override || + pos->sec_levels != sl->sec_levels) + continue; + + return container_of(pos, struct mac802154_llsec_seclevel, + level); + } + + return NULL; +} + +int mac802154_llsec_seclevel_add(struct mac802154_llsec *sec, + const struct ieee802154_llsec_seclevel *sl) +{ + struct mac802154_llsec_seclevel *entry; + + if (llsec_find_seclevel(sec, sl)) + return -EEXIST; + + entry = kmalloc(sizeof(*entry), GFP_KERNEL); + if (!entry) + return -ENOMEM; + + entry->level = *sl; + + list_add_tail_rcu(&entry->level.list, &sec->table.security_levels); + + return 0; +} + +int mac802154_llsec_seclevel_del(struct mac802154_llsec *sec, + const struct ieee802154_llsec_seclevel *sl) +{ + struct mac802154_llsec_seclevel *pos; + + pos = llsec_find_seclevel(sec, sl); + if (!pos) + return -ENOENT; + + list_del_rcu(&pos->level.list); + kfree_rcu(pos, rcu); + + return 0; +} + +static int llsec_recover_addr(struct mac802154_llsec *sec, + struct ieee802154_addr *addr) +{ + __le16 caddr = sec->params.coord_shortaddr; + + addr->pan_id = sec->params.pan_id; + + if (caddr == cpu_to_le16(IEEE802154_ADDR_BROADCAST)) { + return -EINVAL; + } else if (caddr == cpu_to_le16(IEEE802154_ADDR_UNDEF)) { + addr->extended_addr = sec->params.coord_hwaddr; + addr->mode = IEEE802154_ADDR_LONG; + } else { + addr->short_addr = sec->params.coord_shortaddr; + addr->mode = IEEE802154_ADDR_SHORT; + } + + return 0; +} + +static struct mac802154_llsec_key* +llsec_lookup_key(struct mac802154_llsec *sec, + const struct ieee802154_hdr *hdr, + const struct ieee802154_addr *addr, + struct ieee802154_llsec_key_id *key_id) +{ + struct ieee802154_addr devaddr = *addr; + u8 key_id_mode = hdr->sec.key_id_mode; + struct ieee802154_llsec_key_entry *key_entry; + struct mac802154_llsec_key *key; + + if (key_id_mode == IEEE802154_SCF_KEY_IMPLICIT && + devaddr.mode == IEEE802154_ADDR_NONE) { + if (hdr->fc.type == IEEE802154_FC_TYPE_BEACON) { + devaddr.extended_addr = sec->params.coord_hwaddr; + devaddr.mode = IEEE802154_ADDR_LONG; + } else if (llsec_recover_addr(sec, &devaddr) < 0) { + return NULL; + } + } + + list_for_each_entry_rcu(key_entry, &sec->table.keys, list) { + const struct ieee802154_llsec_key_id *id = &key_entry->id; + + if (!(key_entry->key->frame_types & BIT(hdr->fc.type))) + continue; + + if (id->mode != key_id_mode) + continue; + + if (key_id_mode == IEEE802154_SCF_KEY_IMPLICIT) { + if (ieee802154_addr_equal(&devaddr, &id->device_addr)) + goto found; + } else { + if (id->id != hdr->sec.key_id) + continue; + + if ((key_id_mode == IEEE802154_SCF_KEY_INDEX) || + (key_id_mode == IEEE802154_SCF_KEY_SHORT_INDEX && + id->short_source == hdr->sec.short_src) || + (key_id_mode == IEEE802154_SCF_KEY_HW_INDEX && + id->extended_source == hdr->sec.extended_src)) + goto found; + } + } + + return NULL; + +found: + key = container_of(key_entry->key, struct mac802154_llsec_key, key); + if (key_id) + *key_id = key_entry->id; + return llsec_key_get(key); +} + +static void llsec_geniv(u8 iv[16], __le64 addr, + const struct ieee802154_sechdr *sec) +{ + __be64 addr_bytes = (__force __be64) swab64((__force u64) addr); + __be32 frame_counter = (__force __be32) swab32((__force u32) sec->frame_counter); + + iv[0] = 1; /* L' = L - 1 = 1 */ + memcpy(iv + 1, &addr_bytes, sizeof(addr_bytes)); + memcpy(iv + 9, &frame_counter, sizeof(frame_counter)); + iv[13] = sec->level; + iv[14] = 0; + iv[15] = 1; +} + +static int +llsec_do_encrypt_unauth(struct sk_buff *skb, const struct mac802154_llsec *sec, + const struct ieee802154_hdr *hdr, + struct mac802154_llsec_key *key) +{ + u8 iv[16]; + struct scatterlist src; + struct blkcipher_desc req = { + .tfm = key->tfm0, + .info = iv, + .flags = 0, + }; + + llsec_geniv(iv, sec->params.hwaddr, &hdr->sec); + sg_init_one(&src, skb->data, skb->len); + return crypto_blkcipher_encrypt_iv(&req, &src, &src, skb->len); +} + +static struct crypto_aead* +llsec_tfm_by_len(struct mac802154_llsec_key *key, int authlen) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(key->tfm); i++) + if (crypto_aead_authsize(key->tfm[i]) == authlen) + return key->tfm[i]; + + BUG(); +} + +static int +llsec_do_encrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec, + const struct ieee802154_hdr *hdr, + struct mac802154_llsec_key *key) +{ + u8 iv[16]; + unsigned char *data; + int authlen, assoclen, datalen, rc; + struct scatterlist src, assoc[2], dst[2]; + struct aead_request *req; + + authlen = ieee802154_sechdr_authtag_len(&hdr->sec); + llsec_geniv(iv, sec->params.hwaddr, &hdr->sec); + + req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC); + if (!req) + return -ENOMEM; + + sg_init_table(assoc, 2); + sg_set_buf(&assoc[0], skb_mac_header(skb), skb->mac_len); + assoclen = skb->mac_len; + + data = skb_mac_header(skb) + skb->mac_len; + datalen = skb_tail_pointer(skb) - data; + + if (hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC) { + sg_set_buf(&assoc[1], data, 0); + } else { + sg_set_buf(&assoc[1], data, datalen); + assoclen += datalen; + datalen = 0; + } + + sg_init_one(&src, data, datalen); + + sg_init_table(dst, 2); + sg_set_buf(&dst[0], data, datalen); + sg_set_buf(&dst[1], skb_put(skb, authlen), authlen); + + aead_request_set_callback(req, 0, NULL, NULL); + aead_request_set_assoc(req, assoc, assoclen); + aead_request_set_crypt(req, &src, dst, datalen, iv); + + rc = crypto_aead_encrypt(req); + + kfree(req); + + return rc; +} + +static int llsec_do_encrypt(struct sk_buff *skb, + const struct mac802154_llsec *sec, + const struct ieee802154_hdr *hdr, + struct mac802154_llsec_key *key) +{ + if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC) + return llsec_do_encrypt_unauth(skb, sec, hdr, key); + else + return llsec_do_encrypt_auth(skb, sec, hdr, key); +} + +int mac802154_llsec_encrypt(struct mac802154_llsec *sec, struct sk_buff *skb) +{ + struct ieee802154_hdr hdr; + int rc, authlen, hlen; + struct mac802154_llsec_key *key; + u32 frame_ctr; + + hlen = ieee802154_hdr_pull(skb, &hdr); + + if (hlen < 0 || hdr.fc.type != IEEE802154_FC_TYPE_DATA) + return -EINVAL; + + if (!hdr.fc.security_enabled || hdr.sec.level == 0) { + skb_push(skb, hlen); + return 0; + } + + authlen = ieee802154_sechdr_authtag_len(&hdr.sec); + + if (skb->len + hlen + authlen + IEEE802154_MFR_SIZE > IEEE802154_MTU) + return -EMSGSIZE; + + rcu_read_lock(); + + read_lock_bh(&sec->lock); + + if (!sec->params.enabled) { + rc = -EINVAL; + goto fail_read; + } + + key = llsec_lookup_key(sec, &hdr, &hdr.dest, NULL); + if (!key) { + rc = -ENOKEY; + goto fail_read; + } + + read_unlock_bh(&sec->lock); + + write_lock_bh(&sec->lock); + + frame_ctr = be32_to_cpu(sec->params.frame_counter); + hdr.sec.frame_counter = cpu_to_le32(frame_ctr); + if (frame_ctr == 0xFFFFFFFF) { + write_unlock_bh(&sec->lock); + llsec_key_put(key); + rc = -EOVERFLOW; + goto fail; + } + + sec->params.frame_counter = cpu_to_be32(frame_ctr + 1); + + write_unlock_bh(&sec->lock); + + rcu_read_unlock(); + + skb->mac_len = ieee802154_hdr_push(skb, &hdr); + skb_reset_mac_header(skb); + + rc = llsec_do_encrypt(skb, sec, &hdr, key); + llsec_key_put(key); + + return rc; + +fail_read: + read_unlock_bh(&sec->lock); +fail: + rcu_read_unlock(); + return rc; +} + +static struct mac802154_llsec_device* +llsec_lookup_dev(struct mac802154_llsec *sec, + const struct ieee802154_addr *addr) +{ + struct ieee802154_addr devaddr = *addr; + struct mac802154_llsec_device *dev = NULL; + + if (devaddr.mode == IEEE802154_ADDR_NONE && + llsec_recover_addr(sec, &devaddr) < 0) + return NULL; + + if (devaddr.mode == IEEE802154_ADDR_SHORT) { + u32 key = llsec_dev_hash_short(devaddr.short_addr, + devaddr.pan_id); + + hash_for_each_possible_rcu(sec->devices_short, dev, + bucket_s, key) { + if (dev->dev.pan_id == devaddr.pan_id && + dev->dev.short_addr == devaddr.short_addr) + return dev; + } + } else { + u64 key = llsec_dev_hash_long(devaddr.extended_addr); + + hash_for_each_possible_rcu(sec->devices_hw, dev, + bucket_hw, key) { + if (dev->dev.hwaddr == devaddr.extended_addr) + return dev; + } + } + + return NULL; +} + +static int +llsec_lookup_seclevel(const struct mac802154_llsec *sec, + u8 frame_type, u8 cmd_frame_id, + struct ieee802154_llsec_seclevel *rlevel) +{ + struct ieee802154_llsec_seclevel *level; + + list_for_each_entry_rcu(level, &sec->table.security_levels, list) { + if (level->frame_type == frame_type && + (frame_type != IEEE802154_FC_TYPE_MAC_CMD || + level->cmd_frame_id == cmd_frame_id)) { + *rlevel = *level; + return 0; + } + } + + return -EINVAL; +} + +static int +llsec_do_decrypt_unauth(struct sk_buff *skb, const struct mac802154_llsec *sec, + const struct ieee802154_hdr *hdr, + struct mac802154_llsec_key *key, __le64 dev_addr) +{ + u8 iv[16]; + unsigned char *data; + int datalen; + struct scatterlist src; + struct blkcipher_desc req = { + .tfm = key->tfm0, + .info = iv, + .flags = 0, + }; + + llsec_geniv(iv, dev_addr, &hdr->sec); + data = skb_mac_header(skb) + skb->mac_len; + datalen = skb_tail_pointer(skb) - data; + + sg_init_one(&src, data, datalen); + + return crypto_blkcipher_decrypt_iv(&req, &src, &src, datalen); +} + +static int +llsec_do_decrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec, + const struct ieee802154_hdr *hdr, + struct mac802154_llsec_key *key, __le64 dev_addr) +{ + u8 iv[16]; + unsigned char *data; + int authlen, datalen, assoclen, rc; + struct scatterlist src, assoc[2]; + struct aead_request *req; + + authlen = ieee802154_sechdr_authtag_len(&hdr->sec); + llsec_geniv(iv, dev_addr, &hdr->sec); + + req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC); + if (!req) + return -ENOMEM; + + sg_init_table(assoc, 2); + sg_set_buf(&assoc[0], skb_mac_header(skb), skb->mac_len); + assoclen = skb->mac_len; + + data = skb_mac_header(skb) + skb->mac_len; + datalen = skb_tail_pointer(skb) - data; + + if (hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC) { + sg_set_buf(&assoc[1], data, 0); + } else { + sg_set_buf(&assoc[1], data, datalen - authlen); + assoclen += datalen - authlen; + data += datalen - authlen; + datalen = authlen; + } + + sg_init_one(&src, data, datalen); + + aead_request_set_callback(req, 0, NULL, NULL); + aead_request_set_assoc(req, assoc, assoclen); + aead_request_set_crypt(req, &src, &src, datalen, iv); + + rc = crypto_aead_decrypt(req); + + kfree(req); + skb_trim(skb, skb->len - authlen); + + return rc; +} + +static int +llsec_do_decrypt(struct sk_buff *skb, const struct mac802154_llsec *sec, + const struct ieee802154_hdr *hdr, + struct mac802154_llsec_key *key, __le64 dev_addr) +{ + if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC) + return llsec_do_decrypt_unauth(skb, sec, hdr, key, dev_addr); + else + return llsec_do_decrypt_auth(skb, sec, hdr, key, dev_addr); +} + +static int +llsec_update_devkey_record(struct mac802154_llsec_device *dev, + const struct ieee802154_llsec_key_id *in_key) +{ + struct mac802154_llsec_device_key *devkey; + + devkey = llsec_devkey_find(dev, in_key); + + if (!devkey) { + struct mac802154_llsec_device_key *next; + + next = kzalloc(sizeof(*devkey), GFP_ATOMIC); + if (!next) + return -ENOMEM; + + next->devkey.key_id = *in_key; + + spin_lock_bh(&dev->lock); + + devkey = llsec_devkey_find(dev, in_key); + if (!devkey) + list_add_rcu(&next->devkey.list, &dev->dev.keys); + else + kfree(next); + + spin_unlock_bh(&dev->lock); + } + + return 0; +} + +static int +llsec_update_devkey_info(struct mac802154_llsec_device *dev, + const struct ieee802154_llsec_key_id *in_key, + u32 frame_counter) +{ + struct mac802154_llsec_device_key *devkey = NULL; + + if (dev->dev.key_mode == IEEE802154_LLSEC_DEVKEY_RESTRICT) { + devkey = llsec_devkey_find(dev, in_key); + if (!devkey) + return -ENOENT; + } + + if (dev->dev.key_mode == IEEE802154_LLSEC_DEVKEY_RECORD) { + int rc = llsec_update_devkey_record(dev, in_key); + + if (rc < 0) + return rc; + } + + spin_lock_bh(&dev->lock); + + if ((!devkey && frame_counter < dev->dev.frame_counter) || + (devkey && frame_counter < devkey->devkey.frame_counter)) { + spin_unlock_bh(&dev->lock); + return -EINVAL; + } + + if (devkey) + devkey->devkey.frame_counter = frame_counter + 1; + else + dev->dev.frame_counter = frame_counter + 1; + + spin_unlock_bh(&dev->lock); + + return 0; +} + +int mac802154_llsec_decrypt(struct mac802154_llsec *sec, struct sk_buff *skb) +{ + struct ieee802154_hdr hdr; + struct mac802154_llsec_key *key; + struct ieee802154_llsec_key_id key_id; + struct mac802154_llsec_device *dev; + struct ieee802154_llsec_seclevel seclevel; + int err; + __le64 dev_addr; + u32 frame_ctr; + + if (ieee802154_hdr_peek(skb, &hdr) < 0) + return -EINVAL; + if (!hdr.fc.security_enabled) + return 0; + if (hdr.fc.version == 0) + return -EINVAL; + + read_lock_bh(&sec->lock); + if (!sec->params.enabled) { + read_unlock_bh(&sec->lock); + return -EINVAL; + } + read_unlock_bh(&sec->lock); + + rcu_read_lock(); + + key = llsec_lookup_key(sec, &hdr, &hdr.source, &key_id); + if (!key) { + err = -ENOKEY; + goto fail; + } + + dev = llsec_lookup_dev(sec, &hdr.source); + if (!dev) { + err = -EINVAL; + goto fail_dev; + } + + if (llsec_lookup_seclevel(sec, hdr.fc.type, 0, &seclevel) < 0) { + err = -EINVAL; + goto fail_dev; + } + + if (!(seclevel.sec_levels & BIT(hdr.sec.level)) && + (hdr.sec.level == 0 && seclevel.device_override && + !dev->dev.seclevel_exempt)) { + err = -EINVAL; + goto fail_dev; + } + + frame_ctr = le32_to_cpu(hdr.sec.frame_counter); + + if (frame_ctr == 0xffffffff) { + err = -EOVERFLOW; + goto fail_dev; + } + + err = llsec_update_devkey_info(dev, &key_id, frame_ctr); + if (err) + goto fail_dev; + + dev_addr = dev->dev.hwaddr; + + rcu_read_unlock(); + + err = llsec_do_decrypt(skb, sec, &hdr, key, dev_addr); + llsec_key_put(key); + return err; + +fail_dev: + llsec_key_put(key); +fail: + rcu_read_unlock(); + return err; +} diff --git a/kernel/net/mac802154/llsec.h b/kernel/net/mac802154/llsec.h new file mode 100644 index 000000000..950578e1d --- /dev/null +++ b/kernel/net/mac802154/llsec.h @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2014 Fraunhofer ITWM + * + * 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. + * + * Written by: + * Phoebe Buckheister + */ + +#ifndef MAC802154_LLSEC_H +#define MAC802154_LLSEC_H + +#include +#include +#include +#include +#include +#include +#include + +struct mac802154_llsec_key { + struct ieee802154_llsec_key key; + + /* one tfm for each authsize (4/8/16) */ + struct crypto_aead *tfm[3]; + struct crypto_blkcipher *tfm0; + + struct kref ref; +}; + +struct mac802154_llsec_device_key { + struct ieee802154_llsec_device_key devkey; + + struct rcu_head rcu; +}; + +struct mac802154_llsec_device { + struct ieee802154_llsec_device dev; + + struct hlist_node bucket_s; + struct hlist_node bucket_hw; + + /* protects dev.frame_counter and the elements of dev.keys */ + spinlock_t lock; + + struct rcu_head rcu; +}; + +struct mac802154_llsec_seclevel { + struct ieee802154_llsec_seclevel level; + + struct rcu_head rcu; +}; + +struct mac802154_llsec { + struct ieee802154_llsec_params params; + struct ieee802154_llsec_table table; + + DECLARE_HASHTABLE(devices_short, 6); + DECLARE_HASHTABLE(devices_hw, 6); + + /* protects params, all other fields are fine with RCU */ + rwlock_t lock; +}; + +void mac802154_llsec_init(struct mac802154_llsec *sec); +void mac802154_llsec_destroy(struct mac802154_llsec *sec); + +int mac802154_llsec_get_params(struct mac802154_llsec *sec, + struct ieee802154_llsec_params *params); +int mac802154_llsec_set_params(struct mac802154_llsec *sec, + const struct ieee802154_llsec_params *params, + int changed); + +int mac802154_llsec_key_add(struct mac802154_llsec *sec, + const struct ieee802154_llsec_key_id *id, + const struct ieee802154_llsec_key *key); +int mac802154_llsec_key_del(struct mac802154_llsec *sec, + const struct ieee802154_llsec_key_id *key); + +int mac802154_llsec_dev_add(struct mac802154_llsec *sec, + const struct ieee802154_llsec_device *dev); +int mac802154_llsec_dev_del(struct mac802154_llsec *sec, + __le64 device_addr); + +int mac802154_llsec_devkey_add(struct mac802154_llsec *sec, + __le64 dev_addr, + const struct ieee802154_llsec_device_key *key); +int mac802154_llsec_devkey_del(struct mac802154_llsec *sec, + __le64 dev_addr, + const struct ieee802154_llsec_device_key *key); + +int mac802154_llsec_seclevel_add(struct mac802154_llsec *sec, + const struct ieee802154_llsec_seclevel *sl); +int mac802154_llsec_seclevel_del(struct mac802154_llsec *sec, + const struct ieee802154_llsec_seclevel *sl); + +int mac802154_llsec_encrypt(struct mac802154_llsec *sec, struct sk_buff *skb); +int mac802154_llsec_decrypt(struct mac802154_llsec *sec, struct sk_buff *skb); + +#endif /* MAC802154_LLSEC_H */ diff --git a/kernel/net/mac802154/mac_cmd.c b/kernel/net/mac802154/mac_cmd.c new file mode 100644 index 000000000..bdccb4ecd --- /dev/null +++ b/kernel/net/mac802154/mac_cmd.c @@ -0,0 +1,162 @@ +/* + * MAC commands interface + * + * Copyright 2007-2012 Siemens AG + * + * 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. + * + * Written by: + * Sergey Lapin + * Dmitry Eremin-Solenikov + * Alexander Smirnov + */ + +#include +#include +#include + +#include +#include +#include + +#include "ieee802154_i.h" +#include "driver-ops.h" + +static int mac802154_mlme_start_req(struct net_device *dev, + struct ieee802154_addr *addr, + u8 channel, u8 page, + u8 bcn_ord, u8 sf_ord, + u8 pan_coord, u8 blx, + u8 coord_realign) +{ + struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev); + int rc = 0; + + ASSERT_RTNL(); + + BUG_ON(addr->mode != IEEE802154_ADDR_SHORT); + + mac802154_dev_set_pan_id(dev, addr->pan_id); + mac802154_dev_set_short_addr(dev, addr->short_addr); + mac802154_dev_set_page_channel(dev, page, channel); + + if (ops->llsec) { + struct ieee802154_llsec_params params; + int changed = 0; + + params.coord_shortaddr = addr->short_addr; + changed |= IEEE802154_LLSEC_PARAM_COORD_SHORTADDR; + + params.pan_id = addr->pan_id; + changed |= IEEE802154_LLSEC_PARAM_PAN_ID; + + params.hwaddr = ieee802154_devaddr_from_raw(dev->dev_addr); + changed |= IEEE802154_LLSEC_PARAM_HWADDR; + + params.coord_hwaddr = params.hwaddr; + changed |= IEEE802154_LLSEC_PARAM_COORD_HWADDR; + + rc = ops->llsec->set_params(dev, ¶ms, changed); + } + + return rc; +} + +static int mac802154_set_mac_params(struct net_device *dev, + const struct ieee802154_mac_params *params) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct ieee802154_local *local = sdata->local; + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + int ret; + + ASSERT_RTNL(); + + /* PHY */ + wpan_dev->wpan_phy->transmit_power = params->transmit_power; + wpan_dev->wpan_phy->cca = params->cca; + wpan_dev->wpan_phy->cca_ed_level = params->cca_ed_level; + + /* MAC */ + wpan_dev->min_be = params->min_be; + wpan_dev->max_be = params->max_be; + wpan_dev->csma_retries = params->csma_retries; + wpan_dev->frame_retries = params->frame_retries; + wpan_dev->lbt = params->lbt; + + if (local->hw.flags & IEEE802154_HW_TXPOWER) { + ret = drv_set_tx_power(local, params->transmit_power); + if (ret < 0) + return ret; + } + + if (local->hw.flags & IEEE802154_HW_CCA_MODE) { + ret = drv_set_cca_mode(local, ¶ms->cca); + if (ret < 0) + return ret; + } + + if (local->hw.flags & IEEE802154_HW_CCA_ED_LEVEL) { + ret = drv_set_cca_ed_level(local, params->cca_ed_level); + if (ret < 0) + return ret; + } + + return 0; +} + +static void mac802154_get_mac_params(struct net_device *dev, + struct ieee802154_mac_params *params) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + + ASSERT_RTNL(); + + /* PHY */ + params->transmit_power = wpan_dev->wpan_phy->transmit_power; + params->cca = wpan_dev->wpan_phy->cca; + params->cca_ed_level = wpan_dev->wpan_phy->cca_ed_level; + + /* MAC */ + params->min_be = wpan_dev->min_be; + params->max_be = wpan_dev->max_be; + params->csma_retries = wpan_dev->csma_retries; + params->frame_retries = wpan_dev->frame_retries; + params->lbt = wpan_dev->lbt; +} + +static struct ieee802154_llsec_ops mac802154_llsec_ops = { + .get_params = mac802154_get_params, + .set_params = mac802154_set_params, + .add_key = mac802154_add_key, + .del_key = mac802154_del_key, + .add_dev = mac802154_add_dev, + .del_dev = mac802154_del_dev, + .add_devkey = mac802154_add_devkey, + .del_devkey = mac802154_del_devkey, + .add_seclevel = mac802154_add_seclevel, + .del_seclevel = mac802154_del_seclevel, + .lock_table = mac802154_lock_table, + .get_table = mac802154_get_table, + .unlock_table = mac802154_unlock_table, +}; + +struct ieee802154_mlme_ops mac802154_mlme_wpan = { + .start_req = mac802154_mlme_start_req, + .get_pan_id = mac802154_dev_get_pan_id, + .get_short_addr = mac802154_dev_get_short_addr, + .get_dsn = mac802154_dev_get_dsn, + + .llsec = &mac802154_llsec_ops, + + .set_mac_params = mac802154_set_mac_params, + .get_mac_params = mac802154_get_mac_params, +}; diff --git a/kernel/net/mac802154/main.c b/kernel/net/mac802154/main.c new file mode 100644 index 000000000..08cb32dc8 --- /dev/null +++ b/kernel/net/mac802154/main.c @@ -0,0 +1,220 @@ +/* + * Copyright (C) 2007-2012 Siemens AG + * + * Written by: + * Alexander Smirnov + * + * 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. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "ieee802154_i.h" +#include "cfg.h" + +static void ieee802154_tasklet_handler(unsigned long data) +{ + struct ieee802154_local *local = (struct ieee802154_local *)data; + struct sk_buff *skb; + + while ((skb = skb_dequeue(&local->skb_queue))) { + switch (skb->pkt_type) { + case IEEE802154_RX_MSG: + /* Clear skb->pkt_type in order to not confuse kernel + * netstack. + */ + skb->pkt_type = 0; + ieee802154_rx(&local->hw, skb); + break; + default: + WARN(1, "mac802154: Packet is of unknown type %d\n", + skb->pkt_type); + kfree_skb(skb); + break; + } + } +} + +struct ieee802154_hw * +ieee802154_alloc_hw(size_t priv_data_len, const struct ieee802154_ops *ops) +{ + struct wpan_phy *phy; + struct ieee802154_local *local; + size_t priv_size; + + if (!ops || !(ops->xmit_async || ops->xmit_sync) || !ops->ed || + !ops->start || !ops->stop || !ops->set_channel) { + pr_err("undefined IEEE802.15.4 device operations\n"); + return NULL; + } + + /* Ensure 32-byte alignment of our private data and hw private data. + * We use the wpan_phy priv data for both our ieee802154_local and for + * the driver's private data + * + * in memory it'll be like this: + * + * +-------------------------+ + * | struct wpan_phy | + * +-------------------------+ + * | struct ieee802154_local | + * +-------------------------+ + * | driver's private data | + * +-------------------------+ + * + * Due to ieee802154 layer isn't aware of driver and MAC structures, + * so lets align them here. + */ + + priv_size = ALIGN(sizeof(*local), NETDEV_ALIGN) + priv_data_len; + + phy = wpan_phy_new(&mac802154_config_ops, priv_size); + if (!phy) { + pr_err("failure to allocate master IEEE802.15.4 device\n"); + return NULL; + } + + phy->privid = mac802154_wpan_phy_privid; + + local = wpan_phy_priv(phy); + local->phy = phy; + local->hw.phy = local->phy; + local->hw.priv = (char *)local + ALIGN(sizeof(*local), NETDEV_ALIGN); + local->ops = ops; + + INIT_LIST_HEAD(&local->interfaces); + mutex_init(&local->iflist_mtx); + + tasklet_init(&local->tasklet, + ieee802154_tasklet_handler, + (unsigned long)local); + + skb_queue_head_init(&local->skb_queue); + + return &local->hw; +} +EXPORT_SYMBOL(ieee802154_alloc_hw); + +void ieee802154_free_hw(struct ieee802154_hw *hw) +{ + struct ieee802154_local *local = hw_to_local(hw); + + BUG_ON(!list_empty(&local->interfaces)); + + mutex_destroy(&local->iflist_mtx); + + wpan_phy_free(local->phy); +} +EXPORT_SYMBOL(ieee802154_free_hw); + +static void ieee802154_setup_wpan_phy_pib(struct wpan_phy *wpan_phy) +{ + /* TODO warn on empty symbol_duration + * Should be done when all drivers sets this value. + */ + + wpan_phy->lifs_period = IEEE802154_LIFS_PERIOD * + wpan_phy->symbol_duration; + wpan_phy->sifs_period = IEEE802154_SIFS_PERIOD * + wpan_phy->symbol_duration; +} + +int ieee802154_register_hw(struct ieee802154_hw *hw) +{ + struct ieee802154_local *local = hw_to_local(hw); + struct net_device *dev; + int rc = -ENOSYS; + + local->workqueue = + create_singlethread_workqueue(wpan_phy_name(local->phy)); + if (!local->workqueue) { + rc = -ENOMEM; + goto out; + } + + hrtimer_init(&local->ifs_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + local->ifs_timer.function = ieee802154_xmit_ifs_timer; + + wpan_phy_set_dev(local->phy, local->hw.parent); + + ieee802154_setup_wpan_phy_pib(local->phy); + + rc = wpan_phy_register(local->phy); + if (rc < 0) + goto out_wq; + + rtnl_lock(); + + dev = ieee802154_if_add(local, "wpan%d", NET_NAME_ENUM, + NL802154_IFTYPE_NODE, + cpu_to_le64(0x0000000000000000ULL)); + if (IS_ERR(dev)) { + rtnl_unlock(); + rc = PTR_ERR(dev); + goto out_phy; + } + + rtnl_unlock(); + + return 0; + +out_phy: + wpan_phy_unregister(local->phy); +out_wq: + destroy_workqueue(local->workqueue); +out: + return rc; +} +EXPORT_SYMBOL(ieee802154_register_hw); + +void ieee802154_unregister_hw(struct ieee802154_hw *hw) +{ + struct ieee802154_local *local = hw_to_local(hw); + + tasklet_kill(&local->tasklet); + flush_workqueue(local->workqueue); + destroy_workqueue(local->workqueue); + + rtnl_lock(); + + ieee802154_remove_interfaces(local); + + rtnl_unlock(); + + wpan_phy_unregister(local->phy); +} +EXPORT_SYMBOL(ieee802154_unregister_hw); + +static int __init ieee802154_init(void) +{ + return ieee802154_iface_init(); +} + +static void __exit ieee802154_exit(void) +{ + ieee802154_iface_exit(); + + rcu_barrier(); +} + +subsys_initcall(ieee802154_init); +module_exit(ieee802154_exit); + +MODULE_DESCRIPTION("IEEE 802.15.4 subsystem"); +MODULE_LICENSE("GPL v2"); diff --git a/kernel/net/mac802154/mib.c b/kernel/net/mac802154/mib.c new file mode 100644 index 000000000..5cf019a57 --- /dev/null +++ b/kernel/net/mac802154/mib.c @@ -0,0 +1,286 @@ +/* + * Copyright 2007-2012 Siemens AG + * + * 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. + * + * Written by: + * Dmitry Eremin-Solenikov + * Sergey Lapin + * Maxim Gorbachyov + * Alexander Smirnov + */ + +#include + +#include +#include +#include + +#include "ieee802154_i.h" +#include "driver-ops.h" + +void mac802154_dev_set_short_addr(struct net_device *dev, __le16 val) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + spin_lock_bh(&sdata->mib_lock); + sdata->wpan_dev.short_addr = val; + spin_unlock_bh(&sdata->mib_lock); +} + +__le16 mac802154_dev_get_short_addr(const struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + __le16 ret; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + spin_lock_bh(&sdata->mib_lock); + ret = sdata->wpan_dev.short_addr; + spin_unlock_bh(&sdata->mib_lock); + + return ret; +} + +__le16 mac802154_dev_get_pan_id(const struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + __le16 ret; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + spin_lock_bh(&sdata->mib_lock); + ret = sdata->wpan_dev.pan_id; + spin_unlock_bh(&sdata->mib_lock); + + return ret; +} + +void mac802154_dev_set_pan_id(struct net_device *dev, __le16 val) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + spin_lock_bh(&sdata->mib_lock); + sdata->wpan_dev.pan_id = val; + spin_unlock_bh(&sdata->mib_lock); +} + +u8 mac802154_dev_get_dsn(const struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + return sdata->wpan_dev.dsn++; +} + +void mac802154_dev_set_page_channel(struct net_device *dev, u8 page, u8 chan) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + struct ieee802154_local *local = sdata->local; + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + res = drv_set_channel(local, page, chan); + if (res) { + pr_debug("set_channel failed\n"); + } else { + mutex_lock(&local->phy->pib_lock); + local->phy->current_channel = chan; + local->phy->current_page = page; + mutex_unlock(&local->phy->pib_lock); + } +} + +int mac802154_get_params(struct net_device *dev, + struct ieee802154_llsec_params *params) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_get_params(&sdata->sec, params); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_set_params(struct net_device *dev, + const struct ieee802154_llsec_params *params, + int changed) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_set_params(&sdata->sec, params, changed); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_add_key(struct net_device *dev, + const struct ieee802154_llsec_key_id *id, + const struct ieee802154_llsec_key *key) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_key_add(&sdata->sec, id, key); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_del_key(struct net_device *dev, + const struct ieee802154_llsec_key_id *id) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_key_del(&sdata->sec, id); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_add_dev(struct net_device *dev, + const struct ieee802154_llsec_device *llsec_dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_dev_add(&sdata->sec, llsec_dev); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_del_dev(struct net_device *dev, __le64 dev_addr) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_dev_del(&sdata->sec, dev_addr); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_add_devkey(struct net_device *dev, + __le64 device_addr, + const struct ieee802154_llsec_device_key *key) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_devkey_add(&sdata->sec, device_addr, key); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_del_devkey(struct net_device *dev, + __le64 device_addr, + const struct ieee802154_llsec_device_key *key) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_devkey_del(&sdata->sec, device_addr, key); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_add_seclevel(struct net_device *dev, + const struct ieee802154_llsec_seclevel *sl) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_seclevel_add(&sdata->sec, sl); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +int mac802154_del_seclevel(struct net_device *dev, + const struct ieee802154_llsec_seclevel *sl) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int res; + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); + res = mac802154_llsec_seclevel_del(&sdata->sec, sl); + mutex_unlock(&sdata->sec_mtx); + + return res; +} + +void mac802154_lock_table(struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_lock(&sdata->sec_mtx); +} + +void mac802154_get_table(struct net_device *dev, + struct ieee802154_llsec_table **t) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + *t = &sdata->sec.table; +} + +void mac802154_unlock_table(struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + BUG_ON(dev->type != ARPHRD_IEEE802154); + + mutex_unlock(&sdata->sec_mtx); +} diff --git a/kernel/net/mac802154/rx.c b/kernel/net/mac802154/rx.c new file mode 100644 index 000000000..c0d67b2b4 --- /dev/null +++ b/kernel/net/mac802154/rx.c @@ -0,0 +1,302 @@ +/* + * Copyright (C) 2007-2012 Siemens AG + * + * 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. + * + * Written by: + * Pavel Smolenskiy + * Maxim Gorbachyov + * Dmitry Eremin-Solenikov + * Alexander Smirnov + */ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "ieee802154_i.h" + +static int ieee802154_deliver_skb(struct sk_buff *skb) +{ + skb->ip_summed = CHECKSUM_UNNECESSARY; + skb->protocol = htons(ETH_P_IEEE802154); + + return netif_receive_skb(skb); +} + +static int +ieee802154_subif_frame(struct ieee802154_sub_if_data *sdata, + struct sk_buff *skb, const struct ieee802154_hdr *hdr) +{ + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + __le16 span, sshort; + int rc; + + pr_debug("getting packet via slave interface %s\n", sdata->dev->name); + + spin_lock_bh(&sdata->mib_lock); + + span = wpan_dev->pan_id; + sshort = wpan_dev->short_addr; + + switch (mac_cb(skb)->dest.mode) { + case IEEE802154_ADDR_NONE: + if (mac_cb(skb)->dest.mode != IEEE802154_ADDR_NONE) + /* FIXME: check if we are PAN coordinator */ + skb->pkt_type = PACKET_OTHERHOST; + else + /* ACK comes with both addresses empty */ + skb->pkt_type = PACKET_HOST; + break; + case IEEE802154_ADDR_LONG: + if (mac_cb(skb)->dest.pan_id != span && + mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST)) + skb->pkt_type = PACKET_OTHERHOST; + else if (mac_cb(skb)->dest.extended_addr == wpan_dev->extended_addr) + skb->pkt_type = PACKET_HOST; + else + skb->pkt_type = PACKET_OTHERHOST; + break; + case IEEE802154_ADDR_SHORT: + if (mac_cb(skb)->dest.pan_id != span && + mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST)) + skb->pkt_type = PACKET_OTHERHOST; + else if (mac_cb(skb)->dest.short_addr == sshort) + skb->pkt_type = PACKET_HOST; + else if (mac_cb(skb)->dest.short_addr == + cpu_to_le16(IEEE802154_ADDR_BROADCAST)) + skb->pkt_type = PACKET_BROADCAST; + else + skb->pkt_type = PACKET_OTHERHOST; + break; + default: + spin_unlock_bh(&sdata->mib_lock); + pr_debug("invalid dest mode\n"); + goto fail; + } + + spin_unlock_bh(&sdata->mib_lock); + + skb->dev = sdata->dev; + + rc = mac802154_llsec_decrypt(&sdata->sec, skb); + if (rc) { + pr_debug("decryption failed: %i\n", rc); + goto fail; + } + + sdata->dev->stats.rx_packets++; + sdata->dev->stats.rx_bytes += skb->len; + + switch (mac_cb(skb)->type) { + case IEEE802154_FC_TYPE_DATA: + return ieee802154_deliver_skb(skb); + default: + pr_warn("ieee802154: bad frame received (type = %d)\n", + mac_cb(skb)->type); + goto fail; + } + +fail: + kfree_skb(skb); + return NET_RX_DROP; +} + +static void +ieee802154_print_addr(const char *name, const struct ieee802154_addr *addr) +{ + if (addr->mode == IEEE802154_ADDR_NONE) + pr_debug("%s not present\n", name); + + pr_debug("%s PAN ID: %04x\n", name, le16_to_cpu(addr->pan_id)); + if (addr->mode == IEEE802154_ADDR_SHORT) { + pr_debug("%s is short: %04x\n", name, + le16_to_cpu(addr->short_addr)); + } else { + u64 hw = swab64((__force u64)addr->extended_addr); + + pr_debug("%s is hardware: %8phC\n", name, &hw); + } +} + +static int +ieee802154_parse_frame_start(struct sk_buff *skb, struct ieee802154_hdr *hdr) +{ + int hlen; + struct ieee802154_mac_cb *cb = mac_cb_init(skb); + + skb_reset_mac_header(skb); + + hlen = ieee802154_hdr_pull(skb, hdr); + if (hlen < 0) + return -EINVAL; + + skb->mac_len = hlen; + + pr_debug("fc: %04x dsn: %02x\n", le16_to_cpup((__le16 *)&hdr->fc), + hdr->seq); + + cb->type = hdr->fc.type; + cb->ackreq = hdr->fc.ack_request; + cb->secen = hdr->fc.security_enabled; + + ieee802154_print_addr("destination", &hdr->dest); + ieee802154_print_addr("source", &hdr->source); + + cb->source = hdr->source; + cb->dest = hdr->dest; + + if (hdr->fc.security_enabled) { + u64 key; + + pr_debug("seclevel %i\n", hdr->sec.level); + + switch (hdr->sec.key_id_mode) { + case IEEE802154_SCF_KEY_IMPLICIT: + pr_debug("implicit key\n"); + break; + + case IEEE802154_SCF_KEY_INDEX: + pr_debug("key %02x\n", hdr->sec.key_id); + break; + + case IEEE802154_SCF_KEY_SHORT_INDEX: + pr_debug("key %04x:%04x %02x\n", + le32_to_cpu(hdr->sec.short_src) >> 16, + le32_to_cpu(hdr->sec.short_src) & 0xffff, + hdr->sec.key_id); + break; + + case IEEE802154_SCF_KEY_HW_INDEX: + key = swab64((__force u64)hdr->sec.extended_src); + pr_debug("key source %8phC %02x\n", &key, + hdr->sec.key_id); + break; + } + } + + return 0; +} + +static void +__ieee802154_rx_handle_packet(struct ieee802154_local *local, + struct sk_buff *skb) +{ + int ret; + struct ieee802154_sub_if_data *sdata; + struct ieee802154_hdr hdr; + + ret = ieee802154_parse_frame_start(skb, &hdr); + if (ret) { + pr_debug("got invalid frame\n"); + kfree_skb(skb); + return; + } + + list_for_each_entry_rcu(sdata, &local->interfaces, list) { + if (sdata->vif.type != NL802154_IFTYPE_NODE || + !netif_running(sdata->dev)) + continue; + + ieee802154_subif_frame(sdata, skb, &hdr); + skb = NULL; + break; + } + + if (skb) + kfree_skb(skb); +} + +static void +ieee802154_monitors_rx(struct ieee802154_local *local, struct sk_buff *skb) +{ + struct sk_buff *skb2; + struct ieee802154_sub_if_data *sdata; + + skb_reset_mac_header(skb); + skb->ip_summed = CHECKSUM_UNNECESSARY; + skb->pkt_type = PACKET_OTHERHOST; + skb->protocol = htons(ETH_P_IEEE802154); + + list_for_each_entry_rcu(sdata, &local->interfaces, list) { + if (sdata->vif.type != NL802154_IFTYPE_MONITOR) + continue; + + if (!ieee802154_sdata_running(sdata)) + continue; + + skb2 = skb_clone(skb, GFP_ATOMIC); + if (skb2) { + skb2->dev = sdata->dev; + ieee802154_deliver_skb(skb2); + + sdata->dev->stats.rx_packets++; + sdata->dev->stats.rx_bytes += skb->len; + } + } +} + +void ieee802154_rx(struct ieee802154_hw *hw, struct sk_buff *skb) +{ + struct ieee802154_local *local = hw_to_local(hw); + u16 crc; + + WARN_ON_ONCE(softirq_count() == 0); + + /* TODO: When a transceiver omits the checksum here, we + * add an own calculated one. This is currently an ugly + * solution because the monitor needs a crc here. + */ + if (local->hw.flags & IEEE802154_HW_RX_OMIT_CKSUM) { + crc = crc_ccitt(0, skb->data, skb->len); + put_unaligned_le16(crc, skb_put(skb, 2)); + } + + rcu_read_lock(); + + ieee802154_monitors_rx(local, skb); + + /* Check if transceiver doesn't validate the checksum. + * If not we validate the checksum here. + */ + if (local->hw.flags & IEEE802154_HW_RX_DROP_BAD_CKSUM) { + crc = crc_ccitt(0, skb->data, skb->len); + if (crc) { + rcu_read_unlock(); + kfree_skb(skb); + return; + } + } + /* remove crc */ + skb_trim(skb, skb->len - 2); + + __ieee802154_rx_handle_packet(local, skb); + + rcu_read_unlock(); +} +EXPORT_SYMBOL(ieee802154_rx); + +void +ieee802154_rx_irqsafe(struct ieee802154_hw *hw, struct sk_buff *skb, u8 lqi) +{ + struct ieee802154_local *local = hw_to_local(hw); + + mac_cb(skb)->lqi = lqi; + skb->pkt_type = IEEE802154_RX_MSG; + skb_queue_tail(&local->skb_queue, skb); + tasklet_schedule(&local->tasklet); +} +EXPORT_SYMBOL(ieee802154_rx_irqsafe); diff --git a/kernel/net/mac802154/tx.c b/kernel/net/mac802154/tx.c new file mode 100644 index 000000000..c62e95695 --- /dev/null +++ b/kernel/net/mac802154/tx.c @@ -0,0 +1,149 @@ +/* + * Copyright 2007-2012 Siemens AG + * + * 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. + * + * Written by: + * Dmitry Eremin-Solenikov + * Sergey Lapin + * Maxim Gorbachyov + * Alexander Smirnov + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "ieee802154_i.h" +#include "driver-ops.h" + +/* IEEE 802.15.4 transceivers can sleep during the xmit session, so process + * packets through the workqueue. + */ +struct ieee802154_xmit_cb { + struct sk_buff *skb; + struct work_struct work; + struct ieee802154_local *local; +}; + +static struct ieee802154_xmit_cb ieee802154_xmit_cb; + +static void ieee802154_xmit_worker(struct work_struct *work) +{ + struct ieee802154_xmit_cb *cb = + container_of(work, struct ieee802154_xmit_cb, work); + struct ieee802154_local *local = cb->local; + struct sk_buff *skb = cb->skb; + struct net_device *dev = skb->dev; + int res; + + rtnl_lock(); + + /* check if ifdown occurred while schedule */ + if (!netif_running(dev)) + goto err_tx; + + res = drv_xmit_sync(local, skb); + if (res) + goto err_tx; + + ieee802154_xmit_complete(&local->hw, skb, false); + + dev->stats.tx_packets++; + dev->stats.tx_bytes += skb->len; + + rtnl_unlock(); + + return; + +err_tx: + /* Restart the netif queue on each sub_if_data object. */ + ieee802154_wake_queue(&local->hw); + rtnl_unlock(); + kfree_skb(skb); + netdev_dbg(dev, "transmission failed\n"); +} + +static netdev_tx_t +ieee802154_tx(struct ieee802154_local *local, struct sk_buff *skb) +{ + struct net_device *dev = skb->dev; + int ret; + + if (!(local->hw.flags & IEEE802154_HW_TX_OMIT_CKSUM)) { + u16 crc = crc_ccitt(0, skb->data, skb->len); + + put_unaligned_le16(crc, skb_put(skb, 2)); + } + + if (skb_cow_head(skb, local->hw.extra_tx_headroom)) + goto err_tx; + + /* Stop the netif queue on each sub_if_data object. */ + ieee802154_stop_queue(&local->hw); + + /* async is priority, otherwise sync is fallback */ + if (local->ops->xmit_async) { + ret = drv_xmit_async(local, skb); + if (ret) { + ieee802154_wake_queue(&local->hw); + goto err_tx; + } + + dev->stats.tx_packets++; + dev->stats.tx_bytes += skb->len; + } else { + INIT_WORK(&ieee802154_xmit_cb.work, ieee802154_xmit_worker); + ieee802154_xmit_cb.skb = skb; + ieee802154_xmit_cb.local = local; + + queue_work(local->workqueue, &ieee802154_xmit_cb.work); + } + + return NETDEV_TX_OK; + +err_tx: + kfree_skb(skb); + return NETDEV_TX_OK; +} + +netdev_tx_t +ieee802154_monitor_start_xmit(struct sk_buff *skb, struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + + skb->skb_iif = dev->ifindex; + + return ieee802154_tx(sdata->local, skb); +} + +netdev_tx_t +ieee802154_subif_start_xmit(struct sk_buff *skb, struct net_device *dev) +{ + struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); + int rc; + + rc = mac802154_llsec_encrypt(&sdata->sec, skb); + if (rc) { + netdev_warn(dev, "encryption failed: %i\n", rc); + kfree_skb(skb); + return NETDEV_TX_OK; + } + + skb->skb_iif = dev->ifindex; + + return ieee802154_tx(sdata->local, skb); +} diff --git a/kernel/net/mac802154/util.c b/kernel/net/mac802154/util.c new file mode 100644 index 000000000..150bf807e --- /dev/null +++ b/kernel/net/mac802154/util.c @@ -0,0 +1,95 @@ +/* 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. + * + * Authors: + * Alexander Aring + * + * Based on: net/mac80211/util.c + */ + +#include "ieee802154_i.h" + +/* privid for wpan_phys to determine whether they belong to us or not */ +const void *const mac802154_wpan_phy_privid = &mac802154_wpan_phy_privid; + +void ieee802154_wake_queue(struct ieee802154_hw *hw) +{ + struct ieee802154_local *local = hw_to_local(hw); + struct ieee802154_sub_if_data *sdata; + + rcu_read_lock(); + list_for_each_entry_rcu(sdata, &local->interfaces, list) { + if (!sdata->dev) + continue; + + netif_wake_queue(sdata->dev); + } + rcu_read_unlock(); +} +EXPORT_SYMBOL(ieee802154_wake_queue); + +void ieee802154_stop_queue(struct ieee802154_hw *hw) +{ + struct ieee802154_local *local = hw_to_local(hw); + struct ieee802154_sub_if_data *sdata; + + rcu_read_lock(); + list_for_each_entry_rcu(sdata, &local->interfaces, list) { + if (!sdata->dev) + continue; + + netif_stop_queue(sdata->dev); + } + rcu_read_unlock(); +} +EXPORT_SYMBOL(ieee802154_stop_queue); + +enum hrtimer_restart ieee802154_xmit_ifs_timer(struct hrtimer *timer) +{ + struct ieee802154_local *local = + container_of(timer, struct ieee802154_local, ifs_timer); + + ieee802154_wake_queue(&local->hw); + + return HRTIMER_NORESTART; +} + +void ieee802154_xmit_complete(struct ieee802154_hw *hw, struct sk_buff *skb, + bool ifs_handling) +{ + if (ifs_handling) { + struct ieee802154_local *local = hw_to_local(hw); + u8 max_sifs_size; + + /* If transceiver sets CRC on his own we need to use lifs + * threshold len above 16 otherwise 18, because it's not + * part of skb->len. + */ + if (hw->flags & IEEE802154_HW_TX_OMIT_CKSUM) + max_sifs_size = IEEE802154_MAX_SIFS_FRAME_SIZE - + IEEE802154_FCS_LEN; + else + max_sifs_size = IEEE802154_MAX_SIFS_FRAME_SIZE; + + if (skb->len > max_sifs_size) + hrtimer_start(&local->ifs_timer, + ktime_set(0, hw->phy->lifs_period * NSEC_PER_USEC), + HRTIMER_MODE_REL); + else + hrtimer_start(&local->ifs_timer, + ktime_set(0, hw->phy->sifs_period * NSEC_PER_USEC), + HRTIMER_MODE_REL); + + consume_skb(skb); + } else { + ieee802154_wake_queue(hw); + consume_skb(skb); + } +} +EXPORT_SYMBOL(ieee802154_xmit_complete); -- cgit 1.2.3-korg