diff --git a/src/drivers.h b/src/drivers.h index b760faa..33ad667 100644 --- a/src/drivers.h +++ b/src/drivers.h @@ -159,5 +159,6 @@ extern SensorDriver hwmon_light; extern SensorDriver iio_buffer_light; extern SensorDriver iio_buffer_compass; extern SensorDriver iio_poll_proximity; +extern SensorDriver input_proximity; gboolean drv_check_udev_sensor_type (GUdevDevice *device, const gchar *match, const char *name); diff --git a/src/drv-input-proximity.c b/src/drv-input-proximity.c new file mode 100644 index 0000000..025efff --- /dev/null +++ b/src/drv-input-proximity.c @@ -0,0 +1,261 @@ + /* + * Copyright (C) 2009 Richard Hughes + * Copyright (C) 2023 Sicelo A. Mhlongo + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as published by + * the Free Software Foundation. + */ + +#include "drivers.h" +#include +#include +#include +#include +#include + +#define BITS_PER_LONG (sizeof(long) * 8) +#define NBITS(x) ((((x)-1)/BITS_PER_LONG)+1) +#define OFF(x) ((x)%BITS_PER_LONG) +#define BIT(x) (1UL<> OFF(bit)) & 1) + +typedef struct DrvData { + GUdevDevice *dev; + gchar *native_path; + int last_switch_state; + GIOChannel *channel; + struct input_event event; + gsize offset; + guint watcher_id; +} DrvData; + +/** + * input_str_to_bitmask: + * @s: string representation of a hexadecimal value + * @bitmask: destination array to store the binary representation of the + * input string + * @max_size: the size of the allocated bitmask array + * + * Convert a hexadecimal value represented as a string into its binary + * equivalent. The binary value is stored in the bitmask array + * + * Returns: the number of bits that are set in the resulting bitmask + */ +static gint +input_str_to_bitmask (const gchar *s, glong *bitmask, size_t max_size) +{ + gint i, j; + g_auto(GStrv) v = NULL; + gint num_bits_set = 0; + + memset (bitmask, 0, max_size); + v = g_strsplit (s, " ", max_size); + for (i = g_strv_length (v) - 1, j = 0; i >= 0; i--, j++) { + gulong val; + + val = strtoul (v[i], NULL, 16); + bitmask[j] = val; + + while (val != 0) { + num_bits_set++; + val &= (val - 1); + } + } + + return num_bits_set; +} + +static gboolean +input_device_is_switch (GUdevDevice *device, glong *bitmask) +{ + gboolean ret = FALSE; + g_autofree gchar *contents = NULL; + g_autofree gchar *native_path = NULL; + g_autofree gchar *path; + g_autoptr(GError) error = NULL; + gint num_bits; + + /* check if the input device has switch capabilities */ + native_path = g_strdup (g_udev_device_get_sysfs_path (device)); + path = g_build_filename (native_path, "../capabilities/sw", NULL); + if (!g_file_test (path, G_FILE_TEST_EXISTS)) { + g_warning_once ("Not a switch [%s]", path); + return FALSE; + } + + ret = g_file_get_contents (path, &contents, NULL, &error); + if (!ret) { + g_warning_once ("Failed to read contents of [%s]: %s", + path, error->message); + return FALSE; + } + + /* convert attributes of input device into a bitmask */ + num_bits = input_str_to_bitmask (contents, bitmask, sizeof (bitmask)); + if ((num_bits == 0) || (num_bits >= SW_CNT)) { + g_warning_once ("Invalid bitmask entry for %s", native_path); + return FALSE; + } + return TRUE; +} + +static gboolean +proximity_changed (GIOChannel *channel, GIOCondition condition, gpointer userdata) { + g_autoptr(GError) error = NULL; + gsize read_bytes; + SensorDevice *sensor_device = userdata; + DrvData *drv_data = (DrvData *) sensor_device->priv; + glong bitmask[NBITS(SW_MAX)]; + ProximityReadings readings; + + while (g_io_channel_read_chars (channel, + ((gchar*)&drv_data->event) + drv_data->offset, + sizeof(struct input_event) - drv_data->offset, + &read_bytes, &error) == G_IO_STATUS_NORMAL) { + + /* check if the event data was completely read */ + if (drv_data->offset + read_bytes < sizeof (struct input_event)) { + drv_data->offset = drv_data->offset + read_bytes; + g_warning ("Incomplete data was read"); + return TRUE; + } + drv_data->offset = 0; + + if (drv_data->event.type != EV_SW) { + continue; + } + + if (drv_data->event.code != SW_FRONT_PROXIMITY) { + continue; + } + + /* check proximity sensor/switch state */ + if (ioctl (g_io_channel_unix_get_fd(channel), EVIOCGSW(sizeof (bitmask)), bitmask) < 0) { + g_warning ("Could not determine proximity sensor state, ioctl EVIOCGSW failed"); + continue; + } + + drv_data->last_switch_state = test_bit (drv_data->event.code, bitmask); + readings.is_near = drv_data->last_switch_state ? PROXIMITY_NEAR_TRUE : PROXIMITY_NEAR_FALSE; + sensor_device->callback_func (sensor_device, (gpointer) &readings, sensor_device->user_data); + } + return TRUE; +} + +static gboolean +watch_input_proximity (gpointer user_data) +{ + SensorDevice *sensor_device = user_data; + DrvData *drv_data = (DrvData *) sensor_device->priv; + int eventfd; + const g_autofree gchar *device_file; + g_autoptr(GError) error = NULL; + GIOStatus status; + glong bitmask[NBITS(SW_MAX)]; + + device_file = g_udev_device_get_device_file (drv_data->dev); + if (device_file == NULL || device_file[0] == '\0') { + return FALSE; + } + + eventfd = open (device_file, O_RDONLY | O_NONBLOCK); + if (eventfd < 0) { + return FALSE; + } + + /* get initial state of sensor */ + if (ioctl (eventfd, EVIOCGSW(sizeof (bitmask)), bitmask) < 0) { + g_warning ("Could not determine sensor state, ioctl EVIOCGSW on %s failed", drv_data->native_path); + close (eventfd); + return FALSE; + } + + /* configure I/O channel and watch it for events */ + g_debug ("Watching %s (%i)", device_file, eventfd); + drv_data->channel = g_io_channel_unix_new (eventfd); + g_io_channel_set_close_on_unref (drv_data->channel, TRUE); + + status = g_io_channel_set_encoding (drv_data->channel, NULL, &error); + if (status != G_IO_STATUS_NORMAL) { + g_warning ("Failed to set encoding for binary data: %s", error->message); + return FALSE; + } + + drv_data->watcher_id = g_io_add_watch (drv_data->channel, G_IO_IN, proximity_changed, sensor_device); + drv_data->last_switch_state = test_bit (SW_FRONT_PROXIMITY, bitmask); + return TRUE; +} + +static gboolean +input_proximity_discover (GUdevDevice *device) +{ + glong bitmask[NBITS(SW_MAX)]; + + if (!input_device_is_switch (device, bitmask)) + return FALSE; + + /* is this SW_FRONT_PROXIMITY? */ + if (!test_bit (SW_FRONT_PROXIMITY, bitmask)) + return FALSE; + + /* Input proximity sensor found */ + g_debug ("Found input proximity sensor at %s", + g_udev_device_get_sysfs_path (device)); + return TRUE; +} + +static void +input_proximity_set_polling (SensorDevice *sensor_device, gboolean state) +{ + DrvData *drv_data = (DrvData *) sensor_device->priv; + + if (drv_data->watcher_id > 0 && state) + return; + + if (drv_data->watcher_id == 0 && !state) + return; + + g_clear_handle_id (&drv_data->watcher_id, g_source_remove); + if (state) { + /* start watching for proximity events */ + watch_input_proximity (sensor_device); + } +} + +static SensorDevice * +input_proximity_open (GUdevDevice *device) +{ + SensorDevice *sensor_device; + DrvData *drv_data; + + sensor_device = g_new0 (SensorDevice, 1); + sensor_device->name = g_strdup (g_udev_device_get_name (device)); + sensor_device->priv = g_new0 (DrvData, 1); + drv_data = (DrvData *) sensor_device->priv; + + drv_data->dev = g_object_ref (device); + drv_data->native_path = g_strdup (g_udev_device_get_sysfs_path (device)); + + return sensor_device; +} + +static void +input_proximity_close (SensorDevice *sensor_device) +{ + DrvData *drv_data = (DrvData *) sensor_device->priv; + + g_clear_object (&drv_data->dev); + g_clear_pointer (&sensor_device->priv, g_free); + g_free (sensor_device); +} + +SensorDriver input_proximity = { + .driver_name = "Input proximity", + .type = DRIVER_TYPE_PROXIMITY, + .discover = input_proximity_discover, + .open = input_proximity_open, + .close = input_proximity_close, + .set_polling = input_proximity_set_polling, +}; diff --git a/src/iio-sensor-proxy.c b/src/iio-sensor-proxy.c index 3f27a53..1884839 100644 --- a/src/iio-sensor-proxy.c +++ b/src/iio-sensor-proxy.c @@ -74,6 +74,7 @@ static const SensorDriver * const drivers[] = { &fake_light, &iio_buffer_compass, &iio_poll_proximity, + &input_proximity, }; static ReadingsUpdateFunc driver_type_to_callback_func (DriverType type); diff --git a/src/meson.build b/src/meson.build index 68af38f..4b822fc 100644 --- a/src/meson.build +++ b/src/meson.build @@ -28,6 +28,7 @@ sources = [ 'drv-iio-buffer-light.c', 'drv-iio-buffer-compass.c', 'drv-iio-poll-proximity.c', + 'drv-input-proximity.c', 'iio-buffer-utils.c', 'accel-mount-matrix.c', 'accel-scale.c', diff --git a/tests/input-proximity-data/device b/tests/input-proximity-data/device new file mode 100644 index 0000000..69b7ad6 --- /dev/null +++ b/tests/input-proximity-data/device @@ -0,0 +1,85 @@ +P: /devices/platform/gpio_keys/input/input4/event4 +N: input/event4 +S: input/by-path/platform-gpio_keys-event +E: DEVLINKS=/dev/input/by-path/platform-gpio_keys-event +E: DEVNAME=/dev/input/event4 +E: ID_INPUT=1 +E: ID_INPUT_KEY=1 +E: ID_INPUT_SWITCH=1 +E: ID_PATH=platform-gpio_keys +E: ID_PATH_TAG=platform-gpio_keys +E: LIBINPUT_DEVICE_GROUP=19/1/1:gpio-keys +E: MAJOR=13 +E: MINOR=68 +E: SUBSYSTEM=input +E: TAGS=:power-switch: +A: dev=13:68\n +L: device=../../input4 +A: power/control=auto\n +A: power/runtime_active_time=0\n +A: power/runtime_status=unsupported\n +A: power/runtime_suspended_time=0\n + +P: /devices/platform/gpio_keys/input/input4 +E: EV=23 +E: ID_FOR_SEAT=input-platform-gpio_keys +E: ID_INPUT=1 +E: ID_INPUT_KEY=1 +E: ID_INPUT_SWITCH=1 +E: ID_PATH=platform-gpio_keys +E: ID_PATH_TAG=platform-gpio_keys +E: KEY=10000 0 0 0 0 0 0 0 0 0 100000 0 1000000 0 0 0 0 +E: MODALIAS=input:b0019v0001p0001e0100-e0,1,5,k98,D4,210,ramlsfw9,A,B, +E: NAME="gpio_keys" +E: PHYS="gpio-keys/input0" +E: PRODUCT=19/1/1/100 +E: PROP=0 +E: SUBSYSTEM=input +E: SW=10e00 +E: TAGS=:seat: +A: capabilities/abs=0\n +A: capabilities/ev=23\n +A: capabilities/ff=0\n +A: capabilities/key=10000 0 0 0 0 0 0 0 0 0 100000 0 1000000 0 0 0 0\n +A: capabilities/led=0\n +A: capabilities/msc=0\n +A: capabilities/rel=0\n +A: capabilities/snd=0\n +A: capabilities/sw=10e00\n +L: device=../../../gpio_keys +A: id/bustype=0019\n +A: id/product=0001\n +A: id/vendor=0001\n +A: id/version=0100\n +A: inhibited=0\n +A: modalias=input:b0019v0001p0001e0100-e0,1,5,k98,D4,210,ramlsfw9,A,B,\n +A: name=gpio_keys\n +A: phys=gpio-keys/input0\n +A: power/control=auto\n +A: power/runtime_active_time=0\n +A: power/runtime_status=unsupported\n +A: power/runtime_suspended_time=0\n +A: properties=0\n +A: uniq=\n + +P: /devices/platform/gpio_keys +E: DRIVER=gpio-keys +E: MODALIAS=of:Ngpio_keysT(null)Cgpio-keys +E: OF_COMPATIBLE_0=gpio-keys +E: OF_COMPATIBLE_N=1 +E: OF_FULLNAME=/gpio_keys +E: OF_NAME=gpio_keys +E: SUBSYSTEM=platform +A: disabled_keys=\n +A: disabled_switches=\n +L: driver=../../../bus/platform/drivers/gpio-keys +A: driver_override=(null)\n +A: keys=152,212,528\n +A: modalias=of:Ngpio_keysT(null)Cgpio-keys\n +L: of_node=../../../firmware/devicetree/base/gpio_keys +A: power/control=auto\n +A: power/runtime_active_time=0\n +A: power/runtime_status=unsupported\n +A: power/runtime_suspended_time=0\n +A: switches=9-11,16\n + diff --git a/tests/input-proximity-data/ioctl b/tests/input-proximity-data/ioctl new file mode 100644 index 0000000..2827621 --- /dev/null +++ b/tests/input-proximity-data/ioctl @@ -0,0 +1,3 @@ +@DEV /dev/input/event4 +EVIOCGSW(0) 4 00020100 +EVIOCGSW(0) 4 000A0100 diff --git a/tests/integration-test.py b/tests/integration-test.py index 694eb12..20c0e92 100755 --- a/tests/integration-test.py +++ b/tests/integration-test.py @@ -457,6 +457,20 @@ class Tests(dbusmock.DBusTestCase): self.stop_daemon() + def test_input_proximity(self): + '''input proximity''' + top_srcdir = os.getenv('top_srcdir', '.') + script = ['umockdev-run', '-d', top_srcdir + '/tests/input-proximity-data/device', + '-i', '/dev/input/event4=%s/tests/input-proximity-data/ioctl' % (top_srcdir), + '--'] + + self.start_daemon(wrapper=script) + + self.assertEqual(self.get_dbus_property('HasProximity'), True) + self.assertEqual(self.get_dbus_property('ProximityNear'), False) + + self.stop_daemon() + def test_iio_buffer_accel(self): '''iio buffer accel''' top_srcdir = os.getenv('top_srcdir', '.')