drv-iio-buffer-accel.c 7.18 KB
Newer Older
1
2
3
4
/*
 * Copyright (c) 2014 Bastien Nocera <hadess@hadess.net>
 *
 * This program is free software; you can redistribute it and/or modify it
5
 * under the terms of the GNU General Public License version 3 as published by
6
7
8
9
 * the Free Software Foundation.
 */

#include "drivers.h"
10
#include "iio-buffer-utils.h"
11
#include "accel-mount-matrix.h"
12
#include "utils.h"
13
14
15
16
17
18
19
20
21
22

#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>

typedef struct {
	guint              timeout_id;

	GUdevDevice *dev;
23
	char *dev_path;
24
	AccelVec3 *mount_matrix;
25
	AccelLocation location;
26
	int device_id;
27
	BufferDrvData *buffer_data;
28
29
30
} DrvData;

static int
31
process_scan (IIOSensorData data, SensorDevice *sensor_device)
32
{
33
	DrvData *drv_data = (DrvData *) sensor_device->priv;
34
35
36
	int i;
	int accel_x, accel_y, accel_z;
	gboolean present_x, present_y, present_z;
37
	AccelReadings readings;
38
	AccelVec3 tmp;
39
	AccelScale scale;
40
41

	if (data.read_size < 0) {
42
		g_warning ("Couldn't read from device '%s': %s", sensor_device->name, g_strerror (errno));
43
44
45
46
		return 0;
	}

	/* Rather than read everything:
47
	 * for (i = 0; i < data.read_size / drv_data->scan_size; i++)...
48
	 * Just read the last one */
49
	i = (data.read_size / drv_data->buffer_data->scan_size) - 1;
50
	if (i < 0) {
51
		g_debug ("Not enough data to read from '%s' (read_size: %d scan_size: %d)", sensor_device->name,
52
			 (int) data.read_size, drv_data->buffer_data->scan_size);
53
54
55
		return 0;
	}

56
57
58
	process_scan_1(data.data + drv_data->buffer_data->scan_size*i, drv_data->buffer_data, "in_accel_x", &accel_x, &scale.x, &present_x);
	process_scan_1(data.data + drv_data->buffer_data->scan_size*i, drv_data->buffer_data, "in_accel_y", &accel_y, &scale.y, &present_y);
	process_scan_1(data.data + drv_data->buffer_data->scan_size*i, drv_data->buffer_data, "in_accel_z", &accel_z, &scale.z, &present_z);
59

60
	g_debug ("Accel read from IIO on '%s': %d, %d, %d (scale %lf,%lf,%lf)", sensor_device->name,
61
62
		 accel_x, accel_y, accel_z,
		 scale.x, scale.y, scale.z);
63

64
65
66
67
	tmp.x = accel_x;
	tmp.y = accel_y;
	tmp.z = accel_z;

68
	if (!apply_mount_matrix (drv_data->mount_matrix, &tmp))
69
70
		g_warning ("Could not apply mount matrix");

71
	//FIXME report errors
72
73
74
	readings.accel_x = tmp.x;
	readings.accel_y = tmp.y;
	readings.accel_z = tmp.z;
75
	copy_accel_scale (&readings.scale, scale);
76
	sensor_device->callback_func (sensor_device, (gpointer) &readings, sensor_device->user_data);
77
78
79
80
81

	return 1;
}

static void
82
83
84
prepare_output (SensorDevice *sensor_device,
		const char   *dev_dir_name,
		const char   *trigger_name)
85
{
86
	DrvData *drv_data = (DrvData *) sensor_device->priv;
87
	g_autoptr(IIOSensorData) data = NULL;
88
	g_auto(IioFd) fp = -1;
89

90
	int buf_len = 127;
91

92
	data = iio_sensor_data_new (drv_data->buffer_data->scan_size * buf_len);
93
94

	/* Attempt to open non blocking to access dev */
95
	fp = open (drv_data->dev_path, O_RDONLY | O_NONBLOCK);
96
	if (fp == -1) { /* If it isn't there make the node */
97
98
		if (!IS_TEST)
			g_warning ("Failed to open '%s' at %s: %s", sensor_device->name, drv_data->dev_path, g_strerror (errno));
99
		return;
100
101
102
	}

	/* Actually read the data */
103
104
	data->read_size = read (fp, data->data, buf_len * drv_data->buffer_data->scan_size);
	if (data->read_size == -1 && errno == EAGAIN) {
105
		g_debug ("No new data available on '%s'", sensor_device->name);
106
		return;
107
108
	}

109
	process_scan (*data, sensor_device);
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
}

