Add hci_read_clock() function

3 files changed, 46 insertions, 0 deletions

diff --git a/include/hci.h b/include/hci.h
index a5435cd1..640e22da 100644
--- a/include/hci.h
+++ b/include/hci.h
@@ -821,6 +821,20 @@ typedef struct {
 } __attribute__ ((packed)) read_afh_map_rp;
 #define READ_AFH_MAP_RP_SIZE 14
 
+#define OCF_READ_CLOCK			0x0007
+typedef struct {
+	uint16_t	handle;
+	uint8_t		which_clock;
+} __attribute__ ((packed)) read_clock_cp;
+#define READ_CLOCK_CP_SIZE 3
+typedef struct {
+	uint8_t		status;
+	uint16_t	handle;
+	uint32_t	clock;
+	uint16_t	accuracy;
+} __attribute__ ((packed)) read_clock_rp;
+#define READ_CLOCK_RP_SIZE 9
+
 /* Testing commands */
 #define OGF_TESTING_CMD		0x3e
 
diff --git a/include/hci_lib.h b/include/hci_lib.h
index f6259f51..ed7ad291 100644
--- a/include/hci_lib.h
+++ b/include/hci_lib.h
@@ -96,6 +96,7 @@ int hci_set_afh_classification(int dd, uint8_t *map, int to);
 int hci_read_link_quality(int dd, uint16_t handle, uint8_t *link_quality, int to);
 int hci_read_rssi(int dd, uint16_t handle, int8_t *rssi, int to);
 int hci_read_afh_map(int dd, uint16_t handle, uint8_t *mode, uint8_t *map, int to);
+int hci_read_clock(int dd, uint16_t handle, uint8_t which, uint32_t *clock, uint16_t *accuracy, int to);
 
 int hci_local_name(int dd, int len, char *name, int to);
 int hci_remote_name(int dd, const bdaddr_t *bdaddr, int len, char *name, int to);
diff --git a/src/hci.c b/src/hci.c
index 87f4874b..387560aa 100644
--- a/src/hci.c
+++ b/src/hci.c
@@ -1620,6 +1620,37 @@ int hci_read_afh_map(int dd, uint16_t handle, uint8_t *mode, uint8_t *map, int t
 	return 0;
 }
 
+int hci_read_clock(int dd, uint16_t handle, uint8_t which, uint32_t *clock, uint16_t *accuracy, int to)
+{
+	read_clock_cp cp;
+	read_clock_rp rp;
+	struct hci_request rq;
+
+	memset(&cp, 0, sizeof(cp));
+	cp.handle      = handle;
+	cp.which_clock = which;
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_STATUS_PARAM;
+	rq.ocf    = OCF_READ_CLOCK;
+	rq.cparam = &cp;
+	rq.clen   = READ_CLOCK_CP_SIZE;
+	rq.rparam = &rp;
+	rq.rlen   = READ_CLOCK_RP_SIZE;
+
+	if (hci_send_req(dd, &rq, to) < 0)
+		return -1;
+
+	if (rp.status) {
+		errno = EIO;
+		return -1;
+	}
+
+	*clock    = rp.clock;
+	*accuracy = rp.accuracy;
+	return 0;
+}
+
 int hci_local_name(int dd, int len, char *name, int to)
 {
 	return hci_read_local_name(dd, len, name, to);