+3

CMakeLists.txt

··· 352 352 option_with_deps(XRT_BUILD_DRIVER_WMR "Enable Windows Mixed Reality driver" DEPENDS LINUX) 353 353 option_with_deps(XRT_BUILD_DRIVER_XREAL_AIR "Enable Xreal Air HMD driver" DEPENDS XRT_HAVE_HIDAPI) 354 354 option_with_deps(XRT_BUILD_DRIVER_SIMULAVR "Enable simula driver" DEPENDS XRT_HAVE_REALSENSE) 355 355 + option_with_deps(XRT_BUILD_DRIVER_SOLARXR "Enable SolarXR driver" DEPENDS XRT_HAVE_LINUX) 355 356 option(XRT_BUILD_DRIVER_SIMULATED "Enable simulated driver" ON) 356 357 357 358 option(XRT_BUILD_SAMPLES "Enable compiling sample code implementations that will not be linked into any final targets" ON) ··· 458 459 "TWRAP" 459 460 "XREAL_AIR" 460 461 "STEAMVR_LIGHTHOUSE" 462 462 + "SOLARXR" 461 463 ) 462 464 463 465 # Package name needs to be known by the native code itself. ··· 693 695 message(STATUS "# DRIVER_WMR: ${XRT_BUILD_DRIVER_WMR}") 694 696 message(STATUS "# DRIVER_XREAL_AIR: ${XRT_BUILD_DRIVER_XREAL_AIR}") 695 697 message(STATUS "# DRIVER_STEAMVR_LIGHTHOUSE: ${XRT_BUILD_DRIVER_STEAMVR_LIGHTHOUSE}") 698 698 + message(STATUS "# DRIVER_SOLARXR: ${XRT_BUILD_DRIVER_SOLARXR}") 696 699 message(STATUS "#####----- Config -----#####") 697 700 # cmake-format: on 698 701

+15

src/xrt/drivers/CMakeLists.txt

··· 526 526 list(APPEND ENABLED_HEADSET_DRIVERS steamvr_lh) 527 527 endif() 528 528 529 529 + if(XRT_BUILD_DRIVER_SOLARXR) 530 530 + add_library( 531 531 + drv_solarxr STATIC 532 532 + solarxr/protocol.c 533 533 + solarxr/protocol.h 534 534 + solarxr/solarxr_device.c 535 535 + solarxr/solarxr_interface.h 536 536 + solarxr/solarxr_ipc_message.h 537 537 + solarxr/solarxr_ipc_socket.c 538 538 + solarxr/solarxr_ipc_socket.h 539 539 + ) 540 540 + target_link_libraries(drv_solarxr PRIVATE xrt-interfaces aux_util ipc_shared) 541 541 + list(APPEND ENABLED_DRIVERS solarxr) 542 542 + endif() 543 543 + 529 544 if(XRT_BUILD_SAMPLES) 530 545 # We build the sample driver to make sure it stays valid, 531 546 # but it never gets linked into a final target.

+432

src/xrt/drivers/solarxr/protocol.c

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

+253

src/xrt/drivers/solarxr/protocol.h

··· 1 1 + // Copyright 2025, rcelyte 2 2 + // SPDX-License-Identifier: BSL-1.0 3 3 + /*! 4 4 + * @file 5 5 + * @brief Parser for a small subset of the SolarXR Flatbuffers protocol, as defined at 6 6 + * https://github.com/SlimeVR/SolarXR-Protocol 7 7 + * @ingroup drv_solarxr 8 8 + */ 9 9 + 10 10 + #pragma once 11 11 + #include "xrt/xrt_defines.h" 12 12 + #include <assert.h> 13 13 + 14 14 + #ifdef __cplusplus 15 15 + extern "C" { 16 16 + #endif 17 17 + 18 18 + typedef uint32_t flatbuffers_uoffset_t; // little-endian byte order 19 19 + 20 20 + enum solarxr_body_part 21 21 + { 22 22 + SOLARXR_BODY_PART_NONE = 0, 23 23 + SOLARXR_BODY_PART_HEAD = 1, 24 24 + SOLARXR_BODY_PART_NECK = 2, 25 25 + SOLARXR_BODY_PART_CHEST = 3, 26 26 + SOLARXR_BODY_PART_WAIST = 4, 27 27 + SOLARXR_BODY_PART_HIP = 5, 28 28 + SOLARXR_BODY_PART_LEFT_UPPER_LEG = 6, 29 29 + SOLARXR_BODY_PART_RIGHT_UPPER_LEG = 7, 30 30 + SOLARXR_BODY_PART_LEFT_LOWER_LEG = 8, 31 31 + SOLARXR_BODY_PART_RIGHT_LOWER_LEG = 9, 32 32 + SOLARXR_BODY_PART_LEFT_FOOT = 10, 33 33 + SOLARXR_BODY_PART_RIGHT_FOOT = 11, 34 34 + SOLARXR_BODY_PART_LEFT_LOWER_ARM = 14, 35 35 + SOLARXR_BODY_PART_RIGHT_LOWER_ARM = 15, 36 36 + SOLARXR_BODY_PART_LEFT_UPPER_ARM = 16, 37 37 + SOLARXR_BODY_PART_RIGHT_UPPER_ARM = 17, 38 38 + SOLARXR_BODY_PART_LEFT_HAND = 18, 39 39 + SOLARXR_BODY_PART_RIGHT_HAND = 19, 40 40 + SOLARXR_BODY_PART_LEFT_SHOULDER = 20, 41 41 + SOLARXR_BODY_PART_RIGHT_SHOULDER = 21, 42 42 + SOLARXR_BODY_PART_UPPER_CHEST = 22, 43 43 + SOLARXR_BODY_PART_LEFT_HIP = 23, 44 44 + SOLARXR_BODY_PART_RIGHT_HIP = 24, 45 45 + SOLARXR_BODY_PART_LEFT_THUMB_PROXIMAL = 25, 46 46 + SOLARXR_BODY_PART_LEFT_THUMB_INTERMEDIATE = 26, 47 47 + SOLARXR_BODY_PART_LEFT_THUMB_DISTAL = 27, 48 48 + SOLARXR_BODY_PART_LEFT_INDEX_PROXIMAL = 28, 49 49 + SOLARXR_BODY_PART_LEFT_INDEX_INTERMEDIATE = 29, 50 50 + SOLARXR_BODY_PART_LEFT_INDEX_DISTAL = 30, 51 51 + SOLARXR_BODY_PART_LEFT_MIDDLE_PROXIMAL = 31, 52 52 + SOLARXR_BODY_PART_LEFT_MIDDLE_INTERMEDIATE = 32, 53 53 + SOLARXR_BODY_PART_LEFT_MIDDLE_DISTAL = 33, 54 54 + SOLARXR_BODY_PART_LEFT_RING_PROXIMAL = 34, 55 55 + SOLARXR_BODY_PART_LEFT_RING_INTERMEDIATE = 35, 56 56 + SOLARXR_BODY_PART_LEFT_RING_DISTAL = 36, 57 57 + SOLARXR_BODY_PART_LEFT_LITTLE_PROXIMAL = 37, 58 58 + SOLARXR_BODY_PART_LEFT_LITTLE_INTERMEDIATE = 38, 59 59 + SOLARXR_BODY_PART_LEFT_LITTLE_DISTAL = 39, 60 60 + SOLARXR_BODY_PART_RIGHT_THUMB_PROXIMAL = 40, 61 61 + SOLARXR_BODY_PART_RIGHT_THUMB_INTERMEDIATE = 41, 62 62 + SOLARXR_BODY_PART_RIGHT_THUMB_DISTAL = 42, 63 63 + SOLARXR_BODY_PART_RIGHT_INDEX_PROXIMAL = 43, 64 64 + SOLARXR_BODY_PART_RIGHT_INDEX_INTERMEDIATE = 44, 65 65 + SOLARXR_BODY_PART_RIGHT_INDEX_DISTAL = 45, 66 66 + SOLARXR_BODY_PART_RIGHT_MIDDLE_PROXIMAL = 46, 67 67 + SOLARXR_BODY_PART_RIGHT_MIDDLE_INTERMEDIATE = 47, 68 68 + SOLARXR_BODY_PART_RIGHT_MIDDLE_DISTAL = 48, 69 69 + SOLARXR_BODY_PART_RIGHT_RING_PROXIMAL = 49, 70 70 + SOLARXR_BODY_PART_RIGHT_RING_INTERMEDIATE = 50, 71 71 + SOLARXR_BODY_PART_RIGHT_RING_DISTAL = 51, 72 72 + SOLARXR_BODY_PART_RIGHT_LITTLE_PROXIMAL = 52, 73 73 + SOLARXR_BODY_PART_RIGHT_LITTLE_INTERMEDIATE = 53, 74 74 + SOLARXR_BODY_PART_RIGHT_LITTLE_DISTAL = 54, 75 75 + SOLARXR_BODY_PART_MAX_ENUM, 76 76 + }; 77 77 + 78 78 + #define FLATBUFFERS_VECTOR(type_) \ 79 79 + struct \ 80 80 + { \ 81 81 + uint32_t length; \ 82 82 + const type_ *data; \ 83 83 + } 84 84 + 85 85 + struct solarxr_tracker_id 86 86 + { // table solarxr_protocol.datatypes.TrackerId 87 87 + bool has_device_id; 88 88 + uint8_t device_id; 89 89 + uint8_t tracker_num; 90 90 + }; 91 91 + 92 92 + struct solarxr_tracker_info 93 93 + { // table solarxr_protocol.data_feed.tracker.TrackerInfo 94 94 + enum solarxr_body_part body_part; 95 95 + FLATBUFFERS_VECTOR(char) display_name; 96 96 + }; 97 97 + 98 98 + typedef struct 99 99 + { 100 100 + flatbuffers_uoffset_t offset; 101 101 + } solarxr_tracker_data_t; 102 102 + 103 103 + struct solarxr_tracker_data 104 104 + { // table solarxr_protocol.data_feed.tracker.TrackerData 105 105 + bool has_info; 106 106 + bool has_rotation; 107 107 + bool has_position; 108 108 + bool has_raw_angular_velocity; 109 109 + bool has_linear_acceleration; 110 110 + struct solarxr_tracker_id tracker_id; 111 111 + struct solarxr_tracker_info info; 112 112 + struct xrt_quat rotation; 113 113 + struct xrt_vec3 position; 114 114 + struct xrt_vec3 raw_angular_velocity; 115 115 + struct xrt_vec3 linear_acceleration; 116 116 + }; 117 117 + 118 118 + typedef struct 119 119 + { 120 120 + flatbuffers_uoffset_t offset; 121 121 + } solarxr_device_data_t; 122 122 + 123 123 + struct solarxr_device_data 124 124 + { // table solarxr_protocol.data_feed.device_data.DeviceData 125 125 + uint8_t id; 126 126 + FLATBUFFERS_VECTOR(solarxr_tracker_data_t) trackers; // solarxr_protocol.data_feed.tracker.TrackerData[] 127 127 + }; 128 128 + 129 129 + typedef struct 130 130 + { 131 131 + flatbuffers_uoffset_t offset; 132 132 + } solarxr_bone_t; 133 133 + 134 134 + struct solarxr_bone 135 135 + { // table solarxr_protocol.data_feed.Bone 136 136 + enum solarxr_body_part body_part; 137 137 + struct xrt_quat rotation_g; 138 138 + float bone_length; 139 139 + struct xrt_vec3 head_position_g; 140 140 + }; 141 141 + 142 142 + struct solarxr_data_feed_update 143 143 + { // table solarxr_protocol.data_feed.DataFeedUpdate 144 144 + FLATBUFFERS_VECTOR(solarxr_device_data_t) devices; // solarxr_protocol.data_feed.device_data.DeviceData[] 145 145 + FLATBUFFERS_VECTOR(solarxr_tracker_data_t) 146 146 + synthetic_trackers; // solarxr_protocol.data_feed.tracker.TrackerData[] 147 147 + FLATBUFFERS_VECTOR(solarxr_bone_t) bones; // solarxr_protocol.data_feed.Bone[] 148 148 + }; 149 149 + 150 150 + enum solarxr_data_feed_message_type 151 151 + { 152 152 + SOLARXR_DATA_FEED_MESSAGE_POLL_DATA_FEED = 1, 153 153 + SOLARXR_DATA_FEED_MESSAGE_DATA_FEED_UPDATE = 3, 154 154 + }; 155 155 + 156 156 + union solarxr_data_feed_message { // union solarxr_protocol.data_feed.DataFeedMessage 157 157 + struct solarxr_data_feed_update data_feed_update; 158 158 + }; 159 159 + 160 160 + typedef struct 161 161 + { 162 162 + flatbuffers_uoffset_t offset; 163 163 + } solarxr_data_feed_message_header_t; 164 164 + 165 165 + struct solarxr_data_feed_message_header 166 166 + { // table solarxr_protocol.data_feed.DataFeedMessageHeader 167 167 + enum solarxr_data_feed_message_type message_type; 168 168 + union solarxr_data_feed_message message; 169 169 + }; 170 170 + 171 171 + struct solarxr_steamvr_trackers_setting 172 172 + { // table solarxr_protocol.rpc.SteamVRTrackersSetting 173 173 + bool waist; 174 174 + bool chest; 175 175 + bool left_foot; 176 176 + bool right_foot; 177 177 + bool left_knee; 178 178 + bool right_knee; 179 179 + bool left_elbow; 180 180 + bool right_elbow; 181 181 + bool left_hand; 182 182 + bool right_hand; 183 183 + }; 184 184 + 185 185 + struct solarxr_settings_response 186 186 + { // table solarxr_protocol.rpc.SettingsResponse 187 187 + struct solarxr_steamvr_trackers_setting steam_vr_trackers; 188 188 + }; 189 189 + 190 190 + enum solarxr_rpc_message_type 191 191 + { 192 192 + SOLARXR_RPC_MESSAGE_TYPE_SETTINGS_REQUEST = 6, 193 193 + SOLARXR_RPC_MESSAGE_TYPE_SETTINGS_RESPONSE = 7, 194 194 + }; 195 195 + 196 196 + union solarxr_rpc_message { // union solarxr_protocol.rpc.RpcMessage 197 197 + struct solarxr_settings_response settings_response; 198 198 + }; 199 199 + 200 200 + typedef struct 201 201 + { 202 202 + flatbuffers_uoffset_t offset; 203 203 + } solarxr_rpc_message_header_t; 204 204 + 205 205 + struct solarxr_rpc_message_header 206 206 + { // table solarxr_protocol.rpc.RpcMessageHeader 207 207 + enum solarxr_rpc_message_type message_type; 208 208 + union solarxr_rpc_message message; 209 209 + }; 210 210 + 211 211 + typedef struct 212 212 + { 213 213 + flatbuffers_uoffset_t offset; 214 214 + } solarxr_message_bundle_t; 215 215 + 216 216 + struct solarxr_message_bundle 217 217 + { // table solarxr_protocol.MessageBundle 218 218 + FLATBUFFERS_VECTOR(solarxr_data_feed_message_header_t) 219 219 + data_feed_msgs; // solarxr_protocol.data_feed.DataFeedMessageHeader[] 220 220 + FLATBUFFERS_VECTOR(solarxr_rpc_message_header_t) rpc_msgs; // solarxr_protocol.rpc.RpcMessageHeader[] 221 221 + }; 222 222 + 223 223 + bool 224 224 + read_solarxr_message_bundle(struct solarxr_message_bundle *out, 225 225 + const uint8_t buffer[], 226 226 + size_t buffer_len, 227 227 + const solarxr_message_bundle_t *ref); 228 228 + bool 229 229 + read_solarxr_data_feed_message_header(struct solarxr_data_feed_message_header *out, 230 230 + const uint8_t buffer[], 231 231 + size_t buffer_len, 232 232 + const solarxr_data_feed_message_header_t *ref); 233 233 + bool 234 234 + read_solarxr_rpc_message_header(struct solarxr_rpc_message_header *out, 235 235 + const uint8_t buffer[], 236 236 + size_t buffer_len, 237 237 + const solarxr_rpc_message_header_t *ref); 238 238 + bool 239 239 + read_solarxr_device_data(struct solarxr_device_data *out, 240 240 + const uint8_t buffer[], 241 241 + size_t buffer_len, 242 242 + const solarxr_device_data_t *ref); 243 243 + bool 244 244 + read_solarxr_tracker_data(struct solarxr_tracker_data *out, 245 245 + const uint8_t buffer[], 246 246 + size_t buffer_len, 247 247 + const solarxr_tracker_data_t *ref); 248 248 + bool 249 249 + read_solarxr_bone(struct solarxr_bone *out, const uint8_t buffer[], size_t buffer_len, const solarxr_bone_t *ref); 250 250 + 251 251 + #ifdef __cplusplus 252 252 + } 253 253 + #endif

