P2P: Process preferred frequency list as part of GO Neg Req/Resp

When processing a GO Negotiation Request and Response, if local driver
supports the preferred channel list extension, then:

- Check if peer's preference for operating channel is already included
  in our preferred channel list and if so, take the oper_channel as is.

- If peer's preference for operating channel is not in local device's
  preferred channel list and peer device has provided its preferred
  frequency list in the GO Negotiation Request/Response, then find a
  channel that is common for both preferred channel lists and use it
  for oper_channel.

- If peer's preference for operating channel is not in local device's
  preferred channel list and peer device doesn't use preferred channel
  list extension, i.e., no preferred channel list in GO Negotiation
  Request/Response, then look for a channel that is common for local
  device's preferred channel list and peer's list of supported channels
  and use it for oper_channel.

- In case no common channel is found, use the peer's preference for
  oper_channel as is.

Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>

src/p2p/p2p_go_neg.c

@@ -545,6 +545,195 @@ int p2p_go_select_channel(struct p2p_data *p2p, struct p2p_device *dev,
 }
 
 
+static void p2p_check_pref_chan_no_recv(struct p2p_data *p2p, int go,
+					struct p2p_device *dev,
+					struct p2p_message *msg,
+					unsigned freq_list[], unsigned int size)
+{
+	u8 op_class, op_channel;
+	unsigned int oper_freq, i, j;
+	int found = 0;
+
+	p2p_dbg(p2p,
+		"Peer didn't provide a preferred frequency list, see if any of our preferred channels are supported by peer device");
+
+	/*
+	 * Search for a common channel in our preferred frequency list which is
+	 * also supported by the peer device.
+	 */
+	for (i = 0; i < size && !found; i++) {
+		/*
+		 * Make sure that the common frequency is:
+		 * 1. Supported by peer
+		 * 2. Allowed for P2P use.
+		 */
+		oper_freq = freq_list[i];
+		if (p2p_freq_to_channel(oper_freq, &op_class,
+					&op_channel) < 0) {
+			p2p_dbg(p2p, "Unsupported frequency %u MHz", oper_freq);
+			continue;
+		}
+		if (!p2p_channels_includes(&p2p->cfg->channels,
+					   op_class, op_channel) &&
+		    (go || !p2p_channels_includes(&p2p->cfg->cli_channels,
+						  op_class, op_channel))) {
+			p2p_dbg(p2p,
+				"Freq %u MHz (oper_class %u channel %u) not allowed for P2P",
+				oper_freq, op_class, op_channel);
+			break;
+		}
+		for (j = 0; j < msg->channel_list_len; j++) {
+
+			if (op_channel != msg->channel_list[j])
+				continue;
+
+			p2p->op_reg_class = op_class;
+			p2p->op_channel = op_channel;
+			os_memcpy(&p2p->channels, &p2p->cfg->channels,
+				  sizeof(struct p2p_channels));
+			found = 1;
+			break;
+		}
+	}
+
+	if (found) {
+		p2p_dbg(p2p,
+			"Freq %d MHz is a preferred channel and is also supported by peer, use it as the operating channel",
+			oper_freq);
+	} else {
+		p2p_dbg(p2p,
+			"None of our preferred channels are supported by peer!. Use: %d MHz for oper_channel",
+			dev->oper_freq);
+	}
+}
+
+
+static void p2p_check_pref_chan_recv(struct p2p_data *p2p, int go,
+				     struct p2p_device *dev,
+				     struct p2p_message *msg,
+				     unsigned freq_list[], unsigned int size)
+{
+	u8 op_class, op_channel;
+	unsigned int oper_freq, i, j;
+	int found = 0;
+
+	/*
+	 * Peer device supports a Preferred Frequency List.
+	 * Search for a common channel in the preferred frequency lists
+	 * of both peer and local devices.
+	 */
+	for (i = 0; i < size && !found; i++) {
+		for (j = 2; j < (msg->pref_freq_list_len / 2); j++) {
+			oper_freq = p2p_channel_to_freq(
+				msg->pref_freq_list[2 * j],
+				msg->pref_freq_list[2 * j + 1]);
+			if (freq_list[i] != oper_freq)
+				continue;
+
+			/*
+			 * Make sure that the found frequency is:
+			 * 1. Supported
+			 * 2. Allowed for P2P use.
+			 */
+			if (p2p_freq_to_channel(oper_freq, &op_class,
+						&op_channel) < 0) {
+				p2p_dbg(p2p, "Unsupported frequency %u MHz",
+					oper_freq);
+				continue;
+			}
+
+			if (!p2p_channels_includes(&p2p->cfg->channels,
+						   op_class, op_channel) &&
+			    (go ||
+			     !p2p_channels_includes(&p2p->cfg->cli_channels,
+						    op_class, op_channel))) {
+				p2p_dbg(p2p,
+					"Freq %u MHz (oper_class %u channel %u) not allowed for P2P",
+					oper_freq, op_class, op_channel);
+				break;
+			}
+			p2p->op_reg_class = op_class;
+			p2p->op_channel = op_channel;
+			os_memcpy(&p2p->channels, &p2p->cfg->channels,
+				  sizeof(struct p2p_channels));
+			found = 1;
+			break;
+		}
+	}
+
+	if (found) {
+		p2p_dbg(p2p,
+			"Freq %d MHz is a common preferred channel for both peer and local, use it as operating channel",
+			oper_freq);
+	} else {
+		p2p_dbg(p2p,
+			"No common preferred channels found! Use: %d MHz for oper_channel",
+			dev->oper_freq);
+	}
+}
+
+
+static void p2p_check_pref_chan(struct p2p_data *p2p, int go,
+				struct p2p_device *dev, struct p2p_message *msg)
+{
+	unsigned int freq_list[P2P_MAX_PREF_CHANNELS], size;
+	unsigned int i;
+	u8 op_class, op_channel;
+
+	/*
+	 * Use the preferred channel list from the driver only if there is no
+	 * forced_freq, e.g., P2P_CONNECT freq=..., and no preferred operating
+	 * channel hardcoded in the configuration file.
+	 */
+	if (!p2p->cfg->get_pref_freq_list || p2p->cfg->num_pref_chan ||
+	    (dev->flags & P2P_DEV_FORCE_FREQ) || p2p->cfg->cfg_op_channel)
+		return;
+
+	/* Obtain our preferred frequency list from driver based on P2P role. */
+	size = P2P_MAX_PREF_CHANNELS;
+	if (p2p->cfg->get_pref_freq_list(p2p->cfg->cb_ctx, go, &size,
+					 freq_list))
+		return;
+
+	/*
+	 * Check if peer's preference of operating channel is in
+	 * our preferred channel list.
+	 */
+	for (i = 0; i < size; i++) {
+		if (freq_list[i] == (unsigned int) dev->oper_freq)
+			break;
+	}
+	if (i != size) {
+		/* Peer operating channel preference matches our preference */
+		if (p2p_freq_to_channel(freq_list[i], &op_class, &op_channel) <
+		    0) {
+			p2p_dbg(p2p,
+				"Peer operating channel preference is unsupported frequency %u MHz",
+				freq_list[i]);
+		} else {
+			p2p->op_reg_class = op_class;
+			p2p->op_channel = op_channel;
+			os_memcpy(&p2p->channels, &p2p->cfg->channels,
+				  sizeof(struct p2p_channels));
+			return;
+		}
+	}
+
+	p2p_dbg(p2p,
+		"Peer operating channel preference: %d MHz is not in our preferred channel list",
+		dev->oper_freq);
+
+	/*
+	  Check if peer's preferred channel list is
+	  * _not_ included in the GO Negotiation Request.
+	  */
+	if (msg->pref_freq_list_len == 0)
+		p2p_check_pref_chan_no_recv(p2p, go, dev, msg, freq_list, size);
+	else
+		p2p_check_pref_chan_recv(p2p, go, dev, msg, freq_list, size);
+}
+
+
 void p2p_process_go_neg_req(struct p2p_data *p2p, const u8 *sa,
 			    const u8 *data, size_t len, int rx_freq)
 {
@@ -802,6 +991,12 @@ void p2p_process_go_neg_req(struct p2p_data *p2p, const u8 *sa,
 		p2p_dbg(p2p, "Peer operating channel preference: %d MHz",
 			dev->oper_freq);
 
+		/*
+		 * Use the driver preferred frequency list extension if
+		 * supported.
+		 */
+		p2p_check_pref_chan(p2p, go, dev, &msg);
+
 		if (msg.config_timeout) {
 			dev->go_timeout = msg.config_timeout[0];
 			dev->client_timeout = msg.config_timeout[1];
@@ -1153,6 +1348,13 @@ void p2p_process_go_neg_resp(struct p2p_data *p2p, const u8 *sa,
 	if (go && p2p_go_select_channel(p2p, dev, &status) < 0)
 		goto fail;
 
+	/*
+	 * Use the driver preferred frequency list extension if local device is
+	 * GO.
+	 */
+	if (go)
+		p2p_check_pref_chan(p2p, go, dev, &msg);
+
 	p2p_set_state(p2p, P2P_GO_NEG);
 	p2p_clear_timeout(p2p);