MRTKLIB 0.7.14
Modernized GNSS Positioning Library
Loading...
Searching...
No Matches
mrtk_bits.c
Go to the documentation of this file.
1/*------------------------------------------------------------------------------
2 * mrtk_bits.c : bit manipulation, CRC, and word decode functions
3 *
4 * Copyright (C) 2026 H.SHIONO (MRTKLIB Project)
5 * Copyright (C) 2023-2025 Cabinet Office, Japan
6 * Copyright (C) 2024-2025 Lighthouse Technology & Consulting Co. Ltd.
7 * Copyright (C) 2023-2025 Japan Aerospace Exploration Agency
8 * Copyright (C) 2023-2025 TOSHIBA ELECTRONIC TECHNOLOGIES CORPORATION
9 * Copyright (C) 2015- Mitsubishi Electric Corp.
10 * Copyright (C) 2014 Geospatial Information Authority of Japan
11 * Copyright (C) 2014 T.SUZUKI
12 * Copyright (C) 2007-2023 T.TAKASU
13 *
14 * SPDX-License-Identifier: BSD-2-Clause
15 *----------------------------------------------------------------------------*/
16/**
17 * @file mrtk_bits.c
18 * @brief MRTKLIB Bits Module — Implementation.
19 *
20 * All functions and static data in this file are cut-and-paste extractions
21 * from the legacy rtkcmn.c with zero algorithmic changes.
22 */
23
24#include "mrtklib/mrtk_bits.h"
25
26#include <stdint.h>
27
28#include "mrtklib/mrtk_trace.h"
29
30/*============================================================================
31 * Forward Declarations (resolved at link time from rtkcmn.c)
32 *===========================================================================*/
33
34/*============================================================================
35 * Private Constants
36 *===========================================================================*/
37
38#define POLYCRC32 0xEDB88320u /* CRC32 polynomial */
39#define POLYCRC24Q 0x1864CFBu /* CRC24Q polynomial */
40
41/* crc tables generated by util/gencrc */
42static const uint16_t tbl_CRC16[] = {
43 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD,
44 0xE1CE, 0xF1EF, 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B, 0xA35A,
45 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B,
46 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
47 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861,
48 0x2802, 0x3823, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96,
49 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, 0x6CA6, 0x7C87,
50 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
51 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A,
52 0x9F59, 0x8F78, 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3,
53 0x5004, 0x4025, 0x7046, 0x6067, 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290,
54 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
55 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E,
56 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, 0xD94C, 0xC96D, 0xF90E, 0xE92F,
57 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, 0xCB7D, 0xDB5C,
58 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
59 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83,
60 0x1CE0, 0x0CC1, 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74,
61 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0};
62static const uint32_t tbl_CRC24Q[] = {
63 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, 0xA18139, 0x27CDC2, 0x2B5434,
64 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493,
65 0xDC7D65, 0x5A319E, 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, 0x0CD1E9,
66 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926,
67 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C,
68 0x56E077, 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, 0x19A3D2, 0x9FEF29,
69 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A,
70 0xAD88F1, 0xA11107, 0x275DFC, 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
71 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, 0x15723B, 0x933EC0, 0x9FA736,
72 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918,
73 0xADC0EE, 0x2B8C15, 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, 0x71BD8B,
74 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52,
75 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571,
76 0x0DB98A, 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, 0x578814, 0xD1C4EF,
77 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC,
78 0x2A3C57, 0x26A5A1, 0xA0E95A, 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863,
79 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, 0x5B59FD, 0xDD1506, 0xD18CF0,
80 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C,
81 0x33D79A, 0xB59B61, 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, 0xEFAAFF,
82 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230,
83 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673,
84 0xB94A88, 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, 0xE37B16, 0x6537ED,
85 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE,
86 0x575035, 0x5BC9C3, 0xDD8538};
87
88/*============================================================================
89 * Bit Extraction / Insertion Functions
90 *===========================================================================*/
91
92/* get unsigned bits -----------------------------------------------------------
93 * get unsigned/signed bits from byte data
94 * args : uint8_t *buff I byte data
95 * int pos I bit position from start of data (bits)
96 * int len I bit length (bits) (len<=32)
97 * return : extracted unsigned/signed bits
98 *-----------------------------------------------------------------------------*/
99extern uint32_t getbitu(const uint8_t* buff, int pos, int len) {
100 uint32_t bits = 0;
101 int i;
102 for (i = pos; i < pos + len; i++) {
103 bits = (bits << 1) + ((buff[i / 8] >> (7 - i % 8)) & 1u);
104 }
105 return bits;
106}
107extern int32_t getbits(const uint8_t* buff, int pos, int len) {
108 uint32_t bits = getbitu(buff, pos, len);
109 if (len <= 0 || 32 <= len || !(bits & (1u << (len - 1)))) {
110 return (int32_t)bits;
111 }
112 return (int32_t)(bits | (~0u << len)); /* extend sign */
113}
114/* set unsigned/signed bits ----------------------------------------------------
115 * set unsigned/signed bits to byte data
116 * args : uint8_t *buff IO byte data
117 * int pos I bit position from start of data (bits)
118 * int len I bit length (bits) (len<=32)
119 * [u]int32_t data I unsigned/signed data
120 * return : none
121 *-----------------------------------------------------------------------------*/
122extern void setbitu(uint8_t* buff, int pos, int len, uint32_t data) {
123 uint32_t mask = 1u << (len - 1);
124 int i;
125 if (len <= 0 || 32 < len) {
126 return;
127 }
128 for (i = pos; i < pos + len; i++, mask >>= 1) {
129 if (data & mask) {
130 buff[i / 8] |= 1u << (7 - i % 8);
131 } else {
132 buff[i / 8] &= ~(1u << (7 - i % 8));
133 }
134 }
135}
136extern void setbits(uint8_t* buff, int pos, int len, int32_t data) {
137 if (data < 0) {
138 data |= 1 << (len - 1);
139 } else {
140 data &= ~(1 << (len - 1)); /* set sign bit */
141 }
142 setbitu(buff, pos, len, (uint32_t)data);
143}
144
145/*============================================================================
146 * CRC Parity Functions
147 *===========================================================================*/
148
149/* crc-32 parity ---------------------------------------------------------------
150 * compute crc-32 parity for novatel raw
151 * args : uint8_t *buff I data
152 * int len I data length (bytes)
153 * return : crc-32 parity
154 * notes : see NovAtel OEMV firmware manual 1.7 32-bit CRC
155 *-----------------------------------------------------------------------------*/
156extern uint32_t rtk_crc32(const uint8_t* buff, int len) {
157 uint32_t crc = 0;
158 int i, j;
159
160 trace(NULL, 4, "rtk_crc32: len=%d\n", len);
161
162 for (i = 0; i < len; i++) {
163 crc ^= buff[i];
164 for (j = 0; j < 8; j++) {
165 if (crc & 1) {
166 crc = (crc >> 1) ^ POLYCRC32;
167 } else {
168 crc >>= 1;
169 }
170 }
171 }
172 return crc;
173}
174/* crc-24q parity --------------------------------------------------------------
175 * compute crc-24q parity for sbas, rtcm3
176 * args : uint8_t *buff I data
177 * int len I data length (bytes)
178 * return : crc-24Q parity
179 * notes : see reference [2] A.4.3.3 Parity
180 *-----------------------------------------------------------------------------*/
181extern uint32_t rtk_crc24q(const uint8_t* buff, int len) {
182 uint32_t crc = 0;
183 int i;
184
185 trace(NULL, 4, "rtk_crc24q: len=%d\n", len);
186
187 for (i = 0; i < len; i++) {
188 crc = ((crc << 8) & 0xFFFFFF) ^ tbl_CRC24Q[(crc >> 16) ^ buff[i]];
189 }
190 return crc;
191}
192/* crc-16 parity ---------------------------------------------------------------
193 * compute crc-16 parity for binex, nvs
194 * args : uint8_t *buff I data
195 * int len I data length (bytes)
196 * return : crc-16 parity
197 * notes : see reference [10] A.3.
198 *-----------------------------------------------------------------------------*/
199extern uint16_t rtk_crc16(const uint8_t* buff, int len) {
200 uint16_t crc = 0;
201 int i;
202
203 trace(NULL, 4, "rtk_crc16: len=%d\n", len);
204
205 for (i = 0; i < len; i++) {
206 crc = (crc << 8) ^ tbl_CRC16[((crc >> 8) ^ buff[i]) & 0xFF];
207 }
208 return crc;
209}
210
211/*============================================================================
212 * Navigation Data Word Decoding
213 *===========================================================================*/
214
215/* decode navigation data word -------------------------------------------------
216 * check party and decode navigation data word
217 * args : uint32_t word I navigation data word (2+30bit)
218 * (previous word D29*-30* + current word D1-30)
219 * uint8_t *data O decoded navigation data without parity
220 * (8bitx3)
221 * return : status (1:ok,0:parity error)
222 * notes : see reference [1] 20.3.5.2 user parity algorithm
223 *-----------------------------------------------------------------------------*/
224extern int decode_word(uint32_t word, uint8_t* data) {
225 const uint32_t hamming[] = {0xBB1F3480, 0x5D8F9A40, 0xAEC7CD00, 0x5763E680, 0x6BB1F340, 0x8B7A89C0};
226 uint32_t parity = 0, w;
227 int i;
228
229 trace(NULL, 5, "decodeword: word=%08x\n", word);
230
231 if (word & 0x40000000) {
232 word ^= 0x3FFFFFC0;
233 }
234
235 for (i = 0; i < 6; i++) {
236 parity <<= 1;
237 for (w = (word & hamming[i]) >> 6; w; w >>= 1) {
238 parity ^= w & 1;
239 }
240 }
241 if (parity != (word & 0x3F)) {
242 return 0;
243 }
244
245 for (i = 0; i < 3; i++) {
246 data[i] = (uint8_t)(word >> (22 - i * 8));
247 }
248 return 1;
249}
tbl_CRC24Q
static const uint32_t tbl_CRC24Q[]
Definition mrtk_bits.c:62
tbl_CRC16
static const uint16_t tbl_CRC16[]
Definition mrtk_bits.c:42
rtk_crc24q
uint32_t rtk_crc24q(const uint8_t *buff, int len)
Compute CRC-24Q parity for SBAS and RTCM3.
Definition mrtk_bits.c:181
rtk_crc16
uint16_t rtk_crc16(const uint8_t *buff, int len)
Compute CRC-16 parity for BINEX and NVS.
Definition mrtk_bits.c:199
setbitu
void setbitu(uint8_t *buff, int pos, int len, uint32_t data)
Set unsigned bits in byte data.
Definition mrtk_bits.c:122
getbitu
uint32_t getbitu(const uint8_t *buff, int pos, int len)
Extract unsigned bits from byte data.
Definition mrtk_bits.c:99
getbits
int32_t getbits(const uint8_t *buff, int pos, int len)
Extract signed bits from byte data.
Definition mrtk_bits.c:107
setbits
void setbits(uint8_t *buff, int pos, int len, int32_t data)
Set signed bits in byte data.
Definition mrtk_bits.c:136
rtk_crc32
uint32_t rtk_crc32(const uint8_t *buff, int len)
Compute CRC-32 parity for NovAtel raw data.
Definition mrtk_bits.c:156
POLYCRC32
#define POLYCRC32
Definition mrtk_bits.c:38
decode_word
int decode_word(uint32_t word, uint8_t *data)
Check parity and decode GPS navigation data word.
Definition mrtk_bits.c:224
mrtk_bits.h
MRTKLIB Bits Module — Binary bit operations and CRC functions.
trace
void trace(mrtk_ctx_t *ctx, int level, const char *format,...)
Output trace message.
Definition mrtk_trace.c:78
mrtk_trace.h
MRTKLIB Trace Module — debug trace and logging functions.