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:
parent
fa836cc00d
commit
1325525e3b
3 changed files with 71 additions and 30 deletions
|
@ -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
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue