From 035937ea7eb454fa455dbf30e5a3f18f324cc4c6 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Thu, 19 May 2016 15:38:11 -0300 Subject: [PATCH] AP_GPS: add some safety checks to _parse_decimal_100() --- libraries/AP_GPS/AP_GPS_NMEA.cpp | 37 +++++++++++++++----------------- 1 file changed, 17 insertions(+), 20 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 614170a832..5036cf5a42 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -187,32 +187,29 @@ int16_t AP_GPS_NMEA::_from_hex(char a) int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p) { - int32_t ret = 100UL * atol(p); - int32_t sign = 1; + char *endptr = nullptr; + long ret = 100 * strtol(p, &endptr, 10); + int sign = ret < 0 ? -1 : 1; - if (*p == '+') { - ++p; - } else if (*p == '-') { - ++p; - sign = -1; + if (ret >= (long)INT32_MAX) { + return INT32_MAX; + } + if (ret <= (long)INT32_MIN) { + return INT32_MIN; + } + if (endptr == nullptr || *endptr != '.') { + return ret; } - while (isdigit(*p)) { - ++p; - } - - if (*p == '.') { - if (isdigit(p[1])) { - ret += sign * 10 * DIGIT_TO_VAL(p[1]); - if (isdigit(p[2])) { - ret += sign * DIGIT_TO_VAL(p[2]); - if (isdigit(p[3])) { - ret += sign * (DIGIT_TO_VAL(p[3]) >= 5); - } + if (isdigit(endptr[1])) { + ret += sign * 10 * DIGIT_TO_VAL(endptr[1]); + if (isdigit(endptr[2])) { + ret += sign * DIGIT_TO_VAL(endptr[2]); + if (isdigit(endptr[3])) { + ret += sign * (DIGIT_TO_VAL(endptr[3]) >= 5); } } } - return ret; }