--- trunk.org/channels/chan_mobile.c	2009-06-05 13:37:44.000000000 +0200
+++ trunk/channels/chan_mobile.c	2009-06-05 15:22:05.000000000 +0200
@@ -167,17 +167,20 @@
 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_cusd(struct mbl_pvt *pvt, char *buf);
 static int handle_sms_prompt(struct mbl_pvt *pvt, char *buf);
 
 /* CLI stuff */
 static char *handle_cli_mobile_show_devices(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a);
 static char *handle_cli_mobile_search(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a);
 static char *handle_cli_mobile_rfcomm(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a);
+static char *handle_cli_mobile_cusd(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a);
 
 static struct ast_cli_entry mbl_cli[] = {
 	AST_CLI_DEFINE(handle_cli_mobile_show_devices, "Show Bluetooth Cell / Mobile devices"),
 	AST_CLI_DEFINE(handle_cli_mobile_search,       "Search for Bluetooth Cell / Mobile devices"),
 	AST_CLI_DEFINE(handle_cli_mobile_rfcomm,       "Send commands to the rfcomm port for debugging"),
+	AST_CLI_DEFINE(handle_cli_mobile_cusd,         "Send CUSD commands to the mobile"),
 };
 
 /* App stuff */
@@ -374,6 +377,7 @@
 static int hfp_send_chup(struct hfp_pvt *hfp);
 static int hfp_send_atd(struct hfp_pvt *hfp, const char *number);
 static int hfp_send_ata(struct hfp_pvt *hfp);
+static int hfp_send_cusd(struct hfp_pvt *hfp, const char *code);
 
 /*
  * bluetooth headset profile helpers
@@ -418,6 +422,7 @@
 	AT_CNMI,
 	AT_CMER,
 	AT_CIND_TEST,
+	AT_CUSD,
 } at_message_t;
 
 static int at_match_prefix(char *buf, char *prefix);
@@ -624,6 +629,54 @@
 	return CLI_SUCCESS;
 }
 
+static char *handle_cli_mobile_cusd(struct ast_cli_entry *e, int cmd, struct ast_cli_args *a)
+{
+	char buf[128];
+	struct mbl_pvt *pvt = NULL;
+
+	switch (cmd) {
+	case CLI_INIT:
+		e->command = "mobile cusd";
+		e->usage =
+			"Usage: mobile cusd <device ID> <command>\n"
+			"       Send cusd <command> to the rfcomm port on the device\n"
+			"       with the specified <device ID>.\n";
+		return NULL;
+	case CLI_GENERATE:
+		return NULL;
+	}
+
+	if (a->argc != 4)
+		return CLI_SHOWUSAGE;
+
+	AST_RWLIST_RDLOCK(&devices);
+	AST_RWLIST_TRAVERSE(&devices, pvt, entry) {
+		if (!strcmp(pvt->id, a->argv[2]))
+			break;
+	}
+	AST_RWLIST_UNLOCK(&devices);
+
+	if (!pvt) {
+		ast_cli(a->fd, "Device %s not found.\n", a->argv[2]);
+		goto e_return;
+	}
+
+	ast_mutex_lock(&pvt->lock);
+	if (!pvt->connected) {
+		ast_cli(a->fd, "Device %s not connected.\n", a->argv[2]);
+		goto e_unlock_pvt;
+	}
+
+	snprintf(buf, sizeof(buf), "%s", a->argv[3]);
+	hfp_send_cusd(pvt->hfp, buf);
+	msg_queue_push(pvt, AT_OK, AT_CUSD);
+
+e_unlock_pvt:
+	ast_mutex_unlock(&pvt->lock);
+e_return:
+	return CLI_SUCCESS;
+}
+
 /*
 
 	Dialplan applications implementation
@@ -1826,6 +1879,8 @@
 		return AT_VGM;
 	} else if (at_match_prefix(buf, "AT+VGS=")) {
 		return AT_VGS;
+	} else if (at_match_prefix(buf, "+CUSD:")) {
+		return AT_CUSD;
 	} else {
 		return AT_UNKNOWN;
 	}
@@ -1895,6 +1950,8 @@
 		return "AT+CMER";
 	case AT_CIND_TEST:
 		return "AT+CIND=?";
+	case AT_CUSD:
+		return "AT+CUSD";
 	}
 }
 
@@ -2507,6 +2564,18 @@
 	return 0;
 }
 
+/*!
+ * \brief Send CUSD.
+ * \param hfp an hfp_pvt struct
+ * \param code the CUSD code to send
+ */
+static int hfp_send_cusd(struct hfp_pvt *hfp, const char *code)
+{
+	char cmd[128];
+	snprintf(cmd, sizeof(cmd), "AT+CUSD=1,\"%s\",15\r", code);
+	return rfcomm_write(hfp->rsock, cmd);
+}
+
 
 /*
  * Bluetooth Headset Profile helpers
@@ -2997,6 +3066,9 @@
 		case AT_VTS:
 			ast_debug(1, "[%s] digit sent successfully\n", pvt->id);
 			break;
+		case AT_CUSD:
+			ast_debug(1, "[%s] CUSD code sent successfully\n", pvt->id);
+			break;
 		case AT_UNKNOWN:
 		default:
 			ast_debug(1, "[%s] recieved OK for unhandled request: %s\n", pvt->id, at_msg2str(entry->response_to));
@@ -3093,6 +3165,9 @@
 		case AT_VTS:
 			ast_debug(1, "[%s] error sending digit\n", pvt->id);
 			break;
+		case AT_CUSD:
+			ast_verb(1, "[%s] Could not send CUSD command\n", pvt->id);
+			break;
 		case AT_UNKNOWN:
 		default:
 			ast_debug(1, "[%s] recieved ERROR for unhandled request: %s\n", pvt->id, at_msg2str(entry->response_to));
@@ -3360,6 +3435,25 @@
 	return 0;
 }
 
+/*!
+ * \brief Handle CUSD 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_cusd(struct mbl_pvt *pvt, char *buf)
+{
+	char response_buf[strlen(buf)];
+	memset (response_buf,'\0',strlen(buf));
+	
+	// Trim the string till the first occourence of " and remove the last char (the ").
+	strncpy(response_buf,strchr(buf,'"') + 1,strlen(strchr(buf,'"'))-2);
+	
+	ast_verb(1, "Got CUSD response from device %s: %s\n", pvt->id,response_buf);
+	return 0;
+}
+
 
 static void *do_monitor_phone(void *data)
 {
@@ -3510,6 +3604,14 @@
 			}
 			ast_mutex_unlock(&pvt->lock);
 			break;
+		case AT_CUSD:
+			ast_mutex_lock(&pvt->lock);
+			if (handle_response_cusd(pvt, buf)) {
+				ast_mutex_unlock(&pvt->lock);
+				goto e_cleanup;
+			}
+			ast_mutex_unlock(&pvt->lock);
+			break;
 		case AT_UNKNOWN:
 			ast_debug(1, "[%s] ignoring unknown message: %s\n", pvt->id, buf);
 			break;