wpa_supplicant(hidl): P2P Iface implementation

Implementation of the methods exposed in the ISupplicantP2pIface hidl
interface. This is a subset of all the P2P control parameters exposed by
wpa_supplicant used by Android framework. So, many of the internal
methods calls are invoked with default values retrieved from
ctrl_iface.c (old socket interface).

Bug: 31497295
Test: Compiles
Change-Id: I5077419fb38e3c5b9162139127caea4487d5777e
diff --git a/wpa_supplicant/hidl/hidl_manager.h b/wpa_supplicant/hidl/hidl_manager.h
index 5f1c7b6..9acaa00 100644
--- a/wpa_supplicant/hidl/hidl_manager.h
+++ b/wpa_supplicant/hidl/hidl_manager.h
@@ -314,6 +314,41 @@
 	WPA_COMPLETED,
     "State value mismatch");
 
+static_assert(
+    static_cast<uint32_t>(
+	ISupplicantP2pIface::GroupCapabilityMask::GROUP_OWNER) ==
+	P2P_GROUP_CAPAB_GROUP_OWNER,
+    "P2P capability value mismatch");
+static_assert(
+    static_cast<uint32_t>(
+	ISupplicantP2pIface::GroupCapabilityMask::PERSISTENT_GROUP) ==
+	P2P_GROUP_CAPAB_PERSISTENT_GROUP,
+    "P2P capability value mismatch");
+static_assert(
+    static_cast<uint32_t>(
+	ISupplicantP2pIface::GroupCapabilityMask::GROUP_LIMIT) ==
+	P2P_GROUP_CAPAB_GROUP_LIMIT,
+    "P2P capability value mismatch");
+static_assert(
+    static_cast<uint32_t>(
+	ISupplicantP2pIface::GroupCapabilityMask::INTRA_BSS_DIST) ==
+	P2P_GROUP_CAPAB_INTRA_BSS_DIST,
+    "P2P capability value mismatch");
+static_assert(
+    static_cast<uint32_t>(
+	ISupplicantP2pIface::GroupCapabilityMask::CROSS_CONN) ==
+	P2P_GROUP_CAPAB_CROSS_CONN,
+    "P2P capability value mismatch");
+static_assert(
+    static_cast<uint32_t>(
+	ISupplicantP2pIface::GroupCapabilityMask::PERSISTENT_RECONN) ==
+	P2P_GROUP_CAPAB_PERSISTENT_RECONN,
+    "P2P capability value mismatch");
+static_assert(
+    static_cast<uint32_t>(
+	ISupplicantP2pIface::GroupCapabilityMask::GROUP_FORMATION) ==
+	P2P_GROUP_CAPAB_GROUP_FORMATION,
+    "P2P capability value mismatch");
 }  // namespace implementation
 }  // namespace V1_0
 }  // namespace wifi
diff --git a/wpa_supplicant/hidl/p2p_iface.cpp b/wpa_supplicant/hidl/p2p_iface.cpp
index 9c0b3f4..6ad0696 100644
--- a/wpa_supplicant/hidl/p2p_iface.cpp
+++ b/wpa_supplicant/hidl/p2p_iface.cpp
@@ -11,6 +11,12 @@
 #include "hidl_return_util.h"
 #include "p2p_iface.h"
 
+namespace {
+const char kConfigMethodStrPbc[] = "pbc";
+const char kConfigMethodStrDisplay[] = "display";
+const char kConfigMethodStrKeypad[] = "keypad";
+}  // namespace
+
 namespace android {
 namespace hardware {
 namespace wifi {
@@ -90,7 +96,7 @@
 }
 
 Return<void> P2pIface::setSsidPostfix(
-    const hidl_string& postfix, setSsidPostfix_cb _hidl_cb)
+    const hidl_vec<uint8_t>& postfix, setSsidPostfix_cb _hidl_cb)
 {
 	return validateAndCall(
 	    this, SupplicantStatusCode::FAILURE_IFACE_INVALID,
@@ -98,18 +104,21 @@
 }
 
 Return<void> P2pIface::setGroupIdle(
-    uint32_t timeout_in_sec, setGroupIdle_cb _hidl_cb)
+    const hidl_string& group_ifname, uint32_t timeout_in_sec,
+    setGroupIdle_cb _hidl_cb)
 {
 	return validateAndCall(
 	    this, SupplicantStatusCode::FAILURE_IFACE_INVALID,
-	    &P2pIface::setGroupIdleInternal, _hidl_cb, timeout_in_sec);
+	    &P2pIface::setGroupIdleInternal, _hidl_cb, group_ifname,
+	    timeout_in_sec);
 }
 