static char *
get_trigger_name (GUdevDevice *device)
{
	GList *devices, *l;
	GUdevClient *client;
	gboolean has_trigger = FALSE;
	char *trigger_name;
	const gchar * const subsystems[] = { "iio", NULL };

	client = g_udev_client_new (subsystems);
	devices = g_udev_client_query_by_subsystem (client, "iio");

	/* Find the associated trigger */
	trigger_name = g_strdup_printf ("accel_3d-dev%s", g_udev_device_get_number (device));
	for (l = devices; l != NULL; l = l->next) {
		GUdevDevice *dev = l->data;

		if (g_strcmp0 (trigger_name, g_udev_device_get_sysfs_attr (dev, "name")) == 0) {
			g_debug ("Found associated trigger at %s", g_udev_device_get_sysfs_path (dev));
			has_trigger = TRUE;
			break;
		}
	}

	g_list_free_full (devices, g_object_unref);
	g_clear_object (&client);

	if (has_trigger)
		return trigger_name;

	g_warning ("Could not find trigger name associated with %s",
		   g_udev_device_get_sysfs_path (device));
	g_free (trigger_name);
	return NULL;
}


static gboolean
read_orientation (gpointer user_data)
{
152
153
	SensorDevice *sensor_device = user_data;
	DrvData *drv_data = (DrvData *) sensor_device->priv;
154

155
	prepare_output (sensor_device, drv_data->buffer_data->dev_dir_name, drv_data->buffer_data->trigger_name);
156
157
158
159
160
161
162

	return G_SOURCE_CONTINUE;
}

static gboolean
iio_buffer_accel_discover (GUdevDevice *device)
{
163
	g_autofree char *trigger_name = NULL;
164

165
166
	if (!drv_check_udev_sensor_type (device, "iio-buffer-accel", NULL))
	    return FALSE;
167

168
169
	/* If we can't find an associated trigger, fallback to the iio-poll-accel driver */
	trigger_name = get_trigger_name (device);
170
171
	if (!trigger_name) {
		g_debug ("Could not find trigger for %s", g_udev_device_get_sysfs_path (device));
172
		return FALSE;
173
	}
174

175
	g_debug ("Found IIO buffer accelerometer at %s", g_udev_device_get_sysfs_path (device));
176
177
178
	return TRUE;
}

179
static void
180
181
iio_buffer_accel_set_polling (SensorDevice *sensor_device,
			      gboolean state)
182
{
183
184
	DrvData *drv_data = (DrvData *) sensor_device->priv;

185
186
187
188
189
190
191
192
193
194
195
	if (drv_data->timeout_id > 0 && state)
		return;
	if (drv_data->timeout_id == 0 && !state)
		return;

	if (drv_data->timeout_id) {
		g_source_remove (drv_data->timeout_id);
		drv_data->timeout_id = 0;
	}

	if (state) {
196
		drv_data->timeout_id = g_timeout_add (700, read_orientation, sensor_device);
197
198
199
200
		g_source_set_name_by_id (drv_data->timeout_id, "[iio_buffer_accel_set_polling] read_orientation");
	}
}

201
static SensorDevice *
202
iio_buffer_accel_open (GUdevDevice *device)
203
{
204
205
206
207
	SensorDevice *sensor_device;
	DrvData *drv_data;
	g_autofree char *trigger_name = NULL;
	BufferDrvData *buffer_data;
208

209
210
	/* Get the trigger name, and build the channels from that */
	trigger_name = get_trigger_name (device);
211
212
	if (!trigger_name)
		return NULL;
213

214
215
216
	buffer_data = buffer_drv_data_new (device, trigger_name);
	if (!buffer_data)
		return NULL;
217

218
	sensor_device = g_new0 (SensorDevice, 1);
219
220
221
	sensor_device->name = g_strdup (g_udev_device_get_property (device, "NAME"));
	if (!sensor_device->name)
		sensor_device->name = g_strdup (g_udev_device_get_name (device));
222
223
224
	sensor_device->priv = g_new0 (DrvData, 1);
	drv_data = (DrvData *) sensor_device->priv;
	drv_data->buffer_data = buffer_data;
225
	drv_data->mount_matrix = setup_mount_matrix (device);
226
	drv_data->location = setup_accel_location (device);
227
	drv_data->dev = g_object_ref (device);
228
	drv_data->dev_path = get_device_file (device);
229

230
	return sensor_device;
231
232
233
}

static void
234
iio_buffer_accel_close (SensorDevice *sensor_device)
235
{
236
237
	DrvData *drv_data = (DrvData *) sensor_device->priv;

238
	g_clear_pointer (&drv_data->buffer_data, buffer_drv_data_free);
239
	g_clear_object (&drv_data->dev);
240
	g_clear_pointer (&drv_data->mount_matrix, g_free);
241
	g_clear_pointer (&drv_data->dev_path, g_free);
242
243
	g_clear_pointer (&sensor_device->priv, g_free);
	g_free (sensor_device);
244
245
246
}

SensorDriver iio_buffer_accel = {
247
	.driver_name = "IIO Buffer accelerometer",
248
	.type = DRIVER_TYPE_ACCEL,
249
250
251

	.discover = iio_buffer_accel_discover,
	.open = iio_buffer_accel_open,
252
	.set_polling = iio_buffer_accel_set_polling,
253
254
	.close = iio_buffer_accel_close,
};