pmaports/temp/iio-sensor-proxy/0004-accelerometer-support-SSC-accelerometer-sensor.patch

From cdc8a661bdeb7f0ada6b08beb6ceee40188d8843 Mon Sep 17 00:00:00 2001
From: Dylan Van Assche <me@dylanvanassche.be>
Date: Mon, 24 Apr 2023 17:01:10 +0200
Subject: [PATCH 4/8] 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    | 145 +++++++++++++++++++++++++++++++++++++++++
 src/iio-sensor-proxy.c |   1 +
 src/meson.build        |   1 +
 4 files changed, 148 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..26364db
--- /dev/null
+++ b/src/drv-ssc-accel.c
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2023 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 <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <gio/gio.h>
+#include <libssc-sensor.h>
+#include <libssc-sensor-accelerometer.h>
+
+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))
+		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 ? error->message : "UNKNOWN");
+		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 ? error->message : "UNKNOWN");
+		g_object_unref (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 ? error->message : "UNKNOWN");
+
+	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 77a75ab..6c0e96f 100644
--- a/src/iio-sensor-proxy.c
+++ b/src/iio-sensor-proxy.c
@@ -78,6 +78,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.45.1