diff --git a/drivers/sensor/lsm6dso/lsm6dso.c b/drivers/sensor/lsm6dso/lsm6dso.c index eea8c0d2db9..21072003fa8 100644 --- a/drivers/sensor/lsm6dso/lsm6dso.c +++ b/drivers/sensor/lsm6dso/lsm6dso.c @@ -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 diff --git a/drivers/sensor/lsm6dso/lsm6dso.h b/drivers/sensor/lsm6dso/lsm6dso.h index e88123a720c..aa1959bf972 100644 --- a/drivers/sensor/lsm6dso/lsm6dso.h +++ b/drivers/sensor/lsm6dso/lsm6dso.h @@ -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); diff --git a/drivers/sensor/lsm6dso/lsm6dso_shub.c b/drivers/sensor/lsm6dso/lsm6dso_shub.c index fbef812b3c5..4da76b5b886 100644 --- a/drivers/sensor/lsm6dso/lsm6dso_shub.c +++ b/drivers/sensor/lsm6dso/lsm6dso_shub.c @@ -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);