Fix Galileo toggling (betaflight#13384)

* Fix Galileo toggling * try something else * cleanup * Fix per review from ledvinap * Prevent buffer overflow on invalid numConfigBlocks
HThuren · Feb 27, 2024 · 37d27ad · 37d27ad
1 parent 79d031b
commit 37d27ad
Showing 1 changed file with 25 additions and 28 deletions.
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
@@ -173,6 +173,14 @@ typedef enum {
 #define UBLOX_GNSS_ENABLE     0x1
 #define UBLOX_GNSS_DEFAULT_SIGCFGMASK 0x10000
 
+#define UBLOX_GNSS_GPS        0x00
+#define UBLOX_GNSS_SBAS       0x01
+#define UBLOX_GNSS_GALILEO    0x02
+#define UBLOX_GNSS_BEIDOU     0x03
+#define UBLOX_GNSS_IMES       0x04
+#define UBLOX_GNSS_QZSS       0x05
+#define UBLOX_GNSS_GLONASS    0x06
+
 typedef struct ubxHeader_s {
     uint8_t preamble1;
     uint8_t preamble2;
@@ -2283,38 +2291,27 @@ static bool UBLOX_parse_gps(void)
         break;
     case CLSMSG(CLASS_CFG, MSG_CFG_GNSS):
         {
-            bool isSBASenabled = false;
-            bool isM8NwithDefaultConfig = false;
-
-            if ((ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks >= 2) &&
-                (ubxRcvMsgPayload.ubxCfgGnss.configblocks[1].gnssId == 1) && //SBAS
-                (ubxRcvMsgPayload.ubxCfgGnss.configblocks[1].flags & UBLOX_GNSS_ENABLE)) { //enabled
-
-                isSBASenabled = true;
-            }
-
-            if ((ubxRcvMsgPayload.ubxCfgGnss.numTrkChHw == 32) &&  //M8N
-                (ubxRcvMsgPayload.ubxCfgGnss.numTrkChUse == 32) &&
-                (ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks == 7) &&
-                (ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].gnssId == 2) && //Galileo
-                (ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].resTrkCh == 4) && //min channels
-                (ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].maxTrkCh == 8) && //max channels
-                !(ubxRcvMsgPayload.ubxCfgGnss.configblocks[2].flags & UBLOX_GNSS_ENABLE)) { //disabled
-
-                isM8NwithDefaultConfig = true;
-            }
-
             const uint16_t messageSize = 4 + (ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks * sizeof(ubxConfigblock_t));
-
             ubxMessage_t tx_buffer;
-            memcpy(&tx_buffer.payload, &ubxRcvMsgPayload, messageSize);
 
-            if (isSBASenabled && (gpsConfig()->sbasMode == SBAS_NONE)) {
-                tx_buffer.payload.cfg_gnss.configblocks[1].flags &= ~UBLOX_GNSS_ENABLE; //Disable SBAS
-            }
+            // prevent buffer overflow on invalid numConfigBlocks
+            const int size = MIN(messageSize, sizeof(tx_buffer.payload));
+            memcpy(&tx_buffer.payload, &ubxRcvMsgPayload, size);
+
+            for (int i = 0; i < ubxRcvMsgPayload.ubxCfgGnss.numConfigBlocks; i++) {
+                if (ubxRcvMsgPayload.ubxCfgGnss.configblocks[i].gnssId == UBLOX_GNSS_SBAS) {
+                    if (gpsConfig()->sbasMode == SBAS_NONE) {
+                        tx_buffer.payload.cfg_gnss.configblocks[i].flags &= ~UBLOX_GNSS_ENABLE; // Disable SBAS
+                    }
+                }
 
-            if (isM8NwithDefaultConfig && gpsConfig()->gps_ublox_use_galileo) {
-                tx_buffer.payload.cfg_gnss.configblocks[2].flags |= UBLOX_GNSS_ENABLE; //Enable Galileo
+                if (ubxRcvMsgPayload.ubxCfgGnss.configblocks[i].gnssId == UBLOX_GNSS_GALILEO) {
+                    if (gpsConfig()->gps_ublox_use_galileo) {
+                        tx_buffer.payload.cfg_gnss.configblocks[i].flags |= UBLOX_GNSS_ENABLE; // Enable Galileo
+                    } else {
+                        tx_buffer.payload.cfg_gnss.configblocks[i].flags &= ~UBLOX_GNSS_ENABLE; // Disable Galileo
+                    }
+                }
             }
 
             ubloxSendConfigMessage(&tx_buffer, MSG_CFG_GNSS, messageSize, false);