1 files changed, 226 insertions, 63 deletions

diff --git a/libgamepad_open_device.c b/libgamepad_open_device.c
index 3c2a751..22163a4 100644
--- a/libgamepad_open_device.c
+++ b/libgamepad_open_device.c
@@ -1,39 +1,165 @@
 /* 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)
 {
-	int err;
-	unsigned int i;
+	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 = NULL;
-	devicep->buttons = NULL;
-	devicep->absolute_axes = NULL;
-	devicep->relative_axes = NULL;
-	memset(devicep->force_feedback_support, 0, sizeof(devicep->force_feedback_support));
 
 	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) {
-			libgamepad_close_device(devicep);
-			return -1;
-		}
+		if (devicep->fd < 0)
+			goto fail;
 		devicep->internals->close_fd = 1;
 	}
 
-	if (ioctl(devicep->fd, EVIOCGID, &id)) {
-		libgamepad_close_device(devicep);
-		return -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) {
@@ -42,68 +168,105 @@ libgamepad_open_device(struct libgamepad_device *devicep, int dirfd, const char
 		return -1;
 	}
 
-	devicep->name              = libevdev_get_name(devicep->internals->dev);
-	devicep->unique_id         = libevdev_get_uniq(devicep->internals->dev);
-	devicep->physical_location = libevdev_get_phys(devicep->internals->dev);
-	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;
-
-	devicep->nbuttons = 0;
-	for (i = 0; i < KEY_CNT; i++) {
-		devicep->button_map[i] = -1;
-		if (libevdev_has_event_code(devicep->internals->dev, EV_KEY, i))
-			devicep->button_map[i] = (int16_t)devicep->nbuttons++;
-	}
 
-	devicep->nabsolute_axes = 0;
-	for (i = 0; i < ABS_CNT; i++) {
-		devicep->absolute_axis_map[i] = -1;
-		if (libevdev_has_event_code(devicep->internals->dev, EV_ABS, i))
-			devicep->absolute_axis_map[i] = (int16_t)devicep->nabsolute_axes++;
-	}
+#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);
 
-	devicep->nrelative_axes = 0;
-	for (i = 0; i < REL_CNT; i++) {
-		devicep->relative_axis_map[i] = -1;
-		if (libevdev_has_event_code(devicep->internals->dev, EV_REL, i))
-			devicep->relative_axis_map[i] = (int16_t)devicep->nrelative_axes++;
-	}
 
-	for (i = 0; i < FF_CNT; i++)
-		if (libevdev_has_event_code(devicep->internals->dev, EV_FF, i))
-			devicep->force_feedback_support[i / 8] |= (uint8_t)(1 << (i & 7));
+#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->nbuttons) {
-		devicep->buttons = calloc(devicep->nbuttons, sizeof(*devicep->buttons));
-		if (!devicep->buttons)
-			goto fail;
-		for (i = 0, n = 0; i < KEY_CNT; i++)
-			if (devicep->button_map[i] >= 0)
-				devicep->buttons[n++] = (uint16_t)i;
-	}
 
 	if (devicep->nabsolute_axes) {
-		devicep->absolute_axes = calloc(devicep->nabsolute_axes, sizeof(*devicep->absolute_axes));
-		if (!devicep->absolute_axes)
+		devicep->internals->absinfo = calloc(devicep->nabsolute_axes, sizeof(*devicep->internals->absinfo));
+		if (!devicep->internals->absinfo)
 			goto fail;
-		for (i = 0, n = 0; i < ABS_CNT; i++)
-			if (devicep->absolute_axis_map[i] >= 0)
-				devicep->absolute_axes[n++] = (uint16_t)i;
 	}
+	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->nrelative_axes) {
-		devicep->relative_axes = calloc(devicep->nrelative_axes, sizeof(*devicep->relative_axes));
-		if (!devicep->relative_axes)
+	if (devicep->nbuttons) {
+		devicep->internals->buttons = calloc((devicep->nbuttons + 7) / 8, sizeof(*devicep->internals->buttons));
+		if (!devicep->internals->buttons)
 			goto fail;
-		for (i = 0, n = 0; i < REL_CNT; i++)
-			if (devicep->relative_axis_map[i] >= 0)
-				devicep->relative_axes[n++] = (uint16_t)i;
+		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;