P2P2: CAPI commands support for USD

Add support for P2P unsynchronized service discovery with
sta_exec_action Prog,P2P.

Signed-off-by: Shivani Baranwal <quic_shivbara@quicinc.com>

Android.mk

@@ -15,6 +15,7 @@ OBJS += atheros.c
 OBJS += ftm.c
 OBJS += dpp.c
 OBJS += dhcp.c
+OBJS += p2p_usd.c
 
 # Initialize CFLAGS to limit to local module
 CFLAGS =

Makefile

@@ -54,6 +54,7 @@ OBJS += powerswitch.o
 OBJS += atheros.o
 OBJS += ftm.o
 OBJS += dpp.o
+OBJS += p2p_usd.o
 
 ifndef NO_TRAFFIC_AGENT
 CFLAGS += -DCONFIG_TRAFFIC_AGENT -DCONFIG_WFA_WMM_AC

p2p.c

@@ -872,6 +872,20 @@ static enum sigma_cmd_result cmd_sta_set_p2p(struct sigma_dut *dut,
 		}
 	}
 
+	val = get_param(cmd, "UnsyncServDsc");
+	if (val) {
+		if (strcasecmp(val, "On") == 0) {
+			/* Support USD functionality */
+			dut->usd_enabled = true;
+			sigma_dut_print(dut, DUT_MSG_INFO,
+					"Unsynchronized Service Discovery ON");
+		} else {
+			dut->usd_enabled = false;
+			sigma_dut_print(dut, DUT_MSG_INFO,
+					"Unsynchronized Service Discovery OFF");
+		}
+	}
+
 	return 1;
 }
 

p2p_usd.c

