ProtoCore v0.0.2
Deterministic, zero-heap network stack for embedded targets
Loading...
Searching...
No Matches
j2735.cpp
Go to the documentation of this file.
1// Copyright (C) 2026 Douglas Quigg (dstroy0) <dquigg123@gmail.com>
2// SPDX-License-Identifier: AGPL-3.0-or-later
3
4/**
5 * @file j2735.cpp
6 * @brief SAE J2735 V2X UPER codec + BSMcore (see j2735.h).
7 */
8
9#include "services/transportation/j2735/j2735.h"
10
11#if PC_ENABLE_J2735
12
13void pc_uper_writer_init(UperWriter *w, uint8_t *buf, size_t cap)
14{
15 w->buf = buf;
16 w->cap = cap;
17 w->bit_pos = 0;
18 w->ok = (buf != nullptr && cap > 0);
19 // Zero the buffer so we can OR bits in - only when it is a real buffer (a null buf leaves ok=false).
20 if (buf)
21 {
22 for (size_t i = 0; i < cap; i++)
23 {
24 buf[i] = 0;
25 }
26 }
27}
28
29size_t pc_uper_writer_finish(UperWriter *w)
30{
31 if (!w->ok)
32 {
33 return 0;
34 }
35 return (w->bit_pos + 7) / 8;
36}
37
38void pc_uper_put_bits(UperWriter *w, uint32_t value, unsigned nbits)
39{
40 if (!w->ok || nbits == 0)
41 {
42 return;
43 }
44 if (w->bit_pos + nbits > w->cap * 8)
45 {
46 w->ok = false;
47 return;
48 }
49 for (unsigned i = 0; i < nbits; i++)
50 {
51 unsigned bit = (value >> (nbits - 1 - i)) & 1u;
52 if (bit)
53 {
54 size_t p = w->bit_pos + i;
55 w->buf[p / 8] |= (uint8_t)(0x80u >> (p % 8));
56 }
57 }
58 w->bit_pos += nbits;
59}
60
61void pc_uper_put_bool(UperWriter *w, bool v)
62{
63 pc_uper_put_bits(w, v ? 1 : 0, 1);
64}
65
66unsigned pc_uper_cint_bits(int64_t lo, int64_t hi)
67{
68 if (hi <= lo)
69 {
70 return 0; // single value: encoded in zero bits
71 }
72 uint64_t range = (uint64_t)(hi - lo); // number of steps above lo is 0..range
73 unsigned bits = 0;
74 while (((uint64_t)1 << bits) <= range)
75 {
76 bits++;
77 }
78 return bits;
79}
80
81void pc_uper_put_cint(UperWriter *w, int64_t value, int64_t lo, int64_t hi)
82{
83 unsigned bits = pc_uper_cint_bits(lo, hi);
84 if (bits == 0)
85 {
86 return;
87 }
88 uint64_t off = (uint64_t)(value - lo);
89 // Values wider than 32 bits are not used by the J2735 fields here, but support up to 32.
90 pc_uper_put_bits(w, (uint32_t)off, bits);
91}
92
93void pc_uper_reader_init(UperReader *r, const uint8_t *buf, size_t nbits)
94{
95 r->buf = buf;
96 r->nbits = nbits;
97 r->bit_pos = 0;
98 r->ok = (buf != nullptr);
99}
100
101uint32_t pc_uper_get_bits(UperReader *r, unsigned nbits)
102{
103 if (!r->ok || nbits == 0)
104 {
105 return 0;
106 }
107 if (r->bit_pos + nbits > r->nbits)
108 {
109 r->ok = false;
110 return 0;
111 }
112 uint32_t v = 0;
113 for (unsigned i = 0; i < nbits; i++)
114 {
115 size_t p = r->bit_pos + i;
116 unsigned bit = (r->buf[p / 8] >> (7 - (p % 8))) & 1u;
117 v = (v << 1) | bit;
118 }
119 r->bit_pos += nbits;
120 return v;
121}
122
123bool pc_uper_get_bool(UperReader *r)
124{
125 return pc_uper_get_bits(r, 1) != 0;
126}
127
128int64_t pc_uper_get_cint(UperReader *r, int64_t lo, int64_t hi)
129{
130 unsigned bits = pc_uper_cint_bits(lo, hi);
131 if (bits == 0)
132 {
133 return lo;
134 }
135 uint32_t off = pc_uper_get_bits(r, bits);
136 return lo + (int64_t)off;
137}
138
139size_t pc_j2735_bsm_core_encode(const J2735BsmCore *c, uint8_t *out, size_t cap)
140{
141 if (!c || !out)
142 {
143 return 0;
144 }
145 UperWriter w;
146 pc_uper_writer_init(&w, out, cap);
147 pc_uper_put_cint(&w, c->msg_count, 0, 127);
148 pc_uper_put_bits(&w, c->id, 32);
149 pc_uper_put_cint(&w, c->sec_mark, 0, 65535);
150 pc_uper_put_cint(&w, c->lat, -900000000, 900000001);
151 pc_uper_put_cint(&w, c->lon, -1799999999, 1800000001);
152 pc_uper_put_cint(&w, c->elev, -4096, 61439);
153 pc_uper_put_cint(&w, c->speed, 0, 8191);
154 pc_uper_put_cint(&w, c->heading, 0, 28800);
155 return pc_uper_writer_finish(&w);
156}
157
158bool pc_j2735_bsm_core_decode(const uint8_t *in, size_t len, J2735BsmCore *c)
159{
160 if (!in || !c)
161 {
162 return false;
163 }
164 UperReader r;
165 pc_uper_reader_init(&r, in, len * 8);
166 c->msg_count = (uint8_t)pc_uper_get_cint(&r, 0, 127);
167 c->id = pc_uper_get_bits(&r, 32);
168 c->sec_mark = (uint16_t)pc_uper_get_cint(&r, 0, 65535);
169 c->lat = (int32_t)pc_uper_get_cint(&r, -900000000, 900000001);
170 c->lon = (int32_t)pc_uper_get_cint(&r, -1799999999, 1800000001);
171 c->elev = (int32_t)pc_uper_get_cint(&r, -4096, 61439);
172 c->speed = (uint16_t)pc_uper_get_cint(&r, 0, 8191);
173 c->heading = (uint16_t)pc_uper_get_cint(&r, 0, 28800);
174 return r.ok;
175}
176
177size_t pc_j2735_spat_encode(const J2735MovementState *states, size_t count, uint8_t *out, size_t cap)
178{
179 if (!out || (count && !states) || count > 31)
180 {
181 return 0;
182 }
183 UperWriter w;
184 pc_uper_writer_init(&w, out, cap);
185 pc_uper_put_cint(&w, (int64_t)count, 0, 31);
186 for (size_t i = 0; i < count; i++)
187 {
188 pc_uper_put_cint(&w, states[i].signal_group, 0, 255);
189 pc_uper_put_cint(&w, states[i].phase, 0, 9);
190 pc_uper_put_cint(&w, states[i].min_end_time, 0, 36000);
191 pc_uper_put_cint(&w, states[i].max_end_time, 0, 36000);
192 }
193 return pc_uper_writer_finish(&w);
194}
195
196bool pc_j2735_spat_decode(const uint8_t *in, size_t len, J2735MovementState *out_states, size_t max_states,
197 size_t *out_count)
198{
199 if (!in || !out_states || !out_count)
200 {
201 return false;
202 }
203 UperReader r;
204 pc_uper_reader_init(&r, in, len * 8);
205 size_t count = (size_t)pc_uper_get_cint(&r, 0, 31);
206 if (!r.ok || count > max_states)
207 {
208 return false;
209 }
210 for (size_t i = 0; i < count; i++)
211 {
212 out_states[i].signal_group = (uint8_t)pc_uper_get_cint(&r, 0, 255);
213 out_states[i].phase = (uint8_t)pc_uper_get_cint(&r, 0, 9);
214 out_states[i].min_end_time = (uint16_t)pc_uper_get_cint(&r, 0, 36000);
215 out_states[i].max_end_time = (uint16_t)pc_uper_get_cint(&r, 0, 36000);
216 }
217 if (!r.ok)
218 {
219 return false;
220 }
221 *out_count = count;
222 return true;
223}
224
225size_t pc_j2735_map_encode(const J2735MapIntersection *isect, const J2735Lane *lanes, size_t count, uint8_t *out,
226 size_t cap)
227{
228 if (!isect || !out || (count && !lanes) || count > 31)
229 {
230 return 0;
231 }
232 UperWriter w;
233 pc_uper_writer_init(&w, out, cap);
234 pc_uper_put_cint(&w, isect->intersection_id, 0, 65535);
235 pc_uper_put_cint(&w, isect->ref_lat, 0, 65535);
236 pc_uper_put_cint(&w, isect->ref_lon, 0, 65535);
237 pc_uper_put_cint(&w, (int64_t)count, 0, 31);
238 for (size_t i = 0; i < count; i++)
239 {
240 pc_uper_put_cint(&w, lanes[i].lane_id, 0, 255);
241 pc_uper_put_bool(&w, lanes[i].is_ingress);
242 pc_uper_put_cint(&w, lanes[i].node_x, -2048, 2047);
243 pc_uper_put_cint(&w, lanes[i].node_y, -2048, 2047);
244 }
245 return pc_uper_writer_finish(&w);
246}
247
248bool pc_j2735_map_decode(const uint8_t *in, size_t len, J2735MapIntersection *isect, J2735Lane *out_lanes,
249 size_t max_lanes, size_t *out_count)
250{
251 if (!in || !isect || !out_lanes || !out_count)
252 {
253 return false;
254 }
255 UperReader r;
256 pc_uper_reader_init(&r, in, len * 8);
257 isect->intersection_id = (uint16_t)pc_uper_get_cint(&r, 0, 65535);
258 isect->ref_lat = (uint16_t)pc_uper_get_cint(&r, 0, 65535);
259 isect->ref_lon = (uint16_t)pc_uper_get_cint(&r, 0, 65535);
260 size_t count = (size_t)pc_uper_get_cint(&r, 0, 31);
261 if (!r.ok || count > max_lanes)
262 {
263 return false;
264 }
265 for (size_t i = 0; i < count; i++)
266 {
267 out_lanes[i].lane_id = (uint8_t)pc_uper_get_cint(&r, 0, 255);
268 out_lanes[i].is_ingress = pc_uper_get_bool(&r);
269 out_lanes[i].node_x = (int16_t)pc_uper_get_cint(&r, -2048, 2047);
270 out_lanes[i].node_y = (int16_t)pc_uper_get_cint(&r, -2048, 2047);
271 }
272 if (!r.ok)
273 {
274 return false;
275 }
276 *out_count = count;
277 return true;
278}
279
280#endif // PC_ENABLE_J2735
j2735.h
SAE J2735 V2X - ASN.1 UPER primitive codec + Basic Safety Message core (PC_ENABLE_J2735).