path: root/libgamepad_open_device.c

/* See LICENSE file for copyright and license details. */
#include "common.h"

#define ELEMS_REQUIRED(BITS, TYPE) ((BITS + BITSOF(TYPE) - 1) / BITSOF(TYPE))
#define MAX2(A, B) ((A) > (B) ? (A) : (B))
#define MAX4(A, B, C, D) (MAX2(MAX2(A, B), MAX2(C, D)))


static void
store_uint(uint64_t value, char *buf)
{
	buf[0] = (char)((value >> 0) & 255);
	buf[1] = (char)((value >> 8) & 255);
	buf[2] = (char)((value >> 16) & 255);
	buf[3] = (char)((value >> 24) & 255);
	buf[4] = (char)((value >> 32) & 255);
	buf[5] = (char)((value >> 40) & 255);
	buf[6] = (char)((value >> 48) & 255);
	buf[7] = (char)((value >> 56) & 255);
}


LIBGAMEPAD_CONST__
static size_t
popcount(uint8_t value)
{
	size_t n = 0;
	for (; value; value >>= 1)
		if (value & 1)
			n += 1;
	return n;
}


static int
get_fingerprint(struct libgamepad_device *device, char non_unique[65], char unique[65])
{
	struct libsha2_state s, s2;
	char buf[128];
	size_t i, n;

	if (libsha2_init(&s, LIBSHA2_256))
		return -1;

	libsha2_update(&s, device->name, 8 * strlen(device->name) + 8);
	libsha2_update(&s, device->physical_location, 8 * strlen(device->physical_location) + 8);

	store_uint(device->vendor, &buf[0]);
	store_uint(device->product, &buf[8]);
	store_uint(device->version, &buf[16]);
	libsha2_update(&s, buf, 3 * 64);

	store_uint(device->nbuttons, buf);
	for (n = 8, i = 0; i < ELEMSOF(device->button_map); i++) {
		if (device->button_map[i] >= 0) {
			store_uint((uint64_t)i, &buf[n]);
			n += 8;
			if (n == sizeof(buf)) {
				libsha2_update(&s, buf, 8 * n);
				n = 0;
			}
		}
	}
	libsha2_update(&s, buf, 8 * n);
	n = 0;

	store_uint(device->nabsolute_axes, buf);
	for (n = 8, i = 0; i < ELEMSOF(device->absolute_axis_map); i++) {
		if (device->absolute_axis_map[i] >= 0) {
			store_uint((uint64_t)i, &buf[n]);
			n += 8;
			if (n == sizeof(buf)) {
				libsha2_update(&s, buf, 8 * n);
				n = 0;
			}
		}
	}
	libsha2_update(&s, buf, 8 * n);
	n = 0;

	store_uint(device->nrelative_axes, buf);
	for (n = 8, i = 0; i < ELEMSOF(device->relative_axis_map); i++) {
		if (device->relative_axis_map[i] >= 0) {
			store_uint((uint64_t)i, &buf[n]);
			n += 8;
			if (n == sizeof(buf)) {
				libsha2_update(&s, buf, 8 * n);
				n = 0;
			}
		}
	}
	libsha2_update(&s, buf, 8 * n);
	n = 0;

	n = 0;
	for (i = 0; i < ELEMSOF(device->force_feedback_support); i++)
		if (device->force_feedback_support[i])
			n += popcount(device->force_feedback_support[i]);
	store_uint((uint64_t)n, buf);
	for (n = 8, i = 0; i < BITSOF(device->force_feedback_support); i++) {
		if (GETBIT(device->force_feedback_support, i)) {
			store_uint((uint64_t)i, &buf[n]);
			n += 8;
			if (n == sizeof(buf)) {
				libsha2_update(&s, buf, 8 * n);
				n = 0;
			}
		}
	}
	libsha2_update(&s, buf, 8 * n);
	n = 0;

	memcpy(&s2, &s, sizeof(s));
	libsha2_update(&s2, device->unique_id, 8 * strlen(device->unique_id) + 8);

	libsha2_digest(&s, NULL, 0, buf);
	libsha2_behex_lower(non_unique, buf, 32);
	libsha2_digest(&s2, NULL, 0, buf);
	libsha2_behex_lower(unique, buf, 32);
	return 0;
}