-Return<void> P2pIface::setPowerSave(bool enable, setPowerSave_cb _hidl_cb)
+Return<void> P2pIface::setPowerSave(
+    const hidl_string& group_ifname, bool enable, setPowerSave_cb _hidl_cb)
 {
 	return validateAndCall(
 	    this, SupplicantStatusCode::FAILURE_IFACE_INVALID,
-	    &P2pIface::setPowerSaveInternal, _hidl_cb, enable);
+	    &P2pIface::setPowerSaveInternal, _hidl_cb, group_ifname, enable);
 }
 
 Return<void> P2pIface::find(uint32_t timeout_in_sec, find_cb _hidl_cb)
@@ -136,7 +145,7 @@
 Return<void> P2pIface::connect(
     const hidl_array<uint8_t, 6>& peer_address,
     ISupplicantP2pIface::WpsProvisionMethod provision_method,
-    const hidl_vec<uint8_t>& pre_selected_pin, bool join_existing_group,
+    const hidl_string& pre_selected_pin, bool join_existing_group,
     bool persistent, uint32_t go_intent, connect_cb _hidl_cb)
 {
 	return validateAndCall(
@@ -165,7 +174,8 @@
 }
 
 Return<void> P2pIface::addGroup(
-    bool persistent, uint32_t persistent_network_id, addGroup_cb _hidl_cb)
+    bool persistent, SupplicantNetworkId persistent_network_id,
+    addGroup_cb _hidl_cb)
 {
 	return validateAndCall(
 	    this, SupplicantStatusCode::FAILURE_IFACE_INVALID,
@@ -201,8 +211,8 @@
 }
 
 Return<void> P2pIface::reinvoke(
-    uint32_t persistent_network_id, const hidl_array<uint8_t, 6>& peer_address,
-    reinvoke_cb _hidl_cb)
+    SupplicantNetworkId persistent_network_id,
+    const hidl_array<uint8_t, 6>& peer_address, reinvoke_cb _hidl_cb)
 {
 	return validateAndCall(
 	    this, SupplicantStatusCode::FAILURE_IFACE_INVALID,
@@ -229,6 +239,14 @@
 	    operating_class);
 }
 
+Return<void> P2pIface::setDisallowedFrequencies(
+    const hidl_vec<FreqRange>& ranges, setDisallowedFrequencies_cb _hidl_cb)
+{
+	return validateAndCall(
+	    this, SupplicantStatusCode::FAILURE_IFACE_INVALID,
+	    &P2pIface::setDisallowedFrequenciesInternal, _hidl_cb, ranges);
+}
+
 Return<void> P2pIface::getSsid(
     const hidl_array<uint8_t, 6>& peer_address, getSsid_cb _hidl_cb)
 {
@@ -394,59 +412,133 @@
 std::pair<SupplicantStatus, std::array<uint8_t, 6>>
 P2pIface::getDeviceAddressInternal()
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	std::array<uint8_t, 6> addr;
+	static_assert(ETH_ALEN == addr.size(), "Size mismatch");
+	os_memcpy(addr.data(), wpa_s->global->p2p_dev_addr, ETH_ALEN);
 	return {{SupplicantStatusCode::SUCCESS, ""}, {}};
 }
 
