+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)
+ 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)
+ 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,
+};
+
+static void handle_tethering(struct wifi_data *wifi)
+{
+ if (!wifi->tethering)
+ return;
+
+ if (!wifi->bridge)
+ return;
+
+ if (wifi->bridged)
+ return;
+
+ DBG("index %d bridge %s", wifi->index, wifi->bridge);
+
+ if (connman_inet_add_to_bridge(wifi->index, wifi->bridge) < 0)
+ return;
+
+ wifi->bridged = true;
+}
+
+static void wifi_newlink(unsigned flags, unsigned change, void *user_data)
+{
+ struct connman_device *device = user_data;
+ struct wifi_data *wifi = connman_device_get_data(device);
+
+ if (!wifi)
+ return;
+
+ DBG("index %d flags %d change %d", wifi->index, flags, change);
+
+ if ((wifi->flags & IFF_UP) != (flags & IFF_UP)) {
+ if (flags & IFF_UP)
+ DBG("interface up");
+ else
+ DBG("interface down");
+ }
+
+ if ((wifi->flags & IFF_LOWER_UP) != (flags & IFF_LOWER_UP)) {
+ if (flags & IFF_LOWER_UP) {
+ DBG("carrier on");
+
+ handle_tethering(wifi);
+ } else
+ DBG("carrier off");
+ }
+
+ wifi->flags = flags;
+}
+
+static int wifi_probe(struct connman_device *device)
+{
+ struct wifi_data *wifi;
+
+ DBG("device %p", device);
+
+ wifi = g_try_new0(struct wifi_data, 1);
+ if (!wifi)
+ return -ENOMEM;
+
+ wifi->state = G_SUPPLICANT_STATE_INACTIVE;
+ wifi->ap_supported = WIFI_AP_UNKNOWN;
+ wifi->tethering_param = NULL;
+
+ connman_device_set_data(device, wifi);
+ wifi->device = connman_device_ref(device);
+
+ wifi->index = connman_device_get_index(device);
+ wifi->flags = 0;
+
+ wifi->watch = connman_rtnl_add_newlink_watch(wifi->index,
+ wifi_newlink, device);
+ if (is_p2p_connecting())
+ add_pending_wifi_device(wifi);
+ else
+ iface_list = g_list_append(iface_list, wifi);
+
+ return 0;
+}
+
+static void remove_networks(struct connman_device *device,
+ struct wifi_data *wifi)
+{
+ GSList *list;
+
+ for (list = wifi->networks; list; list = list->next) {
+ struct connman_network *network = list->data;
+
+ connman_device_remove_network(device, network);
+ connman_network_unref(network);
+ }
+
+ g_slist_free(wifi->networks);
+ wifi->networks = NULL;
+}
+
+static void remove_peers(struct wifi_data *wifi)
+{
+ GSList *list;
+
+ for (list = wifi->peers; list; list = list->next) {
+ struct connman_peer *peer = list->data;
+
+ connman_peer_unregister(peer);
+ connman_peer_unref(peer);
+ }
+
+ g_slist_free(wifi->peers);
+ wifi->peers = NULL;
+}
+
+static void reset_autoscan(struct connman_device *device)
+{
+ struct wifi_data *wifi = connman_device_get_data(device);
+ struct autoscan_params *autoscan;
+
+ DBG("");
+
+ if (!wifi || !wifi->autoscan)
+ return;
+
+ autoscan = wifi->autoscan;
+
+ autoscan->interval = 0;
+
+ if (autoscan->timeout == 0)
+ return;
+
+ g_source_remove(autoscan->timeout);
+ autoscan->timeout = 0;
+
+ connman_device_unref(device);
+}
+
+static void stop_autoscan(struct connman_device *device)
+{
+ const struct wifi_data *wifi = connman_device_get_data(device);
+
+ if (!wifi || !wifi->autoscan)
+ return;
+
+ reset_autoscan(device);
+
+ connman_device_set_scanning(device, CONNMAN_SERVICE_TYPE_WIFI, false);
+}
+
+static void check_p2p_technology(void)
+{
+ bool p2p_exists = false;
+ GList *list;
+
+ for (list = iface_list; list; list = list->next) {
+ struct wifi_data *w = list->data;
+
+ if (w->interface &&
+ g_supplicant_interface_has_p2p(w->interface))
+ p2p_exists = true;
+ }
+
+ if (!p2p_exists) {
+ connman_technology_driver_unregister(&p2p_tech_driver);
+ connman_peer_driver_unregister(&peer_driver);
+ }
+}
+
+static void wifi_remove(struct connman_device *device)
+{
+ struct wifi_data *wifi = connman_device_get_data(device);
+
+ DBG("device %p wifi %p", device, wifi);
+
+ if (!wifi)
+ return;
+
+ stop_autoscan(device);
+
+ if (wifi->p2p_device)
+ p2p_iface_list = g_list_remove(p2p_iface_list, wifi);
+ else
+ iface_list = g_list_remove(iface_list, wifi);
+
+ check_p2p_technology();
+
+ remove_pending_wifi_device(wifi);
+
+ if (connman_device_get_scanning(device, CONNMAN_SERVICE_TYPE_P2P)) {
+ g_source_remove(wifi->p2p_find_timeout);
+ connman_device_unref(wifi->device);
+ }
+
+ if (wifi->p2p_connection_timeout)
+ g_source_remove(wifi->p2p_connection_timeout);
+
+ remove_networks(device, wifi);
+ remove_peers(wifi);
+
+ connman_device_set_powered(device, false);
+ connman_device_set_data(device, NULL);
+ connman_device_unref(wifi->device);
+ connman_rtnl_remove_watch(wifi->watch);
+
+ g_supplicant_interface_set_data(wifi->interface, NULL);
+
+ g_supplicant_interface_cancel(wifi->interface);
+
+ if (wifi->scan_params)
+ g_supplicant_free_scan_params(wifi->scan_params);
+
+ g_free(wifi->autoscan);
+ g_free(wifi->identifier);
+ g_free(wifi);
+}
+
+static bool is_duplicate(GSList *list, gchar *ssid, int ssid_len)
+{
+ GSList *iter;
+
+ for (iter = list; iter; iter = g_slist_next(iter)) {
+ struct scan_ssid *scan_ssid = iter->data;
+
+ if (ssid_len == scan_ssid->ssid_len &&
+ memcmp(ssid, scan_ssid->ssid, ssid_len) == 0)
+ return true;
+ }
+
+ return false;
+}
+
+static int add_scan_param(gchar *hex_ssid, char *raw_ssid, int ssid_len,
+ int freq, GSupplicantScanParams *scan_data,
+ int driver_max_scan_ssids, char *ssid_name)
+{
+ unsigned int i;
+ struct scan_ssid *scan_ssid;
+
+ if ((driver_max_scan_ssids == 0 ||
+ driver_max_scan_ssids > scan_data->num_ssids) &&
+ (hex_ssid || raw_ssid)) {
+ gchar *ssid;
+ unsigned int j = 0, hex;
+
+ if (hex_ssid) {
+ size_t hex_ssid_len = strlen(hex_ssid);
+
+ ssid = g_try_malloc0(hex_ssid_len / 2);
+ if (!ssid)
+ return -ENOMEM;
+
+ for (i = 0; i < hex_ssid_len; i += 2) {
+ sscanf(hex_ssid + i, "%02x", &hex);
+ ssid[j++] = hex;
+ }
+ } else {
+ ssid = raw_ssid;
+ j = ssid_len;
+ }
+
+ /*
+ * If we have already added hidden AP to the list,
+ * then do not do it again. This might happen if you have
+ * used or are using multiple wifi cards, so in that case
+ * you might have multiple service files for same AP.
+ */
+ if (is_duplicate(scan_data->ssids, ssid, j)) {
+ if (hex_ssid)
+ g_free(ssid);
+ return 0;
+ }
+
+ scan_ssid = g_try_new(struct scan_ssid, 1);
+ if (!scan_ssid) {
+ if (hex_ssid)
+ g_free(ssid);
+ return -ENOMEM;
+ }
+
+ memcpy(scan_ssid->ssid, ssid, j);
+ scan_ssid->ssid_len = j;
+ scan_data->ssids = g_slist_prepend(scan_data->ssids,
+ scan_ssid);
+
+ scan_data->num_ssids++;
+
+ DBG("SSID %s added to scanned list of %d entries", ssid_name,
+ scan_data->num_ssids);
+
+ if (hex_ssid)
+ g_free(ssid);
+ } else
+ return -EINVAL;
+
+ scan_data->ssids = g_slist_reverse(scan_data->ssids);
+
+ if (!scan_data->freqs) {
+ scan_data->freqs = g_try_malloc0(sizeof(uint16_t));
+ if (!scan_data->freqs) {
+ g_slist_free_full(scan_data->ssids, g_free);
+ return -ENOMEM;
+ }
+
+ scan_data->num_freqs = 1;
+ scan_data->freqs[0] = freq;
+ } else {
+ bool duplicate = false;
+
+ /* Don't add duplicate entries */
+ for (i = 0; i < scan_data->num_freqs; i++) {
+ if (scan_data->freqs[i] == freq) {
+ duplicate = true;
+ break;
+ }
+ }
+
+ if (!duplicate) {
+ scan_data->num_freqs++;
+ scan_data->freqs = g_try_realloc(scan_data->freqs,
+ sizeof(uint16_t) * scan_data->num_freqs);
+ if (!scan_data->freqs) {
+ g_slist_free_full(scan_data->ssids, g_free);
+ return -ENOMEM;
+ }
+ scan_data->freqs[scan_data->num_freqs - 1] = freq;
+ }
+ }
+
+ return 1;
+}
+
+static int get_hidden_connections(GSupplicantScanParams *scan_data)
+{
+ struct connman_config_entry **entries;
+ GKeyFile *keyfile;
+ gchar **services;
+ char *ssid, *name;
+ int i, ret;
+ bool value;
+ int num_ssids = 0, add_param_failed = 0;
+
+ services = connman_storage_get_services();
+ for (i = 0; services && services[i]; i++) {
+ if (strncmp(services[i], "wifi_", 5) != 0)
+ continue;
+
+ keyfile = connman_storage_load_service(services[i]);
+ if (!keyfile)
+ continue;
+
+ value = g_key_file_get_boolean(keyfile,
+ services[i], "Hidden", NULL);
+ if (!value) {
+ g_key_file_free(keyfile);
+ continue;
+ }
+
+ value = g_key_file_get_boolean(keyfile,
+ services[i], "Favorite", NULL);
+ if (!value) {
+ g_key_file_free(keyfile);
+ continue;
+ }
+
+ ssid = g_key_file_get_string(keyfile,
+ services[i], "SSID", NULL);
+
+ name = g_key_file_get_string(keyfile, services[i], "Name",
+ NULL);
+
+ ret = add_scan_param(ssid, NULL, 0, 0, scan_data, 0, name);
+ if (ret < 0)
+ add_param_failed++;
+ else if (ret > 0)
+ num_ssids++;
+
+ g_free(ssid);
+ g_free(name);
+ g_key_file_free(keyfile);
+ }
+
+ /*
+ * Check if there are any hidden AP that needs to be provisioned.
+ */
+ entries = connman_config_get_entries("wifi");
+ for (i = 0; entries && entries[i]; i++) {
+ int len;
+
+ if (!entries[i]->hidden)
+ continue;
+
+ if (!entries[i]->ssid) {
+ ssid = entries[i]->name;
+ len = strlen(ssid);
+ } else {
+ ssid = entries[i]->ssid;
+ len = entries[i]->ssid_len;
+ }