Index: channels/chan_mobile.c
===================================================================
--- channels/chan_mobile.c	(revision 841)
+++ channels/chan_mobile.c	(working copy)
@@ -320,8 +320,7 @@
  */
 struct hfp_pvt {
 	struct mbl_pvt *owner;		/*!< the mbl_pvt struct that owns this struct */
-	int connected:1;		/*!< whether a service level connection exists or not */
-	int blackberry:1;		/*!< blackberry mode (send CMER before CIND=?) */
+	int initialized:1;		/*!< whether a service level connection exists or not */
 	int nocallsetup:1;		/*!< whether we detected a callsetup indicator */
 	struct hfp_ag brsf;		/*!< the supported feature set of the AG */
 	int cind_index[16];		/*!< the cind/ciev index to name mapping for this AG */
@@ -346,13 +345,13 @@
 };
 
 
-static int hfp_init(struct hfp_pvt *hfp);
-static int hfp_init_sms(struct hfp_pvt *hfp);
-static int hfp_wait(struct hfp_pvt *hfp);
 static int hfp_parse_ciev(struct hfp_pvt *hfp, char *buf, int *value);
 static char *hfp_parse_clip(struct hfp_pvt *hfp, char *buf);
 static int hfp_parse_cmti(struct hfp_pvt *hfp, char *buf);
 static int hfp_parse_cmgr(struct hfp_pvt *hfp, char *buf, char **from_number, char **text);
+static int hfp_parse_brsf(struct hfp_pvt *hfp, const char *buf);
+static int hfp_parse_cind(struct hfp_pvt *hfp, char *buf);
+static int hfp_parse_cind_test(struct hfp_pvt *hfp, char *buf);
 
 static int hfp_brsf2int(struct hfp_hf *hf);
 static struct hfp_ag *hfp_int2brsf(int brsf, struct hfp_ag *ag);
@@ -363,6 +362,7 @@
 static int hfp_send_cmer(struct hfp_pvt *hfp, int status);
 static int hfp_send_clip(struct hfp_pvt *hfp, int status);
 static int hfp_send_vgs(struct hfp_pvt *hfp, int value);
+
 #if 0
 static int hfp_send_vgm(struct hfp_pvt *hfp, int value);
 #endif
@@ -376,10 +376,6 @@
 static int hfp_send_atd(struct hfp_pvt *hfp, const char *number);
 static int hfp_send_ata(struct hfp_pvt *hfp);
 
-static int hfp_read_brsf(struct hfp_pvt *hfp);
-static int hfp_read_cind(struct hfp_pvt *hfp);
-static int hfp_read_cind_test(struct hfp_pvt *hfp);
-
 /*
  * bluetooth headset profile helpers
  */
@@ -418,11 +414,14 @@
 	AT_VGM,
 	AT_VGS,
 	AT_VTS,
+	AT_CMGF,
+	AT_CNMI,
+	AT_CMER,
+	AT_CIND_TEST,
 } at_message_t;
 
 static int at_match_prefix(char *buf, char *prefix);
 static at_message_t at_read_full(int rsock, char *buf, size_t count);