-SupplicantStatus P2pIface::setSsidPostfixInternal(const hidl_string& postfix)
+SupplicantStatus P2pIface::setSsidPostfixInternal(
+    const std::vector<uint8_t>& postfix)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (p2p_set_ssid_postfix(
+		wpa_s->global->p2p, postfix.data(), postfix.size())) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
-SupplicantStatus P2pIface::setGroupIdleInternal(uint32_t timeout_in_sec)
+SupplicantStatus P2pIface::setGroupIdleInternal(
+    const std::string& group_ifname, uint32_t timeout_in_sec)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_group_s =
+	    retrieveGroupIfacePtr(group_ifname);
+	if (!wpa_group_s) {
+		return {SupplicantStatusCode::FAILURE_IFACE_UNKNOWN, ""};
+	}
+	wpa_group_s->conf->p2p_group_idle = timeout_in_sec;
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
-SupplicantStatus P2pIface::setPowerSaveInternal(bool enable)
+SupplicantStatus P2pIface::setPowerSaveInternal(
+    const std::string& group_ifname, bool enable)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_group_s =
+	    retrieveGroupIfacePtr(group_ifname);
+	if (!wpa_group_s) {
+		return {SupplicantStatusCode::FAILURE_IFACE_UNKNOWN, ""};
+	}
+	if (wpa_drv_set_p2p_powersave(wpa_group_s, enable, -1, -1)) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::findInternal(uint32_t timeout_in_sec)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpa_s->wpa_state == WPA_INTERFACE_DISABLED) {
+		return {SupplicantStatusCode::FAILURE_IFACE_DISABLED, ""};
+	}
+	uint32_t search_delay = wpas_p2p_search_delay(wpa_s);
+	if (wpas_p2p_find(
+		wpa_s, timeout_in_sec, P2P_FIND_START_WITH_FULL, 0, nullptr,
+		nullptr, search_delay, 0, nullptr, 0)) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::stopFindInternal()
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpa_s->wpa_state == WPA_INTERFACE_DISABLED) {
+		return {SupplicantStatusCode::FAILURE_IFACE_DISABLED, ""};
+	}
+	wpas_p2p_stop_find(wpa_s);
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::flushInternal()
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	os_memset(wpa_s->p2p_auth_invite, 0, ETH_ALEN);
+	wpa_s->force_long_sd = 0;
+	wpas_p2p_stop_find(wpa_s);
+	wpa_s->parent->p2ps_method_config_any = 0;
+	if (wpa_s->global->p2p)
+		p2p_flush(wpa_s->global->p2p);
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
-std::pair<SupplicantStatus, std::vector<uint8_t>> P2pIface::connectInternal(
+// This method only implements support for subset (needed by Android framework)
+// of parameters that can be specified for connect.
+std::pair<SupplicantStatus, std::string> P2pIface::connectInternal(
     const std::array<uint8_t, 6>& peer_address,
     ISupplicantP2pIface::WpsProvisionMethod provision_method,
-    const std::vector<uint8_t>& pre_selected_pin, bool join_existing_group,
+    const std::string& pre_selected_pin, bool join_existing_group,
     bool persistent, uint32_t go_intent)
 {
-	// TODO: Add implementation.
-	return {{SupplicantStatusCode::SUCCESS, ""}, {}};
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (go_intent > 15) {
+		return {{SupplicantStatusCode::FAILURE_ARGS_INVALID, ""}, {}};
+	}
+	p2p_wps_method wps_method = {};
+	switch (provision_method) {
+	case WpsProvisionMethod::PBC:
+		wps_method = WPS_PBC;
+		break;
+	case WpsProvisionMethod::DISPLAY:
+		wps_method = WPS_PIN_DISPLAY;
+		break;
+	case WpsProvisionMethod::KEYPAD:
+		wps_method = WPS_PIN_KEYPAD;
+		break;
+	}
+	int new_pin = wpas_p2p_connect(
+	    wpa_s, peer_address.data(), pre_selected_pin.data(), wps_method,
+	    persistent, false, join_existing_group, false, go_intent, 0, 0, -1,
+	    false, false, false, VHT_CHANWIDTH_USE_HT, nullptr, 0);
+	if (new_pin < 0) {
+		return {{SupplicantStatusCode::FAILURE_UNKNOWN, ""}, {}};
+	}
+	std::string pin_ret;
+	if (provision_method == WpsProvisionMethod::DISPLAY &&
+	    pre_selected_pin.empty()) {
+		pin_ret.reserve(9);
+		snprintf(&pin_ret[0], pin_ret.size(), "%08d", new_pin);
+	}
+	return {{SupplicantStatusCode::SUCCESS, ""}, pin_ret};
 }
 
 SupplicantStatus P2pIface::cancelConnectInternal()
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpas_p2p_cancel(wpa_s)) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
@@ -454,106 +546,266 @@
     const std::array<uint8_t, 6>& peer_address,
     ISupplicantP2pIface::WpsProvisionMethod provision_method)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	p2ps_provision* prov_param;