+1251

src/xrt/drivers/solarxr/solarxr_device.c

··· 1 1 + // Copyright 2025, rcelyte 2 2 + // SPDX-License-Identifier: BSL-1.0 3 3 + 4 4 + #include "solarxr_interface.h" 5 5 + #include "protocol.h" 6 6 + #include "solarxr_ipc_message.h" 7 7 + #include "solarxr_ipc_socket.h" 8 8 + 9 9 + #include "math/m_api.h" 10 10 + #include "math/m_relation_history.h" 11 11 + #include "math/m_vec3.h" 12 12 + #include "os/os_threading.h" 13 13 + #include "os/os_time.h" 14 14 + #include "util/u_debug.h" 15 15 + #include "util/u_device.h" 16 16 + #include "xrt/xrt_system.h" 17 17 + 18 18 + #include <endian.h> 19 19 + #include <stdatomic.h> 20 20 + #include <stdio.h> 21 21 + #include <wchar.h> 22 22 + 23 23 + #define MAX_GENERIC_TRACKERS 32 24 24 + 25 25 + DEBUG_GET_ONCE_LOG_OPTION(solarxr_log, "SOLARXR_LOG", U_LOGGING_INFO) 26 26 + DEBUG_GET_ONCE_BOOL_OPTION(solarxr_raw_trackers, "SOLARXR_RAW_TRACKERS", false) 27 27 + DEBUG_GET_ONCE_NUM_OPTION(solarxr_sync_delay_ms, "SOLARXR_SYNC_DELAY_MS", 4) 28 28 + DEBUG_GET_ONCE_NUM_OPTION(solarxr_sync_timeout_ms, "SOLARXR_SYNC_TIMEOUT_MS", 50) 29 29 + 30 30 + struct solarxr_device; 31 31 + struct solarxr_generic_tracker 32 32 + { 33 33 + struct xrt_device base; 34 34 + struct os_mutex mutex; 35 35 + struct solarxr_device *parent; // weak reference 36 36 + uint32_t index; 37 37 + struct m_relation_history *history; // weak reference 38 38 + enum solarxr_body_part role; 39 39 + }; 40 40 + 41 41 + struct solarxr_device 42 42 + { 43 43 + struct xrt_device base; 44 44 + struct os_thread thread; 45 45 + struct solarxr_ipc_socket socket; 46 46 + _Atomic(timepoint_ns) next_sync; 47 47 + struct os_mutex mutex; 48 48 + _Atomic(timepoint_ns) timestamp; 49 49 + _Atomic(uint64_t) enabled_bones; 50 50 + static_assert(SOLARXR_BODY_PART_MAX_ENUM <= 64, "bitfield too small"); 51 51 + struct solarxr_device_bone 52 52 + { 53 53 + struct xrt_pose pose; 54 54 + float length; 55 55 + } bones[SOLARXR_BODY_PART_MAX_ENUM]; 56 56 + wchar_t tracker_ids[MAX_GENERIC_TRACKERS]; 57 57 + struct m_relation_history *trackers[MAX_GENERIC_TRACKERS]; 58 58 + struct solarxr_generic_tracker *tracker_refs[MAX_GENERIC_TRACKERS]; 59 59 + bool use_trackers; 60 60 + uint32_t generation; 61 61 + struct xrt_tracking_origin standalone_origin; 62 62 + }; 63 63 + 64 64 + struct span 65 65 + { 66 66 + size_t length; 67 67 + const uint8_t *data; 68 68 + }; 69 69 + 70 70 + static void 71 71 + solarxr_device_destroy(struct xrt_device *xdev); 72 72 + static void 73 73 + solarxr_generic_tracker_destroy(struct xrt_device *xdev); 74 74 + 75 75 + static inline struct solarxr_device * 76 76 + solarxr_device(struct xrt_device *const xdev) 77 77 + { 78 78 + if (xdev == NULL || xdev->destroy != solarxr_device_destroy) { 79 79 + return NULL; 80 80 + } 81 81 + return (struct solarxr_device *)xdev; 82 82 + } 83 83 + 84 84 + static inline struct solarxr_generic_tracker * 85 85 + solarxr_generic_tracker(struct xrt_device *const xdev) 86 86 + { 87 87 + if (xdev == NULL || xdev->destroy != solarxr_generic_tracker_destroy) { 88 88 + return NULL; 89 89 + } 90 90 + return (struct solarxr_generic_tracker *)xdev; 91 91 + } 92 92 + 93 93 + // returns an arbitrary unique value to identify trackers by 94 94 + static inline wchar_t 95 95 + solarxr_tracker_id_to_wchar(struct solarxr_tracker_id id) 96 96 + { 97 97 + if (!id.has_device_id) { 98 98 + id.device_id = 0; 99 99 + } 100 100 + 101 101 + wchar_t out = 0; 102 102 + static_assert(sizeof(id) <= sizeof(out), ""); 103 103 + memcpy(&out, &id, sizeof(id)); 104 104 + return le32toh(out); 105 105 + } 106 106 + 107 107 + static void 108 108 + solarxr_device_sync(struct solarxr_device *const device) 109 109 + { 110 110 + const timepoint_ns time = os_monotonic_get_ns(), next_sync = atomic_exchange(&device->next_sync, INT64_MAX); 111 111 + if (time <= next_sync) { 112 112 + atomic_store(&device->next_sync, next_sync); 113 113 + return; 114 114 + } 115 115 + 116 116 + struct 117 117 + { 118 118 + uint8_t head[sizeof(struct solarxr_ipc_message)]; 119 119 + struct poll_packet 120 120 + { 121 121 + uint32_t _root; 122 122 + uint16_t _table_shared[3]; 123 123 + struct 124 124 + { // table MessageBundle 125 125 + int32_t _table; 126 126 + uint32_t data_feed_msgs; // vector* 127 127 + } bundle; 128 128 + struct 129 129 + { // vector<table DataFeedMessageHeader> 130 130 + uint32_t length; 131 131 + uint32_t values[1]; // table* 132 132 + } data_feed_msgs; 133 133 + uint16_t _table_data_feed_msgs_0[4]; 134 134 + struct 135 135 + { // table DataFeedMessageHeader 136 136 + int32_t _table; 137 137 + uint32_t message; // table* 138 138 + uint8_t message_type; // enum DataFeedMessage 139 139 + uint8_t _pad[3]; 140 140 + } data_feed_msgs_0; 141 141 + struct 142 142 + { // table PollDataFeed 143 143 + int32_t _table; 144 144 + uint32_t config; // table* 145 145 + } message; 146 146 + uint16_t _table_config[6]; 147 147 + struct 148 148 + { // table DataFeedConfig 149 149 + int32_t _table; 150 150 + uint32_t trackers_mask; // table* 151 151 + bool bone_mask; 152 152 + uint8_t _pad[3]; 153 153 + } config; 154 154 + struct 155 155 + { // table DeviceDataMask 156 156 + int32_t _table; 157 157 + uint32_t tracker_data; // table* 158 158 + } data_mask; 159 159 + uint16_t _table_synthetic_trackers_mask[10]; 160 160 + struct 161 161 + { // table TrackerDataMask 162 162 + int32_t _table; 163 163 + bool rotation, position, raw_angular_velocity, linear_acceleration; 164 164 + } synthetic_trackers_mask; 165 165 + } body; 166 166 + } const poll_packet = 167 167 + { 168 168 + .body = 169 169 + { 170 170 + ._root = htole32(offsetof(struct poll_packet, bundle)), 171 171 + ._table_shared = 172 172 + { 173 173 + htole16(sizeof(poll_packet.body._table_shared)), 174 174 + htole16(8), 175 175 + htole16(4), 176 176 + }, 177 177 + .bundle = 178 178 + { 179 179 + ._table = htole32(offsetof(struct poll_packet, bundle) - 180 180 + offsetof(struct poll_packet, _table_shared)), 181 181 + .data_feed_msgs = htole32(offsetof(struct poll_packet, data_feed_msgs) - 182 182 + offsetof(struct poll_packet, bundle.data_feed_msgs)), 183 183 + }, 184 184 + .data_feed_msgs = 185 185 + { 186 186 + .length = htole32(ARRAY_SIZE(poll_packet.body.data_feed_msgs.values)), 187 187 + .values = {htole32(offsetof(struct poll_packet, data_feed_msgs_0) - 188 188 + offsetof(struct poll_packet, data_feed_msgs.values[0]))}, 189 189 + }, 190 190 + ._table_data_feed_msgs_0 = 191 191 + { 192 192 + htole16(sizeof(poll_packet.body._table_data_feed_msgs_0)), 193 193 + htole16(sizeof(poll_packet.body.data_feed_msgs_0) - 194 194 + sizeof(poll_packet.body.data_feed_msgs_0._pad)), 195 195 + htole16(offsetof(struct poll_packet, data_feed_msgs_0.message_type) - 196 196 + offsetof(struct poll_packet, data_feed_msgs_0)), 197 197 + htole16(offsetof(struct poll_packet, data_feed_msgs_0.message) - 198 198 + offsetof(struct poll_packet, data_feed_msgs_0)), 199 199 + }, 200 200 + .data_feed_msgs_0 = 201 201 + { 202 202 + ._table = htole32(offsetof(struct poll_packet, data_feed_msgs_0) - 203 203 + offsetof(struct poll_packet, _table_data_feed_msgs_0)), 204 204 + .message = htole32(offsetof(struct poll_packet, message) - 205 205 + offsetof(struct poll_packet, data_feed_msgs_0.message)), 206 206 + .message_type = 1, // DataFeedMessage::PollDataFeed 207 207 + }, 208 208 + .message = 209 209 + { 210 210 + ._table = htole32(offsetof(struct poll_packet, message) - 211 211 + offsetof(struct poll_packet, _table_shared)), 212 212 + .config = htole32(offsetof(struct poll_packet, config) - 213 213 + offsetof(struct poll_packet, message.config)), 214 214 + }, 215 215 + ._table_config = 216 216 + { 217 217 + htole16(sizeof(poll_packet.body._table_config)), 218 218 + htole16(sizeof(poll_packet.body.config) - sizeof(poll_packet.body.config._pad)), 219 219 + 0, 220 220 + htole16((device->use_trackers && debug_get_bool_option_solarxr_raw_trackers()) * 221 221 + (offsetof(struct poll_packet, config.trackers_mask) - 222 222 + offsetof(struct poll_packet, config))), 223 223 + htole16((device->use_trackers && !debug_get_bool_option_solarxr_raw_trackers()) * 224 224 + (offsetof(struct poll_packet, config.trackers_mask) - 225 225 + offsetof(struct poll_packet, config))), 226 226 + htole16(offsetof(struct poll_packet, config.bone_mask) - 227 227 + offsetof(struct poll_packet, config)), 228 228 + }, 229 229 + .config = 230 230 + { 231 231 + ._table = htole32(offsetof(struct poll_packet, config) - 232 232 + offsetof(struct poll_packet, _table_config)), 233 233 + .trackers_mask = htole32((debug_get_bool_option_solarxr_raw_trackers() 234 234 + ? offsetof(struct poll_packet, data_mask) 235 235 + : offsetof(struct poll_packet, synthetic_trackers_mask)) - 236 236 + offsetof(struct poll_packet, config.trackers_mask)), 237 237 + .bone_mask = true, 238 238 + }, 239 239 + .data_mask = 240 240 + { 241 241 + ._table = htole32(offsetof(struct poll_packet, data_mask) - 242 242 + offsetof(struct poll_packet, _table_shared)), 243 243 + .tracker_data = htole32(offsetof(struct poll_packet, synthetic_trackers_mask) - 244 244 + offsetof(struct poll_packet, data_mask.tracker_data)), 245 245 + }, 246 246 + ._table_synthetic_trackers_mask = 247 247 + { 248 248 + htole16(sizeof(poll_packet.body._table_synthetic_trackers_mask)), 249 249 + htole16(sizeof(poll_packet.body.synthetic_trackers_mask)), 250 250 + 0, 251 251 + 0, 252 252 + htole16(offsetof(struct poll_packet, synthetic_trackers_mask.rotation) - 253 253 + offsetof(struct poll_packet, synthetic_trackers_mask)), 254 254 + htole16(offsetof(struct poll_packet, synthetic_trackers_mask.position) - 255 255 + offsetof(struct poll_packet, synthetic_trackers_mask)), 256 256 + htole16(offsetof(struct poll_packet, synthetic_trackers_mask.raw_angular_velocity) - 257 257 + offsetof(struct poll_packet, synthetic_trackers_mask)), 258 258 + 0, 259 259 + 0, 260 260 + htole16(offsetof(struct poll_packet, synthetic_trackers_mask.linear_acceleration) - 261 261 + offsetof(struct poll_packet, synthetic_trackers_mask)), 262 262 + }, 263 263 + .synthetic_trackers_mask = 264 264 + { 265 265 + ._table = htole32(offsetof(struct poll_packet, synthetic_trackers_mask) - 266 266 + offsetof(struct poll_packet, _table_synthetic_trackers_mask)), 267 267 + .rotation = true, 268 268 + .position = true, 269 269 + .raw_angular_velocity = true, 270 270 + .linear_acceleration = true, 271 271 + }, 272 272 + }, 273 273 + }; 274 274 + 275 275 + solarxr_ipc_socket_send_raw( 276 276 + &device->socket, (const uint8_t *)&poll_packet, 277 277 + solarxr_ipc_message_inline((uint8_t *)&poll_packet, 278 278 + device->use_trackers 279 279 + ? sizeof(poll_packet) 280 280 + : sizeof(poll_packet.head) + offsetof(struct poll_packet, data_mask))); 281 281 + atomic_store(&device->next_sync, time + debug_get_num_option_solarxr_sync_timeout_ms() * U_TIME_1MS_IN_NS); 282 282 + } 283 283 + 284 284 + static xrt_result_t 285 285 + solarxr_device_update_inputs(struct xrt_device *const xdev) 286 286 + { 287 287 + struct solarxr_device *const device = solarxr_device(xdev); 288 288 + assert(device != NULL); 289 289 + 290 290 + for (uint32_t i = 0; i < device->base.input_count; ++i) { 291 291 + device->base.inputs[i].timestamp = atomic_load(&device->timestamp); 292 292 + } 293 293 + return XRT_SUCCESS; 294 294 + } 295 295 + 296 296 + static xrt_result_t 297 297 + solarxr_generic_tracker_update_inputs(struct xrt_device *const xdev) 298 298 + { 299 299 + struct solarxr_generic_tracker *const device = solarxr_generic_tracker(xdev); 300 300 + assert(device != NULL); 301 301 + os_mutex_lock(&device->mutex); 302 302 + 303 303 + struct solarxr_device *const parent = device->parent; 304 304 + if (parent == NULL) { 305 305 + device->base.inputs[0].active = false; 306 306 + } else { 307 307 + device->base.inputs[0].active = (atomic_load(&parent->enabled_bones) & (1llu << device->role)) != 0; 308 308 + device->base.inputs[0].timestamp = atomic_load(&parent->timestamp); 309 309 + } 310 310 + 311 311 + os_mutex_unlock(&device->mutex); 312 312 + return XRT_SUCCESS; 313 313 + } 314 314 + 315 315 + static inline struct xrt_body_skeleton_joint_fb 316 316 + offset_joint(const struct xrt_body_skeleton_joint_fb parent, const int32_t name, const struct xrt_vec3 offset) 317 317 + { 318 318 + return (struct xrt_body_skeleton_joint_fb){ 319 319 + .pose = 320 320 + { 321 321 + .orientation = parent.pose.orientation, 322 322 + .position = m_vec3_add(parent.pose.position, offset), 323 323 + }, 324 324 + .joint = name, 325 325 + .parent_joint = parent.joint, 326 326 + }; 327 327 + } 328 328 + 329 329 + // TODO: filter enabled bones 330 330 + static xrt_result_t 331 331 + solarxr_device_get_body_skeleton(struct xrt_device *const xdev, 332 332 + const enum xrt_input_name body_tracking_type, 333 333 + struct xrt_body_skeleton *const out_value) 334 334 + { 335 335 + struct xrt_body_skeleton_joint_fb *joints; 336 336 + uint32_t joint_count; 337 337 + int32_t none; 338 338 + switch (body_tracking_type) { 339 339 + case XRT_INPUT_FB_BODY_TRACKING: { 340 340 + joints = out_value->body_skeleton_fb.joints; 341 341 + joint_count = ARRAY_SIZE(out_value->body_skeleton_fb.joints); 342 342 + none = XRT_BODY_JOINT_NONE_FB; 343 343 + break; 344 344 + } 345 345 + case XRT_INPUT_META_FULL_BODY_TRACKING: { 346 346 + joints = out_value->full_body_skeleton_meta.joints; 347 347 + joint_count = ARRAY_SIZE(out_value->full_body_skeleton_meta.joints); 348 348 + none = XRT_FULL_BODY_JOINT_NONE_META; 349 349 + break; 350 350 + } 351 351 + default: return XRT_ERROR_NOT_IMPLEMENTED; 352 352 + } 353 353 + 354 354 + struct solarxr_device *const device = solarxr_device(xdev); 355 355 + assert(device != NULL); 356 356 + for (uint32_t i = 0; i < joint_count; ++i) { 357 357 + joints[i] = (struct xrt_body_skeleton_joint_fb){XRT_POSE_IDENTITY, none, none}; 358 358 + } 359 359 + 360 360 + // The standard doesn't describe the layout for these joints more specifically than being "a T-pose" 361 361 + // clang-format off 362 362 + joints[0] = (struct xrt_body_skeleton_joint_fb){XRT_POSE_IDENTITY, XRT_BODY_JOINT_HEAD_FB, XRT_BODY_JOINT_ROOT_FB}; 363 363 + joints[1] = offset_joint(joints[0], XRT_BODY_JOINT_NECK_FB, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_NECK].length, 0.f}); 364 364 + joints[2] = offset_joint(joints[1], XRT_BODY_JOINT_CHEST_FB, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_UPPER_CHEST].length, 0.f}); 365 365 + joints[3] = offset_joint(joints[2], XRT_BODY_JOINT_SPINE_UPPER_FB, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_CHEST].length, 0.f}); 366 366 + joints[4] = offset_joint(joints[3], XRT_BODY_JOINT_SPINE_LOWER_FB, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_WAIST].length, 0.f}); 367 367 + joints[5] = offset_joint(joints[4], XRT_BODY_JOINT_HIPS_FB, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_HIP].length, 0.f}); 368 368 + joints[6] = offset_joint(joints[1], XRT_BODY_JOINT_LEFT_SHOULDER_FB, (struct xrt_vec3){-device->bones[SOLARXR_BODY_PART_LEFT_SHOULDER].length, 0.f, 0.f}); 369 369 + joints[7] = offset_joint(joints[1], XRT_BODY_JOINT_RIGHT_SHOULDER_FB, (struct xrt_vec3){device->bones[SOLARXR_BODY_PART_RIGHT_SHOULDER].length, 0.f, 0.f}); 370 370 + joints[8] = offset_joint(joints[6], XRT_BODY_JOINT_LEFT_ARM_UPPER_FB, (struct xrt_vec3){-device->bones[SOLARXR_BODY_PART_LEFT_UPPER_ARM].length, 0.f, 0.f}); 371 371 + joints[9] = offset_joint(joints[7], XRT_BODY_JOINT_RIGHT_ARM_UPPER_FB, (struct xrt_vec3){device->bones[SOLARXR_BODY_PART_RIGHT_UPPER_ARM].length, 0.f, 0.f}); 372 372 + joints[10] = offset_joint(joints[8], XRT_BODY_JOINT_LEFT_ARM_LOWER_FB, (struct xrt_vec3){-device->bones[SOLARXR_BODY_PART_LEFT_LOWER_ARM].length, 0.f, 0.f}); 373 373 + joints[11] = offset_joint(joints[9], XRT_BODY_JOINT_RIGHT_ARM_LOWER_FB, (struct xrt_vec3){device->bones[SOLARXR_BODY_PART_RIGHT_LOWER_ARM].length, 0.f, 0.f}); 374 374 + joints[12] = offset_joint(joints[10], XRT_BODY_JOINT_LEFT_HAND_WRIST_FB, (struct xrt_vec3){-device->bones[SOLARXR_BODY_PART_LEFT_HAND].length, 0.f, 0.f}); 375 375 + joints[13] = offset_joint(joints[11], XRT_BODY_JOINT_RIGHT_HAND_WRIST_FB, (struct xrt_vec3){device->bones[SOLARXR_BODY_PART_RIGHT_HAND].length, 0.f, 0.f}); 376 376 + if (body_tracking_type != XRT_INPUT_META_FULL_BODY_TRACKING) { 377 377 + return XRT_SUCCESS; 378 378 + } 379 379 + joints[14] = offset_joint(joints[5], XRT_FULL_BODY_JOINT_LEFT_UPPER_LEG_META, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_LEFT_UPPER_LEG].length, 0.f}); 380 380 + joints[15] = offset_joint(joints[5], XRT_FULL_BODY_JOINT_RIGHT_UPPER_LEG_META, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_RIGHT_UPPER_LEG].length, 0.f}); 381 381 + joints[16] = offset_joint(joints[14], XRT_FULL_BODY_JOINT_LEFT_LOWER_LEG_META, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_LEFT_LOWER_LEG].length, 0.f}); 382 382 + joints[17] = offset_joint(joints[15], XRT_FULL_BODY_JOINT_RIGHT_LOWER_LEG_META, (struct xrt_vec3){0.f, -device->bones[SOLARXR_BODY_PART_RIGHT_LOWER_LEG].length, 0.f}); 383 383 + joints[18] = offset_joint(joints[16], XRT_FULL_BODY_JOINT_LEFT_FOOT_TRANSVERSE_META, (struct xrt_vec3){0.f, 0.f, -device->bones[SOLARXR_BODY_PART_LEFT_FOOT].length}); 384 384 + joints[19] = offset_joint(joints[17], XRT_FULL_BODY_JOINT_RIGHT_FOOT_TRANSVERSE_META, (struct xrt_vec3){0.f, 0.f, -device->bones[SOLARXR_BODY_PART_RIGHT_FOOT].length}); 385 385 + // clang-format on 386 386 + return XRT_SUCCESS; 387 387 + } 388 388 + 389 389 + // TODO: filter enabled bones 390 390 + static xrt_result_t 391 391 + solarxr_device_get_body_joints(struct xrt_device *const xdev, 392 392 + const enum xrt_input_name body_tracking_type, 393 393 + const int64_t desired_timestamp_ns, 394 394 + struct xrt_body_joint_set *const out_value) 395 395 + { 396 396 + static const uint32_t jointMap[SOLARXR_BODY_PART_MAX_ENUM] = { 397 397 + [SOLARXR_BODY_PART_HEAD] = XRT_BODY_JOINT_HEAD_FB, 398 398 + [SOLARXR_BODY_PART_NECK] = XRT_BODY_JOINT_NECK_FB, 399 399 + [SOLARXR_BODY_PART_CHEST] = XRT_BODY_JOINT_SPINE_UPPER_FB, 400 400 + [SOLARXR_BODY_PART_WAIST] = XRT_BODY_JOINT_SPINE_LOWER_FB, 401 401 + [SOLARXR_BODY_PART_HIP] = XRT_BODY_JOINT_HIPS_FB, 402 402 + [SOLARXR_BODY_PART_LEFT_UPPER_LEG] = XRT_FULL_BODY_JOINT_LEFT_UPPER_LEG_META, 403 403 + [SOLARXR_BODY_PART_RIGHT_UPPER_LEG] = XRT_FULL_BODY_JOINT_RIGHT_UPPER_LEG_META, 404 404 + [SOLARXR_BODY_PART_LEFT_LOWER_LEG] = XRT_FULL_BODY_JOINT_LEFT_LOWER_LEG_META, 405 405 + [SOLARXR_BODY_PART_RIGHT_LOWER_LEG] = XRT_FULL_BODY_JOINT_RIGHT_LOWER_LEG_META, 406 406 + [SOLARXR_BODY_PART_LEFT_FOOT] = XRT_FULL_BODY_JOINT_LEFT_FOOT_TRANSVERSE_META, 407 407 + [SOLARXR_BODY_PART_RIGHT_FOOT] = XRT_FULL_BODY_JOINT_RIGHT_FOOT_TRANSVERSE_META, 408 408 + [SOLARXR_BODY_PART_LEFT_LOWER_ARM] = XRT_BODY_JOINT_LEFT_ARM_LOWER_FB, 409 409 + [SOLARXR_BODY_PART_RIGHT_LOWER_ARM] = XRT_BODY_JOINT_RIGHT_ARM_LOWER_FB, 410 410 + [SOLARXR_BODY_PART_LEFT_UPPER_ARM] = XRT_BODY_JOINT_LEFT_ARM_UPPER_FB, 411 411 + [SOLARXR_BODY_PART_RIGHT_UPPER_ARM] = XRT_BODY_JOINT_RIGHT_ARM_UPPER_FB, 412 412 + [SOLARXR_BODY_PART_LEFT_HAND] = XRT_BODY_JOINT_LEFT_HAND_WRIST_FB, 413 413 + [SOLARXR_BODY_PART_RIGHT_HAND] = XRT_BODY_JOINT_RIGHT_HAND_WRIST_FB, 414 414 + [SOLARXR_BODY_PART_LEFT_SHOULDER] = XRT_BODY_JOINT_LEFT_SHOULDER_FB, 415 415 + [SOLARXR_BODY_PART_RIGHT_SHOULDER] = XRT_BODY_JOINT_RIGHT_SHOULDER_FB, 416 416 + [SOLARXR_BODY_PART_UPPER_CHEST] = XRT_BODY_JOINT_CHEST_FB, 417 417 + // LEFT_HIP 418 418 + // RIGHT_HIP 419 419 + }; 420 420 + struct xrt_body_joint_location_fb *joints; 421 421 + uint32_t joint_count; 422 422 + switch (body_tracking_type) { 423 423 + case XRT_INPUT_FB_BODY_TRACKING: { 424 424 + joints = out_value->body_joint_set_fb.joint_locations; 425 425 + joint_count = ARRAY_SIZE(out_value->body_joint_set_fb.joint_locations); 426 426 + break; 427 427 + } 428 428 + case XRT_INPUT_META_FULL_BODY_TRACKING: { 429 429 + joints = out_value->full_body_joint_set_meta.joint_locations; 430 430 + joint_count = ARRAY_SIZE(out_value->full_body_joint_set_meta.joint_locations); 431 431 + break; 432 432 + } 433 433 + default: return XRT_ERROR_NOT_IMPLEMENTED; 434 434 + } 435 435 + 436 436 + struct solarxr_device *const device = solarxr_device(xdev); 437 437 + assert(device != NULL); 438 438 + solarxr_device_sync(device); 439 439 + 440 440 + os_mutex_lock(&device->mutex); 441 441 + 442 442 + out_value->base_body_joint_set_meta.sample_time_ns = device->timestamp; 443 443 + out_value->base_body_joint_set_meta.confidence = 1.f; // N/A 444 444 + out_value->base_body_joint_set_meta.skeleton_changed_count = device->generation; 445 445 + out_value->base_body_joint_set_meta.is_active = true; 446 446 + 447 447 + for (uint32_t i = 0; i < joint_count; ++i) { 448 448 + joints[i].relation = (struct xrt_space_relation)XRT_SPACE_RELATION_ZERO; 449 449 + } 450 450 + 451 451 + for (enum solarxr_body_part part = 0; part < ARRAY_SIZE(device->bones); ++part) { 452 452 + const struct xrt_pose pose = device->bones[part].pose; 453 453 + static_assert(ARRAY_SIZE(jointMap) == ARRAY_SIZE(device->bones), ""); 454 454 + const uint32_t index = jointMap[part]; 455 455 + if (index == 0 || index >= joint_count || 456 456 + memcmp(&pose.orientation, &(struct xrt_quat){0}, sizeof(struct xrt_quat)) == 0) { 457 457 + continue; 458 458 + } 459 459 + 460 460 + joints[index].relation = (struct xrt_space_relation){ 461 461 + .relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 462 462 + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 463 463 + XRT_SPACE_RELATION_POSITION_TRACKED_BIT, 464 464 + .pose = pose, 465 465 + }; 466 466 + } 467 467 + 468 468 + out_value->body_pose = (struct xrt_space_relation){ 469 469 + .relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 470 470 + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT, 471 471 + .pose = XRT_POSE_IDENTITY, 472 472 + }; 473 473 + 474 474 + os_mutex_unlock(&device->mutex); 475 475 + return XRT_SUCCESS; 476 476 + } 477 477 + 478 478 + static void 479 479 + solarxr_device_handle_trackers(struct solarxr_device *const device, 480 480 + const struct span buffer, 481 481 + const solarxr_tracker_data_t trackers[const], 482 482 + const uint32_t trackers_len) 483 483 + { 484 484 + for (uint32_t i = 0; i < trackers_len; ++i) { 485 485 + struct solarxr_tracker_data data; 486 486 + if (!read_solarxr_tracker_data(&data, buffer.data, buffer.length, &trackers[i])) { 487 487 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), "read_solarxr_device_data() failed"); 488 488 + continue; 489 489 + } 490 490 + 491 491 + // `wmemchr()` should be SIMD optimized, making it faster than a hash lookup in this case despite being 492 492 + // O(n^2) 493 493 + const wchar_t *const match = wmemchr(device->tracker_ids, solarxr_tracker_id_to_wchar(data.tracker_id), 494 494 + ARRAY_SIZE(device->tracker_ids)); 495 495 + if (match == NULL) { 496 496 + continue; 497 497 + } 498 498 + 499 499 + struct m_relation_history *const history = device->trackers[match - device->tracker_ids]; 500 500 + if (history == NULL) { 501 501 + continue; 502 502 + } 503 503 + 504 504 + struct xrt_space_relation relation = {.pose.orientation.w = 1}; 505 505 + if (data.has_rotation) { 506 506 + relation.relation_flags |= 507 507 + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT; 508 508 + relation.pose.orientation = data.rotation; 509 509 + } 510 510 + 511 511 + if (data.has_position) { 512 512 + relation.relation_flags |= 513 513 + XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT; 514 514 + relation.pose.position = data.position; 515 515 + } 516 516 + 517 517 + if (data.has_raw_angular_velocity) { 518 518 + relation.relation_flags |= XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT; 519 519 + relation.angular_velocity = data.raw_angular_velocity; 520 520 + } 521 521 + 522 522 + if (data.has_linear_acceleration) { 523 523 + relation.relation_flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT; 524 524 + relation.linear_velocity = data.linear_acceleration; 525 525 + } 526 526 + 527 527 + if (relation.relation_flags != 0) { 528 528 + m_relation_history_push(history, &relation, device->socket.timestamp); 529 529 + } 530 530 + } 531 531 + } 532 532 + 533 533 + static struct span 534 534 + solarxr_device_receive_blocking(struct solarxr_device *const device) 535 535 + { 536 536 + do { 537 537 + const uint32_t buffer_len = solarxr_ipc_socket_receive(&device->socket); 538 538 + if (buffer_len != 0) { 539 539 + return (struct span){buffer_len, device->socket.buffer}; 540 540 + } 541 541 + } while (solarxr_ipc_socket_wait_timeout(&device->socket, -1)); 542 542 + return (struct span){0}; 543 543 + } 544 544 + 545 545 + static void * 546 546 + solarxr_network_thread(void *const ptr) 547 547 + { 548 548 + struct solarxr_device *const device = (struct solarxr_device *)ptr; 549 549 + for (struct span buffer; (buffer = solarxr_device_receive_blocking(device)).length != 0;) { 550 550 + 551 551 + struct solarxr_message_bundle bundle; 552 552 + if (!read_solarxr_message_bundle(&bundle, buffer.data, buffer.length, 553 553 + (const solarxr_message_bundle_t *)buffer.data)) { 554 554 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), "read_solarxr_message_bundle() failed"); 555 555 + continue; 556 556 + } 557 557 + 558 558 + bool toggles_changed = false; 559 559 + struct solarxr_steamvr_trackers_setting toggles; 560 560 + 561 561 + for (uint32_t i = 0; i < bundle.rpc_msgs.length; ++i) { 562 562 + struct solarxr_rpc_message_header header; 563 563 + if (!read_solarxr_rpc_message_header(&header, buffer.data, buffer.length, 564 564 + &bundle.rpc_msgs.data[i])) { 565 565 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), 566 566 + "read_solarxr_rpc_message_header() failed"); 567 567 + continue; 568 568 + } 569 569 + 570 570 + if (header.message_type != SOLARXR_RPC_MESSAGE_TYPE_SETTINGS_RESPONSE) { 571 571 + continue; 572 572 + } 573 573 + 574 574 + toggles = header.message.settings_response.steam_vr_trackers; 575 575 + toggles_changed = true; 576 576 + } 577 577 + 578 578 + if (toggles_changed) { 579 579 + // `SOLARXR_BODY_PART_HEAD` always disabled 580 580 + uint64_t bones = 1llu << SOLARXR_BODY_PART_NECK; 581 581 + bones |= (1llu << SOLARXR_BODY_PART_CHEST) * toggles.chest; 582 582 + bones |= (1llu << SOLARXR_BODY_PART_WAIST) * toggles.waist; 583 583 + bones |= (1llu << SOLARXR_BODY_PART_HIP) * toggles.waist; 584 584 + bones |= (1llu << SOLARXR_BODY_PART_LEFT_UPPER_LEG) * toggles.left_knee; 585 585 + bones |= (1llu << SOLARXR_BODY_PART_RIGHT_UPPER_LEG) * toggles.right_knee; 586 586 + bones |= (1llu << SOLARXR_BODY_PART_LEFT_LOWER_LEG) * (toggles.left_knee && toggles.left_foot); 587 587 + bones |= 588 588 + (1llu << SOLARXR_BODY_PART_RIGHT_LOWER_LEG) * (toggles.right_knee && toggles.right_foot); 589 589 + bones |= (1llu << SOLARXR_BODY_PART_LEFT_FOOT) * toggles.left_foot; 590 590 + bones |= (1llu << SOLARXR_BODY_PART_RIGHT_FOOT) * toggles.right_foot; 591 591 + bones |= (1llu << SOLARXR_BODY_PART_LEFT_LOWER_ARM) * (toggles.left_elbow && toggles.left_hand); 592 592 + bones |= 593 593 + (1llu << SOLARXR_BODY_PART_RIGHT_LOWER_ARM) * (toggles.right_elbow && toggles.right_hand); 594 594 + bones |= (1llu << SOLARXR_BODY_PART_LEFT_UPPER_ARM) * toggles.left_elbow; 595 595 + bones |= (1llu << SOLARXR_BODY_PART_RIGHT_UPPER_ARM) * toggles.right_elbow; 596 596 + bones |= (1llu << SOLARXR_BODY_PART_LEFT_HAND) * toggles.left_hand; 597 597 + bones |= (1llu << SOLARXR_BODY_PART_RIGHT_HAND) * toggles.right_hand; 598 598 + bones |= (1llu << SOLARXR_BODY_PART_LEFT_SHOULDER) * toggles.left_elbow; 599 599 + bones |= (1llu << SOLARXR_BODY_PART_RIGHT_SHOULDER) * toggles.right_elbow; 600 600 + bones |= (1llu << SOLARXR_BODY_PART_UPPER_CHEST) * toggles.chest; 601 601 + bones |= (1llu << SOLARXR_BODY_PART_LEFT_HIP) * toggles.waist; 602 602 + bones |= (1llu << SOLARXR_BODY_PART_RIGHT_HIP) * toggles.waist; 603 603 + bones |= ((2llu << SOLARXR_BODY_PART_LEFT_LITTLE_DISTAL) - 604 604 + (1llu << SOLARXR_BODY_PART_LEFT_THUMB_PROXIMAL)) * 605 605 + toggles.left_hand; 606 606 + bones |= ((2llu << SOLARXR_BODY_PART_RIGHT_LITTLE_DISTAL) - 607 607 + (1llu << SOLARXR_BODY_PART_RIGHT_THUMB_PROXIMAL)) * 608 608 + toggles.right_hand; 609 609 + 610 610 + const uint64_t old_bones = atomic_exchange(&device->enabled_bones, bones); 611 611 + if (old_bones != bones) { 612 612 + U_LOG_IFL_D(debug_get_log_option_solarxr_log(), "Bone mask set to 0x%016" PRIx64, 613 613 + bones); 614 614 + } 615 615 + } 616 616 + 617 617 + if (bundle.data_feed_msgs.length == 0) { 618 618 + continue; 619 619 + } 620 620 + #if 0 // for latency testing 621 621 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), "%.3fus", (os_monotonic_get_ns() - (atomic_load(&device->next_sync) - debug_get_num_option_solarxr_sync_timeout_ms() * U_TIME_1MS_IN_NS)) / 1000.); 622 622 + #endif 623 623 + atomic_store(&device->next_sync, 624 624 + device->timestamp + debug_get_num_option_solarxr_sync_delay_ms() * U_TIME_1MS_IN_NS); 625 625 + 626 626 + os_mutex_lock(&device->mutex); 627 627 + 628 628 + FLATBUFFERS_VECTOR(solarxr_bone_t) bones = {0}; 629 629 + for (uint32_t i = 0; i < bundle.data_feed_msgs.length; ++i) { 630 630 + struct solarxr_data_feed_message_header header; 631 631 + if (!read_solarxr_data_feed_message_header(&header, buffer.data, buffer.length, 632 632 + &bundle.data_feed_msgs.data[0])) { 633 633 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), 634 634 + "read_solarxr_data_feed_message_header() failed"); 635 635 + continue; 636 636 + } 637 637 + 638 638 + if (header.message_type != SOLARXR_DATA_FEED_MESSAGE_DATA_FEED_UPDATE) { 639 639 + continue; 640 640 + } 641 641 + 642 642 + if (debug_get_bool_option_solarxr_raw_trackers()) { 643 643 + for (uint32_t j = 0; j < header.message.data_feed_update.devices.length; ++j) { 644 644 + struct solarxr_device_data device_data; 645 645 + if (!read_solarxr_device_data( 646 646 + &device_data, buffer.data, buffer.length, 647 647 + &header.message.data_feed_update.devices.data[j])) { 648 648 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), 649 649 + "read_solarxr_device_data() failed"); 650 650 + continue; 651 651 + } 652 652 + solarxr_device_handle_trackers(device, buffer, device_data.trackers.data, 653 653 + device_data.trackers.length); 654 654 + } 655 655 + } else { 656 656 + solarxr_device_handle_trackers( 657 657 + device, buffer, header.message.data_feed_update.synthetic_trackers.data, 658 658 + header.message.data_feed_update.synthetic_trackers.length); 659 659 + } 660 660 + 661 661 + if (header.message.data_feed_update.bones.length != 0) { 662 662 + bones.length = header.message.data_feed_update.bones.length; 663 663 + bones.data = header.message.data_feed_update.bones.data; 664 664 + } 665 665 + } 666 666 + 667 667 + if (bones.length != 0) { 668 668 + atomic_store(&device->timestamp, device->socket.timestamp); 669 669 + 670 670 + struct solarxr_device_bone newBones[ARRAY_SIZE(device->bones)] = {0}; 671 671 + for (size_t i = 0; i < bones.length; ++i) { 672 672 + struct solarxr_bone bone; 673 673 + if (!read_solarxr_bone(&bone, buffer.data, buffer.length, &bones.data[i])) { 674 674 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), "read_solarxr_bone() failed"); 675 675 + continue; 676 676 + } 677 677 + 678 678 + if (bone.body_part >= ARRAY_SIZE(device->bones)) { 679 679 + static bool _once = false; 680 680 + if (!_once) { 681 681 + _once = true; 682 682 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), 683 683 + "Unexpected SolarXR BodyPart %u", (unsigned)bone.body_part); 684 684 + } 685 685 + continue; 686 686 + } 687 687 + 688 688 + newBones[bone.body_part].pose = (struct xrt_pose){ 689 689 + .orientation = bone.rotation_g, 690 690 + .position = bone.head_position_g, 691 691 + }; 692 692 + newBones[bone.body_part].length = bone.bone_length; 693 693 + } 694 694 + 695 695 + for (uint32_t i = 0; i < ARRAY_SIZE(device->bones); ++i) { 696 696 + if (memcmp(&newBones[i].length, &device->bones[i].length, sizeof(newBones[i].length)) != 697 697 + 0) { 698 698 + ++device->generation; 699 699 + break; 700 700 + } 701 701 + } 702 702 + 703 703 + memcpy(device->bones, newBones, sizeof(device->bones)); 704 704 + } 705 705 + 706 706 + os_mutex_unlock(&device->mutex); 707 707 + } 708 708 + 709 709 + solarxr_ipc_socket_destroy(&device->socket); 710 710 + return NULL; 711 711 + } 712 712 + 713 713 + static void 714 714 + solarxr_device_destroy(struct xrt_device *xdev) 715 715 + { 716 716 + struct solarxr_device *const device = solarxr_device(xdev); 717 717 + assert(device != NULL); 718 718 + os_mutex_lock(&device->mutex); 719 719 + 720 720 + bool tracker_destroying = false; 721 721 + for (uint32_t i = 0; i < ARRAY_SIZE(device->tracker_refs); ++i) { 722 722 + struct solarxr_generic_tracker *const tracker = device->tracker_refs[i]; 723 723 + if (tracker == NULL) { 724 724 + continue; 725 725 + } 726 726 + 727 727 + os_mutex_lock(&tracker->mutex); 728 728 + if (tracker->parent != NULL) { 729 729 + device->tracker_refs[i] = NULL; 730 730 + } else { 731 731 + tracker_destroying = true; 732 732 + } 733 733 + 734 734 + tracker->parent = NULL; 735 735 + tracker->history = NULL; 736 736 + os_mutex_unlock(&tracker->mutex); 737 737 + } 738 738 + 739 739 + while (tracker_destroying) { 740 740 + // a different thread was blocked in `solarxr_generic_tracker_destroy()` 741 741 + os_mutex_unlock(&device->mutex); 742 742 + sched_yield(); 743 743 + os_mutex_lock(&device->mutex); 744 744 + 745 745 + tracker_destroying = false; 746 746 + for (uint32_t i = 0; i < ARRAY_SIZE(device->tracker_refs); ++i) { 747 747 + if (device->tracker_refs[i] != NULL) { 748 748 + tracker_destroying = true; 749 749 + break; 750 750 + } 751 751 + } 752 752 + } 753 753 + 754 754 + solarxr_ipc_socket_destroy(&device->socket); 755 755 + os_mutex_unlock(&device->mutex); 756 756 + 757 757 + if (!pthread_equal(device->thread.thread, pthread_self())) { 758 758 + os_thread_join(&device->thread); 759 759 + os_thread_destroy(&device->thread); 760 760 + } 761 761 + 762 762 + for (size_t i = 0; i < ARRAY_SIZE(device->trackers); ++i) { 763 763 + m_relation_history_destroy(&device->trackers[i]); 764 764 + } 765 765 + 766 766 + os_mutex_destroy(&device->mutex); 767 767 + u_device_free(&device->base); 768 768 + } 769 769 + 770 770 + static xrt_result_t 771 771 + solarxr_generic_tracker_get_tracked_pose(struct xrt_device *const xdev, 772 772 + const enum xrt_input_name name, 773 773 + const int64_t at_timestamp_ns, 774 774 + struct xrt_space_relation *const out_relation) 775 775 + { 776 776 + struct solarxr_generic_tracker *const device = solarxr_generic_tracker(xdev); 777 777 + assert(device != NULL); 778 778 + 779 779 + os_mutex_lock(&device->mutex); 780 780 + 781 781 + *out_relation = (struct xrt_space_relation){0}; 782 782 + xrt_result_t result = XRT_ERROR_INPUT_UNSUPPORTED; // TODO: adding/removing devices at runtime 783 783 + struct solarxr_device *const parent = device->parent; 784 784 + 785 785 + if (parent != NULL && (atomic_load(&parent->enabled_bones) & (1llu << device->role)) != 0) { 786 786 + solarxr_device_sync(parent); 787 787 + assert(device->history != NULL); 788 788 + m_relation_history_get(device->history, at_timestamp_ns, out_relation); 789 789 + result = XRT_SUCCESS; 790 790 + } 791 791 + 792 792 + os_mutex_unlock(&device->mutex); 793 793 + return result; 794 794 + } 795 795 + 796 796 + static void 797 797 + solarxr_generic_tracker_destroy(struct xrt_device *const xdev) 798 798 + { 799 799 + struct solarxr_generic_tracker *const device = solarxr_generic_tracker(xdev); 800 800 + assert(device != NULL); 801 801 + 802 802 + os_mutex_lock(&device->mutex); 803 803 + 804 804 + struct solarxr_device *const parent = device->parent; 805 805 + device->parent = NULL; 806 806 + device->history = NULL; 807 807 + 808 808 + os_mutex_unlock(&device->mutex); 809 809 + 810 810 + if (parent != NULL) { 811 811 + os_mutex_lock(&parent->mutex); 812 812 + parent->tracker_refs[device->index] = NULL; 813 813 + os_mutex_unlock(&parent->mutex); 814 814 + } 815 815 + 816 816 + os_mutex_destroy(&device->mutex); 817 817 + u_device_free(&device->base); 818 818 + } 819 819 + 820 820 + uint32_t 821 821 + solarxr_device_create_xdevs(struct xrt_tracking_origin *const tracking_origin, 822 822 + struct xrt_device *out_xdevs[const], 823 823 + uint32_t out_xdevs_cap) 824 824 + { 825 825 + if (out_xdevs_cap == 0) { 826 826 + return 0; 827 827 + } 828 828 + 829 829 + if (out_xdevs_cap > 1 + MAX_GENERIC_TRACKERS) { 830 830 + out_xdevs_cap = 1 + MAX_GENERIC_TRACKERS; 831 831 + } 832 832 + 833 833 + struct solarxr_device *const device = U_DEVICE_ALLOCATE(struct solarxr_device, U_DEVICE_ALLOC_NO_FLAGS, 2, 0); 834 834 + device->base.name = XRT_DEVICE_FB_BODY_TRACKING; 835 835 + device->base.device_type = XRT_DEVICE_TYPE_BODY_TRACKER; 836 836 + strncpy(device->base.str, "SolarXR IPC Connection", sizeof(device->base.str) - 1); 837 837 + device->base.tracking_origin = tracking_origin; 838 838 + 839 839 + if (device->base.tracking_origin == NULL) { 840 840 + device->base.tracking_origin = &device->standalone_origin; 841 841 + device->standalone_origin = (struct xrt_tracking_origin){ 842 842 + .name = "SolarXR Bridge", 843 843 + .type = XRT_TRACKING_TYPE_OTHER, 844 844 + .initial_offset = XRT_POSE_IDENTITY, 845 845 + }; 846 846 + } 847 847 + 848 848 + device->base.supported.body_tracking = true; 849 849 + device->base.update_inputs = solarxr_device_update_inputs; 850 850 + device->base.get_body_skeleton = solarxr_device_get_body_skeleton; 851 851 + device->base.get_body_joints = solarxr_device_get_body_joints; 852 852 + device->base.destroy = solarxr_device_destroy; 853 853 + device->base.inputs[0].name = XRT_INPUT_FB_BODY_TRACKING; 854 854 + device->base.inputs[1].name = XRT_INPUT_META_FULL_BODY_TRACKING; 855 855 + device->thread.thread = pthread_self(); 856 856 + 857 857 + const bool use_trackers = (out_xdevs_cap >= 2); 858 858 + device->use_trackers = use_trackers; 859 859 + 860 860 + solarxr_ipc_socket_init(&device->socket, debug_get_log_option_solarxr_log()); 861 861 + memset(device->tracker_ids, 0xff, sizeof(device->tracker_ids)); 862 862 + 863 863 + uint32_t trackers_len = 0; 864 864 + struct solarxr_generic_tracker *trackers[MAX_GENERIC_TRACKERS]; 865 865 + 866 866 + // `solarxr_device_destroy()` asserts unless both have attempted initialization 867 867 + if (os_mutex_init(&device->mutex) != 0) { 868 868 + goto fail; 869 869 + } 870 870 + 871 871 + const struct sockaddr_un path = solarxr_ipc_socket_find(debug_get_log_option_solarxr_log(), "SlimeVRRpc"); 872 872 + if (!solarxr_ipc_socket_connect(&device->socket, path)) { 873 873 + goto fail; 874 874 + } 875 875 + 876 876 + memcpy(device->base.serial, path.sun_path, 877 877 + MIN(sizeof(device->base.serial) - sizeof(char), sizeof(path.sun_path))); 878 878 + 879 879 + struct 880 880 + { 881 881 + uint8_t head[sizeof(struct solarxr_ipc_message)]; 882 882 + struct request_packet 883 883 + { 884 884 + uint32_t _root; 885 885 + uint16_t _table_bundle[4]; 886 886 + struct 887 887 + { // table MessageBundle 888 888 + int32_t _table; 889 889 + uint32_t data_feed_msgs; // vector* 890 890 + uint32_t rpc_msgs; // vector* 891 891 + } bundle; 892 892 + struct 893 893 + { // vector<table RpcMessageHeader> 894 894 + uint32_t length; 895 895 + uint32_t values[1]; // table* 896 896 + } rpc_msgs; 897 897 + uint16_t _table_rpc_msgs_0[5]; 898 898 + struct 899 899 + { // table RpcMessageHeader 900 900 + int32_t _table; 901 901 + uint32_t message; // table* 902 902 + uint8_t message_type; // enum RpcMessage 903 903 + uint8_t _pad[3]; 904 904 + } rpc_msgs_0; 905 905 + uint16_t _table_request[2]; 906 906 + struct 907 907 + { // table SettingsRequest 908 908 + int32_t _table; 909 909 + } request; 910 910 + struct 911 911 + { // vector<table DataFeedMessageHeader> 912 912 + uint32_t length; 913 913 + uint32_t values[1]; // table* 914 914 + } data_feed_msgs; 915 915 + uint16_t _table_data_feed_msgs_0[4]; 916 916 + struct 917 917 + { // table DataFeedMessageHeader 918 918 + int32_t _table; 919 919 + uint32_t message; // table* 920 920 + uint8_t message_type; // enum DataFeedMessage 921 921 + uint8_t _pad[3]; 922 922 + } data_feed_msgs_0; 923 923 + uint16_t _table_shared[3]; 924 924 + struct 925 925 + { // table PollDataFeed 926 926 + int32_t _table; 927 927 + uint32_t config; // table* 928 928 + } message; 929 929 + uint16_t _table_config[6]; 930 930 + struct 931 931 + { // table DataFeedConfig 932 932 + int32_t _table; 933 933 + uint32_t trackers_mask; // table* 934 934 + bool bone_mask; 935 935 + uint8_t _pad[3]; 936 936 + } config; 937 937 + struct 938 938 + { // table DeviceDataMask 939 939 + int32_t _table; 940 940 + uint32_t tracker_data; // table* 941 941 + } data_mask; 942 942 + uint16_t _table_synthetic_trackers_mask[3]; 943 943 + struct 944 944 + { // table TrackerDataMask 945 945 + int32_t _table; 946 946 + bool info; 947 947 + } synthetic_trackers_mask; 948 948 + } body; 949 949 + } const request_packet = 950 950 + { 951 951 + .body = 952 952 + { 953 953 + ._root = htole32(offsetof(struct request_packet, bundle)), 954 954 + ._table_bundle = 955 955 + { 956 956 + htole16(sizeof(request_packet.body._table_bundle)), 957 957 + htole16(sizeof(request_packet.body.bundle)), 958 958 + htole16(use_trackers * (offsetof(struct request_packet, bundle.data_feed_msgs) - 959 959 + offsetof(struct request_packet, bundle))), 960 960 + htole16(offsetof(struct request_packet, bundle.rpc_msgs) - 961 961 + offsetof(struct request_packet, bundle)), 962 962 + }, 963 963 + .bundle = 964 964 + { 965 965 + ._table = htole32(offsetof(struct request_packet, bundle) - 966 966 + offsetof(struct request_packet, _table_bundle)), 967 967 + .data_feed_msgs = 968 968 + htole32(use_trackers * (offsetof(struct request_packet, data_feed_msgs) - 969 969 + offsetof(struct request_packet, bundle.data_feed_msgs))), 970 970 + .rpc_msgs = htole32(offsetof(struct request_packet, rpc_msgs) - 971 971 + offsetof(struct request_packet, bundle.rpc_msgs)), 972 972 + }, 973 973 + .rpc_msgs = 974 974 + { 975 975 + .length = htole32(ARRAY_SIZE(request_packet.body.rpc_msgs.values)), 976 976 + .values = {htole32(offsetof(struct request_packet, rpc_msgs_0) - 977 977 + offsetof(struct request_packet, rpc_msgs.values[0]))}, 978 978 + }, 979 979 + ._table_rpc_msgs_0 = 980 980 + { 981 981 + htole16(sizeof(request_packet.body._table_rpc_msgs_0)), 982 982 + htole16(sizeof(request_packet.body.rpc_msgs_0) - 983 983 + sizeof(request_packet.body.rpc_msgs_0._pad)), 984 984 + 0, 985 985 + htole16(offsetof(struct request_packet, rpc_msgs_0.message_type) - 986 986 + offsetof(struct request_packet, rpc_msgs_0)), 987 987 + htole16(offsetof(struct request_packet, rpc_msgs_0.message) - 988 988 + offsetof(struct request_packet, rpc_msgs_0)), 989 989 + }, 990 990 + .rpc_msgs_0 = 991 991 + { 992 992 + ._table = htole32(offsetof(struct request_packet, rpc_msgs_0) - 993 993 + offsetof(struct request_packet, _table_rpc_msgs_0)), 994 994 + .message = htole32(offsetof(struct request_packet, request) - 995 995 + offsetof(struct request_packet, rpc_msgs_0.message)), 996 996 + .message_type = SOLARXR_RPC_MESSAGE_TYPE_SETTINGS_REQUEST, 997 997 + }, 998 998 + ._table_request = 999 999 + { 1000 1000 + htole16(sizeof(request_packet.body._table_request)), 1001 1001 + htole16(sizeof(request_packet.body.request)), 1002 1002 + }, 1003 1003 + .request = 1004 1004 + { 1005 1005 + ._table = htole32(offsetof(struct request_packet, request) - 1006 1006 + offsetof(struct request_packet, _table_request)), 1007 1007 + }, 1008 1008 + .data_feed_msgs = 1009 1009 + { 1010 1010 + .length = htole32(ARRAY_SIZE(request_packet.body.data_feed_msgs.values)), 1011 1011 + .values = {htole32(offsetof(struct request_packet, data_feed_msgs_0) - 1012 1012 + offsetof(struct request_packet, data_feed_msgs.values[0]))}, 1013 1013 + }, 1014 1014 + ._table_data_feed_msgs_0 = 1015 1015 + { 1016 1016 + htole16(sizeof(request_packet.body._table_data_feed_msgs_0)), 1017 1017 + htole16(sizeof(request_packet.body.data_feed_msgs_0) - 1018 1018 + sizeof(request_packet.body.data_feed_msgs_0._pad)), 1019 1019 + htole16(offsetof(struct request_packet, data_feed_msgs_0.message_type) - 1020 1020 + offsetof(struct request_packet, data_feed_msgs_0)), 1021 1021 + htole16(offsetof(struct request_packet, data_feed_msgs_0.message) - 1022 1022 + offsetof(struct request_packet, data_feed_msgs_0)), 1023 1023 + }, 1024 1024 + .data_feed_msgs_0 = 1025 1025 + { 1026 1026 + ._table = htole32(offsetof(struct request_packet, data_feed_msgs_0) - 1027 1027 + offsetof(struct request_packet, _table_data_feed_msgs_0)), 1028 1028 + .message = htole32(offsetof(struct request_packet, message) - 1029 1029 + offsetof(struct request_packet, data_feed_msgs_0.message)), 1030 1030 + .message_type = SOLARXR_DATA_FEED_MESSAGE_POLL_DATA_FEED, 1031 1031 + }, 1032 1032 + ._table_shared = 1033 1033 + { 1034 1034 + htole16(sizeof(request_packet.body._table_shared)), 1035 1035 + htole16(8), 1036 1036 + htole16(4), 1037 1037 + }, 1038 1038 + .message = 1039 1039 + { 1040 1040 + ._table = htole32(offsetof(struct request_packet, message) - 1041 1041 + offsetof(struct request_packet, _table_shared)), 1042 1042 + .config = htole32(offsetof(struct request_packet, config) - 1043 1043 + offsetof(struct request_packet, message.config)), 1044 1044 + }, 1045 1045 + ._table_config = 1046 1046 + { 1047 1047 + htole16(sizeof(request_packet.body._table_config)), 1048 1048 + htole16(sizeof(request_packet.body.config) - sizeof(request_packet.body.config._pad)), 1049 1049 + 0, 1050 1050 + htole16((device->use_trackers && debug_get_bool_option_solarxr_raw_trackers()) * 1051 1051 + (offsetof(struct request_packet, config.trackers_mask) - 1052 1052 + offsetof(struct request_packet, config))), 1053 1053 + htole16((device->use_trackers && !debug_get_bool_option_solarxr_raw_trackers()) * 1054 1054 + (offsetof(struct request_packet, config.trackers_mask) - 1055 1055 + offsetof(struct request_packet, config))), 1056 1056 + htole16(offsetof(struct request_packet, config.bone_mask) - 1057 1057 + offsetof(struct request_packet, config)), 1058 1058 + }, 1059 1059 + .config = 1060 1060 + { 1061 1061 + ._table = htole32(offsetof(struct request_packet, config) - 1062 1062 + offsetof(struct request_packet, _table_config)), 1063 1063 + .trackers_mask = 1064 1064 + htole32((debug_get_bool_option_solarxr_raw_trackers() 1065 1065 + ? offsetof(struct request_packet, data_mask) 1066 1066 + : offsetof(struct request_packet, synthetic_trackers_mask)) - 1067 1067 + offsetof(struct request_packet, config.trackers_mask)), 1068 1068 + .bone_mask = true, 1069 1069 + }, 1070 1070 + .data_mask = 1071 1071 + { 1072 1072 + ._table = htole32(offsetof(struct request_packet, data_mask) - 1073 1073 + offsetof(struct request_packet, _table_shared)), 1074 1074 + .tracker_data = htole32(offsetof(struct request_packet, synthetic_trackers_mask) - 1075 1075 + offsetof(struct request_packet, data_mask.tracker_data)), 1076 1076 + }, 1077 1077 + ._table_synthetic_trackers_mask = 1078 1078 + { 1079 1079 + htole16(sizeof(request_packet.body._table_synthetic_trackers_mask)), 1080 1080 + htole16(sizeof(request_packet.body.synthetic_trackers_mask)), 1081 1081 + htole16(offsetof(struct request_packet, synthetic_trackers_mask.info) - 1082 1082 + offsetof(struct request_packet, synthetic_trackers_mask)), 1083 1083 + }, 1084 1084 + .synthetic_trackers_mask = 1085 1085 + { 1086 1086 + ._table = htole32(offsetof(struct request_packet, synthetic_trackers_mask) - 1087 1087 + offsetof(struct request_packet, _table_synthetic_trackers_mask)), 1088 1088 + .info = true, 1089 1089 + }, 1090 1090 + }, 1091 1091 + }; 1092 1092 + 1093 1093 + if (!solarxr_ipc_socket_send_raw( 1094 1094 + &device->socket, (const uint8_t *)&request_packet, 1095 1095 + solarxr_ipc_message_inline((uint8_t *)&request_packet, 1096 1096 + use_trackers ? sizeof(request_packet) 1097 1097 + : sizeof(request_packet.head) + 1098 1098 + offsetof(struct request_packet, data_feed_msgs)))) { 1099 1099 + U_LOG_IFL_E(debug_get_log_option_solarxr_log(), "solarxr_ipc_socket_send_raw() failed"); 1100 1100 + goto fail; 1101 1101 + } 1102 1102 + 1103 1103 + if (use_trackers) { 1104 1104 + const struct span buffer = solarxr_device_receive_blocking(device); 1105 1105 + if (buffer.length == 0) { 1106 1106 + U_LOG_IFL_E(debug_get_log_option_solarxr_log(), "solarxr_ipc_socket_receive() failed"); 1107 1107 + goto fail; 1108 1108 + } 1109 1109 + 1110 1110 + struct solarxr_message_bundle bundle; 1111 1111 + if (!read_solarxr_message_bundle(&bundle, buffer.data, buffer.length, 1112 1112 + (const solarxr_message_bundle_t *)buffer.data)) { 1113 1113 + U_LOG_IFL_E(debug_get_log_option_solarxr_log(), "read_solarxr_message_bundle() failed"); 1114 1114 + goto fail; 1115 1115 + } 1116 1116 + 1117 1117 + if (bundle.data_feed_msgs.length != 1) { 1118 1118 + U_LOG_IFL_E(debug_get_log_option_solarxr_log(), "Unexpected data feed count"); 1119 1119 + goto fail; 1120 1120 + } 1121 1121 + 1122 1122 + struct solarxr_data_feed_message_header header; 1123 1123 + if (!read_solarxr_data_feed_message_header(&header, buffer.data, buffer.length, 1124 1124 + &bundle.data_feed_msgs.data[0])) { 1125 1125 + U_LOG_IFL_E(debug_get_log_option_solarxr_log(), 1126 1126 + "read_solarxr_data_feed_message_header() failed"); 1127 1127 + goto fail; 1128 1128 + } 1129 1129 + 1130 1130 + if (header.message_type != SOLARXR_DATA_FEED_MESSAGE_DATA_FEED_UPDATE) { 1131 1131 + U_LOG_IFL_E(debug_get_log_option_solarxr_log(), "Unexpected data feed message type"); 1132 1132 + goto fail; 1133 1133 + } 1134 1134 + 1135 1135 + uint32_t tracker_descs_len = 0; 1136 1136 + const solarxr_tracker_data_t *tracker_descs[ARRAY_SIZE(device->trackers)]; 1137 1137 + if (debug_get_bool_option_solarxr_raw_trackers()) { 1138 1138 + for (uint32_t i = 0; i < header.message.data_feed_update.devices.length; ++i) { 1139 1139 + struct solarxr_device_data device_data; 1140 1140 + if (!read_solarxr_device_data(&device_data, buffer.data, buffer.length, 1141 1141 + &header.message.data_feed_update.devices.data[i])) { 1142 1142 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), 1143 1143 + "read_solarxr_device_data() failed"); 1144 1144 + continue; 1145 1145 + } 1146 1146 + 1147 1147 + uint32_t length = device_data.trackers.length; 1148 1148 + if (length >= ARRAY_SIZE(tracker_descs) - tracker_descs_len) { 1149 1149 + length = ARRAY_SIZE(tracker_descs) - tracker_descs_len; 1150 1150 + header.message.data_feed_update.devices.length = i; // early exit 1151 1151 + } 1152 1152 + 1153 1153 + for (uint32_t j = 0; j < length; ++j) { 1154 1154 + tracker_descs[tracker_descs_len++] = &device_data.trackers.data[j]; 1155 1155 + } 1156 1156 + } 1157 1157 + } else { 1158 1158 + tracker_descs_len = 1159 1159 + MIN(header.message.data_feed_update.synthetic_trackers.length, ARRAY_SIZE(tracker_descs)); 1160 1160 + for (uint32_t i = 0; i < tracker_descs_len; ++i) { 1161 1161 + tracker_descs[i] = &header.message.data_feed_update.synthetic_trackers.data[i]; 1162 1162 + } 1163 1163 + } 1164 1164 + 1165 1165 + U_LOG_IFL_I(debug_get_log_option_solarxr_log(), "Enumerated %" PRIu32 " SolarXR %s trackers", 1166 1166 + tracker_descs_len, debug_get_bool_option_solarxr_raw_trackers() ? "physical" : "synthetic"); 1167 1167 + 1168 1168 + if (tracker_descs_len > out_xdevs_cap - 1) { 1169 1169 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), 1170 1170 + "Not enough xdev slots! Omitting %" PRIu32 " trackers", 1171 1171 + tracker_descs_len - (out_xdevs_cap - 1)); 1172 1172 + tracker_descs_len = out_xdevs_cap - 1; 1173 1173 + } 1174 1174 + 1175 1175 + for (uint32_t i = 0; i < tracker_descs_len; ++i) { 1176 1176 + struct solarxr_tracker_data data; 1177 1177 + if (!read_solarxr_tracker_data(&data, buffer.data, buffer.length, tracker_descs[i])) { 1178 1178 + U_LOG_IFL_W(debug_get_log_option_solarxr_log(), "read_solarxr_device_data() failed"); 1179 1179 + continue; 1180 1180 + } 1181 1181 + 1182 1182 + const wchar_t id = solarxr_tracker_id_to_wchar(data.tracker_id); 1183 1183 + 1184 1184 + struct solarxr_generic_tracker *const tracker = 1185 1185 + U_DEVICE_ALLOCATE(struct solarxr_generic_tracker, U_DEVICE_ALLOC_NO_FLAGS, 1, 0); 1186 1186 + if (os_mutex_init(&tracker->mutex) != 0) { 1187 1187 + u_device_free(&tracker->base); 1188 1188 + goto fail; 1189 1189 + } 1190 1190 + 1191 1191 + tracker->base.name = XRT_DEVICE_VIVE_TRACKER; 1192 1192 + tracker->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER; 1193 1193 + snprintf(tracker->base.str, sizeof(tracker->base.str), "SolarXR Tracker %06x", id); 1194 1194 + snprintf(tracker->base.serial, sizeof(tracker->base.serial), "SOLARXR-%06x", id); 1195 1195 + tracker->base.tracking_origin = device->base.tracking_origin; 1196 1196 + tracker->base.supported.orientation_tracking = true; 1197 1197 + tracker->base.supported.position_tracking = true; 1198 1198 + tracker->base.update_inputs = solarxr_generic_tracker_update_inputs; 1199 1199 + tracker->base.get_tracked_pose = solarxr_generic_tracker_get_tracked_pose; 1200 1200 + tracker->base.destroy = solarxr_generic_tracker_destroy; 1201 1201 + tracker->base.inputs[0].name = XRT_INPUT_GENERIC_TRACKER_POSE; 1202 1202 + tracker->role = SOLARXR_BODY_PART_NONE; 1203 1203 + 1204 1204 + device->tracker_ids[i] = id; 1205 1205 + m_relation_history_create(&device->trackers[i]); 1206 1206 + tracker->parent = device; 1207 1207 + tracker->index = i; 1208 1208 + tracker->history = device->trackers[i]; 1209 1209 + device->tracker_refs[i] = tracker; 1210 1210 + 1211 1211 + assert(trackers_len < ARRAY_SIZE(trackers)); 1212 1212 + trackers[trackers_len++] = tracker; 1213 1213 + 1214 1214 + if (!data.has_info) { 1215 1215 + continue; 1216 1216 + } 1217 1217 + 1218 1218 + if (data.info.body_part < ARRAY_SIZE(device->bones)) { 1219 1219 + tracker->role = data.info.body_part; 1220 1220 + } 1221 1221 + 1222 1222 + if (data.info.display_name.length != 0) { 1223 1223 + snprintf(tracker->base.str, sizeof(tracker->base.str), "SolarXR Tracker \"%.*s\"", 1224 1224 + (unsigned)data.info.display_name.length, data.info.display_name.data); 1225 1225 + } 1226 1226 + } 1227 1227 + } 1228 1228 + 1229 1229 + if (os_thread_start(&device->thread, solarxr_network_thread, device) != 0) { 1230 1230 + U_LOG_IFL_E(debug_get_log_option_solarxr_log(), "pthread_create() failed"); 1231 1231 + goto fail; 1232 1232 + } 1233 1233 + 1234 1234 + // early sync to initialize bone lengths needed by `solarxr_device_get_body_skeleton()` 1235 1235 + solarxr_device_sync(device); 1236 1236 + 1237 1237 + assert(1 + trackers_len <= out_xdevs_cap); 1238 1238 + out_xdevs[0] = &device->base; 1239 1239 + for (uint32_t i = 0; i < trackers_len; ++i) { 1240 1240 + out_xdevs[1 + i] = &trackers[i]->base; 1241 1241 + } 1242 1242 + return 1 + trackers_len; 1243 1243 + 1244 1244 + fail: 1245 1245 + for (uint32_t i = 0; i < trackers_len; ++i) { 1246 1246 + solarxr_generic_tracker_destroy(&trackers[i]->base); 1247 1247 + } 1248 1248 + 1249 1249 + solarxr_device_destroy(&device->base); 1250 1250 + return 0; 1251 1251 + }

