xbo_thrustmaster_458s_driver/main.c

/* ===== Includes ===== */

#include <stdlib.h>
#include <stddef.h>
#include <inttypes.h>
#include <string.h>		/* memset */
#include <stdio.h>

#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <libusb-1.0/libusb.h>
/*#include <linux/uinput.h>*/
#include <libevdev-1.0/libevdev/libevdev.h>
#include <libevdev-1.0/libevdev/libevdev-uinput.h>
#include <sys/types.h>		/* ssize_t */

/* ===== Defines ===== */

#define RC_SUCCESS 0
#define RC_LIBUSB_ERROR -1
#define RC_UINPUT_ERROR -2
#define RC_FILE_IO_ERROR -3

#define error_printf(...) printf("[error] " __VA_ARGS__)
#define warn_printf(...) printf("[warn] " __VA_ARGS__)

#ifdef DEBUG
#define debug_printf(...) printf("[debug] " __VA_ARGS__)
#else
#define debug_printf(...)
#endif

#define XBO_MAX_CNT 32
#define XBO_INTERFACE 0
#define XBO_ENDPOINT_IN 0x81
#define XBO_ENDPOINT_OUT 0x01
#define XBO_MAX_PACKET_SIZE 64
#define XBO_DEADZONE_WHEEL 512
#define XBO_DEADZONE_PEDAL 0

/* ===== Structs ===== */

enum xbo_packet_type {
	XBO_PACKET_WAITING_CONNECTION = 0x02,
	XBO_PACKET_START_INPUT = 0X05,
	XBO_PACKET_HEARTBEAT = 0x03,
	XBO_PACKET_INPUT_DATA = 0x20,
	XBO_PACKET_CRASH = 0x01
};

struct xbo_packet_input_data {
	uint8_t packet_type;
	uint8_t const_00;
	uint16_t id;

	uint8_t sync : 1;
	uint8_t dummy : 1;
	uint8_t start : 1;
	uint8_t back : 1;

	uint8_t a : 1;
	uint8_t b : 1;
	uint8_t x : 1;
	uint8_t y : 1;

	uint8_t dpad_up : 1;
	uint8_t dpad_down : 1;
	uint8_t dpad_left : 1;
	uint8_t dpad_right : 1;

	uint8_t bumper_left : 1;
	uint8_t bumper_right : 1;
	uint8_t stick_left_click : 1;
	uint8_t stick_right_click : 1;

	uint16_t trigger_left;
	uint16_t trigger_right;

	int16_t stick_left_x;
	int16_t stick_left_y;
	int16_t stick_right_x;
	int16_t stick_right_y;
};

struct xbo_input_data {
	uint8_t start : 1;
	uint8_t back : 1;

	uint8_t a : 1;
	uint8_t b : 1;
	uint8_t x : 1;
	uint8_t y : 1;

	uint8_t dpad_up : 1;
	uint8_t dpad_down : 1;
	uint8_t dpad_left : 1;
	uint8_t dpad_right : 1;

	uint8_t bumper_left : 1;
	uint8_t bumper_right : 1;

	int16_t wheel;

	uint16_t pedal_gas;
	uint16_t pedal_brake;
};

/* ===== Variables ===== */

uint16_t compatible_ids[][2] = {
		{ 0x044f, 0xb671 } };

uint8_t xbo_packet_start_input[] = { XBO_PACKET_START_INPUT, 0x20, 0x00, 0x01, 0x00 };
uint8_t xbo_packet_crash[] = { XBO_PACKET_CRASH, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e };

/* ===== Functions ===== */

static int32_t claim_device(struct libusb_device *dev, struct libusb_device_handle **handle) {
	int32_t err = 0;

	err = libusb_open(dev, handle);
	if (err != LIBUSB_SUCCESS) {
		error_printf("libusb: %s\n", libusb_error_name(err));
		return RC_LIBUSB_ERROR;
	}

	err = libusb_claim_interface(*handle, XBO_INTERFACE);
	if (err != LIBUSB_SUCCESS) {
		error_printf("libusb: %s\n", libusb_error_name(err));
		return RC_LIBUSB_ERROR;
	}

	return RC_SUCCESS;
}
static int32_t unclaim_device(struct libusb_device_handle *handle) {
	int32_t err = libusb_release_interface(handle, XBO_INTERFACE);
	if (err != LIBUSB_SUCCESS) {
		error_printf("libusb: %s\n", libusb_error_name(err));
		return RC_LIBUSB_ERROR;
	}

	libusb_close(handle);

	return RC_SUCCESS;
}

