Update MODBUS slave feature, Rogers Certification issue1.0.28

13 files changed, 358 insertions, 30 deletions

diff --git a/data/MccMncTable.csv b/data/MccMncTable.csv
index f0246e2..968dbc1 100644
--- a/data/MccMncTable.csv
+++ b/data/MccMncTable.csv
@@ -463,10 +463,10 @@
 "297","02","me","Montenegro","382","Monet/T-mobile",
 "297","03","me","Montenegro","382","Mtel",
 "302","220","ca","Canada","1","Telus Mobility",
-"302","320","ca","Canada","1","mobilicity",
+"302","320","ca","Canada","1","Mobilicity",
 "302","360","ca","Canada","1","Clearnet",
 "302","361","ca","Canada","1","Clearnet",
-"302","370","ca","Canada","1","FIDO (Rogers AT&T/ Microcell)",
+"302","370","ca","Canada","1","Fido",
 "302","380","ca","Canada","1","DMTS Mobility",
 "302","490","ca","Canada","1","WIND",
 "302","500","ca","Canada","1","Videotron",
@@ -487,8 +487,10 @@
 "302","702","ca","Canada","1","MT&T Mobility",
 "302","703","ca","Canada","1","New Tel Mobility",
 "302","710","ca","Canada","1","Globalstar Canada",
-"302","720","ca","Canada","1","Rogers AT&T Wireless",
+"302","720","ca","Canada","1","Rogers Wireless",
+"302","721","ca","Canada","1","Rogers Wireless",
 "302","760","ca","Canada","1","Public Mobile",
+"302","820","ca","Canada","1","Rogers Wireless",
 "308","01","pm","St. Pierre & Miquelon","508","Ameris",
 "310","003","us","United States","1","Unknown",
 "310","004","us","United States","1","Verizon Wireless",
