blob: f58d4b77a402578fb154d15cbc1a038bd1e2df51 [file] [log] [blame]
/*
* 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)