static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
{
u16 max_odr, min_odr, sip = 0, ts_sip = 0;
const struct st_lsm6dsx_reg *ts_dec_reg;
struct st_lsm6dsx_sensor *sensor;
int err = 0, i;
u8 data;
st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
const struct st_lsm6dsx_reg *dec_reg;
sensor = iio_priv(hw->iio_devs[i]);
/* update fifo decimators and sample in pattern */
if (hw->enable_mask & BIT(sensor->id)) {
sensor->sip = sensor->odr / min_odr;
sensor->decimator = max_odr / sensor->odr;
data = st_lsm6dsx_get_decimator_val(sensor->decimator);
} else {
sensor->sip = 0;
sensor->decimator = 0;
data = 0;
}
ts_sip = max_t(u16, ts_sip, sensor->sip);
dec_reg = &hw->settings->decimator[sensor->id];
if (dec_reg->addr) {
int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
err = regmap_update_bits(hw->regmap, dec_reg->addr,
dec_reg->mask, val);
if (err < 0)
return err;
}
sip += sensor->sip;
}
hw->sip = sip + ts_sip;
hw->ts_sip = ts_sip;
/*
* update hw ts decimator if necessary. Decimator for hw timestamp
* is always 1 or 0 in order to have a ts sample for each data
* sample in FIFO
*/
ts_dec_reg = &hw->settings->ts_settings.decimator;
if (ts_dec_reg->addr) {
int val, ts_dec = !!hw->ts_sip;
val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
err = regmap_update_bits(hw->regmap, ts_dec_reg->addr,
ts_dec_reg->mask, val);
}
return err;
}
static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,
bool enable)
{
const struct st_lsm6dsx_shub_settings *hub_settings;
struct st_lsm6dsx_hw *hw = sensor->hw;
unsigned int data;
int err;
/* enable acc sensor as trigger */
err = st_lsm6dsx_sensor_set_enable(sensor, enable);
if (err < 0)
return err;
mutex_lock(&hw->page_lock);
hub_settings = &hw->settings->shub_settings;
err = st_lsm6dsx_set_page(hw, true);
if (err < 0)
goto out;
data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->master_en.mask);
err = regmap_update_bits(hw->regmap, hub_settings->master_en.addr,
hub_settings->master_en.mask, data);
st_lsm6dsx_set_page(hw, false);
out:
mutex_unlock(&hw->page_lock);
return err;
}
/**
* st_lsm6dsx_shub_write - write data to slave device register
*
* Write data from slave device register. SLV0 is used for
* one-shot write operation
*/
static int
st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
u8 *data, int len)
{
const struct st_lsm6dsx_shub_settings *hub_settings;
struct st_lsm6dsx_hw *hw = sensor->hw;
u8 config[2], slv_addr;
int err, i;
hub_settings = &hw->settings->shub_settings;
if (hub_settings->wr_once.addr) {
unsigned int data;
data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask);
err = st_lsm6dsx_shub_write_reg_with_mask(hw,
hub_settings->wr_once.addr,
hub_settings->wr_once.mask,
data);
if (err < 0)
return err;
}
slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
config[0] = sensor->ext_info.addr << 1;
for (i = 0 ; i < len; i++) {
config[1] = addr + i;
err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
sizeof(config));
if (err < 0)
return err;
err = st_lsm6dsx_shub_write_reg(hw, hub_settings->dw_slv0_addr,
&data[i], 1);
if (err < 0)
return err;
err = st_lsm6dsx_shub_master_enable(sensor, true);
if (err < 0)
return err;
st_lsm6dsx_shub_wait_complete(hw);
st_lsm6dsx_shub_master_enable(sensor, false);
}
memset(config, 0, sizeof(config));
return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
}
static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
{
struct st_lsm6dsx_sensor *sensor;
u16 max_odr, min_odr, sip = 0;
int err, i;
u8 data;
st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
const struct st_lsm6dsx_reg *dec_reg;
sensor = iio_priv(hw->iio_devs[i]);
/* update fifo decimators and sample in pattern */
if (hw->enable_mask & BIT(sensor->id)) {
sensor->sip = sensor->odr / min_odr;
sensor->decimator = max_odr / sensor->odr;
data = st_lsm6dsx_get_decimator_val(sensor->decimator);
} else {
sensor->sip = 0;
sensor->decimator = 0;
data = 0;
}
dec_reg = &hw->settings->decimator[sensor->id];
if (dec_reg->addr) {
int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
err = regmap_update_bits(hw->regmap, dec_reg->addr,
dec_reg->mask, val);
if (err < 0)
return err;
}
sip += sensor->sip;
}
hw->sip = sip;
return 0;
}
