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/pm/device.h>
17 #include <zephyr/pm/device_runtime.h>
18 #include <zephyr/sys/atomic.h>
19 #include <zephyr/sys/util.h>
20 
21 LOG_MODULE_REGISTER(analog_axis, CONFIG_INPUT_LOG_LEVEL);
22 
23 struct analog_axis_channel_config {
24 	struct adc_dt_spec adc;
25 	int16_t out_min;
26 	int16_t out_max;
27 	uint16_t axis;
28 	bool invert_input;
29 	bool invert_output;
30 };
31 
32 struct analog_axis_channel_data {
33 	int last_out;
34 };
35 
36 struct analog_axis_config {
37 	uint32_t poll_period_ms;
38 	const struct analog_axis_channel_config *channel_cfg;
39 	struct analog_axis_channel_data *channel_data;
40 	struct analog_axis_calibration *calibration;
41 	const uint8_t num_channels;
42 };
43 
44 struct analog_axis_data {
45 	struct k_sem cal_lock;
46 	analog_axis_raw_data_t raw_data_cb;
47 	struct k_timer timer;
48 	struct k_thread thread;
49 
50 	K_KERNEL_STACK_MEMBER(thread_stack,
51 			      CONFIG_INPUT_ANALOG_AXIS_THREAD_STACK_SIZE);
52 
53 #ifdef CONFIG_PM_DEVICE
54 	atomic_t suspended;
55 	struct k_sem wakeup;
56 #endif
57 };
58 
analog_axis_num_axes(const struct device * dev)59 int analog_axis_num_axes(const struct device *dev)
60 {
61 	const struct analog_axis_config *cfg = dev->config;
62 
63 	return cfg->num_channels;
64 }
65 
analog_axis_calibration_get(const struct device * dev,int channel,struct analog_axis_calibration * out_cal)66 int analog_axis_calibration_get(const struct device *dev,
67 				int channel,
68 				struct analog_axis_calibration *out_cal)
69 {
70 	const struct analog_axis_config *cfg = dev->config;
71 	struct analog_axis_data *data = dev->data;
72 	struct analog_axis_calibration *cal = &cfg->calibration[channel];
73 
74 	if (channel >= cfg->num_channels) {
75 		return -EINVAL;
76 	}
77 
78 	k_sem_take(&data->cal_lock, K_FOREVER);
79 	memcpy(out_cal, cal, sizeof(struct analog_axis_calibration));
80 	k_sem_give(&data->cal_lock);
81 
82 	return 0;
83 }
84 
analog_axis_set_raw_data_cb(const struct device * dev,analog_axis_raw_data_t cb)85 void analog_axis_set_raw_data_cb(const struct device *dev, analog_axis_raw_data_t cb)
86 {
87 	struct analog_axis_data *data = dev->data;
88 
89 	k_sem_take(&data->cal_lock, K_FOREVER);
90 	data->raw_data_cb = cb;
91 	k_sem_give(&data->cal_lock);
92 }
93 
analog_axis_calibration_set(const struct device * dev,int channel,struct analog_axis_calibration * new_cal)94 int analog_axis_calibration_set(const struct device *dev,
95 				int channel,
96 				struct analog_axis_calibration *new_cal)
97 {
98 	const struct analog_axis_config *cfg = dev->config;
99 	struct analog_axis_data *data = dev->data;
100 	struct analog_axis_calibration *cal = &cfg->calibration[channel];
101 
102 	if (channel >= cfg->num_channels) {
103 		return -EINVAL;
104 	}
105 
106 	k_sem_take(&data->cal_lock, K_FOREVER);
107 	memcpy(cal, new_cal, sizeof(struct analog_axis_calibration));
108 	k_sem_give(&data->cal_lock);
109 
110 	return 0;
111 }
112 
analog_axis_out_deadzone(const struct device * dev,int channel,int32_t raw_val)113 static int32_t analog_axis_out_deadzone(const struct device *dev,
114 					int channel,
115 					int32_t raw_val)
116 {
117 	const struct analog_axis_config *cfg = dev->config;
118 	const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[channel];
119 	struct analog_axis_calibration *cal = &cfg->calibration[channel];
120 
121 	int16_t in_range = cal->in_max - cal->in_min;
122 	int16_t out_range = axis_cfg->out_max - axis_cfg->out_min;
123 	int16_t in_mid = DIV_ROUND_CLOSEST(cal->in_min + cal->in_max, 2);
124 	int16_t in_min = cal->in_min;
125 
126 	if (abs(raw_val - in_mid) < cal->in_deadzone) {
127 		return DIV_ROUND_CLOSEST(axis_cfg->out_max + axis_cfg->out_min, 2);
128 	}
129 
130 	in_range -= cal->in_deadzone * 2;
131 	in_min += cal->in_deadzone;
132 	if (raw_val < in_mid) {
133 		raw_val += cal->in_deadzone;
134 	} else {
135 		raw_val -= cal->in_deadzone;
136 	}
137 
138 	return DIV_ROUND_CLOSEST((raw_val - in_min) * out_range, in_range) + axis_cfg->out_min;
139 }
140 
analog_axis_out_linear(const struct device * dev,int channel,int32_t raw_val)141 static int32_t analog_axis_out_linear(const struct device *dev,
142 				      int channel,
143 				      int32_t raw_val)
144 {
145 	const struct analog_axis_config *cfg = dev->config;
146 	const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[channel];
147 	struct analog_axis_calibration *cal = &cfg->calibration[channel];
148 
149 	int16_t in_range = cal->in_max - cal->in_min;
150 	int16_t out_range = axis_cfg->out_max - axis_cfg->out_min;
151 
152 	return DIV_ROUND_CLOSEST((raw_val - cal->in_min) * out_range, in_range) + axis_cfg->out_min;
153 }
154 
analog_axis_loop(const struct device * dev)155 static void analog_axis_loop(const struct device *dev)
156 {
157 	const struct analog_axis_config *cfg = dev->config;
158 	struct analog_axis_data *data = dev->data;
159 	int16_t bufs[cfg->num_channels];
160 	int32_t out;
161 	struct adc_sequence sequence = {
162 		.buffer = bufs,
163 		.buffer_size = sizeof(bufs),
164 	};
165 	const struct analog_axis_channel_config *axis_cfg_0 = &cfg->channel_cfg[0];
166 	int err;
167 	int i;
168 
169 	adc_sequence_init_dt(&axis_cfg_0->adc, &sequence);
170 
171 	for (i = 0; i < cfg->num_channels; i++) {
172 		const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i];
173 
174 		sequence.channels |= BIT(axis_cfg->adc.channel_id);
175 	}
176 
177 	err = adc_read(axis_cfg_0->adc.dev, &sequence);
178 	if (err < 0) {
179 		LOG_ERR("Could not read (%d)", err);
180 		return;
181 	}
182 
183 	k_sem_take(&data->cal_lock, K_FOREVER);
184 
185 	for (i = 0; i < cfg->num_channels; i++) {
186 		const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i];
187 		struct analog_axis_channel_data *axis_data = &cfg->channel_data[i];
188 		struct analog_axis_calibration *cal = &cfg->calibration[i];
189 		int32_t raw_val = bufs[i];
190 
191 		if (axis_cfg->invert_input) {
192 			raw_val *= -1;
193 		}
194 
195 		if (data->raw_data_cb != NULL) {
196 			data->raw_data_cb(dev, i, raw_val);
197 		}
198 
199 		LOG_DBG("%s: ch %d: raw_val: %d", dev->name, i, raw_val);
200 
201 		if (cal->in_deadzone > 0) {
202 			out = analog_axis_out_deadzone(dev, i, raw_val);
203 		} else {
204 			out = analog_axis_out_linear(dev, i, raw_val);
205 		}
206 
207 		out = CLAMP(out, axis_cfg->out_min, axis_cfg->out_max);
208 
209 		if (axis_cfg->invert_output) {
210 			out = axis_cfg->out_max - out;
211 		}
212 
213 		if (axis_data->last_out != out) {
214 			input_report_abs(dev, axis_cfg->axis, out, true, K_FOREVER);
215 		}
216 		axis_data->last_out = out;
217 	}
218 
219 	k_sem_give(&data->cal_lock);
220 }
221 
analog_axis_thread(void * arg1,void * arg2,void * arg3)222 static void analog_axis_thread(void *arg1, void *arg2, void *arg3)
223 {
224 	const struct device *dev = arg1;
225 	const struct analog_axis_config *cfg = dev->config;
226 	struct analog_axis_data *data = dev->data;
227 	int err;
228 	int i;
229 
230 	for (i = 0; i < cfg->num_channels; i++) {
231 		const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i];
232 
233 		if (!adc_is_ready_dt(&axis_cfg->adc)) {
234 			LOG_ERR("ADC controller device not ready");
235 			return;
236 		}
237 
238 		err = adc_channel_setup_dt(&axis_cfg->adc);
239 		if (err < 0) {
240 			LOG_ERR("Could not setup channel #%d (%d)", i, err);
241 			return;
242 		}
243 	}
244 
245 	while (true) {
246 #ifdef CONFIG_PM_DEVICE
247 		if (atomic_get(&data->suspended) == 1) {
248 			k_sem_take(&data->wakeup, K_FOREVER);
249 		}
250 #endif
251 
252 		analog_axis_loop(dev);
253 		k_timer_status_sync(&data->timer);
254 	}
255 }
256 
analog_axis_init(const struct device * dev)257 static int analog_axis_init(const struct device *dev)
258 {
259 	struct analog_axis_data *data = dev->data;
260 	k_tid_t tid;
261 
262 	k_sem_init(&data->cal_lock, 1, 1);
263 	k_timer_init(&data->timer, NULL, NULL);
264 
265 #ifdef CONFIG_PM_DEVICE
266 	k_sem_init(&data->wakeup, 0, 1);
267 #endif
268 
269 	tid = k_thread_create(&data->thread, data->thread_stack,
270 			      K_KERNEL_STACK_SIZEOF(data->thread_stack),
271 			      analog_axis_thread, (void *)dev, NULL, NULL,
272 			      CONFIG_INPUT_ANALOG_AXIS_THREAD_PRIORITY,
273 			      0, K_NO_WAIT);
274 	if (!tid) {
275 		LOG_ERR("thread creation failed");
276 		return -ENODEV;
277 	}
278 
279 	k_thread_name_set(&data->thread, dev->name);
280 
281 #ifndef CONFIG_PM_DEVICE_RUNTIME
282 	const struct analog_axis_config *cfg = dev->config;
283 
284 	k_timer_start(&data->timer,
285 		      K_MSEC(cfg->poll_period_ms), K_MSEC(cfg->poll_period_ms));
286 #else
287 	int ret;
288 
289 	atomic_set(&data->suspended, 1);
290 
291 	pm_device_init_suspended(dev);
292 	ret = pm_device_runtime_enable(dev);
293 	if (ret < 0) {
294 		LOG_ERR("Failed to enable runtime power management");
295 		return ret;
296 	}
297 #endif
298 
299 	return 0;
300 }
301 
302 #ifdef CONFIG_PM_DEVICE
analog_axis_pm_action(const struct device * dev,enum pm_device_action action)303 static int analog_axis_pm_action(const struct device *dev,
304 				 enum pm_device_action action)
305 {
306 	const struct analog_axis_config *cfg = dev->config;
307 	struct analog_axis_data *data = dev->data;
308 
309 	switch (action) {
310 	case PM_DEVICE_ACTION_SUSPEND:
311 		atomic_set(&data->suspended, 1);
312 		k_timer_stop(&data->timer);
313 		break;
314 	case PM_DEVICE_ACTION_RESUME:
315 		k_timer_start(&data->timer,
316 			      K_MSEC(cfg->poll_period_ms),
317 			      K_MSEC(cfg->poll_period_ms));
318 		atomic_set(&data->suspended, 0);
319 		k_sem_give(&data->wakeup);
320 		break;
321 	default:
322 		return -ENOTSUP;
323 	}
324 
325 	return 0;
326 }
327 #endif
328 
329 #define ANALOG_AXIS_CHANNEL_CFG_DEF(node_id) \
330 	{ \
331 		.adc = ADC_DT_SPEC_GET(node_id), \
332 		.out_min = (int16_t)DT_PROP(node_id, out_min), \
333 		.out_max = (int16_t)DT_PROP(node_id, out_max), \
334 		.axis = DT_PROP(node_id, zephyr_axis), \
335 		.invert_input = DT_PROP(node_id, invert_input), \
336 		.invert_output = DT_PROP(node_id, invert_output), \
337 	}
338 
339 #define ANALOG_AXIS_CHANNEL_CAL_DEF(node_id) \
340 	{ \
341 		.in_min = (int16_t)DT_PROP(node_id, in_min), \
342 		.in_max = (int16_t)DT_PROP(node_id, in_max), \
343 		.in_deadzone = DT_PROP(node_id, in_deadzone), \
344 	}
345 
346 #define ANALOG_AXIS_INIT(inst)									\
347 	static const struct analog_axis_channel_config analog_axis_channel_cfg_##inst[] = {	\
348 		DT_INST_FOREACH_CHILD_STATUS_OKAY_SEP(inst, ANALOG_AXIS_CHANNEL_CFG_DEF, (,))	\
349 	};											\
350 												\
351 	static struct analog_axis_channel_data							\
352 		analog_axis_channel_data_##inst[ARRAY_SIZE(analog_axis_channel_cfg_##inst)];	\
353 												\
354 	static struct analog_axis_calibration							\
355 		analog_axis_calibration_##inst[ARRAY_SIZE(analog_axis_channel_cfg_##inst)] = {	\
356 			DT_INST_FOREACH_CHILD_STATUS_OKAY_SEP(					\
357 				inst, ANALOG_AXIS_CHANNEL_CAL_DEF, (,))				\
358 		};										\
359 												\
360 	static const struct analog_axis_config analog_axis_cfg_##inst = {			\
361 		.poll_period_ms = DT_INST_PROP(inst, poll_period_ms),				\
362 		.channel_cfg = analog_axis_channel_cfg_##inst,					\
363 		.channel_data = analog_axis_channel_data_##inst,				\
364 		.calibration = analog_axis_calibration_##inst,					\
365 		.num_channels = ARRAY_SIZE(analog_axis_channel_cfg_##inst),			\
366 	};											\
367 												\
368 	static struct analog_axis_data analog_axis_data_##inst;					\
369 												\
370 	PM_DEVICE_DT_INST_DEFINE(inst, analog_axis_pm_action);					\
371 												\
372 	DEVICE_DT_INST_DEFINE(inst, analog_axis_init, PM_DEVICE_DT_INST_GET(inst),		\
373 			      &analog_axis_data_##inst, &analog_axis_cfg_##inst,		\
374 			      POST_KERNEL, CONFIG_INPUT_INIT_PRIORITY, NULL);
375 
376 DT_INST_FOREACH_STATUS_OKAY(ANALOG_AXIS_INIT)
377