+37

src/xrt/drivers/solarxr/solarxr_interface.h

··· 1 1 + // Copyright 2025, rcelyte 2 2 + // SPDX-License-Identifier: BSL-1.0 3 3 + /*! 4 4 + * @file 5 5 + * @brief SolarXR protocol bridge device 6 6 + * @ingroup drv_solarxr 7 7 + */ 8 8 + 9 9 + #pragma once 10 10 + 11 11 + #include <stdbool.h> 12 12 + #include <stddef.h> 13 13 + #include <stdint.h> 14 14 + struct xrt_device; 15 15 + struct xrt_tracking_origin; 16 16 + 17 17 + #ifdef __cplusplus 18 18 + extern "C" { 19 19 + #endif 20 20 + 21 21 + uint32_t 22 22 + solarxr_device_create_xdevs(struct xrt_tracking_origin *tracking_origin, 23 23 + struct xrt_device *out_xdevs[], 24 24 + uint32_t out_xdevs_cap); 25 25 + 26 26 + static inline struct xrt_device * 27 27 + solarxr_device_create(struct xrt_tracking_origin *const tracking_origin) 28 28 + { 29 29 + struct xrt_device *out = NULL; 30 30 + solarxr_device_create_xdevs(tracking_origin, &out, 1); 31 31 + return out; 32 32 + } 33 33 + 34 34 + 35 35 + #ifdef __cplusplus 36 36 + } 37 37 + #endif