int
libgamepad_open_device(struct libgamepad_device *devicep, int dirfd, const char *path, int mode)
{
	unsigned long int bits[ELEMS_REQUIRED(MAX4(ELEMSOF(devicep->button_map),
	                                           ELEMSOF(devicep->absolute_axis_map),
	                                           ELEMSOF(devicep->relative_axis_map),
	                                           ELEMSOF(devicep->force_feedback_support)), long int)];
	int err, r;
	int16_t j;
	uint16_t n;
	unsigned int i, max;
	struct input_id id;
	char *buf = NULL;
	size_t bufsize = 0;
	void *new;

	memset(devicep, 0, sizeof(*devicep));
	devicep->fd = dirfd;

	devicep->internals = calloc(1, sizeof(*devicep->internals));
	if (!devicep->internals)
		return -1;


	if (path && *path) {
		devicep->fd = openat(dirfd, path, mode);
		if (devicep->fd < 0)
			goto fail;
		devicep->internals->close_fd = 1;
	}


	if (ioctl(devicep->fd, EVIOCGID, &id) < 0)
		goto fail;
	devicep->bus_type = (unsigned int)id.bustype;
	devicep->vendor   = (unsigned int)id.vendor;
	devicep->product  = (unsigned int)id.product;
	devicep->version  = (unsigned int)id.version;


	err = libevdev_new_from_fd(devicep->fd, &devicep->internals->dev);
	if (err < 0) {
		libgamepad_close_device(devicep);
		errno = -err;
		return -1;
	}


#define GET_STRING(EVIOCMACRO, OUTPUT)\
	do {\
		for (;;) {\
			r = ioctl(devicep->fd, EVIOCMACRO(bufsize), buf);\
			if (r < 0)\
				goto fail_free_buf;\
			if ((size_t)r < bufsize)\
				break;\
			new = realloc(buf, bufsize += 32);\
			if (!new)\
				goto fail_free_buf;\
			buf = new;\
		}\
		buf[r] = '\0';\
		*OUTPUT = strdup(buf);\
		if (!*OUTPUT)\
			goto fail_free_buf;\
	} while (0)
	GET_STRING(EVIOCGNAME, &devicep->name);
	GET_STRING(EVIOCGUNIQ, &devicep->unique_id);
	GET_STRING(EVIOCGPHYS, &devicep->physical_location);
	free(buf);


#define CREATE_MAPS(EVENTS, SINGULAR, PLURAL)\
	do {\
		/* Code-to-index map */\
		r = ioctl(devicep->fd, EVIOCGBIT(EVENTS, sizeof(bits)), bits);\
		if (r < 0)\
			goto fail;\
		max = (unsigned int)r * 8;\
		devicep->n##PLURAL = 0;\
		for (i = 0; i < ELEMSOF(devicep->SINGULAR##_map); i++) {\
			devicep->SINGULAR##_map[i] = -1;\
			if (i < max && ((bits[i / BITSOF(*bits)] >> (i % BITSOF(*bits))) & 1))\
				devicep->SINGULAR##_map[i] = (int16_t)devicep->n##PLURAL++;\
		}\
		\
		/* Index-to-code map */\
		if (devicep->n##PLURAL) {\
			devicep->PLURAL = calloc(devicep->n##PLURAL, sizeof(*devicep->PLURAL));\
			if (!devicep->PLURAL)\
				goto fail;\
			for (i = 0, n = 0; i < ELEMSOF(devicep->SINGULAR##_map); i++)\
				if (devicep->SINGULAR##_map[i] >= 0)\
					devicep->PLURAL[n++] = (uint16_t)i;\
		}\
	} while (0)

	CREATE_MAPS(EV_KEY, button, buttons);
	CREATE_MAPS(EV_ABS, absolute_axis, absolute_axes);
	CREATE_MAPS(EV_REL, relative_axis, relative_axes);


	if (devicep->nabsolute_axes) {
		devicep->internals->absinfo = calloc(devicep->nabsolute_axes, sizeof(*devicep->internals->absinfo));
		if (!devicep->internals->absinfo)
			goto fail;
	}
	for (i = 0; i < devicep->nabsolute_axes; i++)
		if (ioctl(devicep->fd, EVIOCGABS((unsigned int)devicep->absolute_axes[i]), &devicep->internals->absinfo[i]) < 0)
			goto fail;


	if (devicep->nbuttons) {
		devicep->internals->buttons = calloc((devicep->nbuttons + 7) / 8, sizeof(*devicep->internals->buttons));
		if (!devicep->internals->buttons)
			goto fail;
		r = ioctl(devicep->fd, EVIOCGKEY(sizeof(bits)), bits);
		if (r < 0)
			goto fail;
		max = (unsigned int)r * 8;
		for (i = 0; i < max && i < ELEMSOF(devicep->button_map); i++) {
			j = devicep->button_map[i];
			if (j >= 0 && GETBIT(bits, i))
				SETBIT(devicep->internals->buttons, (uint16_t)j);
		}
	}


	r = ioctl(devicep->fd, EVIOCGBIT(EV_FF, sizeof(bits)), bits);
	if (r < 0)
		goto fail;
	max = (unsigned int)r * 8;
	for (i = 0; i < BITSOF(devicep->force_feedback_support); i++)
		if (i < max && ((bits[i / BITSOF(*bits)] >> (i % BITSOF(*bits))) & 1))
			SETBIT(devicep->force_feedback_support, i);


	if (get_fingerprint(devicep, devicep->fingerprint, devicep->fingerprint_unique))
		goto fail;

	/* TODO get actual KEY/ABS state */

	return 0;

fail_free_buf:
	free(buf);
fail:
	libgamepad_close_device(devicep);
	return -1;
}