From 8c296891214b6b7ebbd3263550ef9296e9a9ff1f Mon Sep 17 00:00:00 2001 From: Dylan Van Assche Date: Mon, 24 Apr 2023 17:01:10 +0200 Subject: [PATCH 04/10] accelerometer: support SSC accelerometer sensor Add driver for the accelerometer sensor exposed by libssc. Mount matrix is supported through environment variables set by udev rules because extracting the mount matrix from the Qualcomm Snapdragon Sensor Core (SSC) firmware is not possible yet. --- src/drivers.h | 1 + src/drv-ssc-accel.c | 153 +++++++++++++++++++++++++++++++++++++++++ src/iio-sensor-proxy.c | 1 + src/meson.build | 1 + 4 files changed, 156 insertions(+) create mode 100644 src/drv-ssc-accel.c diff --git a/src/drivers.h b/src/drivers.h index 46df2fd..40707ec 100644 --- a/src/drivers.h +++ b/src/drivers.h @@ -164,6 +164,7 @@ extern SensorDriver input_proximity; #ifdef HAS_LIBSSC extern SensorDriver ssc_proximity; extern SensorDriver ssc_light; +extern SensorDriver ssc_accel; #endif gboolean drv_check_udev_sensor_type (GUdevDevice *device, const gchar *match, const char *name); diff --git a/src/drv-ssc-accel.c b/src/drv-ssc-accel.c new file mode 100644 index 0000000..676303b --- /dev/null +++ b/src/drv-ssc-accel.c @@ -0,0 +1,153 @@ +/* + * Copyright (c) 2023-2025 Dylan Van Assche + * + * 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 "accel-mount-matrix.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef struct DrvData { + SSCSensorAccelerometer *sensor; + guint measurement_id; + AccelVec3 *mount_matrix; + AccelLocation location; + AccelScale scale; +} DrvData; + +static gboolean +ssc_accelerometer_discover (GUdevDevice *device) +{ + SSCSensorAccelerometer *sensor = NULL; + + /* Verify presence of FastRPC device */ + if (!drv_check_udev_sensor_type (device, "ssc-accel", NULL)) + return FALSE; + + /* Open and close SSC accelerometer for discovering */ + sensor = ssc_sensor_accelerometer_new_sync (NULL, NULL); + if (!sensor) + return FALSE; + + if (!ssc_sensor_accelerometer_close_sync (sensor, NULL, NULL)) { + g_clear_object (&sensor); + return FALSE; + } + + g_clear_object (&sensor); + + g_debug ("Found SSC accelerometer at %s", g_udev_device_get_sysfs_path (device)); + return TRUE; +} + +static void +measurement_cb (SSCSensorAccelerometer *sensor, gfloat accel_x, gfloat accel_y, gfloat accel_z, gpointer user_data) +{ + SensorDevice *sensor_device = user_data; + DrvData *drv_data = (DrvData *) sensor_device->priv; + AccelReadings readings; + AccelVec3 tmp; + + tmp.x = accel_x; + tmp.y = accel_y; + tmp.z = accel_z; + + if (!apply_mount_matrix (drv_data->mount_matrix, &tmp)) + g_warning ("Could not apply mount matrix"); + + readings.accel_x = tmp.x; + readings.accel_y = tmp.y; + readings.accel_z = tmp.z; + copy_accel_scale (&readings.scale, drv_data->scale); + + sensor_device->callback_func (sensor_device, (gpointer) &readings, sensor_device->user_data); +} + +static SensorDevice * +ssc_accelerometer_open (GUdevDevice *device) +{ + SensorDevice *sensor_device; + DrvData *drv_data; + g_autoptr (GError) error = NULL; + + sensor_device = g_new0 (SensorDevice, 1); + sensor_device->priv = g_new0 (DrvData, 1); + drv_data = (DrvData *) sensor_device->priv; + + /* Setup accel attributes */ + drv_data->mount_matrix = setup_mount_matrix (device); + drv_data->location = setup_accel_location (device); + set_accel_scale (&drv_data->scale, 1.0); + + /* Create sensor */ + drv_data->sensor = ssc_sensor_accelerometer_new_sync (NULL, &error); + if (!drv_data->sensor) { + g_warning ("Creating SSC accelerometer sensor failed: %s", error->message); + g_clear_pointer (&drv_data->mount_matrix, g_free); + g_clear_pointer (&sensor_device->priv, g_free); + g_free (sensor_device); + return NULL; + } + g_object_get (drv_data->sensor, + SSC_SENSOR_NAME, &sensor_device->name, + NULL); + + /* Start listening for measurements */ + drv_data->measurement_id = g_signal_connect (drv_data->sensor, + "measurement", + G_CALLBACK (measurement_cb), + sensor_device); + + /* Enable sensor */ + if (!ssc_sensor_accelerometer_open_sync (drv_data->sensor, NULL, &error)) { + g_warning ("Opening SSC accelerometer sensor failed: %s", error->message); + g_clear_object (&drv_data->sensor); + g_clear_pointer (&drv_data->mount_matrix, g_free); + g_clear_pointer (&sensor_device->priv, g_free); + g_free (sensor_device); + return NULL; + } + + return sensor_device; +} + +static void +ssc_accelerometer_close (SensorDevice *sensor_device) +{ + g_autoptr (GError) error = NULL; + DrvData *drv_data = (DrvData *) sensor_device->priv; + + /* Stop listening for measurements */ + g_warn_if_fail (drv_data->measurement_id > 0); + g_signal_handler_disconnect (drv_data->sensor, drv_data->measurement_id); + + /* Disable sensor */ + if (!ssc_sensor_accelerometer_close_sync (drv_data->sensor, NULL, &error)) + g_warning ("Closing SSC accelerometer sensor failed: %s", error->message); + + g_clear_object (&drv_data->sensor); + g_clear_pointer (&drv_data->mount_matrix, g_free); + g_clear_pointer (&sensor_device->priv, g_free); + g_free (sensor_device); +} + +SensorDriver ssc_accel = { + .driver_name = "SSC accelerometer sensor", + .type = DRIVER_TYPE_ACCEL, + + .discover = ssc_accelerometer_discover, + .open = ssc_accelerometer_open, + .close = ssc_accelerometer_close, +}; diff --git a/src/iio-sensor-proxy.c b/src/iio-sensor-proxy.c index b4eacc3..e99f641 100644 --- a/src/iio-sensor-proxy.c +++ b/src/iio-sensor-proxy.c @@ -82,6 +82,7 @@ static const SensorDriver * const drivers[] = { #ifdef HAS_LIBSSC &ssc_proximity, &ssc_light, + &ssc_accel, #endif }; diff --git a/src/meson.build b/src/meson.build index 2f3c1e6..d59cfdd 100644 --- a/src/meson.build +++ b/src/meson.build @@ -49,6 +49,7 @@ if get_option('ssc-support') sources = sources + [ 'drv-ssc-proximity.c', 'drv-ssc-light.c', + 'drv-ssc-accel.c', ] endif -- 2.47.1