#include #include #include #include #include #include #include #include #include #include #if !defined(__FreeBSD__) && !defined(__APPLE__) #include #include // for alloca #endif #include "json_helpers.h" #include "survive_config.h" #include "survive_default_devices.h" #include "survive_str.h" #include "driver_vive.h" #include "lfsr_lh2.h" //#define DEBUG_WATCHMAN 1 struct SurviveViveData; struct DeviceInfo { const char *name; const char *codename; uint16_t vid; uint16_t pid; enum USB_DEV_t type; struct Endpoint_t { uint8_t num; const char *name; enum USB_IF_t type; } endpoints[MAX_INTERFACES_PER_DEVICE]; struct Magic_t { const char *name; bool code; const uint8_t *magic; const size_t length; } magics[8]; }; enum vive_report_ids { // Possibly 0x03 - Gyro range, 0x02 - Accel range // Tracker/lh1/[rf,usb]: (device -> steamvr): 01 03 02 VIVE_REPORT_IMU_SCALES = 1, // You can get this from doing errors in lighthouse_console: /* 0300000000000000000000000000000000000000000000000000000000000000000000000000000000010000000000325160d6066f0394c59affa228a02870f7 missing_rising_edge : 0 backward_time : 0 pulse_queue_overflow : 0 short_sync : 0 long_sync : 0 invalid_calibration_size : 0 invalid_calibration_crc : 0 invalid_calibration_version : 0 queue_overflow : 0 spammy_sensor : 0 flags : 1 long_optical_packet_delay : 0 0300000000000000000000000080c44300020000000000000000000000000000005700000000000000010000000000000006010000f3ff0357a915ca8ebd47d5 missing_rising_edge : 0 backward_time : 0 pulse_queue_overflow : 0 short_sync : 4441216 long_sync : 2 invalid_calibration_size : 0 invalid_calibration_crc : 0 invalid_calibration_version : 0 queue_overflow : 87 spammy_sensor : 0 flags : 1 long_optical_packet_delay : 0 */ VIVE_REPORT_ERRORS = 0x03, VIVE_REPORT_IMU = 0x20, VIVE_REPORT_USB_TRACKER_LIGHTCAP_V1 = 0x21, VIVE_REPORT_RF_WATCHMAN = 0x23, VIVE_REPORT_RF_WATCHMANx2 = 0x24, VIVE_REPORT_USB_LIGHTCAP_REPORT_V1 = 0x25, // I've seen this with a byte of data; 01. Maybe this turns on and off? // Note: I've now seen this at connection too? Maybe its just what happens when you hold the button down? VIVE_REPORT_RF_TURN_OFF = 0x26, VIVE_REPORT_USB_LIGHTCAP_REPORT_V2 = 0x27, VIVE_REPORT_USB_LIGHTCAP_REPORT_RAW_MODE_1 = 0x28, VIVE_REPORT_COMMAND = 0xff, // Below is from https://github.com/nairol/LighthouseRedox/blob/master/docs/USB%20Protocol.md and not necessarilly // up to date / used VIVE_REPORT_HMD_PROXIMITY = 0x3, VIVE_REPORT_VERSION = 0x05, // Tracker/lh1/[rf,usb]: (steamvr -> device): 04 00 00 00 00 VIVE_REPORT_CHANGE_MODE = 0x04, // Tracker/lh1/[rf,usb]: (steamvr -> device): 07 03 00 00 00 VIVE_REPORT_LIGHTCAP_VERBOSITY = 0x07, // Seems like 0x4/0x7 are used in conjunction: // Sync mode 0: // --> 04 00 00 00 00 // --> 07 03 00 00 00 // Sync mode 1: // --> 04 01 00 00 00 // --> 07 03 00 00 00 // Sync mode 2: // --> 04 03 00 00 00 // --> 07 03 00 00 00 // Tracker/lh1/rf: (steamvr -> device): (CLEAR_FEATURE) // (device -> steamvr): 08 03 00 00 // If clear_feature is sent; seems to be asking how much data is in the user alloc area. // Seems like its maybe current offset as uint32_t, total size as uint32_t. // The tracker seems to not have a userdata section; and responds with 0x8 and then whatever was in the buffer. VIVE_REPORT_HMD_SET_READOFFSET_USERDATA = 0x08, VIVE_REPORT_HMD_READ_USERDATA = 0x09, VIVE_REPORT_CONFIG_READMODE = 0x10, VIVE_REPORT_CONFIG_READ = 0x11, // Sent and echoed by steamvr; no data // Directly proceeds a 0x04... // Possibly a different version packet. Sent when you do 'version' in lh_console. // knuckles -> lighthouse_console: // 13 b5 35 28 5d 03 00 00 00 77 61 74 63 68 6d 61 6e 00 00 00 00 00 00 00 00 56 61 6c 76 65 42 75 | // ..5( ]... .wat chma n... .... .Val veBu // 69 6c 64 65 72 30 32 00 00 09 00 0e 11 00 00 bd 26 1a 02 0a 02 f1 52 2c 5c 00 00 00 00 00 00 00 | // ilde r02. .... .... &... ..R, \... .... VIVE_REPORT_VERSION_ALT = 0x13, // Tracker/lh1/usb: (steamvr -> device): 16 01 00 00 00 VIVE_REPORT_REBOOT = 0x16 }; enum vive_commands { // This command seems to dictate protocol and sync model. I think the first byte is the 'protocol', // the second second and fourth together dictate the sync mode. // Tracker/lh1/rf: (steamvr -> device): 01 00 00 02 00 00 | .... .. // Tracker/lh1/rf: (steamvr -> device): 01 00 00 | .... .. // Tracker/lh1/rf: (lhconsole -> device): sync mode 0: 01 00 00 02 00 00 // Tracker/lh1/rf: (lhconsole -> device): sync mode 1: 01 01 00 02 00 00 // Tracker/lh1/rf: (lhconsole -> device): sync mode 2: 01 01 00 02 01 00 // Knuckles/lh2/rf: (steamvr -> device): 01 00 00 02 00 00 // 00 01 00 // 01 01 00 02 00 00 VIVE_COMMAND_CHANGE_PROTOCOL = 0x87, // Tracker/lh1/rf: (steamvr -> device) // - Sends 2x with no data as SET_CONFIGURATION // - Sends 1x as CLEAR_FEATURE // - Recv back: // ff 83 2d 00 b1 d7 b3 ba 01 00 23 00 00 02 0b 00 00 00 04 6a 19 e6 5b 09 06 00 02 84 0b // b1 d7 b3 | ..-. .... ..#. .... ...j ..[. .... .... // ba 05 09 3a 87 5a 0c 00 02 00 00 0a cf 3b 3c 5a | ...: .Z.. .... .; 22.307137 S: T20 ffff8dc3fcadc000 event_type: S transfer_type: 2 bmRequestType: 0x21 bRequest: 0x09 // (SET_CONFIGURATION) wValue: 0x0300 wIndex: 0x0002 wLength: 64 ( 64) // 22.3068941 9f 04 6f 66 66 21 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 // 00 00 00 | ..of f!.. .... .... .... .... .... .... // 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 // 00 00 00 | .... .... .... .... .... .... .... .... // <-- 22.307294 C: T20 ffff8dc3fcadc000 event_type: C transfer_type: 2 0x00 (0x00): VIVE_COMMAND_POWER_OFF = 0x9F, // Knuckles/lh2/rf: (steamvr -> device): be 5b 32 54 11 cf 83 75 53 8a 08 6a 53 58 d0 b1 // Tracker/lh1/rf: (steamvr -> device): be 5b 32 54 11 cf 83 75 53 8a 08 6a 53 58 d0 b1 | .[2T ...u S..j // SX.. // Note: This is sent regardless of whether there is a device setup VIVE_COMMAND_CONFIGURE_RADIO = 0x96, // Info: WM sent command 0xad with 3 bytes: // 01 10 27 VIVE_COMMAND_PAIR = 0xAD, // Keepalive? // Tracker/lh1/rf: (steamvr -> device): // (device -> steamvr): 57 16 a0 b9 32 33 46 45 41 33 44 44 44 41 00 00 00 00 00 00 00 00 // 00 00 00 00 00 00 00 00 00 00 | W... 23FE A3DD DA.. .... .... .... .... // 00 00 | .. VIVE_COMMAND_CHECK_ALIVE = 0xa1, // Knuckles/lh2/rf: (steamvr->sevice): 00 VIVE_COMMAND_UNKNOWN4 = 0xb5, // Knuckles/lh2/rf: (steamvr->sevice) VIVE_COMMAND_UNKNOWN5 = 0x8c, }; static uint8_t vive_magic_power_on[64] = {0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01}; static uint8_t vive_magic_enable_lighthouse[5] = {VIVE_REPORT_CHANGE_MODE}; static uint8_t vive_magic_enable_lighthouse_more[5] = {VIVE_REPORT_LIGHTCAP_VERBOSITY, 0x03}; static uint8_t vive_magic_power_off[] = { 0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60, 0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0, 0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33, 0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00, }; static uint8_t vive_magic_raw_mode_1[] = {VIVE_REPORT_CHANGE_MODE, 0x01, 0x00, 0x00, 0x00}; static uint8_t vive_request_version_info[] = {VIVE_REPORT_VERSION}; #ifdef __linux__ static uint8_t vive_magic_rf_raw_mode_0[] = { #else // Windows needs 64 size for the wireless dongle static uint8_t vive_magic_rf_raw_mode_0[64] = { #endif VIVE_REPORT_COMMAND, VIVE_COMMAND_CHANGE_PROTOCOL, 0x6, 0x01, 0x00, 0x00, 0x02, 0x00, 0x00}; static uint8_t vive_magic_rf_raw_mode_1[] = { VIVE_REPORT_COMMAND, VIVE_COMMAND_CHANGE_PROTOCOL, 0x6, 0x01, 0x01, 0x00, 0x02, 0x00, 0x00}; #ifdef __linux__ static uint8_t vive_magic_protocol_switch[] = { #else // Windows needs 64 size for the wireless dongle static uint8_t vive_magic_protocol_switch[64] = { #endif VIVE_REPORT_COMMAND, VIVE_COMMAND_CHANGE_PROTOCOL, 0x3, 0x00, 0x01, 0x00}; static uint8_t vive_request_pairing[] = {VIVE_REPORT_COMMAND, VIVE_COMMAND_PAIR, 0x03, 0x01, 0x10, 0x27}; #ifdef __linux__ static uint8_t vive_magic_protocol_super_magic[] = { #else // Windows needs 64 size for the wireless dongle static uint8_t vive_magic_protocol_super_magic[64] = { #endif VIVE_REPORT_COMMAND, VIVE_COMMAND_CONFIGURE_RADIO, 0x10, 0xbe, 0x5b, 0x32, 0x54, 0x11, 0xcf, 0x83, 0x75, 0x53, 0x8a, 0x08, 0x6a, 0x53, 0x58, 0xd0, 0xb1}; #define MAGIC_CTOR(ison, buffer) \ { .name = #buffer, .code = ison, .magic = buffer, .length = sizeof(buffer) } const struct DeviceInfo KnownDeviceTypes[] = { {.vid = 0x0bb4, .pid = 0x2c87, .type = USB_DEV_HMD, .name = "BRD", .codename = "", .endpoints = {{.num = 0x81, .name = "Mainboard", .type = USB_IF_HMD_HEADSET_INFO}}, .magics = {MAGIC_CTOR(true, vive_magic_power_on), MAGIC_CTOR(false, vive_magic_power_off)}}, {.vid = 0x0bb4, .pid = 0x030e, .type = USB_DEV_HMD, .name = "BRD", .codename = "", .endpoints = {{.num = 0x81, .name = "Mainboard Pro", .type = USB_IF_HMD_HEADSET_INFO}}, .magics = {MAGIC_CTOR(true, vive_magic_power_on), MAGIC_CTOR(false, vive_magic_power_off)}}, {.vid = 0x28de, .pid = 0x2000, .type = USB_DEV_HMD_IMU_LH, .name = "HMD IMU & LH", .codename = "HMD", .endpoints = {{.num = 0x81, .name = "IMU", .type = USB_IF_HMD_IMU}, {.num = 0x82, .name = "Lightcap", .type = USB_IF_HMD_LIGHTCAP}}, .magics = {MAGIC_CTOR(true, vive_magic_enable_lighthouse), MAGIC_CTOR(true, vive_magic_enable_lighthouse_more)}}, {.vid = 0x28de, .pid = 0x2101, .type = USB_DEV_WATCHMAN1, .name = "Watchman", .codename = "WM0", .endpoints = {{.num = 0x81, .name = "IMU/Lightcap/Buttons", .type = USB_IF_WATCHMAN1}}, .magics = {// MAGIC_CTOR(true, vive_magic_enable_lighthouse), // MAGIC_CTOR(true, vive_magic_enable_lighthouse_more), MAGIC_CTOR(true, vive_magic_protocol_super_magic), MAGIC_CTOR(true, vive_magic_rf_raw_mode_0), MAGIC_CTOR(true, vive_magic_protocol_switch)}}, {.vid = 0x28de, .pid = 0x2022, .type = USB_DEV_TRACKER0, .name = "Tracker", .codename = "TR0", .endpoints = { {.num = 0x81, .name = "IMU", .type = USB_IF_TRACKER0_IMU}, {.num = 0x82, .name = "Lightcap", .type = USB_IF_TRACKER0_LIGHTCAP}, {.num = 0x83, .name = "Buttons", .type = USB_IF_TRACKER0_BUTTONS}, }, .magics = {MAGIC_CTOR(true, vive_magic_enable_lighthouse), MAGIC_CTOR(true, vive_magic_enable_lighthouse_more)}}, {.vid = 0x28de, .pid = 0x2300, .type = USB_DEV_TRACKER1, .name = "Tracker (2018)", .codename = "T20", .endpoints = { {.num = 0x81, .name = "IMU", .type = USB_IF_TRACKER1_IMU}, {.num = 0x83, .name = "Lightcap", .type = USB_IF_TRACKER1_LIGHTCAP}, {.num = 0x84, .name = "Buttons", .type = USB_IF_TRACKER1_BUTTONS}, }, .magics = {MAGIC_CTOR(true, vive_magic_enable_lighthouse), MAGIC_CTOR(true, vive_magic_enable_lighthouse_more)}}, {.vid = 0x28de, .pid = 0x2012, .type = USB_DEV_W_WATCHMAN1, .name = "Wired Watchman", .codename = "WW0", .endpoints = {{.num = 0x81, .name = "IMU", .type = USB_IF_W_WATCHMAN1_IMU}, {.num = 0x82, .name = "Lightcap", .type = USB_IF_W_WATCHMAN1_LIGHTCAP}, {.num = 0x83, .name = "Buttons", .type = USB_IF_W_WATCHMAN1_BUTTONS}}, .magics = {MAGIC_CTOR(true, vive_magic_enable_lighthouse), MAGIC_CTOR(true, vive_magic_enable_lighthouse_more)}}, {.vid = 0x28de, .pid = 0x2102, .type = USB_DEV_WATCHMAN1, .name = "Knuckles", .codename = "KN0", .endpoints = {{.num = 0x81, .name = "IMU/Lightcap/Buttons", .type = USB_IF_WATCHMAN1}}, .magics = {MAGIC_CTOR(true, vive_magic_protocol_super_magic), MAGIC_CTOR(true, vive_magic_rf_raw_mode_0), MAGIC_CTOR(true, vive_magic_protocol_switch)}}, {0}}; // typedef struct SurviveUSBInterface SurviveUSBInterface; typedef struct SurviveViveData SurviveViveData; const char *survive_usb_interface_str(enum USB_IF_t iface) { switch (iface) { case USB_IF_HMD_HEADSET_INFO: return "USB_IF_HMD_HEADSET_INFO"; case USB_IF_HMD_IMU: return "USB_IF_HMD_IMU"; case USB_IF_WATCHMAN1: return "USB_IF_WATCHMAN1"; case USB_IF_WATCHMAN2: return "USB_IF_WATCHMAN2"; case USB_IF_TRACKER0_IMU: return "USB_IF_TRACKER0_IMU"; case USB_IF_TRACKER_INFO: return "USB_IF_TRACKER_INFO"; case USB_IF_TRACKER1_IMU: return "USB_IF_TRACKER1_IMU"; case USB_IF_W_WATCHMAN1_IMU: return "USB_IF_W_WATCHMAN1_IMU"; case USB_IF_HMD_LIGHTCAP: return "USB_IF_HMD_LIGHTCAP"; case USB_IF_TRACKER0_LIGHTCAP: return "USB_IF_TRACKER0_LIGHTCAP"; case USB_IF_TRACKER1_LIGHTCAP: return "USB_IF_TRACKER1_LIGHTCAP"; case USB_IF_W_WATCHMAN1_LIGHTCAP: return "USB_IF_W_WATCHMAN1_LIGHTCAP"; case USB_IF_HMD_BUTTONS: return "USB_IF_HMD_BUTTONS"; case USB_IF_TRACKER0_BUTTONS: return "USB_IF_TRACKER0_BUTTONS"; case USB_IF_TRACKER1_BUTTONS: return "USB_IF_TRACKER1_BUTTONS"; case USB_IF_W_WATCHMAN1_BUTTONS: return "USB_IF_W_WATCHMAN1_BUTTONS"; } return "UNKNOWN"; } void survive_dump_buffer(SurviveContext *ctx, const uint8_t *data, size_t length) { int bytes_per_row = 32; ctx->printfproc(ctx, "%.7f ", survive_run_time(ctx)); for (size_t i = 0; i < length; i += bytes_per_row) { for (int j = 0; j < bytes_per_row; j++) { if (j > 0 && j % 4 == 0) SURVIVE_INVOKE_HOOK(printf, ctx, " "); if (i + j < length) { SURVIVE_INVOKE_HOOK(printf, ctx, "%02x ", data[i + j]); } else { SURVIVE_INVOKE_HOOK(printf, ctx, " "); } } SURVIVE_INVOKE_HOOK(printf, ctx, " | "); for (int j = 0; j < bytes_per_row; j++) { if (j > 0 && j % 4 == 0) SURVIVE_INVOKE_HOOK(printf, ctx, " "); if (i + j < length) { uint8_t d = data[i + j]; SURVIVE_INVOKE_HOOK(printf, ctx, "%c", d >= 32 && d < 127 ? d : '.'); } else { SURVIVE_INVOKE_HOOK(printf, ctx, " "); } } SURVIVE_INVOKE_HOOK(printf, ctx, "\n"); } } struct SurviveUSBInfo { USBHANDLE handle; SurviveViveData *viveData; const struct DeviceInfo *device_info; struct SurviveObject *so; bool ownsObject; size_t interface_cnt; SurviveUSBInterface interfaces[MAX_INTERFACES_PER_DEVICE]; enum LightcapMode { LightcapMode_unknown = 0, // This is the starting mode after an 0x40 0 0 0 0. Used for LH1 LightcapMode_raw0, // This is the mode used for LH2 LightcapMode_raw1, // Not sure what this is used for tbh LightcapMode_raw2 } lightcapMode; size_t timeWithoutFlag; size_t packetsSeenWaitingForV2; size_t ignoreCnt; size_t active_transfers; FLT nextCfgSubmitTime; struct survive_config_packet *cfg_user; bool request_close, request_reopen; }; struct SurviveViveData { SurviveContext *ctx; size_t udev_cnt; struct SurviveUSBInfo *udev[MAX_USB_DEVS]; struct libusb_context *usbctx; size_t read_count; int seconds_per_hz_output; int cnt_per_device_type[sizeof(KnownDeviceTypes) / sizeof(KnownDeviceTypes[0])]; struct SurviveUSBInfo *hmd_mainboard, *hmd_imu; FLT lastPairTime; bool requestPairing; #ifndef HIDAPI libusb_hotplug_callback_handle callback_handle; #endif }; static void parse_tracker_version_info(SurviveObject *so, const uint8_t *data, size_t size); static int AttachInterface(SurviveViveData *sv, struct SurviveUSBInfo *usbObject, const struct Endpoint_t *endpoint, USBHANDLE devh, usb_callback cb); static int survive_vive_send_haptic(SurviveObject *so, FLT frequency, FLT amplitude, FLT duration_seconds); #ifdef HIDAPI #include "driver_vive.hidapi.h" #else #include "driver_vive.libusb.h" #endif struct survive_config_packet { SurviveContext *ctx; SurviveViveData *sv; struct SurviveUSBInfo *usbInfo; const struct Magic_t *current_magic; uint8_t buffer[256]; uint16_t expected_cfg_length; cstring cfg; double start_time; enum { /** * CONFIG must come first; it effectively gates progress for controllers which might not be turned on yet. */ SURVIVE_CONFIG_STATE_CONFIG, SURVIVE_CONFIG_STATE_MAGICS, SURVIVE_CONFIG_STATE_VERSION, SURVIVE_CONFIG_STATE_IMU_SCALES, SURVIVE_CONFIG_STATE_DONE } state; uint16_t stall_counter; survive_usb_transfer_t *tx; }; #include "driver_vive.config.h" static bool survive_device_is_rf(const struct DeviceInfo *device_info) { switch (device_info->type) { case USB_DEV_HMD: case USB_DEV_HMD_IMU_LH: case USB_DEV_W_WATCHMAN1: case USB_DEV_TRACKER0: case USB_DEV_TRACKER1: return false; } return true; } static struct SurviveUSBInfo *survive_get_usb_info(SurviveObject *so) { return (struct SurviveUSBInfo *)so->driver; } static int survive_vive_send_haptic(SurviveObject *so, FLT frequency, FLT amplitude, FLT duration_seconds) { SurviveViveData *sv = ((struct SurviveUSBInfo *)so->driver)->viveData; SurviveContext *ctx = so->ctx; if (so->object_type == SURVIVE_OBJECT_TYPE_HMD || so->conf == 0) { return 0; } if (NULL == sv) { return -500; } // Due to https://gitlab.freedesktop.org/monado/monado/-/blob/master/src/xrt/drivers/vive/vive_controller.c#L398 FLT high_plus_low = 1000.f * 1000.f / frequency; FLT pulse_low = amplitude * high_plus_low / 2.; /* Vive Controller doesn't vibrate with value == 0. * Not sure if this actually happens, but let's fix it anyway. */ if (pulse_low == 0) pulse_low = 1; FLT pulse_high = high_plus_low - pulse_low; uint16_t repeat_count = duration_seconds * frequency; if (repeat_count == 0) repeat_count = 1; uint16_t pulse_high16 = pulse_high, pulse_low16 = pulse_low; if (pulse_high > UINT16_MAX) pulse_high16 = UINT16_MAX; if (pulse_high < 0) pulse_high16 = 1; if (pulse_low > UINT16_MAX) pulse_low16 = UINT16_MAX; if (pulse_low < 0) pulse_low16 = 1; SV_VERBOSE(100, "Sending haptic pulse to %s %f %f %f %f %f %d", survive_colorize(so->codename), frequency, amplitude, duration_seconds, pulse_high, pulse_low, repeat_count); uint8_t vive_controller_haptic_pulse[] = { VIVE_REPORT_COMMAND, VIVE_COMMAND_HAPTIC_PULSE, 0x07, 0x00, pulse_high16, pulse_high16 >> 8u, pulse_low16, pulse_low16 >> 8u, repeat_count, repeat_count >> 8u, }; struct SurviveUSBInfo *usbInfo = survive_get_usb_info(so); int r = update_feature_report(usbInfo->handle, 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse)); if (r != sizeof(vive_controller_haptic_pulse)) { SV_WARN("HAPTIC FAILED %d", r); return -1; } return 0; } void vive_switch_mode(struct SurviveUSBInfo *driverInfo, enum LightcapMode lightcapMode) { SurviveContext *ctx = driverInfo->so->ctx; SurviveObject *w = driverInfo->so; if (driverInfo->timeWithoutFlag == 0) { driverInfo->timeWithoutFlag = 1; uint8_t buffer[64] = {0}; size_t buffer_length = 0; if (survive_device_is_rf(driverInfo->device_info)) { buffer[0] = VIVE_REPORT_COMMAND; buffer[1] = VIVE_COMMAND_CHANGE_PROTOCOL; buffer[2] = 6; buffer[3] = 1; buffer[4] = lightcapMode == LightcapMode_raw0 ? 0 : 1; buffer[5] = 0; buffer[6] = 2; buffer[7] = lightcapMode == LightcapMode_raw2 ? 1 : 0; buffer[8] = 0; buffer_length = 64; } else { buffer[0] = VIVE_REPORT_CHANGE_MODE; buffer[1] = (lightcapMode == LightcapMode_raw1) ? 1 : (lightcapMode == LightcapMode_raw2) ? 3 : 0; buffer_length = 64; } if (driverInfo->handle) { int r = update_feature_report(driverInfo->handle, 0, buffer, buffer_length); if (r != buffer_length) { SV_WARN("Could not send raw mode to %s (%d)", w->codename, r); return; } if (!survive_device_is_rf(driverInfo->device_info)) { r = update_feature_report(driverInfo->handle, 0, vive_magic_enable_lighthouse_more, sizeof(vive_magic_enable_lighthouse_more)); if (r != buffer_length) { SV_WARN("Could not lighthouse more to %s (%d)", w->codename, r); } } SV_INFO("LightcapMode (%s) %d -> %d (%x)", w->codename, driverInfo->lightcapMode, lightcapMode, buffer[0]); driverInfo->lightcapMode = lightcapMode; } else { static bool transfer_null_warning = false; if (!transfer_null_warning) { SV_WARN("Can't update the usb device %s out of raw 0 mode; dumping data", w->codename); transfer_null_warning = true; } } } } static bool is_mode_switch_usb(uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t length) { return bmRequestType == 0x21 && bRequest == 0x09 && wValue == 0x304 && length >= 8; } static bool is_mode_switch_rf(uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, const uint8_t *data, uint16_t length) { return bmRequestType == 0x21 && bRequest == 0x09 && wValue == 0x3ff && length >= 8 && data[1] == VIVE_COMMAND_CHANGE_PROTOCOL && data[2] >= 6; } void survive_data_on_setup_write(SurviveObject *so, uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, const uint8_t *data, size_t length) { SurviveContext *ctx = so->ctx; struct SurviveUSBInfo *driverInfo = so->driver; driverInfo->timeWithoutFlag = 1; if (is_mode_switch_usb(bmRequestType, bRequest, wValue, wIndex, length)) { enum LightcapMode m = data[1] == 0 ? LightcapMode_raw0 : data[1] == 1 ? LightcapMode_raw1 : LightcapMode_raw2; SV_INFO("LightcapMode usb %s %d -> %d", so->codename, driverInfo->lightcapMode, m); driverInfo->lightcapMode = m; driverInfo->ignoreCnt = 10; } if (is_mode_switch_rf(bmRequestType, bRequest, wValue, wIndex, data, length)) { enum LightcapMode m = data[4] == 0 ? LightcapMode_raw0 : data[7] == 1 ? LightcapMode_raw2 : LightcapMode_raw1; SV_INFO("LightcapMode rf %s %d -> %d", so->codename, driverInfo->lightcapMode, m); driverInfo->lightcapMode = m; driverInfo->ignoreCnt = 10; } // SV_INFO("Setup %s write of %x %d", survive_colorize_codename(so), wValue, length); } void survive_data_cb_locked(uint64_t time_received_us, SurviveUSBInterface *si); void survive_data_cb(uint64_t time_received_us, SurviveUSBInterface *si) { SurviveContext *ctx = si->ctx; survive_get_ctx_lock(ctx); survive_data_cb_locked(time_received_us, si); survive_release_ctx_lock(ctx); } // USB Subsystem static int survive_usb_init(SurviveViveData *sv); int survive_usb_poll(SurviveContext *ctx); static int AttachInterface(SurviveViveData *sv, struct SurviveUSBInfo *usbObject, const struct Endpoint_t *endpoint, USBHANDLE devh, usb_callback cb) { SurviveContext *ctx = sv->ctx; size_t iface_cnt = usbObject->interface_cnt++; int which_interface_am_i = endpoint->type; const char *hname = endpoint->name; int endpoint_num = endpoint->num; SurviveUSBInterface *iface = &usbObject->interfaces[iface_cnt]; SurviveObject *assocobj = usbObject->so; iface->ctx = ctx; iface->sv = sv; iface->which_interface_am_i = which_interface_am_i; iface->assoc_obj = usbObject->so; iface->usbInfo = usbObject; iface->hname = hname; iface->cb = cb; #ifdef HIDAPI iface->buffer = iface->swap_buffer[0]; iface->uh = usbObject->handle->interfaces[endpoint - usbObject->device_info->endpoints]; assert(iface->uh); #ifndef HID_NONBLOCKING iface->servicethread = OGCreateThread(HAPIReceiver, iface); OGUSleep(100000); #else hid_set_nonblocking(iface->uh, 1); // Empty the queue out. If you don't, you might get stale data while (hid_read(iface->uh, iface->buffer, sizeof(iface->buffer)) > 0) { } #endif #else struct libusb_transfer *tx = iface->transfer = libusb_alloc_transfer(0); usbObject->active_transfers++; // printf( "%p %d %p %p\n", iface, which_interface_am_i, tx, devh ); SV_VERBOSE(50, "Attaching %s(0x%x) for %s", hname, endpoint_num, survive_colorize(assocobj ? assocobj->codename : "(unknown)")); if (!iface->transfer) { SV_ERROR(SURVIVE_ERROR_HARWARE_FAULT, "Error: failed on libusb_alloc_transfer for %s", hname); return 4; } memset(iface->swap_buffer, 0xCA, sizeof(iface->swap_buffer)); libusb_fill_interrupt_transfer(tx, devh, endpoint_num, iface->swap_buffer[0], INTBUFFSIZE, handle_transfer, iface, 0); iface->last_submit_time = OGGetAbsoluteTimeUS(); int rc = libusb_submit_transfer(tx); if (rc) { SV_ERROR(SURVIVE_ERROR_HARWARE_FAULT, "Error: Could not submit transfer for %s 0x%02x (Code %d, %s)", hname, endpoint_num, rc, libusb_error_name(rc)); return 6; } #endif return 0; } static const struct DeviceInfo *find_known_device(SurviveContext *ctx, uint16_t idVendor, uint16_t idProduct) { const char *blacklist = survive_configs(ctx, "blacklist-devs", SC_GET, 0); for (const struct DeviceInfo *info = KnownDeviceTypes; info->name && blacklist; info++) { if (info == 0 || strstr(blacklist, info->name)) { continue; } if (info->vid == idVendor && info->pid == idProduct) { return info; } } return 0; } static int survive_start_get_config(SurviveViveData *sv, struct SurviveUSBInfo *usbInfo, int iface); int survive_vive_add_usb_device(SurviveViveData *sv, survive_usb_device_t d) { SurviveContext *ctx = sv->ctx; uint16_t idVendor; uint16_t idProduct; uint8_t class_id; int ret = survive_get_ids(d, &idVendor, &idProduct, &class_id); if (ret < 0) { SV_WARN("Could not get vid:pid for usb device.") return -2; } const struct DeviceInfo *info = find_known_device(ctx, idVendor, idProduct); if (info == 0) { SV_VERBOSE(110, "USB device %04x:%x4x in an unknown type; ignoring", idVendor, idProduct); return -1; } SV_VERBOSE(10, "Enumerating USB device %04x:%04x %s", idVendor, idProduct, survive_colorize(info->name)); struct SurviveUSBInfo *usbInfo = sv->udev[sv->udev_cnt++] = SV_CALLOC(sizeof(struct SurviveUSBInfo)); usbInfo->handle = 0; usbInfo->device_info = info; usbInfo->viveData = sv; ret = survive_open_usb_device(sv, d, usbInfo); if (ret) { sv->udev_cnt--; return -5; } if (info->type == USB_DEV_HMD) { SV_VERBOSE(10, "Mainboard class %d", class_id); if (sv->hmd_mainboard != 0 || class_id != 0) { free(sv->udev[sv->udev_cnt--]); sv->udev[sv->udev_cnt] = 0; return -3; } sv->hmd_mainboard = usbInfo; } else if (info->type == USB_DEV_HMD_IMU_LH) { if (sv->hmd_imu != 0) { SV_WARN("Multiple HMDs are not supported currently.") return -4; } sv->hmd_imu = usbInfo; } int *cnt = (sv->cnt_per_device_type + (usbInfo->device_info - KnownDeviceTypes)); #ifdef HIDAPI if (usbInfo->device_info->codename[0] != 0) { char codename[4] = {0}; strcpy(codename, usbInfo->device_info->codename); codename[2] += (*cnt); *cnt = *cnt + 1; SurviveObject *so = survive_create_device(ctx, "HTC", usbInfo, codename, survive_vive_send_haptic); survive_add_object(ctx, so); usbInfo->so = so; usbInfo->ownsObject = true; } #endif survive_start_get_config(sv, usbInfo, 0); // There should only be one HMD, tie the mainboard interface to the surviveobject if (info->type == USB_DEV_HMD_IMU_LH || info->type == USB_DEV_HMD) { if (sv->hmd_imu != 0 && sv->hmd_mainboard != 0) { sv->hmd_mainboard->so = sv->hmd_imu->so; usbInfo->ownsObject = false; } } #ifdef HIDAPI /* for (const struct Endpoint_t *endpoint = usbInfo->device_info->endpoints; endpoint->name; endpoint++) { int errorCode = AttachInterface(sv, usbInfo, endpoint, usbInfo->handle, survive_data_cb); if (errorCode < 0) { SV_WARN("Could not attach interface %s: %d", endpoint->name, errorCode); } } */ #endif return 0; } int survive_usb_init(SurviveViveData *sv) { SurviveContext *ctx = sv->ctx; int r = survive_usb_subsystem_init(sv); if (r) { SV_ERROR(SURVIVE_ERROR_HARWARE_FAULT, "usb fault %d (%s)\n", r, survive_usb_error_name(r)); return r; } if (setup_hotplug(sv) != 0) { survive_usb_devices_t devs; int ret = survive_get_usb_devices(sv, &devs); if (ret < 0) { SV_ERROR(SURVIVE_ERROR_HARWARE_FAULT, "Couldn't get list of USB devices %d (%s)", ret, survive_usb_error_name(ret)); return ret; } // Open all interfaces. survive_usb_device_enumerator e = 0; for (survive_usb_device_t d = 0; (d = get_next_device(&e, devs)) && sv->udev_cnt < MAX_USB_DEVS;) { survive_vive_add_usb_device(sv, d); } survive_free_usb_devices(devs); SV_INFO("All enumerated devices attached."); } // libUSB initialized. Continue. return 0; } STATIC_CONFIG_ITEM(PAIR_DEVICE, "pair-device", 'b', "Turn on pairing mode", 0) STATIC_CONFIG_ITEM(SECONDS_PER_HZ_OUTPUT, "usb-hz-output", 'i', "Seconds between outputing usb stats", -1) void survive_vive_usb_close(SurviveViveData *sv) { survive_release_ctx_lock(sv->ctx); survive_usb_close(sv); survive_get_ctx_lock(sv->ctx); survive_detach_config(sv->ctx, SECONDS_PER_HZ_OUTPUT_TAG, &sv->seconds_per_hz_output); } static inline bool survive_handle_close_request_flag(struct SurviveUSBInfo *usbInfo) { SurviveViveData *sv = usbInfo->viveData; SurviveContext *ctx = sv->ctx; if (usbInfo->request_close) { int idx = 0; for (idx = 0; idx < sv->udev_cnt && sv->udev[idx] != usbInfo; idx++) ; for (size_t j = 0; j < usbInfo->interface_cnt; j++) { usbInfo->interfaces[j].assoc_obj = 0; } SV_VERBOSE(10, "Closing device %s %s (%d/%zu)", survive_colorize_codename(usbInfo->so), survive_colorize(usbInfo->device_info->name), idx, sv->udev_cnt); if (usbInfo == sv->hmd_imu) { sv->hmd_imu = 0; if (sv->hmd_mainboard != 0) { survive_close_usb_device(sv->hmd_mainboard); sv->hmd_mainboard->so = 0; } } if (usbInfo == sv->hmd_mainboard) sv->hmd_mainboard = 0; sv->udev_cnt--; sv->udev[idx] = sv->udev[sv->udev_cnt]; sv->udev[sv->udev_cnt] = 0; if (usbInfo->ownsObject) { survive_destroy_device(usbInfo->so); } survive_usb_device_t dev = 0; #ifndef HIDAPI dev = libusb_get_device(usbInfo->handle); #endif bool reopen = usbInfo->request_reopen; survive_usb_handle_close(usbInfo->handle); free(usbInfo); if (reopen && dev) { survive_vive_add_usb_device(sv, dev); } return true; } return false; } static int survive_start_get_config(SurviveViveData *sv, struct SurviveUSBInfo *usbInfo, int iface) { SurviveContext *ctx = sv->ctx; survive_usb_transfer_t *tx = survive_usb_transfer_alloc(); if (!tx) { SV_WARN("Could not allocate transfer frame"); return -4; } usbInfo->active_transfers++; struct survive_config_packet *config_packet = SV_CALLOC(sizeof(struct survive_config_packet)); config_packet->tx = tx; config_packet->tx->buffer = config_packet->buffer; config_packet->tx->actual_length = sizeof(config_packet->buffer); usbInfo->cfg_user = config_packet; config_packet->ctx = ctx; config_packet->sv = sv; config_packet->usbInfo = usbInfo; config_packet->current_magic = config_packet->usbInfo->device_info->magics; USBHANDLE dev = usbInfo->handle; config_packet->start_time = survive_run_time(ctx); if (config_packet->usbInfo->device_info->codename[0] == 0) { config_packet->state = SURVIVE_CONFIG_STATE_MAGICS; } setup_packet_state(config_packet); SV_VERBOSE(10, "Requesting config for %s %p %d", survive_colorize(usbInfo->so ? usbInfo->so->codename : usbInfo->device_info->name), (void *)0, config_packet->state); usbInfo->nextCfgSubmitTime = survive_run_time(ctx); return 0; } static void survive_config_poll(struct SurviveUSBInfo *usbInfo); int survive_vive_usb_poll(SurviveContext *ctx, void *v) { SurviveViveData *sv = v; sv->read_count++; static FLT last_print = 0; FLT now = survive_run_time(ctx); FLT time_diff = now - last_print; bool print = sv->seconds_per_hz_output > 0 && time_diff > sv->seconds_per_hz_output; if (print) { size_t total_packets = 0; for (int i = 0; i < sv->udev_cnt; i++) { const char *codename = sv->udev[i]->so->codename; if (sv->udev[i]->so == 0) continue; for (int j = 0; j < sv->udev[i]->interface_cnt; j++) { SurviveUSBInterface *iface = &sv->udev[i]->interfaces[j]; if (iface->assoc_obj) codename = iface->assoc_obj->codename; total_packets += iface->packet_count; FLT avg_cb_time = iface->sum_cb_time / (FLT)(iface->packet_count + .0001) / 1000.; FLT avg_cb_submit_latency = iface->sum_submit_cb_time / (FLT)(iface->packet_count + .0001) / 1000.; if (iface->time_constraint == 0 && iface->packet_count) { iface->time_constraint = 1000. * avg_cb_submit_latency; SV_INFO("Iface %3s %-32s has time constraint of %5.2fms", survive_colorize(codename), survive_colorize(iface->hname), avg_cb_submit_latency); } SV_INFO("Iface %3s %-32s has %5zu packets (%8.2f hz) Avg CB Time: %5.2fms Avg CB Latency: %5.2fms Max " "CB Time: %5.2fms Max CB Latency: %5.2fms Time Violations %4d (%7.5f%%)", survive_colorize(codename), survive_colorize(iface->hname), iface->packet_count, iface->packet_count / time_diff, avg_cb_time, avg_cb_submit_latency, iface->max_cb_time / 1000., iface->max_submit_time / 1000., iface->cb_time_violation, 100. * iface->cb_time_violation / (FLT)(iface->packet_count + .0001)); iface->max_cb_time = iface->max_submit_time = iface->sum_cb_time = iface->sum_submit_cb_time = 0; iface->cb_time_violation = 0; iface->packet_count = 0; } } SV_INFO("Total %4zu packets (%6.2f hz) at %7.3fs", total_packets, total_packets / time_diff, now); last_print = now; } for (int i = 0; i < sv->udev_cnt; i++) { struct SurviveUSBInfo *usbInfo = sv->udev[i]; if ((usbInfo->device_info->pid == 0x2102 || usbInfo->device_info->pid == 0x2101) && usbInfo->so == 0 && sv->requestPairing && (sv->lastPairTime + 1) < now && now > 3) { survive_release_ctx_lock(ctx); int r = update_feature_report(usbInfo->handle, 0, vive_request_pairing, sizeof(vive_request_pairing)); survive_get_ctx_lock(ctx); SV_VERBOSE(10, "Pairing attempt..."); sv->lastPairTime = now; } survive_config_poll(usbInfo); if (survive_handle_close_request_flag(usbInfo)) { i--; } } #ifdef HIDAPI #ifdef HID_NONBLOCKING survive_release_ctx_lock(ctx); for (int i = 0; i < sv->udev_cnt; i++) { for (int j = 0; j < sv->udev[i]->interface_cnt; j++) { SurviveUSBInterface *iface = &sv->udev[i]->interfaces[j]; if (iface->assoc_obj) HAPIReceiver(iface); } } survive_get_ctx_lock(ctx); #else OGUSleep(1); return 0; #endif #else // int r = libusb_handle_events(sv->usbctx); struct timeval tv = {.tv_usec = 10 * 1000}; survive_release_ctx_lock(ctx); int r = libusb_handle_events_timeout(sv->usbctx, &tv); survive_get_ctx_lock(ctx); if (r) { SV_WARN("Libusb poll failed. %d (%s)", r, libusb_error_name(r)); } #endif return 0; } static inline survive_timecode fix_time24(SurviveContext *ctx, survive_timecode time24, survive_timecode refTime) { survive_timecode upper_ref = refTime & 0xFF000000u; survive_timecode lower_ref = refTime & 0x00FFFFFFu; if (lower_ref > time24 && lower_ref - time24 > (1 << 23u)) { upper_ref += 0x01000000; } else if (lower_ref < time24 && time24 - lower_ref > (1 << 23u) && upper_ref > 0) { upper_ref -= 0x01000000; } // SV_VERBOSE(250, "fix_time24 time24: %x refTime: %x upper_ref: %x (%x) rtn: %x", time24, refTime, upper_ref, //refTime & 0xFF000000u, upper_ref | time24); return upper_ref | time24; } /////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// #define POP1 (*(readdata++)) #ifndef _MSC_VER struct __attribute__((__packed__)) unaligned_16_t { int16_t v; }; struct __attribute__((__packed__)) unaligned_32_t { int32_t v; }; struct __attribute__((__packed__)) unaligned_u16_t { uint16_t v; }; struct __attribute__((__packed__)) unaligned_u32_t { uint32_t v; }; #else struct unaligned_16_t { int16_t v; }; struct unaligned_32_t { int32_t v; }; struct unaligned_u16_t { uint16_t v; }; struct unaligned_u32_t { uint32_t v; }; #endif #define POP2 (((((struct unaligned_u16_t *)((readdata += 2) - 2))))->v) #define POP4 (((((struct unaligned_u32_t *)((readdata += 4) - 4))))->v) typedef struct { // could use a bitfield here, but since this data is short-lived, // the space savings probably isn't worth the processing overhead. uint8_t pressedButtonsValid; uint32_t touchedButtonsValid; uint8_t triggerOfBatteryValid; uint8_t batteryChargeValid; uint8_t hardwareIdValid; uint8_t touchpadHorizontalValid; uint8_t touchpadVerticalValid; uint8_t triggerHighResValid; uint8_t proximityValid; uint8_t rawAxisCnt; SurviveAxisVal_t rawAxis[16]; SurviveAxisVal_t /*uint8_t*/ proximity[6]; uint32_t pressedButtons; uint32_t touchedButtons; uint16_t triggerOrBattery; uint8_t batteryCharge; uint32_t hardwareId; SurviveAxisVal_t /*uint16_t*/ touchpadHorizontal; SurviveAxisVal_t /*uint16_t*/ touchpadVertical; SurviveAxisVal_t /*uint16_t*/ triggerHighRes; } buttonEvent; static ButtonQueueEntry *prepareNextButtonEvent(SurviveObject *so) { ButtonQueueEntry *entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]); memset(entry, 0, sizeof(ButtonQueueEntry)); assert(so); entry->so = so; for (int i = 0; i < 16; i++) { entry->ids[i] = SURVIVE_AXIS_UNKNOWN; } entry->buttonId = SURVIVE_BUTTON_UNKNOWN; return entry; } static ButtonQueueEntry *incrementAndPostButtonQueue(SurviveObject *so) { SurviveContext *ctx = so->ctx; if (ctx->buttonQueue.buttonservicesem == 0) return 0; ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); SV_VERBOSE(110, "%s Button event %s %d %s %f", survive_colorize_codename(so), SurviveInputEventStr(entry->eventType), entry->buttonId, SurviveAxisStr(so->object_subtype, entry->ids[0]), entry->axisValues[0]); for (int i = 0; i < (sizeof(entry->ids) / sizeof(entry->ids[0])) && entry->ids[i] != 255; i++) so->axis[entry->ids[i]] = entry->axisValues[i]; if (entry->buttonId != 255) { assert(entry->buttonId < 32); bool isTouch = entry->eventType == SURVIVE_INPUT_EVENT_TOUCH_UP || entry->eventType == SURVIVE_INPUT_EVENT_TOUCH_DOWN; bool isClear = entry->eventType == SURVIVE_INPUT_EVENT_TOUCH_UP || entry->eventType == SURVIVE_INPUT_EVENT_BUTTON_UP; uint32_t mask = 1u << entry->buttonId; uint32_t *maskp = isTouch ? &so->touchmask : &so->buttonmask; uint32_t maskv = *maskp; if (isClear) *maskp &= ~mask; else *maskp |= mask; // assert(maskv != *maskp); } if ((ctx->buttonQueue.nextWriteIndex + 1) % BUTTON_QUEUE_MAX_LEN == ctx->buttonQueue.nextReadIndex) { // There's not enough space to write this entry. Clear it out and move along SV_WARN("Button buffer full"); memset(entry, 0, sizeof(ButtonQueueEntry)); return 0; } entry->isPopulated = 1; ctx->buttonQueue.nextWriteIndex++; // if we've exceeded the size of the buffer, loop around to the beginning. if (ctx->buttonQueue.nextWriteIndex >= BUTTON_QUEUE_MAX_LEN) { ctx->buttonQueue.nextWriteIndex = 0; } OGUnlockSema(ctx->buttonQueue.buttonservicesem); // clear out any old data in the entry so we always start with a clean slate. entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]); memset(entry, 0, sizeof(ButtonQueueEntry)); return prepareNextButtonEvent(so); } enum ButtonEventSource { BUTTON_EVENT_SOURCE_DEFAULT = 0, BUTTON_EVENT_SOURCE_RF = 1, BUTTON_TOUCH_EVENT_SOURCE = 0x80 }; struct DeviceMapping { bool isRF; SurviveObjectSubtype objectSubtype; bool isTouch[64]; enum SurviveButton buttonMap[64]; enum SurviveAxis axisMap[16]; }; static void init_device_mapping(struct DeviceMapping *mapping) { memset(mapping, 0, sizeof(struct DeviceMapping)); for (int i = 0; i < 64; i++) mapping->buttonMap[i] = SURVIVE_BUTTON_UNKNOWN; for (int i = 0; i < 16; i++) mapping->axisMap[i] = SURVIVE_AXIS_UNKNOWN; for (int i = 32; i < 64; i++) { mapping->isTouch[i] = true; } } static struct DeviceMapping *RFWandMapping() { static struct DeviceMapping mapping = {0}; if (mapping.objectSubtype == SURVIVE_OBJECT_SUBTYPE_GENERIC) { init_device_mapping(&mapping); mapping.isRF = true; mapping.objectSubtype = SURVIVE_OBJECT_SUBTYPE_WAND; mapping.axisMap[1] = SURVIVE_AXIS_TRIGGER; mapping.axisMap[2] = SURVIVE_AXIS_TRACKPAD_X; mapping.axisMap[3] = SURVIVE_AXIS_TRACKPAD_Y; mapping.buttonMap[0] = SURVIVE_BUTTON_TRIGGER; mapping.buttonMap[1] = SURVIVE_BUTTON_TRACKPAD; mapping.isTouch[1] = true; mapping.buttonMap[2] = SURVIVE_BUTTON_TRACKPAD; mapping.buttonMap[3] = SURVIVE_BUTTON_SYSTEM; mapping.buttonMap[4] = SURVIVE_BUTTON_GRIP; mapping.buttonMap[5] = SURVIVE_BUTTON_MENU; } return &mapping; } static struct DeviceMapping *WiredWandMapping() { static struct DeviceMapping mapping = {0}; if (mapping.objectSubtype == SURVIVE_OBJECT_SUBTYPE_GENERIC) { init_device_mapping(&mapping); mapping.isRF = false; mapping.objectSubtype = SURVIVE_OBJECT_SUBTYPE_WAND; mapping.axisMap[1] = SURVIVE_AXIS_TRIGGER; mapping.axisMap[2] = SURVIVE_AXIS_TRACKPAD_X; mapping.axisMap[3] = SURVIVE_AXIS_TRACKPAD_Y; mapping.buttonMap[0] = SURVIVE_BUTTON_TRIGGER; mapping.buttonMap[2] = SURVIVE_BUTTON_GRIP; mapping.buttonMap[12] = SURVIVE_BUTTON_MENU; mapping.buttonMap[13] = SURVIVE_BUTTON_SYSTEM; mapping.buttonMap[18] = SURVIVE_BUTTON_TRACKPAD; mapping.buttonMap[20] = SURVIVE_BUTTON_TRACKPAD; mapping.isTouch[20] = true; } return &mapping; } static struct DeviceMapping *RFKnuckles() { static struct DeviceMapping mapping = {0}; if (mapping.objectSubtype == SURVIVE_OBJECT_SUBTYPE_GENERIC) { init_device_mapping(&mapping); mapping.isRF = false; mapping.objectSubtype = SURVIVE_OBJECT_SUBTYPE_KNUCKLES_L; mapping.axisMap[1] = SURVIVE_AXIS_TRIGGER; mapping.axisMap[2] = SURVIVE_AXIS_TRACKPAD_X; mapping.axisMap[3] = SURVIVE_AXIS_TRACKPAD_Y; mapping.axisMap[4] = SURVIVE_AXIS_MIDDLE_FINGER_PROXIMITY; mapping.axisMap[5] = SURVIVE_AXIS_RING_FINGER_PROXIMITY; mapping.axisMap[6] = SURVIVE_AXIS_PINKY_FINGER_PROXIMITY; mapping.axisMap[7] = SURVIVE_AXIS_TRIGGER_FINGER_PROXIMITY; mapping.axisMap[8] = SURVIVE_AXIS_GRIP_FORCE; mapping.axisMap[9] = SURVIVE_AXIS_TRACKPAD_FORCE; mapping.axisMap[10] = SURVIVE_AXIS_JOYSTICK_X; mapping.axisMap[11] = SURVIVE_AXIS_JOYSTICK_Y; mapping.buttonMap[0] = SURVIVE_BUTTON_TRIGGER; mapping.buttonMap[1] = SURVIVE_BUTTON_TRACKPAD; mapping.isTouch[1] = true; mapping.buttonMap[2] = SURVIVE_BUTTON_THUMBSTICK; mapping.buttonMap[3] = SURVIVE_BUTTON_SYSTEM; mapping.buttonMap[4] = SURVIVE_BUTTON_A; mapping.buttonMap[5] = SURVIVE_BUTTON_B; mapping.buttonMap[6] = SURVIVE_BUTTON_MENU; for (int i = 0; i < 32; i++) { mapping.buttonMap[i + 32] = mapping.buttonMap[i]; } mapping.buttonMap[1 + 32] = SURVIVE_BUTTON_UNKNOWN; } return &mapping; } static struct DeviceMapping *WiredTracker() { static struct DeviceMapping mapping = {0}; if (mapping.objectSubtype == SURVIVE_OBJECT_SUBTYPE_GENERIC) { init_device_mapping(&mapping); mapping.isRF = false; mapping.objectSubtype = SURVIVE_OBJECT_SUBTYPE_TRACKER; mapping.buttonMap[13] = SURVIVE_BUTTON_SYSTEM; } return &mapping; } struct DeviceMapping *getDeviceMapping(SurviveObjectSubtype type, enum ButtonEventSource source) { static struct DeviceMapping *DeviceMappings[SURVIVE_OBJECT_SUBTYPE_COUNT * 2] = {0}; static bool init = false; if (init == false) { init = true; DeviceMappings[SURVIVE_OBJECT_SUBTYPE_WAND] = WiredWandMapping(); DeviceMappings[SURVIVE_OBJECT_SUBTYPE_TRACKER] = WiredTracker(); DeviceMappings[SURVIVE_OBJECT_SUBTYPE_TRACKER_GEN2] = WiredTracker(); DeviceMappings[SURVIVE_OBJECT_SUBTYPE_WAND + SURVIVE_OBJECT_SUBTYPE_COUNT] = RFWandMapping(); DeviceMappings[SURVIVE_OBJECT_SUBTYPE_KNUCKLES_R + SURVIVE_OBJECT_SUBTYPE_COUNT] = RFKnuckles(); DeviceMappings[SURVIVE_OBJECT_SUBTYPE_KNUCKLES_L + SURVIVE_OBJECT_SUBTYPE_COUNT] = RFKnuckles(); } return DeviceMappings[type + SURVIVE_OBJECT_SUBTYPE_COUNT * ((source & (BUTTON_EVENT_SOURCE_RF)) != 0)]; } static enum SurviveButton get_button_id_for_idx_from_mapping(struct DeviceMapping *mapping, uint8_t idx, bool *isTouchEvent, enum ButtonEventSource source) { size_t buttonIdx = idx + ((source & BUTTON_TOUCH_EVENT_SOURCE) != 0) * 32; *isTouchEvent = mapping->isTouch[buttonIdx]; enum SurviveButton rtn = mapping->buttonMap[buttonIdx]; assert(rtn == 255 || rtn < 32); return rtn; } static enum SurviveButton get_button_id_for_idx(const SurviveObject *so, uint8_t idx, bool *isTouchEvent, enum ButtonEventSource source) { struct DeviceMapping *mapping = getDeviceMapping(so->object_subtype, source); if (mapping) { return get_button_id_for_idx_from_mapping(mapping, idx, isTouchEvent, source); } return idx; } #define HAS_BIT_FLAG(x, idx) (((x) & (1u << (idx))) != 0u) static inline ButtonQueueEntry *registerButtonOnOff(SurviveObject *so, ButtonQueueEntry *entry, uint32_t incoming_mask, enum ButtonEventSource source) { struct SurviveContext *ctx = so->ctx; for (uint8_t a = 0; a < 32; a++) { bool isTouchEvent = (source & BUTTON_TOUCH_EVENT_SOURCE) != 0; enum SurviveButton id = get_button_id_for_idx(so, a, &isTouchEvent, source); enum SurviveInputEvent eventTypeDown = isTouchEvent ? SURVIVE_INPUT_EVENT_TOUCH_DOWN : SURVIVE_INPUT_EVENT_BUTTON_DOWN; enum SurviveInputEvent eventTypeUp = isTouchEvent ? SURVIVE_INPUT_EVENT_TOUCH_UP : SURVIVE_INPUT_EVENT_BUTTON_UP; uint8_t current_mask = eventTypeDown == SURVIVE_INPUT_EVENT_BUTTON_DOWN ? so->buttonmask : so->touchmask; if (id == 255) { if (HAS_BIT_FLAG(incoming_mask, a)) SV_WARN("%s has unknown button input %d %d", so->codename, a, source); // assert( continue; } if (entry && HAS_BIT_FLAG(incoming_mask, a) != HAS_BIT_FLAG(current_mask, id)) { entry->eventType = HAS_BIT_FLAG(incoming_mask, a) ? eventTypeDown : eventTypeUp; entry->buttonId = id; entry = incrementAndPostButtonQueue(so); } } return entry; } // important! This must be the only place that we're posting to the buttonEntryQueue // if that ever needs to be changed, you will have to add locking so that only one // thread is posting at a time. static void registerButtonEvent(SurviveObject *so, buttonEvent *event, enum ButtonEventSource source) { ButtonQueueEntry *entry = prepareNextButtonEvent(so); struct SurviveContext *ctx = so->ctx; if (event->pressedButtonsValid) { SV_VERBOSE(1000, "buttons %8x", event->pressedButtons); entry = registerButtonOnOff(so, entry, event->pressedButtons, source); } if (event->touchedButtonsValid) { SV_VERBOSE(1000, "touched %8x", event->touchedButtons); entry = registerButtonOnOff(so, entry, event->touchedButtons, source | BUTTON_TOUCH_EVENT_SOURCE); } if (event->triggerHighResValid) { if (so->axis[1] != event->triggerHighRes) { entry->eventType = SURVIVE_INPUT_EVENT_AXIS_CHANGED; entry->ids[0] = 1; entry->axisValues[0] = event->triggerHighRes; entry = incrementAndPostButtonQueue(so); } } if ((event->touchpadHorizontalValid) && (event->touchpadVerticalValid)) { enum SurviveAxis ax = SURVIVE_AXIS_TRACKPAD_X; enum SurviveAxis ay = SURVIVE_AXIS_TRACKPAD_Y; if (so->object_subtype == SURVIVE_OBJECT_SUBTYPE_KNUCKLES_R || so->object_subtype == SURVIVE_OBJECT_SUBTYPE_KNUCKLES_L) { if (!HAS_BIT_FLAG(so->touchmask, SURVIVE_BUTTON_TRACKPAD)) { if ((so->axis[ax] != 0) || (so->axis[ay] != 0)) { entry->eventType = SURVIVE_INPUT_EVENT_AXIS_CHANGED; entry->ids[0] = ax; entry->ids[1] = ay; entry->axisValues[0] = 0; entry->axisValues[1] = 0; entry = incrementAndPostButtonQueue(so); } ax = SURVIVE_AXIS_JOYSTICK_X; ay = SURVIVE_AXIS_JOYSTICK_Y; } } if ((so->axis[ax] != event->touchpadHorizontal) || (so->axis[ay] != event->touchpadVertical)) { entry->eventType = SURVIVE_INPUT_EVENT_AXIS_CHANGED; entry->ids[0] = ax; entry->ids[1] = ay; entry->axisValues[0] = event->touchpadHorizontal; entry->axisValues[1] = event->touchpadVertical; entry = incrementAndPostButtonQueue(so); } } if (event->proximityValid) { size_t axisCnt = 0; for (int i = 0; i < 6; i++) { if (event->proximity[i] != so->axis[SURVIVE_AXIS_MIDDLE_FINGER_PROXIMITY + i]) { entry->eventType = SURVIVE_INPUT_EVENT_AXIS_CHANGED; entry->ids[axisCnt] = SURVIVE_AXIS_MIDDLE_FINGER_PROXIMITY + i; entry->axisValues[axisCnt++] = event->proximity[i]; } } if (axisCnt > 0) { entry = incrementAndPostButtonQueue(so); } } for (int i = 0; i < event->rawAxisCnt; i++) { if (event->rawAxis[i] != so->axis[i]) { entry->eventType = SURVIVE_INPUT_EVENT_AXIS_CHANGED; entry->ids[0] = i; entry->axisValues[0] = event->rawAxis[i]; entry = incrementAndPostButtonQueue(so); } } if (event->batteryChargeValid) { so->charge = event->batteryCharge; } } #define FAILURE_ON_FALSE(x) \ if (!(x)) { \ SV_WARN("Assert failure for parsing watchman: " #x); \ goto failure; \ }; //#define DEBUG_WATCHMAN #define DEBUG_WATCHMAN_ERRORS #ifdef DEBUG_WATCHMAN #define DEBUG_WATCHMAN_ERRORS #endif #define AS_SHORT(a, b) ((uint16_t)(((uint16_t)a) << 8) | (0x00ff & b)) #define POP_BYTE(ptr) ((uint8_t) * ((ptr)++)) #define POP_SHORT(ptr) (((((struct unaligned_u16_t *)(((ptr) += 2) - 2))))->v) #define HAS_FLAG(flags, flag) ((flags & (flag)) == (flag)) char hexstr[512]; static char *packetToHex(const uint8_t *packet, const uint8_t *packetEnd) { int count = packetEnd - packet; int i; for (i = 0; i < count; i++) sprintf(&hexstr[i * 3], "%02x ", packet[i]); hexstr[i * 3] = 0; return hexstr; } char bin[9] = {0}; static char *byteToBin(uint8_t b) { for (int i = 0; i < 8; i++) { bin[i] = ((b >> (7 - i)) & 1) ? '1' : '0'; } bin[8] = 0; return bin; } struct sensorData { uint8_t sensorId; uint8_t edgeCount; }; static int32_t read_light_data(SurviveObject *w, uint16_t time, uint8_t **readPtr, uint8_t *payloadEndPtr, LightcapElement *output, int output_cnt) { uint8_t *payloadPtr = *readPtr; SurviveContext *ctx = w->ctx; uint32_t reference_time = w->activations.last_imu; bool isGen2 = 0; // w->ctx->lh_version == 1; (void)isGen2; if (payloadEndPtr - payloadPtr <= 3) { return 0; } // DEBUG if ((*payloadPtr & 0xE0) == 0xE0) { SV_WARN("Light contains probable non-light data : 0x%02hX [Time:%04hX] [Payload: %s]", *payloadPtr, time, packetToHex(payloadPtr, payloadEndPtr)); } /* * ---=== LIGHT DATA STRUCTURE ===--- * * | SensorData | Time Deltas | End Timestamp | * ╔═════════════╦══════════════════════╦═══════════════╗ * ║ SS SS .. SS ║ DD DD DD DD DD .. DD ║ TT TT TT ║ * ╚═════════════╩══════════════════════╩═══════════════╝ * * Three parts to the packet, the sensor data which contains which sensors were triggered, and the times * deltas between rising and falling of the sensor event. There are always two rising/falling events per * sensor though the ordering is not simple as new sensor events may start before others are finished. * * The meaning and associated led with each 'event' is determined by the edge count as encoded within the * sensor data (see below) * * The time deltas use variable length encoding, so we can't determine how many sensors are in the packet * just from the packet length. However, we do know that there are always two times per sensor (rise and * fall), so there are (2*Sensor)-1 deltas in the packet (-1 because the end time is 'known' yielding two * times). Therefore, the general read process is thus: * * 1) Read off timestamp * 2) Read the first byte from the start of the packet * 3) Read one time delta from the end of the packet (We get two times from this since we know the 'end' time) * 4) Repeatedly: * a) Read one byte from the start of the packet (Led/Flag) * b) Read two time deltas from the end of the packet (not including timestamp) [See encoding below] * c) Stop once we've read all data in the packet * * * TT TT TT * ~~~~~~~~ * Timestamp of the last event [Little Endian] * * eg: * f6 b4 5b = 6010102 * * DD * ~~ * Time deltas between events stored as variable length sequences. The lower 7 bits of each byte are * summed until a byte with the 8th bit set is encountered. [Little endian] * * 8 76543210 * ╔═╦════════╗ * ║S│Value ║ * ╚═╩════════╝ * ╲ ╲_________ 7 Bits of time delta * ╲___________ Stop bit (1 = Value complete, 0 = Continue reading) * * eg: * 0 = 80 [(80 & 7F) = 0] * 127 = FF [(FF & 7F) = 127] * 128 = 80 01 [(80 & 7F) + ((01 & 7F)<<7) = 128] * 255 = FF 01 [(FF & 7F) + ((01 & 7F)<<7) = 255] * 256 = 80 02 [(80 & 7F) + ((02 & 7F)<<7) = 256] * 16383 = FF 7F [(FF & 7F) + ((7F & 7F)<<7) = 16383] * 16384 = 80 80 01 [(80 & 7F) + ((80 & 7F)<<7) + ((01 & 7F)<<14) = 16384] * * * * SS * ~~ * Packed data about which sensor was detected and how many time deltas it's associated event straddles. * 1 Byte per sensor * * 876543 210 * ╔══════╦═══╗ * ║Sensor│EC ║ * ╚══════╩═══╝ * ╲ ╲____ Edge count * ╲__________ Sensor ID of the event * * eg: * 2B = Sensor 5, 3 edges [2B>>3 = 5, 2B & 03 = 3] * * * Example full packet * =================== * * ┌──────┬───────┐ * │Sensor│ Edges │ * ├──────┼───────┤ * │ 5 │ 3 │ * │ 10 │ 1 │ * │ 9 │ 2 │ * │ 5 │ 0 │ * │ 12 │ 0 │ * └──────┴───────┘ End Time : 6010102 * ╲ ╱ * ╲ ╱ * ╔════════════════╦═┄┄┄┄┄┄┄┄┄┄┄═╦══════════╗ * ║ 2b 51 4a 28 60 ║ Time Deltas ║ f6 b4 5b ║ * ╚════════════════╩═┄┄┄┄┄┄┄┄┄┄┄═╩══════════╝ * ╱ ╲ * _______________________╱ ╲_______________________ * ╱ ╲ * ╱ ╲ * ╔═══════╤═══════╤═══════╤══════════╤═══════╤════╤════╤═══════╤═══════╗ * ║ c7 2e │ e6 66 │ 84 1f │ fb 31 0b │ d9 01 │ da │ ca │ db 02 │ e4 02 ║ * ╠═══════╪═══════╪═══════╪══════════╪═══════╪════╪════╪═══════╪═══════╣ * ║ 5959 | 13158 | 3972 | 186619 | 217 | 90 | 74 | 347 |356 ║ * ╚═══════╧═══════╧═══════╧══════════╧═══════╧════╧════╧═══════╧═══════╝ * │ │ │ │ │ │ │ │ │ │ * ┕━━━━━━«E │ │ │ │ │ │ │ │ -> Led 12 : 5799310 -> 5805269 * ┕━━━━━━«D │ │ │ │ │ │ -> Led 5 : 5818427 -> 5822399 * ┕━━━━━━━2━━━━2━━━«C │ │ -> Led 9 : 6009018 -> 6009399 * │ │ ┊ │ │ * ┕━━━━3━━━━3━━━━━━━3━━━━━━«A -> Led 5 : 6009235 -> 6010102 * | ┊ | * ┕━━━━1━━━━━━«B -> Led 10 : 6009325 -> 6009746 * Read order : * A : Ends at 'A' - Skip 3 edges to find start * B : Ends at 'B' - Skip 1 edge to find start * C : Ends at 'C' - Skip 2 edges to find start * D : Ends at 'D' (Since the 'end' edges from ABC have already been 'used'), ends at next edge * E : Ends at 'E' - Ends at next edge */ // Step 1 - Extract deltas between events the corresponding sensors size_t timeIndex = 0; uint32_t times[16] = {0}; size_t maxTimeIndex = sizeof(times) / sizeof(times[0]); size_t maxEvents = maxTimeIndex >> 1; uint8_t *reportOrder = alloca(sizeof(uint8_t*) * maxTimeIndex); struct sensorData* sensors = alloca(sizeof(struct sensorData) * maxEvents); uint8_t *idsPtr = payloadPtr; uint8_t *eventPtr = payloadEndPtr; // Last three bytes of light data are the LSB of the time of the last event eventPtr -= 4; uint32_t lastEventTime = ((uint32_t)(time >> 8) << 24) | (eventPtr[3] << 16) | (eventPtr[2] << 8) | (eventPtr[1] << 0); // The general issue is that the 'time1' field isn't super in sync with the last 3 bytes we use for timing // light events -- it can tip a smidge before those bytes see it or sometimes after. This can cause // wild 1<<24 tick differences which break everything. // // We base it off IMU as a reference because light events can get blocked and it's not out of the ordinary // to not see them for a second or two. (1 << 23) on a 48mhz clock is ~150ms; and the IMU is consistently // much faster than that. if (lastEventTime > reference_time && lastEventTime - reference_time > (1 << 23)) { lastEventTime -= (1 << 24); } else if (reference_time > lastEventTime && reference_time - lastEventTime > (1 << 23)) { lastEventTime += (1 << 24); } times[0] = lastEventTime; SV_VERBOSE(750, "Packet Start Time: %u (0x%x) (ref: %u) Payload: %s", lastEventTime, lastEventTime, reference_time, packetToHex(payloadPtr, payloadEndPtr)); while (idsPtr + (timeIndex >> 1u) < eventPtr) { // Obtain the timing to the previous event // Variable length encoding [if bit 8 is 0, continue into next byte] uint32_t timeDelta = 0; uint8_t *eventPtrStart = eventPtr; while (true) { timeDelta <<= 7; timeDelta |= (*eventPtr & 0x7F); if (((*(eventPtr--)) & 0x80) == 0x80) break; if (idsPtr + (timeIndex >> 1u) > eventPtr) { eventPtr = eventPtrStart; goto exit_while; } } lastEventTime -= timeDelta; if (timeIndex >= 16) { return -8; } // Store the event time times[++timeIndex] = lastEventTime; SV_VERBOSE(750, "Time: [%zd] %u (%u) %s", timeIndex, lastEventTime, timeDelta, packetToHex(eventPtr + 1, eventPtrStart + 1)); } exit_while: if (timeIndex % 2 == 0 && timeIndex > 0) { timeIndex--; do { eventPtr++; } while ((*eventPtr & 0x80) == 0); } assert(timeIndex % 2 == 1); size_t sensor_byte_cnt = eventPtr - idsPtr + 1; if (sensor_byte_cnt > ((timeIndex >> 1) + 1)) { isGen2 = true; // SV_VERBOSE(100, "Likely gen2 data; %x %d %d %d", *idsPtr, sensor_byte_cnt, (timeIndex >> 1), idsPtr - //(eventPtr - (timeIndex >> 1))); idsPtr = eventPtr - (timeIndex >> 1); } // There are two time deltas per 'event' for (int i = 0; i < (timeIndex >> 1) + 1; i++) { sensors[i].sensorId = ((*idsPtr) >> 3) & 0x1F; sensors[i].edgeCount = (*idsPtr) & 0x7; SV_VERBOSE(750, "Sensor: %2d Edge: %d (%02x)", sensors[i].sensorId, sensors[i].edgeCount, (*idsPtr)); idsPtr++; } // Step 2 - Convert events to pulses LightcapElement* les = alloca(sizeof(LightcapElement) * maxEvents); size_t eventCount = (timeIndex + 1) >> 1; // timeIndex>>1 = There are always twice as many time events as sensors memset(les, 0, maxEvents * sizeof(LightcapElement)); memset(reportOrder, 0, maxTimeIndex * sizeof(uint8_t)); timeIndex = -1; for (int i = 0; i < eventCount; i++) { // Get the end time (Increment and find the next 'unused' time) while (times[++timeIndex] == 0) if (timeIndex + 1 >= maxTimeIndex) { return -2; } if (timeIndex >= maxTimeIndex) { return -3; } // Get the start time size_t startTimeIndex = timeIndex + (sensors[i].edgeCount + 1); if (startTimeIndex >= maxTimeIndex) { return -4; } // Store the start index so we can return in ascending time order if (reportOrder[startTimeIndex] != 0) { return -5; } reportOrder[startTimeIndex] = i + 1; LightcapElement *le = &les[i]; // Fill in the LightcapElement data assert(le->sensor_id == 0); le->sensor_id = sensors[i].sensorId; le->timestamp = times[startTimeIndex]; le->length = times[timeIndex] - times[startTimeIndex]; // Flag the start time as 'used' times[startTimeIndex] = 0; // SV_INFO("Light Event : [%i|%i] %li -> %li (%li) [%i-%i]", les[i].sensor_id, sensors[i].edgeCount, // les[i].timestamp, les[i].timestamp + les[i].length, les[i].length, startTimeIndex, timeIndex); } // Output the events in ascending time order uint8_t orderedIndex; for (int i = 0; (i < maxTimeIndex) && output_cnt > 0; i++) { if ((orderedIndex = reportOrder[i]) != 0) { LightcapElement *ol = &les[orderedIndex - 1]; if (!(ol->length != 0 || ol->timestamp != 0)) { return -6; } *(output++) = *ol; output_cnt--; SV_VERBOSE(750, "Light Event [Ordered]: %i [%2i] %u -> %u (%4hu)", i, ol->sensor_id, ol->timestamp, ol->timestamp + ol->length, ol->length); } } return eventCount; } static bool read_imu_data(SurviveObject *w, uint64_t time_in_us, uint16_t time, uint8_t **readPtr, const uint8_t *payloadEndPtr) { uint8_t *payloadPtr = *readPtr; SurviveContext *ctx = w->ctx; if (payloadEndPtr - payloadPtr < 7) { return false; } // First byte is higher res time, followed by 6 shorts uint8_t timeLSB = POP_BYTE(payloadPtr); int16_t aX = POP_SHORT(payloadPtr); int16_t aY = POP_SHORT(payloadPtr); int16_t aZ = POP_SHORT(payloadPtr); int16_t rX = POP_SHORT(payloadPtr); int16_t rY = POP_SHORT(payloadPtr); int16_t rZ = POP_SHORT(payloadPtr); FLT agm[9] = {aX, aY, aZ, rX, rY, rZ}; SV_VERBOSE(750, "%s IMU: %d " Point3_format " " Point3_format " From: %s", w->codename, timeLSB, LINMATH_VEC3_EXPAND(agm), LINMATH_VEC3_EXPAND(agm + 3), packetToHex(*readPtr, payloadPtr)); SURVIVE_INVOKE_HOOK_SO(raw_imu, w, 3, agm, ((uint32_t)time << 16) | (timeLSB << 8), 0); SurviveSensorActivations_register_runtime(&w->activations, w->activations.last_imu, time_in_us); *readPtr = payloadPtr; return true; } #define UPDATE_PTR_AND_RETURN \ *readPtr = payloadPtr; \ return true; static void handle_battery(SurviveObject *w, uint8_t batStatus) { // int8_t percent = (int8_t)((((FLT)(batStatus & 0x7f)) / 0x7f) * 100); int8_t percent = (batStatus & 0x7fu); bool charging = (batStatus & 0x80) == 0x80; if (percent != 0 || charging) { w->charging = charging; w->charge = percent; SurviveContext *ctx = w->ctx; SV_VERBOSE(110, "%s Battery charge %d%%(%s)", w->codename, percent, charging ? "Charging" : "Not charging"); } } static bool read_event(SurviveObject *w, uint64_t time_in_us, uint16_t time, uint8_t **readPtr, uint8_t *payloadEndPtr) { uint8_t *payloadPtr = *readPtr; SurviveContext *ctx = w->ctx; // If we're looking at light data, return if (!HAS_FLAG(*payloadPtr, 0xE0)) return true; // This is some kind of heartbeat if (*payloadPtr == 0xE2) { *readPtr = payloadEndPtr; return true; } /* * Event Flags * =========== * * If input (button, touch, motion) data is present in packet: * ┄╦═╤═╤═╦═╤═╤═╤═╤═╦┄ * ║1│1│1│1│I│-│-│-║ * ┄╩═╧═╧═╩═╧═╧═╧═╧═╩┄ * * If battery data is present in a packet: * ┄╦═╤═╤═╦═╤═╤═╤═╤═╦═══════════════╦┄ * ║1│1│1│0│?│?│?│1║ Battery ║ * ┄╩═╧═╧═╩═╧═╧═╧═╧═╩═══════════════╩┄ * ▲ │ * ╰───────────────────────────────╯ * * If battery data is not present in packet (EG IMU only packets): * ┄╦═╤═╤═╦═╤═╤═╤═╤═╦┄ * ║1│1│1│0│I│?│?│0║ * ┄╩═╧═╧═╩═╧═╧═╧═╧═╩┄ * * I: IMU Data 1 = IMU Data present after event (13 Bytes) * 0 = No IMU Data present after event */ const uint8_t flags = POP_BYTE(payloadPtr); bool flagInput = HAS_FLAG(flags, 0x10); bool flagIMU = HAS_FLAG(flags, 0x08); if (flagInput) { /* * Flags for input events are as follows: * * ┄╦═╤═╤═╦═╤═╤═╤═╤═╦┄ * │1│1│1│1│-│t│m│b│ * ┄╩═╧═╧═╩═╧═╧═╧═╧═╩┄ * * t: Trigger 1 = Trigger data present in event [1 Byte] ╮ * m: Motion 1 = Motion data present in event [4 Byte] ├ If all 0, this is a gen 2 event [See below] * b: Button 1 = Button data present in event [1 Byte] ╯ * * Order of data in payload is as follows: * ┄╦═══════════════╦═══════════════╦═══════════════╦═══════════════╦ * ║ [Button/b] ║ [Trigger/t] ║ [Motion/t] ║ [IMU Data/I] ║ * ┄╩═══════════════╩═══════════════╩═══════════════╩═══════════════╩ */ bool firstGen = ((flags & 0x7) != 0); buttonEvent bEvent = {0}; if (firstGen) { bool flagTrigger = HAS_FLAG(flags, 0x4); bool flagMotion = HAS_FLAG(flags, 0x2); bool flagButton = HAS_FLAG(flags, 0x1); if (flagButton) { bEvent.pressedButtonsValid = 1; bEvent.pressedButtons = POP_BYTE(payloadPtr); } if (flagTrigger) { bEvent.triggerHighResValid = 1; bEvent.triggerHighRes = POP_BYTE(payloadPtr) / 255.; } if (flagMotion) { bEvent.touchpadHorizontalValid = 1; bEvent.touchpadVerticalValid = 1; bEvent.touchpadHorizontal = (int16_t)POP_SHORT(payloadPtr) / 32768.; bEvent.touchpadVertical = (int16_t)POP_SHORT(payloadPtr) / 32768.; } } else { // Second gen event (Eg Knuckles proximity) uint8_t genTwoType = POP_BYTE(payloadPtr); // May be flags, but currently only observed to be 'a1' when knuckles if (genTwoType == 0xA1) { // Knucles /*SV_INFO("GRIP 0x%02hX [Time:%04hX] [Payload: %s] <>", *(payloadPtr-1), time, packetToHex(payloadPtr, payloadEndPtr));*/ // Resistive contact sensors in buttons? uint8_t touchFlags = POP_BYTE(payloadPtr); // 0x01 = Trigger // 0x08 = Menu // 0x10 = Button A // 0x20 = Button B // 0x40 = Thumbstick bEvent.touchedButtonsValid = 1; bEvent.touchedButtons = (touchFlags & ~0x40u); bEvent.touchedButtons |= ((touchFlags & 0x40u) >> 4); bEvent.proximityValid = 1; // Non-touching proximity to fingers bEvent.proximity[0] = POP_BYTE(payloadPtr) / 255.; // Middle finger bEvent.proximity[1] = POP_BYTE(payloadPtr) / 255.; // Ring finger bEvent.proximity[2] = POP_BYTE(payloadPtr) / 255.; // Pinky finger bEvent.proximity[3] = POP_BYTE(payloadPtr) / 255.; // Index finger (trigger) // Contact force (Squeeze strength) bEvent.proximity[4] = POP_BYTE(payloadPtr) / 255.; bEvent.proximity[5] = POP_BYTE(payloadPtr) / 255.; } else { SV_WARN("Unknown gen two event %s 0x%02hX 0b%s [Time:%04hX] [Payload: %s] <>", w->codename, *(payloadPtr - 1), byteToBin(*(payloadPtr - 1)), time, packetToHex(payloadPtr, payloadEndPtr)); // Since we don't know how much data this should consume, proceeding to IMU/Light decode is likely // to choke. return false; } } registerButtonEvent(w, &bEvent, BUTTON_EVENT_SOURCE_RF); } else { /* * Flags for non-input (status) events are as follows: * * ┄╦═╤═╤═╦═╤═╤═╤═╤═╦┄ * │1│1│1│0│-│?│?│b│ * ┄╩═╧═╧═╩═╧═╧═╧═╧═╩┄ * * b: Battery 1 = Battery data present in event [1 Byte] possibly followed by an another event or light data */ bool flagBatteryStatus = HAS_FLAG(flags, 0x1); bool flagUnknown = ((flags & 0x6) != 0); // flagUnknown = true; if (flagUnknown) { SV_WARN("Unknown status event 0x%02hX [Time:%04hX] [Payload: %s] <>", *(payloadPtr - 1), time, packetToHex(payloadPtr, payloadEndPtr)); // Since we don't know how much data this should consume, proceeding to IMU/Light decode is likely // to choke. *readPtr = payloadPtr; return false; } if (flagBatteryStatus) { // Battery Status // Happens On USB plugged in. Switch to wired mode? uint8_t batStatus = POP_BYTE(payloadPtr); handle_battery(w, batStatus); #ifdef KNUCKLES_INFO SV_INFO("KAS: @%04hX | Status [Battery: % 3i%%] [%s]", time, percent, (charging ? "CHARGING" : "ON BATTERY")); #endif // Maybe read another event, IMU data or Light Data bool rtn = read_event(w, time_in_us, time, &payloadPtr, payloadEndPtr); *readPtr = payloadPtr; return rtn; } } // Read off any IMU data present if (flagIMU) read_imu_data(w, time_in_us, time, &payloadPtr, payloadEndPtr); UPDATE_PTR_AND_RETURN } static inline void parse_and_process_lightcap(SurviveObject *w, uint16_t time, uint8_t *payloadPtr, uint8_t *payloadEndPtr) { LightcapElement les[10] = {0}; uint8_t *payloadPtrStart = payloadPtr; int32_t cnt = read_light_data(w, time, &payloadPtr, payloadEndPtr, les, 10); SurviveContext *ctx = w->ctx; #ifndef NDEBUG for (int i = (int)cnt - 1; i >= 0; i--) { uint8_t sensor = survive_map_sensor_id(w, les[i].sensor_id); if (sensor == 255) { cnt = -255; break; } } #endif if (cnt < 0) { SV_WARN("Read light data error %d [Time:%04hX] [Payload: %s]", (int)cnt, time, packetToHex(payloadPtr, payloadEndPtr)); SV_WARN("Full payload: %s", packetToHex(payloadPtrStart, payloadEndPtr)); } else { #ifdef VERIFY_LIGHTCAP LightcapElement les_old[10] = {0}; int les_old_cnt = parse_watchman_lightcap(w->ctx, w->codename, time >> 8, w->activations.last_imu, payloadPtr, payloadEndPtr - payloadPtr, les, 10); assert(cnt == les_old_cnt); #endif for (int i = (int)cnt - 1; i >= 0; i--) { #ifdef DEBUG_WATCHMAN printf("%d: %u [%u]\n", les[i].sensor_id, les[i].length, les[i].timestamp); #endif #ifdef VERIFY_LIGHTCAP assert(memcmp(&les[i], &les_old[i], sizeof(LightcapElement)) == 0); #endif handle_lightcap(w, &les[i]); } } } static int parse_and_process_raw1_lightcap(SurviveObject *obj, uint16_t time, uint8_t *packet, uint8_t length) { bool has_errors = false; uint8_t idx = 0; uint8_t channel = 255; SurviveContext *ctx = obj->ctx; bool dump_binary = false; while (idx < length) { uint8_t data = packet[idx]; if (data & 0x1u) { // Since they flag for this; I assume multiples can appear in a single packet. Need to plug in // second LH to find out... if ((data & 0x0Au) != 0) { static int unknown_count = 0; if(unknown_count++ < 10) { // Currently I've only ever seen 0x1 if the 1 bit is set; I doubt they left 3 bits on the table // though.... SV_WARN("Not entirely sure what this data is; errors may occur (%d, 0x%02x)\n", idx, data); dump_binary = true; } // has_errors = true; goto exit_loop; } // encodes like so: 0bcccc ?F?C bool hasConflict = data & 0x04u; if (hasConflict) { uint8_t conflicted_channel = data >> 4u; SV_WARN("Two or more lighthouses are on channel %d; tracking is most likely going to fail.", conflicted_channel); } channel = data >> 4u; SV_VERBOSE(750, "%s Channel %d (0x%02x)", obj->codename, channel, data); idx++; } else { uint32_t timecode = 0; memcpy(&timecode, packet + idx, sizeof(uint32_t)); uint32_t reference_time = (obj->activations.last_imu); bool sync = timecode & 0x2u; if (!sync) { // O SC // GO YH // ET NA // [??] NX[ 24 bit time @ 48mhz ]CN // encodes like so: 0bXXXX ABTTT TTTT TTTT TTTT TTTT TTTT TTSC bool ootx = (timecode >> 26u) & 1u; bool g = (timecode >> 27u) & 1u; uint32_t time24 = (timecode >> 2u) & 0xFFFFFFu; timecode = fix_time24(ctx, time24, reference_time); uint8_t unused = timecode >> 28; if (unused && dump_binary) { SV_WARN("Not sure what this is: %x", unused); } SV_VERBOSE(750, "Sync %s %02d %d %8u", obj->codename, channel, ootx, timecode); if (channel == 255) { SV_WARN("No channel specified for sync"); dump_binary = true; // has_errors = true; } else { SURVIVE_INVOKE_HOOK_SO(sync, obj, channel, timecode, ootx, g); } } else { // SC // YH // NA // [SNSR][ 25 bit time @ 96mhz ]CN // encodes like so: 0bSSSS STTT TTTT TTTT TTTT TTTT TTTT TFSC // Since nothing in libsurvive thinks anything is 96mhz; pass in a flag bool half_clock_flag = timecode & 0x4u; uint8_t sensor = (timecode >> 27u); timecode = fix_time24(ctx, (timecode >> 3u) & 0xFFFFFFu, reference_time); SV_VERBOSE(750, "Sweep %s %02d.%02d %8u", obj->codename, channel, sensor, timecode); if (channel == 255) { SV_WARN("No channel specified for sweep"); dump_binary = true; // has_errors = true; } else { SURVIVE_INVOKE_HOOK_SO(sweep, obj, channel, survive_map_sensor_id(obj, sensor), timecode, half_clock_flag); } } if (channel == 255) { dump_binary = true; has_errors = true; goto exit_loop; } idx += 4; } } exit_loop: if (dump_binary) { for (int i = 0; i < length; i++) { if ((i + 2) % 4 == 0) SURVIVE_INVOKE_HOOK(printf, ctx, " "); SURVIVE_INVOKE_HOOK(printf, ctx, "%02x ", packet[i]); } SURVIVE_INVOKE_HOOK(printf, ctx, "\n"); } return has_errors ? -1 : idx; } #define VERIFY_LENGTH_OR_FAIL(payloadPtr, len) \ if (payloadEndPtr - payloadPtr < (len)) { \ SV_WARN("%s handle_input needed %d bytes but had %u", w->codename, (len), \ (uint32_t)(payloadEndPtr - payloadPtr)); \ goto exit_failure; \ } static bool handle_input(SurviveObject *w, uint8_t flags, uint8_t **payloadPtr, uint8_t *payloadEndPtr) { /* * Flags for input events are as follows: * * ┄╦═╤═╤═╦═╤═╤═╤═╤═╦┄ * │1│1│1│1│-│t│m│b│ * ┄╩═╧═╧═╩═╧═╧═╧═╧═╩┄ * * t: Trigger 1 = Trigger data present in event [1 Byte] ╮ * m: Motion 1 = Motion data present in event [4 Byte] ├ If all 0, this is a gen 2 event [See below] * b: Button 1 = Button data present in event [1 Byte] ╯ * * Order of data in payload is as follows: * ┄╦═══════════════╦═══════════════╦═══════════════╦═══════════════╦ * ║ [Button/b] ║ [Trigger/t] ║ [Motion/t] ║ [IMU Data/I] ║ * ┄╩═══════════════╩═══════════════╩═══════════════╩═══════════════╩ */ struct SurviveContext *ctx = w->ctx; bool firstGen = ((flags & 0x7) != 0); buttonEvent bEvent = { 0 }; uint8_t *payloadStart = *payloadPtr; if (firstGen) { bool flagUnknown = HAS_FLAG(flags, 0x8); bool flagTrigger = HAS_FLAG(flags, 0x4); bool flagMotion = HAS_FLAG(flags, 0x2); bool flagButton = HAS_FLAG(flags, 0x1); if (flagButton) { bEvent.pressedButtonsValid = 1; VERIFY_LENGTH_OR_FAIL(*payloadPtr, 1); bEvent.pressedButtons = POP_BYTE(*payloadPtr); } if (flagTrigger) { bEvent.triggerHighResValid = 1; VERIFY_LENGTH_OR_FAIL(*payloadPtr, 1); bEvent.triggerHighRes = POP_BYTE(*payloadPtr) / 255.; } if (flagMotion) { bEvent.touchpadHorizontalValid = 1; bEvent.touchpadVerticalValid = 1; VERIFY_LENGTH_OR_FAIL(*payloadPtr, 4); bEvent.touchpadHorizontal = (int16_t)POP_SHORT(*payloadPtr) / 32768.; bEvent.touchpadVertical = (int16_t)POP_SHORT(*payloadPtr) / 32768.; } if (flagUnknown) { SV_VERBOSE(150, "Unknown flag in handle_inut"); } SV_VERBOSE(150, "handle_input flags %d %d %d", flagButton, flagTrigger, flagMotion); } else { // Second gen event (Eg Knuckles proximity) uint8_t genTwoType = POP_BYTE(*payloadPtr); // May be flags, but currently only observed to be 'a1' when knuckles if (genTwoType == 0xA1) { // Knuckles // Resistive contact sensors in buttons? VERIFY_LENGTH_OR_FAIL(*payloadPtr, 7); uint8_t touchFlags = POP_BYTE(*payloadPtr); // 0x01 = Trigger // 0x08 = Menu // 0x10 = Button A // 0x20 = Button B // 0x40 = Thumbstick bEvent.touchedButtonsValid = 1; bEvent.touchedButtons = (touchFlags & ~0x40u); bEvent.touchedButtons |= ((touchFlags & 0x40u) >> 4); bEvent.proximityValid = 1; // Non-touching proximity to fingers bEvent.proximity[0] = POP_BYTE(*payloadPtr) / 255.; // Middle finger bEvent.proximity[1] = POP_BYTE(*payloadPtr) / 255.; // Ring finger bEvent.proximity[2] = POP_BYTE(*payloadPtr) / 255.; // Pinky finger bEvent.proximity[3] = POP_BYTE(*payloadPtr) / 255.; // Index finger (trigger) // Contact force (Squeeze strength) bEvent.proximity[4] = POP_BYTE(*payloadPtr) / 255.; // Trackpad force bEvent.proximity[5] = POP_BYTE(*payloadPtr) / 255.; } else { SV_WARN("Unknown gen two event 0x%02hX 0b%s [Payload: %s] <>", *(*payloadPtr - 1), byteToBin(*(*payloadPtr - 1)), packetToHex(*payloadPtr, payloadEndPtr)); // Since we don't know how much data this should consume, proceeding to IMU/Light decode is likely // to choke. return false; } } registerButtonEvent(w, &bEvent, BUTTON_EVENT_SOURCE_RF); return true; exit_failure: return false; } static void handle_watchman_v2(SurviveObject *w, uint64_t time_in_us, uint16_t time, uint8_t *payloadPtr, uint8_t *payloadEndPtr) { struct SurviveContext *ctx = w->ctx; const uint8_t *originPayloadPtr = payloadPtr; struct SurviveUSBInfo *driverInfo = w->driver; if (driverInfo->nextCfgSubmitTime >= 0) { return; } if (w->ctx->lh_version == 1 && driverInfo->lightcapMode != LightcapMode_raw1) { vive_switch_mode(driverInfo, LightcapMode_raw1); return; } uint8_t flags = POP_BYTE(payloadPtr); bool has_errors = false; // Some kind of startup heartbeat? if (flags == 0xe2) { SV_VERBOSE(500, "Heartbeat(?) packet %s: '%s'", w->codename, packetToHex(payloadPtr, payloadEndPtr)); return; } // Info: Watchman v2(KN1): '34 f2 3f 9f a2 c9 | 61 06 8c 0a 44 ' // Info: Watchman v2(KN1): '34 f2 7a 9e fe c8 | 61 b4 6e 27 02 7e 95 7c 7c d6 b3 7c 5c a6 da 7c bc ' // Info: Watchman v2(KN1): '38 f0 a1 00 2a 00 00 1c 00 01 | 61 32 9d 62 15 2e d8 62 0d 62 26 63 45 ' // Info: Watchman v2(KN1): '31 f4 ff | 61 de 25 d1 62 ' // Touch (not press) A // Info: Watchman v2(KN1): '20 f0 a1 20 00 01 00 00 00 00 ' // Pulling trigger: // Info: Watchman v2(KN1): '20 f4 39 ' // Info: Watchman v2(KN1): '20 f0 a1 00 bb f5 ff e5 07 00 ' // Info: Watchman v2(KN1): '20 f0 a1 00 00 03 00 00 00 00 ' // Info: Watchman v2(KN1): '31 f1 00 | 61 42 a4 ef 80 ee b4 ef c0 ' // Info: Watchman v2(KN1): '31 f4 0c 61 44 56 97 09 46 88 5b b3 8a a1 5b 1b 9a e4 5b 3b 76 f0 5b bb ' // Hitting A: // Info: Watchman v2(KN1): '31 f4 f8 | 61 ca 25 9f 7f 26 49 9f 5f 06 c5 9f bf f6 db 9f 17 8a e8 // 9f 0f ' // Hitting B // Info: Watchman v2(KN1): '31 f4 12 | 61 68 62 56 01 ' // Info: Watchman v2(KN1): '35 f6 5b ef e5 6d 3e | 61 cc 3d de 00 c6 51 ea 59 72 7b ea b9 9a 81 ea 11 ' // Info: Watchman v2(KN1): '35 f6 2d 36 ec 07 1e | 61 da 56 93 c0 02 aa 93 60 ' // Info: Watchman v2(KN1): '35 f6 15 b6 25 98 1b | 61 20 c4 fa 03 fa 89 23 78 f6 c9 23 58 3a 2d 24 b8 ' // Possibly this means the protocol is done switching: // Info: Watchman v2(KN1): '51 20 01 | 61 50 d2 57 01 ' // Info: Watchman v2(KN1): '36 f7 02 18 12 e1 b8 06 | 61 74 7e bf 0b 26 b3 ac 7f 76 d2 ac 5f e2 00 ad 17 ' // Info: Watchman v2(KN1): '80 a5 4c fb 8f f1 f2 f9 d3 ff 2c 00 df ff ' // Info: Watchman v2(KN1): 'c0 92 52 fb 7d f1 ea f9 ce ff 2f 00 dc ff 20 01 ' // bool has_errors = !read_event(w, time, &payloadPtr, payloadEndPtr); bool flagUnknown01 = HAS_FLAG(flags, 0x01); bool flagUnknown02 = HAS_FLAG(flags, 0x02); // Haptic on the trackpad thing on the knuckles? bool flagUnknown04 = HAS_FLAG(flags, 0x04); bool flagUnknown08 = HAS_FLAG(flags, 0x08); bool flagLightcap = HAS_FLAG(flags, 0x10); bool flagInput = HAS_FLAG(flags, 0x20); bool flagMetaData = HAS_FLAG(flags, 0x40); bool flagIMU = HAS_FLAG(flags, 0x80); if (HAS_FLAG(flags, ~0xD1)) { SV_VERBOSE(100, "%s Unknown flag %02x", w->codename, flags); } if (flagIMU) read_imu_data(w, time_in_us, time, &payloadPtr, payloadEndPtr); // These things happen every 10 seconds if (flagMetaData) { uint8_t marker_byte = POP_BYTE(payloadPtr); switch (marker_byte) { case 0x80: { uint8_t battery_byte = POP_BYTE(payloadPtr); handle_battery(w, battery_byte); break; } case 0x20: { // Mode? uint8_t unknown_byte = POP_BYTE(payloadPtr); break; } case 0x30: { // Only have seen zeros here uint8_t unknown_byte1 = POP_BYTE(payloadPtr); uint8_t unknown_byte2 = POP_BYTE(payloadPtr); break; } default: SV_VERBOSE(100, "%.7f %s Unknown metadata marker in v2 (%02x %02x) bytes dropping rest of data %s", survive_run_time(ctx), w->codename, flags, marker_byte, packetToHex(originPayloadPtr, payloadEndPtr)); return; } } else { SV_VERBOSE(700, "%.7f %s ref flag 0x40(0x%x) bytes rest of data %s", survive_run_time(ctx), w->codename, flags, packetToHex(originPayloadPtr, payloadEndPtr)); } if (flagInput) { VERIFY_LENGTH_OR_FAIL(payloadPtr, 1); uint8_t input_flags = POP_BYTE(payloadPtr); has_errors = !handle_input(w, input_flags, &payloadPtr, payloadEndPtr); } if (driverInfo->packetsSeenWaitingForV2 > 200) { driverInfo->packetsSeenWaitingForV2 = 0; driverInfo->timeWithoutFlag = 0; } if (driverInfo->timeWithoutFlag > 0 && driverInfo->timeWithoutFlag < 20) { driverInfo->packetsSeenWaitingForV2++; if (flagLightcap) { driverInfo->timeWithoutFlag = 1; flagLightcap = false; SV_VERBOSE(200, "Discard %s %lu: '%s'", w->codename, driverInfo->timeWithoutFlag, packetToHex(payloadPtr, payloadEndPtr)); } else { driverInfo->timeWithoutFlag++; } } if (flagLightcap && !has_errors) { if (driverInfo->lightcapMode == LightcapMode_raw0) { parse_and_process_lightcap(w, time, payloadPtr, payloadEndPtr); } else { int read = parse_and_process_raw1_lightcap(w, time, payloadPtr, payloadEndPtr - payloadPtr); if (read == -1) has_errors = true; else payloadPtr += read; } } if (driverInfo->timeWithoutFlag == 20) { if (payloadPtr != payloadEndPtr) { has_errors = true; SV_WARN("Did not read full input packet; %ld bytes remain", payloadEndPtr - payloadPtr); } } if (!has_errors) { return; } exit_failure: survive_dump_buffer(ctx, originPayloadPtr, payloadEndPtr - originPayloadPtr); } static bool use_watchman_v2(SurviveObject *w) { if (w->ctx->lh_version == 1) { return true; } switch (w->object_subtype) { case SURVIVE_OBJECT_SUBTYPE_TRACKER: case SURVIVE_OBJECT_SUBTYPE_WAND: return false; default: return true; } } void survive_handle_watchman(SurviveObject *w, uint64_t time_in_us, uint8_t *readdata) { // KASPER'S DECODE SurviveContext *ctx = w ? w->ctx : 0; struct SurviveUSBInfo *driver = w->driver; if (driver->ignoreCnt) { driver->ignoreCnt--; return; } /* * ---=== PACKET STRUCTURE ===--- * Key: * [Optional] - Element may or may not be present depending on flags * 1 = Bit is set * 0 = Bit is not set * - = Bit is "don't care" * ? = Bit meaning is unknown * ║ = Byte boundary * │ = Bit boundary * * * ┊<------- Size is the length of this section in bytes ------->┊ * ┊ ┊ * ╔═══════════════╦═══════════════╬═══════════════╦┄┄┄ ┊ * ║Time MSB ║Size ║Time LSB ║ ┊ * ╚═══════════════╩═══════════════╩═══════════════╩┄┄┄ ┊ * ┊ ┊ * ┄┄┄╬═╤═╤═╦═╤═╤═╤═╤═╦═┄┄┄┄┄┄┄┄┄┄┄═╦═┄┄┄┄┄┄┄┄┄┄┄┄══╣ * For Non-Light events (First three bits = 111) ║1│1│1│ FLAGS ║ [Payload] ║ [Light Data] ║ * ┄┄┄╬═╧═╧═╩═╧═╧═╧═╧═╩═┄┄┄┄┄┄┄┄┄┄┄═╩═┄┄┄┄┄┄┄┄┄┄┄┄══╣ * ┊ ┊ * ┄┄┄╬═══════════════════┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄══╣ * For light-only events (First three bits != 111)║ Light Data ║ * ┄┄┄╩═══════════════════┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄══╝ * * Since bits 7&8 are part of the input event or the sensor id, this places limits on the available id space thus : * The first byte of the light data is sensor data, of which the upper 5 bits are the sensor id, and given * bits 7&8 cannot both be 1, results in the range 00000XXX to 10111XXX yielding a maximum of 24 light * elements per device (In the current format). * * Packet Decode Process: * 1) Read off Time (MSB and LSB) and Payload Size * 2) If bits 6-8 of the byte 4 are set: * a) Read the input/status event * b) Compute the remaining bytes after the input/status event has been read (The size of the input/status * event is not explicitly provided, so must be implied by the event type. * 3) Read an interpret any remaining bytes as light data * * Watchman V2: * * Watchman protocol V2 has the same time / size breakdown as V1 but is followed with a flags byte: * 0bIMPL xxxx * |||| * ||||- Has Light data * |||- Has Input * ||- Has Metadata * |- Has IMU data * */ uint16_t time = AS_SHORT(readdata[0], readdata[2]); uint8_t payloadSize = readdata[1] - 1; uint8_t *payloadPtr = &readdata[3]; uint8_t *payloadEndPtr = payloadPtr + payloadSize; if (use_watchman_v2(w)) { SV_VERBOSE(750, "Watchman v2(%s): '%s'", w->codename, packetToHex(readdata, payloadEndPtr)); handle_watchman_v2(w, time_in_us, time, payloadPtr, payloadEndPtr); return; } SV_VERBOSE(750, "Watchman v1(%s): %s", w->codename, packetToHex(payloadPtr, payloadEndPtr)); /* if (w->ctx->lh_version == -1) { attempt_lh_detection(w, payloadPtr, payloadEndPtr); return; } */ if (driver->lightcapMode != LightcapMode_raw0) { return; } // Read any non-light events that may be in the packet if (!read_event(w, time_in_us, time, &payloadPtr, payloadEndPtr)) { SV_WARN("Read event failed; full payload: %s", packetToHex(&readdata[3], payloadEndPtr)); return; } // Any remaining data after events (if any) have been read off is light data if (payloadPtr < payloadEndPtr) { LightcapElement les[10] = {0}; int32_t cnt = read_light_data(w, time, &payloadPtr, payloadEndPtr, les, 10); if (cnt < 0) { SV_WARN("Read light data error %d [Time:%04hX] [Payload: %s]", (int)cnt, time, packetToHex(payloadPtr, payloadEndPtr)); } else { #ifdef VERIFY_LIGHTCAP LightcapElement les_old[10] = {0}; int les_old_cnt = parse_watchman_lightcap(w->ctx, w->codename, time >> 8, w->activations.last_imu, payloadPtr, payloadEndPtr - payloadPtr, les, 10); assert(cnt == les_old_cnt); #endif for (int i = (int)cnt - 1; i >= 0; i--) { #ifdef DEBUG_WATCHMAN printf("%d: %u [%u]\n", les[i].sensor_id, les[i].length, les[i].timestamp); #endif #ifdef VERIFY_LIGHTCAP assert(memcmp(&les[i], &les_old[i], sizeof(LightcapElement)) == 0); #endif handle_lightcap(w, &les[i]); } } } } #define DEBUG_WATCHMAN_PRINTF(...) \ if (ctx && ctx->log_level > 500) { \ SURVIVE_INVOKE_HOOK(printf, ctx, __VA_ARGS__); \ } SURVIVE_EXPORT int parse_watchman_lightcap(struct SurviveContext *ctx, const char *codename, uint8_t time1, survive_timecode reference_time, uint8_t *readdata, size_t qty, LightcapElement *les, size_t output_cnt) { assert(qty > 0); uint8_t *mptr = readdata + qty - 3 - 1; //-3 for timecode, -1 to DEBUG_WATCHMAN_PRINTF("_%s lc Data: ", codename); for (int i = 0; i < qty; i++) { DEBUG_WATCHMAN_PRINTF("%02x ", readdata[i]); } DEBUG_WATCHMAN_PRINTF("\n"); uint32_t mytime = (mptr[3] << 16) | (mptr[2] << 8) | (mptr[1] << 0); uint32_t times[20] = {0}; int timecount = 0; int leds; int fault = 0; /// Handle uint32_tifying (making sure we keep it incrementing) mytime |= ((uint32_t)time1 << 24); // The general issue is that the 'time1' field isn't super in sync with the last 3 bytes we use for timing // light events -- it can tip a smidge before those bytes see it or sometimes after. This can cause // wild 1<<24 tick differences which break everything. // // We base it off IMU as a reference because light events can get blocked and it's not out of the ordinary // to not see them for a second or two. (1 << 23) on a 48mhz clock is ~150ms; and the IMU is consistently // much faster than that. if (mytime > reference_time && mytime - reference_time > (1 << 23)) { mytime -= (1 << 24); } else if (reference_time > mytime && reference_time - mytime > (1 << 23)) { mytime += (1 << 24); } times[timecount++] = mytime; // DEBUG_WATCHMAN_PRINTF("_%s Packet Start Time: %u\n", codename, mytime); // First, pull off the times, starting with the current time, then all the delta times going backwards. { while (mptr - readdata > (timecount >> 1)) { uint32_t time_delta = 0; DEBUG_WATCHMAN_PRINTF("%s\t", codename); // https://en.wikipedia.org/wiki/Variable-length_quantity uint8_t codebyte = 0; int codelength = 0; while ((codebyte & 0x80) == 0 && mptr >= readdata) { codebyte = *(mptr--); time_delta = (time_delta << 7) | (codebyte & 0x7f); codelength++; if (codelength > 5) { SV_WARN("Code word too long"); fault = 7; goto end; } DEBUG_WATCHMAN_PRINTF("%02x ", codebyte); } times[timecount++] = (mytime -= time_delta); DEBUG_WATCHMAN_PRINTF(" newtime: %u (%u)\n", mytime, time_delta); } leds = timecount >> 1; // Check that the # of sensors at the beginning match the # of parameters we would expect. if (timecount & 1) { SV_WARN("Uneven time count -- %d %d", leds, timecount); fault = 1; goto end; } // Inordinal LED count if (leds != mptr - readdata + 1) { fault = 2; SV_WARN("Bad LED count in packet %d; should be %d", leds, (int)(mptr - readdata + 1)); goto end; } // LED Count does not line up with parameters } size_t times_size = timecount; int lese = 0; // les's end // Second, go through all LEDs and extract the lightevent from them. { uint8_t *marked = alloca(timecount); memset(marked, 0, timecount); int i, parpl = 0; timecount--; int timepl = 0; // This works, but usually returns the values in reverse end-time order. for (i = 0; i < leds; i++) { uint8_t led = readdata[i] >> 3; int adv = readdata[i] & 0x07; while (marked[timepl]) timepl++; DEBUG_WATCHMAN_PRINTF("TP %d TC: %d : ", timepl, timecount); for (int i = 0; i < timecount; i++) { DEBUG_WATCHMAN_PRINTF("%d", marked[i]); } DEBUG_WATCHMAN_PRINTF("\n"); if (timepl > timecount) { fault = 3; goto end; } // Ran off max of list. uint32_t endtime = times[timepl++]; int end = timepl + adv; if (end > timecount) { SV_WARN("Lightfault 4: %d > %d", end, timecount); fault = 4; goto end; } // end referencing off list if (marked[end] > 0) { fault = 5; goto end; } // Already marked trying to be used. uint32_t starttime = times[end]; marked[end] = 1; // Insert all lighting things into a sorted list. This list will be // reverse sorted, but that is to minimize operations. To read it // in sorted order simply read it back backwards. // Use insertion sort, since we should most of the time, be in order. assert(lese < output_cnt); LightcapElement *le = &les[lese++]; le->sensor_id = led; if ((uint32_t)(endtime - starttime) > 65535) { fault = 6; goto end; } // Length of pulse dumb. le->length = endtime - starttime; le->timestamp = starttime; DEBUG_WATCHMAN_PRINTF("_%s Event: %d %u %u-%u\n", codename, led, le->length, endtime, starttime); int swap = lese - 2; while (swap >= 0 && les[swap].timestamp < les[swap + 1].timestamp) { LightcapElement l; memcpy(&l, &les[swap], sizeof(l)); memcpy(&les[swap], &les[swap + 1], sizeof(l)); memcpy(&les[swap + 1], &l, sizeof(l)); swap--; } } } return lese; end : { DEBUG_WATCHMAN_PRINTF("Light decoding fault: %d", fault); DEBUG_WATCHMAN_PRINTF("Info: _%s %u %u ", codename, time1, reference_time); for (int i = 0; i < qty; i++) { DEBUG_WATCHMAN_PRINTF("%02x ", readdata[i]); } DEBUG_WATCHMAN_PRINTF("\n"); return -fault; } } struct SurviveUSBInfo *survive_vive_register_driver(SurviveObject *so, uint16_t vid, uint16_t pid) { struct SurviveUSBInfo *d = calloc(1, sizeof(struct SurviveUSBInfo)); so->driver = d; d->so = so; d->lightcapMode = LightcapMode_raw0; for (const struct DeviceInfo *info = KnownDeviceTypes; info->name; info++) { if (info == 0) { continue; } if (info->vid != vid || info->pid != pid) { continue; } d->device_info = info; break; } return d; } static inline uint16_t read_buffer16(uint8_t *readdata, int idx) { uint16_t rtn; memcpy(&rtn, readdata + idx, sizeof(uint16_t)); return rtn; } static inline uint32_t read_buffer32(uint8_t *readdata, int idx) { uint32_t rtn; memcpy(&rtn, readdata + idx, sizeof(uint32_t)); return rtn; } static uint32_t earliest_working_revision(uint32_t hw_id) { return 1462663157; } static uint32_t latest_working_revision(uint32_t hw_id) { return 1632556731; } static void parse_tracker_version_info(SurviveObject *so, const uint8_t *data, size_t size) { SurviveContext *ctx = so->ctx; #pragma pack(push, 1) struct { uint32_t revision; uint32_t board_model; char fw_name[32]; uint32_t hardware_id; uint32_t a; uint16_t fpga_major_version; uint8_t fpga_minor_version; uint8_t fpga_patch_version; uint8_t h; uint64_t k; } version_info; #pragma pack(pop) char fw_name[33] = {0}; memcpy(&version_info, data, sizeof(version_info)); memcpy(fw_name, version_info.fw_name, 32); for (int i = 0; i < 32; i++) { fw_name[i] = 0x7f & fw_name[i]; } SV_INFO("Device %s has watchman FW version %u and FPGA version %u/%u/%u; named '%31s'. Hardware id 0x%08x Board " "rev: %d (len %d)", survive_colorize(so->codename), version_info.revision, version_info.fpga_major_version, version_info.fpga_minor_version, version_info.fpga_patch_version, fw_name, version_info.hardware_id, version_info.board_model, (int)size); SV_VERBOSE(105, "Extra version info: 0x%x / 0x%x / 0x%lx", version_info.a, version_info.h, version_info.k); uint32_t earliest_version = earliest_working_revision(version_info.hardware_id); uint32_t latest_version = latest_working_revision(version_info.hardware_id); if (earliest_version > version_info.revision) { SV_WARN("The detected version for device %s is %d; the earliest that is verified to work is %d. You may want " "to upgrade. If this version seems to work, please create an issue at " "https://github.com/cntools/libsurvive/issues with this message so we can update the version list.", so->codename, version_info.revision, earliest_version) } else if (latest_version < version_info.revision) { SV_WARN("The detected version for device %s is %d; the latest that is verified to work is %d. You may have to " "upgrade libsurvive to support this device.", so->codename, version_info.revision, latest_version); } } static void parse_tracker_info(SurviveObject *so, uint8_t id, uint8_t *readdata, size_t size) { SurviveContext *ctx = so->ctx; switch (id) { case VIVE_REPORT_IMU_SCALES: { SV_INFO("IMU Scales 1: 0x%x 0x%x", readdata[0], readdata[1]); goto dump_data; break; } case VIVE_REPORT_CHANGE_MODE: { SV_INFO("Info 4: ") goto dump_data; } case VIVE_REPORT_VERSION_ALT: case VIVE_REPORT_VERSION: { parse_tracker_version_info(so, readdata, size); break; } case VIVE_REPORT_HMD_SET_READOFFSET_USERDATA: { SV_INFO("Info 8: 0x%x", readdata[0]); break; } // Start config stream case VIVE_REPORT_CONFIG_READMODE: { break; } // Request more config stream case VIVE_REPORT_CONFIG_READ: { break; } case VIVE_REPORT_COMMAND: { SV_INFO("Received cmd 0x%02x with %d bytes of data", readdata[0], readdata[1]); survive_dump_buffer(ctx, readdata + 2, readdata[1]); break; } default: { SV_INFO("Unknown field 0x%02x for %s", id, so->codename); goto dump_data; } } return; dump_data: survive_dump_buffer(ctx, readdata, size); } void survive_data_cb_locked(uint64_t time_received_us, SurviveUSBInterface *si) { int size = si->actual_len; SurviveContext *ctx = si->ctx; int iface = si->which_interface_am_i; SurviveObject *obj = si->assoc_obj; uint8_t *readdata = si->buffer; uint8_t *enddata = readdata + size; if (obj == 0) return; int id = POP1; size--; if (!obj->driver) { struct SurviveUSBInfo *d = obj->driver = calloc(1, sizeof(struct SurviveUSBInfo)); d->so = obj; } // We handle this here since it's one of the few things we use without a config loaded. if (si->which_interface_am_i == USB_IF_TRACKER_INFO) { parse_tracker_info(obj, id, readdata, size); return; } if (obj->conf == 0 || (si->usbInfo && si->usbInfo->cfg_user)) { if (si->usbInfo) { //si->usbInfo->tryConfigLoad = 1; } return; } switch (si->which_interface_am_i) { case USB_IF_HMD_HEADSET_INFO: { SurviveObject *headset = obj; readdata += 2; buttonEvent event = {0}; event.touchedButtonsValid = 0; event.touchedButtons = POP1; // Lens event.rawAxisCnt = 3; event.rawAxis[1] = POP2; // Lens Separation readdata += 2; event.touchedButtons |= POP1; // Button readdata += 3; readdata++; // Proxchange, No change = 0, Decrease = 1, Increase = 2 readdata++; event.rawAxis[2] = POP2; // Proximity << how close to face are you? Less than 80 = not on face. event.rawAxis[0] = POP2; // IPD << what is this? headset->ison = 1; registerButtonEvent(headset, &event, BUTTON_EVENT_SOURCE_DEFAULT); break; } case USB_IF_HMD_IMU: case USB_IF_W_WATCHMAN1_IMU: case USB_IF_TRACKER0_IMU: case USB_IF_TRACKER1_IMU: { int i; // printf( "%d -> ", size ); for (i = 0; i < 3 && readdata < enddata; i++) { struct unaligned_16_t *acceldata = (struct unaligned_16_t *)readdata; readdata += 12; uint32_t timecode = POP4; uint8_t code = POP1; // printf( "%d ", code ); int8_t cd = code - obj->oldcode; if (cd > 0) { obj->oldcode = code; // XXX XXX BIG TODO!!! Actually recal gyro data. FLT agm[9] = {acceldata[0].v, acceldata[1].v, acceldata[2].v, acceldata[3].v, acceldata[4].v, acceldata[5].v, 0, 0, 0}; SV_VERBOSE(300, "%s %s %7.6f %7.6f %2u", survive_colorize(obj->codename), survive_colorize("IMU"), survive_run_time(ctx), timecode / 48000000., code); // assert(timecode <= obj->timebase_hz); SURVIVE_INVOKE_HOOK_SO(raw_imu, obj, 3, agm, timecode, code); SurviveSensorActivations_register_runtime(&obj->activations, obj->activations.last_imu, time_received_us); } } // DONE OK. break; } case USB_IF_WATCHMAN1: case USB_IF_WATCHMAN2: { SurviveObject *w = obj; if (id == VIVE_REPORT_RF_WATCHMAN) { survive_handle_watchman(w, time_received_us, readdata); } else if (id == VIVE_REPORT_RF_WATCHMANx2) { survive_handle_watchman(w, time_received_us, readdata); survive_handle_watchman(w, time_received_us, readdata + 29); } else if (id == VIVE_REPORT_RF_TURN_OFF) { w->ison = 0; // turning off } else if (id != 0) { SV_WARN("Unknown watchman code %d", id); } break; } case USB_IF_HMD_LIGHTCAP: case USB_IF_TRACKER1_LIGHTCAP: { bool dump_binary = false; if (id == VIVE_REPORT_USB_LIGHTCAP_REPORT_V1) { // LHv1 int i; for (i = 0; i < 9 && readdata < enddata; i++) { assert((enddata - readdata) >= 7); LightcapElement le; le.sensor_id = POP1; le.length = POP2; le.timestamp = POP4; if (le.sensor_id > 0xfd) continue; SV_VERBOSE(300, "%s %s %7.6f %7.6f %2u %2u %5u %08x %4d", survive_colorize(obj->codename), survive_colorize("LIGHTCAP"), survive_run_time(ctx), le.timestamp / 48000000., id, le.sensor_id, le.length, le.timestamp, (int)(si->buffer + size - readdata)); if (obj->ctx->lh_version != 1) { bool success = handle_lightcap(obj, &le); if (!success) dump_binary = 1; } if (dump_binary) { SV_VERBOSE(100, "%s sensor: %2d time: %3.5f length: %4d end_time: %8u", obj->codename, le.sensor_id, le.timestamp / 48000000., le.length, le.length + le.timestamp); } } } else if (id == VIVE_REPORT_USB_LIGHTCAP_REPORT_V2) { // LHv2 if (obj->ctx->lh_version == 0) { return; } survive_notify_gen2(obj, "Report id 39"); // Implies that the user forced gen1 if (obj->ctx->lh_version != 1) { // Shouldn't see this if the user said to use gen1 -- dump the output. static bool force_gen_warning = false; if (!force_gen_warning) { SV_WARN("LH Gen is %d, dumping data", obj->ctx->lh_version); force_gen_warning = true; } dump_binary = true; } else { vive_switch_mode(obj->driver, LightcapMode_raw1); } if (dump_binary) { #pragma pack(push, 1) struct lh2_entry { uint8_t code; // sensor with some bit flag. Continuation flag? uint32_t time; uint32_t data; uint32_t mask; }; #pragma pack(pop) uint32_t samples[4] = {0}, masks[4] = {0}, times[4] = {0}; struct lh2_entry *entries = (struct lh2_entry *)readdata; static uint32_t last_time = 0; for (int i = 0; i < 4; i++) { struct lh2_entry *entry = &entries[i]; if (entry->code == 0xff) break; fprintf(stderr, "sensor: %2u flag: %u time: %8u %3.5f (%7u) ", entry->code & 0x7fu, (entry->code & 0x80u) > 0, entry->time, entry->time / 48000000., entry->time - last_time); for (int k = 0; k < 32; k++) { uint8_t idx = 32 - k - 1; bool d = ((entry->data >> (idx)) & 1u); bool m = ((entry->mask >> (idx)) & 1u); if (m) fprintf(stderr, "%d", d); else fprintf(stderr, "_"); } samples[i] = entry->data; masks[i] = entry->mask; times[i] = entry->time; last_time = entry->time; fprintf(stderr, "\n"); } uint32_t time_since_sync[4] = {0}; survive_channel chan = survive_decipher_channel(samples, masks, times, time_since_sync, 4); fprintf(stderr, "Chan ootx: %d %d\n", chan / 2, chan & 1); for (int i = 0; i < 4; i++) { fprintf(stderr, "time since sync: %8u time of last sync: %8u\n", time_since_sync[i], entries[i].time - time_since_sync[i] * 8); } for (int i = 59 - 7; i < 59; i++) { fprintf(stderr, "%02x ", readdata[i]); } fprintf(stderr, "\n"); } } else if (id == VIVE_REPORT_USB_LIGHTCAP_REPORT_RAW_MODE_1) { survive_notify_gen2(obj, "Report ID 40"); uint8_t *packet = readdata + 1; uint8_t length = readdata[0]; parse_and_process_raw1_lightcap(obj, 0, packet, length); } else if (id == VIVE_REPORT_USB_TRACKER_LIGHTCAP_V1) { SV_INFO("USB lightcap report is of an unexpected type for %s: %d (0x%02x)", obj->codename, id, id); } else { SV_ERROR(SURVIVE_ERROR_HARWARE_FAULT, "USB lightcap report is of an unknown type for %s: %d (0x%02x)", obj->codename, id, id); } break; } case USB_IF_W_WATCHMAN1_LIGHTCAP: case USB_IF_TRACKER0_LIGHTCAP: { if (id == VIVE_REPORT_USB_TRACKER_LIGHTCAP_V1) { int i = 0; for (i = 0; i < 7; i++) { uint16_t *sensorId = (uint16_t *)readdata; uint16_t *length = (uint16_t *)(&(readdata[2])); unsigned long *time = (unsigned long *)(&(readdata[4])); LightcapElement le; le.sensor_id = (uint8_t)POP2; le.length = POP2; le.timestamp = POP4; if (le.sensor_id > 0xfd) continue; // handle_lightcap(obj, &le); } } else { SV_WARN("Unrecognized report id: 0x%02x for %s", id, obj->codename); } break; } case USB_IF_TRACKER0_BUTTONS: case USB_IF_TRACKER1_BUTTONS: case USB_IF_W_WATCHMAN1_BUTTONS: { if (VIVE_REPORT_IMU_SCALES == id) { // 0x00 uint8 1 reportID HID report identifier(= 1) // 0x02 uint16 2 reportType(? ) 0x0B04: Ping(every second) / 0x3C01 : User input // 0x04 uint32 4 reportCount Counter that increases with every report // 0x08 uint32 4 pressedButtons Bit field, see below for individual buttons // 0x0C uint16 2 triggerOrBattery Analog trigger value(user input) / Battery voltage ? (ping) // 0x0E uint8 1 batteryCharge Bit 7 : Charging / Bit 6..0 : Battery charge in percent // 0x10 uint32 4 hardwareID Hardware ID(user input) / 0x00000000 (ping) // 0x14 int16 2 touchpadHorizontal Horizontal thumb position(Left : -32768 / Right : 32767) // 0x16 int16 2 touchpadVertical Vertical thumb position(Bottom : -32768 / Top : 32767) // 0x18 ? 2 ? unknown // 0x1A uint16 2 triggerHighRes Analog trigger value with higher resolution // 0x1C ? 24 ? unknown // 0x34 uint16 2 triggerRawMaybe Analog trigger value, maybe raw sensor data // 0x36 ? 8 ? unknown // 0x3E uint8 1 someBitFieldMaybe 0x00 : ping / 0x64 : user input // 0x3F ? 1 ? unknown // typedef struct //{ // //uint8_t reportId; // uint16_t reportType; // uint32_t reportCount; // uint32_t pressedButtons; // uint16_t triggerOrBattery; // uint8_t batteryCharge; // uint32_t hardwareId; // int16_t touchpadHorizontal; // int16_t touchpadVertical; // uint16_t unknown1; // uint16_t triggerHighRes; // uint8_t unknown2; // uint8_t unknown3; // uint8_t unknown4; // uint16_t triggerRaw; // uint8_t unknown5; // uint8_t unknown6; // maybe some bitfield? // uint8_t unknown7; //} usb_buttons_raw; // usb_buttons_raw *raw = (usb_buttons_raw*) readdata; uint16_t reportType = read_buffer16(readdata, 0); switch(reportType) { case 0x0400: { // Very infrequent; doesnt seem periodic. // 00 04 0b 68 00 00 00 00 00 00 00 3e 0f 93 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 | ...h .... ...> .... .... .... .... .... // 00 04 0b 17 00 00 00 00 00 00 00 3e 0f 93 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 | .... .... ...> .... .... .... .... .... uint8_t second_cnt = readdata[3]; uint8_t battery_info = readdata[13]; handle_battery(obj, battery_info); break; } case 0x0700: { // Not sure what this is exacty; comes in at ~20hz as an incrementing counter. uint8_t unsure = readdata[2]; // Was always 0x3c? uint8_t timer = readdata[3]; SV_VERBOSE(2000, "%f 0x700 Report %2x %2x", OGRelativeTime(), unsure, timer); break; } case 0x0800: { // HMD Spews this at a frequent interval // [ IPD][prox] [F] // 00 08 16 00 00 00 00 00 00 00 00 45 2b 3d 00 00 00 00 00 00 01 00 00 96 01 00 00 00 00 00 // 00 00 uint16_t IPD = readdata[11] | (readdata[12] << 8u); int16_t proximity = readdata[13] | (readdata[14] << 8u); uint8_t onFace = readdata[19]; buttonEvent evt = {0}; evt.rawAxisCnt = 2; evt.rawAxis[0] = IPD / 65536.; evt.rawAxis[1] = proximity / 32768.; evt.pressedButtonsValid = 1; evt.pressedButtons = onFace; registerButtonEvent(obj, &evt, BUTTON_EVENT_SOURCE_DEFAULT); break; } case 0x100: { buttonEvent bEvent; memset(&bEvent, 0, sizeof(bEvent)); bEvent.pressedButtonsValid = 1; bEvent.pressedButtons = read_buffer32(readdata, 0x7); bEvent.triggerHighResValid = 1; bEvent.triggerHighRes = read_buffer16(readdata, 0x19) / 32768.; bEvent.touchpadHorizontalValid = 1; bEvent.touchpadVerticalValid = 1; bEvent.touchpadHorizontal = (int16_t)read_buffer16(readdata, 0x13) / 32768.; bEvent.touchpadVertical = (int16_t)read_buffer16(readdata, 0x15) / 32768.; registerButtonEvent(obj, &bEvent, BUTTON_EVENT_SOURCE_DEFAULT); } default: { survive_dump_buffer(ctx, readdata, size); } } } else { SV_WARN("Unknown report id 0x%02x for %s", id, obj->codename); } } break; default: { SV_WARN("Unknown interface %d for %s", si->which_interface_am_i, obj->codename); } } } /////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// int survive_vive_close(SurviveContext *ctx, void *driver) { SurviveViveData *sv = driver; #ifndef HIDAPI libusb_hotplug_deregister_callback(sv->usbctx, sv->callback_handle); #endif for (int i = 0; i < sv->udev_cnt; i++) { survive_close_usb_device(sv->udev[i]); } while (sv->udev_cnt) { #ifndef HIDAPI survive_release_ctx_lock(ctx); libusb_handle_events(sv->usbctx); survive_get_ctx_lock(ctx); #endif for (int i = 0; i < sv->udev_cnt; i++) { struct SurviveUSBInfo *usbInfo = sv->udev[i]; if (survive_handle_close_request_flag(usbInfo)) { i--; } } } survive_vive_usb_close(sv); free(sv); return 0; } int DriverRegHTCVive(SurviveContext *ctx) { SurviveViveData *sv = SV_CALLOC(sizeof(SurviveViveData)); survive_attach_configi(ctx, SECONDS_PER_HZ_OUTPUT_TAG, &sv->seconds_per_hz_output); sv->requestPairing = survive_configi(ctx, PAIR_DEVICE_TAG, SC_GET, 0); if(sv->seconds_per_hz_output > 0) { SV_INFO("Reporting usb hz in %d second intervals", sv->seconds_per_hz_output); } sv->ctx = ctx; // USB must happen last. if (survive_usb_init(sv)) { // TODO: Cleanup any libUSB stuff sitting around. SV_WARN("USB Init failed"); goto fail_gracefully; } DeviceDriver dd = (DeviceDriver)GetDriver("DriverRegGatt"); if (dd && (survive_config_is_set(ctx, "gatt") == 0 || survive_configi(ctx, "gatt", SC_GET, 0) == 1)) { SurviveDeviceDriverReturn r = dd(ctx); if (r < 0) { SV_WARN("GATT could not start error %d", r); } } bool hasHotplug = true; #ifdef HIDAPI hasHotplug = false; #endif if (sv->udev_cnt || hasHotplug) { survive_add_driver(ctx, sv, survive_vive_usb_poll, survive_vive_close); } else { SV_INFO("No USB devices detected"); goto fail_gracefully; } #ifdef HIDAPI /* // Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if (sv->udev[USB_DEV_HMD_IMU_LHB] && AttachInterface(sv, hmd, USB_IF_HMD_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LHB], 0x82, survive_data_cb, "Lightcap")) { return -12; } // This is a HACK! But it works. Need to investigate further sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; sv->uiface[USB_DEV_TRACKER1_LIGHTCAP].actual_len = 64; */ #endif // Note: don't sleep for HTCVive, the handle_events call can block ctx->poll_min_time_ms = 0; return 0; fail_gracefully: survive_vive_usb_close(sv); free(sv); return -1; } REGISTER_LINKTIME(DriverRegHTCVive)