Manager: Added support for modem independent restart

Modem independent restart added. This includes support for detecting problems
with AT channels and triggering a restart of the modem and handling of
independent restart indications from the system.

Signed-off-by: Sverre Vegge <sverre.vegge@stericsson.com>

5 files changed, 671 insertions, 402 deletions

diff --git a/atchannel.c b/atchannel.c
index 6753abf..5e3d666 100644
--- a/atchannel.c
+++ b/atchannel.c
@@ -124,13 +124,26 @@ static void setAtContext(struct atcontext *ac)
     (void) pthread_setspecific(key, ac);
 }
 
+static void ac_free(void)
+{
+    struct atcontext *ac = NULL;
+    (void) pthread_once(&key_once, make_key);
+    if ((ac = pthread_getspecific(key)) != NULL) {
+        free(ac);
+        LOGD("%s() freed current thread AT context", __func__);
+    } else {
+        LOGW("%s() No AT context exist for current thread, cannot free it",
+            __func__);
+    }
+}
+
 static int initializeAtContext()
 {
     struct atcontext *ac = NULL;
 
     if (pthread_once(&key_once, make_key)) {
-        LOGE("Pthread_once failed!");
-        return -1;
+        LOGE("%s() Pthread_once failed!", __func__);
+        goto error;
     }
 
     ac = pthread_getspecific(key);
@@ -147,9 +160,9 @@ static int initializeAtContext()
         ac->ATBufferCur = ac->ATBuffer;
 
         if (pipe(ac->readerCmdFds)) {
-            LOGE("initializeAtContext(): Failed to create pipe: %s",
+            LOGE("%s(): Failed to create pipe: %s", __func__,
                  strerror(errno));
-            return -1;
+            goto error;
         }
 
         pthread_mutex_init(&ac->commandmutex, NULL);
@@ -160,14 +173,17 @@ static int initializeAtContext()
         ac->timeoutMsec = DEFAULT_AT_TIMEOUT_MSEC;
 
         if (pthread_setspecific(key, ac)) {
-            LOGE("pthread_setspecific failed!");
-            return -1;
+            LOGE("%s() calling pthread_setspecific failed!", __func__);
+            goto error;
         }
     }
-
     LOGI("Initialized new AT Context!");
-
     return 0;
+
+error:
+    LOGE("%s() failed initializing new AT Context!", __func__);
+    free(ac);
+    return -1;
 }
 
 static struct atcontext *getAtContext() {
@@ -414,7 +430,7 @@ static void processLine(const char *line)
             break;
 
         default:               /* This should never be reached */
-            LOGE("Unsupported AT command type %d\n", ac->type);
+            LOGE("Unsupported AT command type %d", ac->type);
             handleUnsolicited(line);
             break;
         }
@@ -579,7 +595,7 @@ static const char *readline()
         struct pollfd pfds[2];
 
         if (0 >= MAX_AT_RESPONSE - (p_read - ac->ATBuffer)) {
-            LOGE("ERROR: Input line exceeded buffer\n");
+            LOGE("%s() ERROR: Input line exceeded buffer", __func__);
             /* Ditch buffer and start over again. */
             ac->ATBufferCur = ac->ATBuffer;
             *ac->ATBufferCur = '\0';
@@ -599,7 +615,7 @@ static const char *readline()
         err = poll(pfds, 2, -1);
 
         if (err < 0) {
-            LOGE("poll: error: %s", strerror(errno));
+            LOGE("%s() poll: error: %s", __func__, strerror(errno));
             return NULL;
         }
 
@@ -612,7 +628,7 @@ static const char *readline()
         }
 
         if (pfds[0].revents & POLLERR) {
-            LOGE("POLLERR! Returning..");
+            LOGE("%s() POLLERR event! Returning...", __func__);
             return NULL;
         }
 
@@ -639,9 +655,9 @@ static const char *readline()
         } else if (count <= 0) {
             /* Read error encountered or EOF reached. */
             if (count == 0)
-                LOGD("atchannel: EOF reached.");
+                LOGD("%s() atchannel: EOF reached.", __func__);
             else
-                LOGD("atchannel: read error %s", strerror(errno));
+                LOGD("%s() atchannel: read error %s", __func__, strerror(errno));
 
             return NULL;
         }
@@ -670,7 +686,7 @@ static const char *readline()
         break;
     }
 
-    LOGI("AT(%d)< %s\n", ac->fd, ret);
+    LOGI("AT(%d)< %s", ac->fd, ret);
     return ret;
 }
 
@@ -698,7 +714,7 @@ static void *readerLoop(void *arg)
 {
     struct atcontext *ac = NULL;
 
-    LOGE("In readerloop!!");
+    LOGI("Entering readerloop!");
 
     setAtContext((struct atcontext *) arg);
     ac = getAtContext();
@@ -735,7 +751,6 @@ static void *readerLoop(void *arg)
     }
 
     onReaderClosed();
-
     return NULL;
 }
 
@@ -771,7 +786,7 @@ static int writeline(const char *s)
         goto error;
     }
 
-    LOGD("AT(%d)> %s\n", ac->fd, p_s);
+    LOGD("AT(%d)> %s", ac->fd, p_s);
     AT_DUMP(">> ", p_s, strlen(p_s));
 
     /* The main string. */
@@ -824,7 +839,7 @@ static int writeCtrlZ(const char *s)
         goto error;
     }
 
-    LOGD("AT> %s\n", p_s);
+    LOGD("AT> %s", p_s);
     AT_DUMP(">* ", p_s, strlen(p_s));
 
     /* The main string. */
@@ -874,7 +889,7 @@ int at_open(int fd, ATUnsolHandler h)
 
     if (initializeAtContext()) {
         LOGE("InitializeAtContext failed!");
-        return -1;
+        goto error;
     }
 
     ac = getAtContext();
@@ -892,41 +907,46 @@ int at_open(int fd, ATUnsolHandler h)
     pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
 
     ret = pthread_create(&ac->tid_reader, &attr, readerLoop, ac);
-
     if (ret < 0) {
         perror("pthread_create");
-        return -1;
+        goto error;
     }
 
-
     return 0;
+error:
+    ac_free();
+    return -1;
 }
 
 /* FIXME is it ok to call this from the reader and the command thread? */
 void at_close()
 {
-    struct atcontext *ac = getAtContext();
+    struct atcontext *ac;
     ssize_t written;
 
-    if (ac->fd >= 0)
-        if (close(ac->fd) != 0)
-            LOGE("FAILED to close fd %d!", ac->fd);
+    /* Find AT context to current thead */
+    (void) pthread_once(&key_once, make_key);
+    if ((ac = pthread_getspecific(key)) != NULL) {
+        if (ac->fd >= 0)
+            if (close(ac->fd) != 0)
+                LOGE("FAILED to close fd %d!", ac->fd);
 
-    ac->fd = -1;
+        ac->fd = -1;
 
-    pthread_mutex_lock(&ac->commandmutex);
+        pthread_mutex_lock(&ac->commandmutex);
 
-    ac->readerClosed = 1;
+        ac->readerClosed = 1;
 
-    pthread_cond_signal(&ac->commandcond);
+        pthread_cond_signal(&ac->commandcond);
 
-    pthread_mutex_unlock(&ac->commandmutex);
+        pthread_mutex_unlock(&ac->commandmutex);
 
-    /* Kick readerloop. */
-    written = write(ac->readerCmdFds[1], "x", 1);
+        /* Kick readerloop. */
+        written = write(ac->readerCmdFds[1], "x", 1);
 
-    if (written < 0)
-        LOGE("FAILED to kick readerloop!");
+        if (written < 0)
+            LOGE("FAILED to kick readerloop!");
+    }
 }
 
 void at_response_free(ATResponse *p_response)