+	const char* config_method_str = nullptr;
+	switch (provision_method) {
+	case WpsProvisionMethod::PBC:
+		config_method_str = kConfigMethodStrPbc;
+		break;
+	case WpsProvisionMethod::DISPLAY:
+		config_method_str = kConfigMethodStrDisplay;
+		break;
+	case WpsProvisionMethod::KEYPAD:
+		config_method_str = kConfigMethodStrKeypad;
+		break;
+	}
+	if (wpas_p2p_prov_disc(
+		wpa_s, peer_address.data(), config_method_str,
+		WPAS_P2P_PD_FOR_GO_NEG, nullptr)) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::addGroupInternal(
-    bool persistent, uint32_t persistent_network_id)
+    bool persistent, SupplicantNetworkId persistent_network_id)
 {
-	// TODO: Add implementation.
-	return {SupplicantStatusCode::SUCCESS, ""};
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	int vht = wpa_s->conf->p2p_go_vht;
+	int ht40 = wpa_s->conf->p2p_go_ht40 || vht;
+	struct wpa_ssid* ssid =
+	    wpa_config_get_network(wpa_s->conf, persistent_network_id);
+	if (ssid == NULL) {
+		if (wpas_p2p_group_add(
+			wpa_s, persistent, 0, 0, ht40, vht,
+			VHT_CHANWIDTH_USE_HT)) {
+			return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+		} else {
+			return {SupplicantStatusCode::SUCCESS, ""};
+		}
+	} else if (ssid->disabled == 2) {
+		if (wpas_p2p_group_add_persistent(
+			wpa_s, ssid, 0, 0, 0, 0, ht40, vht,
+			VHT_CHANWIDTH_USE_HT, NULL, 0, 0)) {
+			return {SupplicantStatusCode::FAILURE_NETWORK_UNKNOWN,
+				""};
+		} else {
+			return {SupplicantStatusCode::SUCCESS, ""};
+		}
+	}
+	return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
 }
 
