mirror of
https://github.com/collabora/libsurvive.git
synced 2026-08-04 00:51:21 -04:00
317 lines
11 KiB
C
317 lines
11 KiB
C
|
|
static int survive_config_submit(struct SurviveUSBInfo *usbInfo) {
|
|
struct survive_config_packet *config_packet = usbInfo->cfg_user;
|
|
SurviveContext *ctx = config_packet->sv->ctx;
|
|
|
|
SV_VERBOSE(110, "Submitting config for %s %s at %f",
|
|
survive_colorize(usbInfo->so ? usbInfo->so->codename : usbInfo->device_info->codename),
|
|
survive_colorize(usbInfo->device_info->name), usbInfo->nextCfgSubmitTime);
|
|
usbInfo->nextCfgSubmitTime = -1;
|
|
|
|
int rc = survive_usb_transfer_submit(config_packet->tx);
|
|
if (rc) {
|
|
SV_WARN("Failed to submit transfer %s %s (%d)", survive_colorize_codename(usbInfo->so),
|
|
survive_colorize(usbInfo->device_info->name), rc);
|
|
return -6;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static inline void setup_config_req(struct survive_config_packet *config_packet) {
|
|
config_packet->state = SURVIVE_CONFIG_STATE_CONFIG;
|
|
survive_usb_setup_get_feature_report(config_packet->tx, VIVE_REPORT_CONFIG_READMODE);
|
|
}
|
|
static inline void setup_version(struct survive_config_packet *packet) {
|
|
packet->state = SURVIVE_CONFIG_STATE_VERSION;
|
|
survive_usb_setup_get_feature_report(packet->tx, VIVE_REPORT_VERSION);
|
|
}
|
|
static inline void setup_config_imu_scales(struct survive_config_packet *packet) {
|
|
packet->state = SURVIVE_CONFIG_STATE_IMU_SCALES;
|
|
survive_usb_setup_get_feature_report(packet->tx, VIVE_REPORT_IMU_SCALES);
|
|
}
|
|
|
|
static inline void setup_magic(struct survive_config_packet *packet) {
|
|
SurviveContext *ctx = packet->ctx;
|
|
SurviveObject *so = packet->usbInfo->so;
|
|
|
|
packet->state = SURVIVE_CONFIG_STATE_MAGICS;
|
|
|
|
SV_VERBOSE(100, "Submitting magic %s at %f sec for %s - %s (length %d)",
|
|
survive_colorize(packet->current_magic->name), survive_run_time(ctx) - packet->start_time,
|
|
survive_colorize(so ? so->codename : "BRD"), survive_colorize(packet->usbInfo->device_info->name),
|
|
(int)packet->current_magic->length);
|
|
survive_usb_setup_update_feature_report(packet->tx, packet->current_magic->magic, packet->current_magic->length);
|
|
do {
|
|
packet->current_magic++;
|
|
} while (packet->current_magic->magic && packet->current_magic->code == 0);
|
|
}
|
|
|
|
void handle_config_tx(survive_usb_transfer_t *transfer);
|
|
static inline void setup_packet_state(struct survive_config_packet *packet) {
|
|
switch (packet->state) {
|
|
case SURVIVE_CONFIG_STATE_MAGICS:
|
|
setup_magic(packet);
|
|
break;
|
|
case SURVIVE_CONFIG_STATE_VERSION:
|
|
setup_version(packet);
|
|
break;
|
|
case SURVIVE_CONFIG_STATE_CONFIG:
|
|
setup_config_req(packet);
|
|
break;
|
|
case SURVIVE_CONFIG_STATE_IMU_SCALES:
|
|
setup_config_imu_scales(packet);
|
|
return;
|
|
case SURVIVE_CONFIG_STATE_DONE:
|
|
return;
|
|
}
|
|
survive_usb_setup_control(packet->tx, packet->usbInfo, handle_config_tx, packet, 1000);
|
|
}
|
|
|
|
void survive_usb_feature_read(SurviveObject *so, const uint8_t *data, size_t length) {
|
|
SurviveContext *ctx = so->ctx;
|
|
switch (data[0]) {
|
|
case VIVE_REPORT_CHANGE_MODE:
|
|
SV_VERBOSE(100, "%s new mode %d", survive_colorize_codename(so), data[1]);
|
|
break;
|
|
case VIVE_REPORT_IMU_SCALES: {
|
|
int gyro_scale_mode = data[1];
|
|
int acc_scale_mode = data[2];
|
|
survive_default_set_imu_scale_modes(so, gyro_scale_mode, acc_scale_mode);
|
|
break;
|
|
}
|
|
case VIVE_REPORT_VERSION:
|
|
parse_tracker_version_info(so, data + 1, length - 1);
|
|
break;
|
|
case VIVE_REPORT_CONFIG_READMODE:
|
|
case VIVE_REPORT_CONFIG_READ:
|
|
break;
|
|
default:
|
|
SV_VERBOSE(10, "============== Unknown feature %s read %x %d", survive_colorize_codename(so), data[0],
|
|
(int)length);
|
|
break;
|
|
}
|
|
}
|
|
void handle_config_tx(survive_usb_transfer_t *transfer) {
|
|
struct survive_config_packet *packet = transfer->user_data;
|
|
SurviveContext *ctx = packet->ctx;
|
|
const uint8_t *buffer = survive_usb_transfer_data(transfer);
|
|
SurviveObject *so = packet->usbInfo->so;
|
|
uint8_t cmd = buffer[0];
|
|
struct SurviveUSBInfo *usbInfo = packet->usbInfo;
|
|
|
|
if (packet->usbInfo->interfaces[0].shutdown) {
|
|
goto cleanup;
|
|
}
|
|
|
|
if (transfer->status == LIBUSB_TRANSFER_STALL || transfer->status == LIBUSB_TRANSFER_TIMED_OUT) {
|
|
SV_VERBOSE(110, "Waiting, Transfer status %d at %f sec for %s (%d)", transfer->status,
|
|
survive_run_time(ctx) - packet->start_time, survive_colorize(so ? so->codename : "unknown"),
|
|
packet->usbInfo->request_close);
|
|
packet->stall_counter++;
|
|
if (packet->usbInfo->device_info->codename[0] == 0) {
|
|
goto cleanup;
|
|
}
|
|
|
|
packet->usbInfo->nextCfgSubmitTime = survive_run_time(ctx) + .02;
|
|
if (packet->stall_counter > 5) {
|
|
packet->stall_counter = 0;
|
|
packet->usbInfo->nextCfgSubmitTime++;
|
|
}
|
|
return;
|
|
}
|
|
packet->stall_counter = 0;
|
|
|
|
if (transfer->status != LIBUSB_TRANSFER_COMPLETED) {
|
|
SV_WARN("Transfer status %d at %f sec for %s", transfer->status, survive_run_time(ctx) - packet->start_time,
|
|
survive_colorize(so ? so->codename : "unknown"));
|
|
goto cleanup;
|
|
}
|
|
|
|
if (so == 0) {
|
|
if (usbInfo->device_info->codename[0] != 0) {
|
|
so = survive_create_device(ctx, "HTC", usbInfo, usbInfo->device_info->codename, survive_vive_send_haptic);
|
|
survive_add_object(ctx, so);
|
|
usbInfo->so = so;
|
|
usbInfo->ownsObject = true;
|
|
}
|
|
}
|
|
|
|
size_t tx_length = survive_usb_transfer_length(transfer);
|
|
|
|
switch (packet->state) {
|
|
case SURVIVE_CONFIG_STATE_MAGICS:
|
|
if (tx_length < packet->current_magic->length) {
|
|
SV_WARN("Magic for %s was not acked; length %d vs %d", so ? survive_colorize(so->codename) : "unknown",
|
|
(int)tx_length, (int)packet->current_magic->length);
|
|
goto resubmit;
|
|
}
|
|
|
|
SV_VERBOSE(110, "Magic inc in %f sec for %s %s %d", survive_run_time(ctx) - packet->start_time,
|
|
so ? survive_colorize(so->codename) : "unknown",
|
|
survive_colorize(packet->usbInfo->device_info->name), (int)tx_length)
|
|
|
|
if (!packet->current_magic->magic) {
|
|
SV_VERBOSE(100, "Magics done in %f sec for %s %s", survive_run_time(ctx) - packet->start_time,
|
|
so ? survive_colorize(so->codename) : "unknown",
|
|
survive_colorize(packet->usbInfo->device_info->name));
|
|
packet->usbInfo->lightcapMode = LightcapMode_raw0;
|
|
if (packet->usbInfo->device_info->codename[0] == 0) {
|
|
packet->state = SURVIVE_CONFIG_STATE_DONE;
|
|
goto cleanup;
|
|
}
|
|
|
|
goto setup_next;
|
|
}
|
|
|
|
setup_magic(packet);
|
|
goto resubmit;
|
|
case SURVIVE_CONFIG_STATE_CONFIG:
|
|
switch (cmd) {
|
|
case VIVE_REPORT_CONFIG_READMODE:
|
|
survive_usb_setup_get_feature_report(transfer, VIVE_REPORT_CONFIG_READ);
|
|
packet->expected_cfg_length = (buffer[1] << 8 | buffer[2]);
|
|
SV_VERBOSE(100, "Config readmode in %f sec for %s expected length: %d",
|
|
survive_run_time(ctx) - packet->start_time, survive_colorize(so->codename),
|
|
packet->expected_cfg_length);
|
|
str_ensure_size(&packet->cfg, packet->expected_cfg_length);
|
|
goto resubmit;
|
|
case VIVE_REPORT_CONFIG_READ: {
|
|
SV_VERBOSE(110, "Config response in %f sec for %s %d %d", survive_run_time(ctx) - packet->start_time,
|
|
survive_colorize(so->codename), buffer[0], buffer[1]);
|
|
|
|
size_t size = buffer[1];
|
|
str_append_n(&packet->cfg, (const char *)&buffer[2], size);
|
|
if (size == 0) {
|
|
// NOTE: Sometimes the actual length is 2 bytes longer than advertised; not sure why
|
|
if (packet->expected_cfg_length > packet->cfg.length) {
|
|
SV_WARN("Config request failed; trying again %f %s (%d > %d)",
|
|
survive_run_time(ctx) - packet->start_time, survive_colorize(so->codename),
|
|
packet->expected_cfg_length, (int)packet->cfg.length);
|
|
str_clear(&packet->cfg);
|
|
setup_config_req(packet);
|
|
goto resubmit;
|
|
}
|
|
|
|
SV_VERBOSE(100, "Config done in %f sec for %s, len %ld", survive_run_time(ctx) - packet->start_time,
|
|
survive_colorize(so->codename), packet->cfg.length);
|
|
|
|
uint8_t uncompressed_data[65536];
|
|
int uncompressed_data_len = survive_simple_inflate(ctx, (uint8_t *)packet->cfg.d, packet->cfg.length,
|
|
uncompressed_data, sizeof(uncompressed_data) - 1);
|
|
if (uncompressed_data_len < 0) {
|
|
SV_ERROR(SURVIVE_ERROR_INVALID_CONFIG, "Invalid config for %s", survive_colorize(so->codename));
|
|
goto cleanup;
|
|
}
|
|
packet->usbInfo->so->conf = SV_CALLOC(uncompressed_data_len + 1);
|
|
packet->usbInfo->so->conf_cnt = uncompressed_data_len;
|
|
memcpy(packet->usbInfo->so->conf, uncompressed_data, uncompressed_data_len);
|
|
|
|
str_free(&packet->cfg);
|
|
goto setup_next;
|
|
}
|
|
goto resubmit;
|
|
}
|
|
default:
|
|
SV_ERROR(SURVIVE_ERROR_GENERAL, "Unknown config command 0x%x", cmd);
|
|
}
|
|
break;
|
|
case SURVIVE_CONFIG_STATE_VERSION: {
|
|
if (tx_length == 0)
|
|
goto resubmit;
|
|
|
|
parse_tracker_version_info(packet->usbInfo->so, &buffer[1], tx_length);
|
|
SURVIVE_INVOKE_HOOK_SO(config, so, so->conf, so->conf_cnt);
|
|
SV_VERBOSE(100, "Version done in %f sec for %s", survive_run_time(ctx) - packet->start_time,
|
|
survive_colorize(so->codename));
|
|
|
|
goto setup_next;
|
|
}
|
|
case SURVIVE_CONFIG_STATE_IMU_SCALES: {
|
|
int gyro_scale_mode = buffer[1];
|
|
int acc_scale_mode = buffer[2];
|
|
survive_default_set_imu_scale_modes(so, gyro_scale_mode, acc_scale_mode);
|
|
goto setup_next;
|
|
}
|
|
default:
|
|
SV_WARN("Config state machine saw packet of type %d at state %d; not sure how to proceed.", cmd, packet->state);
|
|
goto cleanup;
|
|
}
|
|
return;
|
|
|
|
setup_next : {
|
|
packet->state++;
|
|
setup_packet_state(packet);
|
|
if (packet->state == SURVIVE_CONFIG_STATE_DONE)
|
|
goto cleanup;
|
|
else
|
|
goto resubmit;
|
|
};
|
|
resubmit : {
|
|
SV_VERBOSE(110, "Resubmit startup packet for %s %s at %f", survive_colorize(packet->usbInfo->so->codename),
|
|
survive_colorize(packet->usbInfo->device_info->name), packet->usbInfo->nextCfgSubmitTime);
|
|
int submit_transfer_error = survive_config_submit(packet->usbInfo);
|
|
if (submit_transfer_error != 0) {
|
|
SV_WARN("Config state machine could not submit transfer %d\n", submit_transfer_error);
|
|
goto cleanup;
|
|
}
|
|
return;
|
|
}
|
|
cleanup:
|
|
SV_VERBOSE(100, "Cleanup config for %s %s at %f %d/%d", survive_colorize_codename(packet->usbInfo->so),
|
|
survive_colorize(packet->usbInfo->device_info->name), survive_run_time(ctx),
|
|
usbInfo->interfaces[0].shutdown, (int)usbInfo->active_transfers);
|
|
|
|
if (!usbInfo->interfaces[0].shutdown) {
|
|
for (const struct Endpoint_t *endpoint = packet->usbInfo->device_info->endpoints; endpoint->name; endpoint++) {
|
|
int errorCode =
|
|
AttachInterface(packet->sv, packet->usbInfo, endpoint, packet->usbInfo->handle, survive_data_cb);
|
|
if (errorCode < 0) {
|
|
SV_WARN("Could not attach interface %s: %d", endpoint->name, errorCode);
|
|
}
|
|
}
|
|
}
|
|
|
|
packet->usbInfo->ignoreCnt = 10;
|
|
packet->usbInfo->nextCfgSubmitTime = -1;
|
|
packet->usbInfo->cfg_user = 0;
|
|
packet->usbInfo->active_transfers--;
|
|
|
|
if (usbInfo->interfaces[0].shutdown && usbInfo->active_transfers == 0) {
|
|
usbInfo->request_close = true;
|
|
SV_VERBOSE(100, "Acking close for %s", survive_colorize_codename(usbInfo->so));
|
|
}
|
|
|
|
survive_usb_transfer_free(transfer);
|
|
str_free(&packet->cfg);
|
|
free(packet);
|
|
}
|
|
|
|
static void survive_config_poll(struct SurviveUSBInfo *usbInfo) {
|
|
FLT now = survive_run_time(usbInfo->viveData->ctx);
|
|
bool active = true;
|
|
while (active || usbInfo->nextCfgSubmitTime >= 0 && usbInfo->nextCfgSubmitTime < now) {
|
|
active = false;
|
|
if (usbInfo->nextCfgSubmitTime >= 0 && usbInfo->nextCfgSubmitTime < now) {
|
|
if (survive_config_submit(usbInfo) == -6) {
|
|
usbInfo->active_transfers--;
|
|
survive_close_usb_device(usbInfo);
|
|
}
|
|
}
|
|
#ifdef HIDAPI
|
|
struct survive_config_packet *config_packet = usbInfo->cfg_user;
|
|
if (config_packet == 0)
|
|
return;
|
|
|
|
void (*cb)(struct survive_usb_transfer_t *) = config_packet->tx->cb;
|
|
if (!config_packet->tx->processed && cb) {
|
|
config_packet->tx->processed = true;
|
|
cb(config_packet->tx);
|
|
}
|
|
#else
|
|
#endif
|
|
}
|
|
}
|
|
|
|
static void survive_config_cancel(struct survive_config_packet *config_packet) {
|
|
survive_usb_transfer_cancel(config_packet->tx);
|
|
}
|