+static int p2p_tech_probe(struct connman_technology *technology)
+{
+ p2p_technology = technology;
+
+ return 0;
+}
+
+static void p2p_tech_remove(struct connman_technology *technology)
+{
+ p2p_technology = NULL;
+}
+
+static struct connman_technology_driver p2p_tech_driver = {
+ .name = "p2p",
+ .type = CONNMAN_SERVICE_TYPE_P2P,
+ .probe = p2p_tech_probe,
+ .remove = p2p_tech_remove,
+};
+
+static bool is_p2p_connecting(void)
+{
+ GList *list;
+
+ for (list = iface_list; list; list = list->next) {
+ struct wifi_data *wifi = list->data;
+
+ if (wifi->p2p_connecting)
+ return true;
+ }
+
+ return false;
+}
+
+static void add_pending_wifi_device(struct wifi_data *wifi)
+{
+ if (g_list_find(pending_wifi_device, wifi))
+ return;
+
+ pending_wifi_device = g_list_append(pending_wifi_device, wifi);
+}
+
+static struct wifi_data *get_pending_wifi_data(const char *ifname)
+{
+ GList *list;
+
+ for (list = pending_wifi_device; list; list = list->next) {
+ struct wifi_data *wifi;
+ const char *dev_name;
+
+ wifi = list->data;
+ if (!wifi || !wifi->device)
+ continue;
+
+ dev_name = connman_device_get_string(wifi->device, "Interface");
+ if (!g_strcmp0(ifname, dev_name)) {
+ pending_wifi_device = g_list_delete_link(
+ pending_wifi_device, list);
+ return wifi;
+ }
+ }
+
+ return NULL;
+}
+
+static void remove_pending_wifi_device(struct wifi_data *wifi)
+{
+ GList *link;
+
+ link = g_list_find(pending_wifi_device, wifi);
+
+ if (!link)
+ return;
+
+ pending_wifi_device = g_list_delete_link(pending_wifi_device, link);
+}
+
+static void peer_cancel_timeout(struct wifi_data *wifi)
+{
+ if (wifi->p2p_connection_timeout > 0)
+ g_source_remove(wifi->p2p_connection_timeout);
+
+ wifi->p2p_connection_timeout = 0;
+ wifi->p2p_connecting = false;
+
+ if (wifi->pending_peer) {
+ connman_peer_unref(wifi->pending_peer);
+ wifi->pending_peer = NULL;
+ }
+
+ wifi->peer = NULL;
+}
+
+static gboolean peer_connect_timeout(gpointer data)
+{
+ struct wifi_data *wifi = data;
+
+ DBG("");
+
+ if (wifi->p2p_connecting) {
+ enum connman_peer_state state = CONNMAN_PEER_STATE_FAILURE;
+
+ if (g_supplicant_peer_has_requested_connection(wifi->peer))
+ state = CONNMAN_PEER_STATE_IDLE;
+
+ connman_peer_set_state(wifi->pending_peer, state);
+ }
+
+ peer_cancel_timeout(wifi);
+
+ return FALSE;
+}
+
+static void peer_connect_callback(int result, GSupplicantInterface *interface,
+ void *user_data)
+{
+ struct wifi_data *wifi = user_data;
+ struct connman_peer *peer = wifi->pending_peer;
+
+ DBG("peer %p - %d", peer, result);
+
+ if (!peer)
+ return;
+
+ if (result < 0) {
+ peer_connect_timeout(wifi);
+ return;
+ }
+
+ connman_peer_set_state(peer, CONNMAN_PEER_STATE_ASSOCIATION);
+
+ wifi->p2p_connection_timeout = g_timeout_add_seconds(
+ P2P_CONNECTION_TIMEOUT,
+ peer_connect_timeout, wifi);
+}
+
+static int peer_connect(struct connman_peer *peer,
+ enum connman_peer_wps_method wps_method,
+ const char *wps_pin)
+{
+ struct connman_device *device = connman_peer_get_device(peer);
+ GSupplicantPeerParams *peer_params;
+ GSupplicantPeer *gs_peer;
+ struct wifi_data *wifi;
+ bool pbc, pin;
+ int ret;
+
+ DBG("peer %p", peer);
+
+ if (!device)
+ return -ENODEV;
+
+ wifi = connman_device_get_data(device);
+ if (!wifi)
+ return -ENODEV;
+
+ if (wifi->p2p_connecting)
+ return -EBUSY;
+
+ wifi->peer = NULL;
+
+ gs_peer = g_supplicant_interface_peer_lookup(wifi->interface,
+ connman_peer_get_identifier(peer));
+ if (!gs_peer)
+ return -EINVAL;
+
+ pbc = g_supplicant_peer_is_wps_pbc(gs_peer);
+ pin = g_supplicant_peer_is_wps_pin(gs_peer);
+
+ switch (wps_method) {
+ case CONNMAN_PEER_WPS_UNKNOWN:
+ if ((pbc && pin) || pin)
+ return -ENOKEY;
+ break;
+ case CONNMAN_PEER_WPS_PBC:
+ if (!pbc)
+ return -EINVAL;
+ wps_pin = NULL;
+ break;
+ case CONNMAN_PEER_WPS_PIN:
+ if (!pin || !wps_pin)
+ return -EINVAL;
+ break;
+ }
+
+ peer_params = g_try_malloc0(sizeof(GSupplicantPeerParams));
+ if (!peer_params)
+ return -ENOMEM;
+
+ peer_params->path = g_strdup(g_supplicant_peer_get_path(gs_peer));
+ if (wps_pin)
+ peer_params->wps_pin = g_strdup(wps_pin);
+
+ peer_params->master = connman_peer_service_is_master();
+
+ ret = g_supplicant_interface_p2p_connect(wifi->interface, peer_params,
+ peer_connect_callback, wifi);
+ if (ret == -EINPROGRESS) {
+ wifi->pending_peer = connman_peer_ref(peer);
+ wifi->peer = gs_peer;
+ wifi->p2p_connecting = true;
+ } else if (ret < 0)
+ g_free(peer_params);
+
+ return ret;
+}
+
+static int peer_disconnect(struct connman_peer *peer)
+{
+ struct connman_device *device = connman_peer_get_device(peer);
+ GSupplicantPeerParams peer_params = {};
+ GSupplicantPeer *gs_peer;
+ struct wifi_data *wifi;
+ int ret;
+
+ DBG("peer %p", peer);
+
+ if (!device)
+ return -ENODEV;
+
+ wifi = connman_device_get_data(device);
+ if (!wifi)
+ return -ENODEV;
+
+ gs_peer = g_supplicant_interface_peer_lookup(wifi->interface,
+ connman_peer_get_identifier(peer));
+ if (!gs_peer)
+ return -EINVAL;
+
+ peer_params.path = g_strdup(g_supplicant_peer_get_path(gs_peer));
+
+ ret = g_supplicant_interface_p2p_disconnect(wifi->interface,
+ &peer_params);
+ g_free(peer_params.path);
+
+ if (ret == -EINPROGRESS)
+ peer_cancel_timeout(wifi);
+
+ return ret;
+}
+
+struct peer_service_registration {
+ peer_service_registration_cb_t callback;
+ void *user_data;
+};
+
+static bool is_service_wfd(const unsigned char *specs, int length)
+{
+ if (length < 9 || specs[0] != 0 || specs[1] != 0 || specs[2] != 6)
+ return false;
+
+ return true;
+}
+
+static void apply_p2p_listen_on_iface(gpointer data, gpointer user_data)
+{
+ struct wifi_data *wifi = data;
+
+ if (!wifi->interface ||
+ !g_supplicant_interface_has_p2p(wifi->interface))
+ return;
+
+ if (!wifi->servicing) {
+ g_supplicant_interface_p2p_listen(wifi->interface,
+ P2P_LISTEN_PERIOD, P2P_LISTEN_INTERVAL);
+ }
+
+ wifi->servicing++;
+}
+
+static void register_wfd_service_cb(int result,
+ GSupplicantInterface *iface, void *user_data)
+{
+ struct peer_service_registration *reg_data = user_data;
+
+ DBG("");
+
+ if (result == 0)
+ g_list_foreach(iface_list, apply_p2p_listen_on_iface, NULL);
+
+ if (reg_data && reg_data->callback) {
+ reg_data->callback(result, reg_data->user_data);
+ g_free(reg_data);
+ }
+}
+
+static GSupplicantP2PServiceParams *fill_in_peer_service_params(
+ const unsigned char *spec,
+ int spec_length, const unsigned char *query,
+ int query_length, int version)
+{
+ GSupplicantP2PServiceParams *params;
+
+ params = g_try_malloc0(sizeof(GSupplicantP2PServiceParams));
+ if (!params)
+ return NULL;
+
+ if (version > 0) {
+ params->version = version;
+ params->service = g_memdup(spec, spec_length);
+ } else if (query_length > 0 && spec_length > 0) {
+ params->query = g_memdup(query, query_length);
+ params->query_length = query_length;
+
+ params->response = g_memdup(spec, spec_length);
+ params->response_length = spec_length;
+ } else {
+ params->wfd_ies = g_memdup(spec, spec_length);
+ params->wfd_ies_length = spec_length;
+ }
+
+ return params;
+}
+
+static void free_peer_service_params(GSupplicantP2PServiceParams *params)
+{
+ if (!params)
+ return;
+
+ g_free(params->service);
+ g_free(params->query);
+ g_free(params->response);
+ g_free(params->wfd_ies);
+
+ g_free(params);
+}
+
+static int peer_register_wfd_service(const unsigned char *specification,
+ int specification_length,
+ peer_service_registration_cb_t callback,
+ void *user_data)
+{
+ struct peer_service_registration *reg_data = NULL;
+ static GSupplicantP2PServiceParams *params;
+ int ret;
+
+ DBG("");
+
+ if (wfd_service_registered)
+ return -EBUSY;
+
+ params = fill_in_peer_service_params(specification,
+ specification_length, NULL, 0, 0);
+ if (!params)
+ return -ENOMEM;
+
+ reg_data = g_try_malloc0(sizeof(*reg_data));
+ if (!reg_data) {
+ ret = -ENOMEM;
+ goto error;
+ }
+
+ reg_data->callback = callback;
+ reg_data->user_data = user_data;
+
+ ret = g_supplicant_set_widi_ies(params,
+ register_wfd_service_cb, reg_data);
+ if (ret < 0 && ret != -EINPROGRESS)
+ goto error;
+
+ wfd_service_registered = true;
+
+ return ret;
+error:
+ free_peer_service_params(params);
+ g_free(reg_data);
+
+ return ret;
+}
+
+static void register_peer_service_cb(int result,
+ GSupplicantInterface *iface, void *user_data)
+{
+ struct wifi_data *wifi = g_supplicant_interface_get_data(iface);
+ struct peer_service_registration *reg_data = user_data;
+
+ DBG("");
+
+ if (result == 0)
+ apply_p2p_listen_on_iface(wifi, NULL);
+
+ if (reg_data->callback)
+ reg_data->callback(result, reg_data->user_data);
+
+ g_free(reg_data);
+}
+
+static int peer_register_service(const unsigned char *specification,
+ int specification_length,
+ const unsigned char *query,
+ int query_length, int version,
+ peer_service_registration_cb_t callback,
+ void *user_data)
+{
+ struct peer_service_registration *reg_data;
+ GSupplicantP2PServiceParams *params;
+ bool found = false;
+ int ret, ret_f;
+ GList *list;
+
+ DBG("");
+
+ if (specification && !version && !query &&
+ is_service_wfd(specification, specification_length)) {
+ return peer_register_wfd_service(specification,
+ specification_length, callback, user_data);
+ }
+
+ reg_data = g_try_malloc0(sizeof(*reg_data));
+ if (!reg_data)
+ return -ENOMEM;
+
+ reg_data->callback = callback;
+ reg_data->user_data = user_data;
+
+ ret_f = -EOPNOTSUPP;
+
+ for (list = iface_list; list; list = list->next) {
+ struct wifi_data *wifi = list->data;
+ GSupplicantInterface *iface = wifi->interface;
+
+ if (!g_supplicant_interface_has_p2p(iface))
+ continue;
+
+ params = fill_in_peer_service_params(specification,
+ specification_length, query,
+ query_length, version);
+ if (!params) {
+ ret = -ENOMEM;
+ continue;
+ }
+
+ if (!found) {
+ ret_f = g_supplicant_interface_p2p_add_service(iface,
+ register_peer_service_cb, params, reg_data);
+ if (ret_f == 0 || ret_f == -EINPROGRESS)
+ found = true;
+ ret = ret_f;
+ } else
+ ret = g_supplicant_interface_p2p_add_service(iface,
+ register_peer_service_cb, params, NULL);
+ if (ret != 0 && ret != -EINPROGRESS)
+ free_peer_service_params(params);
+ }
+
+ if (ret_f != 0 && ret_f != -EINPROGRESS)
+ g_free(reg_data);
+
+ return ret_f;
+}
+
+static int peer_unregister_wfd_service(void)
+{
+ GSupplicantP2PServiceParams *params;
+ GList *list;
+
+ if (!wfd_service_registered)
+ return -EALREADY;
+
+ params = fill_in_peer_service_params(NULL, 0, NULL, 0, 0);
+ if (!params)
+ return -ENOMEM;
+
+ wfd_service_registered = false;
+
+ g_supplicant_set_widi_ies(params, NULL, NULL);
+
+ for (list = iface_list; list; list = list->next) {
+ struct wifi_data *wifi = list->data;
+
+ if (!g_supplicant_interface_has_p2p(wifi->interface))
+ continue;
+
+ wifi->servicing--;
+ if (!wifi->servicing || wifi->servicing < 0) {
+ g_supplicant_interface_p2p_listen(wifi->interface,
+ 0, 0);
+ wifi->servicing = 0;
+ }
+ }
+
+ return 0;
+}
+
+static int peer_unregister_service(const unsigned char *specification,
+ int specification_length,
+ const unsigned char *query,
+ int query_length, int version)
+{
+ GSupplicantP2PServiceParams *params;
+ bool wfd = false;
+ GList *list;
+ int ret;
+
+ if (specification && !version && !query &&
+ is_service_wfd(specification, specification_length)) {
+ ret = peer_unregister_wfd_service();
+ if (ret != 0 && ret != -EINPROGRESS)
+ return ret;
+ wfd = true;
+ }
+
+ for (list = iface_list; list; list = list->next) {
+ struct wifi_data *wifi = list->data;
+ GSupplicantInterface *iface = wifi->interface;
+
+ if (wfd)
+ goto stop_listening;
+
+ if (!g_supplicant_interface_has_p2p(iface))
+ continue;
+
+ params = fill_in_peer_service_params(specification,
+ specification_length, query,
+ query_length, version);
+ if (!params) {
+ ret = -ENOMEM;
+ continue;
+ }
+
+ ret = g_supplicant_interface_p2p_del_service(iface, params);
+ if (ret != 0 && ret != -EINPROGRESS)
+ free_peer_service_params(params);
+stop_listening:
+ wifi->servicing--;
+ if (!wifi->servicing || wifi->servicing < 0) {
+ g_supplicant_interface_p2p_listen(iface, 0, 0);
+ wifi->servicing = 0;
+ }
+ }
+
+ return 0;
+}
+
+static struct connman_peer_driver peer_driver = {
+ .connect = peer_connect,
+ .disconnect = peer_disconnect,
+ .register_service = peer_register_service,
+ .unregister_service = peer_unregister_service,
+};
+