mirror of
https://mirrors.bfsu.edu.cn/git/linux.git
synced 2024-11-16 00:34:20 +08:00
5c68005083
-----BEGIN PGP SIGNATURE----- iQFSBAABCAA8FiEEq68RxlopcLEwq+PEeb4+QwBBGIYFAmRzP6weHHRvcnZhbGRz QGxpbnV4LWZvdW5kYXRpb24ub3JnAAoJEHm+PkMAQRiGRJYH/2I3eKvKH7uQB/g7 s9PTv+Q3oPKSNCfSdudcmTj8W+yd7CHcfxEmw1/xmTOn/6Vyybk6TFBHUgRDH/un Jkk25N3UXqDIWcEVSYW6PMr/m0FJ4gpHtDNu/8TUTv5Gh/GrlIlfnntXQ+L8UWxQ T+pDf9oAI/MIXctACPLRoOt9fOMCwMZbO0VcGxuTM2M5De9Fu/Mla9OFfViC3QAV LtWrBfTgvNlF16PIunggBsKGIGB7J6qpua9nEzwwCjXn8wVBwGVQ2kfpz4kkEFGU O3UC+oCyjTGvjMCkIqhxKOy5cVNcXoL+xE23HTlPTjuDBxRQi/xEnw6doJ790pTk 5F/ZcKc= =SF/G -----END PGP SIGNATURE----- Merge tag 'v6.4-rc4' into wpan-next/staging Linux 6.4-rc4
513 lines
15 KiB
C
513 lines
15 KiB
C
// SPDX-License-Identifier: GPL-2.0-only
|
|
/*
|
|
* IEEE 802.15.4 scanning management
|
|
*
|
|
* Copyright (C) 2021 Qorvo US, Inc
|
|
* Authors:
|
|
* - David Girault <david.girault@qorvo.com>
|
|
* - Miquel Raynal <miquel.raynal@bootlin.com>
|
|
*/
|
|
|
|
#include <linux/module.h>
|
|
#include <linux/rtnetlink.h>
|
|
#include <net/mac802154.h>
|
|
|
|
#include "ieee802154_i.h"
|
|
#include "driver-ops.h"
|
|
#include "../ieee802154/nl802154.h"
|
|
|
|
#define IEEE802154_BEACON_MHR_SZ 13
|
|
#define IEEE802154_BEACON_PL_SZ 4
|
|
#define IEEE802154_MAC_CMD_MHR_SZ 23
|
|
#define IEEE802154_MAC_CMD_PL_SZ 1
|
|
#define IEEE802154_BEACON_SKB_SZ (IEEE802154_BEACON_MHR_SZ + \
|
|
IEEE802154_BEACON_PL_SZ)
|
|
#define IEEE802154_MAC_CMD_SKB_SZ (IEEE802154_MAC_CMD_MHR_SZ + \
|
|
IEEE802154_MAC_CMD_PL_SZ)
|
|
|
|
/* mac802154_scan_cleanup_locked() must be called upon scan completion or abort.
|
|
* - Completions are asynchronous, not locked by the rtnl and decided by the
|
|
* scan worker.
|
|
* - Aborts are decided by userspace, and locked by the rtnl.
|
|
*
|
|
* Concurrent modifications to the PHY, the interfaces or the hardware is in
|
|
* general prevented by the rtnl. So in most cases we don't need additional
|
|
* protection.
|
|
*
|
|
* However, the scan worker get's triggered without anybody noticing and thus we
|
|
* must ensure the presence of the devices as well as data consistency:
|
|
* - The sub-interface and device driver module get both their reference
|
|
* counters incremented whenever we start a scan, so they cannot disappear
|
|
* during operation.
|
|
* - Data consistency is achieved by the use of rcu protected pointers.
|
|
*/
|
|
static int mac802154_scan_cleanup_locked(struct ieee802154_local *local,
|
|
struct ieee802154_sub_if_data *sdata,
|
|
bool aborted)
|
|
{
|
|
struct wpan_dev *wpan_dev = &sdata->wpan_dev;
|
|
struct wpan_phy *wpan_phy = local->phy;
|
|
struct cfg802154_scan_request *request;
|
|
u8 arg;
|
|
|
|
/* Prevent any further use of the scan request */
|
|
clear_bit(IEEE802154_IS_SCANNING, &local->ongoing);
|
|
cancel_delayed_work(&local->scan_work);
|
|
request = rcu_replace_pointer(local->scan_req, NULL, 1);
|
|
if (!request)
|
|
return 0;
|
|
kvfree_rcu_mightsleep(request);
|
|
|
|
/* Advertize first, while we know the devices cannot be removed */
|
|
if (aborted)
|
|
arg = NL802154_SCAN_DONE_REASON_ABORTED;
|
|
else
|
|
arg = NL802154_SCAN_DONE_REASON_FINISHED;
|
|
nl802154_scan_done(wpan_phy, wpan_dev, arg);
|
|
|
|
/* Cleanup software stack */
|
|
ieee802154_mlme_op_post(local);
|
|
|
|
/* Set the hardware back in its original state */
|
|
drv_set_channel(local, wpan_phy->current_page,
|
|
wpan_phy->current_channel);
|
|
ieee802154_configure_durations(wpan_phy, wpan_phy->current_page,
|
|
wpan_phy->current_channel);
|
|
drv_stop(local);
|
|
synchronize_net();
|
|
sdata->required_filtering = sdata->iface_default_filtering;
|
|
drv_start(local, sdata->required_filtering, &local->addr_filt);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int mac802154_abort_scan_locked(struct ieee802154_local *local,
|
|
struct ieee802154_sub_if_data *sdata)
|
|
{
|
|
ASSERT_RTNL();
|
|
|
|
if (!mac802154_is_scanning(local))
|
|
return -ESRCH;
|
|
|
|
return mac802154_scan_cleanup_locked(local, sdata, true);
|
|
}
|
|
|
|
static unsigned int mac802154_scan_get_channel_time(u8 duration_order,
|
|
u8 symbol_duration)
|
|
{
|
|
u64 base_super_frame_duration = (u64)symbol_duration *
|
|
IEEE802154_SUPERFRAME_PERIOD * IEEE802154_SLOT_PERIOD;
|
|
|
|
return usecs_to_jiffies(base_super_frame_duration *
|
|
(BIT(duration_order) + 1));
|
|
}
|
|
|
|
static void mac802154_flush_queued_beacons(struct ieee802154_local *local)
|
|
{
|
|
struct cfg802154_mac_pkt *mac_pkt, *tmp;
|
|
|
|
list_for_each_entry_safe(mac_pkt, tmp, &local->rx_beacon_list, node) {
|
|
list_del(&mac_pkt->node);
|
|
kfree_skb(mac_pkt->skb);
|
|
kfree(mac_pkt);
|
|
}
|
|
}
|
|
|
|
static void
|
|
mac802154_scan_get_next_channel(struct ieee802154_local *local,
|
|
struct cfg802154_scan_request *scan_req,
|
|
u8 *channel)
|
|
{
|
|
(*channel)++;
|
|
*channel = find_next_bit((const unsigned long *)&scan_req->channels,
|
|
IEEE802154_MAX_CHANNEL + 1,
|
|
*channel);
|
|
}
|
|
|
|
static int mac802154_scan_find_next_chan(struct ieee802154_local *local,
|
|
struct cfg802154_scan_request *scan_req,
|
|
u8 page, u8 *channel)
|
|
{
|
|
mac802154_scan_get_next_channel(local, scan_req, channel);
|
|
if (*channel > IEEE802154_MAX_CHANNEL)
|
|
return -EINVAL;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int mac802154_scan_prepare_beacon_req(struct ieee802154_local *local)
|
|
{
|
|
memset(&local->scan_beacon_req, 0, sizeof(local->scan_beacon_req));
|
|
local->scan_beacon_req.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD;
|
|
local->scan_beacon_req.mhr.fc.dest_addr_mode = IEEE802154_SHORT_ADDRESSING;
|
|
local->scan_beacon_req.mhr.fc.version = IEEE802154_2003_STD;
|
|
local->scan_beacon_req.mhr.fc.source_addr_mode = IEEE802154_NO_ADDRESSING;
|
|
local->scan_beacon_req.mhr.dest.mode = IEEE802154_ADDR_SHORT;
|
|
local->scan_beacon_req.mhr.dest.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
|
|
local->scan_beacon_req.mhr.dest.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
|
|
local->scan_beacon_req.mac_pl.cmd_id = IEEE802154_CMD_BEACON_REQ;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int mac802154_transmit_beacon_req(struct ieee802154_local *local,
|
|
struct ieee802154_sub_if_data *sdata)
|
|
{
|
|
struct sk_buff *skb;
|
|
int ret;
|
|
|
|
skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ, GFP_KERNEL);
|
|
if (!skb)
|
|
return -ENOBUFS;
|
|
|
|
skb->dev = sdata->dev;
|
|
|
|
ret = ieee802154_mac_cmd_push(skb, &local->scan_beacon_req, NULL, 0);
|
|
if (ret) {
|
|
kfree_skb(skb);
|
|
return ret;
|
|
}
|
|
|
|
return ieee802154_mlme_tx(local, sdata, skb);
|
|
}
|
|
|
|
void mac802154_scan_worker(struct work_struct *work)
|
|
{
|
|
struct ieee802154_local *local =
|
|
container_of(work, struct ieee802154_local, scan_work.work);
|
|
struct cfg802154_scan_request *scan_req;
|
|
struct ieee802154_sub_if_data *sdata;
|
|
unsigned int scan_duration = 0;
|
|
struct wpan_phy *wpan_phy;
|
|
u8 scan_req_duration;
|
|
u8 page, channel;
|
|
int ret;
|
|
|
|
/* Ensure the device receiver is turned off when changing channels
|
|
* because there is no atomic way to change the channel and know on
|
|
* which one a beacon might have been received.
|
|
*/
|
|
drv_stop(local);
|
|
synchronize_net();
|
|
mac802154_flush_queued_beacons(local);
|
|
|
|
rcu_read_lock();
|
|
scan_req = rcu_dereference(local->scan_req);
|
|
if (unlikely(!scan_req)) {
|
|
rcu_read_unlock();
|
|
return;
|
|
}
|
|
|
|
sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(scan_req->wpan_dev);
|
|
|
|
/* Wait an arbitrary amount of time in case we cannot use the device */
|
|
if (local->suspended || !ieee802154_sdata_running(sdata)) {
|
|
rcu_read_unlock();
|
|
queue_delayed_work(local->mac_wq, &local->scan_work,
|
|
msecs_to_jiffies(1000));
|
|
return;
|
|
}
|
|
|
|
wpan_phy = scan_req->wpan_phy;
|
|
scan_req_duration = scan_req->duration;
|
|
|
|
/* Look for the next valid chan */
|
|
page = local->scan_page;
|
|
channel = local->scan_channel;
|
|
do {
|
|
ret = mac802154_scan_find_next_chan(local, scan_req, page, &channel);
|
|
if (ret) {
|
|
rcu_read_unlock();
|
|
goto end_scan;
|
|
}
|
|
} while (!ieee802154_chan_is_valid(scan_req->wpan_phy, page, channel));
|
|
|
|
rcu_read_unlock();
|
|
|
|
/* Bypass the stack on purpose when changing the channel */
|
|
rtnl_lock();
|
|
ret = drv_set_channel(local, page, channel);
|
|
rtnl_unlock();
|
|
if (ret) {
|
|
dev_err(&sdata->dev->dev,
|
|
"Channel change failure during scan, aborting (%d)\n", ret);
|
|
goto end_scan;
|
|
}
|
|
|
|
local->scan_page = page;
|
|
local->scan_channel = channel;
|
|
|
|
rtnl_lock();
|
|
ret = drv_start(local, IEEE802154_FILTERING_3_SCAN, &local->addr_filt);
|
|
rtnl_unlock();
|
|
if (ret) {
|
|
dev_err(&sdata->dev->dev,
|
|
"Restarting failure after channel change, aborting (%d)\n", ret);
|
|
goto end_scan;
|
|
}
|
|
|
|
if (scan_req->type == NL802154_SCAN_ACTIVE) {
|
|
ret = mac802154_transmit_beacon_req(local, sdata);
|
|
if (ret)
|
|
dev_err(&sdata->dev->dev,
|
|
"Error when transmitting beacon request (%d)\n", ret);
|
|
}
|
|
|
|
ieee802154_configure_durations(wpan_phy, page, channel);
|
|
scan_duration = mac802154_scan_get_channel_time(scan_req_duration,
|
|
wpan_phy->symbol_duration);
|
|
dev_dbg(&sdata->dev->dev,
|
|
"Scan page %u channel %u for %ums\n",
|
|
page, channel, jiffies_to_msecs(scan_duration));
|
|
queue_delayed_work(local->mac_wq, &local->scan_work, scan_duration);
|
|
return;
|
|
|
|
end_scan:
|
|
rtnl_lock();
|
|
mac802154_scan_cleanup_locked(local, sdata, false);
|
|
rtnl_unlock();
|
|
}
|
|
|
|
int mac802154_trigger_scan_locked(struct ieee802154_sub_if_data *sdata,
|
|
struct cfg802154_scan_request *request)
|
|
{
|
|
struct ieee802154_local *local = sdata->local;
|
|
|
|
ASSERT_RTNL();
|
|
|
|
if (mac802154_is_scanning(local))
|
|
return -EBUSY;
|
|
|
|
if (request->type != NL802154_SCAN_PASSIVE &&
|
|
request->type != NL802154_SCAN_ACTIVE)
|
|
return -EOPNOTSUPP;
|
|
|
|
/* Store scanning parameters */
|
|
rcu_assign_pointer(local->scan_req, request);
|
|
|
|
/* Software scanning requires to set promiscuous mode, so we need to
|
|
* pause the Tx queue during the entire operation.
|
|
*/
|
|
ieee802154_mlme_op_pre(local);
|
|
|
|
sdata->required_filtering = IEEE802154_FILTERING_3_SCAN;
|
|
local->scan_page = request->page;
|
|
local->scan_channel = -1;
|
|
set_bit(IEEE802154_IS_SCANNING, &local->ongoing);
|
|
if (request->type == NL802154_SCAN_ACTIVE)
|
|
mac802154_scan_prepare_beacon_req(local);
|
|
|
|
nl802154_scan_started(request->wpan_phy, request->wpan_dev);
|
|
|
|
queue_delayed_work(local->mac_wq, &local->scan_work, 0);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int mac802154_process_beacon(struct ieee802154_local *local,
|
|
struct sk_buff *skb,
|
|
u8 page, u8 channel)
|
|
{
|
|
struct ieee802154_beacon_hdr *bh = (void *)skb->data;
|
|
struct ieee802154_addr *src = &mac_cb(skb)->source;
|
|
struct cfg802154_scan_request *scan_req;
|
|
struct ieee802154_coord_desc desc;
|
|
|
|
if (skb->len != sizeof(*bh))
|
|
return -EINVAL;
|
|
|
|
if (unlikely(src->mode == IEEE802154_ADDR_NONE))
|
|
return -EINVAL;
|
|
|
|
dev_dbg(&skb->dev->dev,
|
|
"BEACON received on page %u channel %u\n",
|
|
page, channel);
|
|
|
|
memcpy(&desc.addr, src, sizeof(desc.addr));
|
|
desc.page = page;
|
|
desc.channel = channel;
|
|
desc.link_quality = mac_cb(skb)->lqi;
|
|
desc.superframe_spec = get_unaligned_le16(skb->data);
|
|
desc.gts_permit = bh->gts_permit;
|
|
|
|
trace_802154_scan_event(&desc);
|
|
|
|
rcu_read_lock();
|
|
scan_req = rcu_dereference(local->scan_req);
|
|
if (likely(scan_req))
|
|
nl802154_scan_event(scan_req->wpan_phy, scan_req->wpan_dev, &desc);
|
|
rcu_read_unlock();
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int mac802154_transmit_beacon(struct ieee802154_local *local,
|
|
struct wpan_dev *wpan_dev)
|
|
{
|
|
struct cfg802154_beacon_request *beacon_req;
|
|
struct ieee802154_sub_if_data *sdata;
|
|
struct sk_buff *skb;
|
|
int ret;
|
|
|
|
/* Update the sequence number */
|
|
local->beacon.mhr.seq = atomic_inc_return(&wpan_dev->bsn) & 0xFF;
|
|
|
|
skb = alloc_skb(IEEE802154_BEACON_SKB_SZ, GFP_KERNEL);
|
|
if (!skb)
|
|
return -ENOBUFS;
|
|
|
|
rcu_read_lock();
|
|
beacon_req = rcu_dereference(local->beacon_req);
|
|
if (unlikely(!beacon_req)) {
|
|
rcu_read_unlock();
|
|
kfree_skb(skb);
|
|
return -EINVAL;
|
|
}
|
|
|
|
sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
|
|
skb->dev = sdata->dev;
|
|
|
|
rcu_read_unlock();
|
|
|
|
ret = ieee802154_beacon_push(skb, &local->beacon);
|
|
if (ret) {
|
|
kfree_skb(skb);
|
|
return ret;
|
|
}
|
|
|
|
/* Using the MLME transmission helper for sending beacons is a bit
|
|
* overkill because we do not really care about the final outcome.
|
|
*
|
|
* Even though, going through the whole net stack with a regular
|
|
* dev_queue_xmit() is not relevant either because we want beacons to be
|
|
* sent "now" rather than go through the whole net stack scheduling
|
|
* (qdisc & co).
|
|
*
|
|
* Finally, using ieee802154_subif_start_xmit() would only be an option
|
|
* if we had a generic transmit helper which would acquire the
|
|
* HARD_TX_LOCK() to prevent buffer handling conflicts with regular
|
|
* packets.
|
|
*
|
|
* So for now we keep it simple and send beacons with our MLME helper,
|
|
* even if it stops the ieee802154 queue entirely during these
|
|
* transmissions, wich anyway does not have a huge impact on the
|
|
* performances given the current design of the stack.
|
|
*/
|
|
return ieee802154_mlme_tx(local, sdata, skb);
|
|
}
|
|
|
|
void mac802154_beacon_worker(struct work_struct *work)
|
|
{
|
|
struct ieee802154_local *local =
|
|
container_of(work, struct ieee802154_local, beacon_work.work);
|
|
struct cfg802154_beacon_request *beacon_req;
|
|
struct ieee802154_sub_if_data *sdata;
|
|
struct wpan_dev *wpan_dev;
|
|
u8 interval;
|
|
int ret;
|
|
|
|
rcu_read_lock();
|
|
beacon_req = rcu_dereference(local->beacon_req);
|
|
if (unlikely(!beacon_req)) {
|
|
rcu_read_unlock();
|
|
return;
|
|
}
|
|
|
|
sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
|
|
|
|
/* Wait an arbitrary amount of time in case we cannot use the device */
|
|
if (local->suspended || !ieee802154_sdata_running(sdata)) {
|
|
rcu_read_unlock();
|
|
queue_delayed_work(local->mac_wq, &local->beacon_work,
|
|
msecs_to_jiffies(1000));
|
|
return;
|
|
}
|
|
|
|
wpan_dev = beacon_req->wpan_dev;
|
|
interval = beacon_req->interval;
|
|
|
|
rcu_read_unlock();
|
|
|
|
dev_dbg(&sdata->dev->dev, "Sending beacon\n");
|
|
ret = mac802154_transmit_beacon(local, wpan_dev);
|
|
if (ret)
|
|
dev_err(&sdata->dev->dev,
|
|
"Beacon could not be transmitted (%d)\n", ret);
|
|
|
|
if (interval < IEEE802154_ACTIVE_SCAN_DURATION)
|
|
queue_delayed_work(local->mac_wq, &local->beacon_work,
|
|
local->beacon_interval);
|
|
}
|
|
|
|
int mac802154_stop_beacons_locked(struct ieee802154_local *local,
|
|
struct ieee802154_sub_if_data *sdata)
|
|
{
|
|
struct wpan_dev *wpan_dev = &sdata->wpan_dev;
|
|
struct cfg802154_beacon_request *request;
|
|
|
|
ASSERT_RTNL();
|
|
|
|
if (!mac802154_is_beaconing(local))
|
|
return -ESRCH;
|
|
|
|
clear_bit(IEEE802154_IS_BEACONING, &local->ongoing);
|
|
cancel_delayed_work(&local->beacon_work);
|
|
request = rcu_replace_pointer(local->beacon_req, NULL, 1);
|
|
if (!request)
|
|
return 0;
|
|
kvfree_rcu_mightsleep(request);
|
|
|
|
nl802154_beaconing_done(wpan_dev);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int mac802154_send_beacons_locked(struct ieee802154_sub_if_data *sdata,
|
|
struct cfg802154_beacon_request *request)
|
|
{
|
|
struct ieee802154_local *local = sdata->local;
|
|
|
|
ASSERT_RTNL();
|
|
|
|
if (mac802154_is_beaconing(local))
|
|
mac802154_stop_beacons_locked(local, sdata);
|
|
|
|
/* Store beaconing parameters */
|
|
rcu_assign_pointer(local->beacon_req, request);
|
|
|
|
set_bit(IEEE802154_IS_BEACONING, &local->ongoing);
|
|
|
|
memset(&local->beacon, 0, sizeof(local->beacon));
|
|
local->beacon.mhr.fc.type = IEEE802154_FC_TYPE_BEACON;
|
|
local->beacon.mhr.fc.security_enabled = 0;
|
|
local->beacon.mhr.fc.frame_pending = 0;
|
|
local->beacon.mhr.fc.ack_request = 0;
|
|
local->beacon.mhr.fc.intra_pan = 0;
|
|
local->beacon.mhr.fc.dest_addr_mode = IEEE802154_NO_ADDRESSING;
|
|
local->beacon.mhr.fc.version = IEEE802154_2003_STD;
|
|
local->beacon.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
|
|
atomic_set(&request->wpan_dev->bsn, -1);
|
|
local->beacon.mhr.source.mode = IEEE802154_ADDR_LONG;
|
|
local->beacon.mhr.source.pan_id = request->wpan_dev->pan_id;
|
|
local->beacon.mhr.source.extended_addr = request->wpan_dev->extended_addr;
|
|
local->beacon.mac_pl.beacon_order = request->interval;
|
|
if (request->interval <= IEEE802154_MAX_SCAN_DURATION)
|
|
local->beacon.mac_pl.superframe_order = request->interval;
|
|
local->beacon.mac_pl.final_cap_slot = 0xf;
|
|
local->beacon.mac_pl.battery_life_ext = 0;
|
|
/* TODO: Fill this field with the coordinator situation in the network */
|
|
local->beacon.mac_pl.pan_coordinator = 1;
|
|
local->beacon.mac_pl.assoc_permit = 1;
|
|
|
|
if (request->interval == IEEE802154_ACTIVE_SCAN_DURATION)
|
|
return 0;
|
|
|
|
/* Start the beacon work */
|
|
local->beacon_interval =
|
|
mac802154_scan_get_channel_time(request->interval,
|
|
request->wpan_phy->symbol_duration);
|
|
queue_delayed_work(local->mac_wq, &local->beacon_work, 0);
|
|
|
|
return 0;
|
|
}
|