From fe8b7e42a7f813842ff51f4d8f194a7c20534bcb 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/7] accelerometer: support SSC accelerometer sensor

---
 src/drivers.h          |   2 +
 src/drv-ssc-accel.c    | 113 +++++++++++++++++++++++++++++++++++++++++
 src/iio-sensor-proxy.c |   1 +
 src/meson.build        |   1 +
 4 files changed, 117 insertions(+)
 create mode 100644 src/drv-ssc-accel.c

diff --git a/src/drivers.h b/src/drivers.h
index a97528b..a68fcbb 100644
--- a/src/drivers.h
+++ b/src/drivers.h
@@ -163,6 +163,8 @@ extern SensorDriver iio_poll_proximity;
 #ifdef HAS_LIBSSC
 extern SensorDriver ssc_proximity;
 extern SensorDriver ssc_light;
+extern SensorDriver ssc_accel;
+extern SensorDriver ssc_compass;
 #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..a9b5eec
--- /dev/null
+++ b/src/drv-ssc-accel.c
@@ -0,0 +1,113 @@
+/*
+ * 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 <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;
+} DrvData;
+
+static gboolean
+ssc_accelerometer_discover (GUdevDevice *device)
+{
+	if (!drv_check_udev_sensor_type (device, "ssc-accel", NULL))
+		return FALSE;
+
+	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;
+	AccelReadings readings;
+
+	readings.accel_x = accel_x;
+	readings.accel_y = accel_y;
+	readings.accel_z = accel_z;
+	set_accel_scale (&readings.scale, 1.0);
+
+	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;
+
+	/* 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 (&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 f70e996..644e835 100644
--- a/src/iio-sensor-proxy.c
+++ b/src/iio-sensor-proxy.c
@@ -76,6 +76,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 104143c..e44338b 100644
--- a/src/meson.build
+++ b/src/meson.build
@@ -37,6 +37,7 @@ sources = [
   'drv-iio-poll-proximity.c',
   'drv-ssc-proximity.c',
   'drv-ssc-light.c',
+  'drv-ssc-accel.c',
   'iio-buffer-utils.c',
   'accel-mount-matrix.c',
   'accel-scale.c',
-- 
2.40.0