static int set_data_rate(const struct motion_sensor_t *s,
int rate,
int rnd)
{
int ret, val, odr_tbl_size;
uint8_t ctrl_reg, reg_val;
const struct accel_param_pair *data_rates;
struct lsm6ds0_data *data = s->drv_data;
ctrl_reg = get_ctrl_reg(s->type);
data_rates = get_odr_table(s->type, &odr_tbl_size);
reg_val = get_reg_val(rate, rnd, data_rates, odr_tbl_size);
/*
* Lock accel resource to prevent another task from attempting
* to write accel parameters until we are done.
*/
mutex_lock(s->mutex);
ret = raw_read8(s->addr, ctrl_reg, &val);
if (ret != EC_SUCCESS)
goto accel_cleanup;
val = (val & ~LSM6DS0_ODR_MASK) | reg_val;
ret = raw_write8(s->addr, ctrl_reg, val);
/* Now that we have set the odr, update the driver's value. */
if (ret == EC_SUCCESS)
data->base.odr = get_engineering_val(reg_val, data_rates,
odr_tbl_size);
/* CTRL_REG3_G 12h
* [7] low-power mode = 0;
* [6] high pass filter disabled;
* [5:4] 0 keep const 0
* [3:0] HPCF_G
* Table 48 Gyroscope high-pass filter cutoff frequency
*/
if (MOTIONSENSE_TYPE_GYRO == s->type) {
ret = raw_read8(s->addr, LSM6DS0_CTRL_REG3_G, &val);
if (ret != EC_SUCCESS)
goto accel_cleanup;
val &= ~(0x3 << 4); /* clear bit [5:4] */
val = (rate > 119000) ?
(val | (1<<7)) /* set high-power mode */ :
(val & ~(1<<7)); /* set low-power mode */
ret = raw_write8(s->addr, LSM6DS0_CTRL_REG3_G, val);
}
accel_cleanup:
mutex_unlock(s->mutex);
return ret;
}
static int set_data_rate(const struct motion_sensor_t *s,
int rate,
int rnd)
{
int ret, val, odr_tbl_size;
uint8_t ctrl_reg, reg_val;
const struct gyro_param_pair *data_rates;
struct l3gd20_data *data = s->drv_data;
ctrl_reg = get_ctrl_reg(s->type);
data_rates = get_odr_table(s->type, &odr_tbl_size);
reg_val = get_reg_val(rate, rnd, data_rates, odr_tbl_size);
/*
* Lock gyro resource to prevent another task from attempting
* to write gyro parameters until we are done.
*/
mutex_lock(s->mutex);
ret = raw_read8(s->port, s->addr, ctrl_reg, &val);
if (ret != EC_SUCCESS)
goto gyro_cleanup;
val = (val & ~(L3GD20_ODR_MASK | L3GD20_ODR_PD_MASK)) |
(reg_val & ~L3GD20_LOW_ODR_MASK);
ret = raw_write8(s->port, s->addr, ctrl_reg, val);
/* Now that we have set the odr, update the driver's value. */
if (ret == EC_SUCCESS)
data->base.odr = get_engineering_val(reg_val, data_rates,
odr_tbl_size);
ret = raw_read8(s->port, s->addr, L3GD20_LOW_ODR, &val);
if (ret != EC_SUCCESS)
goto gyro_cleanup;
/* We need to clear low_ODR bit for higher data rates */
if (reg_val & L3GD20_LOW_ODR_MASK)
val |= 1;
else
val &= ~1;
ret = raw_write8(s->port, s->addr, L3GD20_LOW_ODR, val);
if (ret != EC_SUCCESS)
goto gyro_cleanup;
/* CTRL_REG5 24h
* [7] low-power mode = 0;
* [6] fifo disabled = 0;
* [5] Stop on fth = 0;
* [4] High pass filter enable = 1;
* [3:2] int1_sel = 0;
* [1:0] out_sel = 1;
*/
ret = raw_read8(s->port, s->addr, L3GD20_CTRL_REG5, &val);
if (ret != EC_SUCCESS)
goto gyro_cleanup;
val |= (1 << 4); /* high-pass filter enabled */
val |= (1 << 0); /* data in data reg are high-pass filtered */
ret = raw_write8(s->port, s->addr, L3GD20_CTRL_REG5, val);
if (ret != EC_SUCCESS)
goto gyro_cleanup;
ret = raw_read8(s->port, s->addr, L3GD20_CTRL_REG2, &val);
if (ret != EC_SUCCESS)
goto gyro_cleanup;
/*
* Table 25. High pass filter mode configuration
* Table 26. High pass filter cut off frequency configuration
*/
val &= 0xf0;
val |= 0x04;
ret = raw_write8(s->port, s->addr, L3GD20_CTRL_REG2, val);
gyro_cleanup:
mutex_unlock(s->mutex);
return ret;
}
