1 files changed, 20 insertions, 39 deletions

diff --git a/tools/csr.c b/tools/csr.c
index 250700d9..e600cec7 100644
--- a/tools/csr.c
+++ b/tools/csr.c
@@ -463,7 +463,7 @@ int csr_write_varid_valueless(int dd, uint16_t seqnum, uint16_t varid)
 	case CSR_VARID_WARM_HALT:
 		return hci_send_cmd(dd, OGF_VENDOR_CMD, 0x00, sizeof(cmd) + 1, cp);
 	}
-		
+
 	memset(&rq, 0, sizeof(rq));
 	rq.ogf    = OGF_VENDOR_CMD;
 	rq.ocf    = 0x00;
@@ -630,7 +630,7 @@ int csr_read_pskey_complex(int dd, uint16_t seqnum, uint16_t pskey, uint16_t sto
 	rq.ocf    = 0x00;
 	rq.event  = EVT_VENDOR;
 	rq.cparam = cp;
-	rq.clen   = sizeof(cmd) + 1;
+	rq.clen   = sizeof(cmd) + length - 1;
 	rq.rparam = rp;
 	rq.rlen   = sizeof(rp);
 
@@ -652,11 +652,12 @@ int csr_read_pskey_complex(int dd, uint16_t seqnum, uint16_t pskey, uint16_t sto
 	return 0;
 }
 
-int csr_read_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint16_t *value)
+int csr_write_pskey_complex(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint8_t *value, uint16_t length)
 {
-	unsigned char cmd[] = { 0x00, 0x00, 0x09, 0x00,
+	unsigned char cmd[] = { 0x02, 0x00, ((length / 2) + 8) & 0xff, ((length / 2) + 8) >> 8,
 				seqnum & 0xff, seqnum >> 8, 0x03, 0x70, 0x00, 0x00,
-				pskey & 0xff, pskey >> 8, 0x01, 0x00,
+				pskey & 0xff, pskey >> 8,
+				(length / 2) & 0xff, (length / 2) >> 8,
 				store & 0xff, store >> 8, 0x00, 0x00 };
 
 	unsigned char cp[254], rp[254];
@@ -666,12 +667,14 @@ int csr_read_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t stor
 	cp[0] = 0xc2;
 	memcpy(cp + 1, cmd, sizeof(cmd));
 
+	memcpy(cp + 17, value, length);
+
 	memset(&rq, 0, sizeof(rq));
 	rq.ogf    = OGF_VENDOR_CMD;
 	rq.ocf    = 0x00;
 	rq.event  = EVT_VENDOR;
 	rq.cparam = cp;
-	rq.clen   = sizeof(cmd) + 1;
+	rq.clen   = sizeof(cmd) + length - 1;
 	rq.rparam = rp;
 	rq.rlen   = sizeof(rp);
 
@@ -688,46 +691,24 @@ int csr_read_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t stor
 		return -1;
 	}
 
-	*value = rp[17] + (rp[18] << 8);
-
 	return 0;
 }
 
-int csr_write_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint16_t value)
+int csr_read_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint16_t *value)
 {
-	unsigned char cmd[] = { 0x02, 0x00, 0x09, 0x00,
-				seqnum & 0xff, seqnum >> 8, 0x03, 0x70, 0x00, 0x00,
-				pskey & 0xff, pskey >> 8, 0x01, 0x00,
-				store & 0xff, store >> 8, value & 0xff, value >> 8 };
-
-	unsigned char cp[254], rp[254];
-	struct hci_request rq;
+	uint8_t array[2] = { 0x00, 0x00 };
+	int err;
 
-	memset(&cp, 0, sizeof(cp));
-	cp[0] = 0xc2;
-	memcpy(cp + 1, cmd, sizeof(cmd));
-
-	memset(&rq, 0, sizeof(rq));
-	rq.ogf    = OGF_VENDOR_CMD;
-	rq.ocf    = 0x00;
-	rq.event  = EVT_VENDOR;
-	rq.cparam = cp;
-	rq.clen   = sizeof(cmd) + 1;
-	rq.rparam = rp;
-	rq.rlen   = sizeof(rp);
+	err = csr_read_pskey_complex(dd, seqnum, pskey, store, array, 2);
 
-	if (hci_send_req(dd, &rq, 2000) < 0)
-		return -1;
+	*value = array[0] + (array[1] << 8);
 
-	if (rp[0] != 0xc2) {
-		errno = EIO;
-		return -1;
-	}
+	return err;
+}
 
-	if ((rp[9] + (rp[10] << 8)) != 0) {
-		errno = ENXIO;
-		return -1;
-	}
+int csr_write_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint16_t value)
+{
+	uint8_t array[2] = { value & 0xff, value >> 8 };
 
-	return 0;
+	return csr_write_pskey_complex(dd, seqnum, pskey, store, array, 2);
 }