static int32_t get_device_array(struct libusb_device_handle **handles, size_t len, size_t *found_cnt) {
	int32_t err = 0;

	struct libusb_device **devs = NULL;
	ssize_t dev_cnt = libusb_get_device_list(NULL, &devs);

	if (dev_cnt < 0) {
		error_printf("libusb: %s\n", libusb_error_name(dev_cnt));
		err = RC_LIBUSB_ERROR;
		return 0;
	}

	struct libusb_device_descriptor desc = { 0 };
	size_t handle_cnt = 0;

	for (size_t i = 0; i < (size_t)dev_cnt; ++i) {
		err = libusb_get_device_descriptor(devs[i], &desc);
		if (err != LIBUSB_SUCCESS) {
			error_printf("libusb: %s\n", libusb_error_name(err));
			err = RC_LIBUSB_ERROR;
			return 0;
		}

		debug_printf("Device %04" PRIx16 ":%04" PRIx16 "\n", desc.idVendor, desc.idProduct);

		for (size_t j = 0; j < sizeof compatible_ids / sizeof compatible_ids[0]; ++j) {
			if (desc.idVendor == compatible_ids[j][0] && desc.idProduct == compatible_ids[j][1]) {
				debug_printf("Found device\n");

				err = claim_device(devs[i], &handles[handle_cnt]);
				if (err != RC_SUCCESS) {
					return err;
				}

				++handle_cnt;
			}
		}

		if (handle_cnt == len) {
			warn_printf("Reached maximum number of devices\n");
			break;
		}
	}

	libusb_free_device_list(devs, 1);

	debug_printf("Found %zi compatible devices\n", handle_cnt);

	*found_cnt = handle_cnt;

	return RC_SUCCESS;
}

static int32_t update_device(struct libusb_device_handle *handle, struct xbo_input_data *input) {
	int32_t err = 0;

	uint8_t packet[XBO_MAX_PACKET_SIZE] = { 0x00 };
	size_t packet_size = 0;

	err = libusb_interrupt_transfer(handle, XBO_ENDPOINT_IN, packet, XBO_MAX_PACKET_SIZE,
			(int32_t *)&packet_size, 0);
	if (err != LIBUSB_SUCCESS) {
		error_printf("libusb: %s\n", libusb_error_name(err));
		return RC_LIBUSB_ERROR;
	}

	switch (packet[0]) {
	case XBO_PACKET_WAITING_CONNECTION:
		debug_printf("Received XBO_PACKET_WAITING_CONNECTION\n");

		err = libusb_interrupt_transfer(handle, XBO_ENDPOINT_OUT, xbo_packet_start_input,
				sizeof(xbo_packet_start_input), NULL, 0);
		if (err != LIBUSB_SUCCESS) {
			error_printf("libusb: %s\n", libusb_error_name(err));
			return RC_LIBUSB_ERROR;
		}
		break;
	case XBO_PACKET_HEARTBEAT:
		debug_printf("Received XBO_PACKET_HEARTBEAT\n");
		break;
	case XBO_PACKET_INPUT_DATA:
		;
		struct xbo_packet_input_data ipacket = *(struct xbo_packet_input_data *)packet;

		input->start = ipacket.start;
		input->back = ipacket.back;

		input->a = ipacket.a;
		input->b = ipacket.b;
		input->x = ipacket.x;
		input->y = ipacket.y;

		input->dpad_up = ipacket.dpad_up;
		input->dpad_down = ipacket.dpad_down;
		input->dpad_left = ipacket.dpad_left;
		input->dpad_right = ipacket.dpad_right;

		input->bumper_left = ipacket.bumper_left;
		input->bumper_right = ipacket.bumper_right;

		input->pedal_gas = ipacket.trigger_right;
		input->pedal_brake = ipacket.stick_left_x;

		input->wheel = (ipacket.trigger_left > 32768 + XBO_DEADZONE_WHEEL
				|| ipacket.trigger_left < 32768 - XBO_DEADZONE_WHEEL ?
				ipacket.trigger_left : 32768 ) - 32768;

		break;
	default:
		warn_printf("Unknown packet type: %02" PRIx8 "\n", packet[0]);
		break;
	}

	return RC_SUCCESS;
}

