michaelh/zephyr
1
0
Fork 0
Code Releases Activity

drivers/sensor: lsm6dso: Fix shub for multi-instance case

Browse source 
In case of multi-instance the driver tries to discover whether
there are devices attached to SDx/SCx sensorhub bus. If not it
just turns the shub_inited variable (inside data structure) to false
and skips doing any further sensorhub related action for that
particular LSM6DSO device instance, regardless the fact that the macro
CONFIG_LSM6DSO_SENSORHUB is enabled.

Moreover, the info found during the enumeration process for a
particular instance (number and types of attached devices) must be
saved inside the per-instance data structure, so that more than one
LSM6DSO device can be used as a sensorhub without interfering with
the others.

Signed-off-by: Armando Visconti <armando.visconti@st.com>
This commit is contained in:
Armando Visconti 2021-05-07 15:41:35 +02:00 • committed by Kumar Gala
commit 1325525e3b
3 changed files with 71 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

54
drivers/sensor/lsm6dso/lsm6dso.c
View file

@ -286,6 +286,10 @@ static int lsm6dso_attr_set(const struct device *dev,
enum sensor_attribute attr,
const struct sensor_value *val)
{
#if defined(CONFIG_LSM6DSO_SENSORHUB)
struct lsm6dso_data *data = dev->data;
#endif /* CONFIG_LSM6DSO_SENSORHUB */
switch (chan) {
case SENSOR_CHAN_ACCEL_XYZ:
return lsm6dso_accel_config(dev, chan, attr, val);
@ -295,6 +299,11 @@ static int lsm6dso_attr_set(const struct device *dev,
case SENSOR_CHAN_MAGN_XYZ:
case SENSOR_CHAN_PRESS:
case SENSOR_CHAN_HUMIDITY:
if (!data->shub_inited) {
LOG_ERR("shub not inited.");
return -ENOTSUP;
}
return lsm6dso_shub_config(dev, chan, attr, val);
#endif /* CONFIG_LSM6DSO_SENSORHUB */
default:
@ -377,12 +386,13 @@ static int lsm6dso_sample_fetch_shub(const struct device *dev)
static int lsm6dso_sample_fetch(const struct device *dev,
enum sensor_channel chan)
{
#if defined(CONFIG_LSM6DSO_SENSORHUB)
struct lsm6dso_data *data = dev->data;
#endif /* CONFIG_LSM6DSO_SENSORHUB */
switch (chan) {
case SENSOR_CHAN_ACCEL_XYZ:
lsm6dso_sample_fetch_accel(dev);
#if defined(CONFIG_LSM6DSO_SENSORHUB)
lsm6dso_sample_fetch_shub(dev);
#endif
break;
case SENSOR_CHAN_GYRO_XYZ:
lsm6dso_sample_fetch_gyro(dev);
@ -399,7 +409,9 @@ static int lsm6dso_sample_fetch(const struct device *dev,
lsm6dso_sample_fetch_temp(dev);
#endif
#if defined(CONFIG_LSM6DSO_SENSORHUB)
lsm6dso_sample_fetch_shub(dev);
if (data->shub_inited) {
lsm6dso_sample_fetch_shub(dev);
}
#endif
break;
default:
@ -536,7 +548,7 @@ static inline int lsm6dso_magn_get_channel(enum sensor_channel chan,
int16_t sample[3];
int idx;
idx = lsm6dso_shub_get_idx(SENSOR_CHAN_MAGN_XYZ);
idx = lsm6dso_shub_get_idx(data->dev, SENSOR_CHAN_MAGN_XYZ);
if (idx < 0) {
LOG_DBG("external magn not supported");
return -ENOTSUP;
@ -580,7 +592,7 @@ static inline void lsm6dso_hum_convert(struct sensor_value *val,
struct hts221_data *ht = &data->hts221;
int idx;
idx = lsm6dso_shub_get_idx(SENSOR_CHAN_HUMIDITY);
idx = lsm6dso_shub_get_idx(data->dev, SENSOR_CHAN_HUMIDITY);
if (idx < 0) {
LOG_DBG("external press/temp not supported");
return;
@ -604,7 +616,7 @@ static inline void lsm6dso_press_convert(struct sensor_value *val,
int32_t raw_val;
int idx;
idx = lsm6dso_shub_get_idx(SENSOR_CHAN_PRESS);
idx = lsm6dso_shub_get_idx(data->dev, SENSOR_CHAN_PRESS);
if (idx < 0) {
LOG_DBG("external press/temp not supported");
return;
@ -627,7 +639,7 @@ static inline void lsm6dso_temp_convert(struct sensor_value *val,
int16_t raw_val;
int idx;
idx = lsm6dso_shub_get_idx(SENSOR_CHAN_PRESS);
idx = lsm6dso_shub_get_idx(data->dev, SENSOR_CHAN_PRESS);
if (idx < 0) {
LOG_DBG("external press/temp not supported");
return;
@ -671,18 +683,38 @@ static int lsm6dso_channel_get(const struct device *dev,
case SENSOR_CHAN_MAGN_Y:
case SENSOR_CHAN_MAGN_Z:
case SENSOR_CHAN_MAGN_XYZ:
if (!data->shub_inited) {
LOG_ERR("attr_set() shub not inited.");
return -ENOTSUP;
}
lsm6dso_magn_get_channel(chan, val, data);
break;
case SENSOR_CHAN_HUMIDITY:
if (!data->shub_inited) {
LOG_ERR("attr_set() shub not inited.");
return -ENOTSUP;
}
lsm6dso_hum_convert(val, data);
break;
case SENSOR_CHAN_PRESS:
if (!data->shub_inited) {
LOG_ERR("attr_set() shub not inited.");
return -ENOTSUP;
}
lsm6dso_press_convert(val, data);
break;
case SENSOR_CHAN_AMBIENT_TEMP:
if (!data->shub_inited) {
LOG_ERR("attr_set() shub not inited.");
return -ENOTSUP;
}
lsm6dso_temp_convert(val, data);
break;
#endif
@ -774,6 +806,7 @@ static int lsm6dso_init(const struct device *dev)
#endif
struct lsm6dso_data *data = dev->data;
LOG_INF("Initialize device %s", dev->name);
data->dev = dev;
#ifdef CONFIG_LSM6DSO_TRIGGER
@ -791,9 +824,10 @@ static int lsm6dso_init(const struct device *dev)
}
#ifdef CONFIG_LSM6DSO_SENSORHUB
data->shub_inited = true;
if (lsm6dso_shub_init(dev) < 0) {
LOG_DBG("failed to initialize external chip");
return -EIO;
LOG_INF("shub: no external chips found");
data->shub_inited = false;
}
#endif

5
drivers/sensor/lsm6dso/lsm6dso.h
View file

@ -145,6 +145,9 @@ struct lsm6dso_data {
int16_t y0;
int16_t y1;
} hts221;
bool shub_inited;
uint8_t num_ext_dev;
uint8_t shub_ext[LSM6DSO_SHUB_MAX_NUM_SLVS];
#endif /* CONFIG_LSM6DSO_SENSORHUB */
uint16_t accel_freq;
@ -171,7 +174,7 @@ struct lsm6dso_data {
#if defined(CONFIG_LSM6DSO_SENSORHUB)
int lsm6dso_shub_init(const struct device *dev);
int lsm6dso_shub_fetch_external_devs(const struct device *dev);
int lsm6dso_shub_get_idx(enum sensor_channel type);
int lsm6dso_shub_get_idx(const struct device *dev, enum sensor_channel type);
int lsm6dso_shub_config(const struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val);

42
drivers/sensor/lsm6dso/lsm6dso_shub.c
View file

@ -46,9 +46,6 @@ LOG_MODULE_DECLARE(LSM6DSO, CONFIG_SENSOR_LOG_LEVEL);
#define LSM6DSO_SHUB_SLVX_WRITE 0x0
#define LSM6DSO_SHUB_SLVX_READ 0x1
static uint8_t num_ext_dev;
static uint8_t shub_ext[LSM6DSO_SHUB_MAX_NUM_SLVS];
static int lsm6dso_shub_write_slave_reg(const struct device *dev,
uint8_t slv_addr, uint8_t slv_reg,
uint8_t *value, uint16_t len);
@ -630,14 +627,15 @@ static int lsm6dso_shub_write_slave_reg(const struct device *dev,
*/
static int lsm6dso_shub_set_data_channel(const struct device *dev)
{
struct lsm6dso_data *data = dev->data;
const struct lsm6dso_config *cfg = dev->config;
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
uint8_t n, i, slv_cfg[6];
struct lsm6dso_shub_slist *sp;
/* Set data channel for slave devices */
for (n = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
for (n = 0; n < data->num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[data->shub_ext[n]];
i = n * 3;
slv_cfg[i] = (sp->ext_i2c_addr << 1) | LSM6DSO_SHUB_SLVX_READ;
@ -675,18 +673,20 @@ static int lsm6dso_shub_set_data_channel(const struct device *dev)
return 0;
}
int lsm6dso_shub_get_idx(enum sensor_channel type)
int lsm6dso_shub_get_idx(const struct device *dev, enum sensor_channel type)
{
uint8_t n;
struct lsm6dso_data *data = dev->data;
struct lsm6dso_shub_slist *sp;
for (n = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
for (n = 0; n < data->num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[data->shub_ext[n]];
if (sp->type == type)
return n;
}
LOG_ERR("shub: dev %s type %d not supported", dev->name, type);
return -ENOTSUP;
}
@ -701,8 +701,8 @@ int lsm6dso_shub_fetch_external_devs(const struct device *dev)
/* read data from external slave */
lsm6dso_shub_embedded_en(ctx, true);
for (n = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
for (n = 0; n < data->num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[data->shub_ext[n]];
if (lsm6dso_read_reg(ctx, sp->sh_out_reg,
data->ext_data[n], sp->out_data_len) < 0) {
@ -721,18 +721,19 @@ int lsm6dso_shub_config(const struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr,
const struct sensor_value *val)
{
struct lsm6dso_data *data = dev->data;
struct lsm6dso_shub_slist *sp = NULL;
uint8_t n;
for (n = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
for (n = 0; n < data->num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[data->shub_ext[n]];
if (sp->type == chan)
break;
}
if (n == num_ext_dev) {
LOG_DBG("shub: chan not supported");
if (n == data->num_ext_dev) {
LOG_DBG("shub: %s chan %d not supported", dev->name, chan);
return -ENOTSUP;
}
@ -746,12 +747,14 @@ int lsm6dso_shub_config(const struct device *dev, enum sensor_channel chan,
int lsm6dso_shub_init(const struct device *dev)
{
struct lsm6dso_data *data = dev->data;
uint8_t i, n = 0, regn;
uint8_t chip_id;
struct lsm6dso_shub_slist *sp;
LOG_INF("shub: start sensorhub for %s", dev->name);
for (n = 0; n < ARRAY_SIZE(lsm6dso_shub_slist); n++) {
if (num_ext_dev >= LSM6DSO_SHUB_MAX_NUM_SLVS)
if (data->num_ext_dev >= LSM6DSO_SHUB_MAX_NUM_SLVS)
break;
chip_id = 0;
@ -782,17 +785,18 @@ int lsm6dso_shub_init(const struct device *dev)
LOG_INF("shub: Ext Device Chip Id: %02x", chip_id);
sp->ext_i2c_addr = sp->i2c_addr[i];
shub_ext[num_ext_dev++] = n;
data->shub_ext[data->num_ext_dev++] = n;
}
if (num_ext_dev == 0) {
LOG_DBG("shub: dev %s - num_ext_dev %d", dev->name, data->num_ext_dev);
if (data->num_ext_dev == 0) {
LOG_ERR("shub: no slave devices found");
return -EINVAL;
}
/* init external devices */
for (n = 0, regn = 0; n < num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[shub_ext[n]];
for (n = 0, regn = 0; n < data->num_ext_dev; n++) {
sp = &lsm6dso_shub_slist[data->shub_ext[n]];
sp->sh_out_reg = LSM6DSO_SHUB_DATA_OUT + regn;
regn += sp->out_data_len;
sp->dev_init(dev, sp->ext_i2c_addr);