-SupplicantStatus P2pIface::removeGroupInternal(const hidl_string& group_ifname)
+SupplicantStatus P2pIface::removeGroupInternal(const std::string& group_ifname)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_group_s =
+	    retrieveGroupIfacePtr(group_ifname);
+	if (!wpa_group_s) {
+		return {SupplicantStatusCode::FAILURE_IFACE_UNKNOWN, ""};
+	}
+	if (wpas_p2p_group_remove(wpa_group_s, group_ifname.c_str())) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::rejectInternal(
     const std::array<uint8_t, 6>& peer_address)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) {
+		return {SupplicantStatusCode::FAILURE_IFACE_DISABLED, ""};
+	}
+	if (wpas_p2p_reject(wpa_s, peer_address.data())) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::inviteInternal(
-    const hidl_string& group_ifname,
+    const std::string& group_ifname,
     const std::array<uint8_t, 6>& go_device_address,
     const std::array<uint8_t, 6>& peer_address)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpas_p2p_invite_group(
+		wpa_s, group_ifname.c_str(), peer_address.data(),
+		go_device_address.data())) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::reinvokeInternal(
-    uint32_t persistent_network_id, const std::array<uint8_t, 6>& peer_address)
+    SupplicantNetworkId persistent_network_id,
+    const std::array<uint8_t, 6>& peer_address)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	int vht = wpa_s->conf->p2p_go_vht;
+	int ht40 = wpa_s->conf->p2p_go_ht40 || vht;
+	struct wpa_ssid* ssid =
+	    wpa_config_get_network(wpa_s->conf, persistent_network_id);
+	if (ssid == NULL || ssid->disabled != 2) {
+		return {SupplicantStatusCode::FAILURE_NETWORK_UNKNOWN, ""};
+	}
+	if (wpas_p2p_invite(
+		wpa_s, peer_address.data(), ssid, NULL, 0, 0, ht40, vht,
+		VHT_CHANWIDTH_USE_HT, 0)) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::configureExtListenInternal(
     bool enable, uint32_t period_in_millis, uint32_t interval_in_millis)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpas_p2p_ext_listen(wpa_s, period_in_millis, interval_in_millis)) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::setListenChannelInternal(
     uint32_t channel, uint32_t operating_class)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (p2p_set_listen_channel(
+		wpa_s->global->p2p, operating_class, channel, 1)) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
+	return {SupplicantStatusCode::SUCCESS, ""};
+}
+
+SupplicantStatus P2pIface::setDisallowedFrequenciesInternal(
+    const std::vector<FreqRange>& ranges)
+{
+	if (ranges.size() == 0) {
+		return {SupplicantStatusCode::FAILURE_ARGS_INVALID, ""};
+	}
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	using DestT = struct wpa_freq_range_list::wpa_freq_range;
+	DestT* freq_ranges =
+	    static_cast<DestT*>(os_malloc(sizeof(DestT) * ranges.size()));
+	if (!freq_ranges) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
+	uint32_t i = 0;
+	for (const auto& range : ranges) {
+		freq_ranges[i].min = range.min;
+		freq_ranges[i].max = range.max;
+		i++;
+	}
+
+	os_free(wpa_s->global->p2p_disallow_freq.range);
+	wpa_s->global->p2p_disallow_freq.range = freq_ranges;
+	wpa_s->global->p2p_disallow_freq.num = ranges.size();
+	wpas_p2p_update_channel_list(wpa_s, WPAS_P2P_CHANNEL_UPDATE_DISALLOW);
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 std::pair<SupplicantStatus, std::vector<uint8_t>> P2pIface::getSsidInternal(
     const std::array<uint8_t, 6>& peer_address)
 {
-	// TODO: Add implementation.
-	return {{SupplicantStatusCode::SUCCESS, ""}, {}};
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	const struct p2p_peer_info* info =
+	    p2p_get_peer_info(wpa_s->global->p2p, peer_address.data(), 0);
+	if (!info) {
+		return {{SupplicantStatusCode::FAILURE_UNKNOWN, ""}, {}};
+	}
+	const struct p2p_device* dev =
+	    reinterpret_cast<const struct p2p_device*>(
+		(reinterpret_cast<const uint8_t*>(info)) -
+		offsetof(struct p2p_device, info));
+	std::vector<uint8_t> ssid;
+	if (dev && dev->oper_ssid_len) {
+		ssid.assign(
+		    dev->oper_ssid, dev->oper_ssid + dev->oper_ssid_len);
+	}
+	return {{SupplicantStatusCode::SUCCESS, ""}, ssid};
 }
 
 std::pair<SupplicantStatus, uint32_t> P2pIface::getGroupCapabilityInternal(
     const std::array<uint8_t, 6>& peer_address)
 {
-	// TODO: Add implementation.
-	return {{SupplicantStatusCode::SUCCESS, ""}, 0};
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	const struct p2p_peer_info* info =
+	    p2p_get_peer_info(wpa_s->global->p2p, peer_address.data(), 0);
+	if (!info) {
+		return {{SupplicantStatusCode::FAILURE_UNKNOWN, ""}, {}};
+	}
+	return {{SupplicantStatusCode::SUCCESS, ""}, info->group_capab};
 }
 
 SupplicantStatus P2pIface::addBonjourServiceInternal(
     const std::vector<uint8_t>& query, const std::vector<uint8_t>& response)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	struct wpabuf* query_buf = wpabuf_alloc(query.size());
+	if (!query_buf) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
+	wpabuf_put_data(query_buf, query.data(), query.size());
+
+	struct wpabuf* response_buf = wpabuf_alloc(response.size());
+	if (!query_buf) {
+		wpabuf_free(query_buf);
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
+	wpabuf_put_data(response_buf, response.data(), response.size());
+
+	if (wpas_p2p_service_add_bonjour(wpa_s, query_buf, response_buf)) {
+		wpabuf_free(query_buf);
+		wpabuf_free(response_buf);
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::removeBonjourServiceInternal(
     const std::vector<uint8_t>& query)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	struct wpabuf* query_buf = wpabuf_alloc(query.size());
+	if (!query_buf) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
+	wpabuf_put_data(query_buf, query.data(), query.size());
+
+	int ret = wpas_p2p_service_del_bonjour(wpa_s, query_buf);
+	wpabuf_free(query_buf);
+	if (ret) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::addUpnpServiceInternal(
-    uint32_t version, const hidl_string& service_name)
+    uint32_t version, const std::string& service_name)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpas_p2p_service_add_upnp(wpa_s, version, service_name.c_str())) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::removeUpnpServiceInternal(
-    uint32_t version, const hidl_string& service_name)
+    uint32_t version, const std::string& service_name)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpas_p2p_service_del_upnp(wpa_s, version, service_name.c_str())) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
 SupplicantStatus P2pIface::flushServicesInternal(
-    uint32_t version, const hidl_string& service_name)
+    uint32_t version, const std::string& service_name)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	wpas_p2p_service_flush(wpa_s);
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
@@ -561,13 +813,27 @@
     const std::array<uint8_t, 6>& peer_address,
     const std::vector<uint8_t>& query)
 {
-	// TODO: Add implementation.
-	return {{SupplicantStatusCode::SUCCESS, ""}, 0};
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	struct wpabuf* query_buf = wpabuf_alloc(query.size());
+	if (!query_buf) {
+		return {{SupplicantStatusCode::FAILURE_UNKNOWN, ""}, {}};
+	}
+	wpabuf_put_data(query_buf, query.data(), query.size());
+	uint64_t identifier =
+	    wpas_p2p_sd_request(wpa_s, peer_address.data(), query_buf);
+	wpabuf_free(query_buf);
+	if (identifier == 0) {
+		return {{SupplicantStatusCode::FAILURE_UNKNOWN, ""}, {}};
+	}
+	return {{SupplicantStatusCode::SUCCESS, ""}, identifier};
 }
 
 SupplicantStatus P2pIface::cancelServiceDiscoveryInternal(uint64_t identifier)
 {
-	// TODO: Add implementation.
+	struct wpa_supplicant* wpa_s = retrieveIfacePtr();
+	if (wpas_p2p_sd_cancel_request(wpa_s, identifier)) {
+		return {SupplicantStatusCode::FAILURE_UNKNOWN, ""};
+	}
 	return {SupplicantStatusCode::SUCCESS, ""};
 }
 
