| /* |
| * Copyright 2023 Google LLC |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #define DT_DRV_COMPAT analog_axis |
| |
| #include <stdlib.h> |
| #include <zephyr/device.h> |
| #include <zephyr/drivers/adc.h> |
| #include <zephyr/input/input.h> |
| #include <zephyr/input/input_analog_axis.h> |
| #include <zephyr/kernel.h> |
| #include <zephyr/logging/log.h> |
| #include <zephyr/pm/device.h> |
| #include <zephyr/pm/device_runtime.h> |
| #include <zephyr/sys/atomic.h> |
| #include <zephyr/sys/util.h> |
| |
| LOG_MODULE_REGISTER(analog_axis, CONFIG_INPUT_LOG_LEVEL); |
| |
| struct analog_axis_channel_config { |
| struct adc_dt_spec adc; |
| int16_t out_min; |
| int16_t out_max; |
| uint16_t axis; |
| bool invert_input; |
| bool invert_output; |
| }; |
| |
| struct analog_axis_channel_data { |
| int last_out; |
| }; |
| |
| struct analog_axis_config { |
| uint32_t poll_period_ms; |
| const struct analog_axis_channel_config *channel_cfg; |
| struct analog_axis_channel_data *channel_data; |
| struct analog_axis_calibration *calibration; |
| const uint8_t num_channels; |
| }; |
| |
| struct analog_axis_data { |
| struct k_sem cal_lock; |
| analog_axis_raw_data_t raw_data_cb; |
| struct k_timer timer; |
| struct k_thread thread; |
| |
| K_KERNEL_STACK_MEMBER(thread_stack, |
| CONFIG_INPUT_ANALOG_AXIS_THREAD_STACK_SIZE); |
| |
| #ifdef CONFIG_PM_DEVICE |
| atomic_t suspended; |
| struct k_sem wakeup; |
| #endif |
| }; |
| |
| int analog_axis_num_axes(const struct device *dev) |
| { |
| const struct analog_axis_config *cfg = dev->config; |
| |
| return cfg->num_channels; |
| } |
| |
| int analog_axis_calibration_get(const struct device *dev, |
| int channel, |
| struct analog_axis_calibration *out_cal) |
| { |
| const struct analog_axis_config *cfg = dev->config; |
| struct analog_axis_data *data = dev->data; |
| struct analog_axis_calibration *cal = &cfg->calibration[channel]; |
| |
| if (channel >= cfg->num_channels) { |
| return -EINVAL; |
| } |
| |
| k_sem_take(&data->cal_lock, K_FOREVER); |
| memcpy(out_cal, cal, sizeof(struct analog_axis_calibration)); |
| k_sem_give(&data->cal_lock); |
| |
| return 0; |
| } |
| |
| void analog_axis_set_raw_data_cb(const struct device *dev, analog_axis_raw_data_t cb) |
| { |
| struct analog_axis_data *data = dev->data; |
| |
| k_sem_take(&data->cal_lock, K_FOREVER); |
| data->raw_data_cb = cb; |
| k_sem_give(&data->cal_lock); |
| } |
| |
| int analog_axis_calibration_set(const struct device *dev, |
| int channel, |
| struct analog_axis_calibration *new_cal) |
| { |
| const struct analog_axis_config *cfg = dev->config; |
| struct analog_axis_data *data = dev->data; |
| struct analog_axis_calibration *cal = &cfg->calibration[channel]; |
| |
| if (channel >= cfg->num_channels) { |
| return -EINVAL; |
| } |
| |
| k_sem_take(&data->cal_lock, K_FOREVER); |
| memcpy(cal, new_cal, sizeof(struct analog_axis_calibration)); |
| k_sem_give(&data->cal_lock); |
| |
| return 0; |
| } |
| |
| static int32_t analog_axis_out_deadzone(const struct device *dev, |
| int channel, |
| int32_t raw_val) |
| { |
| const struct analog_axis_config *cfg = dev->config; |
| const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[channel]; |
| struct analog_axis_calibration *cal = &cfg->calibration[channel]; |
| |
| int16_t in_range = cal->in_max - cal->in_min; |
| int16_t out_range = axis_cfg->out_max - axis_cfg->out_min; |
| int16_t in_mid = DIV_ROUND_CLOSEST(cal->in_min + cal->in_max, 2); |
| int16_t in_min = cal->in_min; |
| |
| if (abs(raw_val - in_mid) < cal->in_deadzone) { |
| return DIV_ROUND_CLOSEST(axis_cfg->out_max + axis_cfg->out_min, 2); |
| } |
| |
| in_range -= cal->in_deadzone * 2; |
| in_min += cal->in_deadzone; |
| if (raw_val < in_mid) { |
| raw_val += cal->in_deadzone; |
| } else { |
| raw_val -= cal->in_deadzone; |
| } |
| |
| return DIV_ROUND_CLOSEST((raw_val - in_min) * out_range, in_range) + axis_cfg->out_min; |
| } |
| |
| static int32_t analog_axis_out_linear(const struct device *dev, |
| int channel, |
| int32_t raw_val) |
| { |
| const struct analog_axis_config *cfg = dev->config; |
| const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[channel]; |
| struct analog_axis_calibration *cal = &cfg->calibration[channel]; |
| |
| int16_t in_range = cal->in_max - cal->in_min; |
| int16_t out_range = axis_cfg->out_max - axis_cfg->out_min; |
| |
| return DIV_ROUND_CLOSEST((raw_val - cal->in_min) * out_range, in_range) + axis_cfg->out_min; |
| } |
| |
| static void analog_axis_loop(const struct device *dev) |
| { |
| const struct analog_axis_config *cfg = dev->config; |
| struct analog_axis_data *data = dev->data; |
| int16_t bufs[cfg->num_channels]; |
| int32_t out; |
| struct adc_sequence sequence = { |
| .buffer = bufs, |
| .buffer_size = sizeof(bufs), |
| }; |
| const struct analog_axis_channel_config *axis_cfg_0 = &cfg->channel_cfg[0]; |
| int err; |
| int i; |
| |
| adc_sequence_init_dt(&axis_cfg_0->adc, &sequence); |
| |
| for (i = 0; i < cfg->num_channels; i++) { |
| const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i]; |
| |
| sequence.channels |= BIT(axis_cfg->adc.channel_id); |
| } |
| |
| err = adc_read(axis_cfg_0->adc.dev, &sequence); |
| if (err < 0) { |
| LOG_ERR("Could not read (%d)", err); |
| return; |
| } |
| |
| k_sem_take(&data->cal_lock, K_FOREVER); |
| |
| for (i = 0; i < cfg->num_channels; i++) { |
| const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i]; |
| struct analog_axis_channel_data *axis_data = &cfg->channel_data[i]; |
| struct analog_axis_calibration *cal = &cfg->calibration[i]; |
| int32_t raw_val = bufs[i]; |
| |
| if (axis_cfg->invert_input) { |
| raw_val *= -1; |
| } |
| |
| if (data->raw_data_cb != NULL) { |
| data->raw_data_cb(dev, i, raw_val); |
| } |
| |
| LOG_DBG("%s: ch %d: raw_val: %d", dev->name, i, raw_val); |
| |
| if (cal->in_deadzone > 0) { |
| out = analog_axis_out_deadzone(dev, i, raw_val); |
| } else { |
| out = analog_axis_out_linear(dev, i, raw_val); |
| } |
| |
| out = CLAMP(out, axis_cfg->out_min, axis_cfg->out_max); |
| |
| if (axis_cfg->invert_output) { |
| out = axis_cfg->out_max - out; |
| } |
| |
| if (axis_data->last_out != out) { |
| input_report_abs(dev, axis_cfg->axis, out, true, K_FOREVER); |
| } |
| axis_data->last_out = out; |
| } |
| |
| k_sem_give(&data->cal_lock); |
| } |
| |
| static void analog_axis_thread(void *arg1, void *arg2, void *arg3) |
| { |
| const struct device *dev = arg1; |
| const struct analog_axis_config *cfg = dev->config; |
| struct analog_axis_data *data = dev->data; |
| int err; |
| int i; |
| |
| for (i = 0; i < cfg->num_channels; i++) { |
| const struct analog_axis_channel_config *axis_cfg = &cfg->channel_cfg[i]; |
| |
| if (!adc_is_ready_dt(&axis_cfg->adc)) { |
| LOG_ERR("ADC controller device not ready"); |
| return; |
| } |
| |
| err = adc_channel_setup_dt(&axis_cfg->adc); |
| if (err < 0) { |
| LOG_ERR("Could not setup channel #%d (%d)", i, err); |
| return; |
| } |
| } |
| |
| while (true) { |
| #ifdef CONFIG_PM_DEVICE |
| if (atomic_get(&data->suspended) == 1) { |
| k_sem_take(&data->wakeup, K_FOREVER); |
| } |
| #endif |
| |
| analog_axis_loop(dev); |
| k_timer_status_sync(&data->timer); |
| } |
| } |
| |
| static int analog_axis_init(const struct device *dev) |
| { |
| struct analog_axis_data *data = dev->data; |
| k_tid_t tid; |
| |
| k_sem_init(&data->cal_lock, 1, 1); |
| k_timer_init(&data->timer, NULL, NULL); |
| |
| #ifdef CONFIG_PM_DEVICE |
| k_sem_init(&data->wakeup, 0, 1); |
| #endif |
| |
| tid = k_thread_create(&data->thread, data->thread_stack, |
| K_KERNEL_STACK_SIZEOF(data->thread_stack), |
| analog_axis_thread, (void *)dev, NULL, NULL, |
| CONFIG_INPUT_ANALOG_AXIS_THREAD_PRIORITY, |
| 0, K_NO_WAIT); |
| if (!tid) { |
| LOG_ERR("thread creation failed"); |
| return -ENODEV; |
| } |
| |
| k_thread_name_set(&data->thread, dev->name); |
| |
| #ifndef CONFIG_PM_DEVICE_RUNTIME |
| const struct analog_axis_config *cfg = dev->config; |
| |
| k_timer_start(&data->timer, |
| K_MSEC(cfg->poll_period_ms), K_MSEC(cfg->poll_period_ms)); |
| #else |
| int ret; |
| |
| atomic_set(&data->suspended, 1); |
| |
| pm_device_init_suspended(dev); |
| ret = pm_device_runtime_enable(dev); |
| if (ret < 0) { |
| LOG_ERR("Failed to enable runtime power management"); |
| return ret; |
| } |
| #endif |
| |
| return 0; |
| } |
| |
| #ifdef CONFIG_PM_DEVICE |
| static int analog_axis_pm_action(const struct device *dev, |
| enum pm_device_action action) |
| { |
| const struct analog_axis_config *cfg = dev->config; |
| struct analog_axis_data *data = dev->data; |
| |
| switch (action) { |
| case PM_DEVICE_ACTION_SUSPEND: |
| atomic_set(&data->suspended, 1); |
| k_timer_stop(&data->timer); |
| break; |
| case PM_DEVICE_ACTION_RESUME: |
| k_timer_start(&data->timer, |
| K_MSEC(cfg->poll_period_ms), |
| K_MSEC(cfg->poll_period_ms)); |
| atomic_set(&data->suspended, 0); |
| k_sem_give(&data->wakeup); |
| break; |
| default: |
| return -ENOTSUP; |
| } |
| |
| return 0; |
| } |
| #endif |
| |
| #define ANALOG_AXIS_CHANNEL_CFG_DEF(node_id) \ |
| { \ |
| .adc = ADC_DT_SPEC_GET(node_id), \ |
| .out_min = (int16_t)DT_PROP(node_id, out_min), \ |
| .out_max = (int16_t)DT_PROP(node_id, out_max), \ |
| .axis = DT_PROP(node_id, zephyr_axis), \ |
| .invert_input = DT_PROP(node_id, invert_input), \ |
| .invert_output = DT_PROP(node_id, invert_output), \ |
| } |
| |
| #define ANALOG_AXIS_CHANNEL_CAL_DEF(node_id) \ |
| { \ |
| .in_min = (int16_t)DT_PROP(node_id, in_min), \ |
| .in_max = (int16_t)DT_PROP(node_id, in_max), \ |
| .in_deadzone = DT_PROP(node_id, in_deadzone), \ |
| } |
| |
| #define ANALOG_AXIS_INIT(inst) \ |
| static const struct analog_axis_channel_config analog_axis_channel_cfg_##inst[] = { \ |
| DT_INST_FOREACH_CHILD_STATUS_OKAY_SEP(inst, ANALOG_AXIS_CHANNEL_CFG_DEF, (,)) \ |
| }; \ |
| \ |
| static struct analog_axis_channel_data \ |
| analog_axis_channel_data_##inst[ARRAY_SIZE(analog_axis_channel_cfg_##inst)]; \ |
| \ |
| static struct analog_axis_calibration \ |
| analog_axis_calibration_##inst[ARRAY_SIZE(analog_axis_channel_cfg_##inst)] = { \ |
| DT_INST_FOREACH_CHILD_STATUS_OKAY_SEP( \ |
| inst, ANALOG_AXIS_CHANNEL_CAL_DEF, (,)) \ |
| }; \ |
| \ |
| static const struct analog_axis_config analog_axis_cfg_##inst = { \ |
| .poll_period_ms = DT_INST_PROP(inst, poll_period_ms), \ |
| .channel_cfg = analog_axis_channel_cfg_##inst, \ |
| .channel_data = analog_axis_channel_data_##inst, \ |
| .calibration = analog_axis_calibration_##inst, \ |
| .num_channels = ARRAY_SIZE(analog_axis_channel_cfg_##inst), \ |
| }; \ |
| \ |
| static struct analog_axis_data analog_axis_data_##inst; \ |
| \ |
| PM_DEVICE_DT_INST_DEFINE(inst, analog_axis_pm_action); \ |
| \ |
| DEVICE_DT_INST_DEFINE(inst, analog_axis_init, PM_DEVICE_DT_INST_GET(inst), \ |
| &analog_axis_data_##inst, &analog_axis_cfg_##inst, \ |
| POST_KERNEL, CONFIG_INPUT_INIT_PRIORITY, NULL); |
| |
| DT_INST_FOREACH_STATUS_OKAY(ANALOG_AXIS_INIT) |