dualsense-bridge.c

// SPDX-License-Identifier: GPL-2.0
/*
 * quick and dirty tool to present a DualSense-like virtual USB HID device bridging
 * an actual bluetooth-connected DualSense.
 *
 * Compile with `gcc dualsense-bridge.c -ludev -lz
 *
 * It's adapted from Linux's UHID Example
 *
 * Copyright (c) 2012-2013 David Herrmann <dh.herrmann@gmail.com>
 *
 * The code may be used by anyone for any purpose,
 * and can serve as a starting point for developing
 * applications using uhid.
 */

/*
 * UHID Example
 * This example emulates a basic 3 buttons mouse with wheel over UHID. Run this
 * program as root and then use the following keys:
 *   q: Quit the application
 *
 * If uhid is not available as /dev/uhid, then you can pass a different path as
 * first argument.
 * If <linux/uhid.h> is not installed in /usr, then compile this with:
 *   gcc -o ./uhid_test -Wall -I./include ./samples/uhid/uhid-example.c
 * And ignore the warning about kernel headers. However, it is recommended to
 * use the installed uhid.h if available.
 */

#include <errno.h>
#include <fcntl.h>
#include <poll.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <linux/uhid.h>
#include <linux/hidraw.h>
#include <libudev.h>
#include <stdint.h>
#include <zlib.h>

// Copied from an actual DualSense using usbhid-dump
static unsigned char rdesc[] = {
	0x05, 0x01, 0x09, 0x05, 0xA1, 0x01, 0x85, 0x01, 0x09, 0x30, 0x09, 0x31, 0x09, 0x32, 0x09, 0x35
, 0x09, 0x33, 0x09, 0x34, 0x15, 0x00, 0x26, 0xFF, 0x00, 0x75, 0x08, 0x95, 0x06, 0x81, 0x02, 0x06
, 0x00, 0xFF, 0x09, 0x20, 0x95, 0x01, 0x81, 0x02, 0x05, 0x01, 0x09, 0x39, 0x15, 0x00, 0x25, 0x07
, 0x35, 0x00, 0x46, 0x3B, 0x01, 0x65, 0x14, 0x75, 0x04, 0x95, 0x01, 0x81, 0x42, 0x65, 0x00, 0x05
, 0x09, 0x19, 0x01, 0x29, 0x0F, 0x15, 0x00, 0x25, 0x01, 0x75, 0x01, 0x95, 0x0F, 0x81, 0x02, 0x06
, 0x00, 0xFF, 0x09, 0x21, 0x95, 0x0D, 0x81, 0x02, 0x06, 0x00, 0xFF, 0x09, 0x22, 0x15, 0x00, 0x26
, 0xFF, 0x00, 0x75, 0x08, 0x95, 0x34, 0x81, 0x02, 0x85, 0x02, 0x09, 0x23, 0x95, 0x2F, 0x91, 0x02
, 0x85, 0x05, 0x09, 0x33, 0x95, 0x28, 0xB1, 0x02, 0x85, 0x08, 0x09, 0x34, 0x95, 0x2F, 0xB1, 0x02
, 0x85, 0x09, 0x09, 0x24, 0x95, 0x13, 0xB1, 0x02, 0x85, 0x0A, 0x09, 0x25, 0x95, 0x1A, 0xB1, 0x02
, 0x85, 0x20, 0x09, 0x26, 0x95, 0x3F, 0xB1, 0x02, 0x85, 0x21, 0x09, 0x27, 0x95, 0x04, 0xB1, 0x02
, 0x85, 0x22, 0x09, 0x40, 0x95, 0x3F, 0xB1, 0x02, 0x85, 0x80, 0x09, 0x28, 0x95, 0x3F, 0xB1, 0x02
, 0x85, 0x81, 0x09, 0x29, 0x95, 0x3F, 0xB1, 0x02, 0x85, 0x82, 0x09, 0x2A, 0x95, 0x09, 0xB1, 0x02
, 0x85, 0x83, 0x09, 0x2B, 0x95, 0x3F, 0xB1, 0x02, 0x85, 0x84, 0x09, 0x2C, 0x95, 0x3F, 0xB1, 0x02
, 0x85, 0x85, 0x09, 0x2D, 0x95, 0x02, 0xB1, 0x02, 0x85, 0xA0, 0x09, 0x2E, 0x95, 0x01, 0xB1, 0x02
, 0x85, 0xE0, 0x09, 0x2F, 0x95, 0x3F, 0xB1, 0x02, 0x85, 0xF0, 0x09, 0x30, 0x95, 0x3F, 0xB1, 0x02
, 0x85, 0xF1, 0x09, 0x31, 0x95, 0x3F, 0xB1, 0x02, 0x85, 0xF2, 0x09, 0x32, 0x95, 0x0F, 0xB1, 0x02
, 0x85, 0xF4, 0x09, 0x35, 0x95, 0x3F, 0xB1, 0x02, 0x85, 0xF5, 0x09, 0x36, 0x95, 0x03, 0xB1, 0x02
, 0xC0

};