@@ -579,8 +845,16 @@
  */
 wpa_supplicant* P2pIface::retrieveIfacePtr()
 {
-	return wpa_supplicant_get_iface(
-	    (struct wpa_global*)wpa_global_, ifname_.c_str());
+	return wpa_supplicant_get_iface(wpa_global_, ifname_.c_str());
+}
+
+/**
+ * Retrieve the underlying |wpa_supplicant| struct
+ * pointer for this group iface.
+ */
+wpa_supplicant* P2pIface::retrieveGroupIfacePtr(const std::string& group_ifname)
+{
+	return wpa_supplicant_get_iface(wpa_global_, group_ifname.c_str());
 }
 
 }  // namespace implementation
diff --git a/wpa_supplicant/hidl/p2p_iface.h b/wpa_supplicant/hidl/p2p_iface.h
index d340b08..312b8bb 100644
--- a/wpa_supplicant/hidl/p2p_iface.h
+++ b/wpa_supplicant/hidl/p2p_iface.h
@@ -22,9 +22,11 @@
 extern "C" {
 #include "utils/common.h"
 #include "utils/includes.h"
-#include "wpa_supplicant_i.h"
+#include "p2p/p2p.h"
+#include "p2p/p2p_i.h"
+#include "p2p_supplicant.h"
+#include "p2p_supplicant.h"
 #include "config.h"
-#include "driver_i.h"
 }
 
 namespace android {
@@ -63,18 +65,21 @@
 	    registerCallback_cb _hidl_cb) override;
 	Return<void> getDeviceAddress(getDeviceAddress_cb _hidl_cb) override;
 	Return<void> setSsidPostfix(
-	    const hidl_string& postfix, setSsidPostfix_cb _hidl_cb) override;
+	    const hidl_vec<uint8_t>& postfix,
+	    setSsidPostfix_cb _hidl_cb) override;
 	Return<void> setGroupIdle(
-	    uint32_t timeout_in_sec, setGroupIdle_cb _hidl_cb) override;
+	    const hidl_string& group_ifname, uint32_t timeout_in_sec,
+	    setGroupIdle_cb _hidl_cb) override;
 	Return<void> setPowerSave(
-	    bool enable, setPowerSave_cb _hidl_cb) override;
+	    const hidl_string& group_ifname, bool enable,
+	    setPowerSave_cb _hidl_cb) override;
 	Return<void> find(uint32_t timeout_in_sec, find_cb _hidl_cb) override;
 	Return<void> stopFind(stopFind_cb _hidl_cb) override;
 	Return<void> flush(flush_cb _hidl_cb) override;
 	Return<void> connect(
 	    const hidl_array<uint8_t, 6>& peer_address,
 	    ISupplicantP2pIface::WpsProvisionMethod provision_method,
-	    const hidl_vec<uint8_t>& pre_selected_pin, bool join_existing_group,
+	    const hidl_string& pre_selected_pin, bool join_existing_group,
 	    bool persistent, uint32_t go_intent, connect_cb _hidl_cb) override;
 	Return<void> cancelConnect(cancelConnect_cb _hidl_cb) override;
 	Return<void> provisionDiscovery(
@@ -82,7 +87,7 @@
 	    ISupplicantP2pIface::WpsProvisionMethod provision_method,
 	    provisionDiscovery_cb _hidl_cb) override;
 	Return<void> addGroup(
-	    bool persistent, uint32_t persistent_network_id,
+	    bool persistent, SupplicantNetworkId persistent_network_id,
 	    addGroup_cb _hidl_cb) override;
 	Return<void> removeGroup(
 	    const hidl_string& group_ifname, removeGroup_cb _hidl_cb) override;