+86

src/xrt/drivers/solarxr/solarxr_ipc_message.h

··· 1 1 + // Copyright 2025, rcelyte 2 2 + // SPDX-License-Identifier: BSL-1.0 3 3 + 4 4 + #pragma once 5 5 + #include <endian.h> 6 6 + #include <string.h> 7 7 + 8 8 + #ifdef __cplusplus 9 9 + extern "C" { 10 10 + #endif 11 11 + 12 12 + struct solarxr_ipc_message 13 13 + { 14 14 + uint32_t length; 15 15 + uint8_t body[]; 16 16 + }; 17 17 + 18 18 + static inline struct solarxr_ipc_message * 19 19 + solarxr_ipc_message_start(uint8_t head[const], const uint8_t *const end) 20 20 + { 21 21 + if ((size_t)(end - head) < sizeof(struct solarxr_ipc_message)) { 22 22 + return NULL; 23 23 + } 24 24 + 25 25 + struct solarxr_ipc_message *const message = (struct solarxr_ipc_message *)head; 26 26 + message->length = 0; 27 27 + return message; 28 28 + } 29 29 + 30 30 + static inline bool 31 31 + solarxr_ipc_message_write(struct solarxr_ipc_message *const message, 32 32 + const uint8_t *const end, 33 33 + const uint8_t data[const], 34 34 + const uint32_t data_len) 35 35 + { 36 36 + if (message == NULL) { 37 37 + return false; 38 38 + } 39 39 + 40 40 + if ((size_t)(end - message->body) < message->length || end - &message->body[message->length] < data_len) { 41 41 + message->length = UINT32_MAX; 42 42 + return false; 43 43 + } 44 44 + 45 45 + memcpy(&message->body[message->length], data, data_len); 46 46 + message->length += data_len; 47 47 + return true; 48 48 + } 49 49 + 50 50 + static inline uint32_t 51 51 + solarxr_ipc_message_end(struct solarxr_ipc_message *const message, uint8_t **const end_out) 52 52 + { 53 53 + if (message == NULL || message->length >= UINT32_MAX - sizeof(*message)) { 54 54 + return 0; 55 55 + } 56 56 + 57 57 + *end_out = &message->body[message->length]; 58 58 + const uint32_t length = sizeof(*message) + message->length; 59 59 + message->length = htole32(length); 60 60 + return length; 61 61 + } 62 62 + 63 63 + static inline uint32_t 64 64 + solarxr_ipc_message_write_single(uint8_t **const head, 65 65 + const uint8_t *const end, 66 66 + const uint8_t data[const], 67 67 + const uint32_t data_len) 68 68 + { 69 69 + struct solarxr_ipc_message *const message = solarxr_ipc_message_start(*head, end); 70 70 + solarxr_ipc_message_write(message, end, data, data_len); 71 71 + return solarxr_ipc_message_end(message, head); 72 72 + } 73 73 + 74 74 + static inline uint32_t 75 75 + solarxr_ipc_message_inline(uint8_t data[const], const uint32_t data_len) 76 76 + { 77 77 + struct solarxr_ipc_message *const message = solarxr_ipc_message_start(data, &data[data_len]); 78 78 + if (message != NULL) { 79 79 + message->length = &data[data_len] - message->body; 80 80 + } 81 81 + return solarxr_ipc_message_end(message, &(uint8_t *){NULL}); 82 82 + } 83 83 + 84 84 + #ifdef __cplusplus 85 85 + } 86 86 + #endif

