Fix for BUG #469: Bad/Empty GPS NMEA strings returned when the queries are made in a random wait iterative fashion

Fix for BUG #460:  X300: GPGGA sensor most often empty, while RMC is usually OK
- Added checksum verification of NMEA strings
- Improved handling of short or malformed strings
- Fixed GPSDO data synchronization between X300 firmware and host

3 files changed, 119 insertions, 24 deletions

diff --git a/firmware/x300/x300/x300_main.c b/firmware/x300/x300/x300_main.c
index d7fd32ac3..8717bf690 100644
--- a/firmware/x300/x300/x300_main.c
+++ b/firmware/x300/x300/x300_main.c
@@ -345,12 +345,12 @@ static void handle_uarts(void)
     static uint32_t rxoffset = 0;
     for (int rxch = wb_uart_getc(UART0_BASE); rxch != -1; rxch = wb_uart_getc(UART0_BASE))
     {
-        rxoffset = (rxoffset+1) % (NUM_POOL_WORDS32*4);
+        rxoffset++;
         const int shift = ((rxoffset%4) * 8);
         static uint32_t rxword32 = 0;
         if (shift == 0) rxword32 = 0;
-        rxword32 |= ((uint32_t) rxch) << ((rxoffset%4) * 8);
-        rxpool[rxoffset/4] = rxword32;
+        rxword32 |= ((uint32_t) rxch & 0xFF) << shift;
+        rxpool[(rxoffset/4) % NUM_POOL_WORDS32] = rxword32;
         shmem[X300_FW_SHMEM_UART_RX_INDEX] = rxoffset;
     }
 
@@ -445,17 +445,24 @@ int main(void)
         static const uint32_t tick_delta = CPU_CLOCK/1000;
         if (ticks_passed > tick_delta)
         {
-            handle_link_state(); //deal with router table update
+//            handle_link_state(); //deal with router table update
             handle_claim(); //deal with the host claim register
+handle_uarts(); //udp_uart_poll();
             update_leds(); //run the link and activity leds
+handle_uarts(); //udp_uart_poll();
             garp(); //send periodic garps
+handle_uarts(); //udp_uart_poll();
             xge_poll_sfpp_status(0); // Every so often poll XGE Phy to look for SFP+ hotplug events.
+handle_uarts(); //udp_uart_poll();
             xge_poll_sfpp_status(1); // Every so often poll XGE Phy to look for SFP+ hotplug events.
+handle_uarts(); //udp_uart_poll();
             last_cronjob = wb_peek32(SR_ADDR(RB0_BASE, RB_COUNTER));
         }
+//handle_uarts(); //udp_uart_poll();
 
         //run the network stack - poll and handle
         u3_net_stack_handle_one();
+handle_uarts(); //udp_uart_poll();
 
         //run the PCIe listener - poll and fwd to wishbone
         forward_pcie_user_xact_to_wb();
diff --git a/host/lib/usrp/gps_ctrl.cpp b/host/lib/usrp/gps_ctrl.cpp
index d327a84f9..b211ba027 100644
--- a/host/lib/usrp/gps_ctrl.cpp
+++ b/host/lib/usrp/gps_ctrl.cpp
@@ -62,6 +62,20 @@ private:
     return std::string();
   }
 
+    static bool is_nmea_checksum_ok(std::string nmea)
+    {
+        if (nmea.length() < 5 || nmea[0] != '$' || nmea[nmea.length()-3] != '*')
+            return false;
+
+        uint8_t string_crc = uint8_t(strtol(nmea.substr(nmea.length()-2, 2).c_str(), NULL, 16));
+        uint8_t calculated_crc = 0;
+
+        for (size_t i = 1; i < nmea.length()-3; i++)
+            calculated_crc ^= nmea[i];
+
+        return (string_crc == calculated_crc);
+    }
+
   std::string update_cached_sensors(const std::string sensor) {
     if(not gps_detected() || (gps_type != GPS_TYPE_INTERNAL_GPSDO)) {
         UHD_MSG(error) << "get_stat(): unsupported GPS or no GPS detected";
@@ -70,24 +84,40 @@ private:
 
     const std::list<std::string> list = boost::assign::list_of("GPGGA")("GPRMC")("SERVO");
     static const boost::regex status_regex("\\d\\d-\\d\\d-\\d\\d");
+    static const boost::regex gp_msg_regex("^\\$GP.*,\\*[0-9A-F]{2}$");
     std::map<std::string,std::string> msgs;
 
     // Get all GPSDO messages available
     // Creating a map here because we only want the latest of each message type
     for (std::string msg = _recv(); msg.length(); msg = _recv())
     {
-        if (msg.length() < 6)
-            continue;
-
         // Strip any end of line characters
         erase_all(msg, "\r");
         erase_all(msg, "\n");
 
+        if (msg.length() < 6)
+        {
+#ifdef  DEBUG_GPS
+            UHD_MSG(error) << __FUNCTION__ << ": Short NMEA string: " << msg << std::endl;
+#endif
+            continue;
+        }
+
         // Look for SERVO message
         if (boost::regex_search(msg, status_regex, boost::regex_constants::match_continuous))
+        {
             msgs["SERVO"] = msg;
-        else
+        }
+        else if (boost::regex_match(msg, gp_msg_regex) && is_nmea_checksum_ok(msg))
+        {
             msgs[msg.substr(1,5)] = msg;
+        }
+#ifdef  DEBUG_GPS
+        else
+        {
+            UHD_MSG(error) << __FUNCTION__ << ": Malformed NMEA string: " << msg << std::endl;
+        }
+#endif
     }
 
     boost::system_time time = boost::get_system_time();
@@ -131,7 +161,6 @@ public:
       }
       else if(reply.substr(0, 3) == "$GP") i_heard_some_nmea = true; //but keep looking for that "Command Error" response
       else if(reply.length() != 0) i_heard_something_weird = true; //probably wrong baud rate
-      sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
     }
 
     if((i_heard_some_nmea) && (gps_type != GPS_TYPE_INTERNAL_GPSDO)) gps_type = GPS_TYPE_GENERIC_NMEA;
