diff --git a/net/mac802154/ieee802154_i.h b/net/mac802154/ieee802154_i.h index 432bfa87249e..08dd521a51a5 100644 --- a/net/mac802154/ieee802154_i.h +++ b/net/mac802154/ieee802154_i.h @@ -318,6 +318,8 @@ static inline bool mac802154_is_associating(struct ieee802154_local *local) int mac802154_send_disassociation_notif(struct ieee802154_sub_if_data *sdata, struct ieee802154_pan_device *target, u8 reason); +int mac802154_process_disassociation_notif(struct ieee802154_sub_if_data *sdata, + struct sk_buff *skb); int mac802154_process_association_req(struct ieee802154_sub_if_data *sdata, struct sk_buff *skb); diff --git a/net/mac802154/rx.c b/net/mac802154/rx.c index 96040b63a4fc..0024341ef9c5 100644 --- a/net/mac802154/rx.c +++ b/net/mac802154/rx.c @@ -110,6 +110,14 @@ void mac802154_rx_mac_cmd_worker(struct work_struct *work) mac802154_process_association_req(mac_pkt->sdata, mac_pkt->skb); break; + case IEEE802154_CMD_DISASSOCIATION_NOTIFY: + dev_dbg(&mac_pkt->sdata->dev->dev, "processing DISASSOC NOTIF\n"); + if (mac_pkt->sdata->wpan_dev.iftype != NL802154_IFTYPE_COORD) + break; + + mac802154_process_disassociation_notif(mac_pkt->sdata, mac_pkt->skb); + break; + default: break; } diff --git a/net/mac802154/scan.c b/net/mac802154/scan.c index 81d2c2bb1f09..7597072aed57 100644 --- a/net/mac802154/scan.c +++ b/net/mac802154/scan.c @@ -852,3 +852,58 @@ int mac802154_process_association_req(struct ieee802154_sub_if_data *sdata, mutex_unlock(&wpan_dev->association_lock); return ret; } + +int mac802154_process_disassociation_notif(struct ieee802154_sub_if_data *sdata, + struct sk_buff *skb) +{ + struct ieee802154_addr *src = &mac_cb(skb)->source; + struct ieee802154_addr *dest = &mac_cb(skb)->dest; + struct wpan_dev *wpan_dev = &sdata->wpan_dev; + struct ieee802154_pan_device *child; + struct ieee802154_addr target; + bool parent; + u64 teaddr; + + if (skb->len != sizeof(u8)) + return -EINVAL; + + if (unlikely(src->mode != IEEE802154_EXTENDED_ADDRESSING)) + return -EINVAL; + + if (dest->mode == IEEE802154_EXTENDED_ADDRESSING && + unlikely(dest->extended_addr != wpan_dev->extended_addr)) + return -ENODEV; + else if (dest->mode == IEEE802154_SHORT_ADDRESSING && + unlikely(dest->short_addr != wpan_dev->short_addr)) + return -ENODEV; + + if (dest->pan_id != wpan_dev->pan_id) + return -ENODEV; + + target.mode = IEEE802154_EXTENDED_ADDRESSING; + target.extended_addr = src->extended_addr; + teaddr = swab64((__force u64)target.extended_addr); + dev_dbg(&skb->dev->dev, "Processing DISASSOC NOTIF from %8phC\n", &teaddr); + + mutex_lock(&wpan_dev->association_lock); + parent = cfg802154_device_is_parent(wpan_dev, &target); + if (!parent) + child = cfg802154_device_is_child(wpan_dev, &target); + if (!parent && !child) { + mutex_unlock(&wpan_dev->association_lock); + return -EINVAL; + } + + if (parent) { + kfree(wpan_dev->parent); + wpan_dev->parent = NULL; + } else { + list_del(&child->node); + kfree(child); + wpan_dev->nchildren--; + } + + mutex_unlock(&wpan_dev->association_lock); + + return 0; +}