static const char * int16_display_bar(int16_t n) {
	int16_t div = n / 4096;
	if (div == -8) { return "|               |"; }
	if (div == -7) { return "|=              |"; }
	if (div == -6) { return "|==             |"; }
	if (div == -5) { return "|===            |"; }
	if (div == -4) { return "|====           |"; }
	if (div == -3) { return "|=====          |"; }
	if (div == -2) { return "|======         |"; }
	if (div == -1) { return "|=======        |"; }
	if (div == 0)  { return "|========       |"; }
	if (div == 1)  { return "|=========      |"; }
	if (div == 2)  { return "|==========     |"; }
	if (div == 3)  { return "|===========    |"; }
	if (div == 4)  { return "|============   |"; }
	if (div == 5)  { return "|=============  |"; }
	if (div == 6)  { return "|============== |"; }
	if (div == 7)  { return "|===============|"; }
	return "|ERR             |";
}
static const char * uint16_display_bar(uint16_t n) {
	uint16_t div = n / 4096;
	if (div == 0)  { return "|               |"; }
	if (div == 1)  { return "|=              |"; }
	if (div == 2)  { return "|==             |"; }
	if (div == 3)  { return "|===            |"; }
	if (div == 4)  { return "|====           |"; }
	if (div == 5)  { return "|=====          |"; }
	if (div == 6)  { return "|======         |"; }
	if (div == 7)  { return "|=======        |"; }
	if (div == 8)  { return "|========       |"; }
	if (div == 9)  { return "|=========      |"; }
	if (div == 10) { return "|==========     |"; }
	if (div == 11) { return "|===========    |"; }
	if (div == 12) { return "|============   |"; }
	if (div == 13) { return "|=============  |"; }
	if (div == 14) { return "|============== |"; }
	if (div == 15) { return "|===============|"; }
	return "|ERR             |";
}

static void debug_print_input_data(struct xbo_input_data input) {
	debug_printf("Input Data:\n"
			"\tSt: %" PRIi8 "\tBa: %" PRIi8 "\n"
			"\tA: %" PRIi8 "\tB: %" PRIi8 "\tX: %" PRIi8 "\tY: %" PRIi8 "\n"
			"\tDu: %" PRIi8 "\tDd: %" PRIi8 "\tDl: %" PRIi8 "\tDr: %" PRIi8 "\n"
			"\tLb: %" PRIi8 "\tRb: %" PRIi8 "\n"
			"\tPb: %s\tPg: %s\n\tWh: %s\n",
			input.start, input.back,
			input.a, input.b, input.x, input.y,
			input.dpad_up, input.dpad_down,
			input.dpad_left, input.dpad_right,
			input.bumper_left, input.bumper_right,
			uint16_display_bar(input.pedal_brake * 64), uint16_display_bar(input.pedal_gas * 64),
			int16_display_bar(input.wheel));
}

