[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_BEACON_SKB_SZ (IEEE802154_BEACON_MHR_SZ + \
				  IEEE802154_BEACON_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;
}

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

	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;

	/* TODO: support other scanning type */
	if (request->type != NL802154_SCAN_PASSIVE)
		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);

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

	return ieee802154_subif_start_xmit(skb, sdata->dev);
}

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

	if (local->beacon_interval >= 0)
		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 = cpu_to_le16(request->wpan_dev->pan_id);
	local->beacon.mhr.source.extended_addr = cpu_to_le64(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
	20	#define IEEE802154_BEACON_PL_SZ 4
	21	#define IEEE802154_BEACON_SKB_SZ (IEEE802154_BEACON_MHR_SZ + \
	22	IEEE802154_BEACON_PL_SZ)
	23
57588c71 MR	24	/* mac802154_scan_cleanup_locked() must be called upon scan completion or abort.
	25	* - Completions are asynchronous, not locked by the rtnl and decided by the
	26	* scan worker.
	27	* - Aborts are decided by userspace, and locked by the rtnl.
	28	*
	29	* Concurrent modifications to the PHY, the interfaces or the hardware is in
	30	* general prevented by the rtnl. So in most cases we don't need additional
	31	* protection.
	32	*
	33	* However, the scan worker get's triggered without anybody noticing and thus we
	34	* must ensure the presence of the devices as well as data consistency:
	35	* - The sub-interface and device driver module get both their reference
	36	* counters incremented whenever we start a scan, so they cannot disappear
	37	* during operation.
	38	* - Data consistency is achieved by the use of rcu protected pointers.
	39	*/
	40	static int mac802154_scan_cleanup_locked(struct ieee802154_local *local,
	41	struct ieee802154_sub_if_data *sdata,
	42	bool aborted)
	43	{
	44	struct wpan_dev *wpan_dev = &sdata->wpan_dev;
	45	struct wpan_phy *wpan_phy = local->phy;
	46	struct cfg802154_scan_request *request;
	47	u8 arg;
	48
	49	/* Prevent any further use of the scan request */
	50	clear_bit(IEEE802154_IS_SCANNING, &local->ongoing);
	51	cancel_delayed_work(&local->scan_work);
	52	request = rcu_replace_pointer(local->scan_req, NULL, 1);
	53	if (!request)
	54	return 0;
	55	kfree_rcu(request);
	56
	57	/* Advertize first, while we know the devices cannot be removed */
	58	if (aborted)
	59	arg = NL802154_SCAN_DONE_REASON_ABORTED;
	60	else
	61	arg = NL802154_SCAN_DONE_REASON_FINISHED;
	62	nl802154_scan_done(wpan_phy, wpan_dev, arg);
	63
	64	/* Cleanup software stack */
	65	ieee802154_mlme_op_post(local);
	66
	67	/* Set the hardware back in its original state */
	68	drv_set_channel(local, wpan_phy->current_page,
	69	wpan_phy->current_channel);
	70	ieee802154_configure_durations(wpan_phy, wpan_phy->current_page,
	71	wpan_phy->current_channel);
	72	drv_stop(local);
	73	synchronize_net();
	74	sdata->required_filtering = sdata->iface_default_filtering;
	75	drv_start(local, sdata->required_filtering, &local->addr_filt);
	76
	77	return 0;
	78	}
	79
	80	int mac802154_abort_scan_locked(struct ieee802154_local *local,
	81	struct ieee802154_sub_if_data *sdata)
	82	{
	83	ASSERT_RTNL();
	84
	85	if (!mac802154_is_scanning(local))
	86	return -ESRCH;
	87
88	return mac802154_scan_cleanup_locked(local, sdata, true);
89	}
90
91	static unsigned int mac802154_scan_get_channel_time(u8 duration_order,
92	u8 symbol_duration)
93	{
94	u64 base_super_frame_duration = (u64)symbol_duration *
95	IEEE802154_SUPERFRAME_PERIOD * IEEE802154_SLOT_PERIOD;
96
97	return usecs_to_jiffies(base_super_frame_duration *
98	(BIT(duration_order) + 1));
99	}
100
101	static void mac802154_flush_queued_beacons(struct ieee802154_local *local)
102	{
103	struct cfg802154_mac_pkt mac_pkt, tmp;
104
105	list_for_each_entry_safe(mac_pkt, tmp, &local->rx_beacon_list, node) {
106	list_del(&mac_pkt->node);
107	kfree_skb(mac_pkt->skb);
108	kfree(mac_pkt);
109	}
110	}
111
112	static void
113	mac802154_scan_get_next_channel(struct ieee802154_local *local,
114	struct cfg802154_scan_request *scan_req,
115	u8 *channel)
116	{
117	(*channel)++;
118	channel = find_next_bit((const unsigned long )&scan_req->channels,
119	IEEE802154_MAX_CHANNEL + 1,
120	*channel);
121	}
122
123	static int mac802154_scan_find_next_chan(struct ieee802154_local *local,
124	struct cfg802154_scan_request *scan_req,
125	u8 page, u8 *channel)
126	{
127	mac802154_scan_get_next_channel(local, scan_req, channel);
128	if (*channel > IEEE802154_MAX_CHANNEL)
129	return -EINVAL;
130
131	return 0;
132	}
133
134	void mac802154_scan_worker(struct work_struct *work)
135	{
136	struct ieee802154_local *local =
137	container_of(work, struct ieee802154_local, scan_work.work);
138	struct cfg802154_scan_request *scan_req;
139	struct ieee802154_sub_if_data *sdata;
140	unsigned int scan_duration = 0;
141	struct wpan_phy *wpan_phy;
142	u8 scan_req_duration;
143	u8 page, channel;
144	int ret;
145
146	/* Ensure the device receiver is turned off when changing channels
147	* because there is no atomic way to change the channel and know on
148	* which one a beacon might have been received.
149	*/
150	drv_stop(local);
151	synchronize_net();
152	mac802154_flush_queued_beacons(local);
153
154	rcu_read_lock();
155	scan_req = rcu_dereference(local->scan_req);
156	if (unlikely(!scan_req)) {
157	rcu_read_unlock();
158	return;
159	}
160
161	sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(scan_req->wpan_dev);
162
163	/* Wait an arbitrary amount of time in case we cannot use the device */
164	if (local->suspended \|\| !ieee802154_sdata_running(sdata)) {
165	rcu_read_unlock();
166	queue_delayed_work(local->mac_wq, &local->scan_work,
167	msecs_to_jiffies(1000));
168	return;
169	}
170
171	wpan_phy = scan_req->wpan_phy;
172	scan_req_duration = scan_req->duration;
173
174	/* Look for the next valid chan */
175	page = local->scan_page;
176	channel = local->scan_channel;
177	do {
178	ret = mac802154_scan_find_next_chan(local, scan_req, page, &channel);
179	if (ret) {
180	rcu_read_unlock();
181	goto end_scan;
182	}
183	} while (!ieee802154_chan_is_valid(scan_req->wpan_phy, page, channel));
184
185	rcu_read_unlock();
186
187	/* Bypass the stack on purpose when changing the channel */
188	rtnl_lock();
189	ret = drv_set_channel(local, page, channel);
190	rtnl_unlock();
191	if (ret) {
192	dev_err(&sdata->dev->dev,
193	"Channel change failure during scan, aborting (%d)\n", ret);
194	goto end_scan;
195	}
196
197	local->scan_page = page;
198	local->scan_channel = channel;
199
200	rtnl_lock();
201	ret = drv_start(local, IEEE802154_FILTERING_3_SCAN, &local->addr_filt);
202	rtnl_unlock();
203	if (ret) {
204	dev_err(&sdata->dev->dev,
205	"Restarting failure after channel change, aborting (%d)\n", ret);
206	goto end_scan;
207	}
208
209	ieee802154_configure_durations(wpan_phy, page, channel);
210	scan_duration = mac802154_scan_get_channel_time(scan_req_duration,
211	wpan_phy->symbol_duration);
212	dev_dbg(&sdata->dev->dev,
213	"Scan page %u channel %u for %ums\n",
214	page, channel, jiffies_to_msecs(scan_duration));
215	queue_delayed_work(local->mac_wq, &local->scan_work, scan_duration);
216	return;
217
218	end_scan:
219	rtnl_lock();
220	mac802154_scan_cleanup_locked(local, sdata, false);
221	rtnl_unlock();
222	}
223
224	int mac802154_trigger_scan_locked(struct ieee802154_sub_if_data *sdata,
225	struct cfg802154_scan_request *request)
226	{
227	struct ieee802154_local *local = sdata->local;
228
229	ASSERT_RTNL();
230
231	if (mac802154_is_scanning(local))
232	return -EBUSY;
233
234	/* TODO: support other scanning type */
235	if (request->type != NL802154_SCAN_PASSIVE)
236	return -EOPNOTSUPP;
237
238	/* Store scanning parameters */
239	rcu_assign_pointer(local->scan_req, request);
240
241	/* Software scanning requires to set promiscuous mode, so we need to
242	* pause the Tx queue during the entire operation.
243	*/
244	ieee802154_mlme_op_pre(local);
245
246	sdata->required_filtering = IEEE802154_FILTERING_3_SCAN;
247	local->scan_page = request->page;
248	local->scan_channel = -1;
249	set_bit(IEEE802154_IS_SCANNING, &local->ongoing);
250
251	nl802154_scan_started(request->wpan_phy, request->wpan_dev);
252
253	queue_delayed_work(local->mac_wq, &local->scan_work, 0);
254
255	return 0;
256	}
257
258	int mac802154_process_beacon(struct ieee802154_local *local,
259	struct sk_buff *skb,
260	u8 page, u8 channel)
261	{
262	struct ieee802154_beacon_hdr bh = (void )skb->data;
263	struct ieee802154_addr *src = &mac_cb(skb)->source;
264	struct cfg802154_scan_request *scan_req;
265	struct ieee802154_coord_desc desc;
266
267	if (skb->len != sizeof(*bh))
268	return -EINVAL;
269
270	if (unlikely(src->mode == IEEE802154_ADDR_NONE))
271	return -EINVAL;
272
273	dev_dbg(&skb->dev->dev,
274	"BEACON received on page %u channel %u\n",
275	page, channel);
276
277	memcpy(&desc.addr, src, sizeof(desc.addr));
278	desc.page = page;
279	desc.channel = channel;
280	desc.link_quality = mac_cb(skb)->lqi;
281	desc.superframe_spec = get_unaligned_le16(skb->data);
282	desc.gts_permit = bh->gts_permit;
283
284	trace_802154_scan_event(&desc);
285
286	rcu_read_lock();
287	scan_req = rcu_dereference(local->scan_req);
288	if (likely(scan_req))
289	nl802154_scan_event(scan_req->wpan_phy, scan_req->wpan_dev, &desc);
290	rcu_read_unlock();
291
292	return 0;
293	}
3accf476 MR	294
	295	static int mac802154_transmit_beacon(struct ieee802154_local *local,
	296	struct wpan_dev *wpan_dev)
	297	{
	298	struct cfg802154_beacon_request *beacon_req;
	299	struct ieee802154_sub_if_data *sdata;
	300	struct sk_buff *skb;
	301	int ret;
	302
	303	/* Update the sequence number */
	304	local->beacon.mhr.seq = atomic_inc_return(&wpan_dev->bsn) & 0xFF;
	305
	306	skb = alloc_skb(IEEE802154_BEACON_SKB_SZ, GFP_KERNEL);
	307	if (!skb)
	308	return -ENOBUFS;
	309
	310	rcu_read_lock();
	311	beacon_req = rcu_dereference(local->beacon_req);
	312	if (unlikely(!beacon_req)) {
	313	rcu_read_unlock();
	314	kfree_skb(skb);
	315	return -EINVAL;
	316	}
	317
	318	sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
	319	skb->dev = sdata->dev;
	320
	321	rcu_read_unlock();
	322
	323	ret = ieee802154_beacon_push(skb, &local->beacon);
	324	if (ret) {
	325	kfree_skb(skb);
	326	return ret;
	327	}
	328
	329	return ieee802154_subif_start_xmit(skb, sdata->dev);
	330	}
	331
	332	void mac802154_beacon_worker(struct work_struct *work)
	333	{
	334	struct ieee802154_local *local =
	335	container_of(work, struct ieee802154_local, beacon_work.work);
	336	struct cfg802154_beacon_request *beacon_req;
	337	struct ieee802154_sub_if_data *sdata;
	338	struct wpan_dev *wpan_dev;
	339	int ret;
	340
	341	rcu_read_lock();
	342	beacon_req = rcu_dereference(local->beacon_req);
	343	if (unlikely(!beacon_req)) {
	344	rcu_read_unlock();
	345	return;
	346	}
	347
	348	sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
	349
	350	/* Wait an arbitrary amount of time in case we cannot use the device */
	351	if (local->suspended \|\| !ieee802154_sdata_running(sdata)) {
	352	rcu_read_unlock();
	353	queue_delayed_work(local->mac_wq, &local->beacon_work,
	354	msecs_to_jiffies(1000));
	355	return;
	356	}
	357
358	wpan_dev = beacon_req->wpan_dev;
359
360	rcu_read_unlock();
361
362	dev_dbg(&sdata->dev->dev, "Sending beacon\n");
363	ret = mac802154_transmit_beacon(local, wpan_dev);
364	if (ret)
365	dev_err(&sdata->dev->dev,
366	"Beacon could not be transmitted (%d)\n", ret);
367
368	if (local->beacon_interval >= 0)
369	queue_delayed_work(local->mac_wq, &local->beacon_work,
370	local->beacon_interval);
371	}
372
373	int mac802154_stop_beacons_locked(struct ieee802154_local *local,
374	struct ieee802154_sub_if_data *sdata)
375	{
376	struct wpan_dev *wpan_dev = &sdata->wpan_dev;
377	struct cfg802154_beacon_request *request;
378
379	ASSERT_RTNL();
380
381	if (!mac802154_is_beaconing(local))
382	return -ESRCH;
383
384	clear_bit(IEEE802154_IS_BEACONING, &local->ongoing);
385	cancel_delayed_work(&local->beacon_work);
386	request = rcu_replace_pointer(local->beacon_req, NULL, 1);
387	if (!request)
388	return 0;
389	kfree_rcu(request);
390
391	nl802154_beaconing_done(wpan_dev);
392
393	return 0;
394	}
395
396	int mac802154_send_beacons_locked(struct ieee802154_sub_if_data *sdata,
397	struct cfg802154_beacon_request *request)
398	{
399	struct ieee802154_local *local = sdata->local;
400
401	ASSERT_RTNL();
402
403	if (mac802154_is_beaconing(local))
404	mac802154_stop_beacons_locked(local, sdata);
405
406	/* Store beaconing parameters */
407	rcu_assign_pointer(local->beacon_req, request);
408
409	set_bit(IEEE802154_IS_BEACONING, &local->ongoing);
410
411	memset(&local->beacon, 0, sizeof(local->beacon));
412	local->beacon.mhr.fc.type = IEEE802154_FC_TYPE_BEACON;
413	local->beacon.mhr.fc.security_enabled = 0;
414	local->beacon.mhr.fc.frame_pending = 0;
415	local->beacon.mhr.fc.ack_request = 0;
416	local->beacon.mhr.fc.intra_pan = 0;
417	local->beacon.mhr.fc.dest_addr_mode = IEEE802154_NO_ADDRESSING;
418	local->beacon.mhr.fc.version = IEEE802154_2003_STD;
419	local->beacon.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
420	atomic_set(&request->wpan_dev->bsn, -1);
421	local->beacon.mhr.source.mode = IEEE802154_ADDR_LONG;
422	local->beacon.mhr.source.pan_id = cpu_to_le16(request->wpan_dev->pan_id);
423	local->beacon.mhr.source.extended_addr = cpu_to_le64(request->wpan_dev->extended_addr);
424	local->beacon.mac_pl.beacon_order = request->interval;
425	local->beacon.mac_pl.superframe_order = request->interval;
426	local->beacon.mac_pl.final_cap_slot = 0xf;
427	local->beacon.mac_pl.battery_life_ext = 0;
428	/* TODO: Fill this field depending on the coordinator capacity */
429	local->beacon.mac_pl.pan_coordinator = 1;
430	local->beacon.mac_pl.assoc_permit = 1;
431
432	/* Start the beacon work */
433	local->beacon_interval =
434	mac802154_scan_get_channel_time(request->interval,
435	request->wpan_phy->symbol_duration);
436	queue_delayed_work(local->mac_wq, &local->beacon_work, 0);
437
438	return 0;
439	}