@@ -0,0 +1,323 @@
+/*
+ * Sigma Control API DUT (USD functionality)
+ * Copyright (c) 2024, Qualcomm Innovation Center, Inc.
+ * All Rights Reserved.
+ * Licensed under the Clear BSD license. See README for more details.
+ */
+
+#include "sigma_dut.h"
+#include <sys/stat.h>
+#include "wpa_ctrl.h"
+#include "wpa_helpers.h"
+
+
+enum sigma_cmd_result sigma_usd_publish(struct sigma_dut *dut,
+					struct sigma_conn *conn,
+					struct sigma_cmd *cmd)
+{
+	const char *program = get_param(cmd, "Prog");
+	const char *ifname = get_param(cmd, "interface");
+	const char *service_name = get_param(cmd, "ServiceName");
+	const char *ssi = get_param(cmd, "ServSpecificInfoPayload");
+	const char *serv_proto_type = get_param(cmd, "ServProtoType");
+	const char *oper_chan = get_param(cmd, "AdvertiseChannel");
+	const char *bootstrapmethod = get_param(cmd, "PairingBootstrapMethod");
+	char buf[3000];
+	int i, res;
+	size_t len;
+
+	if (!ifname)
+		ifname = get_station_ifname(dut);
+
+	if (bootstrapmethod) {
+		if (wpa_command(ifname, "P2P_SET pairing_setup 1") < 0) {
+			send_resp(dut, conn, SIGMA_ERROR,
+				  "ErrorCode,Failed to set pairing setup");
+			return STATUS_SENT;
+		}
+
+		if (wpa_command(ifname, "P2P_SET pairing_cache 1") < 0) {
+			send_resp(dut, conn, SIGMA_ERROR,
+				  "ErrorCode,Failed to set pairing cache");
+			return STATUS_SENT;
+		}
+
+		if (wpa_command(ifname, "P2P_SET pairing_verification 1") < 0) {
+			send_resp(dut, conn, SIGMA_ERROR,
+				  "ErrorCode,Failed to enable pairing verification");
+			return STATUS_SENT;
+		}
+	}
+
+	if (!service_name)
+		return ERROR_SEND_STATUS;
+
+	res = snprintf(buf, sizeof(buf), "NAN_PUBLISH service_name=%s ttl=100",
+		       service_name);
+	if (res < 0 || res >= sizeof(buf))
+		return ERROR_SEND_STATUS;
+	len = res;
+
+	if (program && strcasecmp(program, "P2P") == 0) {
+		res = snprintf(buf + len, sizeof(buf) - len, " p2p=1");
+		if (res < 0 || res >= sizeof(buf) - len)
+			return ERROR_SEND_STATUS;
+		len += res;
+	}
+
+	if (ssi) {
+		size_t ssi_len = atoi(ssi);
+		char ssi_hex[2048];
+
+		if (ssi_len * 2 + 1 > sizeof(ssi_hex))
+			return ERROR_SEND_STATUS;
+		for (i = 0; i < ssi_len; i++)
+			sprintf(ssi_hex + i * 2, "%02x", i);
+		ssi_hex[ssi_len * 2] = '\0';
+		res = snprintf(buf + len, sizeof(buf) - len, " ssi=%s",
+			       ssi_hex);
+		if (res < 0 || res >= sizeof(buf) - len)
+			return ERROR_SEND_STATUS;
+		len += res;
+	}
+
+	if (serv_proto_type) {
+		res = snprintf(buf + len, sizeof(buf) - len,
+			       " srv_proto_type=%s", serv_proto_type);
+		if (res < 0 || res >= sizeof(buf) - len)
+			return ERROR_SEND_STATUS;
+		len += res;
+	}
+
+	if (oper_chan) {
+		int chan, freq;
+
+		chan = atoi(oper_chan);
+		freq = channel_to_freq(dut, chan);
+		res = snprintf(buf + len, sizeof(buf) - len, " freq=%d", freq);
+	} else {
+		res = snprintf(buf + len, sizeof(buf) - len, " freq=2437");
+	}
+	if (res < 0 || res >= sizeof(buf) - len)
+		return ERROR_SEND_STATUS;
+	len += res;
+
+	if (wpa_command(ifname, buf)) {
+		send_resp(dut, conn, SIGMA_ERROR, "Unable to USD publish");
+		return STATUS_SENT_ERROR;
+	}
+
+	return SUCCESS_SEND_STATUS;
+}
+
+
+enum sigma_cmd_result sigma_usd_subscribe(struct sigma_dut *dut,
+					  struct sigma_conn *conn,
+					  struct sigma_cmd *cmd)
+{
+	const char *program = get_param(cmd, "Prog");
+	const char *ifname = get_param(cmd, "interface");
+	const char *service_name = get_param(cmd, "ServiceName");
+	const char *ssi = get_param(cmd, "ServSpecificInfoPayload");
+	const char *serv_proto_type = get_param(cmd, "ServProtoType");
+	const char *default_chan = get_param(cmd, "DefaultPublishChannel");
+	const char *chan_list = get_param(cmd, "PublishChannelList");
+	const char *bootstrapmethod = get_param(cmd, "PairingBootstrapMethod");
+	char buf[3000];
+	int i, res;
+	size_t len;
+
+	if (!ifname)
+		ifname = get_station_ifname(dut);
+
+	if (bootstrapmethod) {
+		if (wpa_command(ifname, "P2P_SET pairing_setup 1") < 0) {
+			send_resp(dut, conn, SIGMA_ERROR,
+				  "ErrorCode,Failed to set pairing setup");
+			return STATUS_SENT;
+		}
+
+		if (wpa_command(ifname, "P2P_SET pairing_cache 1") < 0) {
+			send_resp(dut, conn, SIGMA_ERROR,
+				  "ErrorCode,Failed to set pairing cache");
+			return STATUS_SENT;
+		}
+
+		if (wpa_command(ifname, "P2P_SET pairing_verification 1") < 0) {
+			send_resp(dut, conn, SIGMA_ERROR,
+				  "ErrorCode,Failed to enable pairing verification");
+			return STATUS_SENT;
+		}
+	}
+
+	if (!service_name)
+		return ERROR_SEND_STATUS;
+
+	res = snprintf(buf, sizeof(buf),
+		       "NAN_SUBSCRIBE service_name=%s ttl=100",
+		       service_name);
+	if (res < 0 || res >= sizeof(buf))
+		return ERROR_SEND_STATUS;
+	len = res;
+
+	if (program && strcasecmp(program, "P2P") == 0) {
+		res = snprintf(buf + len, sizeof(buf) - len, " p2p=1");
+		if (res < 0 || res >= sizeof(buf) - len)
+			return ERROR_SEND_STATUS;
+		len += res;
+	}
+
+	if (ssi) {
+		size_t ssi_len = atoi(ssi);
+		char ssi_hex[2048];
+
+		if (ssi_len * 2 + 1 > sizeof(ssi_hex))
+			return ERROR_SEND_STATUS;
+		for (i = 0; i < ssi_len; i++)
+			sprintf(ssi_hex + i * 2, "%02x", i);
+		ssi_hex[ssi_len * 2] = '\0';
+		res = snprintf(buf + len, sizeof(buf) - len, " ssi=%s",
+			       ssi_hex);
+		if (res < 0 || res >= sizeof(buf) - len)
+			return ERROR_SEND_STATUS;
+		len += res;
+	}
+
+	if (serv_proto_type) {
+		res = snprintf(buf + len, sizeof(buf) - len,
+			       " srv_proto_type=%s", serv_proto_type);
+		if (res < 0 || res >= sizeof(buf) - len)
+			return ERROR_SEND_STATUS;
+		len += res;
+	}
+
+	if (default_chan) {
+		int chan, freq;
+
+		chan = atoi(default_chan);
+		freq = channel_to_freq(dut, chan);
+		res = snprintf(buf + len, sizeof(buf) - len, " freq=%d", freq);
+	} else {
+		res = snprintf(buf + len, sizeof(buf) - len, " freq=2437");
+	}
+	if (res < 0 || res >= sizeof(buf) - len)
+		return ERROR_SEND_STATUS;
+	len += res;
+
+	if (chan_list) {
+		char freq_list[200], *token;
+		size_t flen = 0;
+		char *ch_list = strdup(chan_list);
+
+		if (!ch_list)
+			return ERROR_SEND_STATUS;
+
+		freq_list[0] = '\0';
+		token = strtok(ch_list, " ");
+		while (token && strlen(freq_list) < sizeof(freq_list) - 10) {
+			int chan, freq;
+
+			chan = atoi(token);
+			freq = channel_to_freq(dut, chan);
+			res = snprintf(freq_list, sizeof(freq_list) - flen,
+				       "%d,", freq);
+			if (res < 0 || res >= sizeof(freq_list) - flen)
+				return ERROR_SEND_STATUS;
+			flen += res;
+			token = strtok(NULL, " ");
+		}
+		free(ch_list);
+		if (flen > 0)
+			freq_list[flen - 1] = '\0';
+
+		res = snprintf(buf + len, sizeof(buf) - len, " freq_list=%s",
+			       freq_list);
+	} else {
+		res = snprintf(buf + len, sizeof(buf) - len, " freq_list=5180");
+	}
+	if (res < 0 || res >= sizeof(buf) - len)
+		return ERROR_SEND_STATUS;
+	len += res;
+
+	if (wpa_command(ifname, buf)) {
+		send_resp(dut, conn, SIGMA_ERROR, "Unable to USD subscribe");
+		return STATUS_SENT_ERROR;
+	}
+
+	return SUCCESS_SEND_STATUS;
+}
+
+
+enum sigma_cmd_result usd_cmd_sta_exec_action(struct sigma_dut *dut,
+					      struct sigma_conn *conn,
+					      struct sigma_cmd *cmd)
+{
+	int ret;
+	u8 len;
+	char buf[200];
+	const char *ifname = get_param(cmd, "interface");
+	const char *method_type = get_param(cmd, "MethodType");
+	const char *pairing_bootstrap_method;
+	int bootstrap = 0;
+
+	if (!ifname)
+		ifname = get_station_ifname(dut);
+
+	if (!dut->usd_enabled) {
+		send_resp(dut, conn, SIGMA_ERROR, "errorCode,USD not enabled");
+		return STATUS_SENT;
+	}
+
+	pairing_bootstrap_method = get_param(cmd, "PairingBootstrapMethod");
+	if (pairing_bootstrap_method) {
+		if (strcasecmp(pairing_bootstrap_method, "NonZero") == 0) {
+			/* opportunistic */
+			bootstrap = BIT(0);
+			/* Display pincode and passphrase */
+			bootstrap |= BIT(1) | BIT(2);
+			/* Keypad pincode and passphrase */
+			bootstrap |= BIT(5) | BIT(6);
+		} else {
+			bootstrap = atoi(pairing_bootstrap_method);
+		}
+
+		len = snprintf(buf, sizeof(buf),
+			       "P2P_SET supported_bootstrapmethods %d",
+			       bootstrap);
+		if (len < 0 || len >= sizeof(buf))
+			return ERROR_SEND_STATUS;
+		sigma_dut_print(dut, DUT_MSG_INFO, "PP2_SET to wpa_command: %s",
+				buf);
+		if (wpa_command(ifname, buf)) {
+			send_resp(dut, conn, SIGMA_ERROR,
+				  "errorCode,Unable to set pairing bootstrap method");
+			return STATUS_SENT_ERROR;
+		}
+	}
+
+	if (!method_type) {
+		send_resp(dut, conn, SIGMA_ERROR,
+			  "errorCode,Missing USD Type");
+		return STATUS_SENT_ERROR;
+	}
+
+	if (strcasecmp(method_type, "ADVERTISE") == 0) {
+		ret = sigma_usd_publish(dut, conn, cmd);
+		if (ret < 0)
+			return ret;
+		send_resp(dut, conn, SIGMA_COMPLETE, "NULL");
+		return STATUS_SENT;
+	}
+
+	if (strcasecmp(method_type, "SEEK") == 0) {
+		ret = sigma_usd_subscribe(dut, conn, cmd);
+		if (ret < 0)
+			return ret;
+		send_resp(dut, conn, SIGMA_COMPLETE, "NULL");
+		return STATUS_SENT;
+	}
+
+	send_resp(dut, conn, SIGMA_ERROR,
+		  "errorCode,Unsupported USD MethodType");
+	return STATUS_SENT;
+}

sigma_dut.h

@@ -1265,6 +1265,7 @@ struct sigma_dut {
 	int group_ciphers_capa; /* bitmap of enum sigma_cipher_suites values */
 	int group_mgmt_ciphers_capa; /* bitmap of enum sigma_cipher_suites
 				      * values */
+	bool usd_enabled;
 };
 
 
@@ -1564,4 +1565,9 @@ enum sigma_cmd_result dev_start_test_log(struct sigma_dut *dut,
 /* dnssd.c */
 int mdnssd_init(struct sigma_dut *dut);
 
+/* p2p_usd.c */
+enum sigma_cmd_result usd_cmd_sta_exec_action(struct sigma_dut *dut,
+					      struct sigma_conn *conn,
+					      struct sigma_cmd *cmd);
+
 #endif /* SIGMA_DUT_H */