drivers: sensor: Add tmd2620 driver
Adds tmd2620 driver and devicetree bindings to work in trigger and polling mode supporting Power management. Signed-off-by: Peter Fecher <p.fecher@phytec.de>
This commit is contained in:
parent
546abe778d
commit
924ac2265d
|
@ -124,6 +124,7 @@ add_subdirectory_ifdef(CONFIG_PCNT_ESP32 pcnt_esp32)
|
|||
add_subdirectory_ifdef(CONFIG_ESP32_TEMP esp32_temp)
|
||||
add_subdirectory_ifdef(CONFIG_RPI_PICO_TEMP rpi_pico_temp)
|
||||
add_subdirectory_ifdef(CONFIG_XMC4XXX_TEMP xmc4xxx_temp)
|
||||
add_subdirectory_ifdef(CONFIG_TMD2620 tmd2620)
|
||||
|
||||
if(CONFIG_USERSPACE OR CONFIG_SENSOR_SHELL OR CONFIG_SENSOR_SHELL_BATTERY)
|
||||
# The above if() is needed or else CMake would complain about
|
||||
|
|
|
@ -287,4 +287,6 @@ source "drivers/sensor/rpi_pico_temp/Kconfig"
|
|||
|
||||
source "drivers/sensor/xmc4xxx_temp/Kconfig"
|
||||
|
||||
source "drivers/sensor/tmd2620/Kconfig"
|
||||
|
||||
endif # SENSOR
|
||||
|
|
4
drivers/sensor/tmd2620/CMakeLists.txt
Normal file
4
drivers/sensor/tmd2620/CMakeLists.txt
Normal file
|
@ -0,0 +1,4 @@
|
|||
zephyr_library()
|
||||
|
||||
zephyr_library_sources(tmd2620.c)
|
||||
zephyr_library_sources_ifdef(CONFIG_TMD2620_TRIGGER tmd2620_trigger.c)
|
33
drivers/sensor/tmd2620/Kconfig
Normal file
33
drivers/sensor/tmd2620/Kconfig
Normal file
|
@ -0,0 +1,33 @@
|
|||
# Copyright (c) 2023 PHYTEC Messtechnik GmbH
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
menuconfig TMD2620
|
||||
bool "OSRAM-AMS TMD2620 Proxmimity Sensor"
|
||||
default y
|
||||
depends on DT_HAS_AMS_TMD2620_ENABLED
|
||||
select I2C
|
||||
help
|
||||
Enable driver for TMD2620 Sensor
|
||||
|
||||
if TMD2620
|
||||
|
||||
choice
|
||||
prompt "Trigger Mode"
|
||||
default TMD2620_TRIGGER_NONE
|
||||
help
|
||||
Specify the type of triggering to be used by the driver.
|
||||
|
||||
config TMD2620_TRIGGER_NONE
|
||||
bool "No Trigger"
|
||||
|
||||
config TMD2620_TRIGGER_GLOBAL_THREAD
|
||||
bool "Use global thread"
|
||||
depends on GPIO
|
||||
select TMD2620_TRIGGER
|
||||
|
||||
endchoice # Trigger Mode
|
||||
|
||||
config TMD2620_TRIGGER
|
||||
bool
|
||||
|
||||
endif # TMD2620
|
421
drivers/sensor/tmd2620/tmd2620.c
Normal file
421
drivers/sensor/tmd2620/tmd2620.c
Normal file
|
@ -0,0 +1,421 @@
|
|||
/*
|
||||
* Copyright (c) 2023 PHYTEC Messtechnik GmbH
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#define DT_DRV_COMPAT ams_tmd2620
|
||||
|
||||
#include <zephyr/device.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include <zephyr/drivers/i2c.h>
|
||||
#include <zephyr/pm/device.h>
|
||||
#include <zephyr/sys/__assert.h>
|
||||
#include <zephyr/sys/byteorder.h>
|
||||
#include <zephyr/init.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <string.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "tmd2620.h"
|
||||
|
||||
LOG_MODULE_REGISTER(TMD2620, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
static void tmd2620_gpio_callback(const struct device *dev, struct gpio_callback *cb, uint32_t pins)
|
||||
{
|
||||
LOG_DBG("Interrupt Callback was called");
|
||||
|
||||
struct tmd2620_data *data = CONTAINER_OF(cb, struct tmd2620_data, gpio_cb);
|
||||
|
||||
tmd2620_setup_int(data->dev->config, false);
|
||||
|
||||
#ifdef CONFIG_TMD2620_TRIGGER
|
||||
k_work_submit(&data->work);
|
||||
#else
|
||||
k_sem_give(&data->data_sem);
|
||||
#endif
|
||||
}
|
||||
|
||||
static int tmd2620_configure_interrupt(const struct device *dev)
|
||||
{
|
||||
struct tmd2620_data *data = dev->data;
|
||||
const struct tmd2620_config *config = dev->config;
|
||||
int ret;
|
||||
|
||||
LOG_DBG("Configuring Interrupt.");
|
||||
|
||||
if (!gpio_is_ready_dt(&config->int_gpio)) {
|
||||
LOG_ERR("Interrupt GPIO device not ready");
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
ret = gpio_pin_configure_dt(&config->int_gpio, GPIO_INPUT);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed to configure interrupt pin");
|
||||
return ret;
|
||||
}
|
||||
|
||||
gpio_init_callback(&data->gpio_cb, tmd2620_gpio_callback, BIT(config->int_gpio.pin));
|
||||
|
||||
ret = gpio_add_callback(config->int_gpio.port, &data->gpio_cb);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed to set GPIO callback");
|
||||
return ret;
|
||||
}
|
||||
|
||||
data->dev = dev;
|
||||
|
||||
#ifdef CONFIG_TMD2620_TRIGGER
|
||||
data->work.handler = tmd2620_work_cb;
|
||||
#else
|
||||
k_sem_init(&data->data_sem, 0, K_SEM_MAX_LIMIT);
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int tmd2620_sample_fetch(const struct device *dev, enum sensor_channel chan)
|
||||
{
|
||||
LOG_DBG("Fetching Sample...");
|
||||
struct tmd2620_data *data = dev->data;
|
||||
const struct tmd2620_config *config = dev->config;
|
||||
uint8_t tmp;
|
||||
int ret;
|
||||
|
||||
if (chan != SENSOR_CHAN_ALL && chan != SENSOR_CHAN_PROX) {
|
||||
LOG_ERR("Unsupported sensor channel");
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
#ifndef CONFIG_TMD2620_TRIGGER
|
||||
/* enabling interrupt */
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_INTENAB_REG, TMD2620_INTENAB_PIEN,
|
||||
TMD2620_INTENAB_PIEN);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed enabling interrupt.");
|
||||
return ret;
|
||||
}
|
||||
|
||||
tmd2620_setup_int(config, true);
|
||||
|
||||
/* Enabling proximity and powering up device */
|
||||
tmp = TMD2620_ENABLE_PEN | TMD2620_ENABLE_PON;
|
||||
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_ENABLE_REG, tmp, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed enabling device.");
|
||||
return ret;
|
||||
}
|
||||
|
||||
LOG_DBG("waiting for semaphore..");
|
||||
|
||||
k_sem_take(&data->data_sem, K_FOREVER);
|
||||
#endif
|
||||
ret = i2c_reg_read_byte_dt(&config->i2c, TMD2620_STATUS_REG, &tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed reading status register.");
|
||||
return ret;
|
||||
}
|
||||
|
||||
LOG_DBG("Status register: 0x%x", tmp);
|
||||
if (tmp & TMD2620_STATUS_PINT) {
|
||||
LOG_DBG("Proximity interrupt detected.");
|
||||
|
||||
ret = i2c_reg_read_byte_dt(&config->i2c, TMD2620_PDATA_REG, &data->pdata);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed reading proximity data.");
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef CONFIG_TMD2620_TRIGGER
|
||||
tmp = TMD2620_ENABLE_PEN | TMD2620_ENABLE_PON;
|
||||
|
||||
/* Disabling proximity and powering down device */
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_ENABLE_REG, tmp, 0);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed powering down device.");
|
||||
return ret;
|
||||
}
|
||||
#endif
|
||||
/* clearing interrupt flag */
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_STATUS_REG, TMD2620_STATUS_PINT,
|
||||
TMD2620_STATUS_PINT);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed clearing interrupt flag.");
|
||||
return ret;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int tmd2620_channel_get(const struct device *dev, enum sensor_channel chan,
|
||||
struct sensor_value *val)
|
||||
{
|
||||
struct tmd2620_data *data = dev->data;
|
||||
|
||||
if (chan == SENSOR_CHAN_PROX) {
|
||||
/* inverting sensor data to fit Zephyr */
|
||||
val->val1 = (256 - data->pdata);
|
||||
val->val2 = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
static int tmd2620_sensor_setup(const struct device *dev)
|
||||
{
|
||||
const struct tmd2620_config *config = dev->config;
|
||||
uint8_t chip_id;
|
||||
uint8_t tmp;
|
||||
int ret;
|
||||
|
||||
/* trying to read the id twice, as the sensor does not answer the first request */
|
||||
/* because of this no return code is checked in this line */
|
||||
i2c_reg_read_byte_dt(&config->i2c, TMD2620_ID_REG, &chip_id);
|
||||
|
||||
ret = i2c_reg_read_byte_dt(&config->i2c, TMD2620_ID_REG, &chip_id);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed reading chip id");
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (chip_id != TMD2620_CHIP_ID) {
|
||||
LOG_ERR("Chip id is invalid! Device @%02x is no TMD2620!", config->i2c.addr);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_ENABLE_REG, 0);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("ENABLE Register was not cleared");
|
||||
return ret;
|
||||
}
|
||||
|
||||
tmp = config->wait_time_factor;
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_WTIME_REG, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting wait time");
|
||||
return ret;
|
||||
}
|
||||
|
||||
tmp = config->proximity_low_threshold;
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_PILT_REG, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting PILT");
|
||||
return ret;
|
||||
}
|
||||
|
||||
tmp = config->proximity_high_threshold;
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_PIHT_REG, (255 - tmp));
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting PIHT");
|
||||
return ret;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_TMD2620_TRIGGER
|
||||
tmp = (config->proximity_interrupt_filter << 3);
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_PERS_REG, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting PERS");
|
||||
return ret;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (config->wait_long) {
|
||||
tmp = TMD2620_CFG0_WLONG;
|
||||
} else {
|
||||
tmp = 0;
|
||||
}
|
||||
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_CFG0_REG, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting CFG0");
|
||||
return ret;
|
||||
}
|
||||
|
||||
switch (config->proximity_pulse_length) {
|
||||
case 4:
|
||||
tmp = TMD2620_PCFG0_PPULSE_LEN_4US;
|
||||
break;
|
||||
case 8:
|
||||
tmp = TMD2620_PCFG0_PPULSE_LEN_8US;
|
||||
break;
|
||||
case 16:
|
||||
tmp = TMD2620_PCFG0_PPULSE_LEN_16US;
|
||||
break;
|
||||
case 32:
|
||||
tmp = TMD2620_PCFG0_PPULSE_LEN_32US;
|
||||
break;
|
||||
default:
|
||||
LOG_ERR("Invalid proximity pulse length");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
tmp |= config->proximity_pulse_count;
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_PCFG0_REG, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting PPULSE");
|
||||
return ret;
|
||||
}
|
||||
|
||||
switch (config->proximity_gain) {
|
||||
case 1:
|
||||
tmp = TMD2620_PCFG1_PGAIN_X1;
|
||||
break;
|
||||
case 2:
|
||||
tmp = TMD2620_PCFG1_PGAIN_X2;
|
||||
break;
|
||||
case 4:
|
||||
tmp = TMD2620_PCFG1_PGAIN_X4;
|
||||
break;
|
||||
case 8:
|
||||
tmp = TMD2620_PCFG1_PGAIN_X8;
|
||||
break;
|
||||
default:
|
||||
LOG_ERR("Invalid proximity gain");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
tmp |= config->proximity_led_drive_strength;
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_PCFG1_REG, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting PCGF1");
|
||||
return ret;
|
||||
}
|
||||
|
||||
tmp = TMD2620_CFG3_INT_READ_CLEAR;
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_CFG3_REG, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting CFG3");
|
||||
return ret;
|
||||
}
|
||||
|
||||
tmp = 1; /* enable interrupt */
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_INTENAB_REG, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed setting INTENAB");
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (config->enable_wait_mode) {
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_ENABLE_REG, TMD2620_ENABLE_WEN,
|
||||
TMD2620_ENABLE_WEN);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed enabling wait mode");
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int tmd2620_init(const struct device *dev)
|
||||
{
|
||||
const struct tmd2620_config *config = dev->config;
|
||||
struct tmd2620_data *data = dev->data;
|
||||
int ret;
|
||||
#ifdef CONFIG_TMD2620_TRIGGER
|
||||
uint8_t tmp;
|
||||
#endif
|
||||
|
||||
if (!i2c_is_ready_dt(&config->i2c)) {
|
||||
LOG_ERR("I2C bus not ready!");
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
data->pdata = 0U;
|
||||
|
||||
ret = tmd2620_sensor_setup(dev);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed to configure device");
|
||||
return ret;
|
||||
}
|
||||
|
||||
LOG_DBG("Device setup complete");
|
||||
|
||||
ret = tmd2620_configure_interrupt(dev);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed configuring interrupt!");
|
||||
return ret;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_TMD2620_TRIGGER
|
||||
tmp = TMD2620_ENABLE_PEN | TMD2620_ENABLE_PON;
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_ENABLE_REG, tmp, tmp);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed enabling device.");
|
||||
return ret;
|
||||
}
|
||||
#endif
|
||||
LOG_DBG("Driver init complete.");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_PM_DEVICE
|
||||
static int tmd2620_pm_action(const struct device *dev, enum pm_device_action action)
|
||||
{
|
||||
const struct tmd2620_config *config = dev->config;
|
||||
int ret;
|
||||
|
||||
switch (action) {
|
||||
case PM_DEVICE_ACTION_RESUME:
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_ENABLE_REG,
|
||||
TMD2620_ENABLE_PON, TMD2620_ENABLE_PON);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed enabling sensor.");
|
||||
return ret;
|
||||
}
|
||||
break;
|
||||
|
||||
case PM_DEVICE_ACTION_SUSPEND:
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_ENABLE_REG,
|
||||
TMD2620_ENABLE_PON, 0);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Failed suspending sensor.");
|
||||
return ret;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
static const struct sensor_driver_api tmd2620_driver_api = {
|
||||
.sample_fetch = tmd2620_sample_fetch,
|
||||
.channel_get = tmd2620_channel_get,
|
||||
#ifdef CONFIG_TMD2620_TRIGGER
|
||||
.attr_set = tmd2620_attr_set,
|
||||
.trigger_set = tmd2620_trigger_set,
|
||||
#endif
|
||||
};
|
||||
|
||||
#define TMD2620_INIT_INST(n) \
|
||||
struct tmd2620_data tmd2620_data_##n; \
|
||||
static const struct tmd2620_config tmd2620_config_##n = { \
|
||||
.i2c = I2C_DT_SPEC_INST_GET(n), \
|
||||
.int_gpio = GPIO_DT_SPEC_INST_GET(n, int_gpios), \
|
||||
.proximity_gain = DT_INST_PROP(n, proximity_gain), \
|
||||
.proximity_pulse_length = DT_INST_PROP(n, proximity_pulse_length), \
|
||||
.proximity_pulse_count = DT_INST_PROP(n, proximity_pulse_count), \
|
||||
.proximity_high_threshold = DT_INST_PROP(n, proximity_high_threshold), \
|
||||
.proximity_low_threshold = DT_INST_PROP(n, proximity_low_threshold), \
|
||||
.proximity_led_drive_strength = DT_INST_PROP(n, proximity_led_drive_strength), \
|
||||
.proximity_interrupt_filter = DT_INST_PROP(n, proximity_interrupt_filter), \
|
||||
.enable_wait_mode = DT_INST_PROP(n, enable_wait_mode), \
|
||||
.wait_time_factor = DT_INST_PROP(n, wait_time_factor), \
|
||||
.wait_long = DT_INST_PROP(n, wait_long), \
|
||||
}; \
|
||||
\
|
||||
PM_DEVICE_DT_INST_DEFINE(n, tmd2620_pm_action); \
|
||||
SENSOR_DEVICE_DT_INST_DEFINE(n, tmd2620_init, PM_DEVICE_DT_INST_GET(n), &tmd2620_data_##n, \
|
||||
&tmd2620_config_##n, POST_KERNEL, \
|
||||
CONFIG_SENSOR_INIT_PRIORITY, &tmd2620_driver_api);
|
||||
|
||||
DT_INST_FOREACH_STATUS_OKAY(TMD2620_INIT_INST);
|
177
drivers/sensor/tmd2620/tmd2620.h
Normal file
177
drivers/sensor/tmd2620/tmd2620.h
Normal file
|
@ -0,0 +1,177 @@
|
|||
/*
|
||||
* Copyright (c) 2023 PHYTEC Messtechnik GmbH
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
|
||||
#ifndef ZEPHYR_DRIVERS_SENSOR_TMD2620_TMD2620_H_
|
||||
#define ZEPHYR_DRIVERS_SENSOR_TMD2620_TMD2620_H_
|
||||
|
||||
#define TMD2620_CHIP_ID (0b110101 << 2)
|
||||
|
||||
#define TMD2620_ENABLE_REG 0x80
|
||||
#define TMD2620_ENABLE_WEN BIT(3)
|
||||
#define TMD2620_ENABLE_PEN BIT(2)
|
||||
#define TMD2620_ENABLE_PON BIT(0)
|
||||
|
||||
/* PRATE register defines the time between proximity measurements
|
||||
* while averaging is turned on.
|
||||
* Formular: time between measurements = REG_VALUE * 88µs
|
||||
*/
|
||||
#define TMD2620_PRATE_REG 0x82
|
||||
|
||||
/*
|
||||
* WTIME Register defines the wait time between measurements.
|
||||
* Formular: Wait time = (REG_VALUE + 1) * 2.81ms
|
||||
* If the WLONG bit is set:
|
||||
* Formular: Wait time = (REG_VALUE + 1) * 33.8ms
|
||||
*/
|
||||
#define TMD2620_WTIME_REG 0x83
|
||||
|
||||
/*
|
||||
* PILT Register defines the low interrupt threshold.
|
||||
* If the value generated by the proximity channel is below the
|
||||
* threshold, PPERS value is reached and PIEN is enabled, the INT pin will be asserted
|
||||
*/
|
||||
#define TMD2620_PILT_REG 0x88
|
||||
|
||||
/*
|
||||
* PILT Register defines the high interrupt threshold.
|
||||
* If the value generated by the proximity channel is above the
|
||||
* threshold, PPERS value is reached and PIEN is enabled, the INT pin will be asserted
|
||||
*/
|
||||
#define TMD2620_PIHT_REG 0x8A
|
||||
|
||||
/*
|
||||
* PERS register controls the interrupt filtering capabilities.
|
||||
* With the PPERS bits its possible to configure how many values out of
|
||||
* the threshold have to be generated until a interrupt is generated.
|
||||
* 0: every read cycle
|
||||
* 1: any proximiy value outside of range
|
||||
* 2: 2 consecutive values outside of range
|
||||
* ...
|
||||
*/
|
||||
#define TMD2620_PERS_REG 0x8C
|
||||
#define TMD2620_PERS_PPERS (BIT(4) | BIT(5) | BIT(6) | BIT(7))
|
||||
|
||||
#define TMD2620_CFG0_REG 0x8D
|
||||
#define TMD2620_CFG0_WLONG BIT(2)
|
||||
|
||||
#define TMD2620_PCFG0_REG 0x8E
|
||||
/* pulse length */
|
||||
#define TMD2620_PCFG0_PPULSE_LEN_4US 0
|
||||
#define TMD2620_PCFG0_PPULSE_LEN_8US BIT(6)
|
||||
#define TMD2620_PCFG0_PPULSE_LEN_16US BIT(7)
|
||||
#define TMD2620_PCFG0_PPULSE_LEN_32US (BIT(6) | BIT(7))
|
||||
#define TMD2620_PCFG0_PPULSE_LEN_MASK TMD2620_PCFG0_PPULSE_LEN_32US
|
||||
/* maximum number of pulses */
|
||||
#define TMD2620_PCFG0_PPULSE (BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5))
|
||||
|
||||
#define TMD2620_PCFG1_REG 0x8F
|
||||
/* proximity gain control */
|
||||
#define TMD2620_PCFG1_PGAIN_X1 0
|
||||
#define TMD2620_PCFG1_PGAIN_X2 BIT(6)
|
||||
#define TMD2620_PCFG1_PGAIN_X4 BIT(7)
|
||||
#define TMD2620_PCFG1_PGAIN_X8 (BIT(6) | BIT(7))
|
||||
#define TMD2620_PCFG1_PLDRIVE (BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4))
|
||||
|
||||
#define TMD2620_REVID_REG 0x91
|
||||
|
||||
#define TMD2620_ID_REG 0x92
|
||||
|
||||
#define TMD2620_STATUS_REG 0x93
|
||||
#define TMD2620_STATUS_PSAT BIT(6)
|
||||
#define TMD2620_STATUS_PINT BIT(5)
|
||||
#define TMD2620_STATUS_CINT BIT(3)
|
||||
#define TMD2620_STATUS_ZINT BIT(2)
|
||||
#define TMD2620_STATUS_PSAT_REFLECTIVE BIT(1)
|
||||
#define TMD2620_STATUS_PSAT_AMBIENT BIT(0)
|
||||
|
||||
/* PDATA contains the 1-byte proximity data */
|
||||
#define TMD2620_PDATA_REG 0x9C
|
||||
|
||||
#define TMD2620_REVID2_REG 0x9E
|
||||
#define TMD2620_REVID2_AUX_ID (BIT(0) | BIT(1) | BIT(2) | BIT(3))
|
||||
|
||||
#define TMD2620_CFG3_REG 0xAB
|
||||
#define TMD2620_CFG3_INT_READ_CLEAR BIT(7)
|
||||
#define TMD2620_CFG3_SAI BIT(4)
|
||||
|
||||
#define TMD2620_POFFSET_L_REG 0xC0
|
||||
|
||||
#define TMD2620_POFFSET_H_REG 0xC1
|
||||
|
||||
#define TMD2620_CALIB_REG 0xD7
|
||||
#define TMD2620_CALIB_ELECTRICAL BIT(5)
|
||||
#define TMD2620_CALIB_START_OFFSET_CALIB BIT(0)
|
||||
|
||||
#define TMD2620_CALIBCFG_REG 0xD9
|
||||
#define TMD2620_CALIBCFG_BINSRCH_TARGET (BIT(5) | BIT(6) | BIT(7))
|
||||
#define TMD2620_CALIBCFG_PROX_AUTO_OFFSET_ADJUST BIT(3)
|
||||
#define TMD2620_CALIBCFG_PRX_DATA_AVG (BIT(0) | BIT(1) | BIT(2))
|
||||
|
||||
#define TMD2620_CALIBSTAT_REG 0xDC
|
||||
#define TMD2620_CALIBSTAT_OFFSET_ADJUSTED BIT(2)
|
||||
#define TMD2620_CALIBSTAT_CALIB_FINISHED BIT(0)
|
||||
|
||||
#define TMD2620_INTENAB_REG 0xDD
|
||||
#define TMD2620_INTENAB_PSIEN BIT(6)
|
||||
#define TMD2620_INTENAB_PIEN BIT(5)
|
||||
#define TMD2620_INTENAB_CIEN BIT(3)
|
||||
#define TMD2620_INTENAB_ZIEN BIT(2)
|
||||
|
||||
#define TMD2620_PGAIN_DEFAULT TMD2620_PCFG1_PGAIN_X4
|
||||
#define TMD2620_PLDRIVE_DEFAULT 7
|
||||
#define TMD2620_PPULSE_DEFAULT 15
|
||||
#define TMD2620_PPULSE_LEN_DEFAULT TMD2620_PCFG0_PPULSE_LEN_16US
|
||||
|
||||
struct tmd2620_config {
|
||||
struct i2c_dt_spec i2c;
|
||||
struct gpio_dt_spec int_gpio;
|
||||
uint8_t inst;
|
||||
uint8_t proximity_gain;
|
||||
uint8_t proximity_pulse_length;
|
||||
uint8_t proximity_pulse_count;
|
||||
uint8_t proximity_high_threshold;
|
||||
uint8_t proximity_low_threshold;
|
||||
uint8_t proximity_led_drive_strength;
|
||||
uint8_t proximity_interrupt_filter;
|
||||
uint8_t enable_wait_mode;
|
||||
uint8_t wait_time_factor;
|
||||
uint8_t wait_long;
|
||||
};
|
||||
|
||||
struct tmd2620_data {
|
||||
struct gpio_callback gpio_cb;
|
||||
const struct device *dev;
|
||||
uint8_t pdata;
|
||||
|
||||
#ifdef CONFIG_TMD2620_TRIGGER
|
||||
sensor_trigger_handler_t p_th_handler;
|
||||
const struct sensor_trigger *p_th_trigger;
|
||||
struct k_work work;
|
||||
#else
|
||||
struct k_sem data_sem;
|
||||
#endif
|
||||
};
|
||||
|
||||
static void tmd2620_setup_int(const struct tmd2620_config *config, bool enable)
|
||||
{
|
||||
unsigned int flags = enable ? GPIO_INT_EDGE_TO_ACTIVE : GPIO_INT_DISABLE;
|
||||
|
||||
gpio_pin_interrupt_configure_dt(&config->int_gpio, flags);
|
||||
}
|
||||
|
||||
#ifdef CONFIG_TMD2620_TRIGGER
|
||||
void tmd2620_work_cb(struct k_work *work);
|
||||
|
||||
int tmd2620_attr_set(const struct device *dev, enum sensor_channel chan, enum sensor_attribute attr,
|
||||
const struct sensor_value *val);
|
||||
|
||||
int tmd2620_trigger_set(const struct device *dev, const struct sensor_trigger *trigg,
|
||||
sensor_trigger_handler_t handler);
|
||||
#endif
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_TMD2620_TMD2620_H_ */
|
95
drivers/sensor/tmd2620/tmd2620_trigger.c
Normal file
95
drivers/sensor/tmd2620/tmd2620_trigger.c
Normal file
|
@ -0,0 +1,95 @@
|
|||
/*
|
||||
* Copyright (c) 2023 PHYTEC Messtechnik GmbH
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/device.h>
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/drivers/i2c.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/sys/util.h>
|
||||
|
||||
#include "tmd2620.h"
|
||||
|
||||
LOG_MODULE_DECLARE(TMD2620, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
extern struct tmd2620_data tmd2620_driver;
|
||||
|
||||
void tmd2620_work_cb(struct k_work *work)
|
||||
{
|
||||
LOG_DBG("Work callback was called back.");
|
||||
|
||||
struct tmd2620_data *data = CONTAINER_OF(work, struct tmd2620_data, work);
|
||||
const struct device *dev = data->dev;
|
||||
|
||||
if (data->p_th_handler != NULL) {
|
||||
data->p_th_handler(dev, data->p_th_trigger);
|
||||
}
|
||||
|
||||
tmd2620_setup_int(dev->config, true);
|
||||
}
|
||||
|
||||
int tmd2620_attr_set(const struct device *dev, enum sensor_channel chan, enum sensor_attribute attr,
|
||||
const struct sensor_value *val)
|
||||
{
|
||||
LOG_DBG("Setting sensor attributes.");
|
||||
|
||||
const struct tmd2620_config *config = dev->config;
|
||||
int ret;
|
||||
|
||||
if (chan != SENSOR_CHAN_PROX) {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
if (attr == SENSOR_ATTR_UPPER_THRESH) {
|
||||
ret = i2c_reg_write_byte_dt(&config->i2c, TMD2620_PIHT_REG,
|
||||
(255 - (uint8_t)val->val1));
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
if (attr == SENSOR_ATTR_LOWER_THRESH) {
|
||||
return i2c_reg_write_byte_dt(&config->i2c, TMD2620_PILT_REG, (uint8_t)val->val1);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int tmd2620_trigger_set(const struct device *dev, const struct sensor_trigger *trigg,
|
||||
sensor_trigger_handler_t handler)
|
||||
{
|
||||
LOG_DBG("Setting trigger handler.");
|
||||
|
||||
const struct tmd2620_config *config = dev->config;
|
||||
struct tmd2620_data *data = dev->data;
|
||||
int ret;
|
||||
|
||||
tmd2620_setup_int(dev->config, false);
|
||||
|
||||
if (trigg->type != SENSOR_TRIG_THRESHOLD) {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
if (trigg->chan != SENSOR_CHAN_PROX) {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
data->p_th_trigger = trigg;
|
||||
data->p_th_handler = handler;
|
||||
ret = i2c_reg_update_byte_dt(&config->i2c, TMD2620_INTENAB_REG,
|
||||
TMD2620_INTENAB_PIEN, TMD2620_INTENAB_PIEN);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
tmd2620_setup_int(config, true);
|
||||
if (gpio_pin_get_dt(&config->int_gpio) > 0) {
|
||||
k_work_submit(&data->work);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
80
dts/bindings/sensor/ams,tmd2620.yaml
Normal file
80
dts/bindings/sensor/ams,tmd2620.yaml
Normal file
|
@ -0,0 +1,80 @@
|
|||
# Copyright (c) 2023 Phytec Messtechnik GmbH
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
description: OSRAM ams TMD2620 Proximity Sensor
|
||||
|
||||
compatible: "ams,tmd2620"
|
||||
|
||||
include: [sensor-device.yaml, i2c-device.yaml]
|
||||
|
||||
properties:
|
||||
int-gpios:
|
||||
type: phandle-array
|
||||
required: true
|
||||
description: |
|
||||
The interrupt pin of TMD2620 is open-drain, active low.
|
||||
If connected directly the MCU, the pin should be configured
|
||||
as pull-up, active low.
|
||||
|
||||
proximity-gain:
|
||||
type: int
|
||||
required: true
|
||||
description: Proximity detection gain of the sensor
|
||||
enum:
|
||||
- 1
|
||||
- 2
|
||||
- 4
|
||||
- 8
|
||||
|
||||
proximity-pulse-length:
|
||||
type: int
|
||||
required: true
|
||||
description: IR led pulse length in ms
|
||||
enum:
|
||||
- 4
|
||||
- 8
|
||||
- 16
|
||||
- 32
|
||||
|
||||
proximity-pulse-count:
|
||||
type: int
|
||||
required: true
|
||||
description: count of IR led pulses (min. 1; max. 64)
|
||||
|
||||
proximity-high-threshold:
|
||||
type: int
|
||||
description: high threshold for interrupt. (min. 0; max. 255)
|
||||
|
||||
proximity-low-threshold:
|
||||
type: int
|
||||
description: low threshold for interrupt. (min. 0; max. 255)
|
||||
|
||||
proximity-led-drive-strength:
|
||||
type: int
|
||||
required: true
|
||||
description: |
|
||||
LED drive strength in multiples of 6mA (min. 0; max. 31)
|
||||
|
||||
proximity-interrupt-filter:
|
||||
type: int
|
||||
description: |
|
||||
filters proximity interrupt. (min. 0; max. 15)
|
||||
|
||||
0-> every cycle fires an interrupt
|
||||
1-> 1 consecutive proximity value out of threshold range fires an interrupt
|
||||
2-> 2 consecutive proximity values out of threshold range fires an interrupt
|
||||
...
|
||||
|
||||
enable-wait-mode:
|
||||
type: boolean
|
||||
description: Enables wait mode
|
||||
|
||||
wait-time-factor:
|
||||
type: int
|
||||
description: |
|
||||
time the sensor waits between proximity cycles. (min. 0; max 255).
|
||||
given in multiples of 2.81 starting at 0 for 2.81ms wait time.
|
||||
|
||||
wait-long:
|
||||
type: boolean
|
||||
description: increases the wait time by a factor of x12
|
|
@ -671,3 +671,17 @@ test_i2c_vl53l1x: vl53l1x@68 {
|
|||
int-gpios = <&test_gpio 0 0>;
|
||||
xshut-gpios = <&test_gpio 0 0>;
|
||||
};
|
||||
|
||||
test_i2c_tmd2620: tmd2620@69 {
|
||||
compatible = "ams,tmd2620";
|
||||
reg = <0x69>;
|
||||
int-gpios = <&test_gpio 0 0>;
|
||||
proximity-gain = <4>;
|
||||
proximity-pulse-length = <16>;
|
||||
proximity-pulse-count = <15>;
|
||||
proximity-high-threshold = <255>;
|
||||
proximity-low-threshold = <0>;
|
||||
proximity-led-drive-strength = <4>;
|
||||
proximity-interrupt-filter = <0>;
|
||||
wait-time-factor = <0>;
|
||||
};
|
||||
|
|
Loading…
Reference in a new issue