3 files changed, 39 insertions, 27 deletions

diff --git a/src/MTS_IO_CellularRadio.cpp b/src/MTS_IO_CellularRadio.cpp
index 3f0c037..e171341 100644
--- a/src/MTS_IO_CellularRadio.cpp
+++ b/src/MTS_IO_CellularRadio.cpp
@@ -770,6 +770,34 @@ ICellularRadio::CODE CellularRadio::getRegistration(REGISTRATION& eRegistration)
     return SUCCESS;
 }
 
+ICellularRadio::CODE CellularRadio::getCellularMode(CELLULAR_MODES &networks) {
+    networks = CELLULAR_MODE_NA;
+    std::string cmdResult = sendCommand("AT+COPS?");
+    if (cmdResult.find(ICellularRadio::RSP_OK) == std::string::npos) {
+        printDebug("%s| AT+COPS returned unexpected response: AT+COPS? [%s]", getName().c_str(), cmdResult.c_str());
+        return FAILURE;
+    }
+
+    size_t cursor = 0;
+    const std::vector<std::string> &reply = MTS::Text::split(MTS::Text::getLine(MTS::Text::trim(cmdResult), cursor, cursor), ',');
+    uint8_t op;
+    if (reply.size() < 4 || !MTS::Text::parse(op, reply[3])) {
+        printDebug("Error parsing reply from AT+COPS");
+        return FAILURE;
+    }
+    if (op == 0) {
+        networks = CELLULAR_MODE_2G;
+    } else if (op >= 2 && op <= 6) {
+        networks = CELLULAR_MODE_3G;
+    } else if (op == 7) {
+        networks = CELLULAR_MODE_4G;
+    } else {
+        printDebug("Error: AT+COPS unknown Radio Access Technology");
+        return FAILURE;
+    }
+    return SUCCESS;
+}
+
 ICellularRadio::CODE CellularRadio::convertRegistrationToString(REGISTRATION eRegistration, std::string& sRegistration) {
 
     ICellularRadio::CODE eCode = FAILURE;
diff --git a/src/MTS_IO_EG95Radio.cpp b/src/MTS_IO_EG95Radio.cpp
index 501a164..f934e50 100644
--- a/src/MTS_IO_EG95Radio.cpp
+++ b/src/MTS_IO_EG95Radio.cpp
@@ -24,6 +24,7 @@
 #include <climits>
 
 #include <mts/MTS_Logger.h>
+#include <mts/MTS_Text.h>
 
 using namespace MTS::IO;
 
@@ -55,27 +56,27 @@ ICellularRadio::CODE EG95Radio::getSupportedCellularModes(CELLULAR_MODES &networ
     return SUCCESS;
 }
 
-ICellularRadio::CODE EG95Radio::getCellularMode(CELLULAR_MODES&) {
-    return FAILURE;
-}
-
 ICellularRadio::CODE EG95Radio::setCellularMode(CELLULAR_MODES networks) {
     std::string prefNet;
+    unsigned int prefOnly = 0;
     for (int i = sizeof(networks)*CHAR_BIT-1; i>=0; --i){
         switch (1<<i & networks) {
-        case ICellularRadio::CELLULAR_MODE_2G: prefNet += "01"; break;
-        case ICellularRadio::CELLULAR_MODE_3G: prefNet += "0302"; break;
-        case ICellularRadio::CELLULAR_MODE_4G: prefNet += "04"; break;
+        case ICellularRadio::CELLULAR_MODE_2G: prefNet += "01"  ; prefOnly = 1; break;
+        case ICellularRadio::CELLULAR_MODE_3G: prefNet += "0302"; prefOnly = 2; break;
+        case ICellularRadio::CELLULAR_MODE_4G: prefNet += "04"  ; prefOnly = 3; break;
         default: printError("Unhandled preferred network flag.");
         }
     }
-    std::string sCmd("AT+QCFG=\"nwscanseq\",");
-    sCmd += prefNet;
+    std::string sCmd;
+    if (prefNet.size() == 2) { // *g-only
+        sCmd = "AT+QCFG=\"nwscanmode\"," + std::to_string(prefOnly);
+    } else { // preferred
+        sCmd = "AT+QCFG=\"nwscanseq\"," + prefNet;
+    }
     std::string cmdResult = sendCommand(sCmd);
     if (cmdResult.find(ICellularRadio::RSP_OK) == std::string::npos) {
         printDebug("%s| AT+QCFG returned unexpected response: [%s][%s]", getName().c_str(), sCmd.c_str(), cmdResult.c_str());
         return FAILURE;
     }
-
     return SUCCESS;
 }
diff --git a/src/MTS_IO_TelitRadio.cpp b/src/MTS_IO_TelitRadio.cpp
index c04b47a..7e74ca6 100644
--- a/src/MTS_IO_TelitRadio.cpp
+++ b/src/MTS_IO_TelitRadio.cpp
@@ -682,23 +682,6 @@ ICellularRadio::CODE TelitRadio::getSupportedCellularModes(CELLULAR_MODES &netwo
     return SUCCESS;
 }
 
-ICellularRadio::CODE TelitRadio::getCellularMode(CELLULAR_MODES &networks) {
-    networks = CELLULAR_MODE_NA;
-    std::string sCmd("AT+WS46?");
-    std::string cmdResult = sendCommand(sCmd);
-    if (cmdResult.find("+WS46:") == std::string::npos) {
-        printDebug("%s| AT+WS46? returned unexpected response: [%s][%s]", getName().c_str(), sCmd.c_str(), cmdResult.c_str());
-        return FAILURE;
-    }
-    int wds;
-    size_t cursor;
-    if (MTS::Text::parse(wds, MTS::Text::trim(MTS::Text::getLine(MTS::Text::split(cmdResult, ':')[1], 0, cursor)))) {
-        networks = preferredNetwork(networks, wds);
-        return SUCCESS;
-    }
-    return FAILURE;
-}
-
 ICellularRadio::CODE TelitRadio::setCellularMode(CELLULAR_MODES networks) {
     int wds = 0;
     // 3GPP TS 27.007