1 /*
2 * Copyright 2023 Google LLC
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT analog_axis
8
9 #include <stdlib.h>
10 #include <zephyr/device.h>
11 #include <zephyr/drivers/adc.h>
12 #include <zephyr/input/input.h>
13 #include <zephyr/input/input_analog_axis.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/logging/log.h>
16 #include <zephyr/sys/util.h>
17
18 LOG_MODULE_REGISTER(analog_axis, CONFIG_INPUT_LOG_LEVEL);
19
20 struct analog_axis_channel_config {
21 struct adc_dt_spec adc;
22 int16_t out_min;
23 int16_t out_max;
24 uint16_t axis;
25 bool invert;
26 };
27
28 struct analog_axis_channel_data {
29 int last_out;
30 };
31
32 struct analog_axis_config {
33 uint32_t poll_period_ms;
34 const struct analog_axis_channel_config *channel_cfg;
35 struct analog_axis_channel_data *channel_data;
36 struct analog_axis_calibration *calibration;
37 const uint8_t num_channels;
38 };
39
40 struct analog_axis_data {
41 struct k_mutex cal_lock;
42 analog_axis_raw_data_t raw_data_cb;
43 struct k_timer timer;
44 struct k_thread thread;
45
46 K_KERNEL_STACK_MEMBER(thread_stack,
47 CONFIG_INPUT_ANALOG_AXIS_THREAD_STACK_SIZE);
48 };
49
analog_axis_num_axes(const struct device * dev)50 int analog_axis_num_axes(const struct device *dev)
51 {
52 const struct analog_axis_config *cfg = dev->config;
53
54 return cfg->num_channels;
55 }
56
analog_axis_calibration_get(const struct device * dev,int channel,struct analog_axis_calibration * out_cal)57 int analog_axis_calibration_get(const struct device *dev,
58 int channel,
59 struct analog_axis_calibration *out_cal)
60 {
61 const struct analog_axis_config *cfg = dev->config;
62 struct analog_axis_data *data = dev->data;
63 struct analog_axis_calibration *cal = &cfg->calibration[channel];
64
65 if (channel >= cfg->num_channels) {
66 return -EINVAL;
67 }
68
69 k_mutex_lock(&data->cal_lock, K_FOREVER);
70 memcpy(out_cal, cal, sizeof(struct analog_axis_calibration));
71 k_mutex_unlock(&data->cal_lock);
72
73 return 0;
74 }
75
analog_axis_set_raw_data_cb(const struct device * dev,analog_axis_raw_data_t cb)76 void analog_axis_set_raw_data_cb(const struct device *dev, analog_axis_raw_data_t cb)
77 {
78 struct analog_axis_data *data = dev->data;
79
80 k_mutex_lock(&data->cal_lock, K_FOREVER);
81 data->raw_data_cb = cb;
82 k_mutex_unlock(&data->cal_lock);
83 }
84
analog_axis_calibration_set(const struct device * dev,int channel,struct analog_axis_calibration * new_cal)85 int analog_axis_calibration_set(const struct device *dev,
86 int channel,
87 struct analog_axis_calibration *new_cal)
88 {
89 const struct analog_axis_config *cfg = dev->config;
90 struct analog_axis_data *data = dev->data;
91 struct analog_axis_calibration *cal = &cfg->calibration[channel];
92
93 if (channel >= cfg->num_channels) {
94 return -EINVAL;
95 }
96
97 k_mutex_lock(&data->cal_lock, K_FOREVER);
98 memcpy(cal, new_cal, sizeof(struct analog_axis_calibration));
99 k_mutex_unlock(&data->cal_lock);
100
101 return 0;
102 }
103
analog_axis_loop(const struct device * dev)104 static void analog_axis_loop(const struct device *dev)
105 {
106 const struct analog_axis_config *cfg = dev->config;
107 struct analog_axis_data *data = dev->data;
108 int16_t bufs[cfg->num_channels];
109 int32_t out;
110 struct adc_sequence sequence = {
111 .buffer = bufs,
112 .buffer_size = sizeof(bufs),
113 };
114 const struct analog_axis_channel_config *axis_cfg_0 = &cfg->channel_cfg[0];
115 int err;
116 int i;
117
118 adc_sequence_init_dt(&axis_cfg_0->adc, &sequence);
119
120 for (i = 0; i < cfg->num_channels; i++) {
121 const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i];
122
123 sequence.channels |= BIT(axis_cfg->adc.channel_id);
124 }
125
126 err = adc_read(axis_cfg_0->adc.dev, &sequence);
127 if (err < 0) {
128 LOG_ERR("Could not read (%d)", err);
129 return;
130 }
131
132 k_mutex_lock(&data->cal_lock, K_FOREVER);
133
134 for (i = 0; i < cfg->num_channels; i++) {
135 const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i];
136 struct analog_axis_channel_data *axis_data = &cfg->channel_data[i];
137 struct analog_axis_calibration *cal = &cfg->calibration[i];
138 int16_t in_range = cal->in_max - cal->in_min;
139 int16_t out_range = axis_cfg->out_max - axis_cfg->out_min;
140 int32_t raw_val = bufs[i];
141
142 if (axis_cfg->invert) {
143 raw_val *= -1;
144 }
145
146 if (data->raw_data_cb != NULL) {
147 data->raw_data_cb(dev, i, raw_val);
148 }
149
150 LOG_DBG("%s: ch %d: raw_val: %d", dev->name, i, raw_val);
151
152 out = CLAMP((raw_val - cal->in_min) * out_range / in_range + axis_cfg->out_min,
153 axis_cfg->out_min, axis_cfg->out_max);
154
155 if (cal->out_deadzone > 0) {
156 int16_t center = DIV_ROUND_CLOSEST(
157 axis_cfg->out_max + axis_cfg->out_min, 2);
158 if (abs(out - center) < cal->out_deadzone) {
159 out = center;
160 }
161 }
162
163 if (axis_data->last_out != out) {
164 input_report_abs(dev, axis_cfg->axis, out, true, K_FOREVER);
165 }
166 axis_data->last_out = out;
167 }
168
169 k_mutex_unlock(&data->cal_lock);
170 }
171
analog_axis_thread(void * arg1,void * arg2,void * arg3)172 static void analog_axis_thread(void *arg1, void *arg2, void *arg3)
173 {
174 const struct device *dev = arg1;
175 const struct analog_axis_config *cfg = dev->config;
176 struct analog_axis_data *data = dev->data;
177 int err;
178 int i;
179
180 for (i = 0; i < cfg->num_channels; i++) {
181 const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i];
182
183 if (!adc_is_ready_dt(&axis_cfg->adc)) {
184 LOG_ERR("ADC controller device not ready");
185 return;
186 }
187
188 err = adc_channel_setup_dt(&axis_cfg->adc);
189 if (err < 0) {
190 LOG_ERR("Could not setup channel #%d (%d)", i, err);
191 return;
192 }
193 }
194
195 k_timer_init(&data->timer, NULL, NULL);
196 k_timer_start(&data->timer,
197 K_MSEC(cfg->poll_period_ms), K_MSEC(cfg->poll_period_ms));
198
199 while (true) {
200 analog_axis_loop(dev);
201 k_timer_status_sync(&data->timer);
202 }
203 }
204
analog_axis_init(const struct device * dev)205 static int analog_axis_init(const struct device *dev)
206 {
207 struct analog_axis_data *data = dev->data;
208 k_tid_t tid;
209
210 k_mutex_init(&data->cal_lock);
211
212 tid = k_thread_create(&data->thread, data->thread_stack,
213 K_KERNEL_STACK_SIZEOF(data->thread_stack),
214 analog_axis_thread, (void *)dev, NULL, NULL,
215 CONFIG_INPUT_ANALOG_AXIS_THREAD_PRIORITY,
216 0, K_NO_WAIT);
217 if (!tid) {
218 LOG_ERR("thread creation failed");
219 return -ENODEV;
220 }
221
222 k_thread_name_set(&data->thread, dev->name);
223
224 return 0;
225 }
226
227 #define ANALOG_AXIS_CHANNEL_CFG_DEF(node_id) \
228 { \
229 .adc = ADC_DT_SPEC_GET(node_id), \
230 .out_min = (int16_t)DT_PROP(node_id, out_min), \
231 .out_max = (int16_t)DT_PROP(node_id, out_max), \
232 .axis = DT_PROP(node_id, zephyr_axis), \
233 .invert = DT_PROP(node_id, invert), \
234 }
235
236 #define ANALOG_AXIS_CHANNEL_CAL_DEF(node_id) \
237 { \
238 .in_min = (int16_t)DT_PROP(node_id, in_min), \
239 .in_max = (int16_t)DT_PROP(node_id, in_max), \
240 .out_deadzone = DT_PROP(node_id, out_deadzone), \
241 }
242
243 #define ANALOG_AXIS_INIT(inst) \
244 static const struct analog_axis_channel_config analog_axis_channel_cfg_##inst[] = { \
245 DT_INST_FOREACH_CHILD_STATUS_OKAY_SEP(inst, ANALOG_AXIS_CHANNEL_CFG_DEF, (,)) \
246 }; \
247 \
248 static struct analog_axis_channel_data \
249 analog_axis_channel_data_##inst[ARRAY_SIZE(analog_axis_channel_cfg_##inst)]; \
250 \
251 static struct analog_axis_calibration \
252 analog_axis_calibration##inst[ARRAY_SIZE(analog_axis_channel_cfg_##inst)] = { \
253 DT_INST_FOREACH_CHILD_STATUS_OKAY_SEP( \
254 inst, ANALOG_AXIS_CHANNEL_CAL_DEF, (,)) \
255 }; \
256 \
257 static const struct analog_axis_config analog_axis_cfg_##inst = { \
258 .poll_period_ms = DT_INST_PROP(inst, poll_period_ms), \
259 .channel_cfg = analog_axis_channel_cfg_##inst, \
260 .channel_data = analog_axis_channel_data_##inst, \
261 .calibration = analog_axis_calibration##inst, \
262 .num_channels = ARRAY_SIZE(analog_axis_channel_cfg_##inst), \
263 }; \
264 \
265 static struct analog_axis_data analog_axis_data_##inst; \
266 \
267 DEVICE_DT_INST_DEFINE(inst, analog_axis_init, NULL, \
268 &analog_axis_data_##inst, &analog_axis_cfg_##inst, \
269 POST_KERNEL, CONFIG_INPUT_INIT_PRIORITY, NULL);
270
271 DT_INST_FOREACH_STATUS_OKAY(ANALOG_AXIS_INIT)
272
