1 /* ST Microelectronics LSM6DSV16X 6-axis IMU sensor driver
5  * SPDX-License-Identifier: Apache-2.0
29  * should be selected through accel-odr property in DT
50 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_freq_to_odr_val()
51 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_freq_to_odr_val()
57 		return -EINVAL;  in lsm6dsv16x_freq_to_odr_val()
64 			LOG_DBG("mode: %d - odr: %d", mode, i);  in lsm6dsv16x_freq_to_odr_val()
69 	return -EINVAL;  in lsm6dsv16x_freq_to_odr_val()
84 	return -EINVAL;  in lsm6dsv16x_accel_range_to_fs_val()
91 int lsm6dsv16x_calc_accel_gain(uint8_t fs)  in lsm6dsv16x_calc_accel_gain()  argument
93 	return lsm6dsv16x_accel_fs_map[fs] * GAIN_UNIT_XL / 2;  in lsm6dsv16x_calc_accel_gain()
96 int lsm6dsv16x_calc_gyro_gain(uint8_t fs)  in lsm6dsv16x_calc_gyro_gain()  argument
98 	return lsm6dsv16x_gyro_fs_sens[fs] * GAIN_UNIT_G;  in lsm6dsv16x_calc_gyro_gain()
111 	return -EINVAL;  in lsm6dsv16x_gyro_range_to_fs_val()
114 static int lsm6dsv16x_accel_set_fs_raw(const struct device *dev, uint8_t fs)  in lsm6dsv16x_accel_set_fs_raw()  argument
116 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_accel_set_fs_raw()
117 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_accel_set_fs_raw()
118 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_accel_set_fs_raw()
121 	switch (fs) {  in lsm6dsv16x_accel_set_fs_raw()
135 		return -EIO;  in lsm6dsv16x_accel_set_fs_raw()
139 		return -EIO;  in lsm6dsv16x_accel_set_fs_raw()
142 	data->accel_fs = fs;  in lsm6dsv16x_accel_set_fs_raw()
149 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_accel_set_odr_raw()
150 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_accel_set_odr_raw()
151 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_accel_set_odr_raw()
154 		return -EIO;  in lsm6dsv16x_accel_set_odr_raw()
157 	data->accel_freq = odr;  in lsm6dsv16x_accel_set_odr_raw()
162 static int lsm6dsv16x_gyro_set_fs_raw(const struct device *dev, uint8_t fs)  in lsm6dsv16x_gyro_set_fs_raw()  argument
164 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_gyro_set_fs_raw()
165 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_gyro_set_fs_raw()
166 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_gyro_set_fs_raw()
168 	if (lsm6dsv16x_gy_full_scale_set(ctx, fs) < 0) {  in lsm6dsv16x_gyro_set_fs_raw()
169 		return -EIO;  in lsm6dsv16x_gyro_set_fs_raw()
172 	data->gyro_fs = fs;  in lsm6dsv16x_gyro_set_fs_raw()
178 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_gyro_set_odr_raw()
179 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_gyro_set_odr_raw()
182 		return -EIO;  in lsm6dsv16x_gyro_set_odr_raw()
199 		return -EIO;  in lsm6dsv16x_accel_odr_set()
207 	int fs;  in lsm6dsv16x_accel_range_set()  local
208 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_accel_range_set()
210 	fs = lsm6dsv16x_accel_range_to_fs_val(range);  in lsm6dsv16x_accel_range_set()
211 	if (fs < 0) {  in lsm6dsv16x_accel_range_set()
212 		return fs;  in lsm6dsv16x_accel_range_set()
215 	if (lsm6dsv16x_accel_set_fs_raw(dev, fs) < 0) {  in lsm6dsv16x_accel_range_set()
216 		LOG_DBG("failed to set accelerometer full-scale");  in lsm6dsv16x_accel_range_set()
217 		return -EIO;  in lsm6dsv16x_accel_range_set()
220 	data->acc_gain = lsm6dsv16x_calc_accel_gain(fs);  in lsm6dsv16x_accel_range_set()
229 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_accel_config()
230 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_accel_config()
237 		return lsm6dsv16x_accel_odr_set(dev, val->val1);  in lsm6dsv16x_accel_config()
239 		switch (val->val1) {  in lsm6dsv16x_accel_config()
262 			return -EIO;  in lsm6dsv16x_accel_config()
268 		return -ENOTSUP;  in lsm6dsv16x_accel_config()
279 		return -EIO;  in lsm6dsv16x_gyro_odr_set()
289 		return -EIO;  in lsm6dsv16x_gyro_odr_set()
297 	int fs;  in lsm6dsv16x_gyro_range_set()  local
298 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_gyro_range_set()
300 	fs = lsm6dsv16x_gyro_range_to_fs_val(range);  in lsm6dsv16x_gyro_range_set()
301 	if (fs < 0) {  in lsm6dsv16x_gyro_range_set()
302 		return fs;  in lsm6dsv16x_gyro_range_set()
305 	if (lsm6dsv16x_gyro_set_fs_raw(dev, fs) < 0) {  in lsm6dsv16x_gyro_range_set()
306 		LOG_DBG("failed to set gyroscope full-scale");  in lsm6dsv16x_gyro_range_set()
307 		return -EIO;  in lsm6dsv16x_gyro_range_set()
310 	data->gyro_gain = lsm6dsv16x_calc_gyro_gain(fs);  in lsm6dsv16x_gyro_range_set()
319 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_gyro_config()
320 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_gyro_config()
327 		return lsm6dsv16x_gyro_odr_set(dev, val->val1);  in lsm6dsv16x_gyro_config()
329 		switch (val->val1) {  in lsm6dsv16x_gyro_config()
343 			return -EIO;  in lsm6dsv16x_gyro_config()
348 		LOG_DBG("Gyro attribute not supported.");  in lsm6dsv16x_gyro_config()
349 		return -ENOTSUP;  in lsm6dsv16x_gyro_config()
361 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_attr_set()
373 		if (!data->shub_inited) {  in lsm6dsv16x_attr_set()
375 			return -ENOTSUP;  in lsm6dsv16x_attr_set()
382 		return -ENOTSUP;  in lsm6dsv16x_attr_set()
393 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_accel_get_config()
394 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_accel_get_config()
395 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_accel_get_config()
399 		sensor_g_to_ms2(lsm6dsv16x_accel_fs_map[data->accel_fs], val);  in lsm6dsv16x_accel_get_config()
406 			return -EINVAL;  in lsm6dsv16x_accel_get_config()
411 		val->val1 = lsm6dsv16x_odr_map[mode][data->accel_freq];  in lsm6dsv16x_accel_get_config()
412 		val->val2 = 0;  in lsm6dsv16x_accel_get_config()
422 			val->val1 = 0;  in lsm6dsv16x_accel_get_config()
425 			val->val1 = 1;  in lsm6dsv16x_accel_get_config()
428 			val->val1 = 3;  in lsm6dsv16x_accel_get_config()
431 			val->val1 = 4;  in lsm6dsv16x_accel_get_config()
434 			val->val1 = 5;  in lsm6dsv16x_accel_get_config()
437 			val->val1 = 6;  in lsm6dsv16x_accel_get_config()
440 			val->val1 = 7;  in lsm6dsv16x_accel_get_config()
443 			return -EIO;  in lsm6dsv16x_accel_get_config()
450 		return -ENOTSUP;  in lsm6dsv16x_accel_get_config()
461 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_gyro_get_config()
462 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_gyro_get_config()
463 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_gyro_get_config()
467 		sensor_degrees_to_rad(lsm6dsv16x_gyro_fs_map[data->gyro_fs], val);  in lsm6dsv16x_gyro_get_config()
474 			return -EINVAL;  in lsm6dsv16x_gyro_get_config()
479 		val->val1 = lsm6dsv16x_odr_map[mode][data->gyro_freq];  in lsm6dsv16x_gyro_get_config()
480 		val->val2 = 0;  in lsm6dsv16x_gyro_get_config()
490 			val->val1 = 0;  in lsm6dsv16x_gyro_get_config()
493 			val->val1 = 1;  in lsm6dsv16x_gyro_get_config()
496 			val->val1 = 4;  in lsm6dsv16x_gyro_get_config()
499 			val->val1 = 5;  in lsm6dsv16x_gyro_get_config()
502 			return -EIO;  in lsm6dsv16x_gyro_get_config()
508 		LOG_DBG("Gyro attribute not supported.");  in lsm6dsv16x_gyro_get_config()
509 		return -ENOTSUP;  in lsm6dsv16x_gyro_get_config()
527 		return -ENOTSUP;  in lsm6dsv16x_attr_get()
535 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_sample_fetch_accel()
536 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_sample_fetch_accel()
537 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_sample_fetch_accel()
539 	if (lsm6dsv16x_acceleration_raw_get(ctx, data->acc) < 0) {  in lsm6dsv16x_sample_fetch_accel()
541 		return -EIO;  in lsm6dsv16x_sample_fetch_accel()
549 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_sample_fetch_gyro()
550 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_sample_fetch_gyro()
551 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_sample_fetch_gyro()
553 	if (lsm6dsv16x_angular_rate_raw_get(ctx, data->gyro) < 0) {  in lsm6dsv16x_sample_fetch_gyro()
555 		return -EIO;  in lsm6dsv16x_sample_fetch_gyro()
564 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_sample_fetch_temp()
565 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_sample_fetch_temp()
566 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_sample_fetch_temp()
568 	if (lsm6dsv16x_temperature_raw_get(ctx, &data->temp_sample) < 0) {  in lsm6dsv16x_sample_fetch_temp()
570 		return -EIO;  in lsm6dsv16x_sample_fetch_temp()
582 		return -EIO;  in lsm6dsv16x_sample_fetch_shub()
593 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_sample_fetch()
615 		if (data->shub_inited) {  in lsm6dsv16x_sample_fetch()
621 		return -ENOTSUP;  in lsm6dsv16x_sample_fetch()
647 		lsm6dsv16x_accel_convert(val, data->acc[0], sensitivity);  in lsm6dsv16x_accel_get_channel()
650 		lsm6dsv16x_accel_convert(val, data->acc[1], sensitivity);  in lsm6dsv16x_accel_get_channel()
653 		lsm6dsv16x_accel_convert(val, data->acc[2], sensitivity);  in lsm6dsv16x_accel_get_channel()
657 			lsm6dsv16x_accel_convert(val++, data->acc[i], sensitivity);  in lsm6dsv16x_accel_get_channel()
661 		return -ENOTSUP;  in lsm6dsv16x_accel_get_channel()
671 	return lsm6dsv16x_accel_get_channel(chan, val, data, data->acc_gain);  in lsm6dsv16x_accel_channel_get()
694 		lsm6dsv16x_gyro_convert(val, data->gyro[0], sensitivity);  in lsm6dsv16x_gyro_get_channel()
697 		lsm6dsv16x_gyro_convert(val, data->gyro[1], sensitivity);  in lsm6dsv16x_gyro_get_channel()
700 		lsm6dsv16x_gyro_convert(val, data->gyro[2], sensitivity);  in lsm6dsv16x_gyro_get_channel()
704 			lsm6dsv16x_gyro_convert(val++, data->gyro[i], sensitivity);  in lsm6dsv16x_gyro_get_channel()
708 		return -ENOTSUP;  in lsm6dsv16x_gyro_get_channel()
718 	return lsm6dsv16x_gyro_get_channel(chan, val, data, data->gyro_gain);  in lsm6dsv16x_gyro_channel_get()
728 	int64_t temp_sample = data->temp_sample;  in lsm6dsv16x_gyro_channel_get_temp()
731 	val->val1 = (int32_t)(micro_c / 1000000) + 25;  in lsm6dsv16x_gyro_channel_get_temp()
732 	val->val2 = (int32_t)(micro_c % 1000000);  in lsm6dsv16x_gyro_channel_get_temp()
744 	val->val1 = (int32_t)dval / 1000000;  in lsm6dsv16x_magn_convert()
745 	val->val2 = (int32_t)dval % 1000000;  in lsm6dsv16x_magn_convert()
755 	idx = lsm6dsv16x_shub_get_idx(data->dev, SENSOR_CHAN_MAGN_XYZ);  in lsm6dsv16x_magn_get_channel()
758 		return -ENOTSUP;  in lsm6dsv16x_magn_get_channel()
762 	sample[0] = (int16_t)(data->ext_data[idx][0] |  in lsm6dsv16x_magn_get_channel()
763 			     (data->ext_data[idx][1] << 8));  in lsm6dsv16x_magn_get_channel()
764 	sample[1] = (int16_t)(data->ext_data[idx][2] |  in lsm6dsv16x_magn_get_channel()
765 			     (data->ext_data[idx][3] << 8));  in lsm6dsv16x_magn_get_channel()
766 	sample[2] = (int16_t)(data->ext_data[idx][4] |  in lsm6dsv16x_magn_get_channel()
767 			     (data->ext_data[idx][5] << 8));  in lsm6dsv16x_magn_get_channel()
771 		lsm6dsv16x_magn_convert(val, sample[0], data->magn_gain);  in lsm6dsv16x_magn_get_channel()
774 		lsm6dsv16x_magn_convert(val, sample[1], data->magn_gain);  in lsm6dsv16x_magn_get_channel()
777 		lsm6dsv16x_magn_convert(val, sample[2], data->magn_gain);  in lsm6dsv16x_magn_get_channel()
780 		lsm6dsv16x_magn_convert(val, sample[0], data->magn_gain);  in lsm6dsv16x_magn_get_channel()
781 		lsm6dsv16x_magn_convert(val + 1, sample[1], data->magn_gain);  in lsm6dsv16x_magn_get_channel()
782 		lsm6dsv16x_magn_convert(val + 2, sample[2], data->magn_gain);  in lsm6dsv16x_magn_get_channel()
785 		return -ENOTSUP;  in lsm6dsv16x_magn_get_channel()
796 	struct hts221_data *ht = &data->hts221;  in lsm6dsv16x_hum_convert()
799 	idx = lsm6dsv16x_shub_get_idx(data->dev, SENSOR_CHAN_HUMIDITY);  in lsm6dsv16x_hum_convert()
805 	raw_val = (int16_t)(data->ext_data[idx][0] |  in lsm6dsv16x_hum_convert()
806 			   (data->ext_data[idx][1] << 8));  in lsm6dsv16x_hum_convert()
809 	rh = (ht->y1 - ht->y0) * raw_val + ht->x1 * ht->y0 - ht->x0 * ht->y1;  in lsm6dsv16x_hum_convert()
810 	rh /= (ht->x1 - ht->x0);  in lsm6dsv16x_hum_convert()
813 	val->val1 = rh;  in lsm6dsv16x_hum_convert()
814 	val->val2 = rh * 1000000;  in lsm6dsv16x_hum_convert()
823 	idx = lsm6dsv16x_shub_get_idx(data->dev, SENSOR_CHAN_PRESS);  in lsm6dsv16x_press_convert()
829 	raw_val = (int32_t)(data->ext_data[idx][0] |  in lsm6dsv16x_press_convert()
830 			   (data->ext_data[idx][1] << 8) |  in lsm6dsv16x_press_convert()
831 			   (data->ext_data[idx][2] << 16));  in lsm6dsv16x_press_convert()
835 	val->val1 = (raw_val >> 12) / 10;  in lsm6dsv16x_press_convert()
836 	val->val2 = (raw_val >> 12) % 10 * 100000 +  in lsm6dsv16x_press_convert()
846 	idx = lsm6dsv16x_shub_get_idx(data->dev, SENSOR_CHAN_PRESS);  in lsm6dsv16x_temp_convert()
852 	raw_val = (int16_t)(data->ext_data[idx][3] |  in lsm6dsv16x_temp_convert()
853 			   (data->ext_data[idx][4] << 8));  in lsm6dsv16x_temp_convert()
856 	val->val1 = raw_val / 100;  in lsm6dsv16x_temp_convert()
857 	val->val2 = (int32_t)raw_val % 100 * (10000);  in lsm6dsv16x_temp_convert()
865 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_channel_get()
890 		if (!data->shub_inited) {  in lsm6dsv16x_channel_get()
892 			return -ENOTSUP;  in lsm6dsv16x_channel_get()
899 		if (!data->shub_inited) {  in lsm6dsv16x_channel_get()
901 			return -ENOTSUP;  in lsm6dsv16x_channel_get()
908 		if (!data->shub_inited) {  in lsm6dsv16x_channel_get()
910 			return -ENOTSUP;  in lsm6dsv16x_channel_get()
917 		if (!data->shub_inited) {  in lsm6dsv16x_channel_get()
919 			return -ENOTSUP;  in lsm6dsv16x_channel_get()
926 		return -ENOTSUP;  in lsm6dsv16x_channel_get()
948 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_init_chip()
949 	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;  in lsm6dsv16x_init_chip()
950 	struct lsm6dsv16x_data *lsm6dsv16x = dev->data;  in lsm6dsv16x_init_chip()
952 	uint8_t odr, fs;  in lsm6dsv16x_init_chip()  local
955 	if (cfg->i3c.bus != NULL) {  in lsm6dsv16x_init_chip()
960 		lsm6dsv16x->i3c_dev = i3c_device_find(cfg->i3c.bus, &cfg->i3c.dev_id);  in lsm6dsv16x_init_chip()
961 		if (lsm6dsv16x->i3c_dev == NULL) {  in lsm6dsv16x_init_chip()
963 			return -ENODEV;  in lsm6dsv16x_init_chip()
975 		return -EIO;  in lsm6dsv16x_init_chip()
980 		return -EIO;  in lsm6dsv16x_init_chip()
987 		return -EIO;  in lsm6dsv16x_init_chip()
1004 			return -EIO;  in lsm6dsv16x_init_chip()
1011 	fs = cfg->accel_range;  in lsm6dsv16x_init_chip()
1012 	LOG_DBG("accel range is %d", fs);  in lsm6dsv16x_init_chip()
1013 	if (lsm6dsv16x_accel_set_fs_raw(dev, fs) < 0) {  in lsm6dsv16x_init_chip()
1014 		LOG_ERR("failed to set accelerometer range %d", fs);  in lsm6dsv16x_init_chip()
1015 		return -EIO;  in lsm6dsv16x_init_chip()
1017 	lsm6dsv16x->acc_gain = lsm6dsv16x_calc_accel_gain(fs);  in lsm6dsv16x_init_chip()
1019 	odr = cfg->accel_odr;  in lsm6dsv16x_init_chip()
1023 		return -EIO;  in lsm6dsv16x_init_chip()
1026 	fs = cfg->gyro_range;  in lsm6dsv16x_init_chip()
1027 	LOG_DBG("gyro range is %d", fs);  in lsm6dsv16x_init_chip()
1028 	if (lsm6dsv16x_gyro_set_fs_raw(dev, fs) < 0) {  in lsm6dsv16x_init_chip()
1029 		LOG_ERR("failed to set gyroscope range %d", fs);  in lsm6dsv16x_init_chip()
1030 		return -EIO;  in lsm6dsv16x_init_chip()
1032 	lsm6dsv16x->gyro_gain = lsm6dsv16x_calc_gyro_gain(fs);  in lsm6dsv16x_init_chip()
1034 	odr = cfg->gyro_odr;  in lsm6dsv16x_init_chip()
1035 	LOG_DBG("gyro odr is %d", odr);  in lsm6dsv16x_init_chip()
1036 	lsm6dsv16x->gyro_freq = odr;  in lsm6dsv16x_init_chip()
1039 		return -EIO;  in lsm6dsv16x_init_chip()
1050 			.ibi_len = lsm6dsv16x->i3c_dev->data_length.max_ibi,  in lsm6dsv16x_init_chip()
1052 		if (i3c_ccc_do_setmrl(lsm6dsv16x->i3c_dev, &setmrl) < 0) {  in lsm6dsv16x_init_chip()
1054 			return -EIO;  in lsm6dsv16x_init_chip()
1061 		return -EIO;  in lsm6dsv16x_init_chip()
1070 	const struct lsm6dsv16x_config *cfg = dev->config;  in lsm6dsv16x_init()
1072 	struct lsm6dsv16x_data *data = dev->data;  in lsm6dsv16x_init()
1074 	LOG_INF("Initialize device %s", dev->name);  in lsm6dsv16x_init()
1075 	data->dev = dev;  in lsm6dsv16x_init()
1079 		return -EIO;  in lsm6dsv16x_init()
1083 	if (cfg->trig_enabled) {  in lsm6dsv16x_init()
1086 			return -EIO;  in lsm6dsv16x_init()
1092 	data->shub_inited = true;  in lsm6dsv16x_init()
1095 		data->shub_inited = false;  in lsm6dsv16x_init()
1253  * bus-specific macro at preprocessor time.