[linux-2.6-block.git] / net / mac802154 / scan.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;
	kfree_rcu(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;
	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;

	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);

	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;
	kfree_rcu(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;
	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 depending on the coordinator capacity */
	local->beacon.mac_pl.pan_coordinator = 1;
	local->beacon.mac_pl.assoc_permit = 1;

	/* 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;
}
Commit	Line	Data
57588c71 MR	1	// SPDX-License-Identifier: GPL-2.0-only
	2	/*
	3	* IEEE 802.15.4 scanning management
	4	*
	5	* Copyright (C) 2021 Qorvo US, Inc
	6	* Authors:
	7	* - David Girault <david.girault@qorvo.com>
	8	* - Miquel Raynal <miquel.raynal@bootlin.com>
	9	*/
	10
	11	#include <linux/module.h>
	12	#include <linux/rtnetlink.h>
	13	#include <net/mac802154.h>
	14
	15	#include "ieee802154_i.h"
	16	#include "driver-ops.h"
	17	#include "../ieee802154/nl802154.h"
	18
3accf476 MR	19	#define IEEE802154_BEACON_MHR_SZ 13
3accf476 MR	20	#define IEEE802154_BEACON_PL_SZ 4
e2c3e6f5 MR	21	#define IEEE802154_MAC_CMD_MHR_SZ 23
e2c3e6f5 MR	22	#define IEEE802154_MAC_CMD_PL_SZ 1
3accf476 MR	23	#define IEEE802154_BEACON_SKB_SZ (IEEE802154_BEACON_MHR_SZ + \
3accf476 MR	24	IEEE802154_BEACON_PL_SZ)
e2c3e6f5 MR	25	#define IEEE802154_MAC_CMD_SKB_SZ (IEEE802154_MAC_CMD_MHR_SZ + \
e2c3e6f5 MR	26	IEEE802154_MAC_CMD_PL_SZ)
3accf476	27
57588c71 MR	28	/* mac802154_scan_cleanup_locked() must be called upon scan completion or abort.
	29	* - Completions are asynchronous, not locked by the rtnl and decided by the
	30	* scan worker.
	31	* - Aborts are decided by userspace, and locked by the rtnl.
	32	*
	33	* Concurrent modifications to the PHY, the interfaces or the hardware is in
	34	* general prevented by the rtnl. So in most cases we don't need additional
	35	* protection.
	36	*
	37	* However, the scan worker get's triggered without anybody noticing and thus we
	38	* must ensure the presence of the devices as well as data consistency:
	39	* - The sub-interface and device driver module get both their reference
	40	* counters incremented whenever we start a scan, so they cannot disappear
	41	* during operation.
	42	* - Data consistency is achieved by the use of rcu protected pointers.
	43	*/
	44	static int mac802154_scan_cleanup_locked(struct ieee802154_local *local,
	45	struct ieee802154_sub_if_data *sdata,
	46	bool aborted)
	47	{
	48	struct wpan_dev *wpan_dev = &sdata->wpan_dev;
	49	struct wpan_phy *wpan_phy = local->phy;
	50	struct cfg802154_scan_request *request;
	51	u8 arg;
	52
	53	/* Prevent any further use of the scan request */
	54	clear_bit(IEEE802154_IS_SCANNING, &local->ongoing);
	55	cancel_delayed_work(&local->scan_work);
	56	request = rcu_replace_pointer(local->scan_req, NULL, 1);
	57	if (!request)
	58	return 0;
	59	kfree_rcu(request);
	60
	61	/* Advertize first, while we know the devices cannot be removed */
	62	if (aborted)
	63	arg = NL802154_SCAN_DONE_REASON_ABORTED;
	64	else
	65	arg = NL802154_SCAN_DONE_REASON_FINISHED;
	66	nl802154_scan_done(wpan_phy, wpan_dev, arg);
	67
	68	/* Cleanup software stack */
	69	ieee802154_mlme_op_post(local);
	70
	71	/* Set the hardware back in its original state */
	72	drv_set_channel(local, wpan_phy->current_page,
	73	wpan_phy->current_channel);
	74	ieee802154_configure_durations(wpan_phy, wpan_phy->current_page,
	75	wpan_phy->current_channel);
	76	drv_stop(local);
	77	synchronize_net();
	78	sdata->required_filtering = sdata->iface_default_filtering;
	79	drv_start(local, sdata->required_filtering, &local->addr_filt);
	80
	81	return 0;
	82	}
	83
	84	int mac802154_abort_scan_locked(struct ieee802154_local *local,
	85	struct ieee802154_sub_if_data *sdata)
	86	{
	87	ASSERT_RTNL();
	88
	89	if (!mac802154_is_scanning(local))
	90	return -ESRCH;
	91
92	return mac802154_scan_cleanup_locked(local, sdata, true);
93	}
94
95	static unsigned int mac802154_scan_get_channel_time(u8 duration_order,
96	u8 symbol_duration)
97	{
98	u64 base_super_frame_duration = (u64)symbol_duration *
99	IEEE802154_SUPERFRAME_PERIOD * IEEE802154_SLOT_PERIOD;
100
101	return usecs_to_jiffies(base_super_frame_duration *
102	(BIT(duration_order) + 1));
103	}
104
105	static void mac802154_flush_queued_beacons(struct ieee802154_local *local)
106	{
107	struct cfg802154_mac_pkt mac_pkt, tmp;
108
109	list_for_each_entry_safe(mac_pkt, tmp, &local->rx_beacon_list, node) {
110	list_del(&mac_pkt->node);
111	kfree_skb(mac_pkt->skb);
112	kfree(mac_pkt);
113	}
114	}
115
116	static void
117	mac802154_scan_get_next_channel(struct ieee802154_local *local,
118	struct cfg802154_scan_request *scan_req,
119	u8 *channel)
120	{
121	(*channel)++;
122	channel = find_next_bit((const unsigned long )&scan_req->channels,
123	IEEE802154_MAX_CHANNEL + 1,
124	*channel);
125	}
126
127	static int mac802154_scan_find_next_chan(struct ieee802154_local *local,
128	struct cfg802154_scan_request *scan_req,
129	u8 page, u8 *channel)
130	{
131	mac802154_scan_get_next_channel(local, scan_req, channel);
132	if (*channel > IEEE802154_MAX_CHANNEL)
133	return -EINVAL;
134
135	return 0;
136	}
137
e2c3e6f5 MR	138	static int mac802154_scan_prepare_beacon_req(struct ieee802154_local *local)
	139	{
	140	memset(&local->scan_beacon_req, 0, sizeof(local->scan_beacon_req));
	141	local->scan_beacon_req.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD;
	142	local->scan_beacon_req.mhr.fc.dest_addr_mode = IEEE802154_SHORT_ADDRESSING;
	143	local->scan_beacon_req.mhr.fc.version = IEEE802154_2003_STD;
	144	local->scan_beacon_req.mhr.fc.source_addr_mode = IEEE802154_NO_ADDRESSING;
	145	local->scan_beacon_req.mhr.dest.mode = IEEE802154_ADDR_SHORT;
	146	local->scan_beacon_req.mhr.dest.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
	147	local->scan_beacon_req.mhr.dest.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
	148	local->scan_beacon_req.mac_pl.cmd_id = IEEE802154_CMD_BEACON_REQ;
	149
	150	return 0;
	151	}
	152
	153	static int mac802154_transmit_beacon_req(struct ieee802154_local *local,
	154	struct ieee802154_sub_if_data *sdata)
	155	{
	156	struct sk_buff *skb;
	157	int ret;
	158
	159	skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ, GFP_KERNEL);
	160	if (!skb)
	161	return -ENOBUFS;
	162
	163	skb->dev = sdata->dev;
	164
	165	ret = ieee802154_mac_cmd_push(skb, &local->scan_beacon_req, NULL, 0);
	166	if (ret) {
	167	kfree_skb(skb);
	168	return ret;
	169	}
	170
	171	return ieee802154_mlme_tx(local, sdata, skb);
	172	}
	173