@@ -95,7 +100,7 @@
 	    const hidl_array<uint8_t, 6>& peer_address,
 	    invite_cb _hidl_cb) override;
 	Return<void> reinvoke(
-	    uint32_t persistent_network_id,
+	    SupplicantNetworkId persistent_network_id,
 	    const hidl_array<uint8_t, 6>& peer_address,
 	    reinvoke_cb _hidl_cb) override;
 	Return<void> configureExtListen(
@@ -104,6 +109,9 @@
 	Return<void> setListenChannel(
 	    uint32_t channel, uint32_t operating_class,
 	    setListenChannel_cb _hidl_cb) override;
+	Return<void> setDisallowedFrequencies(
+	    const hidl_vec<FreqRange>& ranges,
+	    setDisallowedFrequencies_cb _hidl_cb) override;
 	Return<void> getSsid(
 	    const hidl_array<uint8_t, 6>& peer_address,
 	    getSsid_cb _hidl_cb) override;
@@ -147,38 +155,43 @@
 	    const sp<ISupplicantP2pIfaceCallback>& callback);
 	std::pair<SupplicantStatus, std::array<uint8_t, 6>>
 	getDeviceAddressInternal();
-	SupplicantStatus setSsidPostfixInternal(const hidl_string& postfix);
-	SupplicantStatus setGroupIdleInternal(uint32_t timeout_in_sec);
-	SupplicantStatus setPowerSaveInternal(bool enable);
+	SupplicantStatus setSsidPostfixInternal(
+	    const std::vector<uint8_t>& postfix);
+	SupplicantStatus setGroupIdleInternal(
+	    const std::string& group_ifname, uint32_t timeout_in_sec);
+	SupplicantStatus setPowerSaveInternal(
+	    const std::string& group_ifname, bool enable);
 	SupplicantStatus findInternal(uint32_t timeout_in_sec);
 	SupplicantStatus stopFindInternal();
 	SupplicantStatus flushInternal();