static int32_t emit(uint16_t uinput, uint16_t type, uint16_t code, int32_t val) {
	struct input_event event = {
			.type = type,
			.code = code,
			.value = val,
			.time.tv_sec = 0,
			.time.tv_usec = 0 };

	if (write(uinput, &event, sizeof(event)) == -1) {
		error_printf("Failed to write to /dev/uinput\n");
		return RC_FILE_IO_ERROR;
	}

	return RC_SUCCESS;
}
int32_t main(void) {
	int32_t err = 0;

	/* Initializing libusb */
	err = libusb_init(NULL);
	if (err != LIBUSB_SUCCESS) {
		error_printf("libusb: %s\n", libusb_error_name(err));
		return RC_LIBUSB_ERROR;
	}

	debug_printf("Initialized libusb\n");

#ifdef DEBUG
	err = libusb_set_option(NULL, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_WARNING);
	if (err != LIBUSB_SUCCESS) {
		error_printf("libusb: %s\n", libusb_error_name(err));
		return RC_LIBUSB_ERROR;
	}
#endif

	debug_printf("Set libusb to debug\n");

	size_t handle_cnt = 0;
	struct libusb_device_handle *handles[XBO_MAX_CNT] = { NULL };

	err = get_device_array(handles, XBO_MAX_CNT, &handle_cnt);
	if (err != RC_SUCCESS) {
		return err;
	}

	/* Initialize evdev */
	struct libevdev *evdev = NULL;
	struct libevdev_uinput *ui_evdev = NULL;
	evdev = libevdev_new();

	libevdev_set_name(evdev, "Thrustmaster Ferrari 458 Spider XBox One Wheel");

	libevdev_enable_event_type(evdev, EV_KEY);
	libevdev_enable_event_type(evdev, EV_ABS);

	libevdev_enable_event_code(evdev, EV_KEY, BTN_START, NULL);
	libevdev_enable_event_code(evdev, EV_KEY, BTN_SELECT, NULL);

	libevdev_enable_event_code(evdev, EV_KEY, BTN_A, NULL);
	libevdev_enable_event_code(evdev, EV_KEY, BTN_B, NULL);
	libevdev_enable_event_code(evdev, EV_KEY, BTN_X, NULL);
	libevdev_enable_event_code(evdev, EV_KEY, BTN_Y, NULL);

	libevdev_enable_event_code(evdev, EV_KEY, BTN_DPAD_UP, NULL);
	libevdev_enable_event_code(evdev, EV_KEY, BTN_DPAD_DOWN, NULL);
	libevdev_enable_event_code(evdev, EV_KEY, BTN_DPAD_LEFT, NULL);
	libevdev_enable_event_code(evdev, EV_KEY, BTN_DPAD_RIGHT, NULL);

	libevdev_enable_event_code(evdev, EV_KEY, BTN_TL2, NULL);
	libevdev_enable_event_code(evdev, EV_KEY, BTN_TR2, NULL);

	struct input_absinfo wheel_info = {
			.value = 0,
			.minimum = -32768,
			.maximum = 32767,
			.fuzz = 0,
			.flat = XBO_DEADZONE_WHEEL,
			.resolution = 1 };
	libevdev_enable_event_code(evdev, EV_ABS, ABS_X, &wheel_info);

	struct input_absinfo pedal_info = {
			.value = 0,
			.minimum = 0,
			.maximum = 1024,
			.fuzz = 0,
			.flat = XBO_DEADZONE_PEDAL,
			.resolution = 1 };
	libevdev_enable_event_code(evdev, EV_ABS, ABS_RX, &pedal_info);
	libevdev_enable_event_code(evdev, EV_ABS, ABS_RY, &pedal_info);

	err = libevdev_uinput_create_from_device(evdev, LIBEVDEV_UINPUT_OPEN_MANAGED, &ui_evdev);
	if (err != 0) {
		error_printf("evdev: Failed to create device\n");
		return err;
	}

	debug_printf("Created evdev device\n");

	/* Handle input */
	struct xbo_input_data input[XBO_MAX_CNT];

	for (;;) {
		for (size_t i = 0; i < handle_cnt; ++i) {
			if (handles[i] == NULL) {
				continue;
			}

			err = update_device(handles[i], &input[i]);
			if (err != RC_SUCCESS) {
				return err;
			}

			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_START, input[i].start);
			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_SELECT, input[i].back);

			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_A, input[i].a);
			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_B, input[i].b);
			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_X, input[i].x);
			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_Y, input[i].y);

			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_DPAD_UP, input[i].dpad_up);
			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_DPAD_DOWN, input[i].dpad_down);
			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_DPAD_LEFT, input[i].dpad_left);
			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_DPAD_RIGHT, input[i].dpad_right);

			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_TL2, input[i].bumper_left);
			libevdev_uinput_write_event(ui_evdev, EV_KEY, BTN_TR2, input[i].bumper_right);

			libevdev_uinput_write_event(ui_evdev, EV_ABS, ABS_RX, input[i].pedal_brake);
			libevdev_uinput_write_event(ui_evdev, EV_ABS, ABS_RY, input[i].pedal_gas);

			libevdev_uinput_write_event(ui_evdev, EV_ABS, ABS_X, input[i].wheel);

			libevdev_uinput_write_event(ui_evdev, EV_SYN, SYN_REPORT, 0);


			debug_print_input_data(input[i]);
		}

		printf("\x1b[7A");
	}

	for (size_t i = 0; i < handle_cnt; ++i) {
		err = unclaim_device(handles[i]);
		if (err != RC_SUCCESS) {
			return err;
		}
	}

	libevdev_uinput_destroy(ui_evdev);
	libevdev_free(evdev);

	debug_printf("Destroyed evdev device\n");

	libusb_exit(NULL);

	debug_printf("Exited libusb\n");

	return RC_SUCCESS;
}