You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

209 lines
4.2KB

  1. /* bits.c - bitfield extraction code
  2. *
  3. * This file is Copyright (c)2010-2018 by the GPSD project
  4. * SPDX-License-Identifier: BSD-2-clause
  5. *
  6. * Bitfield extraction functions. In each, start is a bit index - not
  7. * a byte index - and width is a bit width. The width is bounded above by
  8. * 64 bits.
  9. *
  10. * The sbits() function assumes twos-complement arithmetic. ubits()
  11. * and sbits() assume no padding in integers.
  12. */
  13. #include "gpsd_config.h" /* must be before all includes */
  14. #include <assert.h>
  15. #include <stdint.h>
  16. #include <stdbool.h>
  17. #include <limits.h>
  18. #include <string.h>
  19. #include "bits.h"
  20. uint64_t ubits(unsigned char buf[], unsigned int start, unsigned int width, bool le)
  21. /* extract a (zero-origin) bitfield from the buffer as an unsigned big-endian uint64_t */
  22. {
  23. uint64_t fld = 0;
  24. unsigned int i;
  25. unsigned end;
  26. assert(width <= sizeof(uint64_t) * CHAR_BIT);
  27. for (i = start / CHAR_BIT;
  28. i < (start + width + CHAR_BIT - 1) / CHAR_BIT; i++) {
  29. fld <<= CHAR_BIT;
  30. fld |= (unsigned char)buf[i];
  31. }
  32. end = (start + width) % CHAR_BIT;
  33. if (end != 0) {
  34. fld >>= (CHAR_BIT - end);
  35. }
  36. fld &= ~(~0ULL << width);
  37. /* was extraction as a little-endian requested? */
  38. if (le)
  39. {
  40. uint64_t reversed = 0;
  41. for (i = width; i; --i)
  42. {
  43. reversed <<= 1;
  44. if (fld & 1)
  45. reversed |= 1;
  46. fld >>= 1;
  47. }
  48. fld = reversed;
  49. }
  50. return fld;
  51. }
  52. int64_t sbits(signed char buf[], unsigned int start, unsigned int width, bool le)
  53. /* extract a bitfield from the buffer as a signed big-endian long */
  54. {
  55. uint64_t fld = ubits((unsigned char *)buf, start, width, le);
  56. /* ensure width > 0 as the result of
  57. 1ULL << (width - 1)
  58. is undefined for width <= 0 */
  59. assert(width > 0);
  60. if (fld & (1ULL << (width - 1))) {
  61. fld |= (~0ULL << (width - 1));
  62. }
  63. return (int64_t)fld;
  64. }
  65. union int_float {
  66. int32_t i;
  67. float f;
  68. };
  69. union long_double {
  70. int64_t l;
  71. double d;
  72. };
  73. float getlef32(const char *buf, int off)
  74. {
  75. union int_float i_f;
  76. i_f.i = getles32(buf, off);
  77. return i_f.f;
  78. }
  79. double getled64(const char *buf, int off)
  80. {
  81. union long_double l_d;
  82. l_d.l = getles64(buf, off);
  83. return l_d.d;
  84. }
  85. float getbef32(const char *buf, int off)
  86. {
  87. union int_float i_f;
  88. i_f.i = getbes32(buf, off);
  89. return i_f.f;
  90. }
  91. double getbed64(const char *buf, int off)
  92. {
  93. union long_double l_d;
  94. l_d.l = getbes64(buf, off);
  95. return l_d.d;
  96. }
  97. void putbef32(char *buf, int off, float val)
  98. {
  99. union int_float i_f;
  100. i_f.f = val;
  101. putbe32(buf, off, i_f.i);
  102. }
  103. void shiftleft(unsigned char *data, int size, unsigned short left)
  104. {
  105. unsigned char *byte;
  106. if (left >= CHAR_BIT) {
  107. size -= left/CHAR_BIT;
  108. memmove(data, data + left/CHAR_BIT, (size + CHAR_BIT - 1)/CHAR_BIT);
  109. left %= CHAR_BIT;
  110. }
  111. for (byte = data; size--; ++byte )
  112. {
  113. unsigned char bits;
  114. if (size)
  115. bits = byte[1] >> (CHAR_BIT - left);
  116. else
  117. bits = 0;
  118. *byte <<= left;
  119. *byte |= bits;
  120. }
  121. }
  122. #ifdef __UNUSED__
  123. void putbed64(char *buf, int off, double val)
  124. {
  125. union long_double l_d;
  126. l_d.d = val;
  127. putbe32(buf, (off), (l_d.l) >> 32);
  128. putbe32(buf, (off)+4, (l_d.l));
  129. }
  130. u_int16_t swap_u16(u_int16_t i)
  131. /* byte-swap a 16-bit unsigned int */
  132. {
  133. u_int8_t c1, c2;
  134. c1 = i & 255;
  135. c2 = (i >> 8) & 255;
  136. return (c1 << 8) + c2;
  137. }
  138. u_int32_t swap_u32(u_int32_t i)
  139. /* byte-swap a 32-bit unsigned int */
  140. {
  141. u_int8_t c1, c2, c3, c4;
  142. c1 = i & 255;
  143. c2 = (i >> 8) & 255;
  144. c3 = (i >> 16) & 255;
  145. c4 = (i >> 24) & 255;
  146. return ((u_int32_t)c1 << 24) + ((u_int32_t)c2 << 16) + ((u_int32_t)c3 << 8) + c4;
  147. }
  148. u_int64_t swap_u64(u_int64_t i)
  149. /* byte-swap a 64-bit unsigned int */
  150. {
  151. u_int8_t c1, c2, c3, c4, c5, c6, c7, c8;
  152. c1 = i & 255;
  153. c2 = (i >> 8) & 255;
  154. c3 = (i >> 16) & 255;
  155. c4 = (i >> 24) & 255;
  156. c5 = (i >> 32) & 255;
  157. c6 = (i >> 40) & 255;
  158. c7 = (i >> 48) & 255;
  159. c8 = (i >> 56) & 255;
  160. return ((u_int64_t)c1 << 56) +
  161. ((u_int64_t)c2 << 48) +
  162. ((u_int64_t)c3 << 40) +
  163. ((u_int64_t)c4 << 32) +
  164. ((u_int64_t)c5 << 24) +
  165. ((u_int64_t)c6 << 16) +
  166. ((u_int64_t)c7 << 8) +
  167. c8;
  168. }
  169. #endif /* __UNUSED__ */