57588c71 MR	174	void mac802154_scan_worker(struct work_struct *work)
	175	{
	176	struct ieee802154_local *local =
	177	container_of(work, struct ieee802154_local, scan_work.work);
	178	struct cfg802154_scan_request *scan_req;
	179	struct ieee802154_sub_if_data *sdata;
	180	unsigned int scan_duration = 0;
	181	struct wpan_phy *wpan_phy;
	182	u8 scan_req_duration;
	183	u8 page, channel;
	184	int ret;
	185
	186	/* Ensure the device receiver is turned off when changing channels
	187	* because there is no atomic way to change the channel and know on
	188	* which one a beacon might have been received.
	189	*/
	190	drv_stop(local);
	191	synchronize_net();
	192	mac802154_flush_queued_beacons(local);
	193
	194	rcu_read_lock();
	195	scan_req = rcu_dereference(local->scan_req);
	196	if (unlikely(!scan_req)) {
	197	rcu_read_unlock();
	198	return;
	199	}
	200
	201	sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(scan_req->wpan_dev);
	202
	203	/* Wait an arbitrary amount of time in case we cannot use the device */
	204	if (local->suspended \|\| !ieee802154_sdata_running(sdata)) {
	205	rcu_read_unlock();
	206	queue_delayed_work(local->mac_wq, &local->scan_work,
	207	msecs_to_jiffies(1000));
	208	return;
	209	}
	210
	211	wpan_phy = scan_req->wpan_phy;
	212	scan_req_duration = scan_req->duration;
	213
	214	/* Look for the next valid chan */
	215	page = local->scan_page;
	216	channel = local->scan_channel;
	217	do {
	218	ret = mac802154_scan_find_next_chan(local, scan_req, page, &channel);
	219	if (ret) {
	220	rcu_read_unlock();
	221	goto end_scan;
	222	}
	223	} while (!ieee802154_chan_is_valid(scan_req->wpan_phy, page, channel));
	224
	225	rcu_read_unlock();
	226
	227	/* Bypass the stack on purpose when changing the channel */
	228	rtnl_lock();
	229	ret = drv_set_channel(local, page, channel);
	230	rtnl_unlock();
	231	if (ret) {
	232	dev_err(&sdata->dev->dev,
	233	"Channel change failure during scan, aborting (%d)\n", ret);
	234	goto end_scan;
	235	}
	236
	237	local->scan_page = page;
238	local->scan_channel = channel;
239
240	rtnl_lock();
241	ret = drv_start(local, IEEE802154_FILTERING_3_SCAN, &local->addr_filt);
242	rtnl_unlock();
243	if (ret) {
244	dev_err(&sdata->dev->dev,
245	"Restarting failure after channel change, aborting (%d)\n", ret);
246	goto end_scan;
247	}
248
e2c3e6f5 MR	249	if (scan_req->type == NL802154_SCAN_ACTIVE) {
	250	ret = mac802154_transmit_beacon_req(local, sdata);
	251	if (ret)
	252	dev_err(&sdata->dev->dev,
	253	"Error when transmitting beacon request (%d)\n", ret);
	254	}
	255
57588c71 MR	256	ieee802154_configure_durations(wpan_phy, page, channel);
	257	scan_duration = mac802154_scan_get_channel_time(scan_req_duration,
	258	wpan_phy->symbol_duration);
	259	dev_dbg(&sdata->dev->dev,
	260	"Scan page %u channel %u for %ums\n",
	261	page, channel, jiffies_to_msecs(scan_duration));
	262	queue_delayed_work(local->mac_wq, &local->scan_work, scan_duration);
	263	return;
	264
	265	end_scan:
	266	rtnl_lock();
	267	mac802154_scan_cleanup_locked(local, sdata, false);
	268	rtnl_unlock();
	269	}
	270
	271	int mac802154_trigger_scan_locked(struct ieee802154_sub_if_data *sdata,
	272	struct cfg802154_scan_request *request)
	273	{
	274	struct ieee802154_local *local = sdata->local;
	275
	276	ASSERT_RTNL();
	277
	278	if (mac802154_is_scanning(local))
	279	return -EBUSY;
	280
e2c3e6f5 MR	281	if (request->type != NL802154_SCAN_PASSIVE &&
e2c3e6f5 MR	282	request->type != NL802154_SCAN_ACTIVE)
57588c71 MR	283	return -EOPNOTSUPP;
	284
	285	/* Store scanning parameters */
	286	rcu_assign_pointer(local->scan_req, request);
	287
	288	/* Software scanning requires to set promiscuous mode, so we need to
	289	* pause the Tx queue during the entire operation.
	290	*/
	291	ieee802154_mlme_op_pre(local);
	292
	293	sdata->required_filtering = IEEE802154_FILTERING_3_SCAN;
	294	local->scan_page = request->page;
	295	local->scan_channel = -1;
	296	set_bit(IEEE802154_IS_SCANNING, &local->ongoing);
e2c3e6f5 MR	297	if (request->type == NL802154_SCAN_ACTIVE)
e2c3e6f5 MR	298	mac802154_scan_prepare_beacon_req(local);
57588c71 MR	299
	300	nl802154_scan_started(request->wpan_phy, request->wpan_dev);
	301
	302	queue_delayed_work(local->mac_wq, &local->scan_work, 0);
	303
	304	return 0;
	305	}
	306
	307	int mac802154_process_beacon(struct ieee802154_local *local,
	308	struct sk_buff *skb,
	309	u8 page, u8 channel)
	310	{
	311	struct ieee802154_beacon_hdr bh = (void )skb->data;
	312	struct ieee802154_addr *src = &mac_cb(skb)->source;
	313	struct cfg802154_scan_request *scan_req;
	314	struct ieee802154_coord_desc desc;
	315
	316	if (skb->len != sizeof(*bh))
	317	return -EINVAL;
	318
	319	if (unlikely(src->mode == IEEE802154_ADDR_NONE))
	320	return -EINVAL;
	321
	322	dev_dbg(&skb->dev->dev,
	323	"BEACON received on page %u channel %u\n",
	324	page, channel);
	325
	326	memcpy(&desc.addr, src, sizeof(desc.addr));
	327	desc.page = page;
	328	desc.channel = channel;
	329	desc.link_quality = mac_cb(skb)->lqi;
	330	desc.superframe_spec = get_unaligned_le16(skb->data);
	331	desc.gts_permit = bh->gts_permit;
	332
	333	trace_802154_scan_event(&desc);
	334
	335	rcu_read_lock();
	336	scan_req = rcu_dereference(local->scan_req);
	337	if (likely(scan_req))
	338	nl802154_scan_event(scan_req->wpan_phy, scan_req->wpan_dev, &desc);
	339	rcu_read_unlock();
	340
	341	return 0;
	342	}
