diff --git a/src/core/libs/supl/asn-rrlp/INTEGER.c b/src/core/libs/supl/asn-rrlp/INTEGER.c index fbf768898..b73c06ad9 100644 --- a/src/core/libs/supl/asn-rrlp/INTEGER.c +++ b/src/core/libs/supl/asn-rrlp/INTEGER.c @@ -137,7 +137,7 @@ INTEGER__dump(asn_TYPE_descriptor_t *td, const INTEGER_t *st, asn_app_consume_by } else { accum = (*buf & 0x80) ? -1 : 0; for(; buf < buf_end; buf++) - accum = (accum << 8) | *buf; + accum = (accum * 256) | *buf; } el = INTEGER_map_value2enum(specs, accum); @@ -545,7 +545,7 @@ INTEGER_encode_xer(asn_TYPE_descriptor_t *td, void *sptr, (void)ilevel; (void)flags; - + if(!st || !st->buf) _ASN_ENCODE_FAILED; @@ -818,7 +818,7 @@ asn_INTEGER2long(const INTEGER_t *iptr, long *lptr) { /* Conversion engine */ for(; b < end; b++) - l = (l << 8) | *b; + l = (l * 256) | *b; *lptr = l; return 0; diff --git a/src/core/libs/supl/asn-rrlp/per_support.c b/src/core/libs/supl/asn-rrlp/per_support.c index f5322d195..4bb83ab3b 100644 --- a/src/core/libs/supl/asn-rrlp/per_support.c +++ b/src/core/libs/supl/asn-rrlp/per_support.c @@ -96,7 +96,8 @@ per_get_few_bits(asn_per_data_t *pd, int nbits) { /* The number of available bits in the stream allow * for the following operations to take place without * invoking the ->refill() function */ - accum = per_get_few_bits(&tpd, nbits - 24) << 24; + uint32_t two_twentyfour = 16777216; + accum = per_get_few_bits(&tpd, nbits - 24) * two_twentyfour; accum |= per_get_few_bits(&tpd, 24); } else { per_get_undo(pd, nbits); @@ -424,8 +425,11 @@ per_put_few_bits(asn_per_outp_t *po, uint32_t bits, int obits) { buf[2] = bits >> 8, buf[3] = bits; else { - if(per_put_few_bits(po, bits >> (obits - 24), 24)) return -1; - if(per_put_few_bits(po, bits, obits - 24)) return -1; + if((obits - 24) > 0) + { + if(per_put_few_bits(po, bits >> (obits - 24), 24)) return -1; + } + if(per_put_few_bits(po, bits, obits - 24)) return -1; } ASN_DEBUG("[PER out %u/%x => %02x buf+%ld]", diff --git a/src/core/libs/supl/asn-supl/INTEGER.c b/src/core/libs/supl/asn-supl/INTEGER.c index fbf768898..b73c06ad9 100644 --- a/src/core/libs/supl/asn-supl/INTEGER.c +++ b/src/core/libs/supl/asn-supl/INTEGER.c @@ -137,7 +137,7 @@ INTEGER__dump(asn_TYPE_descriptor_t *td, const INTEGER_t *st, asn_app_consume_by } else { accum = (*buf & 0x80) ? -1 : 0; for(; buf < buf_end; buf++) - accum = (accum << 8) | *buf; + accum = (accum * 256) | *buf; } el = INTEGER_map_value2enum(specs, accum); @@ -545,7 +545,7 @@ INTEGER_encode_xer(asn_TYPE_descriptor_t *td, void *sptr, (void)ilevel; (void)flags; - + if(!st || !st->buf) _ASN_ENCODE_FAILED; @@ -818,7 +818,7 @@ asn_INTEGER2long(const INTEGER_t *iptr, long *lptr) { /* Conversion engine */ for(; b < end; b++) - l = (l << 8) | *b; + l = (l * 256) | *b; *lptr = l; return 0; diff --git a/src/core/libs/supl/asn-supl/per_support.c b/src/core/libs/supl/asn-supl/per_support.c index f5322d195..7f0f3d2ca 100644 --- a/src/core/libs/supl/asn-supl/per_support.c +++ b/src/core/libs/supl/asn-supl/per_support.c @@ -96,7 +96,8 @@ per_get_few_bits(asn_per_data_t *pd, int nbits) { /* The number of available bits in the stream allow * for the following operations to take place without * invoking the ->refill() function */ - accum = per_get_few_bits(&tpd, nbits - 24) << 24; + uint32_t two_twentyfour = 16777216; + accum = per_get_few_bits(&tpd, nbits - 24) * two_twentyfour; accum |= per_get_few_bits(&tpd, 24); } else { per_get_undo(pd, nbits); @@ -424,7 +425,10 @@ per_put_few_bits(asn_per_outp_t *po, uint32_t bits, int obits) { buf[2] = bits >> 8, buf[3] = bits; else { - if(per_put_few_bits(po, bits >> (obits - 24), 24)) return -1; + if((obits - 24) > 0) + { + if(per_put_few_bits(po, bits >> (obits - 24), 24)) return -1; + } if(per_put_few_bits(po, bits, obits - 24)) return -1; } diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.cc b/src/core/system_parameters/beidou_dnav_navigation_message.cc index 4cb83cd1c..2a4476ea5 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.cc +++ b/src/core/system_parameters/beidou_dnav_navigation_message.cc @@ -34,6 +34,7 @@ #include "gnss_satellite.h" #include // for cos, sin, fmod, sqrt, atan2, fabs, floor #include // for string, operator<<, cout, ostream, endl +#include // for std::numeric_limits void Beidou_Dnav_Navigation_Message::reset() @@ -273,55 +274,30 @@ int64_t Beidou_Dnav_Navigation_Message::read_navigation_signed( { int64_t value = 0; int32_t num_of_slices = parameter.size(); - // Discriminate between 64 bits and 32 bits compiler - int32_t long_int_size_bytes = sizeof(int64_t); - if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system - { - // read the MSB and perform the sign extension - if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) - { - value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable - } - else - { - value &= 0; - } - for (int32_t i = 0; i < num_of_slices; i++) - { - for (int32_t j = 0; j < parameter[i].second; j++) - { - value <<= 1; //shift left - value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable) - if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[i].first - j] == 1) - { - value += 1; // insert the bit - } - } - } + // read the MSB and perform the sign extension + if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) + { + value ^= 0xFFFFFFFFFFFFFFFF; // 64 bits variable } - else // we assume we are in a 32 bits system + else { - // read the MSB and perform the sign extension - if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) + value &= 0; + } + // Avoid saturation when decoding + if (value == std::numeric_limits::max() or value == std::numeric_limits::min()) + { + value /= 2; + } + for (int32_t i = 0; i < num_of_slices; i++) + { + for (int32_t j = 0; j < parameter[i].second; j++) { - value ^= 0xFFFFFFFF; - } - else - { - value &= 0; - } - - for (int32_t i = 0; i < num_of_slices; i++) - { - for (int32_t j = 0; j < parameter[i].second; j++) + value *= 2; // shift left the signed integer + value &= 0xFFFFFFFFFFFFFFFE; // reset the corresponding bit (for the 64 bits variable) + if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[i].first - j] == 1) { - value <<= 1; //shift left - value &= 0xFFFFFFFE; //reset the corresponding bit - if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[i].first - j] == 1) - { - value += 1; // insert the bit - } + value += 1; // insert the bit } } } diff --git a/src/core/system_parameters/galileo_navigation_message.cc b/src/core/system_parameters/galileo_navigation_message.cc index c405e6de9..0d5cc6a4b 100644 --- a/src/core/system_parameters/galileo_navigation_message.cc +++ b/src/core/system_parameters/galileo_navigation_message.cc @@ -36,6 +36,7 @@ #include // for DLOG #include // for reverse #include // for operator<< +#include // for std::numeric_limits using CRC_Galileo_INAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; @@ -310,12 +311,16 @@ int64_t Galileo_Navigation_Message::read_navigation_signed(std::bitset::max() or value == std::numeric_limits::min()) + { + value /= 2; + } for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) { - value <<= 1; // shift left + value *= 2; // shift left the signed integer value &= 0xFFFFFFFFFFFFFFFE; // reset the corresponding bit (for the 64 bits variable) if (static_cast(bits[GALILEO_DATA_JK_BITS - parameter[i].first - j]) == 1) { diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 07c20a0b9..94656179d 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -32,6 +32,7 @@ #include "gps_cnav_navigation_message.h" #include "gnss_satellite.h" +#include // for std::numeric_limits void Gps_CNAV_Navigation_Message::reset() @@ -121,12 +122,16 @@ int64_t Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset::max() or value == std::numeric_limits::min()) + { + value /= 2; + } for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) { - value <<= 1; // shift left + value *= 2; // shift left the signed integer value &= 0xFFFFFFFFFFFFFFFELL; // reset the corresponding bit (for the 64 bits variable) if (static_cast(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[i].first - j]) == 1) { diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index d0d394e9a..936c5131f 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -35,6 +35,7 @@ m * \file gps_navigation_message.cc #include // for fmod, abs, floor #include // for memcpy #include // for operator<<, cout, endl +#include // for std::numeric_limits void Gps_Navigation_Message::reset() @@ -211,12 +212,16 @@ int64_t Gps_Navigation_Message::read_navigation_signed(std::bitset::max() or value == std::numeric_limits::min()) + { + value /= 2; + } for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) { - value <<= 1; // shift left + value *= 2; // shift left the signed integer value &= 0xFFFFFFFFFFFFFFFELL; // reset the corresponding bit (for the 64 bits variable) if (static_cast(bits[GPS_SUBFRAME_BITS - parameter[i].first - j]) == 1) {