diff --git a/include/mts/MTS_IO_CellularRadio.h b/include/mts/MTS_IO_CellularRadio.h
index 4d1cd12..f32251c 100644
--- a/include/mts/MTS_IO_CellularRadio.h
+++ b/include/mts/MTS_IO_CellularRadio.h
@@ -72,6 +72,7 @@ namespace MTS {
                 CODE getTime(std::string& sDate, std::string& sTime, std::string& sTimeZone) override;
                 CODE getRoaming(bool& bRoaming) override;
                 CODE getCellularMode(CELLULAR_MODES &networks) override;
+                CODE getSelectedBandsRaw(std::string& sRawBands) override;
 
                 CODE getSignalStrength(int32_t& iRssi) override;
                 CODE getModemLocation(std::string& sLocation) override;
@@ -223,6 +224,32 @@ namespace MTS {
                  */
                 virtual CODE getSimMncLength(uint8_t& iLength);
 
+                /**
+                 * @brief getCarrierFromSimMccMnc - get a carrier name based on the MCC/MNC values.
+                 *
+                 * @param sCarrier - a string to be filled with carrier based on the MCC/MNC values.
+                 *
+                 * @return CODE::SUCCESS when the read is completed successfully,
+                 *         CODE::NOT_APPLICABLE when the modem doesn't support this feature,
+                 *         CODE::NO_RESPONSE when the modem doesn't respond,
+                 *         CODE::ERROR when the radio returns "ERROR" (SIM card removed, SIM card locked etc),
+                 *         CODE::FAILURE otherwise (unexpected response, no data in SIM etc).
+                 */
+                virtual CODE getCarrierFromSimMccMnc(std::string& sCarrier);
+
+                /**
+                 * @brief getCarrierFromSimSpn - get a carrier name from SIM EFspn.
+                 *
+                 * @param sCarrier - a string to be filled with carrier obtained from SIM EFspn.
+                 *
+                 * @return CODE::SUCCESS when the read is completed successfully,
+                 *         CODE::NOT_APPLICABLE when the modem doesn't support this feature,
+                 *         CODE::NO_RESPONSE when the modem doesn't respond,
+                 *         CODE::ERROR when the radio returns "ERROR" (SIM card removed, SIM card locked etc),
+                 *         CODE::FAILURE otherwise (unexpected response, no data in SIM etc).
+                 */
+                virtual CODE getCarrierFromSimSpn(std::string& sCarrier);
+
                 void initMipProfile(Json::Value& jData);
                 bool splitAndAssign(const std::string& sLine, const std::string& sKey, Json::Value& jParent, const std::string& sJsonKey, Json::ValueType eType = Json::ValueType::stringValue);
 
diff --git a/include/mts/MTS_IO_ICellularRadio.h b/include/mts/MTS_IO_ICellularRadio.h
index e061636..f3e15e2 100644
--- a/include/mts/MTS_IO_ICellularRadio.h
+++ b/include/mts/MTS_IO_ICellularRadio.h
@@ -620,6 +620,42 @@ namespace MTS {
                  */
                 virtual CODE getVoiceSupport(Json::Value& jData) = 0;
                 virtual CODE getVoiceSupport(bool& bVoiceEnabled, bool& bSmsOnly) = 0;
+
+                /**
+                * @brief getSelectedBandsRaw - get the raw Selected Bands value for the current radio.
+                *
+                * This commands sends the query to the radio, selects first 3 values from the response,
+                * parses the numbers as dec (Telit GSM and WCDMA fiels) or hex (Quectel GW_band and LTE,
+                * Telit LTE), formats them as comma-separated hex values and saves the result to sRawBands.
+                *
+                * WARNING: The response format is radio-dependent. Please consult the AT command manual
+                * to interpet the resulting values.
+                *
+                * For Telit radios:
+                *
+                * - command: `AT#BND?`;
+                * - example radio response: `#BND: 5,15,800C5`;
+                * - `sRawBands` format: `<GSM, hex, 16-bit>[,<WCDMA, hex, 16-bit>[,<LTE, hex, 64-bit>]]`;
+                * - example `sRawBands` content: `5,F,800C5`.
+                *
+                * For Quectel radios:
+                *
+                * - command: `AT+QCFG="band"`;
+                * - example radio response: `+QCFG: "band",0x93,0x80800c5,0x0`;
+                * - `sRawBands` format: `<GW_band, hex, 16-bit>,<GW_band, hex, 16-bit>,<LTE, hex, 64-bit>`,
+                *   NOTE: the `<GW_band>` value is duplicated for the first two fields;
+                * - example `sRawBands` content: `93,93,80800c5`.
+                *
+                * @param sRawBands - radio-depended string value, shows the currently selected bands.
+                *
+                * @return CODE::SUCCESS when fetched successfully,
+                *         CODE::NOT_APPLICABLE when the modem doesn't support this feature,
+                *         CODE::NO_RESPONSE when the modem doesn't respond,
+                *         CODE::ERROR when the radio returns "ERROR",
+                *         CODE::FAILURE otherwise (unexpected response).
+                */
+                virtual CODE getSelectedBandsRaw(std::string& sRawBands) = 0;
+
         };
     }
 }
