static int set_range(const struct motion_sensor_t *s,
int range,
int rnd)
{
int ret, ctrl_val, range_tbl_size;
uint8_t ctrl_reg, reg_val;
const struct gyro_param_pair *ranges;
struct l3gd20_data *data = (struct l3gd20_data *)s->drv_data;
ctrl_reg = L3GD20_CTRL_REG4;
ranges = get_range_table(s->type, &range_tbl_size);
reg_val = get_reg_val(range, rnd, ranges, range_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, &ctrl_val);
if (ret != EC_SUCCESS)
goto gyro_cleanup;
ctrl_val = (ctrl_val & ~L3GD20_RANGE_MASK) | reg_val;
ret = raw_write8(s->port, s->addr, ctrl_reg, ctrl_val);
/* Now that we have set the range, update the driver's value. */
if (ret == EC_SUCCESS)
data->base.range = get_engineering_val(reg_val, ranges,
range_tbl_size);
gyro_cleanup:
mutex_unlock(s->mutex);
return EC_SUCCESS;
}
static int set_range(const struct motion_sensor_t *s,
int range,
int rnd)
{
int ret, ctrl_val, range_tbl_size;
uint8_t ctrl_reg, reg_val;
const struct accel_param_pair *ranges;
struct lsm6ds0_data *data = s->drv_data;
ctrl_reg = get_ctrl_reg(s->type);
ranges = get_range_table(s->type, &range_tbl_size);
reg_val = get_reg_val(range, rnd, ranges, range_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, &ctrl_val);
if (ret != EC_SUCCESS)
goto accel_cleanup;
ctrl_val = (ctrl_val & ~LSM6DS0_RANGE_MASK) | reg_val;
ret = raw_write8(s->addr, ctrl_reg, ctrl_val);
/* Now that we have set the range, update the driver's value. */
if (ret == EC_SUCCESS)
data->base.range = get_engineering_val(reg_val, ranges,
range_tbl_size);
accel_cleanup:
mutex_unlock(s->mutex);
return ret;
}
