Add link supervision timeout functions

3 files changed, 61 insertions, 0 deletions

diff --git a/include/hci.h b/include/hci.h
index 54bb8849..a5435cd1 100644
--- a/include/hci.h
+++ b/include/hci.h
@@ -677,6 +677,11 @@ typedef struct {
 	uint16_t	link_sup_to;
 } __attribute__ ((packed)) write_link_supervision_timeout_cp;
 #define WRITE_LINK_SUPERVISION_TIMEOUT_CP_SIZE 4
+typedef struct {
+	uint8_t		status;
+	uint16_t	handle;
+} __attribute__ ((packed)) write_link_supervision_timeout_rp;
+#define WRITE_LINK_SUPERVISION_TIMEOUT_RP_SIZE 3
 
 #define MAX_IAC_LAP 0x40
 #define OCF_READ_CURRENT_IAC_LAP	0x0039
diff --git a/include/hci_lib.h b/include/hci_lib.h
index 969db1f2..2edc9e0e 100644
--- a/include/hci_lib.h
+++ b/include/hci_lib.h
@@ -91,6 +91,8 @@ int hci_read_inquiry_mode(int dd, uint8_t *mode, int to);
 int hci_write_inquiry_mode(int dd, uint8_t mode, int to);
 int hci_read_afh_mode(int dd, uint8_t *mode, int to);
 int hci_write_afh_mode(int dd, uint8_t mode, int to);
+int hci_read_link_supervision_timeout(int dd, uint16_t handle, uint16_t *timeout, int to);
+int hci_write_link_supervision_timeout(int dd, uint16_t handle, uint16_t timeout, int to);
 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);
diff --git a/src/hci.c b/src/hci.c
index 63a79d06..10f4fcb6 100644
--- a/src/hci.c
+++ b/src/hci.c
@@ -1432,6 +1432,60 @@ int hci_write_afh_mode(int dd, uint8_t mode, int to)
 	return 0;
 }
 
+int hci_read_link_supervision_timeout(int dd, uint16_t handle, uint16_t *timeout, int to)
+{
+	read_link_supervision_timeout_rp rp;
+	struct hci_request rq;
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_STATUS_PARAM;
+	rq.ocf    = OCF_READ_LINK_SUPERVISION_TIMEOUT;
+	rq.cparam = &handle;
+	rq.clen   = 2;
+	rq.rparam = &rp;
+	rq.rlen   = READ_LINK_SUPERVISION_TIMEOUT_RP_SIZE;
+
+	if (hci_send_req(dd, &rq, to) < 0)
+		return -1;
+
+	if (rp.status) {
+		errno = EIO;
+		return -1;
+	}
+
+	*timeout = rp.link_sup_to;
+	return 0;
+}
+
+int hci_write_link_supervision_timeout(int dd, uint16_t handle, uint16_t timeout, int to)
+{
+	write_link_supervision_timeout_cp cp;
+	write_link_supervision_timeout_rp rp;
+	struct hci_request rq;
+
+	memset(&cp, 0, sizeof(cp));
+	cp.handle      = handle;
+	cp.link_sup_to = timeout;
+
+	memset(&rq, 0, sizeof(rq));
+	rq.ogf    = OGF_STATUS_PARAM;
+	rq.ocf    = OCF_WRITE_LINK_SUPERVISION_TIMEOUT;
+	rq.cparam = &cp;
+	rq.clen   = WRITE_LINK_SUPERVISION_TIMEOUT_CP_SIZE;
+	rq.rparam = &rp;
+	rq.rlen   = WRITE_LINK_SUPERVISION_TIMEOUT_RP_SIZE;
+
+	if (hci_send_req(dd, &rq, to) < 0)
+		return -1;
+
+	if (rp.status) {
+		errno = EIO;
+		return -1;
+	}
+
+	return 0;
+}
+
 int hci_set_afh_classification(int dd, uint8_t *map, int to)
 {
 	set_afh_classification_cp cp;