diff --git a/dev_evdev.c b/dev_evdev.c index cf499a0..bf2e3f3 100644 --- a/dev_evdev.c +++ b/dev_evdev.c @@ -5,12 +5,76 @@ static const char *input_path = "/dev/input/"; static pthread_mutex_t input_acquire_mutex = PTHREAD_MUTEX_INITIALIZER; -static int open_fds[] = { - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, +struct open_resource { + char* sysfs_path; + int fd; +}; + +static struct open_resource open_paths[] = { + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, + { + .fd = -1, + .sysfs_path = NULL + }, }; static bool ev_matches( @@ -43,9 +107,13 @@ void dev_evdev_close(struct libevdev* out_evdev) { } int fd = libevdev_get_fd(out_evdev); - for (int i = 0; i < sizeof(open_fds) / sizeof(int); ++i) { - if (open_fds[i] == fd) { - open_fds[i] = -1; + for (int i = 0; i < sizeof(open_paths) / sizeof(open_paths[0]); ++i) { + if (open_paths[i].fd == fd) { + if (open_paths[i].sysfs_path != NULL) { + free(open_paths[i].sysfs_path); + open_paths[i].sysfs_path = NULL; + } + open_paths[i].fd = -1; } } @@ -69,10 +137,14 @@ int dev_evdev_open( if (mutex_lock_res != 0) { fprintf(stderr, "Cannot lock input mutex: %d\n", mutex_lock_res); res = mutex_lock_res; - goto dev_evdev_open_err; + goto dev_evdev_open_mutex_err; } - char path[MAX_PATH_LEN] = "\0"; + char *const path = malloc(MAX_PATH_LEN); + if (path == NULL) { + res = -ENOMEM; + goto dev_evdev_open_err; + } DIR *d; struct dirent *dir; @@ -101,25 +173,23 @@ int dev_evdev_open( // check if that has been already opened // open_sysfs int skip = 0; - for (int o = 0; o < (sizeof(open_fds) / sizeof(open_fds[0])); ++o) { - if ((open_fds[o] != -1) && (open_fds[o] == fd)) { + for (int o = 0; o < (sizeof(open_paths) / sizeof(open_paths[0])); ++o) { + if ((open_paths[o].fd != -1) && (open_paths[o].sysfs_path != NULL) && (strcmp(open_paths[o].sysfs_path, path) == 0)) { close(fd); skip = 1; break; - } else if (open_fds[o] == -1) { + } else if ((open_paths[o].sysfs_path == NULL) && (open_paths[o].fd == -1)) { open_sysfs_idx = o; } } if ((skip) || (open_sysfs_idx == -1)) { + free(path); continue; - } else { - open_fds[open_sysfs_idx] = fd; } if (libevdev_new_from_fd(fd, out_evdev) != 0) { - //fprintf(stderr, "Cannot initialize libevdev from this device (%s) -- Skipping.\n", path); - open_fds[open_sysfs_idx] = -1; + free(path); close(fd); continue; } @@ -127,11 +197,15 @@ int dev_evdev_open( // try to open the device if (!ev_matches(in_filters, *out_evdev)) { libevdev_free(*out_evdev); - open_fds[open_sysfs_idx] = -1; + free(path); close(fd); continue; } + // register the device as being opened already + open_paths[open_sysfs_idx].sysfs_path = path; + open_paths[open_sysfs_idx].fd = fd; + // the device has been found res = 0; break; @@ -140,7 +214,12 @@ int dev_evdev_open( } dev_evdev_open_err: + if ((path != NULL) && (res != 0)) { + free(path); + } + pthread_mutex_unlock(&input_acquire_mutex); +dev_evdev_open_mutex_err: return res; } diff --git a/virt_ds4.c b/virt_ds4.c index b177d33..b9cfb53 100644 --- a/virt_ds4.c +++ b/virt_ds4.c @@ -370,150 +370,15 @@ static ds4_dpad_status_t ds4_dpad_from_gamepad(uint8_t dpad) { return DPAD_RELEASED; } -/** - * This function arranges HID packets as described on https://www.psdevwiki.com/ps4/DS4-USB - */ -static void compose_hid_report_buffer(int fd, gamepad_status_t *const gamepad_stats, uint8_t buf[64]) { - gamepad_status_t gs = *gamepad_stats; - - const int64_t time_us = gs.last_gyro_motion_time.tv_sec * 1000000 + gs.last_gyro_motion_time.tv_usec; - - static uint32_t empty_reports = 0; - static uint64_t last_time = 0; - const int delta_time = time_us - last_time; - last_time = time_us; - - // find the average Δt in the last 30 non-zero inputs; - // this has to run thousands of times a second so i'm trying to do this as fast as possible - static uint32_t dt_buffer[30] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - static uint32_t dt_sum = 0; - static uint8_t dt_buffer_current = 0; - - if (delta_time == 0) { - empty_reports++; - } else if (delta_time < 1000000 && delta_time > 0 ) { // ignore outliers - dt_sum -= dt_buffer[dt_buffer_current]; - dt_sum += delta_time; - dt_buffer[dt_buffer_current] = delta_time; - - if (dt_buffer_current == 29) { - dt_buffer_current = 0; - } else { - dt_buffer_current++; - } - } - - static uint64_t sim_time = 0; - const double correction_factor = DS4_SPEC_DELTA_TIME / ((double)dt_sum / 30.f); - if (delta_time != 0) { - sim_time += (int)((double)delta_time * correction_factor); - } - - const uint16_t timestamp = sim_time + (int)((double)empty_reports * DS4_SPEC_DELTA_TIME); - - /* - Example data: - 0000 c0 e3 af 02 ac 9c ff ff 43 01 84 08 05 00 2d 00 - 0010 93 7b 56 65 00 00 00 00 c2 aa 03 00 00 00 00 00 - 0020 40 00 00 00 40 00 00 00 00 00 00 00 00 00 00 00 - 0030 04 00 00 00 00 00 00 00 04 02 00 00 00 00 00 00 - // above is useless, just send what is below. - 0040 01 80 7f 80 7e 08 00 5c 00 00 7e d4 06 3b fe 0c - 0050 ff 8c fe 6a 05 4f 15 56 e8 00 00 00 00 00 1b 00 - 0060 00 00 00 80 00 00 00 80 00 00 00 00 80 00 00 00 - 0070 80 00 00 00 00 80 00 00 00 80 00 00 00 00 80 00 - */ - - // see https://www.psdevwiki.com/ps4/DS4-USB - - // buf[12] battery level - - /* - * kernel will do: - * int calib_data = mult_frac(ds4->gyro_calib_data[i].sens_numer, raw_data, ds4->gyro_calib_data[i].sens_denom); - * input_report_abs(ds4->sensors, ds4->gyro_calib_data[i].abs_code, calib_data); - * - * as we know sens_numer is 0, hence calib_data is zero. - */ - /* - const int16_t g_x = ((gs.gyro[0]) * ((double)(180.0)/(double)(M_PI))) / (double)DS4_GYRO_RES_PER_DEG_S; - const int16_t g_y = ((gs.gyro[1]) * ((double)(180.0)/(double)(M_PI))) / (double)DS4_GYRO_RES_PER_DEG_S; - const int16_t g_z = ((gs.gyro[2]) * ((double)(180.0)/(double)(M_PI))) / (double)DS4_GYRO_RES_PER_DEG_S; - const int16_t a_x = ((gs.accel[0]) / ((double)9.8)) / (double)DS4_ACC_RES_PER_G; // TODO: IDK how to test... - const int16_t a_y = ((gs.accel[1]) / ((double)9.8)) / (double)DS4_ACC_RES_PER_G; // TODO: IDK how to test... - const int16_t a_z = ((gs.accel[2]) / ((double)9.8)) / (double)DS4_ACC_RES_PER_G; // TODO: IDK how to test... - */ - - /* - const int16_t g_x = (gs.gyro[0]) / LSB_PER_RAD_S_2000_DEG_S; - const int16_t g_y = (gs.gyro[1]) / LSB_PER_RAD_S_2000_DEG_S; - const int16_t g_z = (gs.gyro[2]) / LSB_PER_RAD_S_2000_DEG_S; - const int16_t a_x = (gs.accel[0]) / LSB_PER_16G; // TODO: IDK how to test... - const int16_t a_y = (gs.accel[1]) / LSB_PER_16G; // TODO: IDK how to test... - const int16_t a_z = (gs.accel[2]) / LSB_PER_16G; // TODO: IDK how to test... - */ - - const int16_t g_x = gs.raw_gyro[0]; - const int16_t g_y = (int16_t)(-1) * gs.raw_gyro[1]; // Swap Y and Z - const int16_t g_z = (int16_t)(-1) * gs.raw_gyro[2]; // Swap Y and Z - const int16_t a_x = gs.raw_accel[0]; - const int16_t a_y = (int16_t)(-1) * gs.raw_accel[1]; // Swap Y and Z - const int16_t a_z = (int16_t)(-1) * gs.raw_accel[2]; // Swap Y and Z - - buf[0] = 0x01; // [00] report ID (0x01) - buf[1] = ((uint64_t)((int64_t)gs.joystick_positions[0][0] + (int64_t)32768) >> (uint64_t)8); // L stick, X axis - buf[2] = ((uint64_t)((int64_t)gs.joystick_positions[0][1] + (int64_t)32768) >> (uint64_t)8); // L stick, Y axis - buf[3] = ((uint64_t)((int64_t)gs.joystick_positions[1][0] + (int64_t)32768) >> (uint64_t)8); // R stick, X axis - buf[4] = ((uint64_t)((int64_t)gs.joystick_positions[1][1] + (int64_t)32768) >> (uint64_t)8); // R stick, Y axis - buf[5] = - (gs.triangle ? 0x80 : 0x00) | - (gs.circle ? 0x40 : 0x00) | - (gs.cross ? 0x20 : 0x00) | - (gs.square ? 0x10 : 0x00) | - (uint8_t)ds4_dpad_from_gamepad(gs.dpad); - buf[6] = - (gs.r3 ? 0x80 : 0x00) | - (gs.l3 ? 0x40 : 0x00) | - (gs.share ? 0x20 : 0x00) | - (gs.option ? 0x10 : 0x00) | - (gs.r2_trigger > 200 ? 0x08 : 0x00) | - (gs.l2_trigger > 200 ? 0x04 : 0x00) | - (gs.r1 ? 0x02 : 0x00) | - (gs.l1 ? 0x01 : 0x00); - - /* - static uint8_t counter = 0; - buf[7] = (((counter++) % (uint8_t)64) << ((uint8_t)2)) | get_buttons_byte3_by_gs(&gs); - */ - - buf[7] = gs.center ? 0x01 : 0x00; - - buf[8] = gs.l2_trigger; - buf[9] = gs.r2_trigger; - memcpy(&buf[10], ×tamp, sizeof(timestamp)); - buf[12] = 0x20; // [12] battery level | this is called sensor_temparature in the kernel driver but is never used... - memcpy(&buf[13], &g_x, sizeof(int16_t)); - memcpy(&buf[15], &g_y, sizeof(int16_t)); - memcpy(&buf[17], &g_z, sizeof(int16_t)); - memcpy(&buf[19], &a_x, sizeof(int16_t)); - memcpy(&buf[21], &a_y, sizeof(int16_t)); - memcpy(&buf[23], &a_z, sizeof(int16_t)); - - buf[30] = 0x1b; // no headset attached - - buf[62] = 0x80; // IDK... it seems constant... - buf[57] = 0x80; // IDK... it seems constant... - buf[53] = 0x80; // IDK... it seems constant... - buf[48] = 0x80; // IDK... it seems constant... - buf[35] = 0x80; // IDK... it seems constant... - buf[44] = 0x80; // IDK... it seems constant... -} - int virt_dualshock_init(virt_dualshock_t *const out_gamepad) { int ret = 0; + out_gamepad->dt_sum = 0; + out_gamepad->dt_buffer_current = 0; + memset(out_gamepad->dt_buffer, 0, sizeof(out_gamepad->dt_buffer)); out_gamepad->debug = false; + out_gamepad->empty_reports = 0; + out_gamepad->last_time = 0; out_gamepad->fd = open(path, O_RDWR | O_CLOEXEC /* | O_NONBLOCK */); if (out_gamepad->fd < 0) { @@ -776,10 +641,135 @@ void virt_dualshock_close(virt_dualshock_t *const out_gamepad) { destroy(out_gamepad->fd); } +/** + * This function arranges HID packets as described on https://www.psdevwiki.com/ps4/DS4-USB + */ void virt_dualshock_compose(virt_dualshock_t *const gamepad, gamepad_status_t *const in_device_status, uint8_t *const out_buf) { - gamepad_status_qam_quirk(in_device_status); + const int64_t time_us = in_device_status->last_gyro_motion_time.tv_sec * 1000000 + in_device_status->last_gyro_motion_time.tv_usec; - compose_hid_report_buffer(gamepad->fd, in_device_status, out_buf); + const int delta_time = time_us - gamepad->last_time; + gamepad->last_time = time_us; + + // find the average Δt in the last 30 non-zero inputs; + // this has to run thousands of times a second so i'm trying to do this as fast as possible + if (delta_time == 0) { + gamepad->empty_reports++; + } else if (delta_time < 1000000 && delta_time > 0 ) { // ignore outliers + gamepad->dt_sum -= gamepad->dt_buffer[gamepad->dt_buffer_current]; + gamepad->dt_sum += delta_time; + gamepad->dt_buffer[gamepad->dt_buffer_current] = delta_time; + + if (gamepad->dt_buffer_current == 29) { + gamepad->dt_buffer_current = 0; + } else { + gamepad->dt_buffer_current++; + } + } + + static uint64_t sim_time = 0; + const double correction_factor = DS4_SPEC_DELTA_TIME / ((double)gamepad->dt_sum / 30.f); + if (delta_time != 0) { + sim_time += (int)((double)delta_time * correction_factor); + } + + const uint16_t timestamp = sim_time + (int)((double)gamepad->empty_reports * DS4_SPEC_DELTA_TIME); + + /* + Example data: + 0000 c0 e3 af 02 ac 9c ff ff 43 01 84 08 05 00 2d 00 + 0010 93 7b 56 65 00 00 00 00 c2 aa 03 00 00 00 00 00 + 0020 40 00 00 00 40 00 00 00 00 00 00 00 00 00 00 00 + 0030 04 00 00 00 00 00 00 00 04 02 00 00 00 00 00 00 + // above is useless, just send what is below. + 0040 01 80 7f 80 7e 08 00 5c 00 00 7e d4 06 3b fe 0c + 0050 ff 8c fe 6a 05 4f 15 56 e8 00 00 00 00 00 1b 00 + 0060 00 00 00 80 00 00 00 80 00 00 00 00 80 00 00 00 + 0070 80 00 00 00 00 80 00 00 00 80 00 00 00 00 80 00 + */ + + // see https://www.psdevwiki.com/ps4/DS4-USB + + // buf[12] battery level + + /* + * kernel will do: + * int calib_data = mult_frac(ds4->gyro_calib_data[i].sens_numer, raw_data, ds4->gyro_calib_data[i].sens_denom); + * input_report_abs(ds4->sensors, ds4->gyro_calib_data[i].abs_code, calib_data); + * + * as we know sens_numer is 0, hence calib_data is zero. + */ + /* + const int16_t g_x = ((in_device_status->gyro[0]) * ((double)(180.0)/(double)(M_PI))) / (double)DS4_GYRO_RES_PER_DEG_S; + const int16_t g_y = ((in_device_status->gyro[1]) * ((double)(180.0)/(double)(M_PI))) / (double)DS4_GYRO_RES_PER_DEG_S; + const int16_t g_z = ((in_device_status->gyro[2]) * ((double)(180.0)/(double)(M_PI))) / (double)DS4_GYRO_RES_PER_DEG_S; + const int16_t a_x = ((in_device_status->accel[0]) / ((double)9.8)) / (double)DS4_ACC_RES_PER_G; // TODO: IDK how to test... + const int16_t a_y = ((in_device_status->accel[1]) / ((double)9.8)) / (double)DS4_ACC_RES_PER_G; // TODO: IDK how to test... + const int16_t a_z = ((in_device_status->accel[2]) / ((double)9.8)) / (double)DS4_ACC_RES_PER_G; // TODO: IDK how to test... + */ + + /* + const int16_t g_x = (in_device_status->gyro[0]) / LSB_PER_RAD_S_2000_DEG_S; + const int16_t g_y = (in_device_status->gyro[1]) / LSB_PER_RAD_S_2000_DEG_S; + const int16_t g_z = (in_device_status->gyro[2]) / LSB_PER_RAD_S_2000_DEG_S; + const int16_t a_x = (in_device_status->accel[0]) / LSB_PER_16G; // TODO: IDK how to test... + const int16_t a_y = (in_device_status->accel[1]) / LSB_PER_16G; // TODO: IDK how to test... + const int16_t a_z = (in_device_status->accel[2]) / LSB_PER_16G; // TODO: IDK how to test... + */ + + const int16_t g_x = in_device_status->raw_gyro[0]; + const int16_t g_y = (int16_t)(-1) * in_device_status->raw_gyro[1]; // Swap Y and Z + const int16_t g_z = (int16_t)(-1) * in_device_status->raw_gyro[2]; // Swap Y and Z + const int16_t a_x = in_device_status->raw_accel[0]; + const int16_t a_y = (int16_t)(-1) * in_device_status->raw_accel[1]; // Swap Y and Z + const int16_t a_z = (int16_t)(-1) * in_device_status->raw_accel[2]; // Swap Y and Z + + out_buf[0] = 0x01; // [00] report ID (0x01) + out_buf[1] = ((uint64_t)((int64_t)in_device_status->joystick_positions[0][0] + (int64_t)32768) >> (uint64_t)8); // L stick, X axis + out_buf[2] = ((uint64_t)((int64_t)in_device_status->joystick_positions[0][1] + (int64_t)32768) >> (uint64_t)8); // L stick, Y axis + out_buf[3] = ((uint64_t)((int64_t)in_device_status->joystick_positions[1][0] + (int64_t)32768) >> (uint64_t)8); // R stick, X axis + out_buf[4] = ((uint64_t)((int64_t)in_device_status->joystick_positions[1][1] + (int64_t)32768) >> (uint64_t)8); // R stick, Y axis + out_buf[5] = + (in_device_status->triangle ? 0x80 : 0x00) | + (in_device_status->circle ? 0x40 : 0x00) | + (in_device_status->cross ? 0x20 : 0x00) | + (in_device_status->square ? 0x10 : 0x00) | + (uint8_t)ds4_dpad_from_gamepad(in_device_status->dpad); + out_buf[6] = + (in_device_status->r3 ? 0x80 : 0x00) | + (in_device_status->l3 ? 0x40 : 0x00) | + (in_device_status->share ? 0x20 : 0x00) | + (in_device_status->option ? 0x10 : 0x00) | + (in_device_status->r2_trigger > 200 ? 0x08 : 0x00) | + (in_device_status->l2_trigger > 200 ? 0x04 : 0x00) | + (in_device_status->r1 ? 0x02 : 0x00) | + (in_device_status->l1 ? 0x01 : 0x00); + + /* + static uint8_t counter = 0; + buf[7] = (((counter++) % (uint8_t)64) << ((uint8_t)2)) | get_buttons_byte3_by_gs(&gs); + */ + + out_buf[7] = in_device_status->center ? 0x01 : 0x00; + + out_buf[8] = in_device_status->l2_trigger; + out_buf[9] = in_device_status->r2_trigger; + memcpy(&out_buf[10], ×tamp, sizeof(timestamp)); + out_buf[12] = 0x20; // [12] battery level | this is called sensor_temparature in the kernel driver but is never used... + memcpy(&out_buf[13], &g_x, sizeof(int16_t)); + memcpy(&out_buf[15], &g_y, sizeof(int16_t)); + memcpy(&out_buf[17], &g_z, sizeof(int16_t)); + memcpy(&out_buf[19], &a_x, sizeof(int16_t)); + memcpy(&out_buf[21], &a_y, sizeof(int16_t)); + memcpy(&out_buf[23], &a_z, sizeof(int16_t)); + + out_buf[30] = 0x1b; // no headset attached + + out_buf[62] = 0x80; // IDK... it seems constant... + out_buf[57] = 0x80; // IDK... it seems constant... + out_buf[53] = 0x80; // IDK... it seems constant... + out_buf[48] = 0x80; // IDK... it seems constant... + out_buf[35] = 0x80; // IDK... it seems constant... + out_buf[44] = 0x80; // IDK... it seems constant... } int virt_dualshock_send(virt_dualshock_t *const gamepad, uint8_t *const out_buf) { diff --git a/virt_ds4.h b/virt_ds4.h index 08b1018..b3eee9d 100644 --- a/virt_ds4.h +++ b/virt_ds4.h @@ -12,6 +12,13 @@ typedef struct virt_dualshock { int fd; bool debug; + + uint32_t dt_sum; + uint8_t dt_buffer_current; + uint32_t dt_buffer[30]; + + uint32_t empty_reports; + uint64_t last_time; } virt_dualshock_t; int virt_dualshock_init(virt_dualshock_t *const gamepad); diff --git a/virt_ds5.c b/virt_ds5.c index 78c9f27..4046e39 100644 --- a/virt_ds5.c +++ b/virt_ds5.c @@ -110,7 +110,13 @@ static void destroy(int fd) int virt_dualsense_init(virt_dualsense_t *const out_gamepad) { int ret = 0; + out_gamepad->dt_sum = 0; + out_gamepad->dt_buffer_current = 0; + memset(out_gamepad->dt_buffer, 0, sizeof(out_gamepad->dt_buffer)); out_gamepad->debug = false; + out_gamepad->empty_reports = 0; + out_gamepad->last_time = 0; + out_gamepad->seq_num = 0; out_gamepad->fd = open(path, O_RDWR | O_CLOEXEC /* | O_NONBLOCK */); if (out_gamepad->fd < 0) { @@ -383,118 +389,101 @@ static ds5_dpad_status_t ds5_dpad_from_gamepad(uint8_t dpad) { return DPAD_RELEASED; } -static void compose_hid_report_buffer(int fd, gamepad_status_t *const gamepad_stats, uint8_t buf[64]) { - gamepad_status_t gs = *gamepad_stats; - - static uint8_t seq_num = 0x00; - - const int64_t time_us = gs.last_gyro_motion_time.tv_sec * 1000000 + gs.last_gyro_motion_time.tv_usec; - - static uint32_t empty_reports = 0; - static uint64_t last_time = 0; - const int delta_time = time_us - last_time; - last_time = time_us; - - // find the average Δt in the last 30 non-zero inputs; - // this has to run thousands of times a second so i'm trying to do this as fast as possible - static uint32_t dt_buffer[30] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - static uint32_t dt_sum = 0; - static uint8_t dt_buffer_current = 0; - - if (delta_time == 0) { - empty_reports++; - } else if (delta_time < 1000000 && delta_time > 0 ) { // ignore outliers - dt_sum -= dt_buffer[dt_buffer_current]; - dt_sum += delta_time; - dt_buffer[dt_buffer_current] = delta_time; - - if (dt_buffer_current == 29) { - dt_buffer_current = 0; - } else { - dt_buffer_current++; - } - } - - static uint64_t sim_time = 0; - const double correction_factor = DS5_SPEC_DELTA_TIME / ((double)dt_sum / 30.f); - if (delta_time != 0) { - sim_time += (int)((double)delta_time * correction_factor); - } - - const uint32_t timestamp = sim_time + (int)((double)empty_reports * DS5_SPEC_DELTA_TIME); - - const int16_t g_x = gs.raw_gyro[0]; - const int16_t g_y = (int16_t)(-1) * gs.raw_gyro[1]; // Swap Y and Z - const int16_t g_z = (int16_t)(-1) * gs.raw_gyro[2]; // Swap Y and Z - const int16_t a_x = gs.raw_accel[0]; - const int16_t a_y = (int16_t)(-1) * gs.raw_accel[1]; // Swap Y and Z - const int16_t a_z = (int16_t)(-1) * gs.raw_accel[2]; // Swap Y and Z - - - buf[0] = DS_INPUT_REPORT_USB; // [00] report ID (0x01) - buf[1] = ((uint64_t)((int64_t)gs.joystick_positions[0][0] + (int64_t)32768) >> (uint64_t)8); // L stick, X axis - buf[2] = ((uint64_t)((int64_t)gs.joystick_positions[0][1] + (int64_t)32768) >> (uint64_t)8); // L stick, Y axis - buf[3] = ((uint64_t)((int64_t)gs.joystick_positions[1][0] + (int64_t)32768) >> (uint64_t)8); // R stick, X axis - buf[4] = ((uint64_t)((int64_t)gs.joystick_positions[1][1] + (int64_t)32768) >> (uint64_t)8); // R stick, Y axis - buf[5] = gs.l2_trigger; // Z - buf[6] = gs.r2_trigger; // RZ - buf[7] = seq_num++; // seq_number - buf[8] = (gs.square ? 0x10 : 0x00) | - (gs.cross ? 0x20 : 0x00) | - (gs.circle ? 0x40 : 0x00) | - (gs.triangle ? 0x80 : 0x00) | - (uint8_t)ds5_dpad_from_gamepad(gs.dpad); - buf[9] = (gs.l1 ? 0x01 : 0x00) | - (gs.r1 ? 0x02 : 0x00) | - (gs.l2_trigger >= 225 ? 0x04 : 0x00) | - (gs.r2_trigger >= 225 ? 0x08 : 0x00) | - (gs.option ? 0x10 : 0x00) | - (gs.share ? 0x20 : 0x00) | - (gs.l3 ? 0x40 : 0x00) | - (gs.r3 ? 0x80 : 0x00); - - // mic button press is 0x04, touchpad press is 0x02 - buf[10] = (gs.l5 ? 0x40 : 0x00) | - (gs.r5 ? 0x80 : 0x00) | - (gs.l4 ? 0x10 : 0x00) | - (gs.r4 ? 0x20 : 0x00) | - (gs.touchpad_press ? 0x02 : 0x00) | - (gs.center ? 0x01 : 0x00); - //buf[11] = ; - - //buf[12] = 0x20; // [12] battery level | this is called sensor_temparature in the kernel driver but is never used... - memcpy(&buf[16], &g_x, sizeof(int16_t)); - memcpy(&buf[18], &g_y, sizeof(int16_t)); - memcpy(&buf[20], &g_z, sizeof(int16_t)); - memcpy(&buf[22], &a_x, sizeof(int16_t)); - memcpy(&buf[24], &a_y, sizeof(int16_t)); - memcpy(&buf[26], &a_z, sizeof(int16_t)); - memcpy(&buf[28], ×tamp, sizeof(timestamp)); - - // TODO: when touch is detected send 0x7F, when not 0x80 - buf[33] = 0x80; //touch0 active? - buf[37] = 0x80; //touch1 active? - -/* - buf[30] = 0x1b; // no headset attached -*/ - buf[62] = 0x80; // IDK... it seems constant... - buf[57] = 0x80; // IDK... it seems constant... - buf[53] = 0x80; // IDK... it seems constant... - buf[48] = 0x80; // IDK... it seems constant... - buf[35] = 0x80; // IDK... it seems constant... - buf[44] = 0x80; // IDK... it seems constant... -} - void virt_dualsense_close(virt_dualsense_t *const out_gamepad) { destroy(out_gamepad->fd); } void virt_dualsense_compose(virt_dualsense_t *const gamepad, gamepad_status_t *const in_device_status, uint8_t *const out_buf) { - gamepad_status_qam_quirk(in_device_status); + const int64_t time_us = in_device_status->last_gyro_motion_time.tv_sec * 1000000 + in_device_status->last_gyro_motion_time.tv_usec; - compose_hid_report_buffer(gamepad->fd, in_device_status, out_buf); + const int delta_time = time_us - gamepad->last_time; + gamepad->last_time = time_us; + + // find the average Δt in the last 30 non-zero inputs; + // this has to run thousands of times a second so i'm trying to do this as fast as possible + if (delta_time == 0) { + gamepad->empty_reports++; + } else if (delta_time < 1000000 && delta_time > 0 ) { // ignore outliers + gamepad->dt_sum -= gamepad->dt_buffer[gamepad->dt_buffer_current]; + gamepad->dt_sum += delta_time; + gamepad->dt_buffer[gamepad->dt_buffer_current] = delta_time; + + if (gamepad->dt_buffer_current == 29) { + gamepad->dt_buffer_current = 0; + } else { + gamepad->dt_buffer_current++; + } + } + + static uint64_t sim_time = 0; + const double correction_factor = DS5_SPEC_DELTA_TIME / ((double)gamepad->dt_sum / 30.f); + if (delta_time != 0) { + sim_time += (int)((double)delta_time * correction_factor); + } + + const uint32_t timestamp = sim_time + (int)((double)gamepad->empty_reports * DS5_SPEC_DELTA_TIME); + + const int16_t g_x = in_device_status->raw_gyro[0]; + const int16_t g_y = (int16_t)(-1) * in_device_status->raw_gyro[1]; // Swap Y and Z + const int16_t g_z = (int16_t)(-1) * in_device_status->raw_gyro[2]; // Swap Y and Z + const int16_t a_x = in_device_status->raw_accel[0]; + const int16_t a_y = (int16_t)(-1) * in_device_status->raw_accel[1]; // Swap Y and Z + const int16_t a_z = (int16_t)(-1) * in_device_status->raw_accel[2]; // Swap Y and Z + + + out_buf[0] = DS_INPUT_REPORT_USB; // [00] report ID (0x01) + out_buf[1] = ((uint64_t)((int64_t)in_device_status->joystick_positions[0][0] + (int64_t)32768) >> (uint64_t)8); // L stick, X axis + out_buf[2] = ((uint64_t)((int64_t)in_device_status->joystick_positions[0][1] + (int64_t)32768) >> (uint64_t)8); // L stick, Y axis + out_buf[3] = ((uint64_t)((int64_t)in_device_status->joystick_positions[1][0] + (int64_t)32768) >> (uint64_t)8); // R stick, X axis + out_buf[4] = ((uint64_t)((int64_t)in_device_status->joystick_positions[1][1] + (int64_t)32768) >> (uint64_t)8); // R stick, Y axis + out_buf[5] = in_device_status->l2_trigger; // Z + out_buf[6] = in_device_status->r2_trigger; // RZ + out_buf[7] = gamepad->seq_num++; // seq_number + out_buf[8] = (in_device_status->square ? 0x10 : 0x00) | + (in_device_status->cross ? 0x20 : 0x00) | + (in_device_status->circle ? 0x40 : 0x00) | + (in_device_status->triangle ? 0x80 : 0x00) | + (uint8_t)ds5_dpad_from_gamepad(in_device_status->dpad); + out_buf[9] = (in_device_status->l1 ? 0x01 : 0x00) | + (in_device_status->r1 ? 0x02 : 0x00) | + (in_device_status->l2_trigger >= 225 ? 0x04 : 0x00) | + (in_device_status->r2_trigger >= 225 ? 0x08 : 0x00) | + (in_device_status->option ? 0x10 : 0x00) | + (in_device_status->share ? 0x20 : 0x00) | + (in_device_status->l3 ? 0x40 : 0x00) | + (in_device_status->r3 ? 0x80 : 0x00); + + // mic button press is 0x04, touchpad press is 0x02 + out_buf[10] = (in_device_status->l5 ? 0x40 : 0x00) | + (in_device_status->r5 ? 0x80 : 0x00) | + (in_device_status->l4 ? 0x10 : 0x00) | + (in_device_status->r4 ? 0x20 : 0x00) | + (in_device_status->touchpad_press ? 0x02 : 0x00) | + (in_device_status->center ? 0x01 : 0x00); + //buf[11] = ; + + //buf[12] = 0x20; // [12] battery level | this is called sensor_temparature in the kernel driver but is never used... + memcpy(&out_buf[16], &g_x, sizeof(int16_t)); + memcpy(&out_buf[18], &g_y, sizeof(int16_t)); + memcpy(&out_buf[20], &g_z, sizeof(int16_t)); + memcpy(&out_buf[22], &a_x, sizeof(int16_t)); + memcpy(&out_buf[24], &a_y, sizeof(int16_t)); + memcpy(&out_buf[26], &a_z, sizeof(int16_t)); + memcpy(&out_buf[28], ×tamp, sizeof(timestamp)); + + // TODO: when touch is detected send 0x7F, when not 0x80 + out_buf[33] = 0x80; //touch0 active? + out_buf[37] = 0x80; //touch1 active? + +/* + buf[30] = 0x1b; // no headset attached +*/ + out_buf[62] = 0x80; // IDK... it seems constant... + out_buf[57] = 0x80; // IDK... it seems constant... + out_buf[53] = 0x80; // IDK... it seems constant... + out_buf[48] = 0x80; // IDK... it seems constant... + out_buf[35] = 0x80; // IDK... it seems constant... + out_buf[44] = 0x80; // IDK... it seems constant... } int virt_dualsense_send(virt_dualsense_t *const gamepad, uint8_t *const out_buf) { diff --git a/virt_ds5.h b/virt_ds5.h index 1d1c5c9..7108b8f 100644 --- a/virt_ds5.h +++ b/virt_ds5.h @@ -12,6 +12,15 @@ typedef struct virt_dualsense { int fd; bool debug; + + uint8_t seq_num; + + uint32_t dt_sum; + uint8_t dt_buffer_current; + uint32_t dt_buffer[30]; + + uint32_t empty_reports; + uint64_t last_time; } virt_dualsense_t; int virt_dualsense_init(virt_dualsense_t *const gamepad);