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

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

View file

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

View file

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

View file

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