-static at_message_t at_read(int rsock);
 static inline const char *at_msg2str(at_message_t msg);
 
 struct msg_queue_entry {
@@ -1845,20 +1844,6 @@
 }
 
 /*!
- * \brief Read an AT message and clasify it.
- * \param rsock an rfcomm socket
- * \return the type of message received
- * \see at_read_full()
- */
-static at_message_t at_read(int rsock)
-{
-	/* this buffer is small as we only need the first few chars to identify
-	 * the message */
-	char buf[16];
-	return at_read_full(rsock, buf, sizeof(buf));
-}
-
-/*!
  * \brief Get the string representation of the given AT message.
  * \param msg the message to process
  * \return a string describing the given message
@@ -1912,6 +1897,14 @@
 		return "AT+VGS";
 	case AT_VTS:
 		return "AT+VTS";
+	case AT_CMGF:
+		return "AT+CMGF";
+	case AT_CNMI:
+		return "AT+CNMI";
+	case AT_CMER:
+		return "AT+CMER";
+	case AT_CIND_TEST:
+		return "AT+CIND=?";
 	}
 }
 
@@ -1921,112 +1914,6 @@
  */
 
 /*!
- * \brief Initilize an HFP service level connection.
- * \param hfp an hfp_pvt struct
- *
- * This function brings up an HFP service level connection.
- *
- * \note This function expects a connected rfcomm socket and it expects the pvt
- * structure to be initilized with zeroes.
- */
-static int hfp_init(struct hfp_pvt *hfp)
-{
-	/* send and receive BRSF data */
-	if (hfp_send_brsf(hfp, &hfp_our_brsf) || !hfp_wait(hfp) || hfp_read_brsf(hfp)) {
-		ast_debug(1, "[%s] error sending/receiving BRSF\n", hfp->owner->id);
-		return -1;
-	}
-
-	/* if this is a blackberry, do CMER now */
-	if (hfp->blackberry) {
-		if (hfp_send_cmer(hfp, 1) || !hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK) {
-			ast_debug(1, "[%s] error sending CMER, try setting 'blackberry=no'\n", hfp->owner->id);
-			return -1;
-		}
-	}
-
-	/* send CIND test */
-	if (hfp_send_cind_test(hfp) || !hfp_wait(hfp) || hfp_read_cind_test(hfp)) {
-		if (!hfp->blackberry)
-			ast_debug(1, "[%s] error performing CIND test, try setting 'blackberry=yes'\n", hfp->owner->id);
-		else
-			ast_debug(1, "[%s] error performing CIND test\n", hfp->owner->id);
-		return -1;
-	}
-
-	/* read current CIND state */
-	if (hfp_send_cind(hfp) || !hfp_wait(hfp) || hfp_read_cind(hfp)) {
-		ast_debug(1, "[%s] error getting CIND state\n", hfp->owner->id);
-		return -1;
-	}
-
-	/* check if a call is active */
-	if (hfp->cind_state[hfp->cind_map.call]) {
-		ast_verb(3, "Bluetooth Device %s has a call in progress - delaying connection.\n", hfp->owner->id);
-		return -1;
-	}
-
-	/* if this is not a blackberry send CMER now */
-	if (!hfp->blackberry) {
-		if (hfp_send_cmer(hfp, 1) || !hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK) {
-			ast_debug(1, "[%s] error sending CMER\n", hfp->owner->id);
-			return -1;
-		}
-	}
-
-	/* enalbe calling line identification notification */
-	if (hfp_send_clip(hfp, 1) || !hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK) {
-		ast_debug(1, "[%s] error enabling calling line notification\n", hfp->owner->id);
-		return -1;
-	}
-
-	/* send current gain levels */
-	if (hfp_send_vgs(hfp, 15) || !hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK) {
-		ast_debug(1, "[%s] error synchronizing gain settings\n", hfp->owner->id);
-		return -1;
-	}
-
-	/* we now have a service level connection */
-	hfp->connected = 1;
-
-	if (hfp_init_sms(hfp)) {
-		ast_debug(1, "[%s] no SMS support\n", hfp->owner->id);
-	} else {
-		hfp->owner->has_sms = 1;
-	}
-
-	return 0;
-}
-
-static int hfp_init_sms(struct hfp_pvt *hfp)
-{
-	/* set the SMS operating mode to text mode */
-	if (hfp_send_cmgf(hfp, 1) || !hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK) {
-		ast_debug(1, "[%s] error setting CMGF\n", hfp->owner->id);
-		return -1;
-	}
-
-	/* turn on SMS new message indication */
-	if (hfp_send_cnmi(hfp) || !hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK) {
-		ast_debug(1, "[%s] error setting CNMI\n", hfp->owner->id);
-		return -1;
-	}
-
-	return 0;
-}
-
-
-/*!
- * \brief Wait a default timeout.
- * \return zero on timeout and non zero on data
- */
-static int hfp_wait(struct hfp_pvt *hfp)
-{
-	int ms = 10000;
-	return rfcomm_wait(hfp->rsock, &ms);
-}
-
-/*!
  * \brief Parse a CIEV event.
  * \param hfp an hfp_pvt struct
  * \param buf the buffer to parse (null terminated)
@@ -2435,27 +2322,19 @@
 }
 
 /*!
- * \brief Read BRSF data.
+ * \brief Parse BRSF data.
  * \param hfp an hfp_pvt struct
+ * \param buf the buffer to parse (null terminated)
  */