static int uhid_write(int fd, const struct uhid_event *ev)
{
	ssize_t ret;

	ret = write(fd, ev, sizeof(*ev));
	if (ret < 0) {
		fprintf(stderr, "Cannot write to uhid: %m\n");
		return -errno;
	} else if (ret != sizeof(*ev)) {
		fprintf(stderr, "Wrong size written to uhid: %zd != %zu\n",
			ret, sizeof(ev));
		return -EFAULT;
	} else {
		return 0;
	}
}

static int forward_input(int fd, int upstream_fd)
{
	struct uhid_event ev;
	uint8_t buf[256];
	int res;

	res = read(upstream_fd, buf, 256);
	if (res < 0) {
		perror("read");
		return -errno;
	} else {
		memset(&ev, 0, sizeof(ev));
		ev.type = UHID_INPUT2;

		if (buf[0] == 0x31 && res == 78) {
			ev.u.input2.size = 64;
			buf[1] = 0x01;
#if 0
			buf[7] = counter;
			counter = (counter + 1) & 0xff;
			buf[54] |= 1 << 3; // according to https://controllers.fandom.com/wiki/Sony_DualSense it states whether it's plugged into USB data
#endif
			memcpy(&ev.u.input2.data, &buf[1], 64);
		} else {
			fprintf(stderr, "Unknown input type %d\n", buf[0]);
			ev.u.input2.size = res;
			memcpy(&ev.u.input2.data, buf, res);
		}

		return uhid_write(fd, &ev);
	}
}

static int create(int fd)
{
	struct uhid_event ev;

	memset(&ev, 0, sizeof(ev));
	ev.type = UHID_CREATE;
	strcpy((char*)ev.u.create.name, "Wireless Controller");
	ev.u.create.rd_data = rdesc;
	ev.u.create.rd_size = sizeof(rdesc);
	ev.u.create.bus = BUS_USB;
	ev.u.create.vendor = 0x054c;
	ev.u.create.product = 0x0ce6;
	ev.u.create.version = 0;
	ev.u.create.country = 0;

	return uhid_write(fd, &ev);
}

static int open_dualsense()
{
	int fd = -1;
	struct udev *udev;
	struct udev_enumerate *enumerate;
	struct udev_list_entry *devices, *dev_list_entry;

	udev = udev_new();
	if (!udev) {
		fprintf(stderr, "Can't create udev\n");
		return -1;
	}

	// Find HID device
	enumerate = udev_enumerate_new(udev);
	udev_enumerate_add_match_subsystem(enumerate, "hid");
	udev_enumerate_add_match_property(enumerate, "HID_ID", "0005:0000054C:00000CE6"); // bluetooth-connected DualSense
	udev_enumerate_scan_devices(enumerate);

	devices = udev_enumerate_get_list_entry(enumerate);

	struct udev_device *hid_dev = NULL, *hidraw_dev = NULL;
	udev_list_entry_foreach(dev_list_entry, devices) {
		const char *sysfs_path;
		if (hid_dev == NULL) {
			sysfs_path = udev_list_entry_get_name(dev_list_entry);
			hid_dev = udev_device_new_from_syspath(udev, sysfs_path);
		}
	}
	udev_enumerate_unref(enumerate);

	if (hid_dev == NULL) {
		udev_unref(udev);
		return -1;
	}

	// Find HIDRaw device
	enumerate = udev_enumerate_new(udev);
	udev_enumerate_add_match_subsystem(enumerate, "hidraw");
	udev_enumerate_add_match_parent(enumerate, hid_dev);
	udev_enumerate_scan_devices(enumerate);
	devices = udev_enumerate_get_list_entry(enumerate);
	udev_list_entry_foreach(dev_list_entry, devices) {
		const char *sysfs_path, *dev_path;
		if (fd == -1) {
			sysfs_path = udev_list_entry_get_name(dev_list_entry);
			hidraw_dev = udev_device_new_from_syspath(udev, sysfs_path);
			dev_path = udev_device_get_devnode(hidraw_dev);
			fd = open(dev_path, O_RDWR); // O_RDWR|O_NONBLOCK
			udev_device_unref(hidraw_dev);
		}
	}
	udev_device_unref(hid_dev);
	udev_enumerate_unref(enumerate);

	udev_unref(udev);

	return fd;
}