diff --git a/host/lib/usrp/x300/x300_fw_uart.cpp b/host/lib/usrp/x300/x300_fw_uart.cpp
index 943b2d9fa..4bf103c5a 100644
--- a/host/lib/usrp/x300/x300_fw_uart.cpp
+++ b/host/lib/usrp/x300/x300_fw_uart.cpp
@@ -38,6 +38,8 @@ struct x300_uart_iface : uart_iface
         rxpool = _iface->peek32(SR_ADDR(X300_FW_SHMEM_BASE, X300_FW_SHMEM_UART_RX_ADDR));
         txpool = _iface->peek32(SR_ADDR(X300_FW_SHMEM_BASE, X300_FW_SHMEM_UART_TX_ADDR));
         poolsize = _iface->peek32(SR_ADDR(X300_FW_SHMEM_BASE, X300_FW_SHMEM_UART_WORDS32));
+        _rxcache.resize(poolsize);
+        _last_device_rxoffset = rxoffset;
         //this->write_uart("HELLO UART\n");
         //this->read_uart(0.1);
     }
@@ -63,39 +65,96 @@ struct x300_uart_iface : uart_iface
 
     int getchar(void)
     {
-        if (_iface->peek32(SR_ADDR(X300_FW_SHMEM_BASE, X300_FW_SHMEM_UART_RX_INDEX)) != rxoffset)
+        if (rxoffset == _last_device_rxoffset)
+            return -1;
+
+        rxoffset++;
+        return int(_rxcache[((rxoffset)/4) % poolsize] >> ((rxoffset%4)*8) & 0xFF);
+    }
+
+    void update_cache(void)
+    {
+        uint32_t device_rxoffset = _iface->peek32(SR_ADDR(X300_FW_SHMEM_BASE, X300_FW_SHMEM_UART_RX_INDEX));
+        uint32_t delta = device_rxoffset - rxoffset;
+
+        while (delta)
         {
-            const int shift = ((rxoffset%4) * 8);
-            const char ch = _iface->peek32(SR_ADDR(rxpool, rxoffset/4)) >> shift;
-            rxoffset = (rxoffset + 1) % (poolsize*4);
-            return ch;
+            if (delta >= poolsize*4)
+            {
+                // all the data is new - reload the entire cache
+                for (uint32_t i = 0; i < poolsize; i++)
+                    _rxcache[i] = _iface->peek32(SR_ADDR(rxpool, i));
+
+                // set rxoffset to the end of the first string
+                rxoffset = device_rxoffset - (poolsize*4) + 1;
+                while (uint8_t(_rxcache[(rxoffset/4) % poolsize] >> ((rxoffset % 4) * 8)) != '\n')
+                    ++rxoffset;
+
+                // clear the partial string in the buffer;
+                _rxbuff.clear();
+            }
+            else if (rxoffset == _last_device_rxoffset)
+            {
+                // new data was added - refresh the portion of the cache that was updated
+                for (uint32_t i = ((_last_device_rxoffset+1)/4) % poolsize; i != (((device_rxoffset)/4)+1) % poolsize; i = (i+1) % poolsize)
+                {
+                    _rxcache[i] = _iface->peek32(SR_ADDR(rxpool, i));
+                }
+            } else {
+                // there is new data, but we aren't done with what we have - check back later
+                break;
+            }
+
+            _last_device_rxoffset = device_rxoffset;
+
+            // check again to see if anything changed while we were updating the cache
+            device_rxoffset = _iface->peek32(SR_ADDR(X300_FW_SHMEM_BASE, X300_FW_SHMEM_UART_RX_INDEX));
+            delta = device_rxoffset - rxoffset;
         }
-        return -1;
     }
 
     std::string read_uart(double timeout)
     {
         const boost::system_time exit_time = boost::get_system_time() + boost::posix_time::microseconds(long(timeout*1e6));
         std::string buff;
+
         while (true)
         {
-            const int ch = this->getchar();
-            if (ch == -1)
+            // Update cache
+            this->update_cache();
+
+            // Get available characters
+            for (int ch = this->getchar(); ch != -1; ch = this->getchar())
             {
-                if (boost::get_system_time() > exit_time) break;
-                boost::this_thread::sleep(boost::posix_time::milliseconds(1));
-                continue;
+                // skip carriage returns
+                if (ch == '\r')
+                    continue;
+
+                // store character to buffer
+                _rxbuff += std::string(1, (char)ch);
+
+                // newline found - return string
+                if (ch == '\n')
+                {
+                    buff = _rxbuff;
+                    _rxbuff.clear();
+                    return buff;
+                }
             }
-            if (ch == '\r') continue;
-            buff += std::string(1, (char)ch);
-            if (ch == '\n') break;
+
+            // no more characters - check time
+            if (boost::get_system_time() > exit_time)
+                break;
         }
-        //UHD_VAR(buff);
+
         return buff;
     }
 
     wb_iface::sptr _iface;
     boost::uint32_t rxoffset, txoffset, txword32, rxpool, txpool, poolsize;
+    boost::uint32_t _last_device_rxoffset;
+    std::vector<uint32_t> _rxcache;
+    std::string _rxbuff;
 };
 
 uart_iface::sptr x300_make_uart_iface(wb_iface::sptr iface)