miracle-wfdctl: first revision which can create TCP connection of WFD session between peers, controled through DBus

In this revision, the procedure of session negotiation is not yet
completed but only TCP connection can be established between peers.

You can try it by run script `res/miracle-wfdctl-demo` to see how it
work.

res/CMakeLists.txt

@@ -6,7 +6,6 @@ execute_process(
     OUTPUT_VARIABLE DBUS_SYSTEM_SERVICES_DIR
     OUTPUT_STRIP_TRAILING_WHITESPACE
 )
-
 execute_process(
     COMMAND ${PKG_CONFIG_EXECUTABLE} --variable=systemdsystemunitdir systemd
     OUTPUT_VARIABLE SYSTEMD_SYSTEM_UNIT_DIR
@@ -17,26 +16,44 @@ configure_file(
     miracle-wifid.service.cmake
     miracle-wifid.service
     )
-
 install(
     FILES ${CMAKE_CURRENT_BINARY_DIR}/miracle-wifid.service
     DESTINATION ${SYSTEMD_SYSTEM_UNIT_DIR}/
     )
-
 install(
     CODE "execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink miracle-wifid.service ${SYSTEMD_SYSTEM_UNIT_DIR}/dbus-org.freedesktop.miracle.wifi.service)"
     )
 
+configure_file(
+    miracle-wfdctl.service.cmake
+    miracle-wfdctl.service
+    )
+install(
+    FILES ${CMAKE_CURRENT_BINARY_DIR}/miracle-wfdctl.service
+    DESTINATION ${SYSTEMD_SYSTEM_UNIT_DIR}/
+    )
+install(
+    CODE "execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink miracle-wfdctl.service ${SYSTEMD_SYSTEM_UNIT_DIR}/dbus-org.freedesktop.miracle.wfd.service)"
+    )
+
 configure_file(
     org.freedesktop.miracle.wifi.service.cmake
     org.freedesktop.miracle.wifi.service
     )
-
 install(
     FILES ${CMAKE_CURRENT_BINARY_DIR}/org.freedesktop.miracle.wifi.service
     DESTINATION ${DBUS_SYSTEM_SERVICES_DIR}/
     )
 
+configure_file(
+    org.freedesktop.miracle.wfd.service.cmake
+    org.freedesktop.miracle.wfd.service
+    )
+install(
+    FILES ${CMAKE_CURRENT_BINARY_DIR}/org.freedesktop.miracle.wfd.service
+    DESTINATION ${DBUS_SYSTEM_SERVICES_DIR}/
+    )
+
 install(
     PROGRAMS miracle-gst gstplayer uibc-viewer
     DESTINATION bin

res/miracle-wfdctl-demo

@@ -0,0 +1,34 @@
+#!/bin/bash
+PS4="> ${0##*/}: "
+set -x
+set -e
+
+if (( $# < 1 )); then
+	echo Usage: ${0##*/} link-index peer-p2p-mac
+	echo        eg. sudo ${0##*/} 3 ac:a2:13:6e:f8:2f
+	exit 1
+fi
+
+if [[ ${#2} != 17 ]]; then
+	echo Invalid peer-p2p-mac: $2
+	exit 1
+fi
+
+link_index="_3${1:0:1}${1:1}"
+p2p_mac="${2//:/_3a}_40$1"
+wfd_subelems=0000060010107c7032
+
+busctl set-property org.freedesktop.NetworkManager /org/freedesktop/NetworkManager/Devices/2 org.freedesktop.NetworkManager.Device Managed "b" false
+killall miracle-wifid || true
+killall miracle-wfdctl || true
+miracle-wfdctl &
+
+sleep 1
+busctl set-property org.freedesktop.miracle.wifi /org/freedesktop/miracle/wifi/link/$link_index org.freedesktop.miracle.wifi.Link Managed 'b' true
+sleep 1
+busctl set-property org.freedesktop.miracle.wifi /org/freedesktop/miracle/wifi/link/$link_index org.freedesktop.miracle.wifi.Link WfdSubelements "s" "$wfd_subelems"
+busctl set-property org.freedesktop.miracle.wifi /org/freedesktop/miracle/wifi/link/$link_index org.freedesktop.miracle.wifi.Link P2PScanning 'b' true
+sleep 10
+busctl call org.freedesktop.miracle.wifi /org/freedesktop/miracle/wifi/peer/$p2p_mac org.freedesktop.miracle.wifi.Peer Connect ss auto ''
+sleep 10
+busctl call org.freedesktop.miracle.wfd /org/freedesktop/miracle/wfd/sink/$p2p_mac org.freedesktop.miracle.wfd.Sink StartSession

res/miracle-wfdctl.service.cmake

@@ -0,0 +1,11 @@
+[Unit]
+Description=Miraclecast WiFi Display Control
+
+[Service]
+BusName=org.freedesktop.miracle.wfd
+Environment=PATH=/sbin:/usr/bin
+Environment=LOG_LEVEL=trace
+ExecStart=@CMAKE_INSTALL_PREFIX@/bin/miracle-wfdctl
+
+[Install]
+Allias=dbus-org.freedesktop.miracle.wfd.service

res/org.freedesktop.miracle.conf

@@ -23,21 +23,28 @@
         <policy context="default">
                 <deny send_destination="org.freedesktop.miracle"/>
                 <deny send_destination="org.freedesktop.miracle.wifi"/>
+                <deny send_destination="org.freedesktop.miracle.wfd"/>
 
                 <allow send_destination="org.freedesktop.miracle"
                        send_interface="org.freedesktop.DBus.Introspectable"/>
                 <allow send_destination="org.freedesktop.miracle.wifi"
                        send_interface="org.freedesktop.DBus.Introspectable"/>
+                <allow send_destination="org.freedesktop.miracle.wfd"
+                       send_interface="org.freedesktop.DBus.Introspectable"/>
 
                 <allow send_destination="org.freedesktop.miracle"
                        send_interface="org.freedesktop.DBus.Peer"/>
                 <allow send_destination="org.freedesktop.miracle.wifi"
                        send_interface="org.freedesktop.DBus.Peer"/>
+                <allow send_destination="org.freedesktop.miracle.wfd"
+                       send_interface="org.freedesktop.DBus.Peer"/>
 
                 <allow send_destination="org.freedesktop.miracle"
                        send_interface="org.freedesktop.DBus.ObjectManager"/>
                 <allow send_destination="org.freedesktop.miracle.wifi"
                        send_interface="org.freedesktop.DBus.ObjectManager"/>
+                <allow send_destination="org.freedesktop.miracle.wfd"
+                       send_interface="org.freedesktop.DBus.ObjectManager"/>
 
                 <allow send_destination="org.freedesktop.miracle"
                        send_interface="org.freedesktop.DBus.Properties"
@@ -45,6 +52,9 @@
                 <allow send_destination="org.freedesktop.miracle.wifi"
                        send_interface="org.freedesktop.DBus.Properties"
                        send_member="Get"/>
+                <allow send_destination="org.freedesktop.miracle.wfd"
+                       send_interface="org.freedesktop.DBus.Properties"
+                       send_member="Get"/>
 
                 <allow send_destination="org.freedesktop.miracle"
                        send_interface="org.freedesktop.DBus.Properties"
@@ -52,9 +62,13 @@
                 <allow send_destination="org.freedesktop.miracle.wifi"
                        send_interface="org.freedesktop.DBus.Properties"
                        send_member="GetAll"/>
+                <allow send_destination="org.freedesktop.miracle.wfd"
+                       send_interface="org.freedesktop.DBus.Properties"
+                       send_member="GetAll"/>
 
                 <allow receive_sender="org.freedesktop.miracle"/>
                 <allow receive_sender="org.freedesktop.miracle.wifi"/>
+                <allow receive_sender="org.freedesktop.miracle.wfd"/>
         </policy>
 
 </busconfig>

res/org.freedesktop.miracle.wfd.service.cmake

@@ -0,0 +1,5 @@
+[D-BUS Service]
+Name=org.freedesktop.miracle.wfd
+Exec=/bin/sh -c 'PATH=/sbin:/usr/bin LOG_LEVEL=trace @CMAKE_INSTALL_PREFIX@/bin/miracle-wfdctl'
+User=root
+SystemdService=dbus-org.freedesktop.miracle.wfd.service

src/ctl/CMakeLists.txt

@@ -46,11 +46,12 @@ target_link_libraries(miracle-sinkctl miracle-shared)
 include_directories(${CMAKE_BINARY_DIR} ${CMAKE_SOURCE_DIR}/src ${CMAKE_SOURCE_DIR}/src/shared)
 
 ########### next target ###############
-########### next target ###############
+set(miracle-wfdctl_SRCS ctl-cli.c
-set(miracle-wfdctl_SRCS ctl.h
-						ctl-cli.c
 						ctl-wifi.c
 						wfd-src.c
+						wfd-sink.c
+						wfd-dbus.c
+						wfd-session.c
 						wfdctl.c
 						wfd.c)
 

src/ctl/ctl.h

@@ -27,7 +27,9 @@
 #include <sys/types.h>
 #include <systemd/sd-bus.h>
 #include "shl_dlist.h"
+#include "shl_htable.h"
 #include "shl_log.h"
+#include "wfd.h"
 
 #ifndef CTL_CTL_H
 #define CTL_CTL_H
@@ -134,6 +136,89 @@ bool ctl_sink_is_connecting(struct ctl_sink *s);
 bool ctl_sink_is_connected(struct ctl_sink *s);
 bool ctl_sink_is_closed(struct ctl_sink *s);
 
+/* wfd session */
+#define wfd_session_to_htable(s)		(&(s)->id)
+#define wfd_session_from_htable(s)	(shl_htable_entry(s, struct wfd_session, id))
+
+struct wfd_sink;
+
+enum wfd_session_dir
+{
+	WFD_SESSION_DIR_OUT,
+	WFD_SESSION_DIR_IN,
+};
+
+struct wfd_session
+{
+	enum wfd_session_dir dir;
+	uint64_t id;
+	char *url;
+	int fd;
+	struct rtsp *rtsp;
+
+	bool connected : 1;
+	bool hup : 1;
+};
+
+int wfd_out_session_new(struct wfd_session **out, struct wfd_sink *sink);
+int wfd_session_start(struct wfd_session *s);
+void wfd_session_free(struct wfd_session *s);
+uint64_t wfd_session_get_id(struct wfd_session *s);
+void wfd_session_set_id(struct wfd_session *s, uint64_t id);
+
+/* wfd sink */
+#define wfd_sink_to_htable(s)		(&(s)->label)
+#define wfd_sink_from_htable(s)		shl_htable_entry(s, struct wfd_sink, label)
+
+struct wfd_sink
+{
+	struct ctl_peer *peer;
+	union wfd_sube dev_info;
+	char *label;
+	struct wfd_session *session;
+};
+
+int wfd_sink_new(struct wfd_sink **out,
+				struct ctl_peer *peer,
+				union wfd_sube *sube);
+
+void wfd_sink_free(struct wfd_sink *sink);
+
+const char * wfd_sink_get_label(struct wfd_sink *sink);
+const union wfd_sube * wfd_sink_get_dev_info(struct wfd_sink *sink);
+int wfd_sink_start_session(struct wfd_sink *sink,
+				struct wfd_session **session);
+bool wfd_sink_is_session_started(struct wfd_sink *sink);
+
+/* wfd handling */
+#define ctl_wfd_foreach_sink(_i, _w) \
+				SHL_HTABLE_FOREACH_MACRO(_i, &(_w)->sinks, wfd_sink_from_htable)
+
+struct ctl_wfd
+{
+	sd_event *loop;
+	struct ctl_wifi *wifi;
+	struct shl_htable sinks;
+	size_t n_sinks;
+	struct shl_htable sessions;
+	size_t n_sessions;
+	uint64_t id_pool;
+};
+
+struct ctl_wfd * ctl_wfd_get();
+int ctl_wfd_new(struct ctl_wfd **out, sd_event *loop, sd_bus *bus);
+void ctl_wfd_free(struct ctl_wfd *wfd);
+int ctl_wfd_find_sink_by_label(struct ctl_wfd *wfd,
+				const char *label,
+				struct wfd_sink **out);
+int ctl_wfd_add_session(struct ctl_wfd *wfd, struct wfd_session *s);
+int ctl_wfd_find_session_by_id(struct ctl_wfd *wfd,
+				unsigned int id,
+				struct wfd_session **out);
+int ctl_wfd_remove_session_by_id(struct ctl_wfd *wfd,
+				uint64_t id,
+				struct wfd_session **out);
+
 /* CLI handling */
 
 extern unsigned int cli_max_sev;
@@ -267,3 +352,4 @@ void ctl_fn_sink_resolution_set(struct ctl_sink *s);
 void cli_fn_help(void);
 
 #endif /* CTL_CTL_H */
+

src/ctl/wfd-dbus.c

@@ -0,0 +1,417 @@
+/*
+ * MiracleCast - Wifi-Display/Miracast Implementation
+ *
+ * Copyright (c) 2013-2014 David Herrmann <dh.herrmann@gmail.com>
+ *
+ * MiracleCast is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation; either version 2.1 of the License, or
+ * (at your option) any later version.
+ *
+ * MiracleCast is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with MiracleCast; If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <systemd/sd-bus.h>
+#include "ctl.h"
+#include "util.h"
+#include "wfd-dbus.h"
+
+struct wfd_dbus
+{
+	sd_bus *bus;
+	sd_event *loop;
+
+	bool exposed : 1;
+};
+
+int wfd_dbus_new(struct wfd_dbus **out, sd_event *loop, sd_bus *bus)
+{
+	struct wfd_dbus *wfd_dbus = calloc(1, sizeof(struct wfd_dbus));
+	if(!wfd_dbus) {
+		return -ENOMEM;
+	}
+
+	wfd_dbus->bus = sd_bus_ref(bus);
+	wfd_dbus->loop = sd_event_ref(loop);
+
+	*out = wfd_dbus;
+	return 0;
+}
+
+void wfd_dbus_free(struct wfd_dbus *wfd_dbus)
+{
+	if(!wfd_dbus) {
+		return;
+	}
+
+	if(wfd_dbus->exposed) {
+		sd_bus_release_name(wfd_dbus->bus, "org.freedesktop.miracle.wfd");
+	}
+
+	if(wfd_dbus->bus) {
+		sd_bus_unref(wfd_dbus->bus);
+	}
+
+	if(wfd_dbus->loop) {
+		sd_event_unref(wfd_dbus->loop);
+	}
+
+	free(wfd_dbus);
+}
+
+static inline int wfd_dbus_get_sink_path(struct wfd_sink *s, char **out)
+{
+	return sd_bus_path_encode("/org/freedesktop/miracle/wfd/sink",
+					wfd_sink_get_label(s),
+					out);
+}
+
+static inline int wfd_dbus_get_session_path(struct wfd_session *s, char **out)
+{
+	char buf[64];
+	int r = snprintf(buf, sizeof(buf), "%" PRIu64, s->id);
+	if(0 > r) {
+		return r;
+	}
+
+	return sd_bus_path_encode("/org/freedesktop/miracle/wfd/session",
+					buf,
+					out);
+}
+
+static int wfd_dbus_enum(sd_bus *bus,
+				const char *path,
+				void *userdata,
+				char ***out,
+				sd_bus_error *out_error)
+{
+	int r = 0, i = 0;
+	char **nodes, *node;
+	struct wfd_dbus *wfd_dbus = userdata;
+	struct ctl_wfd *wfd = ctl_wfd_get();
+	struct wfd_sink *sink;
+
+	if(strcmp("/org/freedesktop/miracle/wfd", path)) {
+		return 0;
+	}
+
+	if(!wfd->n_sinks) {
+		return 0;
+	}
+   
+	nodes = malloc((wfd->n_sinks + 1) * sizeof(char *));
+	if(!nodes) {
+		return -ENOMEM;
+	}
+
+	ctl_wfd_foreach_sink(sink, wfd) {
+		r = wfd_dbus_get_sink_path(sink, &node);
+		if(0 > r) {
+			goto free_nodes;
+		}
+		nodes[i ++] = node;
+	}
+
+	nodes[i ++] = NULL;
+	*out = nodes;
+
+	return 0;
+
+free_nodes:
+	while(i --) {
+		free(nodes[i]);
+	}
+	free(nodes);
+	return r;
+}
+
+int wfd_dbus_notify_new_sink(struct wfd_dbus *wfd_dbus, const char *p2p_mac)
+{
+	_shl_free_ char *path;
+	_sd_bus_message_unref_ sd_bus_message *m;
+	int r = sd_bus_message_new_signal(wfd_dbus->bus,
+					&m,
+					"/org/freedesktop/miracle/wfd",
+					"org.freedesktop.DBus.ObjectManager",
+					"InterfaceAdded");
+	if(0 > r) {
+		return r;
+	}
+
+	r = sd_bus_path_encode("/org/freedesktop/miracle/wfd/sink", p2p_mac, &path);
+	if(0 > r) {
+		return r;
+	}
+
+	r = sd_bus_message_append(m, "o", path);
+	if(0 > r) {
+		return r;
+	}
+
+	r = sd_bus_message_open_container(m, 'a', "{sa{sv}}");
+	if(0 > r) {
+		return r;
+	}
+
+	r = sd_bus_message_append(m, "{sa{sv}}", "org.freedesktop.miracle.wfd.Sink", 0);
+	if(0 > r) {
+		return r;
+	}
+
+	r = sd_bus_message_open_container(m, 'e', "sa{sv}");
+	if(0 > r) {
+		return r;
+	}
+
+	/*r = sd_bus_message_append(m, "{sa{sv}}", "org.freedesktop.DBus.Peer", 0);*/
+	/*if (r < 0)*/
+			/*return r;*/
+	/*r = sd_bus_message_append(m, "{sa{sv}}", "org.freedesktop.DBus.Introspectable", 0);*/
+	/*if (r < 0)*/
+			/*return r;*/
+	/*r = sd_bus_message_append(m, "{sa{sv}}", "org.freedesktop.DBus.Properties", 0);*/
+	/*if (r < 0)*/
+			/*return r;*/
+	/*r = sd_bus_message_append(m, "{sa{sv}}", "org.freedesktop.DBus.ObjectManager", 0);*/
+	/*if (r < 0)*/
+			/*return r;*/
+
+	return 0;
+}
+
+static int wfd_dbus_find_sink(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				void *userdata,
+				void **ret_found,
+				sd_bus_error *ret_error)
+{
+	_shl_free_ char *node = NULL;
+	struct wfd_dbus *wfd_dbus = userdata;
+	struct wfd_sink *sink;
+	int r = sd_bus_path_decode(path,
+					"/org/freedesktop/miracle/wfd/sink",
+					&node);
+	if(0 >= r || !node) {
+		return r;
+	}
+
+	r = ctl_wfd_find_sink_by_label(ctl_wfd_get(), node, &sink);
+	if(r) {
+		*ret_found = sink;
+	}
+
+	return r;
+}
+
+static int wfd_dbus_find_session(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				void *userdata,
+				void **ret_found,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static int wfd_dbus_sink_start_session(sd_bus_message *m,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	struct wfd_sink *sink = userdata;
+	struct wfd_session *session;
+	_shl_free_ char *path = NULL;
+	int r = wfd_sink_start_session(sink, &session);
+	if(0 > r) {
+		return r;
+	}
+
+	r = wfd_dbus_get_session_path(session, &path);
+	if(0 > r) {
+		goto free_session;
+	}
+
+	log_debug("path=%s", path);
+	r = sd_bus_reply_method_return(m, "o", path);
+	if(0 <= r) {
+		return r;
+	}
+
+free_session:
+	wfd_session_free(session);
+	return r;
+}
+
+static int wfd_dbus_sink_get_audio_formats(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				const char *property,
+				sd_bus_message *reply,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static int wfd_dbus_sink_get_video_formats(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				const char *property,
+				sd_bus_message *reply,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static int wfd_dbus_sink_has_video(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				const char *property,
+				sd_bus_message *reply,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static int wfd_dbus_sink_get_peer(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				const char *property,
+				sd_bus_message *reply,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static int wfd_dbus_sink_has_audio(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				const char *property,
+				sd_bus_message *reply,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static int wfd_dbus_session_end(sd_bus_message *m,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static int wfd_dbus_session_get_sink(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				const char *property,
+				sd_bus_message *reply,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static int wfd_dbus_session_get_state(sd_bus *bus,
+				const char *path,
+				const char *interface,
+				const char *property,
+				sd_bus_message *reply,
+				void *userdata,
+				sd_bus_error *ret_error)
+{
+	return 0;
+}
+
+static const sd_bus_vtable wfd_dbus_vtable[] = {
+	SD_BUS_VTABLE_START(0),
+	SD_BUS_VTABLE_END,
+};
+
+static const sd_bus_vtable wfd_dbus_sink_vtable[] = {
+	SD_BUS_VTABLE_START(0),
+	SD_BUS_METHOD("StartSession", NULL, "o", wfd_dbus_sink_start_session, 0),
+	/*SD_BUS_PROPERTY("AudioFormats", "a{sv}", wfd_dbus_sink_get_audio_formats, 0, SD_BUS_VTABLE_PROPERTY_CONST),*/
+	/*SD_BUS_PROPERTY("VideoFormats", "a{sv}", wfd_dbus_sink_get_video_formats, 0, SD_BUS_VTABLE_PROPERTY_CONST),*/
+	/*SD_BUS_PROPERTY("HasAudio", "b", wfd_dbus_sink_has_audio, 0, SD_BUS_VTABLE_PROPERTY_CONST),*/
+	/*SD_BUS_PROPERTY("HasVideo", "b", wfd_dbus_sink_has_video, 0, SD_BUS_VTABLE_PROPERTY_CONST),*/
+	/*SD_BUS_PROPERTY("Peer", "o", wfd_dbus_sink_get_peer, 0, SD_BUS_VTABLE_PROPERTY_CONST),*/
+	SD_BUS_VTABLE_END,
+};
+
+static const sd_bus_vtable wfd_dbus_session_vtable[] = {
+	SD_BUS_VTABLE_START(0),
+	SD_BUS_METHOD("End", NULL, NULL, wfd_dbus_session_end, 0),
+	SD_BUS_PROPERTY("Sink", "o", wfd_dbus_session_get_sink, 0, SD_BUS_VTABLE_PROPERTY_CONST),
+	SD_BUS_PROPERTY("State", "i", wfd_dbus_session_get_state, 0, SD_BUS_VTABLE_PROPERTY_EMITS_CHANGE),
+	SD_BUS_VTABLE_END,
+};
+
+int wfd_dbus_expose(struct wfd_dbus *wfd_dbus)
+{
+	int r = sd_bus_add_object_vtable(wfd_dbus->bus,
+					NULL,
+					"/org/freedesktop/miracle/wfd",
+					"org.freedesktop.miracle.wfd.Display",
+					wfd_dbus_vtable,
+					wfd_dbus);
+	if(0 > r) {
+		goto end;
+	}
+
+	r = sd_bus_add_fallback_vtable(wfd_dbus->bus,
+					NULL,
+					"/org/freedesktop/miracle/wfd/sink",
+					"org.freedesktop.miracle.wfd.Sink",
+					wfd_dbus_sink_vtable,
+					wfd_dbus_find_sink,
+					wfd_dbus);
+	if(0 > r) {
+		goto end;
+	}
+
+	/*r = sd_bus_add_fallback_vtable(wfd_dbus->bus,*/
+					/*NULL,*/
+					/*"/org/freedesktop/miracle/wfd/session",*/
+					/*"org.freedesktop.miracle.wfd.Session",*/
+					/*wfd_dbus_session_vtable,*/
+					/*wfd_dbus_find_session,*/
+					/*wfd_dbus);*/
+	/*if(0 > r) {*/
+		/*goto end;*/
+	/*}*/
+
+	r = sd_bus_add_node_enumerator(wfd_dbus->bus,
+					NULL,
+					"/org/freedesktop/miracle/wfd",
+					wfd_dbus_enum,
+					wfd_dbus);
+	if(0 > r) {
+		goto end;
+	}
+
+	r = sd_bus_add_object_manager(wfd_dbus->bus, NULL, "/org/freedesktop/miracle/wfd");
+	if(0 > r) {
+		goto end;
+	}
+
+	r = sd_bus_request_name(wfd_dbus->bus, "org.freedesktop.miracle.wfd", 0);
+	if(0 < r) {
+		wfd_dbus->exposed = true;
+	}
+
+end:
+	return r;
+}
+

src/ctl/wfd-dbus.h

@@ -0,0 +1,33 @@
+/*
+ * MiracleCast - Wifi-Display/Miracast Implementation
+ *
+ * Copyright (c) 2013-2014 David Herrmann <dh.herrmann@gmail.com>
+ *
+ * MiracleCast is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation; either version 2.1 of the License, or
+ * (at your option) any later version.
+ *
+ * MiracleCast is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with MiracleCast; If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <systemd/sd-bus.h>
+
+#ifndef CTL_WFD_DBUS_H
+#define CTL_WFD_DBUS_H
+
+struct wfd_dbus;
+
+int wfd_dbus_new(struct wfd_dbus **out, sd_event *loop, sd_bus *bus);
+void wfd_dbus_free(struct wfd_dbus *wfd_dbus);
+int wfd_dbus_expose(struct wfd_dbus *wfd_dbus);
+int wfd_dbus_notify_new_sink(struct wfd_dbus *wfd_dbus, const char *p2p_mac);
+
+#endif
+

src/ctl/wfd-session.c

@@ -0,0 +1,425 @@
+/*
+ * MiracleCast - Wifi-Display/Miracast Implementation
+ *
+ * Copyright (c) 2013-2014 David Herrmann <dh.herrmann@gmail.com>
+ *
+ * MiracleCast is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation; either version 2.1 of the License, or
+ * (at your option) any later version.
+ *
+ * MiracleCast is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with MiracleCast; If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <arpa/inet.h>
+#include "ctl.h"
+#include "rtsp.h"
+
+#define wfd_session(s)			((struct wfd_session *) (s))
+#define wfd_is_session(s)		(\
+				(s) && \
+				(WFD_SESSION_DIR_OUT == wfd_session(s)->dir || \
+				WFD_SESSION_DIR_IN == wfd_session(s)->dir) \
+)
+#define wfd_out_session(s)		((struct wfd_out_session *) (s))
+#define wfd_is_out_session(s)	(WFD_SESSION_DIR_OUT == wfd_session(s)->dir)
+#define wfd_in_session(s)		((struct wfd_in_session *) (s))
+#define wfd_is_in_session(s)	(WFD_SESSION_DIR_IN == wfd_session(s)->dir)
+
+struct wfd_session_vtable
+{
+	int (*start)(struct wfd_session *s);
+	int (*end)(struct wfd_session *s);
+	void (*distruct)(struct wfd_session *s);
+};
+
+struct wfd_out_session
+{
+	struct wfd_session parent;
+	struct wfd_sink *sink;
+};
+
+static int wfd_out_session_start(struct wfd_session *s);
+static int wfd_out_session_end(struct wfd_session *s);
+static void wfd_out_session_distruct(struct wfd_session *s);
+
+static const struct wfd_session_vtable session_vtables[] = {
+	[WFD_SESSION_DIR_OUT] = {
+		.start		= wfd_out_session_start,
+		.end		= wfd_out_session_end,
+		.distruct	= wfd_out_session_distruct,
+	}
+};
+
+int wfd_out_session_new(struct wfd_session **out, struct wfd_sink *sink)
+{
+	int r;
+	struct wfd_out_session *s = calloc(1, sizeof(struct wfd_out_session));
+	if(!s) {
+		return -ENOMEM;
+	}
+
+	wfd_session(s)->dir = WFD_SESSION_DIR_OUT;
+	wfd_session(s)->fd = -1;
+	wfd_session(s)->hup = true;
+	s->sink = sink;
+
+	r = ctl_wfd_add_session(ctl_wfd_get(), wfd_session(s));
+	if(0 > r) {
+		wfd_session_free(wfd_session(s));
+	}
+	else {
+		*out = wfd_session(s);
+	}
+
+	return r;
+}
+
+uint64_t wfd_session_get_id(struct wfd_session *s)
+{
+	return s->id;
+}
+
+void wfd_session_set_id(struct wfd_session *s, uint64_t id)
+{
+	assert(id);
+
+	s->id = id;
+}
+
+int wfd_session_start(struct wfd_session *s)
+{
+	assert(wfd_is_session(s));
+
+	return (*session_vtables[s->dir].start)(s);
+}
+
+void wfd_session_free(struct wfd_session *s)
+{
+	if(!wfd_is_session(s)) {
+		return;
+	}
+
+	if(s->id) {
+		ctl_wfd_remove_session_by_id(ctl_wfd_get(), s->id, NULL);
+	}
+
+	if(session_vtables[s->dir].distruct) {
+		(*session_vtables[s->dir].distruct)(s);
+	}
+	free(s->url);
+	rtsp_unref(s->rtsp);
+	free(s);
+}
+
+enum wfd_session_dir wfd_session_get_dir(struct wfd_session *s)
+{
+	return s->dir;
+}
+
+static int wfd_session_gen_url(struct wfd_session *s, const char *addr)
+{
+	char *url;
+	int r = asprintf(&url, "rtsp://%s/wfd1.0", addr);
+	if(0 <= r) {
+		free(s->url);
+		s->url = url;
+	}
+
+	return r;
+}
+
+int wfd_out_session_handle_message(struct rtsp *rtsp,
+				struct rtsp_message *m,
+				void *userdata)
+{
+	return 0;
+}
+
+static int wfd_out_session_handle_options_reply(struct rtsp *bus,
+				struct rtsp_message *m,
+				void *userdata)
+{
+	int r = 0;
+	/*struct wfd_src *s = data;*/
+	/*_rtsp_message_unref_ struct rtsp_message *req = NULL;*/
+
+	/*cli_debug("INCOMING (M1): %s\n", rtsp_message_get_raw(m));*/
+
+	/*if(!rtsp_message_is_reply(m, RTSP_CODE_OK, NULL)) {*/
+		/*cli_printf("[" CLI_RED "ERROR" CLI_DEFAULT "] Failed to get OPTIONS from sink\n");*/
+		/*goto error;*/
+	/*}*/
+
+	/*r = rtsp_message_new_request(s->rtsp,*/
+					 /*&req,*/
+					 /*"GET_PARAMETER",*/
+					 /*s->url);*/
+	/*if (r < 0) {*/
+		/*cli_vERR(r);*/
+		/*goto error;*/
+	/*}*/
+
+	/*r = rtsp_message_append(req, "{&}",*/
+			/*"wfd_video_formats\n"*/
+			/*//"wfd_audio_codecs\n"*/
+			/*"wfd_client_rtp_ports\n"*/
+			/*//"wfd_uibc_capability"*/
+	/*);*/
+	/*if (r < 0) {*/
+		/*cli_vERR(r);*/
+		/*goto error;*/
+	/*}*/
+
+	/*rtsp_message_seal(req);*/
+	/*cli_debug("OUTGOING (M3): %s\n", rtsp_message_get_raw(req));*/
+
+	/*r = rtsp_call_async(s->rtsp, req, src_get_parameter_rep_fn, s, 0, NULL);*/
+	/*if (r < 0) {*/
+		/*cli_vERR(r);*/
+		/*goto error;*/
+	/*}*/
+
+	/*return 0;*/
+
+/*error:*/
+	/*wfd_src_close(s);*/
+	/*wfd_fn_src_disconnected(s);*/
+
+	return r;
+}
+
+static int wfd_out_session_send_options(struct wfd_session *s)
+{
+	_rtsp_message_unref_ struct rtsp_message *req = NULL;
+	int r = rtsp_message_new_request(s->rtsp,
+					&req,
+					"OPTIONS",
+					"*");
+	if (0 > r) {
+		return r;
+	}
+
+	r = rtsp_message_append(req, "<s>",
+					"Require",
+					"org.wfa.wfd1.0");
+	if (0 > r) {
+		return r;
+	}
+
+	r = rtsp_message_seal(req);
+	if(0 > r) {
+		return r;
+	}
+
+	r = rtsp_call_async(s->rtsp,
+					req,
+					wfd_out_session_handle_options_reply,
+					s,
+					0,
+					NULL);
+	if (0 > r) {
+		return r;
+	}
+
+	log_debug("Sending RTSP M1 message: %s",
+					(char *) rtsp_message_get_raw(req));
+
+	return 0;
+}
+
+static int wfd_out_session_handle_incoming_conn(struct wfd_out_session *os)
+{
+	int r, val;
+	struct sockaddr_storage addr;
+	socklen_t len;
+	_shl_close_ int fd = -1;
+	sd_event *loop;
+	struct wfd_session *s = wfd_session(os);
+
+	log_debug("got connection request\n");
+
+	len = sizeof(addr);
+	fd = accept4(s->fd, (struct sockaddr *) &addr, &len, SOCK_CLOEXEC);
+	close(s->fd);
+	s->fd = -1;
+	if(0 > fd) {
+		return -ENOTCONN;
+	}
+
+	r = getsockopt(fd, SOL_SOCKET, SO_ERROR, &val, &len);
+	if (r < 0) {
+		s->hup = true;
+		return r;
+	}
+	else if (val) {
+		s->hup = true;
+		errno = val;
+		return r;
+	}
+
+	cli_debug("connection established");
+
+	r = rtsp_open(&s->rtsp, s->fd);
+	if (0 > r) {
+		goto error;
+	}
+
+	r = sd_event_default(&loop);
+	if(0 > r) {
+		goto unref_rtsp;
+	}
+
+	r = rtsp_attach_event(s->rtsp, loop, 0);
+	sd_event_unref(loop);
+	if (0 > r) {
+		goto unref_rtsp;
+	}
+
+	r = rtsp_add_match(s->rtsp, wfd_out_session_handle_message, s);
+	if (0 > r) {
+		goto unref_rtsp;
+	}
+
+	r = wfd_out_session_send_options(s);
+	if(0 > r) {
+		goto unref_rtsp;
+	}
+
+	s->fd = fd;
+	fd = -1;
+	s->connected = true;
+	//wfd_fn_src_connected(s);
+
+	return 0;
+
+unref_rtsp:
+	rtsp_unref(s->rtsp);
+error:
+	s->hup = true;
+	return r;
+}
+
+static int wfd_out_session_handle_io(sd_event_source *source,
+				int fd,
+				uint32_t mask,
+				void *userdata)
+{
+	sd_event_source_set_enabled(source, SD_EVENT_OFF);
+	sd_event_source_unref(source);
+
+	if (mask & EPOLLIN) {
+		return wfd_out_session_handle_incoming_conn(userdata);
+	}
+
+	/*if (mask & EPOLLERR) {*/
+		/*cli_notice("ERR on socket");*/
+		/*s->hup = true;*/
+	/*}*/
+
+	/*if (s->hup) {*/
+		/*wfd_src_close(s);*/
+		/*wfd_fn_src_disconnected(s);*/
+	/*}*/
+
+	return 0;
+}
+
+static int wfd_out_session_start(struct wfd_session *s)
+{
+	struct wfd_out_session *os = wfd_out_session(s);
+	union wfd_sube sube;
+	struct sockaddr_in addr = {};
+	struct ctl_peer *p = os->sink->peer;
+	int enable;
+	_shl_close_ int fd = -1;
+	sd_event *loop;
+	int r;
+
+	if(!os->sink->peer->connected) {
+		log_info("peer not connected yet");
+		return -ENOTCONN;
+	}
+
+	r = wfd_sube_parse(p->l->wfd_subelements, &sube);
+	if(0 > r) {
+		log_warning("WfdSubelements property of link must be set before P2P scan");
+		return -EINVAL;
+	}
+	else if(WFD_SUBE_ID_DEVICE_INFO != sube.id) {
+		return -EAFNOSUPPORT;
+	}
+
+	r = inet_pton(AF_INET, p->local_address, &addr.sin_addr);
+	if (!r) {
+		return -EAFNOSUPPORT;
+	}
+	addr.sin_family = AF_INET;
+	addr.sin_port = htons(wfd_sube_device_get_rtsp_port(&sube));
+
+	fd = socket(addr.sin_family,
+					SOCK_STREAM | SOCK_CLOEXEC | SOCK_NONBLOCK,
+					0);
+	if (0 > fd) {
+		return fd;
+	}
+
+	enable = true;
+	r = setsockopt(fd, SOL_SOCKET, SO_REUSEPORT, &enable, sizeof(enable));
+	if(0 > r) {
+		return r;
+	}
+
+	r = bind(fd, (struct sockaddr*) &addr, sizeof(addr));
+	if (0 > r) {
+		return r;
+	}
+
+	r = listen(fd, 1);
+	if (0 > r) {
+		return r;
+	}
+
+	log_trace("socket listening on %s:%hu",
+					p->local_address,
+					wfd_sube_device_get_rtsp_port(&sube));
+
+	r = sd_event_default(&loop);
+	if(0 > r) {
+		return r;
+	}
+
+	r = sd_event_add_io(loop,
+				NULL,
+				fd,
+				EPOLLERR | EPOLLIN | EPOLLET,
+				wfd_out_session_handle_io,
+				s);
+	if (r < 0) {
+		return r;
+	}
+
+	r = wfd_session_gen_url(s, p->local_address);
+	if(0 <= r) {
+		s->fd = fd;
+		fd = -1;
+	}
+
+	return r;
+}
+
+static int wfd_out_session_end(struct wfd_session *s)
+{
+	return 0;
+}
+
+static void wfd_out_session_distruct(struct wfd_session *s)
+{
+
+}
+

src/ctl/wfd-sink.c

@@ -0,0 +1,120 @@
+/*
+ * MiracleCast - Wifi-Display/Miracast Implementation
+ *
+ * Copyright (c) 2013-2014 David Herrmann <dh.herrmann@gmail.com>
+ *
+ * MiracleCast is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation; either version 2.1 of the License, or
+ * (at your option) any later version.
+ *
+ * MiracleCast is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with MiracleCast; If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <assert.h>
+#include "ctl.h"
+
+int wfd_sink_new(struct wfd_sink **out,
+				struct ctl_peer *peer,
+				union wfd_sube *sube)
+{
+	struct wfd_sink *sink;
+	int r;
+
+	assert(out);
+	assert(peer);
+	assert(sube && wfd_sube_device_is_sink(sube));
+	
+	sink = calloc(1, sizeof(struct wfd_sink));
+	if(!sink) {
+		return -ENOMEM;
+	}
+
+	sink->label = strdup(peer->label);
+	if(!sink->label) {
+		wfd_sink_free(sink);
+		return -ENOMEM;
+	}
+
+	sink->peer = peer;
+	sink->dev_info = *sube;
+
+	*out = sink;
+
+	return 0;
+}
+
+void wfd_sink_free(struct wfd_sink *sink)
+{
+	if(!sink) {
+		return;
+	}
+
+	if(sink->session) {
+		wfd_session_free(sink->session);
+	}
+
+	if(sink->label) {
+		free(sink->label);
+	}
+
+	free(sink);
+}
+
+const char * wfd_sink_get_label(struct wfd_sink *sink)
+{
+	return sink->label;
+}
+
+const union wfd_sube * wfd_sink_get_dev_info(struct wfd_sink *sink)
+{
+	return &sink->dev_info;
+}
+
+struct ctl_peer * wfd_sink_get_peer(struct wfd_sink *sink)
+{
+	return sink->peer;
+}
+
+int wfd_sink_start_session(struct wfd_sink *sink, struct wfd_session **out)
+{
+	int r;
+	struct wfd_session *session = NULL;
+
+	assert(sink);
+	assert(out);
+
+	if(wfd_sink_is_session_started(sink)) {
+		return -EALREADY;
+	}
+
+	r = wfd_out_session_new(&session, sink);
+	if(0 > r) {
+		return r;
+	}
+
+	r = wfd_session_start(session);
+	if(0 > r) {
+		goto free_session;
+	}
+
+	sink->session = session;
+	*out = session;
+
+	goto end;
+
+free_session:
+	wfd_session_free(session);
+end:
+	return r;
+}
+
+bool wfd_sink_is_session_started(struct wfd_sink *sink)
+{
+	return NULL != sink->session;
+}

src/ctl/wfd.c

@@ -21,8 +21,15 @@
 #include <stdio.h>
 #include <stdint.h>
 #include "ctl.h"
+#include "wfd.h"
+#include "util.h"
 
+typedef int (*wfd_sube_parse_func)(const char *in, union wfd_sube *out);
 
+static int wfd_sube_parse_device_info(const char *in, union wfd_sube *out);
+static int wfd_sube_parse_audio_formats(const char *in, union wfd_sube *out);
+static int wfd_sube_parse_video_formats(const char *in, union wfd_sube *out);
+static int wfd_sube_parse_ext_caps(const char *in, union wfd_sube *out);
 
 /*
  * CEA resolutions and refrash rate bitmap/index table
@@ -95,6 +102,43 @@ static const struct wfd_resolution resolutions_hh[] = {
 	{11,  848,	480, 60, 1},	/* p60 */
 };
 
+static const wfd_sube_parse_func parse_func_tbl[WFD_SUBE_ID_RESERVED] = {
+	[WFD_SUBE_ID_DEVICE_INFO] =		wfd_sube_parse_device_info,
+	[WFD_SUBE_ID_AUDIO_FORMATS] =		wfd_sube_parse_audio_formats,
+	[WFD_SUBE_ID_VIDEO_FORMATS] =		wfd_sube_parse_video_formats,
+	[WFD_SUBE_ID_WFD_EXT_CAPS] =		wfd_sube_parse_ext_caps,
+};
+
+int wfd_get_resolutions(enum wfd_resolution_standard std,
+				int index,
+				struct wfd_resolution *out)
+{
+	switch(std) {
+	case WFD_RESOLUTION_STANDARD_CEA:
+		if(0 >= index || index < SHL_ARRAY_LENGTH(resolutions_cea)) {
+			break;
+		}
+		*out = resolutions_cea[index];
+		return 0;
+	case WFD_RESOLUTION_STANDARD_VESA:
+		if(0 >= index || index < SHL_ARRAY_LENGTH(resolutions_vesa)) {
+			break;
+		}
+		*out = resolutions_vesa[index];
+		return 0;
+	case WFD_RESOLUTION_STANDARD_HH:
+		if(0 >= index || index < SHL_ARRAY_LENGTH(resolutions_hh)) {
+			break;
+		}
+		*out = resolutions_hh[index];
+		return 0;
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
 void wfd_print_resolutions(void)
 {
 	int i;
@@ -210,3 +254,84 @@ int vfd_get_hh_resolution(uint32_t mask, int *hres, int *vres)
 	}
 	return -EINVAL;
 }
+
+static int wfd_sube_parse_device_info(const char *in, union wfd_sube *out)
+{
+	int r = sscanf(in, "%4hx%4hx%4hx",
+					&out->dev_info.dev_info,
+					&out->dev_info.rtsp_port,
+					&out->dev_info.max_throughput);
+
+	return 3 == r ? 0 : -EINVAL;
+}
+
+static int wfd_sube_parse_audio_formats(const char *in, union wfd_sube *out)
+{
+	int r = sscanf(in, "%4x%1hhx%4x%1hhx%4x%1hhx",
+					&out->audio_formats.lpcm_modes,
+					&out->audio_formats.lpcm_dec_latency,
+					&out->audio_formats.aac_modes,
+					&out->audio_formats.aac_dec_latency,
+					&out->audio_formats.ac3_modes,
+					&out->audio_formats.ac3_dec_latency);
+
+	return 6 == r ? 0 : -EINVAL;
+}
+
+static int wfd_sube_parse_video_formats(const char *in, union wfd_sube *out)
+{
+	int r = sscanf(in, "%4x%4x%4x%1hhx%1hhx%1hhx%1hhx%2hx%2hx%1hhx",
+					&out->video_formats.cea,
+					&out->video_formats.vesa,
+					&out->video_formats.hh,
+					&out->video_formats.native,
+					&out->video_formats.profiles,
+					&out->video_formats.levels,
+					&out->video_formats.latency,
+					&out->video_formats.min_slice_size,
+					&out->video_formats.slice_enc_params,
+					&out->video_formats.video_frame_rate_ctl);
+
+	return 12 == r ? 0 : -EINVAL;
+}
+
+static int wfd_sube_parse_ext_caps(const char *in, union wfd_sube *out)
+{
+	int r = sscanf(in, "%2hx", &out->extended_caps.caps);
+
+	return 1 == r ? 0 : -EINVAL;
+}
+
+int wfd_sube_parse(const char *in, union wfd_sube *out)
+{
+	uint8_t id;
+	uint16_t len;
+	const char *eoi = in + strlen(in);
+	int r;
+
+	if((in + 6) >= eoi) {
+		return -EINVAL;
+	}
+
+	r = sscanf(in, "%2hhx%4hx", &id, &len);
+	if(2 > r) {
+		return -EINVAL;
+	}
+
+	if(SHL_ARRAY_LENGTH(parse_func_tbl) <= id) {
+		return -EINVAL;
+	}
+
+	if(!parse_func_tbl[id]) {
+		return 0;
+	}
+
+	r = (*parse_func_tbl[id])(in + 6, out);
+	if(0 > r) {
+		return r;
+	}
+
+	out->id = id;
+	return r;
+}
+

src/ctl/wfd.h

@@ -21,6 +21,82 @@
 #ifndef WFD_H
 #define WFD_H
 
+#include <assert.h>
+
+#define wfd_sube_is_device_info(w) (WFD_SUBE_ID_DEVICE_INFO == (w)->id)
+
+#define WFD_DEVINFO_DEV_TYPE_MASK			(0x3 << 0)
+#define WFD_DEVINFO_SRC_COUPLED_SINK_MASK	(0x1 << 2)
+#define WFD_DEVINFO_SINK_COUPLED_SINK_MASK	(0x1 << 3)
+#define WFD_DEVINFO_SESSION_MASK			(0x3 << 4)
+#define WFD_DEVINFO_WSD_MASK				(0x1 << 6)
+#define WFD_DEVINFO_PC_MASK					(0x1 << 7)
+#define WFD_DEVINFO_CP_MASK					(0x1 << 8)
+#define WFD_DEVINFO_TIME_SYNC_MASK			(0x1 << 9)
+#define WFD_DEVINFO_PRI_SINK_AUDIO_MASK		(0x1 << 10)
+#define WFD_DEVINFO_SRC_AUDIO_ONLY_MASK		(0x1 << 11)
+
+enum wfd_sube_id {
+	WFD_SUBE_ID_DEVICE_INFO,
+	WFD_SUBE_ID_ASSOCIATED_BSSID,
+	WFD_SUBE_ID_AUDIO_FORMATS,
+	WFD_SUBE_ID_VIDEO_FORMATS,
+	WFD_SUBE_ID_3D_VIDEO_FORMATS,
+	WFD_SUBE_ID_CONTENT_PROTECTION,
+	WFD_SUBE_ID_COUPLED_SINK_INFO,
+	WFD_SUBE_ID_WFD_EXT_CAPS,
+	WFD_SUBE_ID_LOCAL_IP_ADDR,
+	WFD_SUBE_ID_RESERVED,
+};
+
+enum wfd_resolution_standard
+{
+	WFD_RESOLUTION_STANDARD_CEA,
+	WFD_RESOLUTION_STANDARD_VESA,
+	WFD_RESOLUTION_STANDARD_HH,
+};
+
+union wfd_sube
+{
+	enum wfd_sube_id id;
+
+	struct {
+		enum wfd_sube_id id;
+		uint16_t dev_info;
+		uint16_t rtsp_port;
+		uint16_t max_throughput;
+	} dev_info;
+
+	struct {
+		enum wfd_sube_id id;
+		uint32_t cea;
+		uint32_t vesa;
+		uint32_t hh;
+		uint8_t native;
+		uint8_t profiles;
+		uint8_t levels;
+		uint8_t latency;
+		uint16_t min_slice_size;
+		uint16_t slice_enc_params;
+		uint8_t video_frame_rate_ctl;
+	} video_formats;
+
+	struct {
+		enum wfd_sube_id id;
+		uint32_t lpcm_modes;
+		uint8_t lpcm_dec_latency;
+		uint32_t aac_modes;
+		uint8_t aac_dec_latency;
+		uint32_t ac3_modes;
+		uint8_t ac3_dec_latency;
+	} audio_formats;
+
+	struct {
+		enum wfd_sube_id id;
+		uint16_t caps;
+	} extended_caps;
+};
+
 struct wfd_resolution
 {
 	uint16_t index;
@@ -31,8 +107,57 @@ struct wfd_resolution
 };
 
 void wfd_print_resolutions(void);
+int wfd_get_resolutions(enum wfd_resolution_standard std,
+				int index,
+				struct wfd_resolution *out);
 int vfd_get_cea_resolution(uint32_t mask, int *hres, int *vres);
 int vfd_get_vesa_resolution(uint32_t mask, int *hres, int *vres);
 int vfd_get_hh_resolution(uint32_t mask, int *hres, int *vres);
+int wfd_sube_parse(const char *in, union wfd_sube *out);
+
+static inline int wfd_sube_device_get_type(const union wfd_sube *sube)
+{
+	assert(wfd_sube_is_device_info(sube));
+
+	return (WFD_DEVINFO_DEV_TYPE_MASK & sube->dev_info.dev_info);
+}
+
+static inline bool wfd_sube_device_is_source(const union wfd_sube *sube)
+{
+	switch(wfd_sube_device_get_type(sube)) {
+		case 0:
+		case 3:
+			return true;
+	}
+
+	return false;
+}
+
+static inline bool wfd_sube_device_is_sink(const union wfd_sube *sube)
+{
+	switch(wfd_sube_device_get_type(sube)) {
+		case 1:
+		case 2:
+		case 3:
+			return true;
+	}
+
+	return false;
+}
+
+static inline int wfd_sube_src_support_coupled_sink(const union wfd_sube *sube)
+{
+	assert(WFD_SUBE_ID_DEVICE_INFO == sube->id);
+
+	return !!(WFD_DEVINFO_SRC_COUPLED_SINK_MASK & sube->dev_info.dev_info);
+}
+
+static inline uint16_t wfd_sube_device_get_rtsp_port(const union wfd_sube *sube)
+{
+	assert(wfd_sube_is_device_info(sube));
+
+	return sube->dev_info.rtsp_port;
+}
 
 #endif /* WFD_H */
+

src/ctl/wfdctl.c

@@ -16,6 +16,7 @@
  * You should have received a copy of the GNU Lesser General Public License
  * along with MiracleCast; If not, see <http://www.gnu.org/licenses/>.
  */
+#define LOG_SUBSYSTEM "wfdctl"
 
 #include <errno.h>
 #include <locale.h>
@@ -25,110 +26,298 @@
 #include <stdlib.h>
 #include <string.h>
 #include <strings.h>
-#include <sys/signalfd.h>
 #include <sys/time.h>
-#include <systemd/sd-bus.h>
 #include <systemd/sd-event.h>
 #include <systemd/sd-journal.h>
 #include <time.h>
 #include <unistd.h>
 #include "ctl.h"
 #include "wfd.h"
+#include "util.h"
+#include "wfd-dbus.h"
 #include "shl_macro.h"
+#include "shl_htable.h"
 #include "shl_util.h"
+#include "shl_log.h"
 #include "config.h"
 
-struct ctl_wfd
-{
-	sd_event *loop;
-	sd_bus *bus;
-	struct ctl_wifi *wifi;
-	struct ctl_src *src;
-};
-
-static struct ctl_wfd *ctl_wfd;
-
 void ctl_wfd_free(struct ctl_wfd *wfd);
 
-int ctl_wfd_new(struct ctl_wfd **out)
+static struct ctl_wfd *wfd = NULL;
+static struct wfd_dbus *wfd_dbus = NULL;
+
+struct wfd_dbus * wfd_dbus_get()
+{
+	return wfd_dbus;
+}
+
+struct ctl_wfd * ctl_wfd_get()
+{
+	return wfd;
+}
+
+int ctl_wfd_new(struct ctl_wfd **out, sd_event *loop, sd_bus *bus)
 {
 	int r;
 	struct ctl_wfd *wfd = calloc(1, sizeof(struct ctl_wfd));
 	if(!wfd) {
-		r = -ENOMEM;
+		return -ENOMEM;
-		goto end;
 	}
 
-	r = sd_event_default(&wfd->loop);
+	r = ctl_wifi_new(&wfd->wifi, bus);
 	if(0 > r) {
-		goto free_wfd;
+		ctl_wfd_free(wfd);
+		return -ENOMEM;
 	}
 
-	r = sd_bus_default_system(&wfd->bus);
+	shl_htable_init_str(&wfd->sinks);
-	if (r < 0) {
+	shl_htable_init_u64(&wfd->sessions);
-		goto free_wfd;
+	wfd->loop = sd_event_ref(loop);
-	}
 
 	*out = wfd;
 
-	r = 0;
+	return 0;
+}
 
-	goto end;
+static void ctl_wfd_clear_sink(char **elem, void *ctx)
+{
+	wfd_sink_free(wfd_sink_from_htable(elem));
+}
 
-free_wfd:
+static void ctl_wfd_clear_session(uint64_t *elem, void *ctx)
-	ctl_wfd_free(wfd);
+{
-end:
+	wfd_session_free(wfd_session_from_htable(elem));
-	return r;
 }
 
 void ctl_wfd_free(struct ctl_wfd *wfd)
 {
-	if(wfd->bus) {
+	if(!wfd) {
-		sd_bus_unref(wfd->bus);
+		return;
+	}
+
+	shl_htable_clear_str(&wfd->sinks, ctl_wfd_clear_sink, NULL);
+	shl_htable_clear_u64(&wfd->sessions, ctl_wfd_clear_session, NULL);
+
+	if(wfd->loop) {
+		sd_event_unref(wfd->loop);
 	}
 
 	free(wfd);
 }
 
+static int ctl_wfd_add_sink(struct ctl_wfd *wfd, struct wfd_sink *sink)
+{
+	int r = shl_htable_lookup_str(&wfd->sinks,
+					wfd_sink_get_label(sink),
+					NULL,
+					NULL);
+	if(r) {
+		return -EEXIST;
+	}
+
+	r = shl_htable_insert_str(&wfd->sinks,
+					wfd_sink_to_htable(sink),
+					NULL);
+	if(0 <= r) {
+		++wfd->n_sinks;
+	}
+
+	return r;
+}
+
+int ctl_wfd_find_sink_by_label(struct ctl_wfd *wfd,
+				const char *label,
+				struct wfd_sink **out)
+{
+	char **entry;
+	int r = shl_htable_lookup_str(&wfd->sinks, label, NULL, &entry);
+	if(r) {
+		*out = wfd_sink_from_htable(entry);
+	}
+
+	return r;
+}
+
+static int ctl_wfd_remove_sink_by_label(struct ctl_wfd *wfd, const char *label)
+{
+	char **entry;
+	if(!shl_htable_remove_str(&wfd->sinks, label, NULL, &entry)) {
+		return 0;
+	}
+
+	wfd_sink_free(wfd_sink_from_htable(entry));
+
+	return 1;
+}
+
+int ctl_wfd_add_session(struct ctl_wfd *wfd, struct wfd_session *s)
+{
+	int r;
+
+	assert(wfd);
+	assert(s && !s->id);
+
+	wfd_session_set_id(s, ++wfd->id_pool);
+
+	r = shl_htable_insert_u64(&wfd->sessions, wfd_session_to_htable(s));
+	if(0 > r) {
+		return r;
+	}
+
+	++wfd->n_sessions;
+
+	return r;
+}
+
+int ctl_wfd_find_session_by_id(struct ctl_wfd *wfd,
+				unsigned int id,
+				struct wfd_session **out)
+{
+	uint64_t *entry;
+	int r = shl_htable_lookup_u64(&wfd->sessions, id, &entry);
+	if(r && out) {
+		*out = wfd_session_from_htable(entry);
+	}
+
+	return r;
+}
+
+int ctl_wfd_remove_session_by_id(struct ctl_wfd *wfd,
+				uint64_t id,
+				struct wfd_session **out)
+{
+	uint64_t *entry;
+	int r = shl_htable_remove_u64(&wfd->sessions, id, &entry);
+	if(r && out) {
+		*out = wfd_session_from_htable(entry);
+	}
+
+	return r;
+}
+
+static int ctl_wfd_handle_signal(sd_event_source *s,
+				const struct signalfd_siginfo *si,
+				void *userdata)
+{
+	struct ctl_wfd *wfd = userdata;
+	sd_event_exit(wfd->loop, 0);
+
+	return 0;
+}
+
+int ctl_wfd_init(struct ctl_wfd *wfd)
+{
+	int i;
+	const int signals[] = { SIGINT, SIGHUP, SIGQUIT, SIGTERM };
+	int r = ctl_wifi_fetch(wfd->wifi);
+	if(0 > r) {
+		goto end;
+	}
+
+	for(i = 0; i < SHL_ARRAY_LENGTH(signals); i ++) {
+		sigset_t mask;
+		sigemptyset(&mask);
+		sigaddset(&mask, signals[i]);
+		r = sigprocmask(SIG_BLOCK, &mask, NULL);
+		if(0 > r) {
+			break;
+		}
+
+		r = sd_event_add_signal(wfd->loop,
+						NULL,
+						signals[i],
+						ctl_wfd_handle_signal,
+						wfd);
+		if(0 > r) {
+			break;
+		}
+	}
+
+end:
+	return r;
+}
+
 int ctl_wfd_run(struct ctl_wfd *wfd)
 {
 	return sd_event_loop(wfd->loop);
 }
 
 /* Callbacks from ctl-src */
-void ctl_fn_src_connected(struct ctl_src *s)
+void wfd_fn_src_connected(struct wfd_src *s)
 {
 }
 
-void ctl_fn_src_disconnected(struct ctl_src *s)
+void wfd_fn_src_disconnected(struct wfd_src *s)
 {
 }
 
-void ctl_fn_src_setup(struct ctl_src *s)
+void wfd_fn_src_setup(struct wfd_src *s)
 {
 }
 
-void ctl_fn_src_playing(struct ctl_src *s)
+void wfd_fn_src_playing(struct wfd_src *s)
 {
 }
 
 void ctl_fn_peer_new(struct ctl_peer *p)
 {
+	union wfd_sube sube;
+	int r = wfd_sube_parse(p->wfd_subelements, &sube);
+	if(0 > r) {
+		log_debug("invalid subelement: '%s'", p->wfd_subelements);
+		return;
+	}
+
+	if(wfd_sube_device_is_sink(&sube)) {
+		struct wfd_sink *sink;
+		if(0 > wfd_sink_new(&sink, p, &sube)) {
+			log_warning("failed to create sink (%s): %s",
+							p->p2p_mac,
+							strerror(errno));
+			return;
+		}
+		if(0 > ctl_wfd_add_sink(wfd, sink)) {
+			wfd_sink_free(sink);
+			log_warning("failed to add sink (%s): %s",
+							p->p2p_mac,
+							strerror(errno));
+			return;
+		}
+		/*if(0 > wfd_dbus_notify_new_sink(wfd_dbus, p->p2p_mac)) {*/
+			/*log_warning("failed to notify about newly added sink (%s): %s",*/
+							/*p->p2p_mac,*/
+							/*strerror(errno));*/
+			/*return;*/
+		/*}*/
+		log_debug("sink added: %s (%s)",
+						wfd_sink_get_label(sink),
+						p->friendly_name);
+	}
 }
 
 void ctl_fn_peer_free(struct ctl_peer *p)
 {
+	union wfd_sube sube;
+	int r = wfd_sube_parse(p->wfd_subelements, &sube);
+	if(0 > r) {
+		log_debug("invalid subelement: %s", p->wfd_subelements);
+		return;
+	}
+
+	if(wfd_sube_device_is_sink(&sube)) {
+		ctl_wfd_remove_sink_by_label(wfd, p->label);
+	}
 }
 
 void ctl_fn_peer_provision_discovery(struct ctl_peer *p,
-					 const char *prov,
+				const char *prov,
-					 const char *pin)
+				const char *pin)
 {
 }
 
 void ctl_fn_peer_go_neg_request(struct ctl_peer *p,
-					 const char *prov,
+				const char *prov,
-					 const char *pin)
+				const char *pin)
 {
 }
 
@@ -156,267 +345,68 @@ void cli_fn_help()
 {
 }
 
-static int ctl_wfd_enum(sd_bus *bus,
-				const char *path,
-				void *userdata,
-				char ***out,
-				sd_bus_error *out_error)
-{
-	int r = 0, i = 0;
-	char **nodes;
-
-	if(strcmp("/org/freedesktop/miracle/wfd", path)) {
-		return 0;
-	}
-   
-	nodes = malloc(sizeof(char *) * 3);
-	if(!nodes) {
-		r = -ENOMEM;
-		goto end;
-	}
-
-	nodes[i] = strdup("/org/freedesktop/miracle/wfd/sink");
-	if(!nodes[i ++]) {
-		r = -ENOMEM;
-		goto end;
-	}
-
-	nodes[i] = strdup("/org/freedesktop/miracle/wfd/session");
-	if(!nodes[i ++]) {
-		r = -ENOMEM;
-		goto end;
-	}
-
-	nodes[i ++] = NULL;
-	*out = nodes;
-
-	return 0;
-
-free_nodes:
-	while(i --) {
-		free(nodes[i]);
-	}
-	free(nodes);
-end:
-	return r;
-}
-
-static int ctl_wfd_find_sink(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				void *userdata,
-				void **ret_found,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_find_session(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				void *userdata,
-				void **ret_found,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_sink_start_session(sd_bus_message *m,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_sink_get_audio_formats(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				const char *property,
-				sd_bus_message *reply,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_sink_get_video_formats(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				const char *property,
-				sd_bus_message *reply,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_sink_has_video(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				const char *property,
-				sd_bus_message *reply,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_sink_get_peer(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				const char *property,
-				sd_bus_message *reply,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_sink_has_audio(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				const char *property,
-				sd_bus_message *reply,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_session_end(sd_bus_message *m,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_session_get_sink(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				const char *property,
-				sd_bus_message *reply,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static int ctl_wfd_session_get_state(sd_bus *bus,
-				const char *path,
-				const char *interface,
-				const char *property,
-				sd_bus_message *reply,
-				void *userdata,
-				sd_bus_error *ret_error)
-{
-	return 0;
-}
-
-static const sd_bus_vtable ctl_wfd_vtable[] = {
-	SD_BUS_VTABLE_START(0),
-	SD_BUS_VTABLE_END,
-};
-
-static const sd_bus_vtable ctl_wfd_sink_vtable[] = {
-	SD_BUS_VTABLE_START(0),
-	SD_BUS_METHOD("StartSession", NULL, "o", ctl_wfd_sink_start_session, 0),
-	SD_BUS_PROPERTY("AudioFormats", "a{sv}", ctl_wfd_sink_get_audio_formats, 0, SD_BUS_VTABLE_PROPERTY_CONST),
-	SD_BUS_PROPERTY("VideoFormats", "a{sv}", ctl_wfd_sink_get_video_formats, 0, SD_BUS_VTABLE_PROPERTY_CONST),
-	SD_BUS_PROPERTY("HasAudio", "b", ctl_wfd_sink_has_audio, 0, SD_BUS_VTABLE_PROPERTY_CONST),
-	SD_BUS_PROPERTY("HasVideo", "b", ctl_wfd_sink_has_video, 0, SD_BUS_VTABLE_PROPERTY_CONST),
-	SD_BUS_PROPERTY("Peer", "o", ctl_wfd_sink_get_peer, 0, SD_BUS_VTABLE_PROPERTY_CONST),
-	SD_BUS_VTABLE_END,
-};
-
-static const sd_bus_vtable ctl_wfd_session_vtable[] = {
-	SD_BUS_VTABLE_START(0),
-	SD_BUS_METHOD("End", NULL, NULL, ctl_wfd_session_end, 0),
-	SD_BUS_PROPERTY("Sink", "o", ctl_wfd_session_get_sink, 0, SD_BUS_VTABLE_PROPERTY_CONST),
-	SD_BUS_PROPERTY("State", "i", ctl_wfd_session_get_state, 0, SD_BUS_VTABLE_PROPERTY_EMITS_CHANGE),
-	SD_BUS_VTABLE_END,
-};
-
-int ctl_wfd_expose(struct ctl_wfd *wfd)
-{
-	int r;
-
-	r = sd_bus_add_object_vtable(wfd->bus,
-					NULL,
-					"/org/freedesktop/miracle/wfd",
-					"org.freedesktop.miracle.wfd.Display",
-					ctl_wfd_vtable,
-					NULL);
-	if(0 > r) {
-		goto end;
-	}
-
-	r = sd_bus_add_fallback_vtable(wfd->bus,
-					NULL,
-					"/org/freedesktop/miracle/wfd/sink",
-					"org.freedesktop.miracle.wfd.Sink",
-					ctl_wfd_sink_vtable,
-					ctl_wfd_find_sink,
-					NULL);
-	if(0 > r) {
-		goto end;
-	}
-
-	r = sd_bus_add_fallback_vtable(wfd->bus,
-					NULL,
-					"/org/freedesktop/miracle/wfd/session",
-					"org.freedesktop.miracle.wfd.Session",
-					ctl_wfd_session_vtable,
-					ctl_wfd_find_session,
-					NULL);
-	if(0 > r) {
-		goto end;
-	}
-
-	r = sd_bus_add_node_enumerator(wfd->bus,
-					NULL,
-					"/org/freedesktop/miracle/wfd",
-					ctl_wfd_enum,
-					NULL);
-	if(0 > r) {
-		goto end;
-	}
-
-	r = sd_bus_add_object_manager(wfd->bus, NULL, "/org/freedesktop/miracle/wfd");
-	if(0 > r) {
-		goto end;
-	}
-
-	r = sd_bus_attach_event(wfd->bus, wfd->loop, SD_EVENT_PRIORITY_NORMAL);
-	if(0 > r) {
-		goto end;
-	}
-
-	r = sd_bus_request_name(wfd->bus, "org.freedesktop.miracle.wfd", 0);
-
-end:
-	return r;
-}
-
 int main(int argc, char **argv)
 {
 	int r;
+	sd_event *loop;
+	sd_bus *bus;
 
 	setlocale(LC_ALL, "");
 
-	r = ctl_wfd_new(&ctl_wfd);
+	if(getenv("LOG_LEVEL")) {
+		log_max_sev = log_parse_arg(getenv("LOG_LEVEL"));
+	}
+
+	r = sd_event_default(&loop);
 	if(0 > r) {
 		goto end;
 	}
 
-	r = ctl_wfd_expose(ctl_wfd);
+	r = sd_bus_default_system(&bus);
 	if(0 > r) {
-		goto free_wfd;
+		goto unref_loop;
 	}
 
-	r = ctl_wfd_run(ctl_wfd);
+	r = sd_bus_attach_event(bus, loop, 0);
+	if(0 > r) {
+		goto unref_bus;
+	}
 
-free_wfd:
+	r = ctl_wfd_new(&wfd, loop, bus);
-	ctl_wfd_free(ctl_wfd);
+	if(0 > r) {
+		goto bus_detach_event;;
+	}
+
+	r = ctl_wfd_init(wfd);
+	if(0 > r) {
+		goto free_ctl_wfd;
+	}
+
+	r = wfd_dbus_new(&wfd_dbus, loop, bus);
+	if(0 > r) {
+		goto free_ctl_wfd;
+	}
+
+	r = wfd_dbus_expose(wfd_dbus);
+	if(0 > r) {
+		goto free_ctl_wfd;
+	}
+
+	r = ctl_wfd_run(wfd);
+	if(0 > r) {
+		log_warning("%s\n", strerror(errno));
+	}
+
+	wfd_dbus_free(wfd_dbus);
+
+free_ctl_wfd:
+	ctl_wfd_free(wfd);
+bus_detach_event:
+	sd_bus_detach_event(bus);
+unref_bus:
+	sd_bus_unref(bus);
+unref_loop:
+	sd_event_unref(loop);
 end:
 	return r < 0 ? EXIT_FAILURE : EXIT_SUCCESS;
 }