matrixfurry.com
The open source OpenXR runtime
1// Copyright 2025, rcelyte
2// SPDX-License-Identifier: BSL-1.0
3
4#include "protocol.h"
5
6#include "math/m_api.h"
7
8#include <endian.h>
9#include <string.h>
10
11typedef int32_t flatbuffers_soffset_t; // little-endian byte order
12typedef uint16_t flatbuffers_voffset_t; // little-endian byte order
13typedef FLATBUFFERS_VECTOR(void) flatbuffers_vector_t;
14
15struct flatbuffers_vtable_t
16{
17 flatbuffers_voffset_t vtable_size;
18 flatbuffers_voffset_t table_size;
19 flatbuffers_voffset_t offsets[];
20};
21
22struct table_data
23{
24 uint16_t length;
25 const uint8_t *data;
26};
27
28static const void *
29read_flatbuffers_uoffset(const uint8_t buffer[const],
30 const size_t buffer_len,
31 const flatbuffers_uoffset_t *const ref,
32 const size_t size)
33{
34 assert((const uint8_t *)ref >= buffer && (const uint8_t *)ref <= &buffer[buffer_len - sizeof(*ref)]);
35 const uint32_t offset = le32toh(*ref);
36 const size_t capacity = &buffer[buffer_len] - (const uint8_t *)ref;
37 if (capacity < size || offset == 0 || offset > capacity - size || (offset % sizeof(uint32_t)) != 0) {
38 return NULL;
39 }
40 return (const uint8_t *)ref + offset;
41}
42
43static struct table_data
44read_flatbuffers_table(const uint8_t buffer[const],
45 const size_t buffer_len,
46 const flatbuffers_uoffset_t *const ref,
47 uint16_t vtable_out[const],
48 const uint16_t vtable_cap)
49{
50 memset(vtable_out, 0, vtable_cap * sizeof(*vtable_out));
51 const flatbuffers_soffset_t *const table = read_flatbuffers_uoffset(buffer, buffer_len, ref, sizeof(*table));
52 if (table == NULL) {
53 return (struct table_data){0};
54 }
55
56 const int32_t vtable_offset = -(int32_t)le32toh(*table);
57 const struct flatbuffers_vtable_t *const vtable =
58 (const struct flatbuffers_vtable_t *)((const uint8_t *)table + vtable_offset);
59 if (vtable_offset < buffer - (const uint8_t *)table ||
60 vtable_offset > &buffer[buffer_len - sizeof(*vtable)] - (const uint8_t *)table) {
61 return (struct table_data){0};
62 }
63
64 const uint16_t vtable_size = le16toh(vtable->vtable_size), table_size = le16toh(vtable->table_size);
65 if (vtable_size < sizeof(*vtable) || vtable_size > &buffer[buffer_len] - (const uint8_t *)vtable ||
66 (vtable_size % sizeof(*vtable->offsets)) != 0 || table_size < sizeof(*table) ||
67 table_size > &buffer[buffer_len] - (const uint8_t *)table) {
68 return (struct table_data){0};
69 }
70
71 for (uint16_t i = 0, length = MIN((vtable_size - sizeof(*vtable)) / sizeof(*vtable->offsets), vtable_cap);
72 i < length; ++i) {
73 const uint16_t offset = le16toh(vtable->offsets[i]);
74 if (offset < table_size) {
75 vtable_out[i] = offset;
76 }
77 }
78
79 return (struct table_data){table_size, (const uint8_t *)table};
80}
81
82static flatbuffers_vector_t
83read_flatbuffers_vector(const uint8_t buffer[const],
84 const size_t buffer_len,
85 const flatbuffers_uoffset_t *const ref,
86 const size_t element_size)
87{
88 const uint32_t *const vector = read_flatbuffers_uoffset(buffer, buffer_len, ref, sizeof(*vector));
89 if (vector == NULL) {
90 return (flatbuffers_vector_t){0};
91 }
92
93 const flatbuffers_vector_t out = {
94 .length = le32toh(*vector),
95 .data = &vector[1],
96 };
97 if (out.length > (&buffer[buffer_len] - (const uint8_t *)out.data) / element_size) {
98 return (flatbuffers_vector_t){0};
99 }
100
101 return out;
102}
103
104static bool
105read_solarxr_quat(struct xrt_quat *const out,
106 const uint8_t buffer[const],
107 const size_t buffer_len,
108 const uint16_t offset)
109{
110 static_assert(offsetof(struct xrt_quat, x) == 0, "");
111 static_assert(offsetof(struct xrt_quat, y) == 4, "");
112 static_assert(offsetof(struct xrt_quat, z) == 8, "");
113 static_assert(offsetof(struct xrt_quat, w) == 12, "");
114 static_assert(sizeof(struct xrt_quat) == 16, "");
115
116 *out = (struct xrt_quat){.w = 1};
117 if (offset == 0 || offset + sizeof(*out) > buffer_len) {
118 return false;
119 }
120
121 memcpy(out, &buffer[offset], sizeof(*out));
122 return true;
123}
124
125static bool
126read_solarxr_vec3f(struct xrt_vec3 *const out,
127 const uint8_t buffer[const],
128 const size_t buffer_len,
129 const uint16_t offset)
130{
131 static_assert(offsetof(struct xrt_vec3, x) == 0, "");
132 static_assert(offsetof(struct xrt_vec3, y) == 4, "");
133 static_assert(offsetof(struct xrt_vec3, z) == 8, "");
134 static_assert(sizeof(struct xrt_vec3) == 12, "");
135
136 *out = (struct xrt_vec3){0};
137 if (offset == 0 || offset + sizeof(*out) > buffer_len) {
138 return false;
139 }
140
141 memcpy(out, &buffer[offset], sizeof(*out));
142 return true;
143}
144
145bool
146read_solarxr_message_bundle(struct solarxr_message_bundle *const out,
147 const uint8_t buffer[const],
148 const size_t buffer_len,
149 const solarxr_message_bundle_t *const ref)
150{
151 *out = (struct solarxr_message_bundle){0};
152 uint16_t bundle_vtable[2];
153 const struct table_data bundle =
154 read_flatbuffers_table(buffer, buffer_len, &ref->offset, bundle_vtable, ARRAY_SIZE(bundle_vtable));
155 if (bundle.length == 0) {
156 return false;
157 }
158
159 if (bundle_vtable[0] != 0 && bundle_vtable[0] + sizeof(flatbuffers_uoffset_t) <= bundle.length) {
160 *(flatbuffers_vector_t *)&out->data_feed_msgs = read_flatbuffers_vector(
161 buffer, buffer_len, (const flatbuffers_uoffset_t *)&bundle.data[bundle_vtable[0]],
162 sizeof(*out->data_feed_msgs.data));
163 }
164
165 if (bundle_vtable[1] != 0 && bundle_vtable[1] + sizeof(flatbuffers_uoffset_t) <= bundle.length) {
166 *(flatbuffers_vector_t *)&out->rpc_msgs = read_flatbuffers_vector(
167 buffer, buffer_len, (const flatbuffers_uoffset_t *)&bundle.data[bundle_vtable[1]],
168 sizeof(*out->rpc_msgs.data));
169 }
170
171 return true;
172}
173
174bool
175read_solarxr_data_feed_message_header(struct solarxr_data_feed_message_header *const out,
176 const uint8_t buffer[const],
177 const size_t buffer_len,
178 const solarxr_data_feed_message_header_t *const ref)
179{
180 *out = (struct solarxr_data_feed_message_header){0};
181 uint16_t header_vtable[2];
182 const struct table_data header =
183 read_flatbuffers_table(buffer, buffer_len, &ref->offset, header_vtable, ARRAY_SIZE(header_vtable));
184 if (header.length == 0) {
185 return false;
186 }
187
188 if (header_vtable[0] == 0 || header_vtable[1] == 0 ||
189 header_vtable[1] + sizeof(flatbuffers_uoffset_t) > header.length) {
190 return true;
191 }
192
193 out->message_type = header.data[header_vtable[0]];
194
195 switch (header.data[header_vtable[0]]) {
196 case SOLARXR_DATA_FEED_MESSAGE_DATA_FEED_UPDATE: {
197 uint16_t update_vtable[3];
198 const struct table_data update = read_flatbuffers_table(
199 buffer, buffer_len, (const flatbuffers_uoffset_t *)&header.data[header_vtable[1]], update_vtable,
200 ARRAY_SIZE(update_vtable));
201 if (update.length == 0) {
202 break;
203 }
204
205 if (update_vtable[0] != 0 && update_vtable[0] + sizeof(flatbuffers_uoffset_t) <= update.length) {
206 *(flatbuffers_vector_t *)&out->message.data_feed_update.devices = read_flatbuffers_vector(
207 buffer, buffer_len, (const flatbuffers_uoffset_t *)&update.data[update_vtable[0]],
208 sizeof(*out->message.data_feed_update.devices.data));
209 }
210
211 if (update_vtable[1] != 0 && update_vtable[1] + sizeof(flatbuffers_uoffset_t) <= update.length) {
212 *(flatbuffers_vector_t *)&out->message.data_feed_update.synthetic_trackers =
213 read_flatbuffers_vector(buffer, buffer_len,
214 (const flatbuffers_uoffset_t *)&update.data[update_vtable[1]],
215 sizeof(*out->message.data_feed_update.synthetic_trackers.data));
216 }
217
218 if (update_vtable[2] != 0 && update_vtable[2] + sizeof(flatbuffers_uoffset_t) <= update.length) {
219 *(flatbuffers_vector_t *)&out->message.data_feed_update.bones = read_flatbuffers_vector(
220 buffer, buffer_len, (const flatbuffers_uoffset_t *)&update.data[update_vtable[2]],
221 sizeof(*out->message.data_feed_update.bones.data));
222 }
223
224 break;
225 }
226 default:;
227 }
228
229 return true;
230}
231
232bool
233read_solarxr_rpc_message_header(struct solarxr_rpc_message_header *const out,
234 const uint8_t buffer[const],
235 const size_t buffer_len,
236 const solarxr_rpc_message_header_t *const ref)
237{
238 *out = (struct solarxr_rpc_message_header){0};
239 uint16_t header_vtable[3];
240 const struct table_data header =
241 read_flatbuffers_table(buffer, buffer_len, &ref->offset, header_vtable, ARRAY_SIZE(header_vtable));
242 if (header.length == 0) {
243 return false;
244 }
245
246 if (header_vtable[1] == 0 || header_vtable[2] == 0 ||
247 header_vtable[2] + sizeof(flatbuffers_uoffset_t) > header.length) {
248 return true;
249 }
250
251 out->message_type = header.data[header_vtable[1]];
252
253 switch (header.data[header_vtable[1]]) {
254 case SOLARXR_RPC_MESSAGE_TYPE_SETTINGS_RESPONSE: {
255 uint16_t message_vtable[1];
256 const struct table_data message = read_flatbuffers_table(
257 buffer, buffer_len, (const flatbuffers_uoffset_t *)&header.data[header_vtable[2]], message_vtable,
258 ARRAY_SIZE(message_vtable));
259 if (message.length == 0 || message_vtable[0] == 0 ||
260 message_vtable[0] + sizeof(flatbuffers_uoffset_t) > message.length) {
261 break;
262 }
263
264 uint16_t trackers_vtable[15];
265 const struct table_data trackers = read_flatbuffers_table(
266 buffer, buffer_len, (const flatbuffers_uoffset_t *)&message.data[message_vtable[0]],
267 trackers_vtable, ARRAY_SIZE(trackers_vtable));
268 if (trackers.length == 0) {
269 break;
270 }
271
272 if (trackers_vtable[0] != 0) {
273 out->message.settings_response.steam_vr_trackers.waist = trackers.data[trackers_vtable[0]];
274 }
275
276 if (trackers_vtable[1] != 0) {
277 out->message.settings_response.steam_vr_trackers.chest = trackers.data[trackers_vtable[1]];
278 }
279
280 if (trackers_vtable[7] != 0) {
281 out->message.settings_response.steam_vr_trackers.left_foot = trackers.data[trackers_vtable[7]];
282 }
283
284 if (trackers_vtable[8] != 0) {
285 out->message.settings_response.steam_vr_trackers.right_foot = trackers.data[trackers_vtable[8]];
286 }
287
288 if (trackers_vtable[9] != 0) {
289 out->message.settings_response.steam_vr_trackers.left_knee = trackers.data[trackers_vtable[9]];
290 }
291
292 if (trackers_vtable[10] != 0) {
293 out->message.settings_response.steam_vr_trackers.right_knee =
294 trackers.data[trackers_vtable[10]];
295 }
296
297 if (trackers_vtable[11] != 0) {
298 out->message.settings_response.steam_vr_trackers.left_elbow =
299 trackers.data[trackers_vtable[11]];
300 }
301
302 if (trackers_vtable[12] != 0) {
303 out->message.settings_response.steam_vr_trackers.right_elbow =
304 trackers.data[trackers_vtable[12]];
305 }
306
307 if (trackers_vtable[13] != 0) {
308 out->message.settings_response.steam_vr_trackers.left_hand = trackers.data[trackers_vtable[13]];
309 }
310
311 if (trackers_vtable[14] != 0) {
312 out->message.settings_response.steam_vr_trackers.right_hand =
313 trackers.data[trackers_vtable[14]];
314 }
315
316 break;
317 }
318 default:;
319 }
320
321 return true;
322}
323
324bool
325read_solarxr_device_data(struct solarxr_device_data *const out,
326 const uint8_t buffer[const],
327 const size_t buffer_len,
328 const solarxr_device_data_t *const ref)
329{
330 *out = (struct solarxr_device_data){0};
331 uint16_t data_vtable[5];
332 const struct table_data data =
333 read_flatbuffers_table(buffer, buffer_len, &ref->offset, data_vtable, ARRAY_SIZE(data_vtable));
334 if (data.length == 0) {
335 return false;
336 }
337
338 if (data_vtable[0] != 0) {
339 out->id = data.data[data_vtable[0]];
340 }
341
342 if (data_vtable[4] != 0 && data_vtable[4] + sizeof(flatbuffers_uoffset_t) <= data.length) {
343 *(flatbuffers_vector_t *)&out->trackers = read_flatbuffers_vector(
344 buffer, buffer_len, (const flatbuffers_uoffset_t *)&data.data[data_vtable[4]],
345 sizeof(*out->trackers.data));
346 }
347
348 return true;
349}
350
351bool
352read_solarxr_tracker_data(struct solarxr_tracker_data *const out,
353 const uint8_t buffer[const],
354 const size_t buffer_len,
355 const solarxr_tracker_data_t *const ref)
356{
357 *out = (struct solarxr_tracker_data){0};
358 uint16_t data_vtable[9];
359 const struct table_data data =
360 read_flatbuffers_table(buffer, buffer_len, &ref->offset, data_vtable, ARRAY_SIZE(data_vtable));
361 if (data.length == 0) {
362 return false;
363 }
364
365 if (data_vtable[0] != 0 && data_vtable[0] + sizeof(flatbuffers_uoffset_t) <= data.length) {
366 uint16_t id_vtable[2];
367 const struct table_data id = read_flatbuffers_table(
368 buffer, buffer_len, (const flatbuffers_uoffset_t *)&data.data[data_vtable[0]], id_vtable,
369 ARRAY_SIZE(id_vtable));
370 if (id_vtable[0] != 0) {
371 out->tracker_id.has_device_id = true;
372 out->tracker_id.device_id = id.data[id_vtable[0]];
373 }
374
375 if (id_vtable[1] != 0) {
376 out->tracker_id.tracker_num = id.data[id_vtable[1]];
377 }
378 }
379
380 if (data_vtable[1] != 0 && data_vtable[1] + sizeof(flatbuffers_uoffset_t) <= data.length) {
381 uint16_t info_vtable[8];
382 const struct table_data info = read_flatbuffers_table(
383 buffer, buffer_len, (const flatbuffers_uoffset_t *)&data.data[data_vtable[1]], info_vtable,
384 ARRAY_SIZE(info_vtable));
385 out->has_info = true;
386 if (info_vtable[1] != 0) {
387 out->info.body_part = info.data[info_vtable[1]];
388 }
389
390 if (info_vtable[7] != 0) {
391 *(flatbuffers_vector_t *)&out->info.display_name = read_flatbuffers_vector(
392 buffer, buffer_len, (const flatbuffers_uoffset_t *)&info.data[info_vtable[7]],
393 sizeof(*out->info.display_name.data));
394 }
395 }
396
397 out->has_rotation = read_solarxr_quat(&out->rotation, data.data, data.length, data_vtable[3]);
398 out->has_position = read_solarxr_vec3f(&out->position, data.data, data.length, data_vtable[4]);
399 out->has_raw_angular_velocity =
400 read_solarxr_vec3f(&out->raw_angular_velocity, data.data, data.length, data_vtable[5]);
401 out->has_linear_acceleration =
402 read_solarxr_vec3f(&out->linear_acceleration, data.data, data.length, data_vtable[8]);
403
404 return true;
405}
406
407bool
408read_solarxr_bone(struct solarxr_bone *const out,
409 const uint8_t buffer[const],
410 const size_t buffer_len,
411 const solarxr_bone_t *const ref)
412{
413 *out = (struct solarxr_bone){0};
414 uint16_t bone_vtable[4];
415 const struct table_data bone =
416 read_flatbuffers_table(buffer, buffer_len, &ref->offset, bone_vtable, ARRAY_SIZE(bone_vtable));
417 if (bone.length == 0) {
418 return false;
419 }
420
421 if (bone_vtable[0] != 0) {
422 out->body_part = bone.data[bone_vtable[0]];
423 }
424
425 read_solarxr_quat(&out->rotation_g, bone.data, bone.length, bone_vtable[1]);
426 if (bone_vtable[2] != 0 && bone_vtable[2] + sizeof(out->bone_length) <= bone.length) {
427 memcpy(&out->bone_length, &bone.data[bone_vtable[2]], sizeof(out->bone_length));
428 }
429
430 read_solarxr_vec3f(&out->head_position_g, bone.data, bone.length, bone_vtable[3]);
431 return true;
432}