-static int hfp_read_brsf(struct hfp_pvt *hfp)
+static int hfp_parse_brsf(struct hfp_pvt *hfp, const char *buf)
 {
 	int brsf;
-	char buf[128];
 
-	/* read the BRSF data */
-	if (at_read_full(hfp->rsock, buf, sizeof(buf)) != AT_BRSF)
-		return -1;
-
 	if (!sscanf(buf, "+BRSF:%d", &brsf))
 		return -1;
 
 	hfp_int2brsf(brsf, &hfp->brsf);
 
-	/* read the OK message */
-	if (!hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK)
-		return -1;
-
 	return 0;
 }
 
@@ -2487,19 +2366,16 @@
 /*!
  * \brief Read the result of the AT+CIND? command.
  * \param hfp an hfp_pvt struct
- * \note hfp_send_cind_test() and hfp_read_cind_test() should be called at
+ * \param buf the buffer to parse (null terminated)
+ * \note hfp_send_cind_test() and hfp_parse_cind_test() should be called at
  * least once before this function is called.
  */
-static int hfp_read_cind(struct hfp_pvt *hfp)
+static int hfp_parse_cind(struct hfp_pvt *hfp, char *buf)
 {
 	int i, state, group;
 	size_t s;
-	char buf[256];
 	char *indicator;
 
-	if (at_read_full(hfp->rsock, buf, sizeof(buf)) != AT_CIND)
-		return -1;
-
 	/* parse current state of all of our indicators.  The list is in the
 	 * following format:
 	 * +CIND: 1,0,2,0,0,0,0
@@ -2536,27 +2412,20 @@
 	if (state == 2)
 		hfp_parse_cind_indicator(hfp, group, indicator);
 
-	/* read the OK message */
-	if (!hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK)
-		return -1;
-
 	return 0;
 }
 
 /*!
- * \brief Read the result of the AT+CIND=? command.
+ * \brief Parse the result of the AT+CIND=? command.
  * \param hfp an hfp_pvt struct
+ * \param buf the buffer to parse (null terminated)
  */
-static int hfp_read_cind_test(struct hfp_pvt *hfp)
+static int hfp_parse_cind_test(struct hfp_pvt *hfp, char *buf)
 {
 	int i, state, group;
 	size_t s;
-	char buf[512];
 	char *indicator, *values;
 
-	if (at_read_full(hfp->rsock, buf, sizeof(buf)) != AT_CIND)
-		return -1;
-
 	hfp->nocallsetup = 1;
 
 	/* parse the indications list.  It is in the follwing format:
@@ -2645,10 +2514,6 @@
 
 	hfp->owner->no_callsetup = hfp->nocallsetup;
 
-	/* read the OK message */
-	if (!hfp_wait(hfp) || at_read(hfp->rsock) != AT_OK)
-		return -1;
-
 	return 0;
 }
 
