1 files changed, 39 insertions, 13 deletions

diff --git a/src/MTS_IO_QuectelRadio.cpp b/src/MTS_IO_QuectelRadio.cpp
index 03593a7..9a95046 100644
--- a/src/MTS_IO_QuectelRadio.cpp
+++ b/src/MTS_IO_QuectelRadio.cpp
@@ -591,6 +591,13 @@ ICellularRadio::CODE QuectelRadio::fumoLocalApply(ICellularRadio::UpdateCb& step
     ICellularRadio::CODE rc;
     std::string sCmd;
 
+    rc = getQuectelFirmware(m_sQuectelFirmware);
+    if (rc != SUCCESS) {
+        callNextStep(stepCb, "FUMO Error: Failed to obtain current firmware version");
+        return rc;
+    }
+    printInfo("Current firmware version: %s", m_sQuectelFirmware.c_str());
+
     // Send "AT+QFOTADL" command to start the upgrade. OK response follows shortly.
     sCmd  = "AT+QFOTADL=\"";
     sCmd += VALUE_MTS_DELTA_PATH;
@@ -941,6 +948,26 @@ ICellularRadio::CODE QuectelRadio::checkFile(bool& bIsFilePresent, const std::st
     return SUCCESS;
 }
 
+ICellularRadio::CODE QuectelRadio::getQuectelFirmware(std::string& sFirmware) {
+    printTrace("%s| Get Quectel-specific firmware version", getName().c_str());
+    sFirmware = ICellularRadio::VALUE_NOT_SUPPORTED;
+    std::string sCmd("AT+QGMR");
+    std::string sResult = sendCommand(sCmd);
+    size_t pos = sResult.find(ICellularRadio::RSP_OK);
+    if (pos == std::string::npos) {
+        printWarning("%s| Unable to get firmware from radio using command [%s]", getName().c_str(), sCmd.c_str());
+        return FAILURE;
+    }
+
+    sFirmware = MTS::Text::trim(sResult.substr(0, pos));
+    if(sFirmware.size() == 0) {
+        printWarning("%s| Unable to get firmware from radio using command [%s]", getName().c_str(), sCmd.c_str());
+        return FAILURE;
+    }
+
+    return SUCCESS;
+}
+
 uint16_t QuectelRadio::getQuectelChecksum(const void* data, size_t nBytes) {
     auto castData = static_cast<const uint8_t*>(data);
     uint16_t iChecksum = 0;
@@ -1085,37 +1112,36 @@ ICellularRadio::CODE QuectelRadio::fumoWaitUpgradeFinished(ICellularRadio::Updat
 ICellularRadio::CODE QuectelRadio::fumoWaitNewFirmware(ICellularRadio::UpdateCb& stepCb) {
     MTS::Timer oTimer;
     oTimer.start();
-    std::string sFirmware;
-    CODE rc = FAILURE;
+    std::string sQuectelFirmware;
+    CODE rc = ERROR;
 
     while (oTimer.getSeconds() < (5 * 60)) { // 5 minutes
 
         MTS::Thread::sleep(10000);
 
-        if (getFirmware(sFirmware) != SUCCESS) {
+        if (getQuectelFirmware(sQuectelFirmware) != SUCCESS) {
             // The radio is probably unavailable
             resetConnection(100);
             continue;
         }
 
-#if 0
-        // TODO: Implement the version check
-        if (sFirmware == m_sFw && sFirmwareBuild == m_sFwBuild) {
-            // Have the same firmware. The radio resets several time
-            // before the firmware is actually get upgraded. So keep polling.
-            continue;
+        printInfo("Firmware version before the upgrade: %s", m_sQuectelFirmware.c_str());
+        printInfo("Current firmware version: %s", sQuectelFirmware.c_str());
+
+        if (sQuectelFirmware == m_sQuectelFirmware) {
+            // Radio will not reset anymore, firmware version left the same, not updated
+            printError("Radio firmware version not changed after upgrade");
+            rc = FAILURE;
+            break;
         }
-#endif
 
         // The firmware numbers have changed
-        printTrace("Current firmware: %s", sFirmware.c_str());
         rc = SUCCESS;
         break;
-
     }
     oTimer.stop();
 
-    if (rc != SUCCESS) {
+    if (rc == ERROR) {
         printError("Radio is not responding");
         callNextStep(stepCb, "FUMO Error: unable to obtain radio after reset");
     }