Line data    Source code

       1             : /* Copyright (c) 2012 Josh Triplett <[email protected]>
       2             :  *
       3             :  * Permission is hereby granted, free of charge, to any person obtaining a copy
       4             :  * of this software and associated documentation files (the "Software"), to
       5             :  * deal in the Software without restriction, including without limitation the
       6             :  * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
       7             :  * sell copies of the Software, and to permit persons to whom the Software is
       8             :  * furnished to do so, subject to the following conditions:
       9             :  *
      10             :  * The above copyright notice and this permission notice shall be included in
      11             :  * all copies or substantial portions of the Software.
      12             :  *
      13             :  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
      14             :  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
      15             :  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
      16             :  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
      17             :  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
      18             :  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
      19             :  * IN THE SOFTWARE.
      20             :  */
      21             : 
      22             : // Reference: https://linux.die.net/man/3/be16toh
      23             : 
      24             : #pragma once
      25             : 
      26             : #include <AP_Common/AP_Common.h>
      27             : #include <AP_Math/AP_Math.h>
      28             : #include <byteswap.h>
      29             : #include <endian.h>
      30             : #include <stdint.h>
      31             : 
      32             : #ifdef __CHECKER__
      33             : #define __ap_bitwise __attribute__((bitwise))
      34             : #define __ap_force __attribute__((force))
      35             : #else
      36             : #define __ap_bitwise
      37             : #define __ap_force
      38             : #endif
      39             : 
      40             : typedef uint16_t __ap_bitwise le16_t;
      41             : typedef uint16_t __ap_bitwise be16_t;
      42             : typedef uint32_t __ap_bitwise le32_t;
      43             : typedef uint32_t __ap_bitwise be32_t;
      44             : typedef uint64_t __ap_bitwise le64_t;
      45             : typedef uint64_t __ap_bitwise be64_t;
      46             : 
      47             : #undef htobe16
      48             : #undef htole16
      49             : #undef be16toh
      50             : #undef le16toh
      51             : #undef htobe32
      52             : #undef htole32
      53             : #undef be32toh
      54             : #undef le32toh
      55             : #undef htobe64
      56             : #undef htole64
      57             : #undef be64toh
      58             : #undef le64toh
      59             : 
      60             : #if !defined (__BYTE_ORDER) && defined (__OpenBSD__)
      61             : #define __BYTE_ORDER __BYTE_ORDER__
      62             : #define __LITTLE_ENDIAN __ORDER_LITTLE_ENDIAN__
      63             : #define __BIG_ENDIAN __ORDER_BIG_ENDIAN__
      64             : #endif
      65             : 
      66             : #if __BYTE_ORDER == __LITTLE_ENDIAN
      67             : #define bswap_16_on_le(x) __bswap_16(x)
      68             : #define bswap_32_on_le(x) __bswap_32(x)
      69             : #define bswap_64_on_le(x) __bswap_64(x)
      70             : #define bswap_16_on_be(x) (x)
      71             : #define bswap_32_on_be(x) (x)
      72             : #define bswap_64_on_be(x) (x)
      73             : #elif __BYTE_ORDER == __BIG_ENDIAN
      74             : #define bswap_16_on_le(x) (x)
      75             : #define bswap_32_on_le(x) (x)
      76             : #define bswap_64_on_le(x) (x)
      77             : #define bswap_16_on_be(x) __bswap_16(x)
      78             : #define bswap_32_on_be(x) __bswap_32(x)
      79             : #define bswap_64_on_be(x) __bswap_64(x)
      80             : #endif
      81             : 
      82   536628626 : static inline le16_t htole16(uint16_t value) { return (le16_t __ap_force) bswap_16_on_be(value); }
      83           0 : static inline le32_t htole32(uint32_t value) { return (le32_t __ap_force) bswap_32_on_be(value); }
      84             : static inline le64_t htole64(uint64_t value) { return (le64_t __ap_force) bswap_64_on_be(value); }
      85             : 
      86  3941838582 : static inline be16_t htobe16(uint16_t value) { return (be16_t __ap_force) bswap_16_on_le(value); }
      87        7340 : static inline be32_t htobe32(uint32_t value) { return (be32_t __ap_force) bswap_32_on_le(value); }
      88         183 : static inline be64_t htobe64(uint64_t value) { return (be64_t __ap_force) bswap_64_on_le(value); }
      89             : 
      90       17754 : static inline uint16_t le16toh(le16_t value) { return bswap_16_on_be((uint16_t __ap_force)value); }
      91             : static inline uint32_t le32toh(le32_t value) { return bswap_32_on_be((uint32_t __ap_force)value); }
      92             : static inline uint64_t le64toh(le64_t value) { return bswap_64_on_be((uint64_t __ap_force)value); }
      93             : 
      94      207626 : static inline uint16_t be16toh(be16_t value) { return bswap_16_on_le((uint16_t __ap_force)value); }
      95         235 : static inline uint32_t be32toh(be32_t value) { return bswap_32_on_le((uint32_t __ap_force)value); }
      96             : static inline uint64_t be64toh(be64_t value) { return bswap_64_on_le((uint64_t __ap_force)value); }
      97             : 
      98             : // methods for misaligned buffers
      99       35498 : static inline uint16_t le16toh_ptr(const uint8_t *p) { return p[0] | (p[1]<<8U); }
     100           0 : static inline uint32_t le24toh_ptr(const uint8_t *p) { return p[0] | (p[1]<<8U) | (p[2]<<16U); }
     101       13848 : static inline uint32_t le32toh_ptr(const uint8_t *p) { return p[0] | (p[1]<<8U) | (p[2]<<16U) | (p[3]<<24U); }
     102           2 : static inline uint64_t le64toh_ptr(const uint8_t *p) { return (uint64_t) p[0] | ((uint64_t) p[1]<<8ULL) | ((uint64_t) p[2]<<16U) | ((uint64_t) p[3]<<24U) | ((uint64_t) p[4]<<32U) | ((uint64_t) p[5]<<40U) | ((uint64_t) p[6]<<48U) | ((uint64_t) p[7]<<56U); }
     103      333331 : static inline uint16_t be16toh_ptr(const uint8_t *p) { return p[1] | (p[0]<<8U); }
     104             : static inline uint32_t be24toh_ptr(const uint8_t *p) { return p[2] | (p[1]<<8U) | (p[0]<<16U); }
     105      741360 : static inline uint32_t be32toh_ptr(const uint8_t *p) { return p[3] | (p[2]<<8U) | (p[1]<<16U) | (p[0]<<24U); }
     106       70239 : static inline uint64_t be64toh_ptr(const uint8_t *p) { return (uint64_t) p[7] | ((uint64_t) p[6]<<8ULL) | ((uint64_t) p[5]<<16U) | ((uint64_t) p[4]<<24U) | ((uint64_t) p[3]<<32U) | ((uint64_t) p[2]<<40U) | ((uint64_t) p[1]<<48U) | ((uint64_t) p[0]<<56U); }
     107             : 
     108      313106 : static inline float be32tofloat_ptr(const uint8_t *p) { return int32_to_float_le(be32toh_ptr(p)); }
     109      313106 : static inline float be32tofloat_ptr(const uint8_t *p, const uint8_t offset) { return be32tofloat_ptr(&p[offset]); }
     110             : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
     111       70239 : static inline double be64todouble_ptr(const uint8_t *p) { return uint64_to_double_le(be64toh_ptr(p)); }
     112       70239 : static inline double be64todouble_ptr(const uint8_t *p, const uint8_t offset) { return be64todouble_ptr(&p[offset]); }
     113             : #endif  // AP_MATH_ALLOW_DOUBLE_FUNCTIONS
     114             : 
     115         771 : static inline void put_le16_ptr(uint8_t *p, uint16_t v) { p[0] = (v&0xFF); p[1] = (v>>8); }
     116           0 : static inline void put_le24_ptr(uint8_t *p, uint32_t v) { p[0] = (v&0xFF); p[1] = (v>>8)&0xFF; p[2] = (v>>16)&0xFF; }
     117        9072 : static inline void put_le32_ptr(uint8_t *p, uint32_t v) { p[0] = (v&0xFF); p[1] = (v>>8)&0xFF; p[2] = (v>>16)&0xFF; p[3] = (v>>24)&0xFF; }
     118           8 : static inline void put_le64_ptr(uint8_t *p, uint64_t v) { p[0] = (v&0xFF); p[1] = (v>>8)&0xFF; p[2] = (v>>16)&0xFF; p[3] = (v>>24)&0xFF; p[4] = (v>>32)&0xFF;p[5] = (v>>40)&0xFF; p[6] = (v>>48)&0xFF;p[7] = (v>>56)&0xFF;}
     119      396755 : static inline void put_be16_ptr(uint8_t *p, uint16_t v) { p[1] = (v&0xFF); p[0] = (v>>8); }
     120             : static inline void put_be24_ptr(uint8_t *p, uint32_t v) { p[2] = (v&0xFF); p[1] = (v>>8)&0xFF; p[0] = (v>>16)&0xFF; }
     121      824346 : static inline void put_be32_ptr(uint8_t *p, uint32_t v) { p[3] = (v&0xFF); p[2] = (v>>8)&0xFF; p[1] = (v>>16)&0xFF; p[0] = (v>>24)&0xFF; }
     122       79542 : static inline void put_be64_ptr(uint8_t *p, uint64_t v) { p[7] = (v&0xFF); p[6] = (v>>8)&0xFF; p[5] = (v>>16)&0xFF; p[4] = (v>>24)&0xFF; p[3] = (v>>32)&0xFF;p[2] = (v>>40)&0xFF; p[1] = (v>>48)&0xFF;p[0] = (v>>56)&0xFF;}
     123             : 
     124             : #undef __ap_bitwise
     125             : #undef __ap_force