DSCP: Allow DSCP Policy Response Action frame to be sent

Add support to prepare and send DSCP response action frame to the
connected AP in response to a new control interface command DSCP_RESP.

Signed-off-by: Veerendranath Jakkam <vjakkam@codeaurora.org>

wpa_supplicant/README

@@ -1135,3 +1135,11 @@ is as shown below:
 
 - External applications shall clear active DSCP policies upon receiving
   "CTRL-EVENT-DISCONNECTED" or "CTRL-EVENT-DSCP-POLICY clear_all" events.
+
+DSCP Response:
+A QoS Management STA that enables DSCP Policy capability shall respond
+with DSCP response on receipt of a successful DSCP request from its
+associated AP.  wpa_supplicant sends DSCP policy response based on the
+control interface command received from the user is as below:
+
+DSCP_RESP <[reset]>/<[solicited] [policy_id=1 status=0...]> [more]

wpa_supplicant/ctrl_iface.c

@@ -11390,6 +11390,71 @@ free_scs_desc:
 }
 
 
+static int wpas_ctrl_iface_send_dscp_resp(struct wpa_supplicant *wpa_s,
+					  const char *cmd)
+{
+	char *pos;
+	struct dscp_policy_status *policy = NULL, *n;
+	int num_policies = 0, ret = -1;
+	struct dscp_resp_data resp_data;
+
+	/*
+	 * format:
+	 * <[reset]>/<[solicited] [policy_id=1 status=0...]> [more]
+	 */
+
+	os_memset(&resp_data, 0, sizeof(resp_data));
+
+	resp_data.more = os_strstr(cmd, "more") != NULL;
+
+	if (os_strstr(cmd, "reset")) {
+		resp_data.reset = true;
+		resp_data.solicited = false;
+		goto send_resp;
+	}
+
+	resp_data.solicited = os_strstr(cmd, "solicited") != NULL;
+
+	pos = os_strstr(cmd, "policy_id=");
+	while (pos) {
+		n = os_realloc(policy, (num_policies + 1) * sizeof(*policy));
+		if (!n)
+			goto fail;
+
+		policy = n;
+		pos += 10;
+		policy[num_policies].id = atoi(pos);
+		if (policy[num_policies].id == 0) {
+			wpa_printf(MSG_ERROR, "DSCP: Invalid policy id");
+			goto fail;
+		}
+
+		pos = os_strstr(pos, "status=");
+		if (!pos) {
+			wpa_printf(MSG_ERROR,
+				   "DSCP: Status is not found for a policy");
+			goto fail;
+		}
+
+		pos += 7;
+		policy[num_policies].status = atoi(pos);
+		num_policies++;
+
+		pos = os_strstr(pos, "policy_id");
+	}
+
+	resp_data.policy = policy;
+	resp_data.num_policies = num_policies;
+send_resp:
+	ret = wpas_send_dscp_response(wpa_s, &resp_data);
+	if (ret)
+		wpa_printf(MSG_ERROR, "DSCP: Failed to send DSCP response");
+fail:
+	os_free(policy);
+	return ret;
+}
+
+
 char * wpa_supplicant_ctrl_iface_process(struct wpa_supplicant *wpa_s,
 					 char *buf, size_t *resp_len)
 {
@@ -12318,6 +12383,9 @@ char * wpa_supplicant_ctrl_iface_process(struct wpa_supplicant *wpa_s,
 	} else if (os_strncmp(buf, "SCS ", 4) == 0) {
 		if (wpas_ctrl_iface_configure_scs(wpa_s, buf + 4))
 			reply_len = -1;
+	} else if (os_strncmp(buf, "DSCP_RESP ", 10) == 0) {
+		if (wpas_ctrl_iface_send_dscp_resp(wpa_s, buf + 10))
+			reply_len = -1;
 	} else {
 		os_memcpy(reply, "UNKNOWN COMMAND\n", 16);
 		reply_len = 16;

wpa_supplicant/robust_av.c

@@ -1268,3 +1268,78 @@ void wpas_handle_qos_mgmt_recv_action(struct wpa_supplicant *wpa_s,
 
 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY "request_end");
 }
+
+
+int wpas_send_dscp_response(struct wpa_supplicant *wpa_s,
+			    struct dscp_resp_data *resp_data)
+{
+	struct wpabuf *buf = NULL;
+	size_t buf_len;
+	int ret = -1, i;
+	u8 resp_control = 0;
+
+	if (wpa_s->wpa_state != WPA_COMPLETED || !wpa_s->current_ssid) {
+		wpa_printf(MSG_ERROR,
+			   "QM: Failed to send DSCP response - not connected to AP");
+		return -1;
+	}
+
+	if (resp_data->solicited && !wpa_s->dscp_req_dialog_token) {
+		wpa_printf(MSG_ERROR, "QM: No ongoing DSCP request");
+		return -1;
+	}
+
+	buf_len = 1 +	/* Category */
+		  3 +	/* OUI */
+		  1 +	/* OUI Type */
+		  1 +	/* OUI Subtype */
+		  1 +	/* Dialog Token */
+		  1 +	/* Response Control */
+		  1 +	/* Count */
+		  2 * resp_data->num_policies;  /* Status list */
+	buf = wpabuf_alloc(buf_len);
+	if (!buf) {
+		wpa_printf(MSG_ERROR,
+			   "QM: Failed to allocate DSCP policy response");
+		return -1;
+	}
+
+	wpabuf_put_u8(buf, WLAN_ACTION_VENDOR_SPECIFIC_PROTECTED);
+	wpabuf_put_be24(buf, OUI_WFA);
+	wpabuf_put_u8(buf, QM_ACTION_OUI_TYPE);
+	wpabuf_put_u8(buf, QM_DSCP_POLICY_RESP);
+
+	wpabuf_put_u8(buf, resp_data->solicited ?
+		      wpa_s->dscp_req_dialog_token : 0);
+
+	if (resp_data->more)
+		resp_control |= DSCP_POLICY_CTRL_MORE;
+	if (resp_data->reset)
+		resp_control |= DSCP_POLICY_CTRL_RESET;
+	wpabuf_put_u8(buf, resp_control);
+
+	wpabuf_put_u8(buf, resp_data->num_policies);
+	for (i = 0; i < resp_data->num_policies; i++) {
+		wpabuf_put_u8(buf, resp_data->policy[i].id);
+		wpabuf_put_u8(buf, resp_data->policy[i].status);
+	}
+
+	wpa_hexdump_buf(MSG_MSGDUMP, "DSCP response frame: ", buf);
+	ret = wpa_drv_send_action(wpa_s, wpa_s->assoc_freq, 0, wpa_s->bssid,
+				  wpa_s->own_addr, wpa_s->bssid,
+				  wpabuf_head(buf), wpabuf_len(buf), 0);
+	if (ret < 0) {
+		wpa_msg(wpa_s, MSG_INFO, "QM: Failed to send DSCP response");
+		goto fail;
+	}
+
+	/*
+	 * Mark DSCP request complete whether response sent is solicited or
+	 * unsolicited
+	 */
+	wpa_s->dscp_req_dialog_token = 0;
+
+fail:
+	wpabuf_free(buf);
+	return ret;
+}

wpa_supplicant/wpa_supplicant_i.h

@@ -524,6 +524,19 @@ struct robust_av_data {
 	bool valid_config;
 };
 
+struct dscp_policy_status {
+	u8 id;
+	u8 status;
+};
+
+struct dscp_resp_data {
+	bool more;
+	bool reset;
+	bool solicited;
+	struct dscp_policy_status *policy;
+	int num_policies;
+};
+
 #ifdef CONFIG_PASN
 
 struct pasn_fils {
@@ -1846,6 +1859,8 @@ void wpas_handle_qos_mgmt_recv_action(struct wpa_supplicant *wpa_s,
 				      const u8 *src,
 				      const u8 *buf, size_t len);
 void wpas_dscp_deinit(struct wpa_supplicant *wpa_s);
+int wpas_send_dscp_response(struct wpa_supplicant *wpa_s,
+			    struct dscp_resp_data *resp_data);
 
 int wpas_pasn_auth_start(struct wpa_supplicant *wpa_s,
 			 const u8 *bssid, int akmp, int cipher,