1 // SPDX-License-Identifier: ISC
2 /*
3  * Copyright (c) 2012 Broadcom Corporation
4  */
5 #include <linux/slab.h>
6 #include <linux/netdevice.h>
7 #include <linux/etherdevice.h>
8 #include <linux/rtnetlink.h>
9 #include <net/cfg80211.h>
10 
11 #include <brcmu_wifi.h>
12 #include <brcmu_utils.h>
13 #include <defs.h>
14 #include "core.h"
15 #include "debug.h"
16 #include "fwil.h"
17 #include "fwil_types.h"
18 #include "p2p.h"
19 #include "cfg80211.h"
20 #include "feature.h"
21 
22 /* parameters used for p2p escan */
23 #define P2PAPI_SCAN_NPROBES 1
24 #define P2PAPI_SCAN_DWELL_TIME_MS 80
25 #define P2PAPI_SCAN_SOCIAL_DWELL_TIME_MS 40
26 #define P2PAPI_SCAN_HOME_TIME_MS 60
27 #define P2PAPI_SCAN_NPROBS_TIME_MS 30
28 #define P2PAPI_SCAN_AF_SEARCH_DWELL_TIME_MS 100
29 #define WL_SCAN_CONNECT_DWELL_TIME_MS 200
30 #define WL_SCAN_JOIN_PROBE_INTERVAL_MS 20
31 
32 #define BRCMF_P2P_WILDCARD_SSID		"DIRECT-"
33 #define BRCMF_P2P_WILDCARD_SSID_LEN	(sizeof(BRCMF_P2P_WILDCARD_SSID) - 1)
34 
35 #define SOCIAL_CHAN_1		1
36 #define SOCIAL_CHAN_2		6
37 #define SOCIAL_CHAN_3		11
38 #define IS_P2P_SOCIAL_CHANNEL(channel) ((channel == SOCIAL_CHAN_1) || \
39 					 (channel == SOCIAL_CHAN_2) || \
40 					 (channel == SOCIAL_CHAN_3))
41 #define BRCMF_P2P_TEMP_CHAN	SOCIAL_CHAN_3
42 #define SOCIAL_CHAN_CNT		3
43 #define AF_PEER_SEARCH_CNT	2
44 
45 #define BRCMF_SCB_TIMEOUT_VALUE	20
46 
47 #define P2P_VER			9	/* P2P version: 9=WiFi P2P v1.0 */
48 #define P2P_PUB_AF_CATEGORY	0x04
49 #define P2P_PUB_AF_ACTION	0x09
50 #define P2P_AF_CATEGORY		0x7f
51 #define P2P_OUI			"\x50\x6F\x9A"	/* P2P OUI */
52 #define P2P_OUI_LEN		3		/* P2P OUI length */
53 
54 /* Action Frame Constants */
55 #define DOT11_ACTION_HDR_LEN	2	/* action frame category + action */
56 #define DOT11_ACTION_CAT_OFF	0	/* category offset */
57 #define DOT11_ACTION_ACT_OFF	1	/* action offset */
58 
59 #define P2P_AF_DWELL_TIME		200
60 #define P2P_AF_MIN_DWELL_TIME		100
61 #define P2P_AF_MED_DWELL_TIME		400
62 #define P2P_AF_LONG_DWELL_TIME		1000
63 #define P2P_AF_TX_MAX_RETRY		5
64 #define P2P_AF_MAX_WAIT_TIME		msecs_to_jiffies(2000)
65 #define P2P_INVALID_CHANNEL		-1
66 #define P2P_CHANNEL_SYNC_RETRY		5
67 #define P2P_AF_FRM_SCAN_MAX_WAIT	msecs_to_jiffies(450)
68 #define P2P_DEFAULT_SLEEP_TIME_VSDB	200
69 #define P2P_AF_RETRY_DELAY_TIME		40
70 
71 /* WiFi P2P Public Action Frame OUI Subtypes */
72 #define P2P_PAF_GON_REQ		0	/* Group Owner Negotiation Req */
73 #define P2P_PAF_GON_RSP		1	/* Group Owner Negotiation Rsp */
74 #define P2P_PAF_GON_CONF	2	/* Group Owner Negotiation Confirm */
75 #define P2P_PAF_INVITE_REQ	3	/* P2P Invitation Request */
76 #define P2P_PAF_INVITE_RSP	4	/* P2P Invitation Response */
77 #define P2P_PAF_DEVDIS_REQ	5	/* Device Discoverability Request */
78 #define P2P_PAF_DEVDIS_RSP	6	/* Device Discoverability Response */
79 #define P2P_PAF_PROVDIS_REQ	7	/* Provision Discovery Request */
80 #define P2P_PAF_PROVDIS_RSP	8	/* Provision Discovery Response */
81 #define P2P_PAF_SUBTYPE_INVALID	255	/* Invalid Subtype */
82 
83 /* WiFi P2P Action Frame OUI Subtypes */
84 #define P2P_AF_NOTICE_OF_ABSENCE	0	/* Notice of Absence */
85 #define P2P_AF_PRESENCE_REQ		1	/* P2P Presence Request */
86 #define P2P_AF_PRESENCE_RSP		2	/* P2P Presence Response */
87 #define P2P_AF_GO_DISC_REQ		3	/* GO Discoverability Request */
88 
89 /* P2P Service Discovery related */
90 #define P2PSD_ACTION_CATEGORY		0x04	/* Public action frame */
91 #define P2PSD_ACTION_ID_GAS_IREQ	0x0a	/* GAS Initial Request AF */
92 #define P2PSD_ACTION_ID_GAS_IRESP	0x0b	/* GAS Initial Response AF */
93 #define P2PSD_ACTION_ID_GAS_CREQ	0x0c	/* GAS Comeback Request AF */
94 #define P2PSD_ACTION_ID_GAS_CRESP	0x0d	/* GAS Comeback Response AF */
95 
96 #define BRCMF_P2P_DISABLE_TIMEOUT	msecs_to_jiffies(500)
97 
98 /* Mask for retry counter of custom dwell time */
99 #define CUSTOM_RETRY_MASK 0xff000000
100 /**
101  * struct brcmf_p2p_disc_st_le - set discovery state in firmware.
102  *
103  * @state: requested discovery state (see enum brcmf_p2p_disc_state).
104  * @chspec: channel parameter for %WL_P2P_DISC_ST_LISTEN state.
105  * @dwell: dwell time in ms for %WL_P2P_DISC_ST_LISTEN state.
106  */
107 struct brcmf_p2p_disc_st_le {
108 	u8 state;
109 	__le16 chspec;
110 	__le16 dwell;
111 };
112 
113 /**
114  * enum brcmf_p2p_disc_state - P2P discovery state values
115  *
116  * @WL_P2P_DISC_ST_SCAN: P2P discovery with wildcard SSID and P2P IE.
117  * @WL_P2P_DISC_ST_LISTEN: P2P discovery off-channel for specified time.
118  * @WL_P2P_DISC_ST_SEARCH: P2P discovery with P2P wildcard SSID and P2P IE.
119  */
120 enum brcmf_p2p_disc_state {
121 	WL_P2P_DISC_ST_SCAN,
122 	WL_P2P_DISC_ST_LISTEN,
123 	WL_P2P_DISC_ST_SEARCH
124 };
125 
126 /**
127  * struct brcmf_p2p_scan_le - P2P specific scan request.
128  *
129  * @type: type of scan method requested (values: 'E' or 'S').
130  * @reserved: reserved (ignored).
131  * @eparams: parameters used for type 'E'.
132  * @sparams: parameters used for type 'S'.
133  */
134 struct brcmf_p2p_scan_le {
135 	u8 type;
136 	u8 reserved[3];
137 	union {
138 		struct brcmf_escan_params_le eparams;
139 		struct brcmf_scan_params_le sparams;
140 	};
141 };
142 
143 /**
144  * struct brcmf_p2p_pub_act_frame - WiFi P2P Public Action Frame
145  *
146  * @category: P2P_PUB_AF_CATEGORY
147  * @action: P2P_PUB_AF_ACTION
148  * @oui: P2P_OUI
149  * @oui_type: OUI type - P2P_VER
150  * @subtype: OUI subtype - P2P_TYPE_*
151  * @dialog_token: nonzero, identifies req/rsp transaction
152  * @elts: Variable length information elements.
153  */
154 struct brcmf_p2p_pub_act_frame {
155 	u8	category;
156 	u8	action;
157 	u8	oui[3];
158 	u8	oui_type;
159 	u8	subtype;
160 	u8	dialog_token;
161 	u8	elts[];
162 };
163 
164 /**
165  * struct brcmf_p2p_action_frame - WiFi P2P Action Frame
166  *
167  * @category: P2P_AF_CATEGORY
168  * @oui: OUI - P2P_OUI
169  * @type: OUI Type - P2P_VER
170  * @subtype: OUI Subtype - P2P_AF_*
171  * @dialog_token: nonzero, identifies req/resp tranaction
172  * @elts: Variable length information elements.
173  */
174 struct brcmf_p2p_action_frame {
175 	u8	category;
176 	u8	oui[3];
177 	u8	type;
178 	u8	subtype;
179 	u8	dialog_token;
180 	u8	elts[];
181 };
182 
183 /**
184  * struct brcmf_p2psd_gas_pub_act_frame - Wi-Fi GAS Public Action Frame
185  *
186  * @category: 0x04 Public Action Frame
187  * @action: 0x6c Advertisement Protocol
188  * @dialog_token: nonzero, identifies req/rsp transaction
189  * @query_data: Query Data. SD gas ireq SD gas iresp
190  */
191 struct brcmf_p2psd_gas_pub_act_frame {
192 	u8	category;
193 	u8	action;
194 	u8	dialog_token;
195 	u8	query_data[];
196 };
197 
198 /**
199  * struct brcmf_config_af_params - Action Frame Parameters for tx.
200  *
201  * @mpc_onoff: To make sure to send successfully action frame, we have to
202  *             turn off mpc  0: off, 1: on,  (-1): do nothing
203  * @search_channel: 1: search peer's channel to send af
204  * @extra_listen: keep the dwell time to get af response frame.
205  */
206 struct brcmf_config_af_params {
207 	s32 mpc_onoff;
208 	bool search_channel;
209 	bool extra_listen;
210 };
211 
212 /**
213  * brcmf_p2p_is_pub_action() - true if p2p public type frame.
214  *
215  * @frame: action frame data.
216  * @frame_len: length of action frame data.
217  *
218  * Determine if action frame is p2p public action type
219  */
brcmf_p2p_is_pub_action(void * frame,u32 frame_len)220 static bool brcmf_p2p_is_pub_action(void *frame, u32 frame_len)
221 {
222 	struct brcmf_p2p_pub_act_frame *pact_frm;
223 
224 	if (frame == NULL)
225 		return false;
226 
227 	pact_frm = (struct brcmf_p2p_pub_act_frame *)frame;
228 	if (frame_len < sizeof(*pact_frm))
229 		return false;
230 
231 	if (pact_frm->category == P2P_PUB_AF_CATEGORY &&
232 	    pact_frm->action == P2P_PUB_AF_ACTION &&
233 	    pact_frm->oui_type == P2P_VER &&
234 	    memcmp(pact_frm->oui, P2P_OUI, P2P_OUI_LEN) == 0)
235 		return true;
236 
237 	return false;
238 }
239 
240 /**
241  * brcmf_p2p_is_p2p_action() - true if p2p action type frame.
242  *
243  * @frame: action frame data.
244  * @frame_len: length of action frame data.
245  *
246  * Determine if action frame is p2p action type
247  */
brcmf_p2p_is_p2p_action(void * frame,u32 frame_len)248 static bool brcmf_p2p_is_p2p_action(void *frame, u32 frame_len)
249 {
250 	struct brcmf_p2p_action_frame *act_frm;
251 
252 	if (frame == NULL)
253 		return false;
254 
255 	act_frm = (struct brcmf_p2p_action_frame *)frame;
256 	if (frame_len < sizeof(*act_frm))
257 		return false;
258 
259 	if (act_frm->category == P2P_AF_CATEGORY &&
260 	    act_frm->type  == P2P_VER &&
261 	    memcmp(act_frm->oui, P2P_OUI, P2P_OUI_LEN) == 0)
262 		return true;
263 
264 	return false;
265 }
266 
267 /**
268  * brcmf_p2p_is_gas_action() - true if p2p gas action type frame.
269  *
270  * @frame: action frame data.
271  * @frame_len: length of action frame data.
272  *
273  * Determine if action frame is p2p gas action type
274  */
brcmf_p2p_is_gas_action(void * frame,u32 frame_len)275 static bool brcmf_p2p_is_gas_action(void *frame, u32 frame_len)
276 {
277 	struct brcmf_p2psd_gas_pub_act_frame *sd_act_frm;
278 
279 	if (frame == NULL)
280 		return false;
281 
282 	sd_act_frm = (struct brcmf_p2psd_gas_pub_act_frame *)frame;
283 	if (frame_len < sizeof(*sd_act_frm))
284 		return false;
285 
286 	if (sd_act_frm->category != P2PSD_ACTION_CATEGORY)
287 		return false;
288 
289 	if (sd_act_frm->action == P2PSD_ACTION_ID_GAS_IREQ ||
290 	    sd_act_frm->action == P2PSD_ACTION_ID_GAS_IRESP ||
291 	    sd_act_frm->action == P2PSD_ACTION_ID_GAS_CREQ ||
292 	    sd_act_frm->action == P2PSD_ACTION_ID_GAS_CRESP)
293 		return true;
294 
295 	return false;
296 }
297 
298 /**
299  * brcmf_p2p_print_actframe() - debug print routine.
300  *
301  * @tx: Received or to be transmitted
302  * @frame: action frame data.
303  * @frame_len: length of action frame data.
304  *
305  * Print information about the p2p action frame
306  */
307 
308 #ifdef DEBUG
309 
brcmf_p2p_print_actframe(bool tx,void * frame,u32 frame_len)310 static void brcmf_p2p_print_actframe(bool tx, void *frame, u32 frame_len)
311 {
312 	struct brcmf_p2p_pub_act_frame *pact_frm;
313 	struct brcmf_p2p_action_frame *act_frm;
314 	struct brcmf_p2psd_gas_pub_act_frame *sd_act_frm;
315 
316 	if (!frame || frame_len <= 2)
317 		return;
318 
319 	if (brcmf_p2p_is_pub_action(frame, frame_len)) {
320 		pact_frm = (struct brcmf_p2p_pub_act_frame *)frame;
321 		switch (pact_frm->subtype) {
322 		case P2P_PAF_GON_REQ:
323 			brcmf_dbg(TRACE, "%s P2P Group Owner Negotiation Req Frame\n",
324 				  (tx) ? "TX" : "RX");
325 			break;
326 		case P2P_PAF_GON_RSP:
327 			brcmf_dbg(TRACE, "%s P2P Group Owner Negotiation Rsp Frame\n",
328 				  (tx) ? "TX" : "RX");
329 			break;
330 		case P2P_PAF_GON_CONF:
331 			brcmf_dbg(TRACE, "%s P2P Group Owner Negotiation Confirm Frame\n",
332 				  (tx) ? "TX" : "RX");
333 			break;
334 		case P2P_PAF_INVITE_REQ:
335 			brcmf_dbg(TRACE, "%s P2P Invitation Request  Frame\n",
336 				  (tx) ? "TX" : "RX");
337 			break;
338 		case P2P_PAF_INVITE_RSP:
339 			brcmf_dbg(TRACE, "%s P2P Invitation Response Frame\n",
340 				  (tx) ? "TX" : "RX");
341 			break;
342 		case P2P_PAF_DEVDIS_REQ:
343 			brcmf_dbg(TRACE, "%s P2P Device Discoverability Request Frame\n",
344 				  (tx) ? "TX" : "RX");
345 			break;
346 		case P2P_PAF_DEVDIS_RSP:
347 			brcmf_dbg(TRACE, "%s P2P Device Discoverability Response Frame\n",
348 				  (tx) ? "TX" : "RX");
349 			break;
350 		case P2P_PAF_PROVDIS_REQ:
351 			brcmf_dbg(TRACE, "%s P2P Provision Discovery Request Frame\n",
352 				  (tx) ? "TX" : "RX");
353 			break;
354 		case P2P_PAF_PROVDIS_RSP:
355 			brcmf_dbg(TRACE, "%s P2P Provision Discovery Response Frame\n",
356 				  (tx) ? "TX" : "RX");
357 			break;
358 		default:
359 			brcmf_dbg(TRACE, "%s Unknown P2P Public Action Frame\n",
360 				  (tx) ? "TX" : "RX");
361 			break;
362 		}
363 	} else if (brcmf_p2p_is_p2p_action(frame, frame_len)) {
364 		act_frm = (struct brcmf_p2p_action_frame *)frame;
365 		switch (act_frm->subtype) {
366 		case P2P_AF_NOTICE_OF_ABSENCE:
367 			brcmf_dbg(TRACE, "%s P2P Notice of Absence Frame\n",
368 				  (tx) ? "TX" : "RX");
369 			break;
370 		case P2P_AF_PRESENCE_REQ:
371 			brcmf_dbg(TRACE, "%s P2P Presence Request Frame\n",
372 				  (tx) ? "TX" : "RX");
373 			break;
374 		case P2P_AF_PRESENCE_RSP:
375 			brcmf_dbg(TRACE, "%s P2P Presence Response Frame\n",
376 				  (tx) ? "TX" : "RX");
377 			break;
378 		case P2P_AF_GO_DISC_REQ:
379 			brcmf_dbg(TRACE, "%s P2P Discoverability Request Frame\n",
380 				  (tx) ? "TX" : "RX");
381 			break;
382 		default:
383 			brcmf_dbg(TRACE, "%s Unknown P2P Action Frame\n",
384 				  (tx) ? "TX" : "RX");
385 		}
386 
387 	} else if (brcmf_p2p_is_gas_action(frame, frame_len)) {
388 		sd_act_frm = (struct brcmf_p2psd_gas_pub_act_frame *)frame;
389 		switch (sd_act_frm->action) {
390 		case P2PSD_ACTION_ID_GAS_IREQ:
391 			brcmf_dbg(TRACE, "%s P2P GAS Initial Request\n",
392 				  (tx) ? "TX" : "RX");
393 			break;
394 		case P2PSD_ACTION_ID_GAS_IRESP:
395 			brcmf_dbg(TRACE, "%s P2P GAS Initial Response\n",
396 				  (tx) ? "TX" : "RX");
397 			break;
398 		case P2PSD_ACTION_ID_GAS_CREQ:
399 			brcmf_dbg(TRACE, "%s P2P GAS Comeback Request\n",
400 				  (tx) ? "TX" : "RX");
401 			break;
402 		case P2PSD_ACTION_ID_GAS_CRESP:
403 			brcmf_dbg(TRACE, "%s P2P GAS Comeback Response\n",
404 				  (tx) ? "TX" : "RX");
405 			break;
406 		default:
407 			brcmf_dbg(TRACE, "%s Unknown P2P GAS Frame\n",
408 				  (tx) ? "TX" : "RX");
409 			break;
410 		}
411 	}
412 }
413 
414 #else
415 
brcmf_p2p_print_actframe(bool tx,void * frame,u32 frame_len)416 static void brcmf_p2p_print_actframe(bool tx, void *frame, u32 frame_len)
417 {
418 }
419 
420 #endif
421 
422 
423 /**
424  * brcmf_p2p_set_firmware() - prepare firmware for peer-to-peer operation.
425  *
426  * @ifp: ifp to use for iovars (primary).
427  * @p2p_mac: mac address to configure for p2p_da_override
428  */
brcmf_p2p_set_firmware(struct brcmf_if * ifp,u8 * p2p_mac)429 static int brcmf_p2p_set_firmware(struct brcmf_if *ifp, u8 *p2p_mac)
430 {
431 	struct brcmf_pub *drvr = ifp->drvr;
432 	s32 ret = 0;
433 
434 	brcmf_fil_cmd_int_set(ifp, BRCMF_C_DOWN, 1);
435 	brcmf_fil_iovar_int_set(ifp, "apsta", 1);
436 	brcmf_fil_cmd_int_set(ifp, BRCMF_C_UP, 1);
437 
438 	/* In case of COB type, firmware has default mac address
439 	 * After Initializing firmware, we have to set current mac address to
440 	 * firmware for P2P device address. This must be done with discovery
441 	 * disabled.
442 	 */
443 	brcmf_fil_iovar_int_set(ifp, "p2p_disc", 0);
444 
445 	ret = brcmf_fil_iovar_data_set(ifp, "p2p_da_override", p2p_mac,
446 				       ETH_ALEN);
447 	if (ret)
448 		bphy_err(drvr, "failed to update device address ret %d\n", ret);
449 
450 	return ret;
451 }
452 
453 /**
454  * brcmf_p2p_generate_bss_mac() - derive mac addresses for P2P.
455  *
456  * @p2p: P2P specific data.
457  * @dev_addr: optional device address.
458  *
459  * P2P needs mac addresses for P2P device and interface. If no device
460  * address it specified, these are derived from a random ethernet
461  * address.
462  */
brcmf_p2p_generate_bss_mac(struct brcmf_p2p_info * p2p,u8 * dev_addr)463 static void brcmf_p2p_generate_bss_mac(struct brcmf_p2p_info *p2p, u8 *dev_addr)
464 {
465 	struct brcmf_if *pri_ifp = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->ifp;
466 	bool random_addr = false;
467 	bool local_admin = false;
468 
469 	if (!dev_addr || is_zero_ether_addr(dev_addr)) {
470 		/* If the primary interface address is already locally
471 		 * administered, create a new random address.
472 		 */
473 		if (pri_ifp->mac_addr[0] & 0x02) {
474 			random_addr = true;
475 		} else {
476 			dev_addr = pri_ifp->mac_addr;
477 			local_admin = true;
478 		}
479 	}
480 
481 	/* Generate the P2P Device Address obtaining a random ethernet
482 	 * address with the locally administered bit set.
483 	 */
484 	if (random_addr)
485 		eth_random_addr(p2p->dev_addr);
486 	else
487 		memcpy(p2p->dev_addr, dev_addr, ETH_ALEN);
488 
489 	if (local_admin)
490 		p2p->dev_addr[0] |= 0x02;
491 
492 	/* Generate the P2P Interface Address.  If the discovery and connection
493 	 * BSSCFGs need to simultaneously co-exist, then this address must be
494 	 * different from the P2P Device Address, but also locally administered.
495 	 */
496 	memcpy(p2p->conn_int_addr, p2p->dev_addr, ETH_ALEN);
497 	p2p->conn_int_addr[0] |= 0x02;
498 	p2p->conn_int_addr[4] ^= 0x80;
499 
500 	memcpy(p2p->conn2_int_addr, p2p->dev_addr, ETH_ALEN);
501 	p2p->conn2_int_addr[0] |= 0x02;
502 	p2p->conn2_int_addr[4] ^= 0x90;
503 }
504 
505 /**
506  * brcmf_p2p_scan_is_p2p_request() - is cfg80211 scan request a P2P scan.
507  *
508  * @request: the scan request as received from cfg80211.
509  *
510  * returns true if one of the ssids in the request matches the
511  * P2P wildcard ssid; otherwise returns false.
512  */
brcmf_p2p_scan_is_p2p_request(struct cfg80211_scan_request * request)513 static bool brcmf_p2p_scan_is_p2p_request(struct cfg80211_scan_request *request)
514 {
515 	struct cfg80211_ssid *ssids = request->ssids;
516 	int i;
517 
518 	for (i = 0; i < request->n_ssids; i++) {
519 		if (ssids[i].ssid_len != BRCMF_P2P_WILDCARD_SSID_LEN)
520 			continue;
521 
522 		brcmf_dbg(INFO, "comparing ssid \"%s\"", ssids[i].ssid);
523 		if (!memcmp(BRCMF_P2P_WILDCARD_SSID, ssids[i].ssid,
524 			    BRCMF_P2P_WILDCARD_SSID_LEN))
525 			return true;
526 	}
527 	return false;
528 }
529 
530 /**
531  * brcmf_p2p_set_discover_state - set discover state in firmware.
532  *
533  * @ifp: low-level interface object.
534  * @state: discover state to set.
535  * @chanspec: channel parameters (for state @WL_P2P_DISC_ST_LISTEN only).
536  * @listen_ms: duration to listen (for state @WL_P2P_DISC_ST_LISTEN only).
537  */
brcmf_p2p_set_discover_state(struct brcmf_if * ifp,u8 state,u16 chanspec,u16 listen_ms)538 static s32 brcmf_p2p_set_discover_state(struct brcmf_if *ifp, u8 state,
539 					u16 chanspec, u16 listen_ms)
540 {
541 	struct brcmf_p2p_disc_st_le discover_state;
542 	s32 ret = 0;
543 	brcmf_dbg(TRACE, "enter\n");
544 
545 	discover_state.state = state;
546 	discover_state.chspec = cpu_to_le16(chanspec);
547 	discover_state.dwell = cpu_to_le16(listen_ms);
548 	ret = brcmf_fil_bsscfg_data_set(ifp, "p2p_state", &discover_state,
549 					sizeof(discover_state));
550 	return ret;
551 }
552 
553 /**
554  * brcmf_p2p_deinit_discovery() - disable P2P device discovery.
555  *
556  * @p2p: P2P specific data.
557  *
558  * Resets the discovery state and disables it in firmware.
559  */
brcmf_p2p_deinit_discovery(struct brcmf_p2p_info * p2p)560 static s32 brcmf_p2p_deinit_discovery(struct brcmf_p2p_info *p2p)
561 {
562 	struct brcmf_cfg80211_vif *vif;
563 
564 	brcmf_dbg(TRACE, "enter\n");
565 
566 	/* Set the discovery state to SCAN */
567 	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
568 	(void)brcmf_p2p_set_discover_state(vif->ifp, WL_P2P_DISC_ST_SCAN, 0, 0);
569 
570 	/* Disable P2P discovery in the firmware */
571 	vif = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif;
572 	(void)brcmf_fil_iovar_int_set(vif->ifp, "p2p_disc", 0);
573 
574 	return 0;
575 }
576 
577 /**
578  * brcmf_p2p_enable_discovery() - initialize and configure discovery.
579  *
580  * @p2p: P2P specific data.
581  *
582  * Initializes the discovery device and configure the virtual interface.
583  */
brcmf_p2p_enable_discovery(struct brcmf_p2p_info * p2p)584 static int brcmf_p2p_enable_discovery(struct brcmf_p2p_info *p2p)
585 {
586 	struct brcmf_pub *drvr = p2p->cfg->pub;
587 	struct brcmf_cfg80211_vif *vif;
588 	s32 ret = 0;
589 
590 	brcmf_dbg(TRACE, "enter\n");
591 	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
592 	if (!vif) {
593 		bphy_err(drvr, "P2P config device not available\n");
594 		ret = -EPERM;
595 		goto exit;
596 	}
597 
598 	if (test_bit(BRCMF_P2P_STATUS_ENABLED, &p2p->status)) {
599 		brcmf_dbg(INFO, "P2P config device already configured\n");
600 		goto exit;
601 	}
602 
603 	/* Re-initialize P2P Discovery in the firmware */
604 	vif = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif;
605 	ret = brcmf_fil_iovar_int_set(vif->ifp, "p2p_disc", 1);
606 	if (ret < 0) {
607 		bphy_err(drvr, "set p2p_disc error\n");
608 		goto exit;
609 	}
610 	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
611 	ret = brcmf_p2p_set_discover_state(vif->ifp, WL_P2P_DISC_ST_SCAN, 0, 0);
612 	if (ret < 0) {
613 		bphy_err(drvr, "unable to set WL_P2P_DISC_ST_SCAN\n");
614 		goto exit;
615 	}
616 
617 	/*
618 	 * Set wsec to any non-zero value in the discovery bsscfg
619 	 * to ensure our P2P probe responses have the privacy bit
620 	 * set in the 802.11 WPA IE. Some peer devices may not
621 	 * initiate WPS with us if this bit is not set.
622 	 */
623 	ret = brcmf_fil_bsscfg_int_set(vif->ifp, "wsec", AES_ENABLED);
624 	if (ret < 0) {
625 		bphy_err(drvr, "wsec error %d\n", ret);
626 		goto exit;
627 	}
628 
629 	set_bit(BRCMF_P2P_STATUS_ENABLED, &p2p->status);
630 exit:
631 	return ret;
632 }
633 
634 /**
635  * brcmf_p2p_escan() - initiate a P2P scan.
636  *
637  * @p2p: P2P specific data.
638  * @num_chans: number of channels to scan.
639  * @chanspecs: channel parameters for @num_chans channels.
640  * @search_state: P2P discover state to use.
641  * @bss_type: type of P2P bss.
642  */
brcmf_p2p_escan(struct brcmf_p2p_info * p2p,u32 num_chans,u16 chanspecs[],s32 search_state,enum p2p_bss_type bss_type)643 static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
644 			   u16 chanspecs[], s32 search_state,
645 			   enum p2p_bss_type bss_type)
646 {
647 	struct brcmf_pub *drvr = p2p->cfg->pub;
648 	s32 ret = 0;
649 	s32 memsize = offsetof(struct brcmf_p2p_scan_le,
650 			       eparams.params_le.channel_list);
651 	s32 nprobes;
652 	s32 active;
653 	u32 i;
654 	u8 *memblk;
655 	struct brcmf_cfg80211_vif *vif;
656 	struct brcmf_p2p_scan_le *p2p_params;
657 	struct brcmf_scan_params_le *sparams;
658 
659 	memsize += num_chans * sizeof(__le16);
660 	memblk = kzalloc(memsize, GFP_KERNEL);
661 	if (!memblk)
662 		return -ENOMEM;
663 
664 	vif = p2p->bss_idx[bss_type].vif;
665 	if (vif == NULL) {
666 		bphy_err(drvr, "no vif for bss type %d\n", bss_type);
667 		ret = -EINVAL;
668 		goto exit;
669 	}
670 	p2p_params = (struct brcmf_p2p_scan_le *)memblk;
671 	sparams = &p2p_params->eparams.params_le;
672 
673 	switch (search_state) {
674 	case WL_P2P_DISC_ST_SEARCH:
675 		/*
676 		 * If we in SEARCH STATE, we don't need to set SSID explictly
677 		 * because dongle use P2P WILDCARD internally by default, use
678 		 * null ssid, which it is already due to kzalloc.
679 		 */
680 		break;
681 	case WL_P2P_DISC_ST_SCAN:
682 		/*
683 		 * wpa_supplicant has p2p_find command with type social or
684 		 * progressive. For progressive, we need to set the ssid to
685 		 * P2P WILDCARD because we just do broadcast scan unless
686 		 * setting SSID.
687 		 */
688 		sparams->ssid_le.SSID_len =
689 				cpu_to_le32(BRCMF_P2P_WILDCARD_SSID_LEN);
690 		memcpy(sparams->ssid_le.SSID, BRCMF_P2P_WILDCARD_SSID,
691 		       BRCMF_P2P_WILDCARD_SSID_LEN);
692 		break;
693 	default:
694 		bphy_err(drvr, " invalid search state %d\n", search_state);
695 		ret = -EINVAL;
696 		goto exit;
697 	}
698 
699 	brcmf_p2p_set_discover_state(vif->ifp, search_state, 0, 0);
700 
701 	/*
702 	 * set p2p scan parameters.
703 	 */
704 	p2p_params->type = 'E';
705 
706 	/* determine the scan engine parameters */
707 	sparams->bss_type = DOT11_BSSTYPE_ANY;
708 	sparams->scan_type = BRCMF_SCANTYPE_ACTIVE;
709 
710 	eth_broadcast_addr(sparams->bssid);
711 	sparams->home_time = cpu_to_le32(P2PAPI_SCAN_HOME_TIME_MS);
712 
713 	/*
714 	 * SOCIAL_CHAN_CNT + 1 takes care of the Progressive scan
715 	 * supported by the supplicant.
716 	 */
717 	if (num_chans == SOCIAL_CHAN_CNT || num_chans == (SOCIAL_CHAN_CNT + 1))
718 		active = P2PAPI_SCAN_SOCIAL_DWELL_TIME_MS;
719 	else if (num_chans == AF_PEER_SEARCH_CNT)
720 		active = P2PAPI_SCAN_AF_SEARCH_DWELL_TIME_MS;
721 	else if (brcmf_get_vif_state_any(p2p->cfg, BRCMF_VIF_STATUS_CONNECTED))
722 		active = -1;
723 	else
724 		active = P2PAPI_SCAN_DWELL_TIME_MS;
725 
726 	/* Override scan params to find a peer for a connection */
727 	if (num_chans == 1) {
728 		active = WL_SCAN_CONNECT_DWELL_TIME_MS;
729 		/* WAR to sync with presence period of VSDB GO.
730 		 * send probe request more frequently
731 		 */
732 		nprobes = active / WL_SCAN_JOIN_PROBE_INTERVAL_MS;
733 	} else {
734 		nprobes = active / P2PAPI_SCAN_NPROBS_TIME_MS;
735 	}
736 
737 	if (nprobes <= 0)
738 		nprobes = 1;
739 
740 	brcmf_dbg(INFO, "nprobes # %d, active_time %d\n", nprobes, active);
741 	sparams->active_time = cpu_to_le32(active);
742 	sparams->nprobes = cpu_to_le32(nprobes);
743 	sparams->passive_time = cpu_to_le32(-1);
744 	sparams->channel_num = cpu_to_le32(num_chans &
745 					   BRCMF_SCAN_PARAMS_COUNT_MASK);
746 	for (i = 0; i < num_chans; i++)
747 		sparams->channel_list[i] = cpu_to_le16(chanspecs[i]);
748 
749 	/* set the escan specific parameters */
750 	p2p_params->eparams.version = cpu_to_le32(BRCMF_ESCAN_REQ_VERSION);
751 	p2p_params->eparams.action =  cpu_to_le16(WL_ESCAN_ACTION_START);
752 	p2p_params->eparams.sync_id = cpu_to_le16(0x1234);
753 	/* perform p2p scan on primary device */
754 	ret = brcmf_fil_bsscfg_data_set(vif->ifp, "p2p_scan", memblk, memsize);
755 	if (!ret)
756 		set_bit(BRCMF_SCAN_STATUS_BUSY, &p2p->cfg->scan_status);
757 exit:
758 	kfree(memblk);
759 	return ret;
760 }
761 
762 /**
763  * brcmf_p2p_run_escan() - escan callback for peer-to-peer.
764  *
765  * @cfg: driver private data for cfg80211 interface.
766  * @ifp: interface control.
767  * @request: scan request from cfg80211.
768  *
769  * Determines the P2P discovery state based to scan request parameters and
770  * validates the channels in the request.
771  */
brcmf_p2p_run_escan(struct brcmf_cfg80211_info * cfg,struct brcmf_if * ifp,struct cfg80211_scan_request * request)772 static s32 brcmf_p2p_run_escan(struct brcmf_cfg80211_info *cfg,
773 			       struct brcmf_if *ifp,
774 			       struct cfg80211_scan_request *request)
775 {
776 	struct brcmf_p2p_info *p2p = &cfg->p2p;
777 	struct brcmf_pub *drvr = cfg->pub;
778 	s32 err = 0;
779 	s32 search_state = WL_P2P_DISC_ST_SCAN;
780 	struct brcmf_cfg80211_vif *vif;
781 	struct net_device *dev = NULL;
782 	int i, num_nodfs = 0;
783 	u16 *chanspecs;
784 
785 	brcmf_dbg(TRACE, "enter\n");
786 
787 	if (!request) {
788 		err = -EINVAL;
789 		goto exit;
790 	}
791 
792 	if (request->n_channels) {
793 		chanspecs = kcalloc(request->n_channels, sizeof(*chanspecs),
794 				    GFP_KERNEL);
795 		if (!chanspecs) {
796 			err = -ENOMEM;
797 			goto exit;
798 		}
799 		vif = p2p->bss_idx[P2PAPI_BSSCFG_CONNECTION].vif;
800 		if (vif)
801 			dev = vif->wdev.netdev;
802 		if (request->n_channels == 3 &&
803 		    request->channels[0]->hw_value == SOCIAL_CHAN_1 &&
804 		    request->channels[1]->hw_value == SOCIAL_CHAN_2 &&
805 		    request->channels[2]->hw_value == SOCIAL_CHAN_3) {
806 			/* SOCIAL CHANNELS 1, 6, 11 */
807 			search_state = WL_P2P_DISC_ST_SEARCH;
808 			brcmf_dbg(INFO, "P2P SEARCH PHASE START\n");
809 		} else if (dev != NULL &&
810 			   vif->wdev.iftype == NL80211_IFTYPE_P2P_GO) {
811 			/* If you are already a GO, then do SEARCH only */
812 			brcmf_dbg(INFO, "Already a GO. Do SEARCH Only\n");
813 			search_state = WL_P2P_DISC_ST_SEARCH;
814 		} else {
815 			brcmf_dbg(INFO, "P2P SCAN STATE START\n");
816 		}
817 
818 		/*
819 		 * no P2P scanning on passive or DFS channels.
820 		 */
821 		for (i = 0; i < request->n_channels; i++) {
822 			struct ieee80211_channel *chan = request->channels[i];
823 
824 			if (chan->flags & (IEEE80211_CHAN_RADAR |
825 					   IEEE80211_CHAN_NO_IR))
826 				continue;
827 
828 			chanspecs[i] = channel_to_chanspec(&p2p->cfg->d11inf,
829 							   chan);
830 			brcmf_dbg(INFO, "%d: chan=%d, channel spec=%x\n",
831 				  num_nodfs, chan->hw_value, chanspecs[i]);
832 			num_nodfs++;
833 		}
834 		err = brcmf_p2p_escan(p2p, num_nodfs, chanspecs, search_state,
835 				      P2PAPI_BSSCFG_DEVICE);
836 		kfree(chanspecs);
837 	}
838 exit:
839 	if (err)
840 		bphy_err(drvr, "error (%d)\n", err);
841 	return err;
842 }
843 
844 
845 /**
846  * brcmf_p2p_find_listen_channel() - find listen channel in ie string.
847  *
848  * @ie: string of information elements.
849  * @ie_len: length of string.
850  *
851  * Scan ie for p2p ie and look for attribute 6 channel. If available determine
852  * channel and return it.
853  */
brcmf_p2p_find_listen_channel(const u8 * ie,u32 ie_len)854 static s32 brcmf_p2p_find_listen_channel(const u8 *ie, u32 ie_len)
855 {
856 	u8 channel_ie[5];
857 	s32 listen_channel;
858 	s32 err;
859 
860 	err = cfg80211_get_p2p_attr(ie, ie_len,
861 				    IEEE80211_P2P_ATTR_LISTEN_CHANNEL,
862 				    channel_ie, sizeof(channel_ie));
863 	if (err < 0)
864 		return err;
865 
866 	/* listen channel subel length format:     */
867 	/* 3(country) + 1(op. class) + 1(chan num) */
868 	listen_channel = (s32)channel_ie[3 + 1];
869 
870 	if (listen_channel == SOCIAL_CHAN_1 ||
871 	    listen_channel == SOCIAL_CHAN_2 ||
872 	    listen_channel == SOCIAL_CHAN_3) {
873 		brcmf_dbg(INFO, "Found my Listen Channel %d\n", listen_channel);
874 		return listen_channel;
875 	}
876 
877 	return -EPERM;
878 }
879 
880 
881 /**
882  * brcmf_p2p_scan_prep() - prepare scan based on request.
883  *
884  * @wiphy: wiphy device.
885  * @request: scan request from cfg80211.
886  * @vif: vif on which scan request is to be executed.
887  *
888  * Prepare the scan appropriately for type of scan requested. Overrides the
889  * escan .run() callback for peer-to-peer scanning.
890  */
brcmf_p2p_scan_prep(struct wiphy * wiphy,struct cfg80211_scan_request * request,struct brcmf_cfg80211_vif * vif)891 int brcmf_p2p_scan_prep(struct wiphy *wiphy,
892 			struct cfg80211_scan_request *request,
893 			struct brcmf_cfg80211_vif *vif)
894 {
895 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
896 	struct brcmf_p2p_info *p2p = &cfg->p2p;
897 	int err;
898 
899 	if (brcmf_p2p_scan_is_p2p_request(request)) {
900 		/* find my listen channel */
901 		err = brcmf_p2p_find_listen_channel(request->ie,
902 						    request->ie_len);
903 		if (err < 0)
904 			return err;
905 
906 		p2p->afx_hdl.my_listen_chan = err;
907 
908 		clear_bit(BRCMF_P2P_STATUS_GO_NEG_PHASE, &p2p->status);
909 		brcmf_dbg(INFO, "P2P: GO_NEG_PHASE status cleared\n");
910 
911 		err = brcmf_p2p_enable_discovery(p2p);
912 		if (err)
913 			return err;
914 
915 		/* override .run_escan() callback. */
916 		cfg->escan_info.run = brcmf_p2p_run_escan;
917 	}
918 	return 0;
919 }
920 
921 
922 /**
923  * brcmf_p2p_discover_listen() - set firmware to discover listen state.
924  *
925  * @p2p: p2p device.
926  * @channel: channel nr for discover listen.
927  * @duration: time in ms to stay on channel.
928  *
929  */
930 static s32
brcmf_p2p_discover_listen(struct brcmf_p2p_info * p2p,u16 channel,u32 duration)931 brcmf_p2p_discover_listen(struct brcmf_p2p_info *p2p, u16 channel, u32 duration)
932 {
933 	struct brcmf_pub *drvr = p2p->cfg->pub;
934 	struct brcmf_cfg80211_vif *vif;
935 	struct brcmu_chan ch;
936 	s32 err = 0;
937 
938 	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
939 	if (!vif) {
940 		bphy_err(drvr, "Discovery is not set, so we have nothing to do\n");
941 		err = -EPERM;
942 		goto exit;
943 	}
944 
945 	if (test_bit(BRCMF_P2P_STATUS_DISCOVER_LISTEN, &p2p->status)) {
946 		bphy_err(drvr, "Previous LISTEN is not completed yet\n");
947 		/* WAR: prevent cookie mismatch in wpa_supplicant return OK */
948 		goto exit;
949 	}
950 
951 	ch.chnum = channel;
952 	ch.bw = BRCMU_CHAN_BW_20;
953 	p2p->cfg->d11inf.encchspec(&ch);
954 	err = brcmf_p2p_set_discover_state(vif->ifp, WL_P2P_DISC_ST_LISTEN,
955 					   ch.chspec, (u16)duration);
956 	if (!err) {
957 		set_bit(BRCMF_P2P_STATUS_DISCOVER_LISTEN, &p2p->status);
958 		p2p->remain_on_channel_cookie++;
959 	}
960 exit:
961 	return err;
962 }
963 
964 
965 /**
966  * brcmf_p2p_remain_on_channel() - put device on channel and stay there.
967  *
968  * @wiphy: wiphy device.
969  * @wdev: wireless device.
970  * @channel: channel to stay on.
971  * @duration: time in ms to remain on channel.
972  * @cookie: cookie.
973  */
brcmf_p2p_remain_on_channel(struct wiphy * wiphy,struct wireless_dev * wdev,struct ieee80211_channel * channel,unsigned int duration,u64 * cookie)974 int brcmf_p2p_remain_on_channel(struct wiphy *wiphy, struct wireless_dev *wdev,
975 				struct ieee80211_channel *channel,
976 				unsigned int duration, u64 *cookie)
977 {
978 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
979 	struct brcmf_p2p_info *p2p = &cfg->p2p;
980 	s32 err;
981 	u16 channel_nr;
982 
983 	channel_nr = ieee80211_frequency_to_channel(channel->center_freq);
984 	brcmf_dbg(TRACE, "Enter, channel: %d, duration ms (%d)\n", channel_nr,
985 		  duration);
986 
987 	err = brcmf_p2p_enable_discovery(p2p);
988 	if (err)
989 		goto exit;
990 	err = brcmf_p2p_discover_listen(p2p, channel_nr, duration);
991 	if (err)
992 		goto exit;
993 
994 	memcpy(&p2p->remain_on_channel, channel, sizeof(*channel));
995 	*cookie = p2p->remain_on_channel_cookie;
996 	cfg80211_ready_on_channel(wdev, *cookie, channel, duration, GFP_KERNEL);
997 
998 exit:
999 	return err;
1000 }
1001 
1002 
1003 /**
1004  * brcmf_p2p_notify_listen_complete() - p2p listen has completed.
1005  *
1006  * @ifp: interfac control.
1007  * @e: event message. Not used, to make it usable for fweh event dispatcher.
1008  * @data: payload of message. Not used.
1009  *
1010  */
brcmf_p2p_notify_listen_complete(struct brcmf_if * ifp,const struct brcmf_event_msg * e,void * data)1011 int brcmf_p2p_notify_listen_complete(struct brcmf_if *ifp,
1012 				     const struct brcmf_event_msg *e,
1013 				     void *data)
1014 {
1015 	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
1016 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1017 
1018 	brcmf_dbg(TRACE, "Enter\n");
1019 	if (test_and_clear_bit(BRCMF_P2P_STATUS_DISCOVER_LISTEN,
1020 			       &p2p->status)) {
1021 		if (test_and_clear_bit(BRCMF_P2P_STATUS_WAITING_NEXT_AF_LISTEN,
1022 				       &p2p->status)) {
1023 			clear_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME,
1024 				  &p2p->status);
1025 			brcmf_dbg(INFO, "Listen DONE, wake up wait_next_af\n");
1026 			complete(&p2p->wait_next_af);
1027 		}
1028 
1029 		cfg80211_remain_on_channel_expired(&ifp->vif->wdev,
1030 						   p2p->remain_on_channel_cookie,
1031 						   &p2p->remain_on_channel,
1032 						   GFP_KERNEL);
1033 	}
1034 	return 0;
1035 }
1036 
1037 
1038 /**
1039  * brcmf_p2p_cancel_remain_on_channel() - cancel p2p listen state.
1040  *
1041  * @ifp: interfac control.
1042  *
1043  */
brcmf_p2p_cancel_remain_on_channel(struct brcmf_if * ifp)1044 void brcmf_p2p_cancel_remain_on_channel(struct brcmf_if *ifp)
1045 {
1046 	if (!ifp)
1047 		return;
1048 	brcmf_p2p_set_discover_state(ifp, WL_P2P_DISC_ST_SCAN, 0, 0);
1049 	brcmf_p2p_notify_listen_complete(ifp, NULL, NULL);
1050 }
1051 
1052 
1053 /**
1054  * brcmf_p2p_act_frm_search() - search function for action frame.
1055  *
1056  * @p2p: p2p device.
1057  * @channel: channel on which action frame is to be trasmitted.
1058  *
1059  * search function to reach at common channel to send action frame. When
1060  * channel is 0 then all social channels will be used to send af
1061  */
brcmf_p2p_act_frm_search(struct brcmf_p2p_info * p2p,u16 channel)1062 static s32 brcmf_p2p_act_frm_search(struct brcmf_p2p_info *p2p, u16 channel)
1063 {
1064 	struct brcmf_pub *drvr = p2p->cfg->pub;
1065 	s32 err;
1066 	u32 channel_cnt;
1067 	u16 *default_chan_list;
1068 	u32 i;
1069 	struct brcmu_chan ch;
1070 
1071 	brcmf_dbg(TRACE, "Enter\n");
1072 
1073 	if (channel)
1074 		channel_cnt = AF_PEER_SEARCH_CNT;
1075 	else
1076 		channel_cnt = SOCIAL_CHAN_CNT;
1077 	default_chan_list = kcalloc(channel_cnt, sizeof(*default_chan_list),
1078 				    GFP_KERNEL);
1079 	if (default_chan_list == NULL) {
1080 		bphy_err(drvr, "channel list allocation failed\n");
1081 		err = -ENOMEM;
1082 		goto exit;
1083 	}
1084 	ch.bw = BRCMU_CHAN_BW_20;
1085 	if (channel) {
1086 		ch.chnum = channel;
1087 		p2p->cfg->d11inf.encchspec(&ch);
1088 		/* insert same channel to the chan_list */
1089 		for (i = 0; i < channel_cnt; i++)
1090 			default_chan_list[i] = ch.chspec;
1091 	} else {
1092 		ch.chnum = SOCIAL_CHAN_1;
1093 		p2p->cfg->d11inf.encchspec(&ch);
1094 		default_chan_list[0] = ch.chspec;
1095 		ch.chnum = SOCIAL_CHAN_2;
1096 		p2p->cfg->d11inf.encchspec(&ch);
1097 		default_chan_list[1] = ch.chspec;
1098 		ch.chnum = SOCIAL_CHAN_3;
1099 		p2p->cfg->d11inf.encchspec(&ch);
1100 		default_chan_list[2] = ch.chspec;
1101 	}
1102 	err = brcmf_p2p_escan(p2p, channel_cnt, default_chan_list,
1103 			      WL_P2P_DISC_ST_SEARCH, P2PAPI_BSSCFG_DEVICE);
1104 	kfree(default_chan_list);
1105 exit:
1106 	return err;
1107 }
1108 
1109 
1110 /**
1111  * brcmf_p2p_afx_handler() - afx worker thread.
1112  *
1113  * @work:
1114  *
1115  */
brcmf_p2p_afx_handler(struct work_struct * work)1116 static void brcmf_p2p_afx_handler(struct work_struct *work)
1117 {
1118 	struct afx_hdl *afx_hdl = container_of(work, struct afx_hdl, afx_work);
1119 	struct brcmf_p2p_info *p2p = container_of(afx_hdl,
1120 						  struct brcmf_p2p_info,
1121 						  afx_hdl);
1122 	struct brcmf_pub *drvr = p2p->cfg->pub;
1123 	s32 err;
1124 
1125 	if (!afx_hdl->is_active)
1126 		return;
1127 
1128 	if (afx_hdl->is_listen && afx_hdl->my_listen_chan)
1129 		/* 100ms ~ 300ms */
1130 		err = brcmf_p2p_discover_listen(p2p, afx_hdl->my_listen_chan,
1131 						100 * get_random_u32_inclusive(1, 3));
1132 	else
1133 		err = brcmf_p2p_act_frm_search(p2p, afx_hdl->peer_listen_chan);
1134 
1135 	if (err) {
1136 		bphy_err(drvr, "ERROR occurred! value is (%d)\n", err);
1137 		if (test_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL,
1138 			     &p2p->status))
1139 			complete(&afx_hdl->act_frm_scan);
1140 	}
1141 }
1142 
1143 
1144 /**
1145  * brcmf_p2p_af_searching_channel() - search channel.
1146  *
1147  * @p2p: p2p device info struct.
1148  *
1149  */
brcmf_p2p_af_searching_channel(struct brcmf_p2p_info * p2p)1150 static s32 brcmf_p2p_af_searching_channel(struct brcmf_p2p_info *p2p)
1151 {
1152 	struct afx_hdl *afx_hdl = &p2p->afx_hdl;
1153 	struct brcmf_cfg80211_vif *pri_vif;
1154 	s32 retry;
1155 
1156 	brcmf_dbg(TRACE, "Enter\n");
1157 
1158 	pri_vif = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif;
1159 
1160 	reinit_completion(&afx_hdl->act_frm_scan);
1161 	set_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL, &p2p->status);
1162 	afx_hdl->is_active = true;
1163 	afx_hdl->peer_chan = P2P_INVALID_CHANNEL;
1164 
1165 	/* Loop to wait until we find a peer's channel or the
1166 	 * pending action frame tx is cancelled.
1167 	 */
1168 	retry = 0;
1169 	while ((retry < P2P_CHANNEL_SYNC_RETRY) &&
1170 	       (afx_hdl->peer_chan == P2P_INVALID_CHANNEL)) {
1171 		afx_hdl->is_listen = false;
1172 		brcmf_dbg(TRACE, "Scheduling action frame for sending.. (%d)\n",
1173 			  retry);
1174 		/* search peer on peer's listen channel */
1175 		schedule_work(&afx_hdl->afx_work);
1176 		wait_for_completion_timeout(&afx_hdl->act_frm_scan,
1177 					    P2P_AF_FRM_SCAN_MAX_WAIT);
1178 		if ((afx_hdl->peer_chan != P2P_INVALID_CHANNEL) ||
1179 		    (!test_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL,
1180 			       &p2p->status)))
1181 			break;
1182 
1183 		if (afx_hdl->my_listen_chan) {
1184 			brcmf_dbg(TRACE, "Scheduling listen peer, channel=%d\n",
1185 				  afx_hdl->my_listen_chan);
1186 			/* listen on my listen channel */
1187 			afx_hdl->is_listen = true;
1188 			schedule_work(&afx_hdl->afx_work);
1189 			wait_for_completion_timeout(&afx_hdl->act_frm_scan,
1190 						    P2P_AF_FRM_SCAN_MAX_WAIT);
1191 		}
1192 		if ((afx_hdl->peer_chan != P2P_INVALID_CHANNEL) ||
1193 		    (!test_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL,
1194 			       &p2p->status)))
1195 			break;
1196 		retry++;
1197 
1198 		/* if sta is connected or connecting, sleep for a while before
1199 		 * retry af tx or finding a peer
1200 		 */
1201 		if (test_bit(BRCMF_VIF_STATUS_CONNECTED, &pri_vif->sme_state) ||
1202 		    test_bit(BRCMF_VIF_STATUS_CONNECTING, &pri_vif->sme_state))
1203 			msleep(P2P_DEFAULT_SLEEP_TIME_VSDB);
1204 	}
1205 
1206 	brcmf_dbg(TRACE, "Completed search/listen peer_chan=%d\n",
1207 		  afx_hdl->peer_chan);
1208 	afx_hdl->is_active = false;
1209 
1210 	clear_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL, &p2p->status);
1211 
1212 	return afx_hdl->peer_chan;
1213 }
1214 
1215 
1216 /**
1217  * brcmf_p2p_scan_finding_common_channel() - was escan used for finding channel
1218  *
1219  * @cfg: common configuration struct.
1220  * @bi: bss info struct, result from scan.
1221  *
1222  */
brcmf_p2p_scan_finding_common_channel(struct brcmf_cfg80211_info * cfg,struct brcmf_bss_info_le * bi)1223 bool brcmf_p2p_scan_finding_common_channel(struct brcmf_cfg80211_info *cfg,
1224 					   struct brcmf_bss_info_le *bi)
1225 
1226 {
1227 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1228 	struct afx_hdl *afx_hdl = &p2p->afx_hdl;
1229 	struct brcmu_chan ch;
1230 	u8 *ie;
1231 	s32 err;
1232 	u8 p2p_dev_addr[ETH_ALEN];
1233 
1234 	if (!test_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL, &p2p->status))
1235 		return false;
1236 
1237 	if (bi == NULL) {
1238 		brcmf_dbg(TRACE, "ACTION FRAME SCAN Done\n");
1239 		if (afx_hdl->peer_chan == P2P_INVALID_CHANNEL)
1240 			complete(&afx_hdl->act_frm_scan);
1241 		return true;
1242 	}
1243 
1244 	ie = ((u8 *)bi) + le16_to_cpu(bi->ie_offset);
1245 	memset(p2p_dev_addr, 0, sizeof(p2p_dev_addr));
1246 	err = cfg80211_get_p2p_attr(ie, le32_to_cpu(bi->ie_length),
1247 				    IEEE80211_P2P_ATTR_DEVICE_INFO,
1248 				    p2p_dev_addr, sizeof(p2p_dev_addr));
1249 	if (err < 0)
1250 		err = cfg80211_get_p2p_attr(ie, le32_to_cpu(bi->ie_length),
1251 					    IEEE80211_P2P_ATTR_DEVICE_ID,
1252 					    p2p_dev_addr, sizeof(p2p_dev_addr));
1253 	if ((err >= 0) &&
1254 	    (ether_addr_equal(p2p_dev_addr, afx_hdl->tx_dst_addr))) {
1255 		if (!bi->ctl_ch) {
1256 			ch.chspec = le16_to_cpu(bi->chanspec);
1257 			cfg->d11inf.decchspec(&ch);
1258 			bi->ctl_ch = ch.control_ch_num;
1259 		}
1260 		afx_hdl->peer_chan = bi->ctl_ch;
1261 		brcmf_dbg(TRACE, "ACTION FRAME SCAN : Peer %pM found, channel : %d\n",
1262 			  afx_hdl->tx_dst_addr, afx_hdl->peer_chan);
1263 		complete(&afx_hdl->act_frm_scan);
1264 	}
1265 	return true;
1266 }
1267 
1268 /**
1269  * brcmf_p2p_abort_action_frame() - abort action frame.
1270  *
1271  * @cfg: common configuration struct.
1272  *
1273  */
brcmf_p2p_abort_action_frame(struct brcmf_cfg80211_info * cfg)1274 static s32 brcmf_p2p_abort_action_frame(struct brcmf_cfg80211_info *cfg)
1275 {
1276 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1277 	struct brcmf_cfg80211_vif *vif;
1278 	s32 err;
1279 	s32 int_val = 1;
1280 
1281 	brcmf_dbg(TRACE, "Enter\n");
1282 
1283 	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
1284 	err = brcmf_fil_bsscfg_data_set(vif->ifp, "actframe_abort", &int_val,
1285 					sizeof(s32));
1286 	if (err)
1287 		brcmf_err(" aborting action frame has failed (%d)\n", err);
1288 
1289 	return err;
1290 }
1291 
1292 /**
1293  * brcmf_p2p_stop_wait_next_action_frame() - finish scan if af tx complete.
1294  *
1295  * @cfg: common configuration struct.
1296  *
1297  */
1298 static void
brcmf_p2p_stop_wait_next_action_frame(struct brcmf_cfg80211_info * cfg)1299 brcmf_p2p_stop_wait_next_action_frame(struct brcmf_cfg80211_info *cfg)
1300 {
1301 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1302 	struct brcmf_if *ifp = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->ifp;
1303 	s32 err;
1304 
1305 	if (test_bit(BRCMF_P2P_STATUS_SENDING_ACT_FRAME, &p2p->status) &&
1306 	    (test_bit(BRCMF_P2P_STATUS_ACTION_TX_COMPLETED, &p2p->status) ||
1307 	     test_bit(BRCMF_P2P_STATUS_ACTION_TX_NOACK, &p2p->status))) {
1308 		brcmf_dbg(TRACE, "*** Wake UP ** abort actframe iovar\n");
1309 		/* if channel is not zero, "actfame" uses off channel scan.
1310 		 * So abort scan for off channel completion.
1311 		 */
1312 		if (p2p->af_sent_channel) {
1313 			/* abort actframe using actframe_abort or abort scan */
1314 			err = brcmf_p2p_abort_action_frame(cfg);
1315 			if (err)
1316 				brcmf_notify_escan_complete(cfg, ifp, true,
1317 							    true);
1318 		}
1319 	} else if (test_bit(BRCMF_P2P_STATUS_WAITING_NEXT_AF_LISTEN,
1320 			    &p2p->status)) {
1321 		brcmf_dbg(TRACE, "*** Wake UP ** abort listen for next af frame\n");
1322 		/* So abort scan to cancel listen */
1323 		brcmf_notify_escan_complete(cfg, ifp, true, true);
1324 	}
1325 }
1326 
1327 
1328 /**
1329  * brcmf_p2p_gon_req_collision() - Check if go negotiaton collission
1330  *
1331  * @p2p: p2p device info struct.
1332  * @mac: MAC address.
1333  *
1334  * return true if recevied action frame is to be dropped.
1335  */
1336 static bool
brcmf_p2p_gon_req_collision(struct brcmf_p2p_info * p2p,u8 * mac)1337 brcmf_p2p_gon_req_collision(struct brcmf_p2p_info *p2p, u8 *mac)
1338 {
1339 	struct brcmf_cfg80211_info *cfg = p2p->cfg;
1340 	struct brcmf_if *ifp;
1341 
1342 	brcmf_dbg(TRACE, "Enter\n");
1343 
1344 	if (!test_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME, &p2p->status) ||
1345 	    !p2p->gon_req_action)
1346 		return false;
1347 
1348 	brcmf_dbg(TRACE, "GO Negotiation Request COLLISION !!!\n");
1349 	/* if sa(peer) addr is less than da(my) addr, then this device
1350 	 * process peer's gon request and block to send gon req.
1351 	 * if not (sa addr > da addr),
1352 	 * this device will process gon request and drop gon req of peer.
1353 	 */
1354 	ifp = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif->ifp;
1355 	if (memcmp(mac, ifp->mac_addr, ETH_ALEN) < 0) {
1356 		brcmf_dbg(INFO, "Block transmit gon req !!!\n");
1357 		p2p->block_gon_req_tx = true;
1358 		/* if we are finding a common channel for sending af,
1359 		 * do not scan more to block to send current gon req
1360 		 */
1361 		if (test_and_clear_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL,
1362 				       &p2p->status))
1363 			complete(&p2p->afx_hdl.act_frm_scan);
1364 		if (test_and_clear_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME,
1365 				       &p2p->status))
1366 			brcmf_p2p_stop_wait_next_action_frame(cfg);
1367 		return false;
1368 	}
1369 
1370 	/* drop gon request of peer to process gon request by this device. */
1371 	brcmf_dbg(INFO, "Drop received gon req !!!\n");
1372 
1373 	return true;
1374 }
1375 
1376 
1377 /**
1378  * brcmf_p2p_notify_action_frame_rx() - received action frame.
1379  *
1380  * @ifp: interfac control.
1381  * @e: event message. Not used, to make it usable for fweh event dispatcher.
1382  * @data: payload of message, containing action frame data.
1383  *
1384  */
brcmf_p2p_notify_action_frame_rx(struct brcmf_if * ifp,const struct brcmf_event_msg * e,void * data)1385 int brcmf_p2p_notify_action_frame_rx(struct brcmf_if *ifp,
1386 				     const struct brcmf_event_msg *e,
1387 				     void *data)
1388 {
1389 	struct brcmf_pub *drvr = ifp->drvr;
1390 	struct brcmf_cfg80211_info *cfg = drvr->config;
1391 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1392 	struct afx_hdl *afx_hdl = &p2p->afx_hdl;
1393 	struct wireless_dev *wdev;
1394 	u32 mgmt_frame_len = e->datalen - sizeof(struct brcmf_rx_mgmt_data);
1395 	struct brcmf_rx_mgmt_data *rxframe = (struct brcmf_rx_mgmt_data *)data;
1396 	u8 *frame = (u8 *)(rxframe + 1);
1397 	struct brcmf_p2p_pub_act_frame *act_frm;
1398 	struct brcmf_p2psd_gas_pub_act_frame *sd_act_frm;
1399 	struct brcmu_chan ch;
1400 	struct ieee80211_mgmt *mgmt_frame;
1401 	s32 freq;
1402 	u16 mgmt_type;
1403 	u8 action;
1404 
1405 	if (e->datalen < sizeof(*rxframe)) {
1406 		brcmf_dbg(SCAN, "Event data to small. Ignore\n");
1407 		return 0;
1408 	}
1409 
1410 	ch.chspec = be16_to_cpu(rxframe->chanspec);
1411 	cfg->d11inf.decchspec(&ch);
1412 	/* Check if wpa_supplicant has registered for this frame */
1413 	brcmf_dbg(INFO, "ifp->vif->mgmt_rx_reg %04x\n", ifp->vif->mgmt_rx_reg);
1414 	mgmt_type = (IEEE80211_STYPE_ACTION & IEEE80211_FCTL_STYPE) >> 4;
1415 	if ((ifp->vif->mgmt_rx_reg & BIT(mgmt_type)) == 0)
1416 		return 0;
1417 
1418 	brcmf_p2p_print_actframe(false, frame, mgmt_frame_len);
1419 
1420 	action = P2P_PAF_SUBTYPE_INVALID;
1421 	if (brcmf_p2p_is_pub_action(frame, mgmt_frame_len)) {
1422 		act_frm = (struct brcmf_p2p_pub_act_frame *)frame;
1423 		action = act_frm->subtype;
1424 		if ((action == P2P_PAF_GON_REQ) &&
1425 		    (brcmf_p2p_gon_req_collision(p2p, (u8 *)e->addr))) {
1426 			if (test_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL,
1427 				     &p2p->status) &&
1428 			    (ether_addr_equal(afx_hdl->tx_dst_addr, e->addr))) {
1429 				afx_hdl->peer_chan = ch.control_ch_num;
1430 				brcmf_dbg(INFO, "GON request: Peer found, channel=%d\n",
1431 					  afx_hdl->peer_chan);
1432 				complete(&afx_hdl->act_frm_scan);
1433 			}
1434 			return 0;
1435 		}
1436 		/* After complete GO Negotiation, roll back to mpc mode */
1437 		if ((action == P2P_PAF_GON_CONF) ||
1438 		    (action == P2P_PAF_PROVDIS_RSP))
1439 			brcmf_set_mpc(ifp, 1);
1440 		if (action == P2P_PAF_GON_CONF) {
1441 			brcmf_dbg(TRACE, "P2P: GO_NEG_PHASE status cleared\n");
1442 			clear_bit(BRCMF_P2P_STATUS_GO_NEG_PHASE, &p2p->status);
1443 		}
1444 	} else if (brcmf_p2p_is_gas_action(frame, mgmt_frame_len)) {
1445 		sd_act_frm = (struct brcmf_p2psd_gas_pub_act_frame *)frame;
1446 		action = sd_act_frm->action;
1447 	}
1448 
1449 	if (test_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME, &p2p->status) &&
1450 	    (p2p->next_af_subtype == action)) {
1451 		brcmf_dbg(TRACE, "We got a right next frame! (%d)\n", action);
1452 		clear_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME,
1453 			  &p2p->status);
1454 		/* Stop waiting for next AF. */
1455 		brcmf_p2p_stop_wait_next_action_frame(cfg);
1456 	}
1457 
1458 	mgmt_frame = kzalloc(offsetof(struct ieee80211_mgmt, u) +
1459 			     mgmt_frame_len, GFP_KERNEL);
1460 	if (!mgmt_frame) {
1461 		bphy_err(drvr, "No memory available for action frame\n");
1462 		return -ENOMEM;
1463 	}
1464 	memcpy(mgmt_frame->da, ifp->mac_addr, ETH_ALEN);
1465 	brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BSSID, mgmt_frame->bssid,
1466 			       ETH_ALEN);
1467 	memcpy(mgmt_frame->sa, e->addr, ETH_ALEN);
1468 	mgmt_frame->frame_control = cpu_to_le16(IEEE80211_STYPE_ACTION);
1469 	memcpy(mgmt_frame->u.body, frame, mgmt_frame_len);
1470 	mgmt_frame_len += offsetof(struct ieee80211_mgmt, u.body);
1471 
1472 	freq = ieee80211_channel_to_frequency(ch.control_ch_num,
1473 					      ch.band == BRCMU_CHAN_BAND_2G ?
1474 					      NL80211_BAND_2GHZ :
1475 					      NL80211_BAND_5GHZ);
1476 
1477 	wdev = &ifp->vif->wdev;
1478 	cfg80211_rx_mgmt(wdev, freq, 0, (u8 *)mgmt_frame, mgmt_frame_len, 0);
1479 
1480 	kfree(mgmt_frame);
1481 	return 0;
1482 }
1483 
1484 
1485 /**
1486  * brcmf_p2p_notify_action_tx_complete() - transmit action frame complete
1487  *
1488  * @ifp: interfac control.
1489  * @e: event message. Not used, to make it usable for fweh event dispatcher.
1490  * @data: not used.
1491  *
1492  */
brcmf_p2p_notify_action_tx_complete(struct brcmf_if * ifp,const struct brcmf_event_msg * e,void * data)1493 int brcmf_p2p_notify_action_tx_complete(struct brcmf_if *ifp,
1494 					const struct brcmf_event_msg *e,
1495 					void *data)
1496 {
1497 	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
1498 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1499 
1500 	brcmf_dbg(INFO, "Enter: event %s, status=%d\n",
1501 		  e->event_code == BRCMF_E_ACTION_FRAME_OFF_CHAN_COMPLETE ?
1502 		  "ACTION_FRAME_OFF_CHAN_COMPLETE" : "ACTION_FRAME_COMPLETE",
1503 		  e->status);
1504 
1505 	if (!test_bit(BRCMF_P2P_STATUS_SENDING_ACT_FRAME, &p2p->status))
1506 		return 0;
1507 
1508 	if (e->event_code == BRCMF_E_ACTION_FRAME_COMPLETE) {
1509 		if (e->status == BRCMF_E_STATUS_SUCCESS) {
1510 			set_bit(BRCMF_P2P_STATUS_ACTION_TX_COMPLETED,
1511 				&p2p->status);
1512 			if (!p2p->wait_for_offchan_complete)
1513 				complete(&p2p->send_af_done);
1514 		} else {
1515 			set_bit(BRCMF_P2P_STATUS_ACTION_TX_NOACK, &p2p->status);
1516 			/* If there is no ack, we don't need to wait for
1517 			 * WLC_E_ACTION_FRAME_OFFCHAN_COMPLETE event
1518 			 */
1519 			brcmf_p2p_stop_wait_next_action_frame(cfg);
1520 		}
1521 
1522 	} else {
1523 		complete(&p2p->send_af_done);
1524 	}
1525 	return 0;
1526 }
1527 
1528 
1529 /**
1530  * brcmf_p2p_tx_action_frame() - send action frame over fil.
1531  *
1532  * @p2p: p2p info struct for vif.
1533  * @af_params: action frame data/info.
1534  *
1535  * Send an action frame immediately without doing channel synchronization.
1536  *
1537  * This function waits for a completion event before returning.
1538  * The WLC_E_ACTION_FRAME_COMPLETE event will be received when the action
1539  * frame is transmitted.
1540  */
brcmf_p2p_tx_action_frame(struct brcmf_p2p_info * p2p,struct brcmf_fil_af_params_le * af_params)1541 static s32 brcmf_p2p_tx_action_frame(struct brcmf_p2p_info *p2p,
1542 				     struct brcmf_fil_af_params_le *af_params)
1543 {
1544 	struct brcmf_pub *drvr = p2p->cfg->pub;
1545 	struct brcmf_cfg80211_vif *vif;
1546 	struct brcmf_p2p_action_frame *p2p_af;
1547 	s32 err = 0;
1548 
1549 	brcmf_dbg(TRACE, "Enter\n");
1550 
1551 	reinit_completion(&p2p->send_af_done);
1552 	clear_bit(BRCMF_P2P_STATUS_ACTION_TX_COMPLETED, &p2p->status);
1553 	clear_bit(BRCMF_P2P_STATUS_ACTION_TX_NOACK, &p2p->status);
1554 
1555 	/* check if it is a p2p_presence response */
1556 	p2p_af = (struct brcmf_p2p_action_frame *)af_params->action_frame.data;
1557 	if (p2p_af->subtype == P2P_AF_PRESENCE_RSP)
1558 		vif = p2p->bss_idx[P2PAPI_BSSCFG_CONNECTION].vif;
1559 	else
1560 		vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
1561 
1562 	err = brcmf_fil_bsscfg_data_set(vif->ifp, "actframe", af_params,
1563 					sizeof(*af_params));
1564 	if (err) {
1565 		bphy_err(drvr, " sending action frame has failed\n");
1566 		goto exit;
1567 	}
1568 
1569 	p2p->af_sent_channel = le32_to_cpu(af_params->channel);
1570 	p2p->af_tx_sent_jiffies = jiffies;
1571 
1572 	if (test_bit(BRCMF_P2P_STATUS_DISCOVER_LISTEN, &p2p->status) &&
1573 	    p2p->af_sent_channel ==
1574 	    ieee80211_frequency_to_channel(p2p->remain_on_channel.center_freq))
1575 		p2p->wait_for_offchan_complete = false;
1576 	else
1577 		p2p->wait_for_offchan_complete = true;
1578 
1579 	brcmf_dbg(TRACE, "Waiting for %s tx completion event\n",
1580 		  (p2p->wait_for_offchan_complete) ?
1581 		   "off-channel" : "on-channel");
1582 
1583 	wait_for_completion_timeout(&p2p->send_af_done, P2P_AF_MAX_WAIT_TIME);
1584 
1585 	if (test_bit(BRCMF_P2P_STATUS_ACTION_TX_COMPLETED, &p2p->status)) {
1586 		brcmf_dbg(TRACE, "TX action frame operation is success\n");
1587 	} else {
1588 		err = -EIO;
1589 		brcmf_dbg(TRACE, "TX action frame operation has failed\n");
1590 	}
1591 	/* clear status bit for action tx */
1592 	clear_bit(BRCMF_P2P_STATUS_ACTION_TX_COMPLETED, &p2p->status);
1593 	clear_bit(BRCMF_P2P_STATUS_ACTION_TX_NOACK, &p2p->status);
1594 
1595 exit:
1596 	return err;
1597 }
1598 
1599 
1600 /**
1601  * brcmf_p2p_pub_af_tx() - public action frame tx routine.
1602  *
1603  * @cfg: driver private data for cfg80211 interface.
1604  * @af_params: action frame data/info.
1605  * @config_af_params: configuration data for action frame.
1606  *
1607  * routine which transmits ation frame public type.
1608  */
brcmf_p2p_pub_af_tx(struct brcmf_cfg80211_info * cfg,struct brcmf_fil_af_params_le * af_params,struct brcmf_config_af_params * config_af_params)1609 static s32 brcmf_p2p_pub_af_tx(struct brcmf_cfg80211_info *cfg,
1610 			       struct brcmf_fil_af_params_le *af_params,
1611 			       struct brcmf_config_af_params *config_af_params)
1612 {
1613 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1614 	struct brcmf_pub *drvr = cfg->pub;
1615 	struct brcmf_fil_action_frame_le *action_frame;
1616 	struct brcmf_p2p_pub_act_frame *act_frm;
1617 	s32 err = 0;
1618 	u16 ie_len;
1619 
1620 	action_frame = &af_params->action_frame;
1621 	act_frm = (struct brcmf_p2p_pub_act_frame *)(action_frame->data);
1622 
1623 	config_af_params->extra_listen = true;
1624 
1625 	switch (act_frm->subtype) {
1626 	case P2P_PAF_GON_REQ:
1627 		brcmf_dbg(TRACE, "P2P: GO_NEG_PHASE status set\n");
1628 		set_bit(BRCMF_P2P_STATUS_GO_NEG_PHASE, &p2p->status);
1629 		config_af_params->mpc_onoff = 0;
1630 		config_af_params->search_channel = true;
1631 		p2p->next_af_subtype = act_frm->subtype + 1;
1632 		p2p->gon_req_action = true;
1633 		/* increase dwell time to wait for RESP frame */
1634 		af_params->dwell_time = cpu_to_le32(P2P_AF_MED_DWELL_TIME);
1635 		break;
1636 	case P2P_PAF_GON_RSP:
1637 		p2p->next_af_subtype = act_frm->subtype + 1;
1638 		/* increase dwell time to wait for CONF frame */
1639 		af_params->dwell_time = cpu_to_le32(P2P_AF_MED_DWELL_TIME);
1640 		break;
1641 	case P2P_PAF_GON_CONF:
1642 		/* If we reached till GO Neg confirmation reset the filter */
1643 		brcmf_dbg(TRACE, "P2P: GO_NEG_PHASE status cleared\n");
1644 		clear_bit(BRCMF_P2P_STATUS_GO_NEG_PHASE, &p2p->status);
1645 		/* turn on mpc again if go nego is done */
1646 		config_af_params->mpc_onoff = 1;
1647 		/* minimize dwell time */
1648 		af_params->dwell_time = cpu_to_le32(P2P_AF_MIN_DWELL_TIME);
1649 		config_af_params->extra_listen = false;
1650 		break;
1651 	case P2P_PAF_INVITE_REQ:
1652 		config_af_params->search_channel = true;
1653 		p2p->next_af_subtype = act_frm->subtype + 1;
1654 		/* increase dwell time */
1655 		af_params->dwell_time = cpu_to_le32(P2P_AF_MED_DWELL_TIME);
1656 		break;
1657 	case P2P_PAF_INVITE_RSP:
1658 		/* minimize dwell time */
1659 		af_params->dwell_time = cpu_to_le32(P2P_AF_MIN_DWELL_TIME);
1660 		config_af_params->extra_listen = false;
1661 		break;
1662 	case P2P_PAF_DEVDIS_REQ:
1663 		config_af_params->search_channel = true;
1664 		p2p->next_af_subtype = act_frm->subtype + 1;
1665 		/* maximize dwell time to wait for RESP frame */
1666 		af_params->dwell_time = cpu_to_le32(P2P_AF_LONG_DWELL_TIME);
1667 		break;
1668 	case P2P_PAF_DEVDIS_RSP:
1669 		/* minimize dwell time */
1670 		af_params->dwell_time = cpu_to_le32(P2P_AF_MIN_DWELL_TIME);
1671 		config_af_params->extra_listen = false;
1672 		break;
1673 	case P2P_PAF_PROVDIS_REQ:
1674 		ie_len = le16_to_cpu(action_frame->len) -
1675 			 offsetof(struct brcmf_p2p_pub_act_frame, elts);
1676 		if (cfg80211_get_p2p_attr(&act_frm->elts[0], ie_len,
1677 					  IEEE80211_P2P_ATTR_GROUP_ID,
1678 					  NULL, 0) < 0)
1679 			config_af_params->search_channel = true;
1680 		config_af_params->mpc_onoff = 0;
1681 		p2p->next_af_subtype = act_frm->subtype + 1;
1682 		/* increase dwell time to wait for RESP frame */
1683 		af_params->dwell_time = cpu_to_le32(P2P_AF_MED_DWELL_TIME);
1684 		break;
1685 	case P2P_PAF_PROVDIS_RSP:
1686 		/* wpa_supplicant send go nego req right after prov disc */
1687 		p2p->next_af_subtype = P2P_PAF_GON_REQ;
1688 		/* increase dwell time to MED level */
1689 		af_params->dwell_time = cpu_to_le32(P2P_AF_MED_DWELL_TIME);
1690 		config_af_params->extra_listen = false;
1691 		break;
1692 	default:
1693 		bphy_err(drvr, "Unknown p2p pub act frame subtype: %d\n",
1694 			 act_frm->subtype);
1695 		err = -EINVAL;
1696 	}
1697 	return err;
1698 }
1699 
brcmf_p2p_check_dwell_overflow(u32 requested_dwell,unsigned long dwell_jiffies)1700 static bool brcmf_p2p_check_dwell_overflow(u32 requested_dwell,
1701 					   unsigned long dwell_jiffies)
1702 {
1703 	if ((requested_dwell & CUSTOM_RETRY_MASK) &&
1704 	    (jiffies_to_msecs(jiffies - dwell_jiffies) >
1705 	    (requested_dwell & ~CUSTOM_RETRY_MASK))) {
1706 		brcmf_err("Action frame TX retry time over dwell time!\n");
1707 		return true;
1708 	}
1709 	return false;
1710 }
1711 /**
1712  * brcmf_p2p_send_action_frame() - send action frame .
1713  *
1714  * @cfg: driver private data for cfg80211 interface.
1715  * @ndev: net device to transmit on.
1716  * @af_params: configuration data for action frame.
1717  */
brcmf_p2p_send_action_frame(struct brcmf_cfg80211_info * cfg,struct net_device * ndev,struct brcmf_fil_af_params_le * af_params)1718 bool brcmf_p2p_send_action_frame(struct brcmf_cfg80211_info *cfg,
1719 				 struct net_device *ndev,
1720 				 struct brcmf_fil_af_params_le *af_params)
1721 {
1722 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1723 	struct brcmf_if *ifp = netdev_priv(ndev);
1724 	struct brcmf_fil_action_frame_le *action_frame;
1725 	struct brcmf_config_af_params config_af_params;
1726 	struct afx_hdl *afx_hdl = &p2p->afx_hdl;
1727 	struct brcmf_pub *drvr = cfg->pub;
1728 	u16 action_frame_len;
1729 	bool ack = false;
1730 	u8 category;
1731 	u8 action;
1732 	s32 tx_retry;
1733 	s32 extra_listen_time;
1734 	uint delta_ms;
1735 	unsigned long dwell_jiffies = 0;
1736 	bool dwell_overflow = false;
1737 
1738 	u32 requested_dwell = le32_to_cpu(af_params->dwell_time);
1739 	action_frame = &af_params->action_frame;
1740 	action_frame_len = le16_to_cpu(action_frame->len);
1741 
1742 	brcmf_p2p_print_actframe(true, action_frame->data, action_frame_len);
1743 
1744 	/* Add the default dwell time. Dwell time to stay off-channel */
1745 	/* to wait for a response action frame after transmitting an  */
1746 	/* GO Negotiation action frame                                */
1747 	af_params->dwell_time = cpu_to_le32(P2P_AF_DWELL_TIME);
1748 
1749 	category = action_frame->data[DOT11_ACTION_CAT_OFF];
1750 	action = action_frame->data[DOT11_ACTION_ACT_OFF];
1751 
1752 	/* initialize variables */
1753 	p2p->next_af_subtype = P2P_PAF_SUBTYPE_INVALID;
1754 	p2p->gon_req_action = false;
1755 
1756 	/* config parameters */
1757 	config_af_params.mpc_onoff = -1;
1758 	config_af_params.search_channel = false;
1759 	config_af_params.extra_listen = false;
1760 
1761 	if (brcmf_p2p_is_pub_action(action_frame->data, action_frame_len)) {
1762 		/* p2p public action frame process */
1763 		if (brcmf_p2p_pub_af_tx(cfg, af_params, &config_af_params)) {
1764 			/* Just send unknown subtype frame with */
1765 			/* default parameters.                  */
1766 			bphy_err(drvr, "P2P Public action frame, unknown subtype.\n");
1767 		}
1768 	} else if (brcmf_p2p_is_gas_action(action_frame->data,
1769 					   action_frame_len)) {
1770 		/* service discovery process */
1771 		if (action == P2PSD_ACTION_ID_GAS_IREQ ||
1772 		    action == P2PSD_ACTION_ID_GAS_CREQ) {
1773 			/* configure service discovery query frame */
1774 			config_af_params.search_channel = true;
1775 
1776 			/* save next af suptype to cancel */
1777 			/* remaining dwell time           */
1778 			p2p->next_af_subtype = action + 1;
1779 
1780 			af_params->dwell_time =
1781 				cpu_to_le32(P2P_AF_MED_DWELL_TIME);
1782 		} else if (action == P2PSD_ACTION_ID_GAS_IRESP ||
1783 			   action == P2PSD_ACTION_ID_GAS_CRESP) {
1784 			/* configure service discovery response frame */
1785 			af_params->dwell_time =
1786 				cpu_to_le32(P2P_AF_MIN_DWELL_TIME);
1787 		} else {
1788 			bphy_err(drvr, "Unknown action type: %d\n", action);
1789 			goto exit;
1790 		}
1791 	} else if (brcmf_p2p_is_p2p_action(action_frame->data,
1792 					   action_frame_len)) {
1793 		/* do not configure anything. it will be */
1794 		/* sent with a default configuration     */
1795 	} else {
1796 		bphy_err(drvr, "Unknown Frame: category 0x%x, action 0x%x\n",
1797 			 category, action);
1798 		return false;
1799 	}
1800 
1801 	/* if connecting on primary iface, sleep for a while before sending
1802 	 * af tx for VSDB
1803 	 */
1804 	if (test_bit(BRCMF_VIF_STATUS_CONNECTING,
1805 		     &p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->sme_state))
1806 		msleep(50);
1807 
1808 	/* if scan is ongoing, abort current scan. */
1809 	if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status))
1810 		brcmf_abort_scanning(cfg);
1811 
1812 	memcpy(afx_hdl->tx_dst_addr, action_frame->da, ETH_ALEN);
1813 
1814 	/* To make sure to send successfully action frame, turn off mpc */
1815 	if (config_af_params.mpc_onoff == 0)
1816 		brcmf_set_mpc(ifp, 0);
1817 
1818 	/* set status and destination address before sending af */
1819 	if (p2p->next_af_subtype != P2P_PAF_SUBTYPE_INVALID) {
1820 		/* set status to cancel the remained dwell time in rx process */
1821 		set_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME, &p2p->status);
1822 	}
1823 
1824 	p2p->af_sent_channel = 0;
1825 	set_bit(BRCMF_P2P_STATUS_SENDING_ACT_FRAME, &p2p->status);
1826 	/* validate channel and p2p ies */
1827 	if (config_af_params.search_channel &&
1828 	    IS_P2P_SOCIAL_CHANNEL(le32_to_cpu(af_params->channel)) &&
1829 	    p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif->saved_ie.probe_req_ie_len) {
1830 		afx_hdl = &p2p->afx_hdl;
1831 		afx_hdl->peer_listen_chan = le32_to_cpu(af_params->channel);
1832 
1833 		if (brcmf_p2p_af_searching_channel(p2p) ==
1834 							P2P_INVALID_CHANNEL) {
1835 			bphy_err(drvr, "Couldn't find peer's channel.\n");
1836 			goto exit;
1837 		}
1838 
1839 		/* Abort scan even for VSDB scenarios. Scan gets aborted in
1840 		 * firmware but after the check of piggyback algorithm. To take
1841 		 * care of current piggback algo, lets abort the scan here
1842 		 * itself.
1843 		 */
1844 		brcmf_notify_escan_complete(cfg, ifp, true, true);
1845 
1846 		/* update channel */
1847 		af_params->channel = cpu_to_le32(afx_hdl->peer_chan);
1848 	}
1849 	dwell_jiffies = jiffies;
1850 	dwell_overflow = brcmf_p2p_check_dwell_overflow(requested_dwell,
1851 							dwell_jiffies);
1852 
1853 	tx_retry = 0;
1854 	while (!p2p->block_gon_req_tx &&
1855 	       (!ack) && (tx_retry < P2P_AF_TX_MAX_RETRY) &&
1856 		!dwell_overflow) {
1857 		if (af_params->channel)
1858 			msleep(P2P_AF_RETRY_DELAY_TIME);
1859 
1860 		ack = !brcmf_p2p_tx_action_frame(p2p, af_params);
1861 		tx_retry++;
1862 		dwell_overflow = brcmf_p2p_check_dwell_overflow(requested_dwell,
1863 								dwell_jiffies);
1864 	}
1865 	if (!ack) {
1866 		bphy_err(drvr, "Failed to send Action Frame(retry %d)\n",
1867 			 tx_retry);
1868 		clear_bit(BRCMF_P2P_STATUS_GO_NEG_PHASE, &p2p->status);
1869 	}
1870 
1871 exit:
1872 	clear_bit(BRCMF_P2P_STATUS_SENDING_ACT_FRAME, &p2p->status);
1873 
1874 	/* WAR: sometimes dongle does not keep the dwell time of 'actframe'.
1875 	 * if we coundn't get the next action response frame and dongle does
1876 	 * not keep the dwell time, go to listen state again to get next action
1877 	 * response frame.
1878 	 */
1879 	if (ack && config_af_params.extra_listen && !p2p->block_gon_req_tx &&
1880 	    test_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME, &p2p->status) &&
1881 	    p2p->af_sent_channel == afx_hdl->my_listen_chan) {
1882 		delta_ms = jiffies_to_msecs(jiffies - p2p->af_tx_sent_jiffies);
1883 		if (le32_to_cpu(af_params->dwell_time) > delta_ms)
1884 			extra_listen_time = le32_to_cpu(af_params->dwell_time) -
1885 					    delta_ms;
1886 		else
1887 			extra_listen_time = 0;
1888 		if (extra_listen_time > 50) {
1889 			set_bit(BRCMF_P2P_STATUS_WAITING_NEXT_AF_LISTEN,
1890 				&p2p->status);
1891 			brcmf_dbg(INFO, "Wait more time! actual af time:%d, calculated extra listen:%d\n",
1892 				  le32_to_cpu(af_params->dwell_time),
1893 				  extra_listen_time);
1894 			extra_listen_time += 100;
1895 			if (!brcmf_p2p_discover_listen(p2p,
1896 						       p2p->af_sent_channel,
1897 						       extra_listen_time)) {
1898 				unsigned long duration;
1899 
1900 				extra_listen_time += 100;
1901 				duration = msecs_to_jiffies(extra_listen_time);
1902 				wait_for_completion_timeout(&p2p->wait_next_af,
1903 							    duration);
1904 			}
1905 			clear_bit(BRCMF_P2P_STATUS_WAITING_NEXT_AF_LISTEN,
1906 				  &p2p->status);
1907 		}
1908 	}
1909 
1910 	if (p2p->block_gon_req_tx) {
1911 		/* if ack is true, supplicant will wait more time(100ms).
1912 		 * so we will return it as a success to get more time .
1913 		 */
1914 		p2p->block_gon_req_tx = false;
1915 		ack = true;
1916 	}
1917 
1918 	clear_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME, &p2p->status);
1919 	/* if all done, turn mpc on again */
1920 	if (config_af_params.mpc_onoff == 1)
1921 		brcmf_set_mpc(ifp, 1);
1922 
1923 	return ack;
1924 }
1925 
1926 /**
1927  * brcmf_p2p_notify_rx_mgmt_p2p_probereq() - Event handler for p2p probe req.
1928  *
1929  * @ifp: interface pointer for which event was received.
1930  * @e: even message.
1931  * @data: payload of event message (probe request).
1932  */
brcmf_p2p_notify_rx_mgmt_p2p_probereq(struct brcmf_if * ifp,const struct brcmf_event_msg * e,void * data)1933 s32 brcmf_p2p_notify_rx_mgmt_p2p_probereq(struct brcmf_if *ifp,
1934 					  const struct brcmf_event_msg *e,
1935 					  void *data)
1936 {
1937 	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
1938 	struct brcmf_p2p_info *p2p = &cfg->p2p;
1939 	struct afx_hdl *afx_hdl = &p2p->afx_hdl;
1940 	struct brcmf_cfg80211_vif *vif = ifp->vif;
1941 	struct brcmf_rx_mgmt_data *rxframe = (struct brcmf_rx_mgmt_data *)data;
1942 	struct brcmu_chan ch;
1943 	u8 *mgmt_frame;
1944 	u32 mgmt_frame_len;
1945 	s32 freq;
1946 	u16 mgmt_type;
1947 
1948 	brcmf_dbg(INFO, "Enter: event %d reason %d\n", e->event_code,
1949 		  e->reason);
1950 
1951 	if (e->datalen < sizeof(*rxframe)) {
1952 		brcmf_dbg(SCAN, "Event data to small. Ignore\n");
1953 		return 0;
1954 	}
1955 
1956 	ch.chspec = be16_to_cpu(rxframe->chanspec);
1957 	cfg->d11inf.decchspec(&ch);
1958 
1959 	if (test_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL, &p2p->status) &&
1960 	    (ether_addr_equal(afx_hdl->tx_dst_addr, e->addr))) {
1961 		afx_hdl->peer_chan = ch.control_ch_num;
1962 		brcmf_dbg(INFO, "PROBE REQUEST: Peer found, channel=%d\n",
1963 			  afx_hdl->peer_chan);
1964 		complete(&afx_hdl->act_frm_scan);
1965 	}
1966 
1967 	/* Firmware sends us two proberesponses for each idx one. At the */
1968 	/* moment anything but bsscfgidx 0 is passed up to supplicant    */
1969 	if (e->bsscfgidx == 0)
1970 		return 0;
1971 
1972 	/* Filter any P2P probe reqs arriving during the GO-NEG Phase */
1973 	if (test_bit(BRCMF_P2P_STATUS_GO_NEG_PHASE, &p2p->status)) {
1974 		brcmf_dbg(INFO, "Filtering P2P probe_req in GO-NEG phase\n");
1975 		return 0;
1976 	}
1977 
1978 	/* Check if wpa_supplicant has registered for this frame */
1979 	brcmf_dbg(INFO, "vif->mgmt_rx_reg %04x\n", vif->mgmt_rx_reg);
1980 	mgmt_type = (IEEE80211_STYPE_PROBE_REQ & IEEE80211_FCTL_STYPE) >> 4;
1981 	if ((vif->mgmt_rx_reg & BIT(mgmt_type)) == 0)
1982 		return 0;
1983 
1984 	mgmt_frame = (u8 *)(rxframe + 1);
1985 	mgmt_frame_len = e->datalen - sizeof(*rxframe);
1986 	freq = ieee80211_channel_to_frequency(ch.control_ch_num,
1987 					      ch.band == BRCMU_CHAN_BAND_2G ?
1988 					      NL80211_BAND_2GHZ :
1989 					      NL80211_BAND_5GHZ);
1990 
1991 	cfg80211_rx_mgmt(&vif->wdev, freq, 0, mgmt_frame, mgmt_frame_len, 0);
1992 
1993 	brcmf_dbg(INFO, "mgmt_frame_len (%d) , e->datalen (%d), chanspec (%04x), freq (%d)\n",
1994 		  mgmt_frame_len, e->datalen, ch.chspec, freq);
1995 
1996 	return 0;
1997 }
1998 
1999 
2000 /**
2001  * brcmf_p2p_get_current_chanspec() - Get current operation channel.
2002  *
2003  * @p2p: P2P specific data.
2004  * @chanspec: chanspec to be returned.
2005  */
brcmf_p2p_get_current_chanspec(struct brcmf_p2p_info * p2p,u16 * chanspec)2006 static void brcmf_p2p_get_current_chanspec(struct brcmf_p2p_info *p2p,
2007 					   u16 *chanspec)
2008 {
2009 	struct brcmf_if *ifp;
2010 	u8 mac_addr[ETH_ALEN];
2011 	struct brcmu_chan ch;
2012 	struct brcmf_bss_info_le *bi;
2013 	u8 *buf;
2014 
2015 	ifp = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->ifp;
2016 
2017 	if (brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BSSID, mac_addr,
2018 				   ETH_ALEN) == 0) {
2019 		buf = kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
2020 		if (buf != NULL) {
2021 			*(__le32 *)buf = cpu_to_le32(WL_BSS_INFO_MAX);
2022 			if (brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BSS_INFO,
2023 						   buf, WL_BSS_INFO_MAX) == 0) {
2024 				bi = (struct brcmf_bss_info_le *)(buf + 4);
2025 				*chanspec = le16_to_cpu(bi->chanspec);
2026 				kfree(buf);
2027 				return;
2028 			}
2029 			kfree(buf);
2030 		}
2031 	}
2032 	/* Use default channel for P2P */
2033 	ch.chnum = BRCMF_P2P_TEMP_CHAN;
2034 	ch.bw = BRCMU_CHAN_BW_20;
2035 	p2p->cfg->d11inf.encchspec(&ch);
2036 	*chanspec = ch.chspec;
2037 }
2038 
2039 /**
2040  * brcmf_p2p_ifchange - Change a P2P Role.
2041  * @cfg: driver private data for cfg80211 interface.
2042  * @if_type: interface type.
2043  * Returns 0 if success.
2044  */
brcmf_p2p_ifchange(struct brcmf_cfg80211_info * cfg,enum brcmf_fil_p2p_if_types if_type)2045 int brcmf_p2p_ifchange(struct brcmf_cfg80211_info *cfg,
2046 		       enum brcmf_fil_p2p_if_types if_type)
2047 {
2048 	struct brcmf_p2p_info *p2p = &cfg->p2p;
2049 	struct brcmf_pub *drvr = cfg->pub;
2050 	struct brcmf_cfg80211_vif *vif;
2051 	struct brcmf_fil_p2p_if_le if_request;
2052 	s32 err;
2053 	u16 chanspec;
2054 
2055 	brcmf_dbg(TRACE, "Enter\n");
2056 
2057 	vif = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif;
2058 	if (!vif) {
2059 		bphy_err(drvr, "vif for P2PAPI_BSSCFG_PRIMARY does not exist\n");
2060 		return -EPERM;
2061 	}
2062 	brcmf_notify_escan_complete(cfg, vif->ifp, true, true);
2063 	vif = p2p->bss_idx[P2PAPI_BSSCFG_CONNECTION].vif;
2064 	if (!vif) {
2065 		bphy_err(drvr, "vif for P2PAPI_BSSCFG_CONNECTION does not exist\n");
2066 		return -EPERM;
2067 	}
2068 	brcmf_set_mpc(vif->ifp, 0);
2069 
2070 	/* In concurrency case, STA may be already associated in a particular */
2071 	/* channel. so retrieve the current channel of primary interface and  */
2072 	/* then start the virtual interface on that.                          */
2073 	brcmf_p2p_get_current_chanspec(p2p, &chanspec);
2074 
2075 	if_request.type = cpu_to_le16((u16)if_type);
2076 	if_request.chspec = cpu_to_le16(chanspec);
2077 	memcpy(if_request.addr, p2p->conn_int_addr, sizeof(if_request.addr));
2078 
2079 	brcmf_cfg80211_arm_vif_event(cfg, vif);
2080 	err = brcmf_fil_iovar_data_set(vif->ifp, "p2p_ifupd", &if_request,
2081 				       sizeof(if_request));
2082 	if (err) {
2083 		bphy_err(drvr, "p2p_ifupd FAILED, err=%d\n", err);
2084 		brcmf_cfg80211_arm_vif_event(cfg, NULL);
2085 		return err;
2086 	}
2087 	err = brcmf_cfg80211_wait_vif_event(cfg, BRCMF_E_IF_CHANGE,
2088 					    BRCMF_VIF_EVENT_TIMEOUT);
2089 	brcmf_cfg80211_arm_vif_event(cfg, NULL);
2090 	if (!err)  {
2091 		bphy_err(drvr, "No BRCMF_E_IF_CHANGE event received\n");
2092 		return -EIO;
2093 	}
2094 
2095 	err = brcmf_fil_cmd_int_set(vif->ifp, BRCMF_C_SET_SCB_TIMEOUT,
2096 				    BRCMF_SCB_TIMEOUT_VALUE);
2097 
2098 	return err;
2099 }
2100 
brcmf_p2p_request_p2p_if(struct brcmf_p2p_info * p2p,struct brcmf_if * ifp,u8 ea[ETH_ALEN],enum brcmf_fil_p2p_if_types iftype)2101 static int brcmf_p2p_request_p2p_if(struct brcmf_p2p_info *p2p,
2102 				    struct brcmf_if *ifp, u8 ea[ETH_ALEN],
2103 				    enum brcmf_fil_p2p_if_types iftype)
2104 {
2105 	struct brcmf_fil_p2p_if_le if_request;
2106 	int err;
2107 	u16 chanspec;
2108 
2109 	/* we need a default channel */
2110 	brcmf_p2p_get_current_chanspec(p2p, &chanspec);
2111 
2112 	/* fill the firmware request */
2113 	memcpy(if_request.addr, ea, ETH_ALEN);
2114 	if_request.type = cpu_to_le16((u16)iftype);
2115 	if_request.chspec = cpu_to_le16(chanspec);
2116 
2117 	err = brcmf_fil_iovar_data_set(ifp, "p2p_ifadd", &if_request,
2118 				       sizeof(if_request));
2119 
2120 	return err;
2121 }
2122 
brcmf_p2p_disable_p2p_if(struct brcmf_cfg80211_vif * vif)2123 static int brcmf_p2p_disable_p2p_if(struct brcmf_cfg80211_vif *vif)
2124 {
2125 	struct brcmf_cfg80211_info *cfg = wdev_to_cfg(&vif->wdev);
2126 	struct net_device *pri_ndev = cfg_to_ndev(cfg);
2127 	struct brcmf_if *ifp = netdev_priv(pri_ndev);
2128 	const u8 *addr = vif->wdev.netdev->dev_addr;
2129 
2130 	return brcmf_fil_iovar_data_set(ifp, "p2p_ifdis", addr, ETH_ALEN);
2131 }
2132 
brcmf_p2p_release_p2p_if(struct brcmf_cfg80211_vif * vif)2133 static int brcmf_p2p_release_p2p_if(struct brcmf_cfg80211_vif *vif)
2134 {
2135 	struct brcmf_cfg80211_info *cfg = wdev_to_cfg(&vif->wdev);
2136 	struct net_device *pri_ndev = cfg_to_ndev(cfg);
2137 	struct brcmf_if *ifp = netdev_priv(pri_ndev);
2138 	const u8 *addr = vif->wdev.netdev->dev_addr;
2139 
2140 	return brcmf_fil_iovar_data_set(ifp, "p2p_ifdel", addr, ETH_ALEN);
2141 }
2142 
2143 /**
2144  * brcmf_p2p_create_p2pdev() - create a P2P_DEVICE virtual interface.
2145  *
2146  * @p2p: P2P specific data.
2147  * @wiphy: wiphy device of new interface.
2148  * @addr: mac address for this new interface.
2149  */
brcmf_p2p_create_p2pdev(struct brcmf_p2p_info * p2p,struct wiphy * wiphy,u8 * addr)2150 static struct wireless_dev *brcmf_p2p_create_p2pdev(struct brcmf_p2p_info *p2p,
2151 						    struct wiphy *wiphy,
2152 						    u8 *addr)
2153 {
2154 	struct brcmf_pub *drvr = p2p->cfg->pub;
2155 	struct brcmf_cfg80211_vif *p2p_vif;
2156 	struct brcmf_if *p2p_ifp;
2157 	struct brcmf_if *pri_ifp;
2158 	int err;
2159 	u32 bsscfgidx;
2160 
2161 	if (p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif)
2162 		return ERR_PTR(-ENOSPC);
2163 
2164 	p2p_vif = brcmf_alloc_vif(p2p->cfg, NL80211_IFTYPE_P2P_DEVICE);
2165 	if (IS_ERR(p2p_vif)) {
2166 		bphy_err(drvr, "could not create discovery vif\n");
2167 		return (struct wireless_dev *)p2p_vif;
2168 	}
2169 
2170 	pri_ifp = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->ifp;
2171 
2172 	/* firmware requires unique mac address for p2pdev interface */
2173 	if (addr && ether_addr_equal(addr, pri_ifp->mac_addr)) {
2174 		bphy_err(drvr, "discovery vif must be different from primary interface\n");
2175 		err = -EINVAL;
2176 		goto fail;
2177 	}
2178 
2179 	brcmf_p2p_generate_bss_mac(p2p, addr);
2180 	brcmf_p2p_set_firmware(pri_ifp, p2p->dev_addr);
2181 
2182 	brcmf_cfg80211_arm_vif_event(p2p->cfg, p2p_vif);
2183 	brcmf_fweh_p2pdev_setup(pri_ifp, true);
2184 
2185 	/* Initialize P2P Discovery in the firmware */
2186 	err = brcmf_fil_iovar_int_set(pri_ifp, "p2p_disc", 1);
2187 	if (err < 0) {
2188 		bphy_err(drvr, "set p2p_disc error\n");
2189 		brcmf_fweh_p2pdev_setup(pri_ifp, false);
2190 		brcmf_cfg80211_arm_vif_event(p2p->cfg, NULL);
2191 		goto fail;
2192 	}
2193 
2194 	/* wait for firmware event */
2195 	err = brcmf_cfg80211_wait_vif_event(p2p->cfg, BRCMF_E_IF_ADD,
2196 					    BRCMF_VIF_EVENT_TIMEOUT);
2197 	brcmf_cfg80211_arm_vif_event(p2p->cfg, NULL);
2198 	brcmf_fweh_p2pdev_setup(pri_ifp, false);
2199 	if (!err) {
2200 		bphy_err(drvr, "timeout occurred\n");
2201 		err = -EIO;
2202 		goto fail;
2203 	}
2204 
2205 	/* discovery interface created */
2206 	p2p_ifp = p2p_vif->ifp;
2207 	p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif = p2p_vif;
2208 	memcpy(p2p_ifp->mac_addr, p2p->dev_addr, ETH_ALEN);
2209 	memcpy(&p2p_vif->wdev.address, p2p->dev_addr, sizeof(p2p->dev_addr));
2210 
2211 	/* verify bsscfg index for P2P discovery */
2212 	err = brcmf_fil_iovar_int_get(pri_ifp, "p2p_dev", &bsscfgidx);
2213 	if (err < 0) {
2214 		bphy_err(drvr, "retrieving discover bsscfg index failed\n");
2215 		goto fail;
2216 	}
2217 
2218 	WARN_ON(p2p_ifp->bsscfgidx != bsscfgidx);
2219 
2220 	init_completion(&p2p->send_af_done);
2221 	INIT_WORK(&p2p->afx_hdl.afx_work, brcmf_p2p_afx_handler);
2222 	init_completion(&p2p->afx_hdl.act_frm_scan);
2223 	init_completion(&p2p->wait_next_af);
2224 
2225 	return &p2p_vif->wdev;
2226 
2227 fail:
2228 	brcmf_free_vif(p2p_vif);
2229 	return ERR_PTR(err);
2230 }
2231 
brcmf_p2p_get_conn_idx(struct brcmf_cfg80211_info * cfg)2232 static int brcmf_p2p_get_conn_idx(struct brcmf_cfg80211_info *cfg)
2233 {
2234 	int i;
2235 	struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
2236 
2237 	if (!ifp)
2238 		return -ENODEV;
2239 
2240 	for (i = P2PAPI_BSSCFG_CONNECTION; i < P2PAPI_BSSCFG_MAX; i++) {
2241 		if (!cfg->p2p.bss_idx[i].vif) {
2242 			if (i == P2PAPI_BSSCFG_CONNECTION2 &&
2243 			    !(brcmf_feat_is_enabled(ifp, BRCMF_FEAT_RSDB))) {
2244 				brcmf_err("Multi p2p not supported");
2245 				return -EIO;
2246 			}
2247 			return i;
2248 		}
2249 	}
2250 	return -EIO;
2251 }
2252 
2253 /**
2254  * brcmf_p2p_add_vif() - create a new P2P virtual interface.
2255  *
2256  * @wiphy: wiphy device of new interface.
2257  * @name: name of the new interface.
2258  * @name_assign_type: origin of the interface name
2259  * @type: nl80211 interface type.
2260  * @params: contains mac address for P2P device.
2261  */
brcmf_p2p_add_vif(struct wiphy * wiphy,const char * name,unsigned char name_assign_type,enum nl80211_iftype type,struct vif_params * params)2262 struct wireless_dev *brcmf_p2p_add_vif(struct wiphy *wiphy, const char *name,
2263 				       unsigned char name_assign_type,
2264 				       enum nl80211_iftype type,
2265 				       struct vif_params *params)
2266 {
2267 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
2268 	struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
2269 	struct brcmf_pub *drvr = cfg->pub;
2270 	struct brcmf_cfg80211_vif *vif;
2271 	enum brcmf_fil_p2p_if_types iftype;
2272 	int err = 0;
2273 	int connidx;
2274 	u8 *p2p_intf_addr;
2275 
2276 	if (brcmf_cfg80211_vif_event_armed(cfg))
2277 		return ERR_PTR(-EBUSY);
2278 
2279 	brcmf_dbg(INFO, "adding vif \"%s\" (type=%d)\n", name, type);
2280 
2281 	switch (type) {
2282 	case NL80211_IFTYPE_P2P_CLIENT:
2283 		iftype = BRCMF_FIL_P2P_IF_CLIENT;
2284 		break;
2285 	case NL80211_IFTYPE_P2P_GO:
2286 		iftype = BRCMF_FIL_P2P_IF_GO;
2287 		break;
2288 	case NL80211_IFTYPE_P2P_DEVICE:
2289 		return brcmf_p2p_create_p2pdev(&cfg->p2p, wiphy,
2290 					       params->macaddr);
2291 	default:
2292 		return ERR_PTR(-EOPNOTSUPP);
2293 	}
2294 
2295 	vif = brcmf_alloc_vif(cfg, type);
2296 	if (IS_ERR(vif))
2297 		return (struct wireless_dev *)vif;
2298 	brcmf_cfg80211_arm_vif_event(cfg, vif);
2299 
2300 	connidx = brcmf_p2p_get_conn_idx(cfg);
2301 
2302 	if (connidx == P2PAPI_BSSCFG_CONNECTION)
2303 		p2p_intf_addr = cfg->p2p.conn_int_addr;
2304 	else if (connidx == P2PAPI_BSSCFG_CONNECTION2)
2305 		p2p_intf_addr = cfg->p2p.conn2_int_addr;
2306 	else
2307 		err = -EINVAL;
2308 
2309 	if (!err)
2310 		err =  brcmf_p2p_request_p2p_if(&cfg->p2p, ifp,
2311 						p2p_intf_addr, iftype);
2312 
2313 	if (err) {
2314 		brcmf_err("request p2p interface failed\n");
2315 		brcmf_cfg80211_arm_vif_event(cfg, NULL);
2316 		goto fail;
2317 	}
2318 
2319 	/* wait for firmware event */
2320 	err = brcmf_cfg80211_wait_vif_event(cfg, BRCMF_E_IF_ADD,
2321 					    BRCMF_VIF_EVENT_TIMEOUT);
2322 	brcmf_cfg80211_arm_vif_event(cfg, NULL);
2323 	if (!err) {
2324 		bphy_err(drvr, "timeout occurred\n");
2325 		err = -EIO;
2326 		goto fail;
2327 	}
2328 
2329 	/* interface created in firmware */
2330 	ifp = vif->ifp;
2331 	if (!ifp) {
2332 		bphy_err(drvr, "no if pointer provided\n");
2333 		err = -ENOENT;
2334 		goto fail;
2335 	}
2336 
2337 	strncpy(ifp->ndev->name, name, sizeof(ifp->ndev->name) - 1);
2338 	ifp->ndev->name_assign_type = name_assign_type;
2339 	err = brcmf_net_attach(ifp, true);
2340 	if (err) {
2341 		bphy_err(drvr, "Registering netdevice failed\n");
2342 		free_netdev(ifp->ndev);
2343 		goto fail;
2344 	}
2345 
2346 	cfg->p2p.bss_idx[connidx].vif = vif;
2347 	/* Disable firmware roaming for P2P interface  */
2348 	brcmf_fil_iovar_int_set(ifp, "roam_off", 1);
2349 	if (iftype == BRCMF_FIL_P2P_IF_GO) {
2350 		/* set station timeout for p2p */
2351 		brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCB_TIMEOUT,
2352 				      BRCMF_SCB_TIMEOUT_VALUE);
2353 	}
2354 	return &ifp->vif->wdev;
2355 
2356 fail:
2357 	brcmf_free_vif(vif);
2358 	return ERR_PTR(err);
2359 }
2360 
2361 /**
2362  * brcmf_p2p_del_vif() - delete a P2P virtual interface.
2363  *
2364  * @wiphy: wiphy device of interface.
2365  * @wdev: wireless device of interface.
2366  */
brcmf_p2p_del_vif(struct wiphy * wiphy,struct wireless_dev * wdev)2367 int brcmf_p2p_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev)
2368 {
2369 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
2370 	struct brcmf_p2p_info *p2p = &cfg->p2p;
2371 	struct brcmf_cfg80211_vif *vif;
2372 	enum nl80211_iftype iftype;
2373 	bool wait_for_disable = false;
2374 	int err;
2375 
2376 	brcmf_dbg(TRACE, "delete P2P vif\n");
2377 	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
2378 
2379 	iftype = vif->wdev.iftype;
2380 	brcmf_cfg80211_arm_vif_event(cfg, vif);
2381 	switch (iftype) {
2382 	case NL80211_IFTYPE_P2P_CLIENT:
2383 		if (test_bit(BRCMF_VIF_STATUS_DISCONNECTING, &vif->sme_state))
2384 			wait_for_disable = true;
2385 		break;
2386 
2387 	case NL80211_IFTYPE_P2P_GO:
2388 		if (!brcmf_p2p_disable_p2p_if(vif))
2389 			wait_for_disable = true;
2390 		break;
2391 
2392 	case NL80211_IFTYPE_P2P_DEVICE:
2393 		if (!p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif)
2394 			return 0;
2395 		brcmf_p2p_cancel_remain_on_channel(vif->ifp);
2396 		brcmf_p2p_deinit_discovery(p2p);
2397 		break;
2398 
2399 	default:
2400 		return -ENOTSUPP;
2401 	}
2402 
2403 	clear_bit(BRCMF_P2P_STATUS_GO_NEG_PHASE, &p2p->status);
2404 	brcmf_dbg(INFO, "P2P: GO_NEG_PHASE status cleared\n");
2405 
2406 	if (wait_for_disable)
2407 		wait_for_completion_timeout(&cfg->vif_disabled,
2408 					    BRCMF_P2P_DISABLE_TIMEOUT);
2409 
2410 	err = 0;
2411 	if (iftype != NL80211_IFTYPE_P2P_DEVICE) {
2412 		brcmf_vif_clear_mgmt_ies(vif);
2413 		err = brcmf_p2p_release_p2p_if(vif);
2414 	}
2415 	if (!err) {
2416 		/* wait for firmware event */
2417 		err = brcmf_cfg80211_wait_vif_event(cfg, BRCMF_E_IF_DEL,
2418 						    BRCMF_VIF_EVENT_TIMEOUT);
2419 		if (!err)
2420 			err = -EIO;
2421 		else
2422 			err = 0;
2423 	}
2424 	brcmf_remove_interface(vif->ifp, true);
2425 
2426 	brcmf_cfg80211_arm_vif_event(cfg, NULL);
2427 	if (iftype != NL80211_IFTYPE_P2P_DEVICE) {
2428 		if (vif == p2p->bss_idx[P2PAPI_BSSCFG_CONNECTION].vif)
2429 			p2p->bss_idx[P2PAPI_BSSCFG_CONNECTION].vif = NULL;
2430 		if (vif == p2p->bss_idx[P2PAPI_BSSCFG_CONNECTION2].vif)
2431 			p2p->bss_idx[P2PAPI_BSSCFG_CONNECTION2].vif = NULL;
2432 	}
2433 
2434 	return err;
2435 }
2436 
brcmf_p2p_ifp_removed(struct brcmf_if * ifp,bool locked)2437 void brcmf_p2p_ifp_removed(struct brcmf_if *ifp, bool locked)
2438 {
2439 	struct brcmf_cfg80211_info *cfg;
2440 	struct brcmf_cfg80211_vif *vif;
2441 
2442 	brcmf_dbg(INFO, "P2P: device interface removed\n");
2443 	vif = ifp->vif;
2444 	cfg = wdev_to_cfg(&vif->wdev);
2445 	cfg->p2p.bss_idx[P2PAPI_BSSCFG_DEVICE].vif = NULL;
2446 	if (!locked) {
2447 		rtnl_lock();
2448 		wiphy_lock(cfg->wiphy);
2449 		cfg80211_unregister_wdev(&vif->wdev);
2450 		wiphy_unlock(cfg->wiphy);
2451 		rtnl_unlock();
2452 	} else {
2453 		cfg80211_unregister_wdev(&vif->wdev);
2454 	}
2455 	brcmf_free_vif(vif);
2456 }
2457 
brcmf_p2p_start_device(struct wiphy * wiphy,struct wireless_dev * wdev)2458 int brcmf_p2p_start_device(struct wiphy *wiphy, struct wireless_dev *wdev)
2459 {
2460 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
2461 	struct brcmf_p2p_info *p2p = &cfg->p2p;
2462 	struct brcmf_cfg80211_vif *vif;
2463 	int err;
2464 
2465 	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
2466 	mutex_lock(&cfg->usr_sync);
2467 	err = brcmf_p2p_enable_discovery(p2p);
2468 	if (!err)
2469 		set_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state);
2470 	mutex_unlock(&cfg->usr_sync);
2471 	return err;
2472 }
2473 
brcmf_p2p_stop_device(struct wiphy * wiphy,struct wireless_dev * wdev)2474 void brcmf_p2p_stop_device(struct wiphy *wiphy, struct wireless_dev *wdev)
2475 {
2476 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
2477 	struct brcmf_p2p_info *p2p = &cfg->p2p;
2478 	struct brcmf_cfg80211_vif *vif;
2479 
2480 	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
2481 	/* This call can be result of the unregister_wdev call. In that case
2482 	 * we dont want to do anything anymore. Just return. The config vif
2483 	 * will have been cleared at this point.
2484 	 */
2485 	if (p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif == vif) {
2486 		mutex_lock(&cfg->usr_sync);
2487 		/* Set the discovery state to SCAN */
2488 		(void)brcmf_p2p_set_discover_state(vif->ifp,
2489 						   WL_P2P_DISC_ST_SCAN, 0, 0);
2490 		brcmf_abort_scanning(cfg);
2491 		clear_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state);
2492 		mutex_unlock(&cfg->usr_sync);
2493 	}
2494 }
2495 
2496 /**
2497  * brcmf_p2p_attach() - attach for P2P.
2498  *
2499  * @cfg: driver private data for cfg80211 interface.
2500  * @p2pdev_forced: create p2p device interface at attach.
2501  */
brcmf_p2p_attach(struct brcmf_cfg80211_info * cfg,bool p2pdev_forced)2502 s32 brcmf_p2p_attach(struct brcmf_cfg80211_info *cfg, bool p2pdev_forced)
2503 {
2504 	struct brcmf_pub *drvr = cfg->pub;
2505 	struct brcmf_p2p_info *p2p;
2506 	struct brcmf_if *pri_ifp;
2507 	s32 err = 0;
2508 	void *err_ptr;
2509 
2510 	p2p = &cfg->p2p;
2511 	p2p->cfg = cfg;
2512 
2513 	pri_ifp = brcmf_get_ifp(cfg->pub, 0);
2514 	p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif = pri_ifp->vif;
2515 
2516 	if (p2pdev_forced) {
2517 		err_ptr = brcmf_p2p_create_p2pdev(p2p, NULL, NULL);
2518 		if (IS_ERR(err_ptr)) {
2519 			bphy_err(drvr, "P2P device creation failed.\n");
2520 			err = PTR_ERR(err_ptr);
2521 		}
2522 	} else {
2523 		p2p->p2pdev_dynamically = true;
2524 	}
2525 	return err;
2526 }
2527 
2528 /**
2529  * brcmf_p2p_detach() - detach P2P.
2530  *
2531  * @p2p: P2P specific data.
2532  */
brcmf_p2p_detach(struct brcmf_p2p_info * p2p)2533 void brcmf_p2p_detach(struct brcmf_p2p_info *p2p)
2534 {
2535 	struct brcmf_cfg80211_vif *vif;
2536 
2537 	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
2538 	if (vif != NULL) {
2539 		brcmf_p2p_cancel_remain_on_channel(vif->ifp);
2540 		brcmf_p2p_deinit_discovery(p2p);
2541 		brcmf_remove_interface(vif->ifp, false);
2542 	}
2543 	/* just set it all to zero */
2544 	memset(p2p, 0, sizeof(*p2p));
2545 }
2546 
2547