Fix: Support devices with CSCB mode 0 only

MBM devices do not support CSCB mode 1 (remove topics).  It is thus
necessary to use mode 0 and sending an empty topic list in order to turn
off all topics.  This patch will first query the CSCB modes supported by
the hardware.  If mode 1 is supported, then the old behavior is used,
otherwise the new behavior is used.

diff --git a/drivers/atmodem/cbs.c b/drivers/atmodem/cbs.c
index b2a977c..661207b 100644 (file)
--- a/drivers/atmodem/cbs.c
+++ b/drivers/atmodem/cbs.c
@@ -25,6 +25,7 @@
 
 #define _GNU_SOURCE
 #include <string.h>
+#include <stdio.h>
 
 #include <glib.h>
 
@@ -39,6 +40,12 @@
 #include "atmodem.h"
 
 static const char *none_prefix[] = { NULL };
+static const char *cscb_prefix[] = { "+CSCB:", NULL };
+
+struct cbs_data {
+       GAtChat *chat;
+       gboolean cscb_mode_1;
+};
 
 static void at_cbm_notify(GAtResult *result, gpointer user_data)
 {
@@ -96,7 +103,7 @@ static void at_cscb_set_cb(gboolean ok, GAtResult *result, gpointer user_data)
 static void at_cbs_set_topics(struct ofono_cbs *cbs, const char *topics,
                                ofono_cbs_set_cb_t cb, void *user_data)
 {
-       GAtChat *chat = ofono_cbs_get_data(cbs);
+       struct cbs_data *data = ofono_cbs_get_data(cbs);
        struct cb_data *cbd = cb_data_new(cb, user_data);
        char *buf;
        unsigned int id;
@@ -106,7 +113,7 @@ static void at_cbs_set_topics(struct ofono_cbs *cbs, const char *topics,
 
        buf = g_strdup_printf("AT+CSCB=0,\"%s\"", topics);
 
-       id = g_at_chat_send(chat, buf, none_prefix,
+       id = g_at_chat_send(data->chat, buf, none_prefix,
                                at_cscb_set_cb, cbd, g_free);
 
        g_free(buf);
@@ -124,13 +131,19 @@ error:
 static void at_cbs_clear_topics(struct ofono_cbs *cbs,
                                ofono_cbs_set_cb_t cb, void *user_data)
 {
-       GAtChat *chat = ofono_cbs_get_data(cbs);
+       struct cbs_data *data = ofono_cbs_get_data(cbs);
        struct cb_data *cbd = cb_data_new(cb, user_data);
+       char buf[256];
 
        if (!cbd)
                goto error;
 
-       if (g_at_chat_send(chat, "AT+CSCB=1,\"0-65535\"", none_prefix,
+       if (data->cscb_mode_1)
+               sprintf(buf, "AT+CSCB=1,\"0-65535\"");
+       else
+               sprintf(buf, "AT+CSCB=0,\"\"");
+
+       if (g_at_chat_send(data->chat, buf, none_prefix,
                                at_cscb_set_cb, cbd, g_free) > 0)
                return;
 
@@ -144,7 +157,7 @@ error:
 static void at_cbs_register(gboolean ok, GAtResult *result, gpointer user)
 {
        struct ofono_cbs *cbs = user;
-       GAtChat *chat = ofono_cbs_get_data(cbs);
+       struct cbs_data *data = ofono_cbs_get_data(cbs);
 
        /* This driver assumes that something else will properly setup
         * CNMI notifications to deliver CBS broadcasts via +CBM.  We do
@@ -154,23 +167,69 @@ static void at_cbs_register(gboolean ok, GAtResult *result, gpointer user)
         * The default SMS driver will setup the CNMI for +CBM delivery
         * appropriately for us
         */
-       g_at_chat_register(chat, "+CBM:", at_cbm_notify, TRUE, cbs, NULL);
+       g_at_chat_register(data->chat, "+CBM:", at_cbm_notify, TRUE, cbs, NULL);
 
        ofono_cbs_register(cbs);
 }
 
-static int at_cbs_probe(struct ofono_cbs *cbs, unsigned int vendor,
-                               void *data)
+static void at_cscb_support_cb(gboolean ok, GAtResult *result, gpointer user)
 {
-       GAtChat *chat = data;
+       struct ofono_cbs *cbs = user;
+       struct cbs_data *data = ofono_cbs_get_data(cbs);
+       gint range[2];
+       GAtResultIter iter;
+       char buf[256];
+
+       dump_response("at_cscb_support_cb", ok, result);
 
-       ofono_cbs_set_data(cbs, chat);
+       if (!ok)
+               goto error;
+
+       g_at_result_iter_init(&iter, result);
+
+       if (!g_at_result_iter_next(&iter, "+CSCB:"))
+               goto error;
+
+       if (!g_at_result_iter_open_list(&iter))
+               goto error;
 
-       /* Start with CBS not accepting any channels.  The core will
-        * power on / set preferred channels when it is ready
+       while (g_at_result_iter_next_range(&iter, &range[0], &range[1]))
+               if (1 >= range[0] && 1 <= range[1])
+                       data->cscb_mode_1 = TRUE;
+
+       g_at_result_iter_close_list(&iter);
+
+       /* Assume that if CSCB mode 1 is supported, then we need to use
+        * it to remove topics, otherwise we need to set the entire list
+        * of new topics using CSCB mode 0.
         */
-       g_at_chat_send(chat, "AT+CSCB=1,\"0-65535\"", none_prefix,
-                               at_cbs_register, cbs, NULL);
+       if (data->cscb_mode_1)
+               sprintf(buf, "AT+CSCB=1,\"0-65535\"");
+       else
+               sprintf(buf, "AT+CSCB=0,\"\"");
+
+       if (g_at_chat_send(data->chat, buf, none_prefix,
+                               at_cbs_register, cbs, NULL) > 0)
+               return;
+
+error:
+       ofono_error("CSCB not supported");
+       ofono_cbs_remove(cbs);
+}
+
+static int at_cbs_probe(struct ofono_cbs *cbs, unsigned int vendor,
+                               void *user)
+{
+       GAtChat *chat = user;
+       struct cbs_data *data;
+
+       data = g_new0(struct cbs_data, 1);
+       data->chat = chat;
+
+       ofono_cbs_set_data(cbs, data);
+
+       g_at_chat_send(chat, "AT+CSCB=?", cscb_prefix,
+                       at_cscb_support_cb, cbs, NULL);
 
        return 0;
 }