Browse Source

AP_GPS: add some safety checks to _parse_decimal_100()

mission-4.1.18
Lucas De Marchi 9 years ago
parent
commit
035937ea7e
  1. 35
      libraries/AP_GPS/AP_GPS_NMEA.cpp

35
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 AP_GPS_NMEA::_parse_decimal_100(const char *p)
{ {
int32_t ret = 100UL * atol(p); char *endptr = nullptr;
int32_t sign = 1; long ret = 100 * strtol(p, &endptr, 10);
int sign = ret < 0 ? -1 : 1;
if (*p == '+') { if (ret >= (long)INT32_MAX) {
++p; return INT32_MAX;
} else if (*p == '-') {
++p;
sign = -1;
} }
if (ret <= (long)INT32_MIN) {
while (isdigit(*p)) { return INT32_MIN;
++p;
} }
if (endptr == nullptr || *endptr != '.') {
if (*p == '.') { return ret;
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; return ret;
} }

Loading…
Cancel
Save