+338

src/xrt/drivers/solarxr/solarxr_ipc_socket.c

··· 1 1 + // Copyright 2025, rcelyte 2 2 + // SPDX-License-Identifier: BSL-1.0 3 3 + 4 4 + #include "solarxr_ipc_socket.h" 5 5 + #include "solarxr_ipc_message.h" 6 6 + 7 7 + #include "os/os_time.h" 8 8 + #include "shared/ipc_message_channel.h" 9 9 + #include "util/u_file.h" 10 10 + 11 11 + #include <endian.h> 12 12 + #include <errno.h> 13 13 + #include <netinet/in.h> 14 14 + #include <poll.h> 15 15 + #include <sched.h> 16 16 + #include <stdlib.h> 17 17 + #include <string.h> 18 18 + #include <sys/stat.h> 19 19 + #include <unistd.h> 20 20 + 21 21 + #define SLIMEVR_IDENTIFIER "dev.slimevr.SlimeVR" 22 22 + 23 23 + static bool 24 24 + solarxr_ipc_socket_ensure_capacity(struct solarxr_ipc_socket *const state, const uint32_t capacity) 25 25 + { 26 26 + if (state->buffer_cap >= capacity) { 27 27 + return true; 28 28 + } 29 29 + 30 30 + if (capacity > 0x100000u) { 31 31 + U_LOG_IFL_E(state->log_level, "packet too large"); 32 32 + return false; 33 33 + } 34 34 + 35 35 + uint8_t *const new_buffer = realloc(state->buffer, capacity); 36 36 + if (new_buffer == NULL) { 37 37 + U_LOG_IFL_E(state->log_level, "realloc failed"); 38 38 + return false; 39 39 + } 40 40 + 41 41 + state->buffer = new_buffer; 42 42 + state->buffer_cap = capacity; 43 43 + return true; 44 44 + } 45 45 + 46 46 + void 47 47 + solarxr_ipc_socket_init(struct solarxr_ipc_socket *const state, const enum u_logging_level log_level) 48 48 + { 49 49 + *state = (struct solarxr_ipc_socket){ 50 50 + .ipc_handle = XRT_IPC_HANDLE_INVALID, 51 51 + .log_level = log_level, 52 52 + .timestamp = os_monotonic_get_ns(), 53 53 + }; 54 54 + } 55 55 + 56 56 + static bool 57 57 + solarxr_ipc_socket_close(struct solarxr_ipc_socket *const state) 58 58 + { 59 59 + struct ipc_message_channel channel = { 60 60 + .ipc_handle = atomic_exchange(&state->ipc_handle, XRT_IPC_HANDLE_INVALID), 61 61 + .log_level = state->log_level, 62 62 + }; 63 63 + if (channel.ipc_handle == XRT_IPC_HANDLE_INVALID) { 64 64 + return false; 65 65 + } 66 66 + 67 67 + shutdown(channel.ipc_handle, SHUT_RDWR); // unblock `solarxr_ipc_socket_wait_timeout()` 68 68 + static_assert(sizeof(state->reference.count) == sizeof(volatile _Atomic(int)), ""); 69 69 + 70 70 + while (atomic_load((volatile _Atomic(int) *)&state->reference.count) != 0) { 71 71 + sched_yield(); 72 72 + } 73 73 + 74 74 + ipc_message_channel_close(&channel); 75 75 + return true; 76 76 + } 77 77 + 78 78 + static void 79 79 + solarxr_ipc_socket_free_buffer(struct solarxr_ipc_socket *const state) 80 80 + { 81 81 + free(state->buffer); 82 82 + state->buffer = NULL; 83 83 + state->buffer_cap = 0; 84 84 + } 85 85 + 86 86 + void 87 87 + solarxr_ipc_socket_destroy(struct solarxr_ipc_socket *const state) 88 88 + { 89 89 + // guards against double-free 90 90 + if (solarxr_ipc_socket_close(state)) { 91 91 + solarxr_ipc_socket_free_buffer(state); 92 92 + } 93 93 + } 94 94 + 95 95 + static bool 96 96 + check_socket_path(const enum u_logging_level log_level, const char path[const]) 97 97 + { 98 98 + struct stat result = {0}; 99 99 + const bool found = stat(path, &result) == 0 && S_ISSOCK(result.st_mode); 100 100 + if (!found) { 101 101 + U_LOG_IFL_W(log_level, "path not found: %s", path); 102 102 + } 103 103 + return found; 104 104 + } 105 105 + 106 106 + struct sockaddr_un 107 107 + solarxr_ipc_socket_find(const enum u_logging_level log_level, const char filename[const]) 108 108 + { 109 109 + struct sockaddr_un addr = { 110 110 + .sun_family = AF_UNIX, 111 111 + }; 112 112 + const size_t filename_len = strlen(filename); 113 113 + if (filename_len >= sizeof(addr.sun_path)) { 114 114 + U_LOG_IFL_E(log_level, "filename too long"); 115 115 + return (struct sockaddr_un){0}; 116 116 + } 117 117 + 118 118 + const ssize_t runtime_dir_len = 119 119 + u_file_get_path_in_runtime_dir("", addr.sun_path, sizeof(addr.sun_path) - 1 - filename_len); 120 120 + if (runtime_dir_len <= 0 || (size_t)runtime_dir_len >= sizeof(addr.sun_path) - 1 - filename_len) { 121 121 + U_LOG_IFL_E(log_level, "u_file_get_path_in_runtime_dir() failed"); 122 122 + return (struct sockaddr_un){0}; 123 123 + } 124 124 + 125 125 + // try path in runtime dir 126 126 + memcpy(&addr.sun_path[runtime_dir_len], filename, filename_len + 1); // include null terminator 127 127 + if (check_socket_path(log_level, addr.sun_path)) { 128 128 + return addr; 129 129 + } 130 130 + 131 131 + // try path used by SlimeVR flatpak 132 132 + static const char flatpak_runtime_dir[] = "app/" SLIMEVR_IDENTIFIER "/"; 133 133 + if (sizeof(addr.sun_path) - 1 - filename_len - runtime_dir_len >= sizeof(flatpak_runtime_dir) - 1) { 134 134 + memcpy(&addr.sun_path[runtime_dir_len], flatpak_runtime_dir, sizeof(flatpak_runtime_dir) - 1); 135 135 + memcpy(&addr.sun_path[runtime_dir_len + (sizeof(flatpak_runtime_dir) - 1)], filename, filename_len + 1); 136 136 + if (check_socket_path(log_level, addr.sun_path)) { 137 137 + return addr; 138 138 + } 139 139 + } 140 140 + 141 141 + // try fallback path in SlimeVR data dir 142 142 + const char *env; 143 143 + int path_len = 0; 144 144 + if ((env = getenv("XDG_DATA_HOME")) != NULL) { 145 145 + path_len = 146 146 + snprintf(addr.sun_path, sizeof(addr.sun_path), "%s/" SLIMEVR_IDENTIFIER "/%s", env, filename); 147 147 + } else if ((env = getenv("HOME")) != NULL) { 148 148 + path_len = snprintf(addr.sun_path, sizeof(addr.sun_path), "%s/.local/share/" SLIMEVR_IDENTIFIER "/%s", 149 149 + env, filename); 150 150 + } 151 151 + 152 152 + if (path_len <= 0 || (unsigned)path_len >= sizeof(addr.sun_path) || 153 153 + !check_socket_path(log_level, addr.sun_path)) { 154 154 + U_LOG_IFL_E(log_level, "failed to resolve socket path"); 155 155 + return (struct sockaddr_un){0}; 156 156 + } 157 157 + 158 158 + return addr; 159 159 + } 160 160 + 161 161 + bool 162 162 + solarxr_ipc_socket_connect(struct solarxr_ipc_socket *const state, const struct sockaddr_un addr) 163 163 + { 164 164 + solarxr_ipc_socket_close(state); 165 165 + 166 166 + xrt_ipc_handle_t ipc_handle = XRT_IPC_HANDLE_INVALID; 167 167 + if (addr.sun_family != AF_UNIX) { 168 168 + goto fail; 169 169 + } 170 170 + 171 171 + #ifdef SOCK_CLOEXEC 172 172 + const int flags = SOCK_CLOEXEC; 173 173 + #else 174 174 + const int flags = 0; 175 175 + #endif 176 176 + ipc_handle = socket(PF_UNIX, SOCK_STREAM | flags, 0); 177 177 + if (ipc_handle == XRT_IPC_HANDLE_INVALID) { 178 178 + U_LOG_IFL_E(state->log_level, "socket() failed"); 179 179 + goto fail; 180 180 + } 181 181 + 182 182 + U_LOG_IFL_I(state->log_level, "Connecting to domain socket: %s", addr.sun_path); 183 183 + if (connect(ipc_handle, (const struct sockaddr *)&addr, sizeof(addr)) != 0) { 184 184 + U_LOG_IFL_E(state->log_level, "connect() failed: %s", strerror(errno)); 185 185 + goto fail; 186 186 + } 187 187 + 188 188 + // optimistic allocation to avoid needless reallocs during receive 189 189 + solarxr_ipc_socket_ensure_capacity(state, 0x1000); 190 190 + 191 191 + atomic_store(&state->ipc_handle, ipc_handle); 192 192 + return true; 193 193 + 194 194 + fail: 195 195 + xrt_ipc_handle_close(ipc_handle); 196 196 + 197 197 + // The IPC handle is used as a lock guard in `solarxr_ipc_socket_destroy()`, so this is the last place to safely 198 198 + // free the buffer without one 199 199 + solarxr_ipc_socket_free_buffer(state); 200 200 + 201 201 + return false; 202 202 + } 203 203 + 204 204 + // release reference with `xrt_reference_dec(&state->reference);` 205 205 + static bool 206 206 + solarxr_ipc_socket_reference_channel(struct solarxr_ipc_socket *const state, 207 207 + struct ipc_message_channel *const channel_out) 208 208 + { 209 209 + xrt_reference_inc(&state->reference); 210 210 + 211 211 + const xrt_ipc_handle_t ipc_handle = atomic_load(&state->ipc_handle); 212 212 + if (ipc_handle == XRT_IPC_HANDLE_INVALID) { 213 213 + xrt_reference_dec(&state->reference); 214 214 + return false; 215 215 + } 216 216 + 217 217 + *channel_out = (struct ipc_message_channel){ 218 218 + .ipc_handle = ipc_handle, 219 219 + .log_level = state->log_level, 220 220 + }; 221 221 + return true; 222 222 + } 223 223 + 224 224 + bool 225 225 + solarxr_ipc_socket_wait_timeout(struct solarxr_ipc_socket *const state, const time_duration_ns timeout) 226 226 + { 227 227 + struct ipc_message_channel channel; 228 228 + if (!solarxr_ipc_socket_reference_channel(state, &channel)) { 229 229 + return false; 230 230 + } 231 231 + 232 232 + struct pollfd fd = { 233 233 + channel.ipc_handle, 234 234 + POLLIN, 235 235 + 0, 236 236 + }; 237 237 + const struct timespec timeout_ts = { 238 238 + .tv_sec = timeout / U_TIME_1S_IN_NS, 239 239 + .tv_nsec = timeout % U_TIME_1S_IN_NS, 240 240 + }; 241 241 + const bool result = (ppoll(&fd, 1, (timeout >= 0) ? &timeout_ts : NULL, NULL) != -1) 242 242 + ? (fd.revents & POLLERR) == 0 243 243 + : errno == EINTR; 244 244 + 245 245 + xrt_reference_dec(&state->reference); 246 246 + return result; 247 247 + } 248 248 + 249 249 + bool 250 250 + solarxr_ipc_socket_send_raw(struct solarxr_ipc_socket *const state, 251 251 + const uint8_t packet[const], 252 252 + const uint32_t packet_len) 253 253 + { 254 254 + struct ipc_message_channel channel; 255 255 + if (!solarxr_ipc_socket_reference_channel(state, &channel)) { 256 256 + return false; 257 257 + } 258 258 + 259 259 + const xrt_result_t result = ipc_send(&channel, packet, packet_len); 260 260 + 261 261 + xrt_reference_dec(&state->reference); 262 262 + return result == XRT_SUCCESS; 263 263 + } 264 264 + 265 265 + static bool 266 266 + recv_nonblock(const struct ipc_message_channel channel, 267 267 + uint8_t buffer[const], 268 268 + uint32_t *const head, 269 269 + const uint32_t buffer_cap) 270 270 + { 271 271 + // TODO: use a platform agnostic function like `ipc_receive()`, but with support for partial reads 272 272 + const ssize_t length = recv(channel.ipc_handle, &buffer[*head], buffer_cap - *head, MSG_DONTWAIT); 273 273 + if (length < 0) { 274 274 + if (errno == EAGAIN) { 275 275 + return true; 276 276 + } 277 277 + U_LOG_IFL_E(channel.log_level, "recv() failed: %s", strerror(errno)); 278 278 + return false; 279 279 + } 280 280 + 281 281 + if (length > buffer_cap - *head) { 282 282 + U_LOG_IFL_E(channel.log_level, "recv() returned invalid length"); 283 283 + return false; 284 284 + } 285 285 + 286 286 + *head += (size_t)length; 287 287 + return true; 288 288 + } 289 289 + 290 290 + uint32_t 291 291 + solarxr_ipc_socket_receive(struct solarxr_ipc_socket *const state) 292 292 + { 293 293 + struct ipc_message_channel channel; 294 294 + if (!solarxr_ipc_socket_reference_channel(state, &channel)) { 295 295 + return 0; 296 296 + } 297 297 + 298 298 + if (state->head == state->buffer_len) { 299 299 + state->buffer_len = 0; 300 300 + state->head = 0; 301 301 + } 302 302 + 303 303 + if (state->buffer_len == 0) { 304 304 + struct solarxr_ipc_message header; 305 305 + if (!solarxr_ipc_socket_ensure_capacity(state, sizeof(header)) || 306 306 + !recv_nonblock(channel, state->buffer, &state->head, sizeof(header))) { 307 307 + goto fail; 308 308 + } 309 309 + 310 310 + if (state->head < sizeof(header)) { 311 311 + goto unref; 312 312 + } 313 313 + 314 314 + memcpy(&header, state->buffer, sizeof(header)); 315 315 + 316 316 + const uint32_t packet_length = le32toh(header.length) - sizeof(header); 317 317 + if (!solarxr_ipc_socket_ensure_capacity(state, packet_length)) { 318 318 + goto fail; 319 319 + } 320 320 + 321 321 + state->buffer_len = packet_length; 322 322 + state->head = 0; 323 323 + state->timestamp = os_monotonic_get_ns(); 324 324 + } 325 325 + 326 326 + if (!recv_nonblock(channel, state->buffer, &state->head, state->buffer_len)) { 327 327 + goto fail; 328 328 + } 329 329 + 330 330 + unref: 331 331 + xrt_reference_dec(&state->reference); 332 332 + return (state->head < state->buffer_len) ? 0 : state->buffer_len; 333 333 + 334 334 + fail: 335 335 + xrt_reference_dec(&state->reference); 336 336 + solarxr_ipc_socket_destroy(state); 337 337 + return 0; 338 338 + }