static void destroy(int fd)
{
	struct uhid_event ev;

	memset(&ev, 0, sizeof(ev));
	ev.type = UHID_DESTROY;

	uhid_write(fd, &ev);
}

static void handle_output(struct uhid_event *ev, int upstream_fd)
{
	static int counter;

	if (ev->u.output.rtype != UHID_OUTPUT_REPORT)
		return;

	fprintf(stderr, "Report data (%d):\n\t", ev->u.output.size);
	for (int i = 0; i < ev->u.output.size; i++)
		printf("%hhx ", ev->u.output.data[i]);
	puts("\n");

	if ((ev->u.output.size == 63 || ev->u.output.size == 48) && ev->u.output.data[0] == 2) {
		uint8_t buf[256];
		fprintf(stderr, "Forwarding known packet (%d)\n", counter);
		buf[0] = 0x31;
		buf[1] = (counter << 4) | 0x0;
		buf[2] = 0x10;
		if (++counter == 16)
			counter = 0;
		memcpy(&buf[3], &ev->u.output.data[1], 47);
		memset(&buf[50], 0, 24);

		uint32_t crc;
		uint8_t seed = 0xA2;
		crc = crc32(0, &seed, 1);
		crc = crc32(crc, buf, 74);

		buf[74] = crc & 0xff;
		crc >>= 8;
		buf[75] = crc & 0xff;
		crc >>= 8;
		buf[76] = crc & 0xff;
		crc >>= 8;
		buf[77] = crc & 0xff;

		write(upstream_fd, buf, 78);
	}
}

static int event(int fd, int upstream_fd)
{
	struct uhid_event ev, ev2;
	ssize_t ret;

	memset(&ev, 0, sizeof(ev));
	ret = read(fd, &ev, sizeof(ev));
	if (ret == 0) {
		fprintf(stderr, "Read HUP on uhid-cdev\n");
		return -EFAULT;
	} else if (ret < 0) {
		fprintf(stderr, "Cannot read uhid-cdev: %m\n");
		return -errno;
	} else if (ret != sizeof(ev)) {
		fprintf(stderr, "Invalid size read from uhid-dev: %zd != %zu\n",
			ret, sizeof(ev));
		return -EFAULT;
	}

	switch (ev.type) {
	case UHID_START:
		fprintf(stderr, "UHID_START from uhid-dev\n");
		break;
	case UHID_STOP:
		fprintf(stderr, "UHID_STOP from uhid-dev\n");
		break;
	case UHID_OPEN:
		fprintf(stderr, "UHID_OPEN from uhid-dev\n");
		break;
	case UHID_CLOSE:
		fprintf(stderr, "UHID_CLOSE from uhid-dev\n");
		break;
	case UHID_OUTPUT:
		fprintf(stderr, "UHID_OUTPUT from uhid-dev\n");
		handle_output(&ev, upstream_fd);
		break;
	case UHID_OUTPUT_EV:
		fprintf(stderr, "UHID_OUTPUT_EV from uhid-dev\n");
		break;
	case UHID_GET_REPORT:
		fprintf(stderr, "UHID_GET_REPORT from uhid-dev (rnum: %d / rtype: %d)\n", ev.u.get_report.rnum, ev.u.get_report.rtype);

		uint8_t buf[256];
		int res, i;

		buf[0] = ev.u.get_report.rnum; /* Report Number */
		res = ioctl(upstream_fd, HIDIOCGFEATURE(256), buf);
		if (res < 0) {
			perror("HIDIOCGFEATURE");
		} else {
			printf("Report data:\n\t");
			for (i = 0; i < res; i++)
				printf("%hhx ", buf[i]);
			puts("\n");

			/*
			uint32_t crc; // seed: 0xA3
			uint8_t seed = 0xA3;
			crc = crc32(0, &seed, 1);
			crc = crc32(crc, buf, res - 4);
			printf("%d VS %d\n", crc, *((uint32_t*) &buf[res-4]));
			*/

			memset(&ev2, 0, sizeof(ev2));
			ev2.type = UHID_GET_REPORT_REPLY;
			ev2.u.get_report_reply.id = ev.u.get_report.id;
			ev2.u.get_report_reply.size = res;
			memcpy(&ev2.u.get_report_reply.data, buf, res - 4);

			uhid_write(fd, &ev2);
		}

		break;
	case UHID_SET_REPORT:
		fprintf(stderr, "UHID_SET_REPORT from uhid-dev (rnum: %d / rtype: %d / size: %d)\n", ev.u.set_report.rnum, ev.u.set_report.rtype, ev.u.set_report.size);
		printf("\t");
		for (int i = 0; i < ev.u.set_report.size; i++)
			printf("%hhx ", ev.u.set_report.data[i]);
		puts("\n");

		//TODO: forward stuff

		memset(&ev2, 0, sizeof(ev2));
		ev2.type = UHID_SET_REPORT_REPLY;
		ev2.u.set_report_reply.id = ev.u.set_report.id;
		ev2.u.set_report_reply.err = 0;

		uhid_write(fd, &ev2);

		break;
	default:
		fprintf(stderr, "Invalid event from uhid-dev: %u\n", ev.type);
	}

	return 0;
}