@@ -2925,6 +2790,83 @@
 */
 
 /*!
+ * \brief Handle the BRSF response.
+ * \param pvt a mbl_pvt structure
+ * \param buf a null terminated buffer containing an AT message
+ * \retval 0 success
+ * \retval -1 error
+ */
+static int handle_response_brsf(struct mbl_pvt *pvt, char *buf)
+{
+	struct msg_queue_entry *entry;
+	if ((entry = msg_queue_head(pvt)) && entry->expected == AT_BRSF) {
+		if (hfp_parse_brsf(pvt->hfp, buf)) {
+			ast_debug(1, "[%s] error parsing BRSF\n", pvt->id);
+			goto e_return;
+		}
+
+		if (msg_queue_push(pvt, AT_OK, AT_BRSF)) {
+			ast_debug(1, "[%s] error handling BRSF\n", pvt->id);
+			goto e_return;
+		}
+
+		msg_queue_free_and_pop(pvt);
+	} else if (entry) {
+		ast_debug(1, "[%s] recieved unexpected AT message 'BRSF' when expecting %s, ignoring\n", pvt->id, at_msg2str(entry->expected));
+	} else {
+		ast_debug(1, "[%s] recieved unexpected AT message 'BRSF'\n", pvt->id);
+	}
+
+	return 0;
+
+e_return:
+	msg_queue_free_and_pop(pvt);
+	return -1;
+}
+
+/*!
+ * \brief Handle the CIND response.
+ * \param pvt a mbl_pvt structure
+ * \param buf a null terminated buffer containing an AT message
+ * \retval 0 success
+ * \retval -1 error
+ */
+static int handle_response_cind(struct mbl_pvt *pvt, char *buf)
+{
+	struct msg_queue_entry *entry;
+	if ((entry = msg_queue_head(pvt)) && entry->expected == AT_CIND) {
+		switch (entry->response_to) {
+		case AT_CIND_TEST:
+			if (hfp_parse_cind_test(pvt->hfp, buf) || msg_queue_push(pvt, AT_OK, AT_CIND_TEST)) {
+				ast_debug(1, "[%s] error performing CIND test\n", pvt->id);
+				goto e_return;
+			}
+			break;
+		case AT_CIND:
+			if (hfp_parse_cind(pvt->hfp, buf) || msg_queue_push(pvt, AT_OK, AT_CIND)) {
+				ast_debug(1, "[%s] error getting CIND state\n", pvt->id);
+				goto e_return;
+			}
+			break;
+		default:
+			ast_debug(1, "[%s] error getting CIND state\n", pvt->id);
+			goto e_return;
+		}
+		msg_queue_free_and_pop(pvt);
+	} else if (entry) {
+		ast_debug(1, "[%s] recieved unexpected AT message 'CIND' when expecting %s, ignoring\n", pvt->id, at_msg2str(entry->expected));
+	} else {
+		ast_debug(1, "[%s] recieved unexpected AT message 'CIND'\n", pvt->id);
+	}
+
+	return 0;
+
+e_return:
+	msg_queue_free_and_pop(pvt);
+	return -1;
+}
+
+/*!
  * \brief Handle OK AT messages.
  * \param pvt a mbl_pvt structure
  * \param buf a null terminated buffer containing an AT message
@@ -2936,6 +2878,108 @@
 	struct msg_queue_entry *entry;
 	if ((entry = msg_queue_head(pvt)) && entry->expected == AT_OK) {
 		switch (entry->response_to) {
+
+		/* initilization stuff */
+		case AT_BRSF:
+			ast_debug(1, "[%s] BSRF sent successfully\n", pvt->id);
+
+			/* If this is a blackberry do CMER now, otherwise
+			 * continue with CIND as normal. */
+			if (pvt->blackberry) {
+				if (hfp_send_cmer(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CMER)) {
+					ast_debug(1, "[%s] error sending CMER\n", pvt->id);
+					goto e_return;
+				}
+			} else {
+				if (hfp_send_cind_test(pvt->hfp) || msg_queue_push(pvt, AT_CIND, AT_CIND_TEST)) {
+					ast_debug(1, "[%s] error sending CIND test\n", pvt->id);
+					goto e_return;
+				}
+			}
+			break;
+		case AT_CIND_TEST:
+			ast_debug(1, "[%s] CIND test sent successfully\n", pvt->id);
+
+			if (hfp_send_cind(pvt->hfp) || msg_queue_push(pvt, AT_CIND, AT_CIND)) {
+				ast_debug(1, "[%s] error requesting CIND state\n", pvt->id);
+				goto e_return;
+			}
+			break;
+		case AT_CIND:
+			ast_debug(1, "[%s] CIND sent successfully\n", pvt->id);
+
+			/* check if a call is active */
+			if (pvt->hfp->cind_state[pvt->hfp->cind_map.call]) {
+				ast_verb(3, "Bluetooth Device %s has a call in progress - delaying connection.\n", pvt->id);
+				goto e_return;
+			}
+
+			/* If this is NOT a blackberry proceed with CMER,
+			 * otherwise send CLIP. */
+			if (!pvt->blackberry) {
+				if (hfp_send_cmer(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CMER)) {
+					ast_debug(1, "[%s] error sending CMER\n", pvt->id);
+					goto e_return;
+				}
+			} else {
+				if (hfp_send_clip(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CLIP)) {
+					ast_debug(1, "[%s] error enabling calling line notification\n", pvt->id);
+					goto e_return;
+				}
+			}
+			break;
+		case AT_CMER:
+			ast_debug(1, "[%s] CMER sent successfully\n", pvt->id);
+
+			/* If this is a blackberry proceed with the CIND test,
+			 * otherwise send CLIP. */
+			if (pvt->blackberry) {
+				if (hfp_send_cind_test(pvt->hfp) || msg_queue_push(pvt, AT_CIND, AT_CIND_TEST)) {
+					ast_debug(1, "[%s] error sending CIND test\n", pvt->id);
+					goto e_return;
+				}
+			} else {
+				if (hfp_send_clip(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CLIP)) {
+					ast_debug(1, "[%s] error enabling calling line notification\n", pvt->id);
+					goto e_return;
+				}
+			}
+			break;
+		case AT_CLIP:
+			ast_debug(1, "[%s] caling line indication enabled\n", pvt->id);
+			if (hfp_send_vgs(pvt->hfp, 15) || msg_queue_push(pvt, AT_OK, AT_VGS)) {
+				ast_debug(1, "[%s] error synchronizing gain settings\n", pvt->id);
+				goto e_return;
+			}
+
+			pvt->timeout = -1;
+			pvt->hfp->initialized = 1;
+			ast_verb(3, "Bluetooth Device %s initialized and ready.\n", pvt->id);
+
+			break;
+		case AT_VGS:
+			ast_debug(1, "[%s] volume level synchronization successful\n", pvt->id);
+
+			/* set the SMS operating mode to text mode */
+			if (hfp_send_cmgf(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CMGF)) {
+				ast_debug(1, "[%s] error setting CMGF\n", pvt->id);
+				goto e_return;
+			}
+			break;
+		case AT_CMGF:
+			ast_debug(1, "[%s] sms text mode enabled\n", pvt->id);
+			/* turn on SMS new message indication */
+			if (hfp_send_cnmi(pvt->hfp) || msg_queue_push(pvt, AT_OK, AT_CNMI)) {
+				ast_debug(1, "[%s] error setting CNMI\n", pvt->id);
+				goto e_return;
+			}
+			break;
+		case AT_CNMI:
+			ast_debug(1, "[%s] sms new message indication enabled\n", pvt->id);
+			pvt->has_sms = 1;
+			break;
+		/* end initilization stuff */
+
 		case AT_A:
 			ast_debug(1, "[%s] answer sent successfully\n", pvt->id);
 			pvt->needchup = 1;
