Change device address and reset

1 files changed, 45 insertions, 9 deletions

diff --git a/tools/hciattach_st.c b/tools/hciattach_st.c
index 98179d71..ee3ca2a7 100644
--- a/tools/hciattach_st.c
+++ b/tools/hciattach_st.c
@@ -32,20 +32,32 @@
 #include <stdint.h>
 #include <string.h>
 
+static int debug = 0;
+
 static int do_command(int fd, uint8_t ogf, uint16_t ocf,
 			uint8_t *cparam, int clen, uint8_t *rparam, int rlen)
 {
-	uint16_t opcode = (uint16_t) ((ocf & 0x03ff) | (ogf << 10));
+	//uint16_t opcode = (uint16_t) ((ocf & 0x03ff) | (ogf << 10));
 	unsigned char cp[254], rp[254];
 	int len, size, offset = 3;
 
-	memset(cp, 0, sizeof(cp));
 	cp[0] = 0x01;
-	cp[1] = opcode & 0xff;
-	cp[2] = opcode >> 8;
-	cp[3] = 0x00;
+	cp[1] = ocf & 0xff;
+	cp[2] = ogf << 2 | ocf >> 8;
+	cp[3] = clen;
+
+	if (clen > 0)
+		memcpy(cp + 4, cparam, clen);
+
+	if (debug) {
+		int i;
+		printf("[<");
+		for (i = 0; i < clen + 4; i++)
+			printf(" %02x", cp[i]);
+		printf("]\n");
+	}
 
-	if (write(fd, cp, 4) < 0)
+	if (write(fd, cp, clen + 4) < 0)
 		return -1;
 
 	do {
@@ -61,18 +73,28 @@ static int do_command(int fd, uint8_t ogf, uint16_t ocf,
 		offset += len;
 	} while (offset < rp[2] + 3);
 
+	if (debug) {
+		int i;
+		printf("[>");
+		for (i = 0; i < offset; i++)
+			printf(" %02x", rp[i]);
+		printf("]\n");
+	}
+
 	if (rp[0] != 0x04) {
 		errno = EIO;
 		return -1;
 	}
 
 	switch (rp[1]) {
-	case 0x0e:
+	case 0x0e:	/* command complete */
 		if (rp[6] != 0x00)
 			return -ENXIO;
 		offset = 3 + 4;
 		size = rp[2] - 4;
 		break;
+	case 0x0f:	/* command status */
+		/* fall through */
 	default:
 		offset = 3;
 		size = rp[2];
@@ -89,16 +111,30 @@ static int do_command(int fd, uint8_t ogf, uint16_t ocf,
 
 int stlc2500_init(int fd)
 {
+	unsigned char cmd[16];
 	unsigned char buf[254];
 	int len;
 
 	len = do_command(fd, 0x04, 0x0001, NULL, 0, buf, sizeof(buf));
 
-	//printf("STLC2500 R%d.%d\n", buf[2], buf[1]);
+	printf("Patch: STLC2500_R%d_%02d_*.ptc\n", buf[2], buf[1]);
 
 	len = do_command(fd, 0xff, 0x000f, NULL, 0, buf, sizeof(buf));
 
-	printf("%s\n", buf + 3);
+	printf("%s\n", buf);
+
+	cmd[0] = 0xfe;
+	cmd[1] = 0x06;
+	cmd[2] = 0xba;
+	cmd[3] = 0xab;
+	cmd[4] = 0x00;
+	cmd[5] = 0xe1;
+	cmd[6] = 0x80;
+	cmd[7] = 0x00;
+
+	len = do_command(fd, 0xff, 0x0022, cmd, 8, buf, sizeof(buf));
+
+	len = do_command(fd, 0x03, 0x0003, NULL, 0, buf, sizeof(buf));
 
 	return 0;
 }