Bluetooth: Add support for Get Clock Info mgmt command

This patch implements support for the Get Clock Information mgmt
command. This is done by performing one or two HCI_Read_Clock commands
and creating the response from the stored values in the hci_dev and
hci_conn structs.

Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

1 files changed, 144 insertions, 0 deletions

diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 727ae15f9c36..6faa4616cbfe 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -85,6 +85,7 @@ static const u16 mgmt_commands[] = {
         MGMT_OP_SET_PRIVACY,
         MGMT_OP_LOAD_IRKS,
         MGMT_OP_GET_CONN_INFO,
+        MGMT_OP_GET_CLOCK_INFO,
 };
 
 static const u16 mgmt_events[] = {
@@ -571,6 +572,22 @@ static struct pending_cmd *mgmt_pending_find(u16 opcode, struct hci_dev *hdev)
         return NULL;
 }
 
+static struct pending_cmd *mgmt_pending_find_data(u16 opcode,
+                                                  struct hci_dev *hdev,
+                                                  const void *data)
+{
+        struct pending_cmd *cmd;
+
+        list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
+                if (cmd->user_data != data)
+                        continue;
+                if (cmd->opcode == opcode)
+                        return cmd;
+        }
+
+        return NULL;
+}
+
 static u8 create_scan_rsp_data(struct hci_dev *hdev, u8 *ptr)
 {
         u8 ad_len = 0;
@@ -4820,6 +4837,132 @@ unlock:
         return err;
 }
 
+static void get_clock_info_complete(struct hci_dev *hdev, u8 status)
+{
+        struct mgmt_cp_get_clock_info *cp;
+        struct mgmt_rp_get_clock_info rp;
+        struct hci_cp_read_clock *hci_cp;
+        struct pending_cmd *cmd;
+        struct hci_conn *conn;
+
+        BT_DBG("%s status %u", hdev->name, status);
+
+        hci_dev_lock(hdev);
+
+        hci_cp = hci_sent_cmd_data(hdev, HCI_OP_READ_CLOCK);
+        if (!hci_cp)
+                goto unlock;
+
+        if (hci_cp->which) {
+                u16 handle = __le16_to_cpu(hci_cp->handle);
+                conn = hci_conn_hash_lookup_handle(hdev, handle);
+        } else {
+                conn = NULL;
+        }
+
+        cmd = mgmt_pending_find_data(MGMT_OP_GET_CLOCK_INFO, hdev, conn);
+        if (!cmd)
+                goto unlock;
+
+        cp = cmd->param;
+
+        memset(&rp, 0, sizeof(rp));
+        memcpy(&rp.addr, &cp->addr, sizeof(rp.addr));
+
+        if (status)
+                goto send_rsp;
+
+        rp.local_clock = cpu_to_le32(hdev->clock);
+
+        if (conn) {
+                rp.piconet_clock = cpu_to_le32(conn->clock);
+                rp.accuracy = cpu_to_le16(conn->clock_accuracy);
+        }
+
+send_rsp:
+        cmd_complete(cmd->sk, cmd->index, cmd->opcode, mgmt_status(status),
+                     &rp, sizeof(rp));
+        mgmt_pending_remove(cmd);
+        if (conn)
+                hci_conn_drop(conn);
+
+unlock:
+        hci_dev_unlock(hdev);
+}
+
+static int get_clock_info(struct sock *sk, struct hci_dev *hdev, void *data,
+                         u16 len)
+{
+        struct mgmt_cp_get_clock_info *cp = data;
+        struct mgmt_rp_get_clock_info rp;
+        struct hci_cp_read_clock hci_cp;
+        struct pending_cmd *cmd;
+        struct hci_request req;
+        struct hci_conn *conn;
+        int err;
+
+        BT_DBG("%s", hdev->name);
+
+        memset(&rp, 0, sizeof(rp));
+        bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr);
+        rp.addr.type = cp->addr.type;
+
+        if (cp->addr.type != BDADDR_BREDR)
+                return cmd_complete(sk, hdev->id, MGMT_OP_GET_CLOCK_INFO,
+                                    MGMT_STATUS_INVALID_PARAMS,
+                                    &rp, sizeof(rp));
+
+        hci_dev_lock(hdev);
+
+        if (!hdev_is_powered(hdev)) {
+                err = cmd_complete(sk, hdev->id, MGMT_OP_GET_CLOCK_INFO,
+                                   MGMT_STATUS_NOT_POWERED, &rp, sizeof(rp));
+                goto unlock;
+        }
+
+        if (bacmp(&cp->addr.bdaddr, BDADDR_ANY)) {
+                conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK,
+                                               &cp->addr.bdaddr);
+                if (!conn || conn->state != BT_CONNECTED) {
+                        err = cmd_complete(sk, hdev->id,
+                                           MGMT_OP_GET_CLOCK_INFO,
+                                           MGMT_STATUS_NOT_CONNECTED,
+                                           &rp, sizeof(rp));
+                        goto unlock;
+                }
+        } else {
+                conn = NULL;
+        }
+
+        cmd = mgmt_pending_add(sk, MGMT_OP_GET_CLOCK_INFO, hdev, data, len);
+        if (!cmd) {
+                err = -ENOMEM;
+                goto unlock;
+        }
+
+        hci_req_init(&req, hdev);
+
+        memset(&hci_cp, 0, sizeof(hci_cp));
+        hci_req_add(&req, HCI_OP_READ_CLOCK, sizeof(hci_cp), &hci_cp);
+
+        if (conn) {
+                hci_conn_hold(conn);
+                cmd->user_data = conn;
+
+                hci_cp.handle = cpu_to_le16(conn->handle);
+                hci_cp.which = 0x01; /* Piconet clock */
+                hci_req_add(&req, HCI_OP_READ_CLOCK, sizeof(hci_cp), &hci_cp);
+        }
+
+        err = hci_req_run(&req, get_clock_info_complete);
+        if (err < 0)
+                mgmt_pending_remove(cmd);
+
+unlock:
+        hci_dev_unlock(hdev);
+        return err;
+}
+
 static const struct mgmt_handler {
         int (*func) (struct sock *sk, struct hci_dev *hdev, void *data,
                      u16 data_len);
@@ -4876,6 +5019,7 @@ static const struct mgmt_handler {
         { set_privacy,            false, MGMT_SET_PRIVACY_SIZE },
         { load_irks,              true,  MGMT_LOAD_IRKS_SIZE },
         { get_conn_info,          false, MGMT_GET_CONN_INFO_SIZE },
+        { get_clock_info,         false, MGMT_GET_CLOCK_INFO_SIZE },
 };