@@ -2971,6 +3015,10 @@
 		ast_debug(1, "[%s] recieved unexpected AT message 'OK'\n", pvt->id);
 	}
 	return 0;
+
+e_return:
+	msg_queue_free_and_pop(pvt);
+	return -1;
 }
 
 /*!
@@ -2982,13 +3030,48 @@
  */
 static int handle_response_error(struct mbl_pvt *pvt, char *buf)
 {
-	int res = 0;
 	struct msg_queue_entry *entry;
 	if ((entry = msg_queue_head(pvt))
 			&& (entry->expected == AT_OK
 			|| entry->expected == AT_ERROR
 			|| entry->expected == AT_SMS_PROMPT)) {
 		switch (entry->response_to) {
+
+		/* initilization stuff */
+		case AT_BRSF:
+			ast_debug(1, "[%s] error reading BSRF\n", pvt->id);
+			goto e_return;
+		case AT_CIND_TEST:
+			break;
+		case AT_CIND:
+			break;
+		case AT_CMER:
+			break;
+		case AT_CLIP:
+			ast_debug(1, "[%s] error enabling calling line indication\n", pvt->id);
+			goto e_return;
+			break;
+		case AT_VGS:
+			ast_debug(1, "[%s] volume level synchronization failed\n", pvt->id);
+
+			/* this is not a fatal error, let's continue with initilization */
+
+			/* set the SMS operating mode to text mode */
+			if (hfp_send_cmgf(pvt->hfp, 1) || msg_queue_push(pvt, AT_OK, AT_CMGF)) {
+				ast_debug(1, "[%s] error setting CMGF\n", pvt->id);
+				goto e_return;
+			}
+			break;
+		case AT_CMGF:
+			ast_debug(1, "[%s] error setting CMGF\n", pvt->id);
+			ast_debug(1, "[%s] no SMS support\n", pvt->id);
+			break;
+		case AT_CNMI:
+			ast_debug(1, "[%s] error setting CNMI\n", pvt->id);
+			ast_debug(1, "[%s] no SMS support\n", pvt->id);
+			break;
+		/* end initilization stuff */
+
 		case AT_A:
 			ast_debug(1, "[%s] answer failed\n", pvt->id);
 			mbl_queue_hangup(pvt);
@@ -2999,8 +3082,7 @@
 			break;
 		case AT_CHUP:
 			ast_debug(1, "[%s] error sending hangup, disconnecting\n", pvt->id);
-			res = -1;
-			break;
+			goto e_return;
 		case AT_CMGR:
 			ast_debug(1, "[%s] error reading sms message\n", pvt->id);
 			pvt->incoming_sms = 0;
@@ -3024,7 +3106,11 @@
 		ast_debug(1, "[%s] recieved unexpected AT message 'ERROR'\n", pvt->id);
 	}
 
-	return res;
+	return 0;
+
+e_return:
+	msg_queue_free_and_pop(pvt);
+	return -1;
 }
 
 /*!
@@ -3264,17 +3350,23 @@
 	char buf[256];
 	int t;
 	at_message_t at_msg;
+	struct msg_queue_entry *entry;
 
+	/* Note: At one point the initilization procedure was neatly contained
+	 * in the hfp_init() function, but that initilization method did not
+	 * work with non standard devices.  As a result, the initilization
+	 * procedure is not spread throughout the event handling loop.
+	 */
+
+	/* start initilization with the BRSF request */
 	ast_mutex_lock(&pvt->lock);
-	if (hfp_init(hfp)) {
-		ast_mutex_unlock(&pvt->lock);
-		ast_verb(3, "Error initializing Bluetooth device %s.\n", pvt->id);
+	pvt->timeout = 10000;
+	if (hfp_send_brsf(hfp, &hfp_our_brsf)  || msg_queue_push(pvt, AT_BRSF, AT_BRSF)) {
+		ast_debug(1, "[%s] error sending BRSF\n", hfp->owner->id);
 		goto e_cleanup;
 	}
 	ast_mutex_unlock(&pvt->lock);
 
-	ast_verb(3, "Bluetooth Device %s initialised and ready.\n", pvt->id);
-
 	while (!check_unloading()) {
 		ast_mutex_lock(&pvt->lock);
 		t = pvt->timeout;
@@ -3282,6 +3374,29 @@
 
 		if (!rfcomm_wait(pvt->rfcomm_socket, &t)) {
 			ast_debug(1, "[%s] timeout waiting for rfcomm data, disconnecting\n", pvt->id);
+			ast_mutex_lock(&pvt->lock);
+			if (!hfp->initialized) {
+				if ((entry = msg_queue_head(pvt))) {
+					switch (entry->response_to) {
+					case AT_CIND_TEST:
+						if (pvt->blackberry)
+							ast_debug(1, "[%s] timeout during CIND test\n", hfp->owner->id);
+						else
+							ast_debug(1, "[%s] timeout during CIND test, try setting 'blackberry=yes'\n", hfp->owner->id);
+						break;
+					case AT_CMER:
+						if (pvt->blackberry)
+							ast_debug(1, "[%s] timeout after sending CMER, try setting 'blackberry=no'\n", hfp->owner->id);
+						else
+							ast_debug(1, "[%s] timeout after sending CMER\n", hfp->owner->id);
+						break;
+					default:
+						ast_debug(1, "[%s] timeout while waiting for %s in response to %s\n", pvt->id, at_msg2str(entry->expected), at_msg2str(entry->response_to));
+						break;
+					}
+				}
+			}
+			ast_mutex_unlock(&pvt->lock);
 			goto e_cleanup;
 		}
 
@@ -3296,6 +3411,22 @@
 		ast_debug(1, "[%s] %s\n", pvt->id, buf);
 
 		switch (at_msg) {
+		case AT_BRSF:
+			ast_mutex_lock(&pvt->lock);
+			if (handle_response_brsf(pvt, buf)) {
+				ast_mutex_unlock(&pvt->lock);
+				goto e_cleanup;
+			}
+			ast_mutex_unlock(&pvt->lock);
+			break;
+		case AT_CIND:
+			ast_mutex_lock(&pvt->lock);
+			if (handle_response_cind(pvt, buf)) {
+				ast_mutex_unlock(&pvt->lock);
+				goto e_cleanup;
+			}
+			ast_mutex_unlock(&pvt->lock);
+			break;
 		case AT_OK:
 			ast_mutex_lock(&pvt->lock);
 			if (handle_response_ok(pvt, buf)) {
@@ -3374,6 +3505,9 @@
 
 e_cleanup:
 
+	if (!hfp->initialized)
+		ast_verb(3, "Error initializing Bluetooth device %s.\n", pvt->id);
+
 	ast_mutex_lock(&pvt->lock);
 	if (pvt->owner) {
 		ast_debug(1, "[%s] device disconnected, hanging up owner\n", pvt->id);
@@ -3927,7 +4061,6 @@
 
 		pvt->hfp->owner = pvt;
 		pvt->hfp->rport = pvt->rfcomm_port;
-		pvt->hfp->blackberry = pvt->blackberry;
 		pvt->hfp->nocallsetup = pvt->no_callsetup;
 	}
 
