wifi: Add WFD utilities needed by the IWD backend

Since the NM D-Bus API talks to the client using raw WFD IE bytes and
IWD's API wants/provides some of the actual values from the WFD IE
instead (e.g. boolean Source and Sink properties), we need to be able to
parse and build the WFD IE from those property values to do the
translation, add utilities for this.  Use one of them to build the WFD IE
property on NMWifiP2PPeer objects.

[thaller@redhat.com: modify original patch to use
  nm_g_variant_unref() and add missing #define]

src/core/devices/wifi/nm-iwd-manager.h

@@ -26,6 +26,7 @@
 #define NM_IWD_ADHOC_INTERFACE         "net.connman.iwd.AdHoc"
 #define NM_IWD_STATION_INTERFACE       "net.connman.iwd.Station"
 #define NM_IWD_P2P_PEER_INTERFACE      "net.connman.iwd.p2p.Peer"
+#define NM_IWD_P2P_WFD_INTERFACE       "net.connman.iwd.p2p.Display"
 
 #define NM_TYPE_IWD_MANAGER (nm_iwd_manager_get_type())
 #define NM_IWD_MANAGER(obj) (G_TYPE_CHECK_INSTANCE_CAST((obj), NM_TYPE_IWD_MANAGER, NMIwdManager))

src/core/devices/wifi/nm-wifi-p2p-peer.c

@@ -427,11 +427,14 @@ nm_wifi_p2p_peer_update_from_iwd_object(NMWifiP2PPeer *peer, GDBusObject *obj)
     gboolean                        changed    = FALSE;
     nm_auto_ref_string NMRefString *peer_path  = NULL;
     gs_unref_object GDBusProxy     *peer_proxy = NULL;
+    gs_unref_object GDBusProxy     *wfd_proxy  = NULL;
     GVariant                       *value;
+    gs_unref_bytes GBytes          *wfd_ies = NULL;
 
     g_return_val_if_fail(NM_IS_WIFI_P2P_PEER(peer), FALSE);
 
     peer_proxy = G_DBUS_PROXY(g_dbus_object_get_interface(obj, NM_IWD_P2P_PEER_INTERFACE));
+    wfd_proxy  = G_DBUS_PROXY(g_dbus_object_get_interface(obj, NM_IWD_P2P_WFD_INTERFACE));
     g_return_val_if_fail(peer_proxy, FALSE);
 
     peer_path = nm_ref_string_new(g_dbus_object_get_object_path(obj));
@@ -457,6 +460,45 @@ nm_wifi_p2p_peer_update_from_iwd_object(NMWifiP2PPeer *peer, GDBusObject *obj)
         changed |= nm_wifi_p2p_peer_set_address(peer, g_variant_get_string(value, NULL));
     nm_g_variant_unref(value);
 
+    if (wfd_proxy) {
+        NMIwdWfdInfo wfd = {};
+
+        value      = g_dbus_proxy_get_cached_property(wfd_proxy, "Source");
+        wfd.source = value && g_variant_is_of_type(value, G_VARIANT_TYPE_BOOLEAN)
+                     && g_variant_get_boolean(value);
+        nm_g_variant_unref(value);
+
+        value    = g_dbus_proxy_get_cached_property(wfd_proxy, "Sink");
+        wfd.sink = value && g_variant_is_of_type(value, G_VARIANT_TYPE_BOOLEAN)
+                   && g_variant_get_boolean(value);
+        nm_g_variant_unref(value);
+
+        value    = g_dbus_proxy_get_cached_property(wfd_proxy, "Port");
+        wfd.port = (value && g_variant_is_of_type(value, G_VARIANT_TYPE_UINT16))
+                       ? g_variant_get_uint16(value)
+                       : 0;
+        nm_g_variant_unref(value);
+
+        value         = g_dbus_proxy_get_cached_property(wfd_proxy, "HasAudio");
+        wfd.has_audio = value && g_variant_is_of_type(value, G_VARIANT_TYPE_BOOLEAN)
+                        && g_variant_get_boolean(value);
+        nm_g_variant_unref(value);
+
+        value        = g_dbus_proxy_get_cached_property(wfd_proxy, "HasUIBC");
+        wfd.has_uibc = value && g_variant_is_of_type(value, G_VARIANT_TYPE_BOOLEAN)
+                       && g_variant_get_boolean(value);
+        nm_g_variant_unref(value);
+
+        value      = g_dbus_proxy_get_cached_property(wfd_proxy, "HasContentProtection");
+        wfd.has_cp = value && g_variant_is_of_type(value, G_VARIANT_TYPE_BOOLEAN)
+                     && g_variant_get_boolean(value);
+        nm_g_variant_unref(value);
+
+        wfd_ies = nm_wifi_utils_build_wfd_ies(&wfd);
+    }
+
+    changed |= nm_wifi_p2p_peer_set_wfd_ies(peer, wfd_ies);
+
     g_object_thaw_notify(G_OBJECT(peer));
 
     return changed;

src/core/devices/wifi/nm-wifi-utils.c

@@ -1800,3 +1800,151 @@ nm_wifi_utils_connection_to_iwd_config(NMConnection *connection,
 
     return g_steal_pointer(&file);
 }
+
+/* Wi-Fi Display Technical Specification v2.1.0 Table 27 */
+enum wfd_subelem_type {
+    WFD_SUBELEM_WFD_DEVICE_INFORMATION   = 0,
+    WFD_SUBELEM_ASSOCIATED_BSSID         = 1,
+    WFD_SUBELEM_COUPLED_SINK_INFORMATION = 6,
+    WFD_SUBELEM_EXTENDED_CAPABILITY      = 7,
+    WFD_SUBELEM_LOCAL_IP_ADDRESS         = 8,
+    WFD_SUBELEM_SESION_INFORMATION       = 9,
+    WFD_SUBELEM_ALTERNATIVE_MAC_ADDRESS  = 10,
+    WFD_SUBELEM_R2_DEVICE_INFORMATION    = 11,
+};
+
+bool
+nm_wifi_utils_parse_wfd_ies(GBytes *ies, NMIwdWfdInfo *out_wfd)
+{
+    size_t         len;
+    const uint8_t *data         = g_bytes_get_data(ies, &len);
+    const uint8_t *dev_info     = NULL;
+    uint16_t       dev_info_len = 0;
+    uint16_t       dev_info_flags;
+    const uint8_t *ext_capability     = NULL;
+    uint16_t       ext_capability_len = 0;
+
+    /* The single WFD IEs array provided by the client is supposed to be sent to
+     * the peer in the different frame types that may include the WFD IE: Probe
+     * Request/Response, Beacon, (Re)Association Request/Response, GO
+     * Negotiation Request/Response/Confirm and Provision Discovery
+     * Request/Response.
+     *
+     * It's going to be a subset of the elements allowed in all those frames.
+     * Validate that it contains at least a valid WFD Device Information (with
+     * the Session Available bit true) and that the sequence of subelements is
+     * valid.
+     */
+    while (len) {
+        uint8_t  subelem_id;
+        uint16_t subelem_len;
+
+        /* Does the subelement header fit */
+        if (len < 3)
+            return FALSE;
+
+        subelem_id  = data[0];
+        subelem_len = (data[1] << 8) | data[2];
+        data += 3;
+        len -= 3;
+
+        if (subelem_len > len)
+            return FALSE;
+
+        if (subelem_id == WFD_SUBELEM_WFD_DEVICE_INFORMATION) {
+            /* Is there a duplicate WFD Device Information */
+            if (dev_info)
+                return FALSE;
+
+            dev_info     = data;
+            dev_info_len = subelem_len;
+        }
+
+        if (subelem_id == WFD_SUBELEM_EXTENDED_CAPABILITY) {
+            /* Is there a duplicate WFD Extended Capability */
+            if (ext_capability)
+                return FALSE;
+
+            ext_capability     = data;
+            ext_capability_len = subelem_len;
+        }
+
+        data += subelem_len;
+        len -= subelem_len;
+    }
+
+    if (!dev_info || dev_info_len != 6)
+        return FALSE;
+
+    dev_info_flags = (dev_info[0] << 8) | dev_info[1];
+
+    /* Secondary sink not supported */
+    if ((dev_info_flags & 3) == 2)
+        return FALSE;
+
+    /* Must be available for WFD Session */
+    if (((dev_info_flags >> 4) & 3) != 1)
+        return FALSE;
+
+    /* TDLS persistent group re-invocation not supported */
+    if ((dev_info_flags >> 13) & 1)
+        return FALSE;
+
+    /* All other flags indicate support but not a requirement for something
+     * so not preserving them in the IEs IWD eventually sends doesn't break
+     * basic functionality.
+     */
+
+    if (ext_capability && ext_capability_len != 2)
+        return FALSE;
+
+    if (!out_wfd)
+        return TRUE;
+
+    out_wfd->source = NM_IN_SET(dev_info_flags & 3, 0, 3);
+    out_wfd->sink   = NM_IN_SET(dev_info_flags & 3, 1, 3);
+    out_wfd->port   = (dev_info[2] << 8) | dev_info[3];
+    out_wfd->has_audio =
+        out_wfd->sink ? ((dev_info_flags >> 10) & 1) == 0 : (((dev_info_flags >> 11) & 1) == 1);
+    out_wfd->has_uibc = ext_capability && (ext_capability[1] & 1) == 1;
+    out_wfd->has_cp   = ((dev_info_flags >> 8) & 1) == 1;
+    return TRUE;
+}
+
+GBytes *
+nm_wifi_utils_build_wfd_ies(const NMIwdWfdInfo *wfd)
+{
+    uint8_t  data[64];
+    uint8_t *ptr = data;
+
+    *ptr++ = WFD_SUBELEM_WFD_DEVICE_INFORMATION;
+    *ptr++ = 0; /* WFD Subelement length */
+    *ptr++ = 6;
+    *ptr++ = 0;                                               /* WFD Device Information bitmap: */
+    *ptr++ = (wfd->source ? (wfd->sink ? 3 : 0) : 1) | 0x10 | /* WFD Session Available */
+             (wfd->has_cp ? 0x100 : 0) | (wfd->has_audio ? 0 : 0x400);
+    *ptr++ = wfd->port >> 8;
+    *ptr++ = wfd->port & 255;
+    *ptr++ = 0; /* WFD Device Maximum throughput */
+    *ptr++ = 10;
+
+    if (wfd->has_uibc) {
+        *ptr++ = WFD_SUBELEM_EXTENDED_CAPABILITY;
+        *ptr++ = 0; /* WFD Subelement length */
+        *ptr++ = 2;
+        *ptr++ = 0x00; /* WFD Extended Capability Bitmap: */
+        *ptr++ = 0x10; /* UIBC Support */
+    }
+
+    return g_bytes_new(data, ptr - data);
+}
+
+bool
+nm_wifi_utils_wfd_info_eq(const NMIwdWfdInfo *a, const NMIwdWfdInfo *b)
+{
+    if (!a || !b)
+        return a == b;
+
+    return a->source == b->source && a->sink == b->sink && a->port == b->port
+           && a->has_audio == b->has_audio && a->has_uibc == b->has_uibc && a->has_cp == b->has_cp;
+}

src/core/devices/wifi/nm-wifi-utils.h

@@ -20,6 +20,15 @@ typedef enum {
     NM_IWD_NETWORK_SECURITY_8021X,
 } NMIwdNetworkSecurity;
 
+typedef struct {
+    bool     source;
+    bool     sink;
+    uint16_t port;
+    bool     has_audio;
+    bool     has_uibc;
+    bool     has_cp;
+} NMIwdWfdInfo;
+
 gboolean nm_wifi_utils_complete_connection(GBytes       *ssid,
                                            const char   *bssid,
                                            _NM80211Mode  mode,
@@ -43,4 +52,8 @@ char    *nm_wifi_utils_get_iwd_config_filename(const char          *ssid,
 GKeyFile *
 nm_wifi_utils_connection_to_iwd_config(NMConnection *conn, char **out_filename, GError **error);
 
+bool    nm_wifi_utils_parse_wfd_ies(GBytes *ies, NMIwdWfdInfo *out_wfd);
+GBytes *nm_wifi_utils_build_wfd_ies(const NMIwdWfdInfo *wfd);
+bool    nm_wifi_utils_wfd_info_eq(const NMIwdWfdInfo *a, const NMIwdWfdInfo *b);
+
 #endif /* __NM_WIFI_UTILS_H__ */