Allow reading of complex PS keys

3 files changed, 99 insertions, 23 deletions

diff --git a/tools/csr.c b/tools/csr.c
index 4829a66d..6b34fc3f 100644
--- a/tools/csr.c
+++ b/tools/csr.c
@@ -386,6 +386,12 @@ char *csr_pskeytostr(uint16_t pskey)
 		return "Minimum encryption key length";
 	case CSR_PSKEY_ENC_KEY_LMAX:
 		return "Maximum encryption key length";
+	case CSR_PSKEY_LOCAL_SUPPORTED_FEATURES:
+		return "Local supported features block";
+	case CSR_PSKEY_HCI_LMP_LOCAL_VERSION:
+		return "The HCI and LMP version reported locally";
+	case CSR_PSKEY_LMP_REMOTE_VERSION:
+		return "The LMP version reported remotely";
 	case CSR_PSKEY_HOSTIO_MAP_SCO_PCM:
 		return "Map SCO over PCM";
 	case CSR_PSKEY_UART_BAUDRATE:
@@ -486,6 +492,48 @@ int csr_read_varid_uint16(int dd, uint16_t seqnum, uint16_t varid, uint16_t *val
 	return 0;
 }
 
+int csr_read_pskey_complex(int dd, uint16_t seqnum, uint16_t pskey, uint8_t *value, uint16_t length)
+{
+	unsigned char cmd[] = { 0x00, 0x00, ((length / 2) + 8) & 0xff, ((length / 2) + 8) >> 8,
+				seqnum & 0xff, seqnum >> 8, 0x03, 0x70, 0x00, 0x00,
+				pskey & 0xff, pskey >> 8,
+				(length / 2) & 0xff, (length / 2) >> 8,
+				0x00, 0x00, 0x00, 0x00 };
+
+	unsigned char cp[254], rp[254];
+	struct hci_request rq;
+
+	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);
+
+	if (hci_send_req(dd, &rq, 2000) < 0)
+		return -1;
+
+	if (rp[0] != 0xc2) {
+		errno = EIO;
+		return -1;
+	}
+
+	if ((rp[9] + (rp[10] << 8)) != 0) {
+		errno = ENXIO;
+		return -1;
+	}
+
+	memcpy(value, rp + 17, length);
+
+	return 0;
+}
+
 int csr_read_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t *value)
 {
 	unsigned char cmd[] = { 0x00, 0x00, 0x09, 0x00,
diff --git a/tools/csr.h b/tools/csr.h
index 347e1d54..61df145e 100644
--- a/tools/csr.h
+++ b/tools/csr.h
@@ -77,5 +77,6 @@ char *csr_pskeytostr(uint16_t pskey);
 
 int csr_read_varid_complex(int dd, uint16_t seqnum, uint16_t varid, uint8_t *value, uint16_t length);
 int csr_read_varid_uint16(int dd, uint16_t seqnum, uint16_t varid, uint16_t *value);
+int csr_read_pskey_complex(int dd, uint16_t seqnum, uint16_t pskey, uint8_t *value, uint16_t length);
 int csr_read_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t *value);
 int csr_write_pskey_uint16(int dd, uint16_t seqnum, uint16_t pskey, uint16_t value);
diff --git a/tools/pskey.c b/tools/pskey.c
index 2b4bc210..9a4dd547 100644
--- a/tools/pskey.c
+++ b/tools/pskey.c
@@ -43,8 +43,9 @@
 #include "csr.h"
 
 #define CSR_TYPE_NULL	0
-#define CSR_TYPE_UINT8	1
-#define CSR_TYPE_UINT16	2
+#define CSR_TYPE_ARRAY	1
+#define CSR_TYPE_UINT8	2
+#define CSR_TYPE_UINT16	3
 
 static int write_pskey(int dd, uint16_t pskey, int type, int argc, char *argv[])
 {
@@ -73,19 +74,38 @@ static int write_pskey(int dd, uint16_t pskey, int type, int argc, char *argv[])
 
 static int read_pskey(int dd, uint16_t pskey, int type)
 {
-	uint16_t value;
-	int err;
+	uint8_t array[64];
+	uint16_t value = 0;
+	int i, err, size = sizeof(array);
 
-	if (type != CSR_TYPE_UINT8 && type != CSR_TYPE_UINT16) {
+	memset(array, 0, sizeof(array));
+
+	if (type != CSR_TYPE_ARRAY &&
+			type != CSR_TYPE_UINT8 &&
+			type != CSR_TYPE_UINT16) {
 		errno = EFAULT;
 		return -1;
 	}
 
-	err = csr_read_pskey_uint16(dd, 0x4711, pskey, &value);
-	if (err < 0)
-		return err;
+	if (type != CSR_TYPE_ARRAY) {
+		err = csr_read_pskey_uint16(dd, 0x4711, pskey, &value);
+		if (err < 0)
+			return err;
+
+		printf("%s: 0x%04x (%d)\n", csr_pskeytostr(pskey), value, value);
+	} else {
+		if (pskey == CSR_PSKEY_LOCAL_SUPPORTED_FEATURES)
+			size = 8;
 
-	printf("%s: 0x%04x (%d)\n", csr_pskeytostr(pskey), value, value);
+		err = csr_read_pskey_complex(dd, 0x4711, pskey, array, size);
+		if (err < 0)
+			return err;
+
+		printf("%s:", csr_pskeytostr(pskey));
+		for (i = 0; i < size; i++)
+			printf(" 0x%02x", array[i]);
+		printf("\n");
+	}
 
 	return err;
 }
@@ -95,31 +115,38 @@ static struct {
 	int type;
 	char *str;
 } storage[] = {
-	{ CSR_PSKEY_ENC_KEY_LMIN,          CSR_TYPE_UINT16, "keymin"   },
-	{ CSR_PSKEY_ENC_KEY_LMAX,          CSR_TYPE_UINT16, "keymax"   },
-	{ CSR_PSKEY_HCI_LMP_LOCAL_VERSION, CSR_TYPE_UINT16, "version"  },
-	{ CSR_PSKEY_LMP_REMOTE_VERSION,    CSR_TYPE_UINT8,  "remver"   },
-	{ CSR_PSKEY_HOSTIO_MAP_SCO_PCM,    CSR_TYPE_UINT16, "mapsco"   },
-	{ CSR_PSKEY_UART_BAUDRATE,         CSR_TYPE_UINT16, "baudrate" },
-	{ CSR_PSKEY_HOST_INTERFACE,        CSR_TYPE_UINT16, "hostintf" },
-	{ CSR_PSKEY_USB_VENDOR_ID,         CSR_TYPE_UINT16, "usbvid"   },
-	{ CSR_PSKEY_USB_PRODUCT_ID,        CSR_TYPE_UINT16, "usbpid"   },
-	{ CSR_PSKEY_USB_DFU_PRODUCT_ID,    CSR_TYPE_UINT16, "dfupid"   },
-	{ CSR_PSKEY_INITIAL_BOOTMODE,      CSR_TYPE_UINT16, "bootmode" },
+	{ CSR_PSKEY_ENC_KEY_LMIN,             CSR_TYPE_UINT16, "keymin"   },
+	{ CSR_PSKEY_ENC_KEY_LMAX,             CSR_TYPE_UINT16, "keymax"   },
+	{ CSR_PSKEY_LOCAL_SUPPORTED_FEATURES, CSR_TYPE_ARRAY,  "features" },
+	{ CSR_PSKEY_HCI_LMP_LOCAL_VERSION,    CSR_TYPE_UINT16, "version"  },
+	{ CSR_PSKEY_LMP_REMOTE_VERSION,       CSR_TYPE_UINT8,  "remver"   },
+	{ CSR_PSKEY_HOSTIO_MAP_SCO_PCM,       CSR_TYPE_UINT16, "mapsco"   },
+	{ CSR_PSKEY_UART_BAUDRATE,            CSR_TYPE_UINT16, "baudrate" },
+	{ CSR_PSKEY_HOST_INTERFACE,           CSR_TYPE_UINT16, "hostintf" },
+	{ CSR_PSKEY_USB_VENDOR_ID,            CSR_TYPE_UINT16, "usbvid"   },
+	{ CSR_PSKEY_USB_PRODUCT_ID,           CSR_TYPE_UINT16, "usbpid"   },
+	{ CSR_PSKEY_USB_DFU_PRODUCT_ID,       CSR_TYPE_UINT16, "dfupid"   },
+	{ CSR_PSKEY_INITIAL_BOOTMODE,         CSR_TYPE_UINT16, "bootmode" },
 	{ 0x0000, CSR_TYPE_NULL, NULL },
 };
 
 static void usage(void)
 {
-	int i;
+	int i, pos = 0;
 
 	printf("pskey - Utility for changing CSR persistent storage\n\n");
 	printf("Usage:\n"
 		"\tpskey [-i <dev>] <key> [value]\n\n");
 
 	printf("Keys:\n\t");
-		for (i = 0; storage[i].pskey; i++)
-			printf("%s ", storage[i].str);
+	for (i = 0; storage[i].pskey; i++) {
+		printf("%s ", storage[i].str);
+		pos += strlen(storage[i].str) + 1;
+		if (pos > 60) {
+			printf("\n\t");
+			pos = 0;
+		}
+	}
 	printf("\n");
 }