diff --git a/include/mts/MTS_IO_QuectelRadio.h b/include/mts/MTS_IO_QuectelRadio.h
index 4e82261..c54e560 100644
--- a/include/mts/MTS_IO_QuectelRadio.h
+++ b/include/mts/MTS_IO_QuectelRadio.h
@@ -57,6 +57,8 @@ namespace MTS {
                 CODE disableVoiceSupport() override;
                 CODE getVoiceSupport(bool& bVoiceEnabled, bool& bSmsOnly) override;
 
+                CODE getSelectedBandsRaw(std::string& sRawBands) override;
+
             protected:
                 enum class UE_USAGE_SETTING : uint8_t {
                     UNKNOWN_MODE = 0, // Unknown mode
@@ -79,6 +81,8 @@ namespace MTS {
                 virtual CODE getUeUsageSetting(QuectelRadio::UE_USAGE_SETTING& us);
                 virtual CODE convertToUeUsageSetting(const std::string& sSetting, QuectelRadio::UE_USAGE_SETTING& us);
 
+                static bool isContainsSignChar(const std::string& str);
+
             private:
                 // private variable to save old firmware versions during FOTA
                 std::string m_sQuectelFirmware;
diff --git a/include/mts/MTS_IO_TelitRadio.h b/include/mts/MTS_IO_TelitRadio.h
index ffdddd9..a06da54 100644
--- a/include/mts/MTS_IO_TelitRadio.h
+++ b/include/mts/MTS_IO_TelitRadio.h
@@ -50,6 +50,8 @@ namespace MTS {
                 CODE fumoLocalInject(int fd, UpdateCb& stepCb) override;
                 CODE fumoLocalApply(UpdateCb& stepCb) override;
 
+                CODE getSelectedBandsRaw(std::string& sRawBands) override;
+
             protected:
                 TelitRadio(const std::string& sName, const std::string& sRadioPort);
 
@@ -71,6 +73,8 @@ namespace MTS {
                 virtual CODE fumoWriteGroupsABD(int fd, UpdateCb& stepCb);
                 //virtual CODE fumoWriteGroupC(int fd, UpdateCb& stepCb);
 
+                static bool isContainsSignChar(const std::string& str);
+
             private:
                 virtual CODE getSimLockAttempts(int& iAttemptsPin, int& iAttemptsPuk, const std::string& sLockStatus);
                 ICellularRadio::CODE wdsList(std::set<int> &wds);
diff --git a/src/MTS_IO_CellularRadio.cpp b/src/MTS_IO_CellularRadio.cpp
index 6236c3f..afdfc1e 100644
--- a/src/MTS_IO_CellularRadio.cpp
+++ b/src/MTS_IO_CellularRadio.cpp
@@ -147,7 +147,9 @@ bool CellularRadio::resetConnection(uint32_t iTimeoutMillis) {
         } else {
             printInfo("%s| Successfully re-opened radio port [%s]", m_sName.c_str(), m_sRadioPort.c_str());
             printDebug("%s| Recovering 'echo' after connection reset", m_sName.c_str()); // see CellularRadio::initialize
-            setEcho(m_bEchoEnabled);
+            if (setEcho(m_bEchoEnabled) != SUCCESS) {
+                printWarning("%s| Failed to recover 'echo' after connection reset", m_sName.c_str());
+            }
             break;
         }
 
@@ -425,26 +427,110 @@ ICellularRadio::CODE CellularRadio::getType(std::string& sType) {
 
 ICellularRadio::CODE CellularRadio::getCarrier(std::string& sCarrier) {
     printTrace("%s| Get Carrier", m_sName.c_str());
-    if (m_sCarrier == "") {
-        std::string sMcc;
-        std::string sMnc;
-        if (getSimMccMnc(sMcc, sMnc) == CODE::SUCCESS) {
-            Json::Value jLookup = MccMncTable::getInstance()->lookup(sMcc, sMnc);
-            printTrace("%s| MCC-MNC Lookup: [%s][%s][%s]", m_sName.c_str(),
-                       sMcc.c_str(), sMnc.c_str(), jLookup.toStyledString().c_str());
-            if (jLookup.isMember(ICellularRadio::KEY_CARRIER)) {
-                m_sCarrier = jLookup[ICellularRadio::KEY_CARRIER].asString();
-            } else {
-                printWarning("%s| MCC-MNC Lookup did not contain carrier", m_sName.c_str());
-                return FAILURE;
+
+    CODE rc = SUCCESS;
+    std::string t_sCarrier;
+
+    do {
+        if (m_sCarrier == "") {
+            rc = getCarrierFromSimSpn(t_sCarrier);
+            if (rc == SUCCESS) {
+                m_sCarrier = t_sCarrier;
+                break;
+            }
+
+            rc = getCarrierFromSimMccMnc(t_sCarrier);
+            if (rc == SUCCESS) {
+                m_sCarrier = t_sCarrier;
+                break;
             }
+        }
+    } while (false);
+
+    sCarrier = m_sCarrier;
+    return rc;
+}
+
+ICellularRadio::CODE CellularRadio::getCarrierFromSimMccMnc(std::string& sCarrier) {
+    printTrace("%s| Get Carrier from SIM MCC/MNC", m_sName.c_str());
+
+    std::string sMcc;
+    std::string sMnc;
+
+    CODE rc;
+
+    rc = getSimMccMnc(sMcc, sMnc);
+    if (rc == SUCCESS) {
+        Json::Value jLookup = MccMncTable::getInstance()->lookup(sMcc, sMnc);
+        printTrace("%s| MCC-MNC Lookup: [%s][%s][%s]", m_sName.c_str(),
+                    sMcc.c_str(), sMnc.c_str(), jLookup.toStyledString().c_str());
+        if (jLookup.isMember(ICellularRadio::KEY_CARRIER)) {
+            sCarrier = jLookup[ICellularRadio::KEY_CARRIER].asString();
         } else {
-            printWarning("%s| SIM did no contain MCC or MNC", m_sName.c_str());
+            printWarning("%s| MCC-MNC Lookup does not contain the carrier", m_sName.c_str());
             return FAILURE;
         }
+    } else {
+        printWarning("%s| SIM did no contain MCC or MNC", m_sName.c_str());
+        return rc;
+    }
+
+    return SUCCESS;
+}
+
+ICellularRadio::CODE CellularRadio::getCarrierFromSimSpn(std::string& sCarrier) {
+    printTrace("%s| Get Carrier from SIM SPN", m_sName.c_str());
+
+    const int iEfspnId = 0x6F46;
+    const uint8_t iOffsetHigh = 1;
+    const uint8_t iOffsetLow = 0;
+    const uint8_t iNumBytes = 16;
+
+    CODE rc;
+    std::string sEFspnContent;
+
+    rc = simAccessReadBinary(iEfspnId, iOffsetLow, iOffsetHigh, iNumBytes, sEFspnContent);
+    if (rc != SUCCESS) {
+        printError("%s| Failed to determine the service provider name", m_sName.c_str());
+        return rc;
+    }
+
+    if (sEFspnContent.length() != 32) {
+        printError("%s| Invalid length of the service provider name: expected [32], actual: [%d]", m_sName.c_str(), sEFspnContent.length());
+        return FAILURE;
+    }
+
+    uint8_t iSpnPart;
+
+    for (size_t i = 0; i < sEFspnContent.length(); i += 2) {
+        std::string sPart = sEFspnContent.substr(i, 2);
+
+        // parse hex to unsigned byte
+        if (!MTS::Text::parseHex(iSpnPart, sPart)) {
+            printError("%s| Unexpected SIM EFspn content: [%s]", m_sName.c_str(), sEFspnContent.c_str());
+            return FAILURE;
+        }
+
+        // skip 0xFF bytes
+        if (iSpnPart == 0xFF) {
+            continue;
+        }
+
+        sCarrier.push_back(iSpnPart);
+    }
+
+    /**
+     * Example of radio response when SIM card does not contain the service provider name:
+     * Raw response from the radio: [
+     *   +CRSM: 144,0,FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF
+     *   OK
+     * ]
+     */
+    if (sCarrier.empty()) {
+        printError("%s| SIM EFspn does not contain the service provider name", m_sName.c_str());
+        return FAILURE;
     }
 
-    sCarrier = m_sCarrier;
     return SUCCESS;
 }
 
@@ -1663,3 +1749,9 @@ ICellularRadio::CODE CellularRadio::sendBasicQuery(const std::string& sCmd, cons
 
     return SUCCESS;
 }
+
+ICellularRadio::CODE CellularRadio::getSelectedBandsRaw(std::string& sRawBands)
+{
+    printTrace("%s| Acquiring selected bands: not applicable", m_sName.c_str());
+    return NOT_APPLICABLE;
+}
diff --git a/src/MTS_IO_LE910EUGRadio.cpp b/src/MTS_IO_LE910EUGRadio.cpp
index 9e970b3..1906815 100644
--- a/src/MTS_IO_LE910EUGRadio.cpp
+++ b/src/MTS_IO_LE910EUGRadio.cpp
@@ -27,7 +27,7 @@
  A more elaborate description
 */
 
-
+#include <mts/MTS_Logger.h>
 #include <mts/MTS_IO_LE910EUGRadio.h>
 
 using namespace MTS::IO;
diff --git a/src/MTS_IO_LE910NAGRadio.cpp b/src/MTS_IO_LE910NAGRadio.cpp
index 602466f..73a1d08 100644
--- a/src/MTS_IO_LE910NAGRadio.cpp
+++ b/src/MTS_IO_LE910NAGRadio.cpp
@@ -27,6 +27,7 @@
  A more elaborate description
 */
 
+#include <mts/MTS_Logger.h>
 #include <mts/MTS_IO_LE910NAGRadio.h>
 
 using namespace MTS::IO;
@@ -38,4 +39,3 @@ LE910NAGRadio::LE910NAGRadio(const std::string& sPort)
 {
 
 }
-
diff --git a/src/MTS_IO_LE910SVGRadio.cpp b/src/MTS_IO_LE910SVGRadio.cpp
index a43165a..431b9c2 100644
--- a/src/MTS_IO_LE910SVGRadio.cpp
+++ b/src/MTS_IO_LE910SVGRadio.cpp
@@ -27,6 +27,7 @@
  A more elaborate description
 */
 
+#include <mts/MTS_Logger.h>
 #include <mts/MTS_IO_LE910SVGRadio.h>
 
 using namespace MTS::IO;
diff --git a/src/MTS_IO_MccMncTable.cpp b/src/MTS_IO_MccMncTable.cpp
index 054a6fa..97f92ab 100644
--- a/src/MTS_IO_MccMncTable.cpp
+++ b/src/MTS_IO_MccMncTable.cpp
@@ -21,7 +21,7 @@
 /*!
  \file MTS_IO_MccMncTable.cpp
  \brief Auto-Generated MCC-MNC Lookup Table
- \date 2021-05-27
+ \date 2021-11-05
  \author sgodinez
 
  An Auto-Generated MCC-MNC Lookup Table
@@ -543,10 +543,10 @@ void MccMncTable::createTable() {
     m_mTable[663][47] = "me,Montenegro,382,Monet/T-mobile,";
     m_mTable[663][63] = "me,Montenegro,382,Mtel,";
     m_mTable[770][544] = "ca,Canada,1,Telus Mobility,";
-    m_mTable[770][800] = "ca,Canada,1,mobilicity,";
+    m_mTable[770][800] = "ca,Canada,1,Mobilicity,";
     m_mTable[770][864] = "ca,Canada,1,Clearnet,";
     m_mTable[770][865] = "ca,Canada,1,Clearnet,";
-    m_mTable[770][880] = "ca,Canada,1,FIDO (Rogers AT&T/ Microcell),";
+    m_mTable[770][880] = "ca,Canada,1,Fido,";
     m_mTable[770][896] = "ca,Canada,1,DMTS Mobility,";
     m_mTable[770][1168] = "ca,Canada,1,WIND,";
     m_mTable[770][1280] = "ca,Canada,1,Videotron,";
@@ -567,8 +567,10 @@ void MccMncTable::createTable() {
     m_mTable[770][1794] = "ca,Canada,1,MT&T Mobility,";
     m_mTable[770][1795] = "ca,Canada,1,New Tel Mobility,";
     m_mTable[770][1808] = "ca,Canada,1,Globalstar Canada,";
-    m_mTable[770][1824] = "ca,Canada,1,Rogers AT&T Wireless,";
+    m_mTable[770][1824] = "ca,Canada,1,Rogers Wireless,";
+    m_mTable[770][1825] = "ca,Canada,1,Rogers Wireless,";
     m_mTable[770][1888] = "ca,Canada,1,Public Mobile,";
+    m_mTable[770][2080] = "ca,Canada,1,Rogers Wireless,";
     m_mTable[776][31] = "pm,St. Pierre & Miquelon,508,Ameris,";
     m_mTable[784][3] = "us,United States,1,Unknown,";
     m_mTable[784][4] = "us,United States,1,Verizon Wireless,";
diff --git a/src/MTS_IO_QuectelRadio.cpp b/src/MTS_IO_QuectelRadio.cpp
index 7a1a1a7..b1e1e59 100644
--- a/src/MTS_IO_QuectelRadio.cpp
+++ b/src/MTS_IO_QuectelRadio.cpp
@@ -1468,3 +1468,63 @@ ICellularRadio::CODE QuectelRadio::getVoiceSupport(bool& bVoiceEnabled, bool& bS
 
     return SUCCESS;
 }
+
+ICellularRadio::CODE QuectelRadio::getSelectedBandsRaw(std::string& sRawBands) {
+    printTrace("%s| Acquiring selected bands", getName().c_str());
+    CODE rc;
+
+    const std::string sCommand = "AT+QCFG=\"band\"";
+    const std::string sLabel = "+QCFG: \"band\",";
+    const int dTimeout = 1000;
+    std::string sResult;
+
+    rc = sendBasicQuery(sCommand, sLabel, sResult, dTimeout);
+    if (rc != SUCCESS) {
+        return rc;
+    }
+
+    std::vector<std::string> vParts = MTS::Text::split(sResult, ',');
+    uint8_t iNumBandParams = 0;
+
+
+    if (vParts.size() > 0) {
+        uint16_t iSelectedBands = 0;
+        // Duplicate the <GW_band> value to the first two fields
+        // <GW_band> contains information for both the GSM and WCDMA bands
+        if (!isContainsSignChar(vParts[0]) && MTS::Text::parseHex(iSelectedBands, MTS::Text::trim(vParts[0])) ) {
+            sRawBands = MTS::Text::formatHex(iSelectedBands) + "," + MTS::Text::formatHex(iSelectedBands);
+            iNumBandParams++;
+        } else {
+            printWarning("%s| Error during parse number from string: [%s]. Assuming that no GSM and WCDMA bands selected", getName().c_str(), vParts[0].c_str());
+            sRawBands = "ffff,ffff";
+        }
+    } else {
+        sRawBands = "ffff,ffff";
+    }
+
+    if (vParts.size() > 1) {
+        uint64_t iSelectedBands = 0;
+        if (!isContainsSignChar(vParts[1]) && MTS::Text::parseHex(iSelectedBands, MTS::Text::trim(vParts[1]))) {
+            sRawBands += "," + MTS::Text::formatHex(iSelectedBands); // LTE bands
+            iNumBandParams++;
+        } else {
+            printWarning("%s| Error during parse number from string: [%s]. Assuming that no LTE bands selected", getName().c_str(), vParts[0].c_str());
+            sRawBands += ",ffffffffffffffff";
+        }
+    } else {
+        sRawBands += ",ffffffffffffffff";
+    }
+
+    // All other fragments - ignored for now.
+
+    // Return success if at least one band param was extracted; otherwise failure
+    return (iNumBandParams > 0) ? SUCCESS : FAILURE;
+}
+
+bool MTS::IO::QuectelRadio::isContainsSignChar(const std::string& str) {
+    if (str.find_first_of("+-") == std::string::npos) {
+        return false;
+    }
+
+    return true;
+}
diff --git a/src/MTS_IO_SerialConnection.cpp b/src/MTS_IO_SerialConnection.cpp
index 936d28d..d13c565 100644
--- a/src/MTS_IO_SerialConnection.cpp
+++ b/src/MTS_IO_SerialConnection.cpp
@@ -546,6 +546,10 @@ int SerialConnection::doRead(char* pBuffer, const uint32_t& iSize, int32_t& time
         } else {
             if (FD_ISSET(h, &readfs)) {
                 result = ::read(h, pBuffer, iSize);
+                if (result ==  0) {
+                    printTrace("SERIAL| serial port [%s] disconnected", m_sPortName.c_str());
+                    result = -1;
+                }
             } else {
                 //socket closed?
                 result = -1;
diff --git a/src/MTS_IO_TelitRadio.cpp b/src/MTS_IO_TelitRadio.cpp
index 1f5f4cf..5db2ddf 100644
--- a/src/MTS_IO_TelitRadio.cpp
+++ b/src/MTS_IO_TelitRadio.cpp
@@ -23,6 +23,7 @@
 #include <mts/MTS_Logger.h>
 #include <mts/MTS_Thread.h>
 #include <mts/MTS_Text.h>
+#include <mts/MTS_Timer.h>
 
 #include <unistd.h>
 
@@ -1013,8 +1014,11 @@ ICellularRadio::CODE TelitRadio::abortFotaWriteABD() {
 }
 
 ICellularRadio::CODE TelitRadio::fumoWaitUpgradeFinished(ICellularRadio::UpdateCb& stepCb) {
-    const uint32_t duAttachTimeout = 6 * 60 * 1000;// wait for 6 minutes for the radio to attach
-    const uint32_t duUrcTimeout = 60 * 1000;       // wait for 1 minutes for the next URC message
+    const uint32_t duFirstAttachTimeout = 6 * 60 * 1000; // wait for 6 minutes for the radio to attach
+    const uint32_t duSecondAttachTimeout = 60 * 1000;    // wait for 1 minute for the radio to attach
+    const uint32_t duThirdAttachTimeout = 5 * 1000;      // wait for 5 seconds for the radio to attach
+    const uint32_t duUrcTimeout = 60 * 1000;             // wait for 1 minutes for the next URC message
+    const uint32_t timeoutMillis = 30 * 60 * 1000;       // wait for 30 minutes in case if radio will send invalid message (garbage)
     const std::string sFotaUrcPrefix = "#OTAEV:";  // prefix for the URC notification messages
     const std::string sFotaUrcEndSuccess = "Module Upgraded To New Fw";
     const std::string sFotaUrcEndFailed = "OTA Fw Upgrade Failed";
@@ -1024,17 +1028,32 @@ ICellularRadio::CODE TelitRadio::fumoWaitUpgradeFinished(ICellularRadio::UpdateC
     std::string sResponse;
 
     // It's now detached. Try to reconnect
-    if (!resetConnection(duAttachTimeout)) {
-        printError("Can't connect to the radio in %d ms", (duAttachTimeout));
+    if (!resetConnection(duFirstAttachTimeout)) {
+        printError("Can't connect to the radio in %d ms", (duFirstAttachTimeout));
         callNextStep(stepCb, "FUMO Error: unable to obtain radio after reset");
         return ERROR;
     }
 
-    while (true) {
+    Timer timer;
+    timer.start();
 
-        sResponse = sendCommand("", vFotaBailStrings, duUrcTimeout, 0x00);
+    while (timer.getMillis() <= (uint64_t)timeoutMillis) {
+
+        sResponse = waitResponse(vFotaBailStrings, duUrcTimeout);
         printTrace("Radio response: [%s]", sResponse.c_str());
 
+        if (sResponse.empty()) {
+            // Radio detached again. Try to reconnect
+            if (!resetConnection(duSecondAttachTimeout)) {
+                printError("Can't connect to the radio in %d ms", (duSecondAttachTimeout));
+                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");
@@ -1058,6 +1077,13 @@ ICellularRadio::CODE TelitRadio::fumoWaitUpgradeFinished(ICellularRadio::UpdateC
 
     }
 
+    // Required to set echo to disable, so we performed resetConnection again.
+    if (!resetConnection(duThirdAttachTimeout)) {
+        printError("Can't connect to the radio in %d ms", (duThirdAttachTimeout));
+        callNextStep(stepCb, "FUMO Error: unable to reset connection to radio");
+        return ERROR;
+    }
+
     return rc;
 }
 
@@ -1083,3 +1109,73 @@ ICellularRadio::CODE TelitRadio::fumoCheckNewFirmware(ICellularRadio::UpdateCb&
 
     return rc;
 }
+
+ICellularRadio::CODE TelitRadio::getSelectedBandsRaw(std::string& sRawBands) {
+    printTrace("%s| Acquiring selected bands", getName().c_str());
+    CODE rc;
+
+    const std::string sCommand = "AT#BND?";
+    const std::string sLabel = "#BND: ";
+    const int dTimeout = 1000;
+    std::string sResult;
+
+    rc = sendBasicQuery(sCommand, sLabel, sResult, dTimeout);
+    if (rc != SUCCESS) {
+        return rc;
+    }
+
+    std::vector<std::string> vParts = MTS::Text::split(sResult, ',');
+    uint8_t iNumBandParams = 0;
+
+    if (vParts.size() > 0) {
+        uint16_t iSelectedBands = 0;
+        if (!isContainsSignChar(vParts[0]) && MTS::Text::parse(iSelectedBands, MTS::Text::trim(vParts[0]))) {
+            sRawBands = MTS::Text::formatHex(iSelectedBands); // GSM bands
+            iNumBandParams++;
+        } else {
+            printWarning("%s| Error during parse number from string: [%s]. Assuming that no GSM bands selected", getName().c_str(), vParts[0].c_str());
+            sRawBands = "ffff";
+        }
+    } else {
+        sRawBands = "ffff";
+    }
+
+    if (vParts.size() > 1) {
+        uint16_t iSelectedBands = 0;
+        if (!isContainsSignChar(vParts[1]) && MTS::Text::parse(iSelectedBands, MTS::Text::trim(vParts[1]))) {
+            sRawBands += "," + MTS::Text::formatHex(iSelectedBands); // WCDMA bands
+            iNumBandParams++;
+        } else {
+            printWarning("%s| Error during parse number from string: [%s]. Assuming that no WCDMA bands selected", getName().c_str(), vParts[0].c_str());
+            sRawBands += ",ffff";
+        }
+    } else {
+        sRawBands += ",ffff";
+    }
+
+    if (vParts.size() > 2) {
+        uint64_t iSelectedBands = 0;
+        if (!isContainsSignChar(vParts[2]) && MTS::Text::parseHex(iSelectedBands, MTS::Text::trim(vParts[2]))) {
+            sRawBands += "," + MTS::Text::formatHex(iSelectedBands); // LTE bands
+            iNumBandParams++;
+        } else {
+            printWarning("%s| Error during parse number from string: [%s]. Assuming that no LTE bands selected", getName().c_str(), vParts[0].c_str());
+            sRawBands += ",ffffffffffffffff";
+        }
+    } else {
+        sRawBands += ",ffffffffffffffff";
+    }
+
+    // All other fragments - ignored for now.
+
+    // Return success if at least one band param was extracted; otherwise failure
+    return (iNumBandParams > 0) ? SUCCESS : FAILURE;
+}
+
+bool MTS::IO::TelitRadio::isContainsSignChar(const std::string& str) {
+    if (str.find_first_of("+-") == std::string::npos) {
+        return false;
+    }
+
+    return true;
+}