diff options
| -rw-r--r-- | tools/csr.c | 59 | ||||
| -rw-r--r-- | tools/csr.h | 1 |
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); |