@@ -991,7 +1011,7 @@ static int at_send_command_full_nolock(const char *command,
 
 #ifndef USE_NP
     struct timespec ts;
-#endif                          /*USE_NP */
+#endif /*USE_NP */
 
     struct atcontext *ac = getAtContext();
 
@@ -1027,7 +1047,7 @@ static int at_send_command_full_nolock(const char *command,
     if (timeoutMsec != 0)
         setTimespecRelative(&ts, timeoutMsec);
 
-#endif                          /*USE_NP */
+#endif /*USE_NP */
 
     while (ac->response->finalResponse == NULL && ac->readerClosed == 0) {
         if (timeoutMsec != 0) {
@@ -1039,7 +1059,7 @@ static int at_send_command_full_nolock(const char *command,
             err =
                 pthread_cond_timeout_np(&ac->commandcond,
                                         &ac->commandmutex, timeoutMsec);
-#endif                          /*USE_NP */
+#endif /*USE_NP */
         } else
             err = pthread_cond_wait(&ac->commandcond, &ac->commandmutex);
 
@@ -1063,8 +1083,8 @@ static int at_send_command_full_nolock(const char *command,
     }
 
     ac->response = NULL;
-
     err = 0;
+
 error:
     clearPendingCommand();
 
@@ -1088,8 +1108,6 @@ static int at_send_command_full(const char *command, ATCommandType type,
 
     struct atcontext *ac = getAtContext();
 
-    LOGE("--- %s", command);
-
     if (0 != pthread_equal(ac->tid_reader, pthread_self()))
         /* Cannot be called from reader thread. */
         return AT_ERROR_INVALID_THREAD;
@@ -1117,9 +1135,10 @@ void at_send_escape(void)
     int written;
 
     do
-        written = write(ac->fd, "\033", 1);
-
+        written = write(ac->fd, " ", 1);
     while ((written < 0 && errno == EINTR) || (written == 0));
+
+    LOGI("%s() sent space on at channel to abort command", __func__);
 }
 
 /**
@@ -1354,9 +1373,12 @@ int at_handshake()
     }
 
     if (err == 0) {
-        /* Pause for a bit to let the input buffer drain any unmatched OK's
-           (they will appear as extraneous unsolicited responses). */
-        LOGD("pausing..");
+        /*
+         * Pause for a bit to let the input buffer drain any unmatched OK's
+         * (they will appear as extraneous unsolicited responses).
+         */
+        LOGD("%s() pausing %d ms to drain unmatched OK's...",
+             __func__, HANDSHAKE_TIMEOUT_MSEC);
         sleepMsec(HANDSHAKE_TIMEOUT_MSEC);
     }
 
diff --git a/atchannel.h b/atchannel.h
index 9102ab7..56fffa0 100644
--- a/atchannel.h
+++ b/atchannel.h
@@ -43,12 +43,12 @@ extern "C" {
 #define AT_ERROR_COMMAND_PENDING -2
 #define AT_ERROR_CHANNEL_CLOSED -3
 #define AT_ERROR_TIMEOUT -4
-#define AT_ERROR_INVALID_THREAD -5  /* AT commands may not be issued from
-                                       reader thread (or unsolicited response
-                                       callback */
-#define AT_ERROR_INVALID_RESPONSE -6    /* Eg an at_send_command_singleline that
-                                           did not get back an intermediate
-                                           response */
+#define AT_ERROR_INVALID_THREAD   -5    /* AT commands may not be issued from
+                                         * reader thread (or unsolicited
+                                         * response callback) */
+#define AT_ERROR_INVALID_RESPONSE -6    /* E.g. an at_send_command_singleline
+                                         * that did not get back an intermediate
+                                         * response */
 
 typedef enum {
     CME_PHONE_FAILURE = 0,
diff --git a/u300-ril-manager.c b/u300-ril-manager.c
index 69b3fbc..c4ef018 100644
--- a/u300-ril-manager.c
+++ b/u300-ril-manager.c
@@ -40,10 +40,39 @@
 #define LOG_TAG "RILV"
 #include <utils/Log.h>
 
-/*
- * Begin section flagged under
- * EXTERNAL_MODEM_CONTROL_MODULE_DISABLED
- */
+typedef struct managerArgs {
+    int channels;
+    RILRequestGroup *parsedGroups[RIL_MAX_NR_OF_CHANNELS];
+    char *type;
+    char *args[RIL_MAX_NR_OF_CHANNELS];
+    char *xarg;
+} managerArgs;
+
+static struct managerArgs mgrArgs;
+static bool dbusIsHere = false;
+
+pthread_t s_tid_managerRunner;
+
+static void releaseCommandThreads(void)
+{
+    pthread_mutex_lock(&ril_manager_wait_mutex);
+    managerRelease = true;
+    pthread_cond_broadcast(&ril_manager_wait_cond);
+    LOGD("%s():Released command execution queue thread(s)", __func__);
+    pthread_mutex_unlock(&ril_manager_wait_mutex);
+}
+
+static void haltCommandThreads(void)
+{
+    pthread_mutex_lock(&ril_manager_wait_mutex);
+    managerRelease = false;
+    LOGD("%s():Halted command execution queue thread(s)", __func__);
+    pthread_mutex_unlock(&ril_manager_wait_mutex);
+}
+
+/******************************************************************************
+ * Start section that includes DBUS communication with MID module             *
+ ******************************************************************************/
 #ifndef EXTERNAL_MODEM_CONTROL_MODULE_DISABLED
 #include <dbus/dbus.h>
 #include <poll.h>
@@ -68,18 +97,9 @@ static pthread_mutex_t s_dbus_watch_mutex = PTHREAD_MUTEX_INITIALIZER;
 pthread_t s_tid_dbusRunner;
 #define UNUSED(expr) do { (void)(expr); } while (0)
 
-static void releaseATthreads(void)
-{
-    pthread_mutex_lock(&s_dbus_mid_wait_mutex);
-    dbusRelease = true;
-    pthread_cond_broadcast(&s_dbus_mid_wait_cond);
-    LOGD("%s():Released AT threads", __func__);
-    pthread_mutex_unlock(&s_dbus_mid_wait_mutex);
-}
-
 /* MID signal message handler */
 static DBusHandlerResult midSignalHandler(DBusConnection *dbcon, DBusMessage
-                                            *msg, void *data)
+                                          *msg, void *data)
 {
     DBusMessageIter args;
     DBusPendingCall* pending;
@@ -88,6 +108,13 @@ static DBusHandlerResult midSignalHandler(DBusConnection *dbcon, DBusMessage
     UNUSED(data);
     const char* signame = return_buf;
 
+    /*
+     * managerRelease is here used a state indication of QueueRunners.
+     * TRUE   = Indicates queuethreads are running normally, any event is can be
+     *          considered an new state indication.
+     * FASLSE = Indicates queuethreads are already aware of the "restart" and
+     *          can be considered in the restarting state.
+     */
     if (dbus_message_is_signal(msg, "com.stericsson.mid.Modem",
                                "StateChange")) {
         if (!dbus_message_iter_init(msg, &args)) {
@@ -98,14 +125,36 @@ static DBusHandlerResult midSignalHandler(DBusConnection *dbcon, DBusMessage
             dbus_message_iter_get_basic(&args, &signame);
             LOGD("%s(): Got Signal with value %s", __func__, signame);
 
-            if (strncmp(signame, "on", 2) == 0)
-                releaseATthreads();
-            else if (strncmp(signame, "off", 3) == 0) {
-                LOGD("%s(): Modem is off. Cleans up RIL threads and prepare "
-                     "to go back to initial state.\n", __func__);
-                onATReaderClosed();
+            if (strncmp(signame, "on", 2) == 0) {
+                if (managerRelease) {
+                    LOGD("%s() Received unexpected \"on\" in already running "
+                         "state. Ignored...", __func__);
+                } else {
+                    LOGD("%s() Received \"on\". Releasing queue threads..."
+                         , __func__);
+                    releaseCommandThreads();
+                }
+            } else if (strncmp(signame, "prepare_off", 11) == 0) {
+                if (managerRelease) {
+                    LOGD("%s(): Received \"prepare_off\". Unhandled..."
+                         , __func__);
+                    /* TODO: add functionality of early modemcleanup here */
+                } else {
+                    LOGD("%s(): Received \"prepare_off\". Queue threads (already) "
+                         "stopped waiting for \"on\"...", __func__);
+                }
+            } else if (strncmp(signame, "off", 3) == 0) {
+                if (managerRelease) {
+                    LOGD("%s(): Received \"off\". Signal queue threads and "
+                         "prepare to go back to initial state...", __func__);
+                    haltCommandThreads();
+                    signalCloseQueues();
+                } else {
+                    LOGD("%s(): Received \"off\". Queue threads (already) "
+                         "stopped waiting for \"on\"...", __func__);
+                }
             } else
-                LOGD("%s(): message \"%s\" ignored.\n", __func__, signame);
+                LOGD("%s(): message \"%s\" ignored.", __func__, signame);
 
             return DBUS_HANDLER_RESULT_HANDLED;
         }
@@ -248,6 +297,7 @@ exit:
 
 static void notifyForEvent(int index, short event)  {
     unsigned int flags = 0;
+
     if (event & POLLIN)
         flags |= DBUS_WATCH_READABLE;
     if (event & POLLOUT)
@@ -326,7 +376,7 @@ finally1:
     return res;
 }
 
-static int requestStateWithResponse(DBusConnection *dbcon,
+static int requestMIDWithResponse(DBusConnection *dbcon,
                                     char *requestMethod,
                                     char *response)
 {
@@ -334,45 +384,41 @@ static int requestStateWithResponse(DBusConnection *dbcon,
     char *pTemp = temp;
     int ret = 0;
 
-    DBusPendingCall *pending;
-    DBusMessage *dbmsg;
+    DBusPendingCall *pending = NULL;
+    DBusMessage *dbmsg = NULL;
     DBusMessageIter args;
 
     dbmsg = dbus_message_new_method_call(DBUS_CONNECTION_NAME,
                                          DBUS_OBJECT_PATH,
                                          DBUS_OBJECT_INTERFACE,
                                          requestMethod);
-    if (!dbmsg) {
+    if (dbmsg == NULL) {
         LOGE("%s(): Failed to create a method call", __func__);
         goto error;
     }
 
     if (!dbus_connection_send_with_reply(dbcon, dbmsg, &pending, -1)) {
         LOGE("%s(): Failed to send method call", __func__);
-        dbus_message_unref(dbmsg);
         goto error;
     }
 
-    if (!pending) {
+    if (pending == NULL) {
         LOGE("%s(): Failed to send method call, connection closed", __func__);
-        dbus_message_unref(dbmsg);
         goto error;
     }
 
     dbus_connection_flush(dbcon);
     dbus_message_unref(dbmsg);
-
     dbus_pending_call_block(pending);
 
     dbmsg = dbus_pending_call_steal_reply(pending);
-    if (!dbmsg) {
+    dbus_pending_call_unref(pending);
+
+    if (dbmsg == NULL) {
         LOGE("%s(): Error on the received message.", __func__);
-        dbus_pending_call_unref(pending);
         goto error;
     }
 
-    dbus_pending_call_unref(pending);
-
     if (!dbus_message_iter_init(dbmsg, &args)) {
         LOGE(" %s(): Received message has no arguments!", __func__);
         goto error;
@@ -382,7 +428,7 @@ static int requestStateWithResponse(DBusConnection *dbcon,
     } else {
         dbus_message_iter_get_basic(&args, &pTemp);
         strncpy(response, pTemp, BUF_MID_RESPONSE_SIZE);
-        LOGD("%s(): Got message, pTemp:\"%s\", response: \"%s\"\n",
+        LOGD("%s(): Got message, pTemp:\"%s\", response: \"%s\"",
              __func__, pTemp, response);
     }
 
@@ -394,13 +440,27 @@ exit:
     return ret;
 }
 
+static bool queryModemOn(DBusConnection *dbcon) {
+    char responseArray[BUF_MID_RESPONSE_SIZE];
+    char *pResponse = responseArray;
+
+    if (requestMIDWithResponse(dbcon, "GetState", pResponse) != 0) {
+        LOGE("%s(): Failed to query state of MID.", __func__);
+    } else {
+        if (strncmp(pResponse, "on", 2) == 0) {
+            return true;
+        }
+        else
+            LOGD("%s(): %s returned and ignored.", __func__, pResponse);
+    }
+    return false;
+}
+
 static void *dbusAndThreadRunner(void *param)
 {
     DBusConnection *dbcon = (DBusConnection *)param;
     DBusError err;
     int ret;
-    char responseArray[BUF_MID_RESPONSE_SIZE];
-    char *pResponse = responseArray;
 
     int i;
 
@@ -431,15 +491,8 @@ static void *dbusAndThreadRunner(void *param)
         goto error;
     }
 
-    if (requestStateWithResponse(dbcon, "GetState", pResponse) != 0) {
-        LOGE("%s(): Failed to query state of MID.", __func__);
-        goto error;
-    } else {
-        if (strncmp(pResponse, "on", 2) == 0)
-            releaseATthreads();
-        else
-            LOGD("%s(): %s returned and ignored.\n", __func__, pResponse);
-    }
+    if (queryModemOn(dbcon))
+        releaseCommandThreads();
 
     for (;;) {
         ret = poll(dbus_used_pollfds_tab, DBUS_MAX_WATCHERS, -1);
@@ -451,7 +504,7 @@ static void *dbusAndThreadRunner(void *param)
                 }
             }
         }
-     }
+    }
 
     return 0;
 
@@ -471,9 +524,113 @@ error:
     return NULL;
 }
 #endif /* EXTERNAL_MODEM_CONTROL_MODULE_DISABLED */
+/******************************************************************************
+ * End section that includes DBUS communication with MID module               *
+ ******************************************************************************/
 
 pthread_t s_tid_queueRunner[RIL_MAX_NR_OF_CHANNELS];
 
+static void *managerRunner(void *param)
+{
+    int activeThreads;
+    int ret;
+    int i;
+    pthread_attr_t attr;
+    struct queueArgs *queueArgs[RIL_MAX_NR_OF_CHANNELS] = { NULL, NULL };
+
+    for(;;) {
+        activeThreads = 0;
+        ret = 0;
+
+        for (i = 0; i < mgrArgs.channels; i++) {
+            int err;
+            queueArgs[i] = malloc(sizeof(struct queueArgs));
+            memset(queueArgs[i], 0, sizeof(struct queueArgs));
+
+            queueArgs[i]->channels = mgrArgs.channels;
+            queueArgs[i]->group = mgrArgs.parsedGroups[i];
+            queueArgs[i]->type = mgrArgs.type;
+            queueArgs[i]->index = i;
+            queueArgs[i]->arg = mgrArgs.args[i];
+            queueArgs[i]->xarg = mgrArgs.xarg;
+
+            if ((err = pthread_attr_init(&attr)) != 0)
+                LOGE("%s() failed to initialize pthread attribute: %s",
+                    __func__, strerror(err));
+
+            if ((err = pthread_attr_setdetachstate(&attr,
+                    PTHREAD_CREATE_DETACHED)) != 0)
+                LOGE("%s() failed to set the "
+                     "PTHREAD_CREATE_DETACHED attribute: %s",
+                     __func__, strerror(err));
+
+            if ((err = pthread_create(&s_tid_queueRunner[i], &attr,
+                 queueRunner, queueArgs[i])) != 0) {
+                LOGE("%s() failed to create queue runner thread: %s",
+                    __func__, strerror(err));
+                free(queueArgs[i]);
+            } else {
+                activeThreads++;
+            }
+            /* memory is freed within the queuerunner assigned the args */
+            queueArgs[i] = NULL;
+        }
+
+        /* Since dbus is not enabled we start threads instantly */
+        if (!dbusIsHere)
+            releaseCommandThreads();
+
+        ret = pthread_mutex_lock(&ril_manager_queue_exit_mutex);
+        if (ret != 0)
+            LOGE("%s(): Failed to get mutex lock. err: %s", __func__,
+                 strerror(-ret));
+
+        while (activeThreads > 0) {
+            ret = pthread_cond_wait(&ril_manager_queue_exit_cond,
+                                    &ril_manager_queue_exit_mutex);
+            if (ret != 0)
+                LOGE("%s(): pthread_cond_wait Failed. err: %s", __func__,
+                    strerror(-ret));
+            else {
+                activeThreads--;
+            }
+        }
+        ret = pthread_mutex_unlock(&ril_manager_queue_exit_mutex);
+        if (ret != 0)
+            LOGE("%s(): Failed to unlock mutex. err: %s", __func__,
+                 strerror(-ret));
+
+#ifndef EXTERNAL_MODEM_CONTROL_MODULE_DISABLED
+        DBusConnection *dbcon = (DBusConnection *)param;
+        char responseArray[BUF_MID_RESPONSE_SIZE];
+        char *pResponse = responseArray;
+
+        /* Signal MID to restart Modem */
+        if (requestMIDWithResponse(dbcon, "Reboot", pResponse) != 0) {
+            LOGE("%s(): Failed to reboot modem."
+                    "Restarting threads anyway.", __func__);
+        } else {
+            if (strncmp(pResponse, "OK", 2) == 0) {
+                LOGI("%s(): %s returned. Modem restarting!",
+                        __func__, pResponse);
+                /* Instruct QueueRunners to wait for 'on' from MID */
+                haltCommandThreads();
+            }
+            else
+                /*
+                 * In the event we are not allowed to do a modem reboot we have
+                 * little to do but try a direct restart of the queuerunners.
+                 * AT channels will anyway be re-opened.
+                 */
+                LOGD("%s(): %s returned from MID on \"reboot\" request. "
+                     "Continuing... (letting queue runners execute immediatly)",
+                     __func__, pResponse);
+        }
+#endif /* EXTERNAL_MODEM_CONTROL_MODULE_DISABLED */
+    }
+    return NULL;
+}
+
 #ifndef CAIF_SOCKET_SUPPORT_DISABLED
 static bool createNetworkInterface(const char *ifname, int connection_id)
 {
@@ -528,16 +685,15 @@ const RIL_RadioFunctions *RIL_Init(const struct RIL_Env *env, int argc,
 {
     int opt;
     int i;
-    int channels = 2;
-    char *type = NULL;
+    int err;
     char *groups = NULL;
-    char *args[RIL_MAX_NR_OF_CHANNELS] = { NULL, NULL };
-    char *xarg = NULL;
-    RILRequestGroup *parsedGroups[RIL_MAX_NR_OF_CHANNELS] =
-        { NULL, NULL };
-    struct queueArgs *queueArgs[RIL_MAX_NR_OF_CHANNELS] = { NULL, NULL };
     pthread_attr_t attr;
 
+    /* Initialize manager arguments */
+    mgrArgs.channels = 2;
+    mgrArgs.type = NULL;
+    mgrArgs.xarg = NULL;
+
     LOGI("**************************************************\n"
         "Starting ST-Ericsson RIL...\n"
         "**************************************************");
@@ -548,8 +704,8 @@ const RIL_RadioFunctions *RIL_Init(const struct RIL_Env *env, int argc,
     while (-1 != (opt = getopt(argc, argv, "c:n:g:p:s:x:i:"))) {
         switch (opt) {
         case 'c':
-            type = optarg;
-            LOGI("Using channel type %s.", type);
+            mgrArgs.type = optarg;
+            LOGI("Using channel type %s.", mgrArgs.type);
             break;
 
         case 'n':
@@ -558,24 +714,24 @@ const RIL_RadioFunctions *RIL_Init(const struct RIL_Env *env, int argc,
 
         case 'g':
             groups = optarg;
-            channels = parseGroups(groups, parsedGroups);
+            mgrArgs.channels = parseGroups(groups, mgrArgs.parsedGroups);
             LOGI("RIL command group(s) "
                 "(DEFAULT and AUXILIARY may be omitted): %s", groups);
             break;
 
         case 'p':
-            args[0] = optarg;
-            LOGI("Primary AT channel: %s\n", args[0]);
+            mgrArgs.args[0] = optarg;
+            LOGI("Primary AT channel: %s", mgrArgs.args[0]);
             break;
 
         case 's':
-            args[1] = optarg;
-            LOGI("Secondary AT channel: %s\n", args[1]);
+            mgrArgs.args[1] = optarg;
+            LOGI("Secondary AT channel: %s", mgrArgs.args[1]);
             break;
 
         case 'x':
-            xarg = optarg;
-            LOGI("Extra argument %s.", xarg);
+            mgrArgs.xarg = optarg;
+            LOGI("Extra argument %s.", mgrArgs.xarg);
             break;
 
         case 'i':
@@ -594,7 +750,7 @@ const RIL_RadioFunctions *RIL_Init(const struct RIL_Env *env, int argc,
         LOGI("%s(): RIL command groups was not supplied. Using default "
             "configuration DEFAULT and AUXILIARY groups (2 AT channels).",
             __func__);
-        channels = parseGroups("", parsedGroups);
+        mgrArgs.channels = parseGroups("", mgrArgs.parsedGroups);
     }
 
     if (ril_iface == NULL || strcmp(ril_iface, "") == 0) {
@@ -604,13 +760,13 @@ const RIL_RadioFunctions *RIL_Init(const struct RIL_Env *env, int argc,
     }
 
 #ifndef CAIF_SOCKET_SUPPORT_DISABLED
-    if (type == NULL || strncasecmp(type, "", 1) == 0) {
+    if (mgrArgs.type == NULL || strncasecmp(mgrArgs.type, "", 1) == 0) {
         LOGW("%s: AT/Data channel type was not supplied."
              " Falling back to CAIF!", __func__);
-        type = "CAIF";
+        mgrArgs.type = "CAIF";
     }
 
-    if (strncasecmp(type, "CAIF", 4) == 0) {
+    if (strncasecmp(mgrArgs.type, "CAIF", 4) == 0) {
         for (i = 0; i < RIL_MAX_NUMBER_OF_PDP_CONTEXTS; i++) {
             char ifaceName[MAX_IFNAME_LEN];
             snprintf(ifaceName, MAX_IFNAME_LEN, "%s%d", ril_iface, i);
@@ -619,26 +775,24 @@ const RIL_RadioFunctions *RIL_Init(const struct RIL_Env *env, int argc,
         }
     }
 #else
-    if (type == NULL || strcmp(type, "") == 0) {
-        LOGE("%s(): AT/Data channel type was not supplied!\n", __func__);
+    if (mgrArgs.type == NULL || strcmp(mgrArgs.type, "") == 0) {
+        LOGE("%s(): AT/Data channel type was not supplied!", __func__);
         goto error;
     }
 #endif
 
 #ifndef EXTERNAL_MODEM_CONTROL_MODULE_DISABLED
-    int err;
     DBusError dbusErr;
     DBusConnection *dbcon;
     dbus_error_init(&dbusErr);
 
     /* Connect to system dbus */
     dbcon = dbus_bus_get(DBUS_BUS_SYSTEM, &dbusErr);
-    dbusRelease = false;
+    haltCommandThreads();
     if (dbcon == NULL || dbus_error_is_set(&dbusErr)) {
         LOGW("[DBUS]: DBUS interface unavailable. No communication with MID.");
-        dbusIsHere = false;
     } else {
-        LOGI("[DBUS]: Connected to system dbus.\n");
+        LOGI("[DBUS]: Connected to system dbus.");
         dbusIsHere = true;
         err = pthread_attr_init(&attr);
         if (err != 0)
@@ -660,32 +814,28 @@ const RIL_RadioFunctions *RIL_Init(const struct RIL_Env *env, int argc,
     }
 #endif /* EXTERNAL_MODEM_CONTROL_MODULE_DISABLED */
 
-    for (i = 0; i < channels; i++) {
-        int err;
-        queueArgs[i] = malloc(sizeof(struct queueArgs));
-        memset(queueArgs[i], 0, sizeof(struct queueArgs));
-
-        queueArgs[i]->channels = channels;
-        queueArgs[i]->group = parsedGroups[i];
-        queueArgs[i]->type = type;
-        queueArgs[i]->index = i;
-        queueArgs[i]->arg = args[i];
-        queueArgs[i]->xarg = xarg;
-
-        if ((err = pthread_attr_init(&attr)) != 0)
-            LOGE("%s() failed to initialize pthread attribute: %s",
-                __func__, strerror(err));
-
-        if ((err = pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED))
-             != 0)
-            LOGE("%s() failed to set the PTHREAD_CREATE_DETACHED attribute: %s",
-                __func__, strerror(err));
-
-        if ((err = pthread_create(&s_tid_queueRunner[i], &attr,
-             queueRunner, queueArgs[i])) != 0)
-            LOGE("%s() failed to create queue runner thread: %s",
-                __func__, strerror(err));
-    }
+    /* Start Manager thread. */
+    err = pthread_attr_init(&attr);
+    if (err != 0)
+        LOGW("%s(): Failed to initialize RIL Manager pthread attribute: %s",
+             __func__, strerror(err));
+
+    err = pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
+    if (err != 0)
+        LOGW("%s(): Failed to set the RIL Manager PTHREAD_CREATE_DETACHED "
+             "attribute: %s", __func__, strerror(err));
+
+#ifndef EXTERNAL_MODEM_CONTROL_MODULE_DISABLED
+    err = pthread_create(&s_tid_managerRunner, &attr,
+                         managerRunner, (void *)dbcon);
+#else
+    err = pthread_create(&s_tid_managerRunner, &attr,
+                         managerRunner, NULL);
+#endif /* EXTERNAL_MODEM_CONTROL_MODULE_DISABLED */
+
+    if (err != 0)
+        LOGE("%s(): Failed to create RIL manager runner thread: %s", __func__,
+             strerror(err));
 
     return &g_callbacks;
 
diff --git a/u300-ril.c b/u300-ril.c
index 8eea5b0..61ac2f6 100644
--- a/u300-ril.c
+++ b/u300-ril.c
@@ -73,13 +73,13 @@
      ? (a).tv_nsec op(b).tv_nsec \
      : (a).tv_sec op(b).tv_sec)
 
-#ifndef EXTERNAL_MODEM_CONTROL_MODULE_DISABLED
-pthread_mutex_t s_dbus_mid_wait_mutex = PTHREAD_MUTEX_INITIALIZER;
-pthread_cond_t s_dbus_mid_wait_cond = PTHREAD_COND_INITIALIZER;
 
-bool dbusIsHere = false;
-bool dbusRelease = false;
-#endif /* EXTERNAL_MODEM_CONTROL_MODULE_DISABLED */
+bool managerRelease = false;
+pthread_mutex_t ril_manager_wait_mutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_cond_t ril_manager_wait_cond = PTHREAD_COND_INITIALIZER;
+
+pthread_mutex_t ril_manager_queue_exit_mutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_cond_t ril_manager_queue_exit_cond = PTHREAD_COND_INITIALIZER;
 
 /*** Declarations ***/
 static void onRequest(int request, void *data, size_t datalen,
@@ -88,7 +88,7 @@ static int supports(int requestCode);
 static void onCancel(RIL_Token t);
 static const char *getVersion(void);
 static int isRadioOn(void);
-static void signalCloseQueues(void);
+static void signalManager(void);
 extern const char *requestToString(int request);
 
 static RIL_RadioState onStateRequest(void);
@@ -484,7 +484,7 @@ void releaseScreenStateLock(void)
             __func__,  strerror(err));
 }
 
-static struct RequestQueue *getRequestQueue(int request)
+static RequestQueue *getRequestQueue(int request)
 {
     size_t i, j;
 
@@ -517,31 +517,52 @@ static struct RequestQueue *getRequestQueue(int request)
     return RILRequestGroups[CMD_QUEUE_AUXILIARY].requestQueue;
 }
 
-static void processRequest(int request, void *data, size_t datalen,
-                           RIL_Token t)
-{
-    LOGI("processRequest: %s", requestToString(request));
+/*** Callback methods from the RIL library to us ***/
+static const RIL_CardStatus staticSimStatus = {
+    .card_state = RIL_CARDSTATE_ABSENT,
+    .universal_pin_state = RIL_PINSTATE_UNKNOWN,
+    .gsm_umts_subscription_app_index = 0,
+    .cdma_subscription_app_index = 0,
+    .num_applications = 0
+};
 
+static bool requestStateFilter(int request, RIL_Token t)
+{
     /*
-     * These commands won't accept RADIO_NOT_AVAILABLE, so we just return
-     * GENERIC_FAILURE if we're not in SIM_STATE_READY.
+     * These commands will not accept RADIO_NOT_AVAILABLE and cannot be executed
+     * before we are in SIM_STATE_READY so we just return GENERIC_FAILURE if
+     * not in SIM_STATE_READY.
      */
     if (s_state != RADIO_STATE_SIM_READY
         && (request == RIL_REQUEST_WRITE_SMS_TO_SIM ||
-            request == RIL_REQUEST_DELETE_SMS_ON_SIM ||
-            request == RIL_REQUEST_SCREEN_STATE)) {
+            request == RIL_REQUEST_DELETE_SMS_ON_SIM)) {
         RIL_onRequestComplete(t, RIL_E_GENERIC_FAILURE, NULL, 0);
-        return;
+        return true;
     }
 
-    /*
-     * Ignore all requests except RIL_REQUEST_GET_SIM_STATUS
-     * when RADIO_STATE_UNAVAILABLE.
-     */
-    if (s_state == RADIO_STATE_UNAVAILABLE
-        && request != RIL_REQUEST_GET_SIM_STATUS) {
-        RIL_onRequestComplete(t, RIL_E_RADIO_NOT_AVAILABLE, NULL, 0);
-        return;
+    /* Ignore all requsts while is radio_state_unavailable */
+    if (s_state == RADIO_STATE_UNAVAILABLE) {
+        /*
+         * The following command(s) must never fail. Return static state for
+         * these command(s) while in RADIO_STATE_UNAVAILABLE.
+         */
+        if (request == RIL_REQUEST_GET_SIM_STATUS) {
+            RIL_onRequestComplete(t, RIL_REQUEST_GET_SIM_STATUS,
+                                  (char *) &staticSimStatus,
+                                  sizeof(staticSimStatus));
+        }
+        /*
+         * The following command must never fail. Return static state for this
+         * command while in RADIO_STATE_UNAVAILABLE.
+         */
+        else if (request == RIL_REQUEST_SCREEN_STATE) {
+            RIL_onRequestComplete(t, RIL_E_GENERIC_FAILURE, NULL, 0);
+        }
+        /* Ignore all other requests when RADIO_STATE_UNAVAILABLE */
+        else {
+            RIL_onRequestComplete(t, RIL_E_RADIO_NOT_AVAILABLE, NULL, 0);
+        }
+        return true;
     }
 
     /*
@@ -562,9 +583,10 @@ static void processRequest(int request, void *data, size_t datalen,
              request == RIL_REQUEST_GET_IMEISV ||
              request == RIL_REQUEST_GET_IMEI ||
              request == RIL_REQUEST_DEVICE_IDENTITY ||
-             request == RIL_REQUEST_BASEBAND_VERSION)) {
+             request == RIL_REQUEST_BASEBAND_VERSION ||
+             request == RIL_REQUEST_SCREEN_STATE)) {
         RIL_onRequestComplete(t, RIL_E_RADIO_NOT_AVAILABLE, NULL, 0);
-        return;
+        return true;
     }
 
     /*
@@ -579,9 +601,10 @@ static void processRequest(int request, void *data, size_t datalen,
              request == RIL_REQUEST_GET_IMEISV ||
              request == RIL_REQUEST_GET_IMEI ||
              request == RIL_REQUEST_DEVICE_IDENTITY ||
-             request == RIL_REQUEST_BASEBAND_VERSION)) {
+             request == RIL_REQUEST_BASEBAND_VERSION ||
+             request == RIL_REQUEST_SCREEN_STATE)) {
         RIL_onRequestComplete(t, RIL_E_GENERIC_FAILURE, NULL, 0);
-        return;
+        return true;
     }
 
     /*
@@ -609,11 +632,23 @@ static void processRequest(int request, void *data, size_t datalen,
              request == RIL_REQUEST_DTMF ||
              request == RIL_REQUEST_DTMF_START ||
              request == RIL_REQUEST_DTMF_STOP ||
-             request == RIL_REQUEST_LAST_CALL_FAIL_CAUSE)) {
+             request == RIL_REQUEST_LAST_CALL_FAIL_CAUSE ||
+             request == RIL_REQUEST_SCREEN_STATE)) {
         RIL_onRequestComplete(t, RIL_E_GENERIC_FAILURE, NULL, 0);
-        return;
+        return true;
     }
 
+    return false;
+}
+
+static void processRequest(int request, void *data, size_t datalen,
+                           RIL_Token t)
+{
+    LOGI("processRequest: %s", requestToString(request));
+
+    if (requestStateFilter(request, t))
+        goto finally;
+
     switch (request) {
 
     /* Basic Voice Call */
@@ -897,9 +932,10 @@ static void processRequest(int request, void *data, size_t datalen,
         RIL_onRequestComplete(t, RIL_E_REQUEST_NOT_SUPPORTED, NULL, 0);
         break;
     }
-}
 
-/*** Callback methods from the RIL library to us ***/
+finally:
+    return;
+}
 
 /**
  * Call from RIL to us to make a RIL_REQUEST.
@@ -912,6 +948,12 @@ static void onRequest(int request, void *data, size_t datalen, RIL_Token t)
     RequestQueue *q = &s_requestQueueDefault;
     int err;
 
+    /* In radio state unavailable no requests are to enter the queues */
+    if (s_state == RADIO_STATE_UNAVAILABLE) {
+        (void)requestStateFilter(request, t);
+        goto finally;
+    }
+
     q = getRequestQueue(request);
 
     r = calloc(1, sizeof(RILRequest));
@@ -948,6 +990,7 @@ static void onRequest(int request, void *data, size_t datalen, RIL_Token t)
         LOGE("%s() failed to release queue mutex: %s!",
             __func__, strerror(err));
 
+finally:
     return;
 }
 
@@ -1463,11 +1506,12 @@ static void onUnsolicited(const char *s, const char *sms_pdu)
         onOemUnsolHook(s);
 }
 
-static void signalCloseQueues(void)
+void signalCloseQueues(void)
 {
     unsigned int i;
+    setRadioState(RADIO_STATE_UNAVAILABLE);
 
-    for (i = 0; i < (sizeof(s_requestQueues) / sizeof(RequestQueue *)); i++) {
+    for (i = 0; i < NUM_ELEMS(s_requestQueues); i++) {
         int err;
         RequestQueue *q = s_requestQueues[i];
         if ((err = pthread_mutex_lock(&q->queueMutex)) != 0)
@@ -1485,28 +1529,48 @@ static void signalCloseQueues(void)
     }
 }
 
-/* Called on command or reader thread. */
-void onATReaderClosed()
+static void signalManager(void)
 {
-    LOGI("AT channel closed");
+    int err;
 
-    setRadioState(RADIO_STATE_UNAVAILABLE);
+    if ((err = pthread_mutex_lock(&ril_manager_queue_exit_mutex)) != 0)
+        LOG_FATAL("%s() failed to take RIL Manager AT fail mutex: %s",
+                  __func__, strerror(err));
+
+    if ((err = pthread_cond_signal(&ril_manager_queue_exit_cond)) != 0)
+        LOGW("%s() failed to signal RIL Manager: %s",
+             __func__, strerror(err));
+
+    if ((err = pthread_mutex_unlock(&ril_manager_queue_exit_mutex)) != 0)
+        LOG_FATAL("%s() failed to take RIL Manager AT Fail mutex: %s",
+                  __func__, strerror(err));
+}
+
+/* Called on command or reader thread. */
+static void onATReaderClosed()
+{
+    LOGI("AT channel closed, closing queues!");
     signalCloseQueues();
 }
 
-/* Called on command thread. */
+/* Callback from AT Channel. Called on command thread. */
 static void onATTimeout()
 {
-    LOGI("AT channel timeout; restarting..\n");
-    /* Last resort, throw escape on the line, close the channel
-     * and hope for the best.
-     */
+    LOGI("AT channel timeout. Trying to abort command and check channel.");
+
+    /* Throw escape on the channel and check sanity with handshake */
     at_send_escape();
 
-    setRadioState(RADIO_STATE_UNAVAILABLE);
-    signalCloseQueues();
+    if (at_handshake() >= 0) {
+        LOGI("AT channel sanity check successful. Continuing...");
+    }
+    else {
+        LOG_FATAL("%s() Channel sanity check failed!", __func__);
+        signalCloseQueues();
 
-    /* TODO We may cause a radio reset here. */
+        /* Prevent further command execution */
+        at_close();
+    }
 }
 
 int parseGroups(char* groups, RILRequestGroup **parsedGroups)
@@ -1549,242 +1613,276 @@ void *queueRunner(void *param)
     struct queueArgs *queueArgs = (struct queueArgs *) param;
     struct RequestQueue *q = NULL;
 
-    for (;;) {
-#ifndef EXTERNAL_MODEM_CONTROL_MODULE_DISABLED
-        if (dbusIsHere) {
-            LOGI("[DBUS]: queueRunner %d waiting for dbus", queueArgs->index);
-            ret = pthread_mutex_lock(&s_dbus_mid_wait_mutex);
-            if (ret != 0)
-                LOGE("%s(): Failed to get mutex lock. err: %s", __func__,
-                     strerror(-ret));
-
-            while (!dbusRelease) {
-                ret = pthread_cond_wait(&s_dbus_mid_wait_cond,
-                                        &s_dbus_mid_wait_mutex);
-                if (ret != 0)
-                    LOGE("%s(): pthread_cond_wait Failed. err: %s", __func__,
-                        strerror(-ret));
-            }
+    LOGI("%s() thread index %d waiting for Manager release flag", __func__,
+         queueArgs->index);
 
-            ret = pthread_mutex_unlock(&s_dbus_mid_wait_mutex);
-            if (ret != 0)
-                LOGE("%s(): Failed to unlock mutex. err: %s", __func__,
-                     strerror(-ret));
-        }
-#endif /* EXTERNAL_MODEM_CONTROL_MODULE_DISABLED */
+    ret = pthread_mutex_lock(&ril_manager_wait_mutex);
+    if (ret != 0)
+        LOGE("%s(): Failed to get mutex lock. err: %s", __func__,
+                strerror(-ret));
 
-        LOGI("queueRunner starting!");
-        fd = -1;
-        while (fd < 0) {
-            if (queueArgs->type == NULL) {
-                LOGE("%s(): Unsupported channel type. Bailing out!", __func__);
-                free(queueArgs);
-                return NULL;
-            }
+    while (!managerRelease) {
+        ret = pthread_cond_wait(&ril_manager_wait_cond,
+                                &ril_manager_wait_mutex);
+        if (ret != 0)
+            LOGE("%s(): pthread_cond_wait Failed. err: %s", __func__,
+                strerror(-ret));
+    }
+
+    ret = pthread_mutex_unlock(&ril_manager_wait_mutex);
+    if (ret != 0)
+        LOGE("%s(): Failed to unlock mutex. err: %s", __func__,
+                strerror(-ret));
+
+    LOGI("%s() index %d setting up AT socket channel", __func__,
+         queueArgs->index);
+    fd = -1;
+    while (fd < 0) {
+        if (queueArgs->type == NULL) {
+            LOGE("%s(): Unsupported channel type. Bailing out!", __func__);
+            free(queueArgs);
+            return NULL;
+        }
 
-            if (!strncmp(queueArgs->type, "CAIF", 4)) {
+        if (!strncmp(queueArgs->type, "CAIF", 4)) {
 #ifndef CAIF_SOCKET_SUPPORT_DISABLED
-                int cf_prio = CAIF_PRIO_HIGH;
+            int cf_prio = CAIF_PRIO_HIGH;
 
-                struct sockaddr_caif addr = {
-                    .family = AF_CAIF,
-                    .u.at.type = CAIF_ATTYPE_PLAIN
-                };
+            struct sockaddr_caif addr = {
+                .family = AF_CAIF,
+                .u.at.type = CAIF_ATTYPE_PLAIN
+            };
 
-                fd = socket(AF_CAIF, SOCK_SEQPACKET, CAIFPROTO_AT);
-                if (fd < 0) {
-                    LOGE("%s(): failed to create socket. errno: %d(%s).",
-                        __func__, errno, strerror(-errno));
-                }
+            fd = socket(AF_CAIF, SOCK_SEQPACKET, CAIFPROTO_AT);
+            if (fd < 0) {
+                LOGE("%s(): failed to create socket. errno: %d(%s).",
+                    __func__, errno, strerror(-errno));
+            }
 
-                if (setsockopt(fd, SOL_SOCKET, SO_PRIORITY, &cf_prio,
-                    sizeof(cf_prio)) != 0)
-                    LOGE("%s(): Not able to set socket priority. Errno:%d(%s).",
-                         __func__, errno, strerror(-errno));
+            if (setsockopt(fd, SOL_SOCKET, SO_PRIORITY, &cf_prio,
+                sizeof(cf_prio)) != 0)
+                LOGE("%s(): Not able to set socket priority. Errno:%d(%s).",
+                     __func__, errno, strerror(-errno));
 
-                ret = connect(fd, (struct sockaddr *) &addr, sizeof(addr));
-                if (ret != 0)
-                    LOGE("%s(): Failed to connect. errno: %d(%s).", __func__,
-                        errno, strerror(-errno));
+            ret = connect(fd, (struct sockaddr *) &addr, sizeof(addr));
+            if (ret != 0)
+                LOGE("%s(): Failed to connect. errno: %d(%s).", __func__,
+                    errno, strerror(-errno));
 #else
-                LOGE("%s(): Unsupported channel type CAIF. Bailing out!",
-                    __func__);
+            LOGE("%s(): Unsupported channel type CAIF. Bailing out!",
+                __func__);
+            free(queueArgs);
+            return NULL;
+#endif
+        } else if (!strncmp(queueArgs->type, "UNIX", 4)) {
+            struct sockaddr_un addr;
+            int len;
+            if (queueArgs->arg == NULL) {
+                LOGE("%s(): No path specified for UNIX socket!"
+                    " Bailing out!", __func__);
+                free(queueArgs);
+                return NULL;
+            }
+            bzero((char *) &addr, sizeof(addr));
+            addr.sun_family = AF_UNIX;
+
+            strncpy(addr.sun_path, queueArgs->arg,
+                    sizeof(addr.sun_path));
+            len = strlen(addr.sun_path) + sizeof(addr.sun_family);
+            fd = socket(AF_UNIX, SOCK_STREAM, 0);
+            (void)connect(fd, (struct sockaddr *) &addr, len);
+        } else if (!strncmp(queueArgs->type, "IP", 2)) {
+            int port;
+            if (!queueArgs->arg) {
+                LOGE("%s(): No port specified for IP socket! "
+                    "Bailing out!", __func__);
                 free(queueArgs);
                 return NULL;
-#endif
-            } else if (!strncmp(queueArgs->type, "UNIX", 4)) {
-                struct sockaddr_un addr;
-                int len;
-                if (queueArgs->arg == NULL) {
-                    LOGE("%s(): No path specified for UNIX socket!"
-                        " Bailing out!", __func__);
-                    free(queueArgs);
-                    return NULL;
-                }
-                bzero((char *) &addr, sizeof(addr));
-                addr.sun_family = AF_UNIX;
-
-                strncpy(addr.sun_path, queueArgs->arg,
-                        sizeof(addr.sun_path));
-                len = strlen(addr.sun_path) + sizeof(addr.sun_family);
-                fd = socket(AF_UNIX, SOCK_STREAM, 0);
-                (void)connect(fd, (struct sockaddr *) &addr, len);
-            } else if (!strncmp(queueArgs->type, "IP", 2)) {
-                int port;
-                if (!queueArgs->arg) {
-                    LOGE("%s(): No port specified for IP socket! "
-                        "Bailing out!", __func__);
-                    free(queueArgs);
-                    return NULL;
-                }
-                port = atoi(queueArgs->arg);
-                if (queueArgs->xarg) {
-                    char *host = queueArgs->xarg;
-                    fd = socket_network_client(host, port, SOCK_STREAM);
-                } else
-                    fd = socket_loopback_client(port, SOCK_STREAM);
-            } else if (!strncmp(queueArgs->type, "TTY", 3)) {
-                struct termios ios;
-                fd = open(queueArgs->arg, O_RDWR);
-
-                /* Disable echo on serial ports. */
-                tcgetattr(fd, &ios);
-                cfmakeraw(&ios);
-                cfsetospeed(&ios, B115200);
-                cfsetispeed(&ios, B115200);
-                ios.c_cflag |= CREAD | CLOCAL;
-                tcflush(fd, TCIOFLUSH);
-                tcsetattr(fd, TCSANOW, &ios);
-            } else if (!strncmp(queueArgs->type, "CHAR", 4))
-                fd = open(queueArgs->arg, O_RDWR);
-
-            if (fd < 0) {
-                LOGE("%s() failed to open AT channel type:%s %s %s err:%s. "
-                     "retrying in 10 s!",__func__,  queueArgs->type,
-                     queueArgs->arg ? queueArgs->arg : "",
-                     queueArgs->xarg ? queueArgs->xarg : "",
-                     strerror(errno));
-                sleep(10);
             }
+            port = atoi(queueArgs->arg);
+            if (queueArgs->xarg) {
+                char *host = queueArgs->xarg;
+                fd = socket_network_client(host, port, SOCK_STREAM);
+            } else
+                fd = socket_loopback_client(port, SOCK_STREAM);
+        } else if (!strncmp(queueArgs->type, "TTY", 3)) {
+            struct termios ios;
+            fd = open(queueArgs->arg, O_RDWR);
+
+            /* Disable echo on serial ports. */
+            tcgetattr(fd, &ios);
+            cfmakeraw(&ios);
+            cfsetospeed(&ios, B115200);
+            cfsetispeed(&ios, B115200);
+            ios.c_cflag |= CREAD | CLOCAL;
+            tcflush(fd, TCIOFLUSH);
+            tcsetattr(fd, TCSANOW, &ios);
+        } else if (!strncmp(queueArgs->type, "CHAR", 4))
+            fd = open(queueArgs->arg, O_RDWR);
+
+        if (fd < 0) {
+            LOGE("%s() failed to open AT channel type:%s %s %s err:%s. "
+                 "retrying in 10 s!",__func__,  queueArgs->type,
+                 queueArgs->arg ? queueArgs->arg : "",
+                 queueArgs->xarg ? queueArgs->xarg : "",
+                 strerror(errno));
+            sleep(10);
         }
-        ret = at_open(fd, onUnsolicited);
+    }
+    ret = at_open(fd, onUnsolicited);
 
-        if (ret < 0) {
-            LOGE("%s(): AT error %d on at_open!", __func__, ret);
-            at_close();
-            continue;
-        }
+    if (ret < 0) {
+        LOGE("%s(): AT error %d on at_open!", __func__, ret);
+        goto exit;
+    }
 
-        at_set_on_reader_closed(onATReaderClosed);
-        at_set_on_timeout(onATTimeout);
+    at_set_on_reader_closed(onATReaderClosed);
+    at_set_on_timeout(onATTimeout);
 
-        if (!initializeCommon()) {
-            LOGE("%s(): initializeCommon() failed!", __func__);
-            at_close();
-            continue;
-        }
+    if (!initializeCommon()) {
+        LOGE("%s(): initializeCommon() failed!", __func__);
+        goto exit;
+    }
 
-        q = queueArgs->group->requestQueue;
-        q->closed = 0;
+    q = queueArgs->group->requestQueue;
+    q->closed = 0;
 
-        if (queueArgs->group->group == CMD_QUEUE_DEFAULT) {
-            if (!initializeDefault()) {
-                LOGE("%s() failed to initialize default AT channel!",
-                    __func__);
-                at_close();
-                continue;
-            }
-            at_make_default_channel();
+    if (queueArgs->group->group == CMD_QUEUE_DEFAULT) {
+        if (!initializeDefault()) {
+            LOGE("%s() failed to initialize default AT channel!",
+                __func__);
+            goto exit;
         }
+        at_make_default_channel();
+    }
 
-        at_set_timeout_msec(1000 * 60 * 3);
+    at_set_timeout_msec(1000 * 60 * 3);
 
-        LOGI("Looping the requestQueue!");
-        for (;;) {
-            RILRequest *r;
-            RILEvent *e;
-            struct timeval tv;
-            struct timespec ts;
-            int err;
+    RILRequest *r = NULL;
+    RILEvent   *e = NULL;
 
-            memset(&ts, 0, sizeof(ts));
+    LOGI("Looping the requestQueue for index %d!", queueArgs->index);
+    for (;;) {
+        struct timeval tv;
+        struct timespec ts;
+        int err;
 
-            if ((err = pthread_mutex_lock(&q->queueMutex)) != 0) {
-                LOGE("%s() failed to take queue mutex: %s!",
-                    __func__, strerror(err));
-                break;
-            }
+        memset(&ts, 0, sizeof(ts));
 
-            if (q->closed != 0) {
-                LOGW("%s(): AT Channel error, attempting to recover!",
-                    __func__);
-                if ((err = pthread_mutex_unlock(&q->queueMutex)) != 0)
-                    LOGE("Failed to release queue mutex: %s!", strerror(err));
-                break;
-            }
+        if ((err = pthread_mutex_lock(&q->queueMutex)) != 0) {
+            LOGE("%s() failed to take queue mutex: %s!",
+                __func__, strerror(err));
+            /* Need to restart all threads and restart modem.*/
+            break;
+        }
 
-            while (q->closed == 0 && q->requestList == NULL &&
-                   q->eventList == NULL) {
-                if ((err = pthread_cond_wait(&q->cond, &q->queueMutex)) != 0)
-                    LOGE("%s() failed to broadcast queue update: %s!",
-                        __func__, strerror(err));
-            }
+        if (q->closed != 0) {
+            LOGW("%s() index %d queue close indication, ending current thread!",
+                __func__, queueArgs->index);
+            if ((err = pthread_mutex_unlock(&q->queueMutex)) != 0)
+                LOGE("Failed to release queue mutex: %s!", strerror(err));
+            break;
+        }
 
-            /* eventList is prioritized, smallest abstime first. */
-            if (q->closed == 0 && q->requestList == NULL && q->eventList) {
-                err = pthread_cond_timedwait(&q->cond, &q->queueMutex,
-                                             &q->eventList->abstime);
-                if (err && err != ETIMEDOUT)
-                    LOGE("%s(): Timedwait returned unexpected error: %s!",
-                         __func__, strerror(err));
-            }
+        while (q->closed == 0 && q->requestList == NULL &&
+               q->eventList == NULL) {
+            if ((err = pthread_cond_wait(&q->cond, &q->queueMutex)) != 0)
+                LOGE("%s() failed to broadcast queue update: %s!",
+                    __func__, strerror(err));
+        }
 
-            if (q->closed != 0) {
-                if ((err = pthread_mutex_unlock(&q->queueMutex)) != 0)
-                    LOGW("%s(): Failed to release queue mutex: %s!",
-                        __func__, strerror(err));
-                continue; /* Catch the closed bit at the top of the loop. */
-            }
+        /* eventList is prioritized, smallest abstime first. */
+        if (q->closed == 0 && q->requestList == NULL && q->eventList) {
+            err = pthread_cond_timedwait(&q->cond, &q->queueMutex,
+                                         &q->eventList->abstime);
+            if (err && err != ETIMEDOUT)
+                LOGE("%s(): Timedwait returned unexpected error: %s!",
+                     __func__, strerror(err));
+        }
 
-            e = NULL;
-            r = NULL;
+        if (q->closed != 0) {
+            if ((err = pthread_mutex_unlock(&q->queueMutex)) != 0)
+                LOGW("%s(): Failed to release queue mutex: %s!",
+                    __func__, strerror(err));
+            break;
+        }
 
-            gettimeofday(&tv, NULL);
+        e = NULL;
+        r = NULL;
 
-            ts.tv_sec = tv.tv_sec;
-            ts.tv_nsec = tv.tv_usec * 1000;
+        gettimeofday(&tv, NULL);
 
-            if (q->eventList != NULL &&
-                timespec_cmp(q->eventList->abstime, ts, <)) {
-                e = q->eventList;
-                q->eventList = e->next;
-            }
+        ts.tv_sec = tv.tv_sec;
+        ts.tv_nsec = tv.tv_usec * 1000;
 
-            if (q->requestList != NULL) {
-                r = q->requestList;
-                q->requestList = r->next;
-            }
+        if (q->eventList != NULL &&
+            timespec_cmp(q->eventList->abstime, ts, <)) {
+            e = q->eventList;
+            q->eventList = e->next;
+        }
 
-            if ((err = pthread_mutex_unlock(&q->queueMutex)) != 0)
-                LOGW("%s(): Failed to release queue mutex: %s!",
-                    __func__, strerror(err));
+        if (q->requestList != NULL) {
+            r = q->requestList;
+            q->requestList = r->next;
+        }
 
-            if (e) {
-                e->eventCallback(e->param);
-                free(e);
-            }
+        if ((err = pthread_mutex_unlock(&q->queueMutex)) != 0)
+            LOGW("%s(): Failed to release queue mutex: %s!",
+                __func__, strerror(err));
 
-            if (r) {
-                processRequest(r->request, r->data, r->datalen, r->token);
-                freeRequestData(r->request, r->data, r->datalen);
-                free(r);
-            }
+        if (e) {
+            e->eventCallback(e->param);
+            free(e);
         }
 
-        at_close();
-        LOGI("Re-opening after close");
+        if (r) {
+            processRequest(r->request, r->data, r->datalen, r->token);
+            freeRequestData(r->request, r->data, r->datalen);
+            free(r);
+        }
+    }
+
+    /* Final cleanup of queues. Radio state must be unavailable at this point */
+    assert(s_state == RADIO_STATE_UNAVAILABLE);
+
+    LOGI("%s() index %d start flushing all remaining requests and events!",
+         __func__, queueArgs->index);
+    /*
+     * NOTE: There cannot be events that will generate response to earlier
+     * requests. If so we have to let all events trigger immediatly and refuse
+     * further events to be put on the queue.
+     */
+    /* Request queue cleanup */
+    while (q != NULL && q->requestList != NULL) {
+        r = q->requestList;
+        q->requestList = r->next;
+        if(!requestStateFilter(r->request, r->token)) {
+            LOGE("%s() tried to send immidiate response to request but it was "
+                 "not stopped by filter. Undefined behavior expected! Error!",
+                 __func__);
+        }
+        freeRequestData(r->request, r->data, r->datalen);
+        free(r);
+    }
+    /* Event queue cleanup */
+    while (q != NULL && q->eventList != NULL) {
+        e = q->eventList;
+        q->eventList = e->next;
+        free(e);
     }
+    LOGI("%s() index %d finished flushing, queues emptied", __func__,
+         queueArgs->index);
+
+exit:
+    /* Make sure A channel is closed in case queueRunner triggered the exit */
+    at_close();
+    /*
+     * Finally signal RIL Manager that this queueRunner and
+     * AT channel is closed.
+     */
+    signalManager();
 
+    LOGD("%s() thread with index %d ending", __func__, queueArgs->index);
     free(queueArgs);
     return NULL;
 }
diff --git a/u300-ril.h b/u300-ril.h
index 9ed4f36..735e1c7 100644
--- a/u300-ril.h
+++ b/u300-ril.h
@@ -36,7 +36,7 @@ bool getScreenState(void);
 void setScreenState(bool screenIsOn);
 void releaseScreenStateLock(void);
 void *queueRunner(void *param);
-void onATReaderClosed();
+void signalCloseQueues(void);
 
 /*
  * Maximum number of neighborhood cells
@@ -51,13 +51,12 @@ extern char ril_iface[MAX_IFNAME_LEN];
 extern const struct RIL_Env *s_rilenv;
 extern const RIL_RadioFunctions g_callbacks;
 
-#ifndef EXTERNAL_MODEM_CONTROL_MODULE_DISABLED
-extern pthread_mutex_t s_dbus_mid_wait_mutex;
-extern pthread_cond_t s_dbus_mid_wait_cond;
+extern bool managerRelease;
+extern pthread_mutex_t ril_manager_wait_mutex;
+extern pthread_cond_t ril_manager_wait_cond;
 
-extern bool dbusIsHere;
-extern bool dbusRelease;
-#endif /* EXTERNAL_MODEM_CONTROL_MODULE_DISABLED */
+extern pthread_mutex_t ril_manager_queue_exit_mutex;
+extern pthread_cond_t ril_manager_queue_exit_cond;
 
 int getRestrictedState(void);