1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
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
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
|
/*
* STMicroelectronics lsm6ds3 trigger driver
*
* Copyright 2014 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*
* Licensed under the GPL-2.
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/iio/iio.h>
#include <linux/iio/trigger.h>
#include <linux/interrupt.h>
#include <linux/iio/events.h>
#include "st_lsm6ds3.h"
#define ST_LSM6DS3_SRC_FUNC_ADDR 0x53
#define ST_LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b
#define ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10
#define ST_LSM6DS3_SRC_TILT_DATA_AVL 0x20
#define ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80
#define ST_LSM6DS3_FIFO_DATA_AVL 0x80
#define ST_LSM6DS3_FIFO_DATA_OVR 0x40
static struct workqueue_struct *st_lsm6ds3_wq;
void st_lsm6ds3_flush_works()
{
flush_workqueue(st_lsm6ds3_wq);
}
irqreturn_t st_lsm6ds3_save_timestamp(int irq, void *private)
{
struct timespec ts;
struct lsm6ds3_data *cdata = private;
get_monotonic_boottime(&ts);
cdata->timestamp = timespec_to_ns(&ts);
queue_work(st_lsm6ds3_wq, &cdata->data_work);
disable_irq_nosync(irq);
return IRQ_HANDLED;
}
static void st_lsm6ds3_irq_management(struct work_struct *data_work)
{
struct lsm6ds3_data *cdata;
u8 src_value = 0x00, src_fifo = 0x00;
cdata = container_of((struct work_struct *)data_work,
struct lsm6ds3_data, data_work);
cdata->tf->read(cdata, ST_LSM6DS3_SRC_FUNC_ADDR, 1, &src_value, true);
cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_AVL_ADDR, 1,
&src_fifo, true);
if (src_fifo & ST_LSM6DS3_FIFO_DATA_AVL) {
if (src_fifo & ST_LSM6DS3_FIFO_DATA_OVR) {
st_lsm6ds3_set_fifo_mode(cdata, BYPASS);
st_lsm6ds3_set_fifo_mode(cdata, CONTINUOS);
dev_err(cdata->dev,
"data fifo overrun, reduce fifo size.\n");
} else
st_lsm6ds3_read_fifo(cdata, false);
}
if (src_value & ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL) {
iio_push_event(cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR],
IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR,
0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
cdata->timestamp);
if (cdata->sign_motion_event_ready) {
iio_push_event(cdata->indio_dev[
ST_INDIO_DEV_SIGN_MOTION],
IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION,
0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
cdata->timestamp);
cdata->sign_motion_event_ready = false;
}
}
if (src_value & ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL) {
iio_trigger_poll_chained(
cdata->trig[ST_INDIO_DEV_STEP_COUNTER], 0);
}
if (src_value & ST_LSM6DS3_SRC_TILT_DATA_AVL) {
iio_push_event(cdata->indio_dev[ST_INDIO_DEV_TILT],
IIO_UNMOD_EVENT_CODE(IIO_TILT,
0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
cdata->timestamp);
}
enable_irq(cdata->irq);
return;
}
int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
const struct iio_trigger_ops *trigger_ops)
{
int err, i, n;
if (!st_lsm6ds3_wq)
st_lsm6ds3_wq = create_workqueue(cdata->name);
if (!st_lsm6ds3_wq)
return -EINVAL;
INIT_WORK(&cdata->data_work, st_lsm6ds3_irq_management);
for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
cdata->trig[i] = iio_trigger_alloc("%s-trigger",
cdata->indio_dev[i]->name);
if (!cdata->trig[i]) {
dev_err(cdata->dev,
"failed to allocate iio trigger.\n");
err = -ENOMEM;
goto deallocate_trigger;
}
iio_trigger_set_drvdata(cdata->trig[i], cdata->indio_dev[i]);
cdata->trig[i]->ops = trigger_ops;
cdata->trig[i]->dev.parent = cdata->dev;
}
err = request_threaded_irq(cdata->irq, st_lsm6ds3_save_timestamp, NULL,
IRQF_TRIGGER_HIGH, cdata->name, cdata);
if (err)
goto deallocate_trigger;
for (n = 0; n < ST_INDIO_DEV_NUM; n++) {
err = iio_trigger_register(cdata->trig[n]);
if (err < 0) {
dev_err(cdata->dev,
"failed to register iio trigger.\n");
goto free_irq;
}
cdata->indio_dev[n]->trig = cdata->trig[n];
}
return 0;
free_irq:
free_irq(cdata->irq, cdata);
for (n--; n >= 0; n--)
iio_trigger_unregister(cdata->trig[n]);
deallocate_trigger:
for (i--; i >= 0; i--)
iio_trigger_free(cdata->trig[i]);
return err;
}
EXPORT_SYMBOL(st_lsm6ds3_allocate_triggers);
void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata)
{
int i;
free_irq(cdata->irq, cdata);
for (i = 0; i < ST_INDIO_DEV_NUM; i++)
iio_trigger_unregister(cdata->trig[i]);
}
EXPORT_SYMBOL(st_lsm6ds3_deallocate_triggers);
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 trigger driver");
MODULE_LICENSE("GPL v2");
|
