mac802154: Handle disassociations

[linux-2.6-block.git] / net / mac802154 / scan.c

diff --git a/net/mac802154/scan.c b/net/mac802154/scan.c
index 5dd50e1ce329333f8e8e164b165038716f8f6f60..e2f2e1235ec6e533161a0f662d63d5301af30174 100644 (file)
--- a/net/mac802154/scan.c
+++ b/net/mac802154/scan.c
@@ -637,3 +637,63 @@ int mac802154_process_association_resp(struct ieee802154_sub_if_data *sdata,
 
        return 0;
 }
+
+int mac802154_send_disassociation_notif(struct ieee802154_sub_if_data *sdata,
+                                       struct ieee802154_pan_device *target,
+                                       u8 reason)
+{
+       struct ieee802154_disassociation_notif_frame frame = {};
+       u64 teaddr = swab64((__force u64)target->extended_addr);
+       struct ieee802154_local *local = sdata->local;
+       struct wpan_dev *wpan_dev = &sdata->wpan_dev;
+       struct sk_buff *skb;
+       int ret;
+
+       frame.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD;
+       frame.mhr.fc.security_enabled = 0;
+       frame.mhr.fc.frame_pending = 0;
+       frame.mhr.fc.ack_request = 1;
+       frame.mhr.fc.intra_pan = 1;
+       frame.mhr.fc.dest_addr_mode = (target->mode == IEEE802154_ADDR_LONG) ?
+               IEEE802154_EXTENDED_ADDRESSING : IEEE802154_SHORT_ADDRESSING;
+       frame.mhr.fc.version = IEEE802154_2003_STD;
+       frame.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
+       frame.mhr.source.mode = IEEE802154_ADDR_LONG;
+       frame.mhr.source.pan_id = wpan_dev->pan_id;
+       frame.mhr.source.extended_addr = wpan_dev->extended_addr;
+       frame.mhr.dest.mode = target->mode;
+       frame.mhr.dest.pan_id = wpan_dev->pan_id;
+       if (target->mode == IEEE802154_ADDR_LONG)
+               frame.mhr.dest.extended_addr = target->extended_addr;
+       else
+               frame.mhr.dest.short_addr = target->short_addr;
+       frame.mhr.seq = atomic_inc_return(&wpan_dev->dsn) & 0xFF;
+       frame.mac_pl.cmd_id = IEEE802154_CMD_DISASSOCIATION_NOTIFY;
+       frame.disassoc_pl = reason;
+
+       skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ + sizeof(frame.disassoc_pl),
+                       GFP_KERNEL);
+       if (!skb)
+               return -ENOBUFS;
+
+       skb->dev = sdata->dev;
+
+       ret = ieee802154_mac_cmd_push(skb, &frame, &frame.disassoc_pl,
+                                     sizeof(frame.disassoc_pl));
+       if (ret) {
+               kfree_skb(skb);
+               return ret;
+       }
+
+       ret = ieee802154_mlme_tx_one_locked(local, sdata, skb);
+       if (ret) {
+               dev_warn(&sdata->dev->dev,
+                        "No DISASSOC ACK received from %8phC\n", &teaddr);
+               if (ret > 0)
+                       ret = (ret == IEEE802154_NO_ACK) ? -EREMOTEIO : -EIO;
+               return ret;
+       }
+
+       dev_dbg(&sdata->dev->dev, "DISASSOC ACK received from %8phC\n", &teaddr);
+       return 0;
+}