tools/btpclient: Add connect, disconnect commands

This patch adds start and stop connect, disconnect commands for btp
client.

Change-Id: Ic4f3ef69664c6c9204812afc475a00e114525f11
Signed-off-by: Amit Purwar <amit.purwar@samsung.com>

diff --git a/tools/btpclient.c b/tools/btpclient.c
index 0986c98..6ac43fa 100644 (file)
--- a/tools/btpclient.c
+++ b/tools/btpclient.c
@@ -113,6 +113,17 @@ static char *dupuuid2str(const uint8_t *uuid, uint8_t len)
        }
 }
 
+static bool match_dev_addr_type(const char *addr_type_str, uint8_t addr_type)
+{
+       if (addr_type == BTP_GAP_ADDR_PUBLIC && strcmp(addr_type_str, "public"))
+               return false;
+
+       if (addr_type == BTP_GAP_ADDR_RANDOM && strcmp(addr_type_str, "random"))
+               return false;
+
+       return true;
+}
+
 
 static bool str2addr(const char *str, uint8_t *addr)
 {
@@ -165,6 +176,34 @@ static struct btp_adapter *find_adapter_by_path(const char *path)
        return NULL;
 }
 
+static struct btp_device *find_device_by_address(struct btp_adapter *adapter,
+                                                       const bdaddr_t *addr,
+                                                       uint8_t addr_type)
+{
+       const struct l_queue_entry *entry;
+       const char *str;
+       char addr_str[18];
+
+       if (!ba2str(addr, addr_str))
+               return NULL;
+
+       for (entry = l_queue_get_entries(adapter->devices); entry;
+                                                       entry = entry->next) {
+               struct btp_device *device = entry->data;
+
+               l_dbus_proxy_get_property(device->proxy, "Address", "s", &str);
+               if (strcmp(str, addr_str))
+                       continue;
+
+               l_dbus_proxy_get_property(device->proxy, "AddressType", "s",
+                                                                       &str);
+               if (match_dev_addr_type(str, addr_type))
+                       return device;
+       }
+
+       return NULL;
+}
+
 static void btp_gap_read_commands(uint8_t index, const void *param,
                                        uint16_t length, void *user_data)
 {
@@ -188,6 +227,8 @@ static void btp_gap_read_commands(uint8_t index, const void *param,
        commands |= (1 << BTP_OP_GAP_STOP_ADVERTISING);
        commands |= (1 << BTP_OP_GAP_START_DISCOVERY);
        commands |= (1 << BTP_OP_GAP_STOP_DISCOVERY);
+       commands |= (1 << BTP_OP_GAP_CONNECT);
+       commands |= (1 << BTP_OP_GAP_DISCONNECT);
 
        commands = L_CPU_TO_LE16(commands);
 
@@ -1313,6 +1354,129 @@ static void btp_gap_stop_discovery(uint8_t index, const void *param,
                                        stop_discovery_reply, NULL, NULL);
 }
 
+static void connect_reply(struct l_dbus_proxy *proxy,
+                               struct l_dbus_message *result, void *user_data)
+{
+       uint8_t adapter_index = L_PTR_TO_UINT(user_data);
+       struct btp_adapter *adapter = find_adapter_by_index(adapter_index);
+
+       if (!adapter) {
+               btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
+                                                               BTP_ERROR_FAIL);
+               return;
+       }
+
+       if (l_dbus_message_is_error(result)) {
+               const char *name, *desc;
+
+               l_dbus_message_get_error(result, &name, &desc);
+               l_error("Failed to connect (%s), %s", name, desc);
+
+               btp_send_error(btp, BTP_GAP_SERVICE, adapter_index,
+                                                               BTP_ERROR_FAIL);
+               return;
+       }
+
+       btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_CONNECT, adapter_index, 0,
+                                                                       NULL);
+}
+
+static void btp_gap_connect(uint8_t index, const void *param, uint16_t length,
+                                                               void *user_data)
+{
+       struct btp_adapter *adapter = find_adapter_by_index(index);
+       const struct btp_gap_connect_cp *cp = param;
+       struct btp_device *device;
+       bool prop;
+       uint8_t status = BTP_ERROR_FAIL;
+
+       if (!adapter) {
+               status = BTP_ERROR_INVALID_INDEX;
+               goto failed;
+       }
+
+       /* Adapter needs to be powered to be able to connect */
+       if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
+                                                                       !prop)
+               goto failed;
+
+       device = find_device_by_address(adapter, &cp->address,
+                                                       cp->address_type);
+
+       if (!device)
+               goto failed;
+
+       l_dbus_proxy_method_call(device->proxy, "Connect", NULL, connect_reply,
+                                       L_UINT_TO_PTR(adapter->index), NULL);
+
+       return;
+
+failed:
+       btp_send_error(btp, BTP_GAP_SERVICE, index, status);
+}
+
+static void disconnect_reply(struct l_dbus_proxy *proxy,
+                               struct l_dbus_message *result, void *user_data)
+{
+       uint8_t adapter_index = L_PTR_TO_UINT(user_data);
+       struct btp_adapter *adapter = find_adapter_by_index(adapter_index);
+
+       if (!adapter) {
+               btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
+                                                               BTP_ERROR_FAIL);
+               return;
+       }
+
+       if (l_dbus_message_is_error(result)) {
+               const char *name, *desc;
+
+               l_dbus_message_get_error(result, &name, &desc);
+               l_error("Failed to disconnect (%s), %s", name, desc);
+
+               btp_send_error(btp, BTP_GAP_SERVICE, adapter_index,
+                                                               BTP_ERROR_FAIL);
+               return;
+       }
+
+       btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_DISCONNECT, adapter_index, 0,
+                                                                       NULL);
+}
+
+static void btp_gap_disconnect(uint8_t index, const void *param,
+                                       uint16_t length, void *user_data)
+{
+       struct btp_adapter *adapter = find_adapter_by_index(index);
+       const struct btp_gap_disconnect_cp *cp = param;
+       uint8_t status = BTP_ERROR_FAIL;
+       struct btp_device *device;
+       bool prop;
+
+       if (!adapter) {
+               status = BTP_ERROR_INVALID_INDEX;
+               goto failed;
+       }
+
+       /* Adapter needs to be powered to be able to connect */
+       if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
+                                                                       !prop)
+               goto failed;
+
+       device = find_device_by_address(adapter, &cp->address,
+                                                       cp->address_type);
+
+       if (!device)
+               goto failed;
+
+       l_dbus_proxy_method_call(device->proxy, "Disconnect", NULL,
+                                       disconnect_reply,
+                                       L_UINT_TO_PTR(adapter->index), NULL);
+
+       return;
+
+failed:
+       btp_send_error(btp, BTP_GAP_SERVICE, index, status);
+}
+
 static void btp_gap_device_found_ev(struct l_dbus_proxy *proxy)
 {
        struct btp_device_found_ev ev;
@@ -1385,6 +1549,12 @@ static void register_gap_service(void)
 
        btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_DISCOVERY,
                                        btp_gap_stop_discovery, NULL, NULL);
+
+       btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_CONNECT, btp_gap_connect,
+                                                               NULL, NULL);
+
+       btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_DISCONNECT,
+                                               btp_gap_disconnect, NULL, NULL);
 }
 
 static void btp_core_read_commands(uint8_t index, const void *param,