+61

src/xrt/drivers/solarxr/solarxr_ipc_socket.h

··· 1 1 + // Copyright 2025, rcelyte 2 2 + // SPDX-License-Identifier: BSL-1.0 3 3 + 4 4 + #pragma once 5 5 + #include "util/u_logging.h" 6 6 + #include "util/u_time.h" 7 7 + #include "xrt/xrt_handles.h" 8 8 + 9 9 + #include <stdatomic.h> 10 10 + #ifdef XRT_OS_WINDOWS 11 11 + #include <afunix.h> 12 12 + #else 13 13 + #include <sys/un.h> 14 14 + #endif 15 15 + 16 16 + #ifdef __cplusplus 17 17 + extern "C" { 18 18 + #endif 19 19 + 20 20 + struct solarxr_ipc_socket 21 21 + { 22 22 + _Atomic(xrt_ipc_handle_t) ipc_handle; 23 23 + struct xrt_reference reference; 24 24 + enum u_logging_level log_level; 25 25 + timepoint_ns timestamp; 26 26 + uint32_t head, buffer_len, buffer_cap; 27 27 + uint8_t *buffer; 28 28 + }; 29 29 + 30 30 + void 31 31 + solarxr_ipc_socket_init(struct solarxr_ipc_socket *state, enum u_logging_level log_level); 32 32 + 33 33 + void 34 34 + solarxr_ipc_socket_destroy(struct solarxr_ipc_socket *state); // thread safe 35 35 + 36 36 + struct sockaddr_un 37 37 + solarxr_ipc_socket_find(enum u_logging_level log_level, const char filename[]); 38 38 + 39 39 + bool 40 40 + solarxr_ipc_socket_connect(struct solarxr_ipc_socket *state, struct sockaddr_un addr); 41 41 + 42 42 + bool 43 43 + solarxr_ipc_socket_wait_timeout(struct solarxr_ipc_socket *state, time_duration_ns timeout); // thread safe 44 44 + 45 45 + bool 46 46 + solarxr_ipc_socket_send_raw(struct solarxr_ipc_socket *state, 47 47 + const uint8_t packet[], 48 48 + uint32_t packet_len); // thread safe 49 49 + 50 50 + uint32_t 51 51 + solarxr_ipc_socket_receive(struct solarxr_ipc_socket *state); 52 52 + 53 53 + static inline bool 54 54 + solarxr_ipc_socket_is_connected(struct solarxr_ipc_socket *const state) 55 55 + { 56 56 + return atomic_load(&state->ipc_handle) != XRT_IPC_HANDLE_INVALID; 57 57 + } 58 58 + 59 59 + #ifdef __cplusplus 60 60 + } 61 61 + #endif

+4

src/xrt/targets/common/CMakeLists.txt

··· 209 209 target_link_libraries(target_lists PRIVATE drv_svr) 210 210 endif() 211 211 212 212 + if(XRT_BUILD_DRIVER_SOLARXR) 213 213 + target_link_libraries(target_lists PRIVATE drv_solarxr) 214 214 + endif() 215 215 + 212 216 #### 213 217 # Instance 214 218 #