-	std::pair<SupplicantStatus, std::vector<uint8_t>> connectInternal(
+	std::pair<SupplicantStatus, std::string> connectInternal(
 	    const std::array<uint8_t, 6>& peer_address,
 	    ISupplicantP2pIface::WpsProvisionMethod provision_method,
-	    const std::vector<uint8_t>& pre_selected_pin,
-	    bool join_existing_group, bool persistent, uint32_t go_intent);
+	    const std::string& pre_selected_pin, bool join_existing_group,
+	    bool persistent, uint32_t go_intent);
 	SupplicantStatus cancelConnectInternal();
 	SupplicantStatus provisionDiscoveryInternal(
 	    const std::array<uint8_t, 6>& peer_address,
 	    ISupplicantP2pIface::WpsProvisionMethod provision_method);
 	SupplicantStatus addGroupInternal(
-	    bool persistent, uint32_t persistent_network_id);
-	SupplicantStatus removeGroupInternal(const hidl_string& group_ifname);
+	    bool persistent, SupplicantNetworkId persistent_network_id);
+	SupplicantStatus removeGroupInternal(const std::string& group_ifname);
 	SupplicantStatus rejectInternal(
 	    const std::array<uint8_t, 6>& peer_address);
 	SupplicantStatus inviteInternal(
-	    const hidl_string& group_ifname,
+	    const std::string& group_ifname,
 	    const std::array<uint8_t, 6>& go_device_address,
 	    const std::array<uint8_t, 6>& peer_address);
 	SupplicantStatus reinvokeInternal(
-	    uint32_t persistent_network_id,
+	    SupplicantNetworkId persistent_network_id,
 	    const std::array<uint8_t, 6>& peer_address);
 	SupplicantStatus configureExtListenInternal(
 	    bool enable, uint32_t period_in_millis,
 	    uint32_t interval_in_millis);
 	SupplicantStatus setListenChannelInternal(
 	    uint32_t channel, uint32_t operating_class);
+	SupplicantStatus setDisallowedFrequenciesInternal(
+	    const std::vector<FreqRange>& ranges);
 	std::pair<SupplicantStatus, std::vector<uint8_t>> getSsidInternal(
 	    const std::array<uint8_t, 6>& peer_address);
 	std::pair<SupplicantStatus, uint32_t> getGroupCapabilityInternal(
@@ -189,21 +202,23 @@
 	SupplicantStatus removeBonjourServiceInternal(
 	    const std::vector<uint8_t>& query);
 	SupplicantStatus addUpnpServiceInternal(
-	    uint32_t version, const hidl_string& service_name);
+	    uint32_t version, const std::string& service_name);
 	SupplicantStatus removeUpnpServiceInternal(
-	    uint32_t version, const hidl_string& service_name);
+	    uint32_t version, const std::string& service_name);
 	SupplicantStatus flushServicesInternal(
-	    uint32_t version, const hidl_string& service_name);
+	    uint32_t version, const std::string& service_name);
 	std::pair<SupplicantStatus, uint64_t> requestServiceDiscoveryInternal(
 	    const std::array<uint8_t, 6>& peer_address,
 	    const std::vector<uint8_t>& query);
 	SupplicantStatus cancelServiceDiscoveryInternal(uint64_t identifier);
 
 	struct wpa_supplicant* retrieveIfacePtr();
+	struct wpa_supplicant* retrieveGroupIfacePtr(
+	    const std::string& group_ifname);
 
 	// Reference to the global wpa_struct. This is assumed to be valid for
 	// the lifetime of the process.
-	const struct wpa_global* wpa_global_;
+	struct wpa_global* wpa_global_;
 	// Name of the iface this hidl object controls
 	const std::string ifname_;
 	bool is_valid_;
diff --git a/wpa_supplicant/hidl/supplicant.cpp b/wpa_supplicant/hidl/supplicant.cpp
index aba4402..409547f 100644
--- a/wpa_supplicant/hidl/supplicant.cpp
+++ b/wpa_supplicant/hidl/supplicant.cpp
@@ -165,7 +165,15 @@
 
 SupplicantStatus Supplicant::setConcurrencyPriorityInternal(IfaceType type)
 {
-	// TODO: Add implementation.
+	if (type == IfaceType::STA) {
+		wpa_global_->conc_pref =
+		    wpa_global::wpa_conc_pref::WPA_CONC_PREF_STA;
+	} else if (type == IfaceType::P2P) {
+		wpa_global_->conc_pref =
+		    wpa_global::wpa_conc_pref::WPA_CONC_PREF_P2P;
+	} else {
+		return {SupplicantStatusCode::FAILURE_ARGS_INVALID, ""};
+	}
 	return SupplicantStatus{SupplicantStatusCode::SUCCESS, ""};
 }
 }  // namespace implementation