set up the SCO connection and make noise

1 files changed, 266 insertions, 29 deletions

diff --git a/make-hsp-ag.c b/make-hsp-ag.c
index e4c5d01..b72181c 100644
--- a/make-hsp-ag.c
+++ b/make-hsp-ag.c
@@ -19,13 +19,18 @@
 
 #include <gio/gio.h>
 
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/sco.h>
+
 
 /* ---------------------------------------------------------------------------------------------------- */
 
 static GDBusNodeInfo *introspection_data = NULL;
 
-static GDBusProxy *manager;
-
 /* Introspection data for the service we are exporting */
 static const gchar introspection_xml[] =
   "<node>"
@@ -43,10 +48,23 @@ static const gchar introspection_xml[] =
   "      <arg type='a{sv}' name='opts' direction='in'/>"
   "    </method>"
   "  </interface>"
+  "  <interface name='org.bluez.MediaEndpoint1'>"
+  "    <method name='SelectConfiguration'>"
+  "      <arg type='ay' name='caps' direction='in'/>"
+  "    </method>"
+  "    <method name='SetConfiguration'>"
+  "      <arg type='o' name='transport' direction='in'/>"
+  "      <arg type='ay' name='config' direction='in'/>"
+  "    </method>"
+  "    <method name='ClearConfiguration'>"
+  "    </method>"
+  "    <method name='Release'>"
+  "    </method>"
+  "  </interface>"
   "</node>";
 
 static gboolean
-my_io_cb (GIOChannel *source, GIOCondition condition, gpointer data)
+rfcomm_io_cb (GIOChannel *source, GIOCondition condition, gpointer data)
 {
   gchar buf[512];
   GIOStatus st;
@@ -76,6 +94,8 @@ my_io_cb (GIOChannel *source, GIOCondition condition, gpointer data)
       write (fd, "OK\r\n", 3);
     }
     else if (g_str_has_prefix (buf, "AT+CMER=")) {
+      write (fd, "+VGM:15\r\n", 9);
+      write (fd, "+VGS:15\r\n", 9);
       write (fd, "OK\r\n", 3);
     }
     else {
@@ -85,8 +105,115 @@ my_io_cb (GIOChannel *source, GIOCondition condition, gpointer data)
   return TRUE;
 }
 
+static struct sco_options so;
+
+static gboolean
+sco_io_cb (GIOChannel *source, GIOCondition condition, gpointer data)
+{
+  gchar buf[1024];
+  gsize length;
+  gint fd = g_io_channel_unix_get_fd (source);
+
+  if (condition & G_IO_IN) {
+    length = recv (fd, buf, 1024, 0);
+    if (length <= 0)
+      perror ("recv()");
+    g_print ("-");
+  } else if (condition & G_IO_OUT) {
+    gint i;
+
+    for (i = 0; i < 48; i++)
+      buf[i] = g_random_int_range (0, 255);
+
+    length = send (fd, buf, 48, 0);
+    if (length != 48)
+      perror ("send()");
+
+    g_print ("+");
+    usleep(2500);
+  } else if (condition & (G_IO_ERR | G_IO_HUP)) {
+    g_print ("got error\n");
+    return FALSE;
+  }
+  return TRUE;
+}
+
+static int
+start_codec_connection (gchar *src_addr,
+                        gchar *dst_addr)
+{
+  struct sockaddr_sco addr;
+  int err, i;
+  int sock;
+  GIOCondition cond;
+  GIOChannel *io;
+  bdaddr_t src;
+  bdaddr_t dst;
+  int voice = 0x60;
+  socklen_t len;
+
+  for (i = 5; i >= 0; i--, src_addr += 3)
+    src.b[i] = strtol(src_addr, NULL, 16);
+  for (i = 5; i >= 0; i--, dst_addr += 3)
+    dst.b[i] = strtol(dst_addr, NULL, 16);
+
+  sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
+  if (sock < 0) {
+    g_printerr("socket(SEQPACKET, SCO)");
+    return -1;
+  }
+
+  memset(&addr, 0, sizeof(addr));
+  addr.sco_family = AF_BLUETOOTH;
+  bacpy(&addr.sco_bdaddr, &src);
+
+  if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+    perror("bind()");
+    return -1;
+  }
+
+  if (voice) {
+    struct bt_voice opts;
+
+    /* SCO voice setting */
+    memset(&opts, 0, sizeof(opts));
+    opts.setting = voice;
+    if (setsockopt(sock, SOL_BLUETOOTH, BT_VOICE, &opts, sizeof(opts)) < 0) {
+      perror("setsockopt()");
+      return -1;
+    }
+  }
+
+  memset(&addr, 0, sizeof(addr));
+  addr.sco_family = AF_BLUETOOTH;
+  bacpy(&addr.sco_bdaddr, &dst);
+
+  err = connect(sock, (struct sockaddr *) &addr, sizeof(addr));
+  if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS)) {
+    perror("connect()");
+    return -1;
+  }
+
+  len = sizeof(so);
+  if (getsockopt(sock, SOL_SCO, SCO_OPTIONS, &so, &len) < 0) {
+    perror("getsockopt()");
+    return -1;
+  }
+
+  g_print ("MTU %d\n", so.mtu);
+
+  io = g_io_channel_unix_new (sock);
+  g_io_channel_set_close_on_unref(io, TRUE);
+  g_io_channel_set_flags(io, G_IO_FLAG_NONBLOCK, NULL);
+
+  cond = G_IO_IN | G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
+  g_io_add_watch(io, cond, sco_io_cb, NULL);
+
+  return 0;
+}
+
 static void
