BCSP initialization improvements

1 files changed, 109 insertions, 25 deletions

diff --git a/tools/hciattach.c b/tools/hciattach.c
index cee27e46..78deb53b 100644
--- a/tools/hciattach.c
+++ b/tools/hciattach.c
@@ -230,21 +230,67 @@ static int digi(int fd, struct uart_t *u, struct termios *ti)
 	return 0;
 }
 
+static int read_check(int fd, void *buf, int count)
+{
+	int res;
+	
+	do{
+		res = read(fd, buf, count);
+		if (res != -1) {
+			buf += res; count -= res;
+		}
+	} while (count && (errno == 0 || errno == EINTR));
+	
+	if (count)
+		return -1;
+	
+	return 0;
+}
+
 /*
  * BCSP specific initialization
  */
+int serial_fd;
+
+static void bcsp_tshy_sig_alarm(int sig)
+{
+	static int retries=0;
+	unsigned char bcsp_sync_pkt[10] = {0xc0,0x00,0x41,0x00,0xbe,0xda,0xdc,0xed,0xed,0xc0};
+	
+	if (retries < 10) {
+		retries++;
+		write(serial_fd, &bcsp_sync_pkt, 10);
+		alarm(1);
+		return;
+	}
+	fprintf(stderr, "BCSP initialization timed out\n");
+	exit(1);
+}
+
+static void bcsp_tconf_sig_alarm(int sig)
+{
+	static int retries=0;
+	unsigned char bcsp_conf_pkt[10] = {0xc0,0x00,0x41,0x00,0xbe,0xad,0xef,0xac,0xed,0xc0};
+	if (retries < 10){
+		retries++;
+		write(serial_fd, &bcsp_conf_pkt, 10);
+		alarm(1);
+		return;
+	}
+	fprintf(stderr, "BCSP initialization timed out\n");
+	exit(1);
+}
+
 static int bcsp(int fd, struct uart_t *u, struct termios *ti)
 {
 	unsigned char byte, bcsph[4], bcspp[4],
-		bcsp_sync_pkt[10] = {0xc0,0x00,0x41,0x00,0xbe,0xda,0xdc,0xed,0xed,0xc0},
 		bcsp_sync_resp_pkt[10] = {0xc0,0x00,0x41,0x00,0xbe,0xac,0xaf,0xef,0xee,0xc0},
-		bcsp_conf_pkt[10] = {0xc0,0x00,0x41,0x00,0xbe,0xad,0xef,0xac,0xed,0xc0},
 		bcsp_conf_resp_pkt[10] = {0xc0,0x00,0x41,0x00,0xbe,0xde,0xad,0xd0,0xd0,0xc0},
-		bcspsync[4] = {0xda, 0xdc, 0xed, 0xed},
+		bcspsync[4]     = {0xda, 0xdc, 0xed, 0xed},
 		bcspsyncresp[4] = {0xac,0xaf,0xef,0xee},
-		bcspconf[4] = {0xad,0xef,0xac,0xed},
+		bcspconf[4]     = {0xad,0xef,0xac,0xed},
 		bcspconfresp[4] = {0xde,0xad,0xd0,0xd0};
-	int sync_sent = 0;
+	struct sigaction sa;
 
 	if (set_speed(fd, ti, u->speed) < 0) {
 		perror("Can't set default baud rate");
@@ -259,54 +305,92 @@ static int bcsp(int fd, struct uart_t *u, struct termios *ti)
 		return -1;
 	}
 
+	alarm(0);
+
+	serial_fd = fd;
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_flags = SA_NOCLDSTOP;
+	sa.sa_handler = bcsp_tshy_sig_alarm;
+	sigaction(SIGALRM, &sa, NULL);
+
 	/* State = shy */
 
+	bcsp_tshy_sig_alarm(0);
 	while (1) {
 		do {
-			read(fd, &byte, 1);
-		} while(byte != 0xC0);
+			if (read_check(fd, &byte, 1) == -1){
+				perror("Failed to read");
+				return -1;
+			}
+		} while (byte != 0xC0);
 		
 		do {
-			read(fd, &bcsph[0], 1);
-		} while(bcsph[0] == 0xC0);
+			if ( read_check(fd, &bcsph[0], 1) == -1){
+				perror("Failed to read");
+				return -1;
+			}
+		  
+		} while (bcsph[0] == 0xC0);
+		
+		if ( read_check(fd, &bcsph[1], 3) == -1){
+			perror("Failed to read");
+			return -1;
+		}
 		
-		read(fd, &bcsph[1], 3);
 		if (((bcsph[0] + bcsph[1] + bcsph[2]) & 0xFF) != (unsigned char)~bcsph[3])
-		      continue;
+			continue;
 		if (bcsph[1] != 0x41 || bcsph[2] != 0x00)
-		      continue;
-		
-		read(fd, &bcspp, 4);
+			continue;
+
+		if (read_check(fd, &bcspp, 4) == -1){
+			perror("Failed to read");
+			return -1;
+		}
+
 		if (!memcmp(bcspp, bcspsync, 4)) {
 			write(fd, &bcsp_sync_resp_pkt,10);
-			if (!sync_sent) {
-				write(fd, &bcsp_sync_pkt, 10); /* muzzled = false */
-				sync_sent = 1;
-			}
 		} else if (!memcmp(bcspp, bcspsyncresp, 4))
 			break;
 	}
 
 	/* State = curious */
 
-	write(fd, &bcsp_conf_pkt, 10);
+	alarm(0);
+	sa.sa_handler = bcsp_tconf_sig_alarm;
+	sigaction(SIGALRM, &sa, NULL);
+	alarm(1);
+
 	while (1) {
 		do {
-			read(fd, &byte, 1);
-		} while(byte != 0xC0);
+			if (read_check(fd, &byte, 1) == -1){
+				perror("Failed to read");
+				return -1;
+			}
+		} while (byte != 0xC0);
 
 		do {
-			read(fd, &bcsph[0], 1);
-		} while(bcsph[0] == 0xC0);
+			if (read_check(fd, &bcsph[0], 1) == -1){
+			      perror("Failed to read");
+			      return -1;
+			}
+		} while (bcsph[0] == 0xC0);
 
-		read(fd, &bcsph[1], 3);
+		if (read_check(fd, &bcsph[1], 3) == -1){
+			perror("Failed to read");
+			return -1;
+		}
+		
 		if (((bcsph[0] + bcsph[1] + bcsph[2]) & 0xFF) != (unsigned char)~bcsph[3])
 			continue;
 
 		if (bcsph[1] != 0x41 || bcsph[2] != 0x00)
 			continue;
 
-		read(fd, &bcspp, 4);
+		if (read_check(fd, &bcspp, 4) == -1){
+			perror("Failed to read");
+			return -1;
+		}
+
 		if (!memcmp(bcspp, bcspsync, 4))
 			write(fd, &bcsp_sync_resp_pkt, 10);
 		else if (!memcmp(bcspp, bcspconf, 4))