1 files changed, 426 insertions, 0 deletions
diff --git a/src/core.c b/src/core.c
new file mode 100644
index 0000000..87ef420
--- /dev/null
+++ b/src/core.c
@@ -0,0 +1,426 @@
+/*****************************************************************************
+ *
+ * mtdev - Multitouch Protocol Translation Library (MIT license)
+ *
+ * Copyright (C) 2010 Henrik Rydberg <rydberg@euromail.se>
+ * Copyright (C) 2010 Canonical Ltd.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ ****************************************************************************/
+
+#include "state.h"
+#include "iobuf.h"
+#include "evbuf.h"
+#include "match.h"
+
+static inline int istouch(const struct mtdev_slot *data,
+			  const struct mtdev *dev)
+{
+	return data->touch_major ||
+		!mtdev_has_mt_event(dev, ABS_MT_TOUCH_MAJOR);
+}
+
+static inline int isfilled(unsigned int mask)
+{
+	return GETBIT(mask, mtdev_abs2mt(ABS_MT_POSITION_X)) &&
+		GETBIT(mask, mtdev_abs2mt(ABS_MT_POSITION_Y));
+}
+
+/* Response-augmented EWMA filter, courtesy of Vojtech Pavlik */
+static int defuzz(int value, int old_val, int fuzz)
+{
+	if (fuzz) {
+		if (value > old_val - fuzz / 2 && value < old_val + fuzz / 2)
+			return old_val;
+
+		if (value > old_val - fuzz && value < old_val + fuzz)
+			return (old_val * 3 + value) / 4;
+
+		if (value > old_val - fuzz * 2 && value < old_val + fuzz * 2)
+			return (old_val + value) / 2;
+	}
+
+	return value;
+}
+
+/*
+ * solve - solve contact matching problem
+ * @state: mtdev state
+ * @dev: device capabilities
+ * @sid: array of current tracking ids
+ * @sx: array of current position x
+ * @sy: array of current position y
+ * @sn: number of current contacts
+ * @nid: array of new or matched tracking ids, to be filled
+ * @nx: array of new position x
+ * @ny: array of new position y
+ * @nn: number of new contacts
+ * @touch: which of the new contacts to fill
+ */
+static void solve(struct mtdev_state *state, const struct mtdev *dev,
+		  const int *sid, const int *sx, const int *sy, int sn,
+		  int *nid, const int *nx, const int *ny, int nn,
+		  bitmask_t touch)
+{
+	int A[DIM2_FINGER], *row;
+	int n2s[DIM_FINGER];
+	int id, i, j;
+
+	/* setup distance matrix for contact matching */
+	for (j = 0; j < sn; j++) {
+		row = A + nn * j;
+		for (i = 0; i < nn; i++)
+			row[i] = dist2(nx[i] - sx[j], ny[i] - sy[j]);
+	}
+
+	mtdev_match(n2s, A, nn, sn);
+
+	/* update matched contacts and create new ones */
+	foreach_bit(i, touch) {
+		j = n2s[i];
+		id = j >= 0 ? sid[j] : MT_ID_NULL;
+		if (id == MT_ID_NULL)
+			id = state->lastid++ & MT_ID_MAX;
+		nid[i] = id;
+	}
+}
+
+/*
+ * assign_tracking_id - assign tracking ids to all contacts
+ * @state: mtdev state
+ * @dev: device capabilities
+ * @data: array of all present contacts, to be filled
+ * @prop: array of all set contacts properties
+ * @size: number of contacts in array
+ * @touch: which of the contacts are actual touches
+ */
+static void assign_tracking_id(struct mtdev_state *state,
+			       const struct mtdev *dev,
+			       struct mtdev_slot *data, bitmask_t *prop,
+			       int size, bitmask_t touch)
+{
+	int sid[DIM_FINGER], sx[DIM_FINGER], sy[DIM_FINGER], sn = 0;
+	int nid[DIM_FINGER], nx[DIM_FINGER], ny[DIM_FINGER], i;
+	foreach_bit(i, state->used) {
+		sid[sn] = state->data[i].tracking_id;
+		sx[sn] = state->data[i].position_x;
+		sy[sn] = state->data[i].position_y;
+		sn++;
+	}
+	for (i = 0; i < size; i++) {
+		nx[i] = data[i].position_x;
+		ny[i] = data[i].position_y;
+	}
+	solve(state, dev, sid, sx, sy, sn, nid, nx, ny, size, touch);
+	for (i = 0; i < size; i++) {
+		data[i].tracking_id = GETBIT(touch, i) ? nid[i] : MT_ID_NULL;
+		SETBIT(prop[i], mtdev_abs2mt(ABS_MT_TRACKING_ID));
+	}
+}
+
+/*
+ * process_typeA - consume MT events and update mtdev state
+ * @state: mtdev state
+ * @data: array of all present contacts, to be filled
+ * @prop: array of all set contacts properties, to be filled
+ *
+ * This function is called when a SYN_REPORT is seen, right before
+ * that event is pushed to the queue.
+ *
+ * Returns -1 if the packet is not MT related and should not affect
+ * the current mtdev state.
+ */
+static int process_typeA(struct mtdev_state *state,
+			 struct mtdev_slot *data, bitmask_t *prop)
+{
+	struct input_event ev;
+	int consumed, mtcode;
+	int mtcnt = 0, size = 0;
+	prop[size] = 0;
+	while (!evbuf_empty(&state->inbuf)) {
+		evbuf_get(&state->inbuf, &ev);
+		consumed = 0;
+		switch (ev.type) {
+		case EV_SYN:
+			switch (ev.code) {
+			case SYN_MT_REPORT:
+				if (size < DIM_FINGER && isfilled(prop[size]))
+					size++;
+				if (size < DIM_FINGER)
+					prop[size] = 0;
+				mtcnt++;
+				consumed = 1;
+				break;
+			}
+			break;
+		case EV_KEY:
+			switch (ev.code) {
+			case BTN_TOUCH:
+				mtcnt++;
+				break;
+			}
+			break;
+		case EV_ABS:
+			if (size < DIM_FINGER && mtdev_is_absmt(ev.code)) {
+				mtcode = mtdev_abs2mt(ev.code);
+				set_sval(&data[size], mtcode, ev.value);
+				SETBIT(prop[size], mtcode);
+				mtcnt++;
+				consumed = 1;
+			}
+			break;
+		}
+		if (!consumed)
+			evbuf_put(&state->outbuf, &ev);
+	}
+	return mtcnt ? size : -1;
+}
+
+/*
+ * process_typeB - propagate events without parsing
+ * @state: mtdev state
+ *
+ * This function is called when a SYN_REPORT is seen, right before
+ * that event is pushed to the queue.
+ */
+static void process_typeB(struct mtdev_state *state)
+{
+	struct input_event ev;
+	while (!evbuf_empty(&state->inbuf)) {
+		evbuf_get(&state->inbuf, &ev);
+		evbuf_put(&state->outbuf, &ev);
+	}
+}
+
+/*
+ * filter_data - apply input filtering on new incoming data
+ * @state: mtdev state
+ * @dev: device capabilities
+ * @data: the incoming data to filter
+ * @prop: the properties to filter
+ * @slot: the slot the data refers to
+ */
+static void filter_data(const struct mtdev_state *state,
+			const struct mtdev *dev,
+			struct mtdev_slot *data, bitmask_t prop,
+			int slot)
+{
+	int i;
+	foreach_bit(i, prop) {
+		int fuzz = mtdev_get_abs_fuzz(dev, mtdev_mt2abs(i));
+		int oldval = get_sval(&state->data[slot], i);
+		int value = get_sval(data, i);
+		set_sval(data, i, defuzz(value, oldval, fuzz));
+	}
+}
+
+/*
+ * push_slot_changes - propagate state changes
+ * @state: mtdev state
+ * @data: the incoming data to propagate
+ * @prop: the properties to propagate
+ * @slot: the slot the data refers to
+ * @syn: reference to the SYN_REPORT event
+ */
+static void push_slot_changes(struct mtdev_state *state,
+			      const struct mtdev_slot *data, bitmask_t prop,
+			      int slot, const struct input_event *syn)
+{
+	struct input_event ev;
+	int i, count = 0;
+	foreach_bit(i, prop)
+		if (get_sval(&state->data[slot], i) != get_sval(data, i))
+			count++;
+	if (!count)
+		return;
+	ev.time = syn->time;
+	ev.type = EV_ABS;
+	ev.code = ABS_MT_SLOT;
+	ev.value = slot;
+	if (state->slot != ev.value) {
+		evbuf_put(&state->outbuf, &ev);
+		state->slot = ev.value;
+	}
+	foreach_bit(i, prop) {
+		ev.code = mtdev_mt2abs(i);
+		ev.value = get_sval(data, i);
+		if (get_sval(&state->data[slot], i) != ev.value) {
+			evbuf_put(&state->outbuf, &ev);
+			set_sval(&state->data[slot], i, ev.value);
+		}
+	}
+}
+
+/*
+ * apply_typeA_changes - parse and propagate state changes
+ * @state: mtdev state
+ * @dev: device capabilities
+ * @data: array of data to apply
+ * @prop: array of properties to apply
+ * @size: number of contacts in array
+ * @syn: reference to the SYN_REPORT event
+ */
+static void apply_typeA_changes(struct mtdev_state *state,
+				const struct mtdev *dev,
+				struct mtdev_slot *data, const bitmask_t *prop,
+				int size, const struct input_event *syn)
+{
+	bitmask_t unused = ~state->used;
+	bitmask_t used = 0;
+	int i, slot, id;
+	for (i = 0; i < size; i++) {
+		id = data[i].tracking_id;
+		foreach_bit(slot, state->used) {
+			if (state->data[slot].tracking_id != id)
+				continue;
+			filter_data(state, dev, &data[i], prop[i], slot);
+			push_slot_changes(state, &data[i], prop[i], slot, syn);
+			SETBIT(used, slot);
+			id = MT_ID_NULL;
+			break;
+		}
+		if (id != MT_ID_NULL) {
+			slot = firstbit(unused);
+			push_slot_changes(state, &data[i], prop[i], slot, syn);
+			SETBIT(used, slot);
+			CLEARBIT(unused, slot);
+		}
+	}
+
+	/* clear unused slots and update slot usage */
+	foreach_bit(slot, state->used & ~used) {
+		struct mtdev_slot tdata = state->data[slot];
+		bitmask_t tprop = BITMASK(mtdev_abs2mt(ABS_MT_TRACKING_ID));
+		tdata.tracking_id = MT_ID_NULL;
+		push_slot_changes(state, &tdata, tprop, slot, syn);
+	}
+	state->used = used;
+}
+
+/*
+ * convert_A_to_B - propagate a type A packet as a type B packet
+ * @state: mtdev state
+ * @dev: device capabilities
+ * @syn: reference to the SYN_REPORT event
+ */
+static void convert_A_to_B(struct mtdev_state *state,
+			   const struct mtdev *dev,
+			   const struct input_event *syn)
+{
+	struct mtdev_slot data[DIM_FINGER];
+	bitmask_t prop[DIM_FINGER];
+	int size = process_typeA(state, data, prop);
+	if (size < 0)
+		return;
+	if (!mtdev_has_mt_event(dev, ABS_MT_TRACKING_ID)) {
+		bitmask_t touch = 0;
+		int i;
+		for (i = 0; i < size; i++)
+			MODBIT(touch, i, istouch(&data[i], dev));
+		assign_tracking_id(state, dev, data, prop, size, touch);
+	}
+	apply_typeA_changes(state, dev, data, prop, size, syn);
+}
+
+struct mtdev *mtdev_new(void)
+{
+	return calloc(1, sizeof(struct mtdev));
+}
+
+int mtdev_init(struct mtdev *dev)
+{
+	int i;
+	memset(dev, 0, sizeof(struct mtdev));
+	dev->state = calloc(1, sizeof(struct mtdev_state));
+	if (!dev->state)
+		return -ENOMEM;
+	for (i = 0; i < DIM_FINGER; i++)
+		dev->state->data[i].tracking_id = MT_ID_NULL;
+	return 0;
+}
+
+int mtdev_open(struct mtdev *dev, int fd)
+{
+	int ret = -EINVAL;
+
+	if (!dev || fd < 0)
+		goto error;
+	ret = mtdev_init(dev);
+	if (ret)
+		goto error;
+	ret = mtdev_configure(dev, fd);
+	if (ret)
+		goto error;
+	return 0;
+
+ error:
+	mtdev_close(dev);
+	return ret;
+}
+
+struct mtdev *mtdev_new_open(int fd)
+{
+	struct mtdev *dev;
+
+	dev = mtdev_new();
+	if (!dev)
+		return NULL;
+	if (!mtdev_open(dev, fd))
+		return dev;
+
+	mtdev_delete(dev);
+	return NULL;
+}
+
+void mtdev_put_event(struct mtdev *dev, const struct input_event *ev)
+{
+	struct mtdev_state *state = dev->state;
+	if (ev->type == EV_SYN && ev->code == SYN_REPORT) {
+		bitmask_t head = state->outbuf.head;
+		if (mtdev_has_mt_event(dev, ABS_MT_SLOT))
+			process_typeB(state);
+		else
+			convert_A_to_B(state, dev, ev);
+		if (state->outbuf.head != head)
+			evbuf_put(&state->outbuf, ev);
+	} else {
+		evbuf_put(&state->inbuf, ev);
+	}
+}
+
+void mtdev_close_delete(struct mtdev *dev)
+{
+	mtdev_close(dev);
+	mtdev_delete(dev);
+}
+
+void mtdev_close(struct mtdev *dev)
+{
+	if (dev) {
+		free(dev->state);
+		memset(dev, 0, sizeof(struct mtdev));
+	}
+}
+
+void mtdev_delete(struct mtdev *dev)
+{
+	free(dev);
+}