Index: addons/chan_mobile.c
===================================================================
--- addons/chan_mobile.c	(revision 207364)
+++ addons/chan_mobile.c	(working copy)
@@ -161,6 +161,7 @@
 static int handle_response_ring(struct mbl_pvt *pvt, char *buf);
 static int handle_response_cmti(struct mbl_pvt *pvt, char *buf);
 static int handle_response_cmgr(struct mbl_pvt *pvt, char *buf);
+static int handle_response_cnmi(struct mbl_pvt *pvt, char *buf);
 static int handle_sms_prompt(struct mbl_pvt *pvt, char *buf);
 
 /* CLI stuff */
@@ -321,6 +322,7 @@
 	int cind_index[16];		/*!< the cind/ciev index to name mapping for this AG */
 	int cind_state[16];		/*!< the cind/ciev state for this AG */
 	struct hfp_cind cind_map;	/*!< the cind name to index mapping for this AG */
+	int cnmi;			/*!< the accepted cnmi value */
 	int rsock;			/*!< our rfcomm socket */
 	int rport;			/*!< our rfcomm port */
 };
@@ -347,6 +349,7 @@
 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_parse_cnmi_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 +366,7 @@
 #endif
 static int hfp_send_dtmf(struct hfp_pvt *hfp, char digit);
 static int hfp_send_cmgf(struct hfp_pvt *hfp, int mode);
+static int hfp_send_cnmi_test(struct hfp_pvt *hfp);
 static int hfp_send_cnmi(struct hfp_pvt *hfp);
 static int hfp_send_cmgr(struct hfp_pvt *hfp, int index);
 static int hfp_send_cmgs(struct hfp_pvt *hfp, const char *number);
@@ -412,6 +416,7 @@
 	AT_VTS,
 	AT_CMGF,
 	AT_CNMI,
+	AT_CNMI_TEST,
 	AT_CMER,
 	AT_CIND_TEST,
 } at_message_t;
