summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--tools/csr.c59
-rw-r--r--tools/csr.h1
2 files changed, 21 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);
}
diff --git a/tools/csr.h b/tools/csr.h
index 1d9a18c2..7d928d13 100644
--- a/tools/csr.h
+++ b/tools/csr.h
@@ -83,5 +83,6 @@ int csr_read_varid_complex(int dd, uint16_t seqnum, uint16_t varid, uint8_t *val
int csr_read_varid_uint16(int dd, uint16_t seqnum, uint16_t varid, uint16_t *value);
int csr_read_varid_uint32(int dd, uint16_t seqnum, uint16_t varid, uint32_t *value);
int csr_read_pskey_complex(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint8_t *value, uint16_t length);
+int csr_write_pskey_complex(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint8_t *value, uint16_t length);
int csr_read_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint16_t *value);
int csr_write_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t store, uint16_t value);