1 files changed, 100 insertions, 0 deletions

diff --git a/src/MTS_IO_QuectelRadio.cpp b/src/MTS_IO_QuectelRadio.cpp
index b1e1e59..ae6200e 100644
--- a/src/MTS_IO_QuectelRadio.cpp
+++ b/src/MTS_IO_QuectelRadio.cpp
@@ -1044,6 +1044,7 @@ uint16_t QuectelRadio::getQuectelChecksum(const void* data, size_t nBytes) {
 
 ICellularRadio::CODE QuectelRadio::fumoWaitUpgradeFinished(ICellularRadio::UpdateCb& stepCb) {
     const uint32_t duUrcTimeout = 4 * 60 * 1000;  // wait for 4 minutes for the next URC message
+    const uint32_t duAttachTimeout = 30 * 1000;  // wait up to 30 seconds for the radio to attach
     const std::string sFotaUrcPrefix = "+QIND: \"FOTA\"";  // prefix for the URC notification messages
     const std::string sFotaUrcStart = "\"START\"";
     const std::string sFotaUrcProgress = "\"UPDATING\"";
@@ -1058,6 +1059,18 @@ ICellularRadio::CODE QuectelRadio::fumoWaitUpgradeFinished(ICellularRadio::Updat
         sResponse = waitResponse(vFotaBailStrings, duUrcTimeout);
         printTrace("Radio response: [%s]", sResponse.c_str());
 
+        if (sResponse.empty()) {
+            // Radio detached again. Try to reconnect
+            if (!resetConnection(duAttachTimeout)) {
+                printError("Can't connect to the radio in %d ms", (duAttachTimeout));
+                callNextStep(stepCb, "FUMO Error: unable to obtain radio after second reset");
+                return ERROR;
+            }
+
+            sResponse = waitResponse(vFotaBailStrings, duUrcTimeout);
+            printTrace("Radio response: [%s]", sResponse.c_str());
+        }
+
         if (sResponse.find(sFotaUrcPrefix) == std::string::npos) {
             printError("No URC messages from the radio in %d ms", duUrcTimeout);
             callNextStep(stepCb, "FUMO Error: timeout, radio is not responding");
@@ -1528,3 +1541,90 @@ bool MTS::IO::QuectelRadio::isContainsSignChar(const std::string& str) {
 
     return true;
 }
+
+ICellularRadio::CODE QuectelRadio::isDivctlSupported(bool& bSupported) {
+    const std::string sCommand = "AT+QCFG=\"divctl\"";
+    const std::string sLabel = "+QCFG: \"divctl\",";
+    const int dTimeout = 1000; // ms
+    std::string sResult;
+
+    CODE rc;
+
+    rc = sendBasicQuery(sCommand, sLabel, sResult, dTimeout);
+    if (rc == ERROR) {
+        bSupported = false;
+        return SUCCESS;
+    } else if (rc == SUCCESS) {
+        if (sResult.find("(\"lte\",\"wcdma\")") != std::string::npos) {
+            bSupported = true;
+        } else {
+            bSupported = false;
+        }
+        return rc;
+    } else {
+        return rc;
+    }
+}
+
+ICellularRadio::CODE QuectelRadio::setRxDiversity(const Json::Value& jArgs) {
+    if (jArgs["enabled"].asString() != "1" && jArgs["enabled"].asString() != "0") {
+        return FAILURE;
+    }
+
+    ICellularRadio::CODE rc;
+    std::string sCmd;
+    bool bSupported;
+
+    rc = isDivctlSupported(bSupported);
+    if (rc != SUCCESS) {
+        printError("%s| Failed to determine the AT+QCFG=\"divctl\" support: [%d]", getName().c_str(), rc);
+        return rc;
+    }
+
+    if (!bSupported) {
+        /* Old command string for EG95 radios: AT+QCFG="diversity",(0-1) */
+        sCmd = "AT+QCFG=\"diversity\",";
+        sCmd += jArgs["enabled"].asString();
+
+        return sendBasicCommand(sCmd);
+    }
+
+    std::string sValue;
+
+    /* Reverse obtained value because the new command string works in the following way:
+         0 - enable,
+         1 - disable, use the main antenna as the only antenna.
+         2 - disable, use the diversity antenna as the only antenna.
+    */
+    if (jArgs["enabled"].asString() == "1") {
+        sValue = "0";
+    } else {
+        sValue = "1";
+    }
+
+    /* New command string for EG95 radios:
+         AT+QCFG=\"divctl\",\"lte\",(0-2)
+         AT+QCFG=\"divctl\",\"wcdma\",(0-2)
+    */
+    const int dTimeout = 1000; // ms
+
+    sCmd = "AT+QCFG=\"divctl\",\"lte\",";
+    sCmd += sValue;
+
+    rc = sendBasicCommand(sCmd, dTimeout);
+    if (rc != SUCCESS) {
+        printError("%s| Failed to set diversity for LTE network mode: [%d]", getName().c_str(), rc);
+        return rc;
+    }
+
+    sCmd = "AT+QCFG=\"divctl\",\"wcdma\",";
+    sCmd += sValue;
+
+    rc = sendBasicCommand(sCmd, dTimeout);
+    if (rc != SUCCESS) {
+        printError("%s| Failed to set diversity for WCDMA network mode: [%d]", getName().c_str(), rc);
+        return rc;
+    }
+
+    return SUCCESS;
+}
\ No newline at end of file