implement hci_{read, write}_current_iac_lap()

3 files changed, 57 insertions, 0 deletions

diff --git a/include/hci.h b/include/hci.h
index 90d148f0..e12352b4 100644
--- a/include/hci.h
+++ b/include/hci.h
@@ -329,6 +329,22 @@ typedef struct {
 } __attribute__ ((packed)) host_buffer_size_cp;
 #define HOST_BUFFER_SIZE_CP_SIZE 7
 
+#define MAX_IAC_LAP 0x40
+#define OCF_READ_CURRENT_IAC_LAP 0x0039
+typedef struct {
+  uint8_t status;
+  uint8_t num_current_iac;
+  uint8_t lap[3][MAX_IAC_LAP];
+} __attribute__ ((packed)) read_current_iac_lap_rp;
+#define READ_CURRENT_IAC_LAP_RP_SIZE 2+3*MAX_IAC_LAP
+
+#define OCF_WRITE_CURRENT_IAC_LAP 0x003A
+typedef struct {
+  uint8_t num_current_iac;
+  uint8_t lap[3][MAX_IAC_LAP];
+} __attribute__ ((packed)) write_current_iac_lap_cp;
+#define WRITE_CURRENT_IAC_LAP_CP_SIZE 1+3*MAX_IAC_LAP
+
 /* Link Control */
 #define OGF_LINK_CTL	0x01 
 #define OCF_CREATE_CONN		0x0005
diff --git a/include/hci_lib.h b/include/hci_lib.h
index b988de5f..afa2fb51 100644
--- a/include/hci_lib.h
+++ b/include/hci_lib.h
@@ -70,6 +70,8 @@ int hci_read_remote_features(int dd, uint16_t handle, uint8_t *features, int to)
 int hci_read_remote_version(int dd, uint16_t handle, struct hci_version *ver, int to);
 int hci_read_local_version(int dd, struct hci_version *ver, int to);
 int hci_class_of_dev(int dd, uint8_t *class, int to);
+int hci_read_current_iac_lap(int dd, uint8_t *num_iac, uint8_t *lap, int to);
+int hci_write_current_iac_lap(int dd, uint8_t num_iac, uint8_t *lap, int to);
 
 int hci_for_each_dev(int flag, int(*func)(int s, int dev_id, long arg), long arg);
 int hci_get_route(bdaddr_t *bdaddr);
diff --git a/src/hci.c b/src/hci.c
index 1424ddcd..40a19fb2 100644
--- a/src/hci.c
+++ b/src/hci.c
@@ -923,3 +923,42 @@ int hci_class_of_dev(int dd, uint8_t *class, int to)
 	return 0;
 }
 
+int hci_read_current_iac_lap(int dd, uint8_t *num_iac, uint8_t *lap, int to)
+{
+  read_current_iac_lap_rp rp;
+  struct hci_request rq;
+  
+  memset(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_HOST_CTL;
+  rq.ocf = OCF_READ_CURRENT_IAC_LAP;
+  rq.rparam = &rp;
+  rq.rlen = READ_CURRENT_IAC_LAP_RP_SIZE;
+  if (0 > hci_send_req(dd, &rq, to))
+	 return -1;
+  if (rp.status) {
+	 errno = EIO;
+	 return -1;
+  }
+  *num_iac = rp.num_current_iac;
+  memcpy(lap, rp.lap, 3*rp.num_current_iac);
+  return 0;
+}
+
+int hci_write_current_iac_lap(int dd, uint8_t num_iac, uint8_t *lap, int to)
+{
+  write_current_iac_lap_cp cp;
+  struct hci_request rq;
+  
+  memset(&cp, 0, sizeof(cp));
+  cp.num_current_iac = num_iac;
+  memcpy(&cp.lap, lap, 3*num_iac);
+  
+  memset(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_HOST_CTL;
+  rq.ocf = OCF_WRITE_CURRENT_IAC_LAP;
+  rq.cparam = &cp;
+  rq.clen = WRITE_CURRENT_IAC_LAP_CP_SIZE;
+  if (0 > hci_send_req(dd, &rq, to))
+	 return -1;
+  return 0;
+}