static bool btn1_down;
static bool btn2_down;
static bool btn3_down;
static signed char abs_hor;
static signed char abs_ver;
static signed char wheel;

static int keyboard(int fd)
{
	char buf[128];
	ssize_t ret, i;

	ret = read(STDIN_FILENO, buf, sizeof(buf));
	if (ret == 0) {
		fprintf(stderr, "Read HUP on stdin\n");
		return -EFAULT;
	} else if (ret < 0) {
		fprintf(stderr, "Cannot read stdin: %m\n");
		return -errno;
	}

	for (i = 0; i < ret; ++i) {
		switch (buf[i]) {
		case 'q':
			return -ECANCELED;
		default:
			fprintf(stderr, "Invalid input: %c\n", buf[i]);
		}
	}

	return 0;
}

int main(int argc, char **argv)
{
	int fd, upstream_fd;
	const char *path = "/dev/uhid";
	struct pollfd pfds[3];
	int ret;
	struct termios state;

	ret = tcgetattr(STDIN_FILENO, &state);
	if (ret) {
		fprintf(stderr, "Cannot get tty state\n");
	} else {
		state.c_lflag &= ~ICANON;
		state.c_cc[VMIN] = 1;
		ret = tcsetattr(STDIN_FILENO, TCSANOW, &state);
		if (ret)
			fprintf(stderr, "Cannot set tty state\n");
	}

	if (argc >= 2) {
		if (!strcmp(argv[1], "-h") || !strcmp(argv[1], "--help")) {
			fprintf(stderr, "Usage: %s [%s]\n", argv[0], path);
			return EXIT_SUCCESS;
		} else {
			path = argv[1];
		}
	}

	fprintf(stderr, "Open DualSense HIDRaw\n");
	upstream_fd = open_dualsense();
	if (upstream_fd < 0) {
		fprintf(stderr, "Error opening DualSense HIDRaw device\n");
		return EXIT_FAILURE;
	}

	fprintf(stderr, "Open uhid-cdev %s\n", path);
	fd = open(path, O_RDWR | O_CLOEXEC);
	if (fd < 0) {
		fprintf(stderr, "Cannot open uhid-cdev %s: %m\n", path);
		return EXIT_FAILURE;
	}

	fprintf(stderr, "Create uhid device\n");
	ret = create(fd);
	if (ret) {
		close(fd);
		return EXIT_FAILURE;
	}

	pfds[0].fd = STDIN_FILENO;
	pfds[0].events = POLLIN;
	pfds[1].fd = fd;
	pfds[1].events = POLLIN;
	pfds[2].fd = upstream_fd;
	pfds[2].events = POLLIN;

	fprintf(stderr, "Press 'q' to quit...\n");
	while (1) {
		ret = poll(pfds, 3, -1);
		if (ret < 0) {
			fprintf(stderr, "Cannot poll for fds: %m\n");
			break;
		}
		if (pfds[0].revents & POLLHUP) {
			fprintf(stderr, "Received HUP on stdin\n");
			break;
		}
		if (pfds[1].revents & POLLHUP) {
			fprintf(stderr, "Received HUP on uhid-cdev\n");
			break;
		}
		if (pfds[2].revents & POLLHUP) {
			fprintf(stderr, "Received HUP on upstream-cdev\n");
			break;
		}

		if (pfds[0].revents & POLLIN) {
			ret = keyboard(fd);
			if (ret)
				break;
		}
		if (pfds[1].revents & POLLIN) {
			ret = event(fd, upstream_fd);
			if (ret)
				break;
		}
		if (pfds[2].revents & POLLIN) {
			ret = forward_input(fd, upstream_fd);
			if (ret)
				break;
		}
	}

	fprintf(stderr, "Destroy uhid device\n");
	destroy(fd);
	return EXIT_SUCCESS;
}