@@ -2237,12 +2242,23 @@
 }
 
 /*!
+ * \brief Send an SMS new message indication test.
+ * \param hfp an hfp_pvt struct
+ */
+static int hfp_send_cnmi_test(struct hfp_pvt *hfp)
+{
+	return rfcomm_write(hfp->rsock, "AT+CNMI=?\r");
+}
+
+/*!
  * \brief Setup SMS new message indication.
  * \param hfp an hfp_pvt struct
  */
 static int hfp_send_cnmi(struct hfp_pvt *hfp)
 {
-	return rfcomm_write(hfp->rsock, "AT+CNMI=2,1,0,0,0\r");
+	char cmd[32];
+	snprintf(cmd, sizeof(cmd), "AT+CNMI=%d,1,0,0,0\r", hfp->cnmi);
+	return rfcomm_write(hfp->rsock, cmd);
 }
 
 /*!
@@ -2405,7 +2421,188 @@
 	return 0;
 }
 
+/*
+ * CNMI parsing code
+ */
+
 /*!
+ * \brief Match a single character.
+ * \param buf a pionter to a pointer to a buffer
+ * \param c the character to match
+ *
+ * In the event of a match, this function will return 0 and advance the buffer
+ * pointer.
+ *
+ * \retval -1 EOL
+ * \retval 1 no match
+ * \retval 0 success
+ */
+static int cnmi_match(char **buf, char c)
+{
+	if (ast_strlen_zero(*buf)) {
+		return -1;
+	}
+
+	if (**buf != c) {
+		return 1;
+	}
+
+	(*buf)++;
+	return 0;
+}
+
+/*!
+ * \brief Matches a digit.
+ * \param buf a pionter to a pointer to a buffer
+ * \param res a pointer to an integer where the result will be stored
+ *
+ * \retval 1 no match
+ * \retval 0 success
+ */
+static int cnmi_match_digit(char **buf, int *res)
+{
+	if (!cnmi_match(buf, '0')) {
+		*res = 0;
+	} else if (!cnmi_match(buf, '1')) {
+		*res = 1;
+	} else if (!cnmi_match(buf, '2')) {
+		*res = 2;
+	} else if (!cnmi_match(buf, '3')) {
+		*res = 3;
+	} else if (!cnmi_match(buf, '4')) {
+		*res = 4;
+	} else if (!cnmi_match(buf, '5')) {
+		*res = 5;
+	} else if (!cnmi_match(buf, '6')) {
+		*res = 6;
+	} else if (!cnmi_match(buf, '7')) {
+		*res = 7;
+	} else if (!cnmi_match(buf, '8')) {
+		*res = 8;
+	} else if (!cnmi_match(buf, '9')) {
+		*res = 9;
+	} else {
+		return 1;
+	}
+	return 0;
+}
+
+/*!
+ * \brief Match a block.
+ * \param buf a pionter to a pointer to a buffer
+ * \param hfp an hfp_pvt struct
+ *
+ * Example input:
+ * \code
+ * (1)
+ * (1-3)
+ * (1,2,3)
+ * (1,3-5)
+ * \endcode
+ *
+ * \retval 1 no match or error
+ * \retval 0 success
+ */
+static int cnmi_match_block(char **buf, struct hfp_pvt *hfp)
+{
+	int res, start, end;
+	res = start = end = 0;
+
+	if (cnmi_match(buf, '(')) {
+		return 1;
+	}
+
+match_digit_or_range:
+	if (cnmi_match_digit(buf, &start)) {
+		return 1;
+	}
+
+	if (start == 2 || start == 3) {
+		res = start;
+	}
+
+	if (!cnmi_match(buf, ')')) {
+		goto done;
+	} else if (!cnmi_match(buf, '-')) {
+		if (cnmi_match_digit(buf, &end)) {
+			return 1;
+		}
+
+		if (end == 2) {
+			res = 2;
+		} else if (start >= 2 && end <= 3) {
+			res = 3;
+		}
+
+		start = end = 0;
+
+		if (!cnmi_match(buf, ')')) {
+			goto done;
+		} else if (!cnmi_match(buf, ',')) {
+			goto match_digit_or_range;
+		} else {
+			return 1;
+		}
+	} else if (!cnmi_match(buf, ',')) {
+		goto match_digit_or_range;
+	} else {
+		return 1;
+	}
+done:
+	hfp->cnmi = res;
+	return 0;
+}
+
+/*!
+ * \brief Match a string.
+ * \param buf a pionter to a pointer to a buffer
+ * \param s a string
+ *
+ * \retval 1 no match or error
+ * \retval 0 success
+ */
+static int cnmi_match_string(char **buf, const char *s)
+{
+	for (; *s; s++) {
+		if (cnmi_match(buf, *s)) {
+			return 1;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * \brief Parse the result of the AT+CNMI=? command.
+ * \param hfp an hfp_pvt struct
+ * \param buf the buffer to parse (null terminated)
+ *
+ * If this function finds an acceptable value in the response it will store
+ * this value in hfp->cnmi.
+ */
+static int hfp_parse_cnmi_test(struct hfp_pvt *hfp, char *buf)
+{
+	char **b = &buf;
+
+	/* parse the accepted sms indication value.  It is in one of the
+	 * follwing formats (we only parse the first block):
+	 * +CNMI: (2),(1),(0,2),(0,1),(0)
+	 * +CNMI: (0,3),(0-2),(0),(0),(0)
+	 * +CNMI: (0,2,3),(1),(0),(0),(0)
+	 */
+
+	if (cnmi_match_string(b, "+CNMI: "))
+		return 1;
+
+	if (cnmi_match_block(b, hfp))
+		return 1;
+
+	if (hfp->cnmi == 0 )
+		return 1;
+
+	return 0;
+}
+
+/*!
  * \brief Parse the result of the AT+CIND=? command.
  * \param hfp an hfp_pvt struct
  * \param buf the buffer to parse (null terminated)
@@ -2954,6 +3151,21 @@
 		case AT_VGS:
 			ast_debug(1, "[%s] volume level synchronization successful\n", pvt->id);
 
+			if (hfp_send_cnmi_test(pvt->hfp) || msg_queue_push(pvt, AT_CNMI, AT_CNMI_TEST)) {
+				ast_debug(1, "[%s] error sending CNMI test, using default values\n", pvt->id);
+
+				pvt->hfp->cnmi = 2;
+
+				/* 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_CNMI_TEST:
+			ast_debug(1, "[%s] CNMI test sent successfully\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);
@@ -3031,6 +3243,7 @@
 			|| entry->expected == AT_ERROR
 			|| entry->expected == AT_CMS_ERROR
 			|| entry->expected == AT_CMGR
+			|| entry->expected == AT_CNMI
 			|| entry->expected == AT_SMS_PROMPT)) {
 		switch (entry->response_to) {
 
@@ -3061,6 +3274,18 @@
 				goto e_return;
 			}
 			break;
+		case AT_CNMI_TEST:
+			ast_debug(1, "[%s] error during CNMI test\n", pvt->id);
+			ast_debug(1, "[%s] using default CNMI settings\n", pvt->id);
+
+			pvt->hfp->cnmi = 2;
+
+			/* 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);
@@ -3331,6 +3556,41 @@
 }
 
 /*!
+ * \brief Handle +CNMI messages.
+ * \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_cnmi(struct mbl_pvt *pvt, char *buf)
+{
+	struct msg_queue_entry *entry;
+	if ((entry = msg_queue_head(pvt)) && entry->expected == AT_CNMI) {
+		if (hfp_parse_cnmi_test(pvt->hfp, buf)) {
+			ast_debug(1, "[%s] error performing CNMI test\n", pvt->id);
+			goto e_return;
+		}
+
+		if (msg_queue_push(pvt, AT_OK, AT_CNMI_TEST)) {
+			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 'CNMI' when expecting %s, ignoring\n", pvt->id, at_msg2str(entry->expected));
+	} else {
+		ast_debug(1, "[%s] recieved unexpected AT message 'CNMI'\n", pvt->id);
+	}
+
+	return 0;
+
+e_return:
+	msg_queue_free_and_pop(pvt);
+	return -1;
+}
+
+/*!
  * \brief Send an SMS message from the queue.
  * \param pvt a mbl_pvt structure
  * \param buf a null terminated buffer containing an AT message
@@ -3503,6 +3763,13 @@
 			}
 			ast_mutex_unlock(&pvt->lock);
 			break;
+		case AT_CNMI:
+			ast_mutex_lock(&pvt->lock);
+			if (handle_response_cnmi(pvt, buf)) {
+				ast_mutex_unlock(&pvt->lock);
+				goto e_cleanup;
+			}
+			ast_mutex_unlock(&pvt->lock);
 		case AT_SMS_PROMPT:
 			ast_mutex_lock(&pvt->lock);
 			if (handle_sms_prompt(pvt, buf)) {