3accf476 MR	343
	344	static int mac802154_transmit_beacon(struct ieee802154_local *local,
	345	struct wpan_dev *wpan_dev)
	346	{
	347	struct cfg802154_beacon_request *beacon_req;
	348	struct ieee802154_sub_if_data *sdata;
	349	struct sk_buff *skb;
	350	int ret;
	351
	352	/* Update the sequence number */
	353	local->beacon.mhr.seq = atomic_inc_return(&wpan_dev->bsn) & 0xFF;
	354
	355	skb = alloc_skb(IEEE802154_BEACON_SKB_SZ, GFP_KERNEL);
	356	if (!skb)
	357	return -ENOBUFS;
	358
	359	rcu_read_lock();
	360	beacon_req = rcu_dereference(local->beacon_req);
	361	if (unlikely(!beacon_req)) {
	362	rcu_read_unlock();
	363	kfree_skb(skb);
	364	return -EINVAL;
	365	}
	366
	367	sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
	368	skb->dev = sdata->dev;
	369
	370	rcu_read_unlock();
	371
	372	ret = ieee802154_beacon_push(skb, &local->beacon);
	373	if (ret) {
	374	kfree_skb(skb);
	375	return ret;
	376	}
	377
1375e3ba MR	378	/* Using the MLME transmission helper for sending beacons is a bit
	379	* overkill because we do not really care about the final outcome.
	380	*
	381	* Even though, going through the whole net stack with a regular
	382	* dev_queue_xmit() is not relevant either because we want beacons to be
	383	* sent "now" rather than go through the whole net stack scheduling
	384	* (qdisc & co).
	385	*
	386	* Finally, using ieee802154_subif_start_xmit() would only be an option
	387	* if we had a generic transmit helper which would acquire the
	388	* HARD_TX_LOCK() to prevent buffer handling conflicts with regular
	389	* packets.
	390	*
	391	* So for now we keep it simple and send beacons with our MLME helper,
	392	* even if it stops the ieee802154 queue entirely during these
	393	* transmissions, wich anyway does not have a huge impact on the
	394	* performances given the current design of the stack.
	395	*/
	396	return ieee802154_mlme_tx(local, sdata, skb);
3accf476 MR	397	}
	398
	399	void mac802154_beacon_worker(struct work_struct *work)
	400	{
	401	struct ieee802154_local *local =
	402	container_of(work, struct ieee802154_local, beacon_work.work);
	403	struct cfg802154_beacon_request *beacon_req;
	404	struct ieee802154_sub_if_data *sdata;
	405	struct wpan_dev *wpan_dev;
	406	int ret;
	407
	408	rcu_read_lock();
	409	beacon_req = rcu_dereference(local->beacon_req);
	410	if (unlikely(!beacon_req)) {
	411	rcu_read_unlock();
	412	return;
	413	}
	414
	415	sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
	416
	417	/* Wait an arbitrary amount of time in case we cannot use the device */
	418	if (local->suspended \|\| !ieee802154_sdata_running(sdata)) {
	419	rcu_read_unlock();
	420	queue_delayed_work(local->mac_wq, &local->beacon_work,
	421	msecs_to_jiffies(1000));
	422	return;
	423	}
	424
	425	wpan_dev = beacon_req->wpan_dev;
	426
	427	rcu_read_unlock();
	428
	429	dev_dbg(&sdata->dev->dev, "Sending beacon\n");
	430	ret = mac802154_transmit_beacon(local, wpan_dev);
	431	if (ret)
	432	dev_err(&sdata->dev->dev,
	433	"Beacon could not be transmitted (%d)\n", ret);
	434
61d7dddf MR	435	queue_delayed_work(local->mac_wq, &local->beacon_work,
61d7dddf MR	436	local->beacon_interval);
3accf476 MR	437	}
	438
	439	int mac802154_stop_beacons_locked(struct ieee802154_local *local,
	440	struct ieee802154_sub_if_data *sdata)
	441	{
	442	struct wpan_dev *wpan_dev = &sdata->wpan_dev;
	443	struct cfg802154_beacon_request *request;
	444
	445	ASSERT_RTNL();
	446
	447	if (!mac802154_is_beaconing(local))
	448	return -ESRCH;
	449
	450	clear_bit(IEEE802154_IS_BEACONING, &local->ongoing);
	451	cancel_delayed_work(&local->beacon_work);
	452	request = rcu_replace_pointer(local->beacon_req, NULL, 1);
	453	if (!request)
	454	return 0;
	455	kfree_rcu(request);
	456
	457	nl802154_beaconing_done(wpan_dev);
	458
	459	return 0;
	460	}
	461
	462	int mac802154_send_beacons_locked(struct ieee802154_sub_if_data *sdata,
	463	struct cfg802154_beacon_request *request)
	464	{
	465	struct ieee802154_local *local = sdata->local;
	466
	467	ASSERT_RTNL();
	468
	469	if (mac802154_is_beaconing(local))
	470	mac802154_stop_beacons_locked(local, sdata);
	471
	472	/* Store beaconing parameters */
	473	rcu_assign_pointer(local->beacon_req, request);
	474
	475	set_bit(IEEE802154_IS_BEACONING, &local->ongoing);
	476
	477	memset(&local->beacon, 0, sizeof(local->beacon));
	478	local->beacon.mhr.fc.type = IEEE802154_FC_TYPE_BEACON;
	479	local->beacon.mhr.fc.security_enabled = 0;
	480	local->beacon.mhr.fc.frame_pending = 0;
	481	local->beacon.mhr.fc.ack_request = 0;
	482	local->beacon.mhr.fc.intra_pan = 0;
	483	local->beacon.mhr.fc.dest_addr_mode = IEEE802154_NO_ADDRESSING;
	484	local->beacon.mhr.fc.version = IEEE802154_2003_STD;
	485	local->beacon.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
	486	atomic_set(&request->wpan_dev->bsn, -1);
	487	local->beacon.mhr.source.mode = IEEE802154_ADDR_LONG;
8338304c MR	488	local->beacon.mhr.source.pan_id = request->wpan_dev->pan_id;
8338304c MR	489	local->beacon.mhr.source.extended_addr = request->wpan_dev->extended_addr;
3accf476 MR	490	local->beacon.mac_pl.beacon_order = request->interval;
	491	local->beacon.mac_pl.superframe_order = request->interval;
	492	local->beacon.mac_pl.final_cap_slot = 0xf;
	493	local->beacon.mac_pl.battery_life_ext = 0;
	494	/* TODO: Fill this field depending on the coordinator capacity */
	495	local->beacon.mac_pl.pan_coordinator = 1;
	496	local->beacon.mac_pl.assoc_permit = 1;
	497
	498	/* Start the beacon work */
	499	local->beacon_interval =
	500	mac802154_scan_get_channel_time(request->interval,
	501	request->wpan_phy->symbol_duration);
	502	queue_delayed_work(local->mac_wq, &local->beacon_work, 0);
	503
	504	return 0;
	505	}