Cellular: Fix cellular statemachine stop and BG96 power up

features/cellular/framework/device/CellularDevice.cpp

@@ -124,20 +124,20 @@ nsapi_error_t CellularDevice::create_state_machine()
         _nw->attach(callback(this, &CellularDevice::stm_callback));
         _state_machine = new CellularStateMachine(*this, *get_queue(), *_nw);
         _state_machine->set_cellular_callback(callback(this, &CellularDevice::stm_callback));
-        err = _state_machine->start_dispatch();
-        if (err) {
-            tr_error("Start state machine failed.");
-            delete _state_machine;
-            _state_machine = NULL;
-        }
-
         if (strlen(_plmn)) {
             _state_machine->set_plmn(_plmn);
         }
         if (strlen(_sim_pin)) {
             _state_machine->set_sim_pin(_sim_pin);
         }
     }
+    err = _state_machine->start_dispatch();
+    if (err) {
+        tr_error("Start state machine failed.");
+        delete _state_machine;
+        _state_machine = NULL;
+        return err;
+    }
     return err;
 }
 

features/cellular/framework/device/CellularStateMachine.cpp

@@ -103,7 +103,6 @@ void CellularStateMachine::stop()
 {
     tr_debug("CellularStateMachine stop");
     if (_queue_thread) {
-        _queue.break_dispatch();
         _queue_thread->terminate();
         delete _queue_thread;
         _queue_thread = NULL;
@@ -366,6 +365,9 @@ void CellularStateMachine::state_device_ready()
                 _status = 0;
                 enter_to_state(STATE_SIM_PIN);
             }
+        } else {
+            _status = 0;
+            enter_to_state(STATE_INIT);
         }
     }
     if (_cb_data.error != NSAPI_ERROR_OK) {
@@ -546,7 +548,7 @@ bool CellularStateMachine::get_current_status(CellularStateMachine::CellularStat
 void CellularStateMachine::event()
 {
     // Don't send Signal quality when in signal quality state or it can confuse callback functions when running retry logic
-    if (_state != STATE_SIGNAL_QUALITY) {
+    if (_state > STATE_SIGNAL_QUALITY) {
         _cb_data.error = _network.get_signal_quality(_signal_quality.rssi, &_signal_quality.ber);
         _cb_data.data = &_signal_quality;
 
@@ -624,15 +626,21 @@ void CellularStateMachine::event()
 
 nsapi_error_t CellularStateMachine::start_dispatch()
 {
-    MBED_ASSERT(!_queue_thread);
+    if (!_queue_thread) {
+        _queue_thread = new rtos::Thread(osPriorityNormal, 2048, NULL, "stm_queue");
+        _event_id = STM_STOPPED;
+    }
 
-    _queue_thread = new rtos::Thread(osPriorityNormal, 2048, NULL, "stm_queue");
-    if (_queue_thread->start(callback(&_queue, &events::EventQueue::dispatch_forever)) != osOK) {
-        report_failure("Failed to start thread.");
-        stop();
-        return NSAPI_ERROR_NO_MEMORY;
+    if (_event_id == STM_STOPPED) {
+        if (_queue_thread->start(callback(&_queue, &events::EventQueue::dispatch_forever)) != osOK) {
+            report_failure("Failed to start thread.");
+            stop();
+            return NSAPI_ERROR_NO_MEMORY;
+        }
     }
 
+    _event_id = -1;
+
     return NSAPI_ERROR_OK;
 }
 

features/cellular/framework/targets/QUECTEL/BG96/QUECTEL_BG96.cpp

@@ -111,31 +111,36 @@ nsapi_error_t QUECTEL_BG96::hard_power_on()
 
 nsapi_error_t QUECTEL_BG96::soft_power_on()
 {
-    if (_rst.is_connected()) {
-        tr_info("Reset modem");
-        _rst = !_active_high;
-        ThisThread::sleep_for(100);
-        _rst = _active_high;
-        ThisThread::sleep_for(150 + 460); // RESET_N timeout from BG96_Hardware_Design_V1.1
-        _rst = !_active_high;
-        ThisThread::sleep_for(500);
+    if (!_rst.is_connected()) {
+        return NSAPI_ERROR_OK;
+    }
 
-        // wait for RDY
-        _at->lock();
-        _at->set_at_timeout(10 * 1000);
-        _at->resp_start();
-        _at->set_stop_tag("RDY");
-        bool rdy = _at->consume_to_stop_tag();
-        _at->set_stop_tag(OK);
-        _at->restore_at_timeout();
-        _at->unlock();
+    tr_info("Reset modem");
+    _rst = !_active_high;
+    ThisThread::sleep_for(100);
+    _rst = _active_high;
+    ThisThread::sleep_for(150 + 460); // RESET_N timeout from BG96_Hardware_Design_V1.1
+    _rst = !_active_high;
+    ThisThread::sleep_for(500);
 
-        if (!rdy) {
-            return NSAPI_ERROR_DEVICE_ERROR;
-        }
+    // wait for RDY
+    _at->lock();
+    _at->set_at_timeout(10 * 1000);
+    _at->resp_start();
+    _at->set_stop_tag("RDY");
+    bool rdy = _at->consume_to_stop_tag();
+    _at->set_stop_tag(OK);
+    _at->restore_at_timeout();
+
+    if (!rdy) {
+        // check if modem was silently powered on
+        _at->clear_error();
+        _at->set_at_timeout(100);
+        _at->cmd_start("AT");
+        _at->cmd_stop_read_resp();
+        _at->restore_at_timeout();
     }
-
-    return NSAPI_ERROR_OK;
+    return _at->unlock_return_error();
 }
 
 nsapi_error_t QUECTEL_BG96::hard_power_off()
@@ -162,26 +167,22 @@ nsapi_error_t QUECTEL_BG96::init()
     _at->cmd_start("AT+CMEE=1"); // verbose responses
     _at->cmd_stop_read_resp();
 
-    if (_at->get_last_error() == NSAPI_ERROR_OK) {
-        do {
-            _at->cmd_start("AT+CFUN=1"); // set full functionality
-            _at->cmd_stop_read_resp();
-
-            // CFUN executed ok
-            if (_at->get_last_error() != NSAPI_ERROR_OK) {
-                // wait some time that modem gets ready for CFUN command, and try again
-                retry++;
-                _at->flush();
-                ThisThread::sleep_for(64); // experimental value
-            } else {
-                // yes continue
-                break;
-            }
-
-            /* code */
-        } while ((retry < 3));
+    if (_at->get_last_error() != NSAPI_ERROR_OK) {
+        return _at->unlock_return_error();
     }
 
+    do {
+        _at->clear_error();
+        _at->cmd_start("AT+CFUN=1"); // set full functionality
+        _at->cmd_stop_read_resp();
+        if (_at->get_last_error() == NSAPI_ERROR_OK) {
+            break;
+        }
+        // wait some time that modem gets ready for CFUN command, and try again
+        retry++;
+        ThisThread::sleep_for(64); // experimental value
+    } while (retry < 3);
+
     return _at->unlock_return_error();
 }