-handle_method_call (GDBusConnection       *connection,
+profile_method_call (GDBusConnection       *connection,
                     const gchar           *sender,
                     const gchar           *object_path,
                     const gchar           *interface_name,
@@ -120,37 +247,30 @@ handle_method_call (GDBusConnection       *connection,
     g_print ("NewConnection %s %d\n", obj, fd);
 
     channel = g_io_channel_unix_new (fd);
-    g_io_add_watch (channel, G_IO_IN, my_io_cb, NULL);
+    g_io_add_watch (channel, G_IO_IN, rfcomm_io_cb, NULL);
+
+    start_codec_connection ("00:15:83:50:D1:E1", "13:11:14:AA:00:B8");
   } else
-    g_print ("%s\n", method_name);
+    g_print ("Unhandled %s\n", method_name);
 
   g_dbus_method_invocation_return_value (invocation, NULL);
 }
 
-/* for now */
-static const GDBusInterfaceVTable interface_vtable =
+static const GDBusInterfaceVTable profile_interface_vtable =
 {
-  handle_method_call,
+  profile_method_call,
   NULL,
   NULL
 };
 
-int
-main (int argc, char *argv[])
+
+static int
+register_profile (GDBusConnection *connection)
 {
-  GDBusConnection * connection;
   GError *error = NULL;
-  guint owner_id;
-  GMainLoop *loop;
+  GDBusProxy *manager;
   guint registration_id;
 
-  g_print ("connecting to system bus\n");
-  connection = g_bus_get_sync (G_BUS_TYPE_SYSTEM, NULL, &error);
-  if (error != NULL) {
-    g_printerr ("error getting bus: %s", error->message);
-    return -1;
-  }
-
   g_print ("making proxy for ProfileManager1\n");
   manager = g_dbus_proxy_new_sync (connection,
       G_DBUS_PROXY_FLAGS_NONE,
@@ -161,13 +281,10 @@ main (int argc, char *argv[])
     return -1;
   }
 
-  introspection_data = g_dbus_node_info_new_for_xml (introspection_xml, NULL);
-  g_assert (introspection_data != NULL);
-
   registration_id = g_dbus_connection_register_object (connection,
-                                                       "/org/myorg/MyObject",
+                                                       "/org/test/Profile",
                                                        introspection_data->interfaces[0],
-                                                       &interface_vtable,
+                                                       &profile_interface_vtable,
                                                        NULL,
                                                        NULL,  /* user_data_free_func */
                                                        NULL); /* GError** */
@@ -177,7 +294,7 @@ main (int argc, char *argv[])
   g_dbus_proxy_call_sync (manager,
                         "RegisterProfile",
                         g_variant_new ("(osa{sv})",
-                                       "/org/myorg/MyObject",
+                                       "/org/test/Profile",
                                        "0000111f-0000-1000-8000-00805f9b34fb", NULL),
                         G_DBUS_CALL_FLAGS_NONE,
                         -1,
@@ -185,16 +302,136 @@ main (int argc, char *argv[])
                         &error);
   if (error) {
     g_printerr ("error registering %s\n", error->message);
+    return -1;
   }
   g_print ("Profile registered\n");
 
+  return 0;
+}
+
+#if 0
+static void
+endpoint_method_call (GDBusConnection       *connection,
+                    const gchar           *sender,
+                    const gchar           *object_path,
+                    const gchar           *interface_name,
+                    const gchar           *method_name,
+                    GVariant              *parameters,
+                    GDBusMethodInvocation *invocation,
+                    gpointer               user_data)
+{
+
+  if (g_strcmp0 (method_name, "SelectConfiguration") == 0) {
+    g_print ("SelectConfiguration\n");
+  } else if (g_strcmp0 (method_name, "SetConfiguration") == 0) {
+    g_print ("SetConfiguration\n");
+  } else if (g_strcmp0 (method_name, "ClearConfiguration") == 0) {
+    g_print ("ClearConfiguration\n");
+  } else if (g_strcmp0 (method_name, "Release") == 0) {
+    g_print ("Release\n");
+  } else {
+    g_print ("Unhandled %s\n", method_name);
+  }
+
+  g_dbus_method_invocation_return_value (invocation, NULL);
+}
+
+
+static const GDBusInterfaceVTable endpoint_interface_vtable =
+{
+  endpoint_method_call,
+  NULL,
+  NULL
+};
+
+static int
+register_endpoint (GDBusConnection * connection)
+{
+  GError *error = NULL;
+  GDBusProxy *media;
+  guint registration_id;
+  GVariantBuilder *b;
+
+  g_print ("Register endpoint\n");
+
+  media = g_dbus_proxy_new_sync (connection,
+      G_DBUS_PROXY_FLAGS_NONE,
+      NULL, "org.bluez", "/org/bluez/hci0", "org.bluez.Media1",
+      NULL, &error);
+  if (error != NULL) {
+    g_printerr ("error getting Media1: %s", error->message);
+    return -1;
+  }
+
+  registration_id = g_dbus_connection_register_object (connection,
+                                                      "/org/test/Endpoint",
+                                                       introspection_data->interfaces[1],
+                                                       &endpoint_interface_vtable,
+                                                       NULL,
+                                                       NULL,  /* user_data_free_func */
+                                                       NULL); /* GError** */
+  g_assert (registration_id > 0);
+
+  b = g_variant_builder_new (G_VARIANT_TYPE ("a{sv}"));
+  g_variant_builder_add (b, "{sv}",
+      "UUID", g_variant_new_string ("0000111f-0000-1000-8000-00805f9b34fb"));
+  g_variant_builder_add (b, "{sv}",
+      "Codec", g_variant_new_byte (1));
+  g_variant_builder_add (b, "{sv}",
+      "Capabilities", g_variant_new_array (G_VARIANT_TYPE_BYTE, NULL, 0));
+
+  g_dbus_proxy_call_sync (media,
+                      "RegisterEndpoint",
+                      g_variant_new ("(oa{sv})",
+                                     "/org/test/Endpoint",
+                                     b),
+                      G_DBUS_CALL_FLAGS_NONE,
+                      -1,
+                      NULL,
+                      &error);
+  g_variant_builder_unref (b);
+
+  if (error) {
+    g_printerr ("error registering %s\n", error->message);
+    return -1;
+  }
+  g_print ("Endpoint registered\n");
+
+  return 0;
+}
+#endif
+
+int
+main (int argc, char *argv[])
+{
+  GDBusConnection * connection;
+  GError *error = NULL;
+  GMainLoop *loop;
+  guint registration_id;
+
+  g_print ("connecting to system bus\n");
+  connection = g_bus_get_sync (G_BUS_TYPE_SYSTEM, NULL, &error);
+  if (error != NULL) {
+    g_printerr ("error getting bus: %s", error->message);
+    return -1;
+  }
+
+  introspection_data = g_dbus_node_info_new_for_xml (introspection_xml, NULL);
+  g_assert (introspection_data != NULL);
+
+  if (register_profile (connection) < 0)
+    return -1;
+
+#if 0
+  if (register_endpoint (connection) < 0)
+    return -1;
+#endif
+
   g_print ("going into mainloop\n");
   loop = g_main_loop_new (NULL, FALSE);
   g_main_loop_run (loop);
   g_print ("exit mainloop\n");
 
-  g_bus_unown_name (owner_id);
-
   g_dbus_node_info_unref (introspection_data);
 
   return 0;