1 files changed, 70 insertions, 28 deletions

diff --git a/tools/hciattach_ath3k.c b/tools/hciattach_ath3k.c
index 70ade304c..728e6605f 100644
--- a/tools/hciattach_ath3k.c
+++ b/tools/hciattach_ath3k.c
@@ -922,14 +922,49 @@ int ath3k_post(int fd, int pm)
 #define WRITE_BAUD_CMD_LEN   6
 #define MAX_CMD_LEN          WRITE_BDADDR_CMD_LEN
 
+static int set_cntrlr_baud(int fd, int speed)
+{
+	int baud;
+	struct timespec tm = { 0, 500000 };
+	unsigned char cmd[MAX_CMD_LEN], rsp[HCI_MAX_EVENT_SIZE];
+	unsigned char *ptr = cmd + 1;
+	hci_command_hdr *ch = (void *)ptr;
+
+	cmd[0] = HCI_COMMAND_PKT;
+
+	/* set controller baud rate to user specified value */
+	ptr = cmd + 1;
+	ch->opcode = htobs(cmd_opcode_pack(HCI_VENDOR_CMD_OGF,
+						HCI_CHG_BAUD_CMD_OCF));
+	ch->plen = 2;
+	ptr += HCI_COMMAND_HDR_SIZE;
+
+	baud = speed/100;
+	ptr[0] = (char)baud;
+	ptr[1] = (char)(baud >> 8);
+
+	if (write(fd, cmd, WRITE_BAUD_CMD_LEN) != WRITE_BAUD_CMD_LEN) {
+		perror("Failed to write change baud rate command");
+		return -ETIMEDOUT;
+	}
+
+	nanosleep(&tm, NULL);
+
+	if (read_hci_event(fd, rsp, sizeof(rsp)) < 0)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
 /*
  * Atheros AR300x specific initialization and configuration file
  * download
  */
-int ath3k_init(int fd, char *bdaddr, int speed)
+int ath3k_init(int fd, int speed, int init_speed, char *bdaddr,
+						struct termios *ti)
 {
 	int r;
-	int baud;
+	int err = 0;
 	struct timespec tm = { 0, 500000 };
 	unsigned char cmd[MAX_CMD_LEN], rsp[HCI_MAX_EVENT_SIZE];
 	unsigned char *ptr = cmd + 1;
@@ -937,11 +972,23 @@ int ath3k_init(int fd, char *bdaddr, int speed)
 
 	cmd[0] = HCI_COMMAND_PKT;
 
+	/* set both controller and host baud rate to maximum possible value */
+	err = set_cntrlr_baud(fd, speed);
+	if (err < 0)
+		return err;
+
+	err = set_speed(fd, ti, speed);
+	if (err < 0) {
+		perror("Can't set required baud rate");
+		return err;
+	}
+
 	/* Download PS and patch */
 	r = ath_ps_download(fd);
 	if (r < 0) {
 		perror("Failed to Download configuration");
-		return -ETIMEDOUT;
+		err = -ETIMEDOUT;
+		goto failed;
 	}
 
 	/* Write BDADDR */
@@ -960,12 +1007,14 @@ int ath3k_init(int fd, char *bdaddr, int speed)
 		if (write(fd, cmd, WRITE_BDADDR_CMD_LEN) !=
 					WRITE_BDADDR_CMD_LEN) {
 			perror("Failed to write BD_ADDR command\n");
-			return -ETIMEDOUT;
+			err = -ETIMEDOUT;
+			goto failed;
 		}
 
 		if (read_hci_event(fd, rsp, sizeof(rsp)) < 0) {
 			perror("Failed to set BD_ADDR\n");
-			return -ETIMEDOUT;
+			err = -ETIMEDOUT;
+			goto failed;
 		}
 	}
 
@@ -975,33 +1024,26 @@ int ath3k_init(int fd, char *bdaddr, int speed)
 	cmd[3] = 0x00;
 
 	r = write(fd, cmd, 4);
-	if (r != 4)
-		return -ETIMEDOUT;
+	if (r != 4) {
+		err = -ETIMEDOUT;
+		goto failed;
+	}
 
 	nanosleep(&tm, NULL);
-	if (read_hci_event(fd, rsp, sizeof(rsp)) < 0)
-		return -ETIMEDOUT;
-
-	/* set controller baud rate to user specified value */
-	ptr = cmd + 1;
-	ch->opcode = htobs(cmd_opcode_pack(HCI_VENDOR_CMD_OGF,
-						HCI_CHG_BAUD_CMD_OCF));
-	ch->plen = 2;
-	ptr += HCI_COMMAND_HDR_SIZE;
-
-	baud = speed/100;
-	ptr[0] = (char)baud;
-	ptr[1] = (char)(baud >> 8);
-
-	if (write(fd, cmd, WRITE_BAUD_CMD_LEN) != WRITE_BAUD_CMD_LEN) {
-		perror("Failed to write change baud rate command");
-		return -ETIMEDOUT;
+	if (read_hci_event(fd, rsp, sizeof(rsp)) < 0) {
+		err = -ETIMEDOUT;
+		goto failed;
 	}
 
-	nanosleep(&tm, NULL);
+	err = set_cntrlr_baud(fd, speed);
+	if (err < 0)
+		return err;
 
-	if (read_hci_event(fd, rsp, sizeof(rsp)) < 0)
-		return -ETIMEDOUT;
+failed:
+	if (err < 0) {
+		set_cntrlr_baud(fd, init_speed);
+		set_speed(fd, ti, init_speed);
+	}
 
-	return 0;
+	return err;
 }