1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * IIO driver for Bosch BNO055 IMU
4 *
5 * Copyright (C) 2021-2022 Istituto Italiano di Tecnologia
6 * Electronic Design Laboratory
7 * Written by Andrea Merello <andrea.merello@iit.it>
8 *
9 * Portions of this driver are taken from the BNO055 driver patch
10 * from Vlad Dogaru which is Copyright (c) 2016, Intel Corporation.
11 *
12 * This driver is also based on BMI160 driver, which is:
13 * Copyright (c) 2016, Intel Corporation.
14 * Copyright (c) 2019, Martin Kelly.
15 */
16
17 #include <linux/bitfield.h>
18 #include <linux/bitmap.h>
19 #include <linux/clk.h>
20 #include <linux/debugfs.h>
21 #include <linux/device.h>
22 #include <linux/firmware.h>
23 #include <linux/gpio/consumer.h>
24 #include <linux/module.h>
25 #include <linux/mutex.h>
26 #include <linux/regmap.h>
27 #include <linux/util_macros.h>
28
29 #include <linux/iio/buffer.h>
30 #include <linux/iio/iio.h>
31 #include <linux/iio/sysfs.h>
32 #include <linux/iio/trigger_consumer.h>
33 #include <linux/iio/triggered_buffer.h>
34
35 #include "bno055.h"
36
37 #define BNO055_FW_UID_FMT "bno055-caldata-%*phN.dat"
38 #define BNO055_FW_GENERIC_NAME "bno055-caldata.dat"
39
40 /* common registers */
41 #define BNO055_PAGESEL_REG 0x7
42
43 /* page 0 registers */
44 #define BNO055_CHIP_ID_REG 0x0
45 #define BNO055_CHIP_ID_MAGIC 0xA0
46 #define BNO055_SW_REV_LSB_REG 0x4
47 #define BNO055_SW_REV_MSB_REG 0x5
48 #define BNO055_ACC_DATA_X_LSB_REG 0x8
49 #define BNO055_ACC_DATA_Y_LSB_REG 0xA
50 #define BNO055_ACC_DATA_Z_LSB_REG 0xC
51 #define BNO055_MAG_DATA_X_LSB_REG 0xE
52 #define BNO055_MAG_DATA_Y_LSB_REG 0x10
53 #define BNO055_MAG_DATA_Z_LSB_REG 0x12
54 #define BNO055_GYR_DATA_X_LSB_REG 0x14
55 #define BNO055_GYR_DATA_Y_LSB_REG 0x16
56 #define BNO055_GYR_DATA_Z_LSB_REG 0x18
57 #define BNO055_EUL_DATA_X_LSB_REG 0x1A
58 #define BNO055_EUL_DATA_Y_LSB_REG 0x1C
59 #define BNO055_EUL_DATA_Z_LSB_REG 0x1E
60 #define BNO055_QUAT_DATA_W_LSB_REG 0x20
61 #define BNO055_LIA_DATA_X_LSB_REG 0x28
62 #define BNO055_LIA_DATA_Y_LSB_REG 0x2A
63 #define BNO055_LIA_DATA_Z_LSB_REG 0x2C
64 #define BNO055_GRAVITY_DATA_X_LSB_REG 0x2E
65 #define BNO055_GRAVITY_DATA_Y_LSB_REG 0x30
66 #define BNO055_GRAVITY_DATA_Z_LSB_REG 0x32
67 #define BNO055_SCAN_CH_COUNT ((BNO055_GRAVITY_DATA_Z_LSB_REG - BNO055_ACC_DATA_X_LSB_REG) / 2)
68 #define BNO055_TEMP_REG 0x34
69 #define BNO055_CALIB_STAT_REG 0x35
70 #define BNO055_CALIB_STAT_MAGN_SHIFT 0
71 #define BNO055_CALIB_STAT_ACCEL_SHIFT 2
72 #define BNO055_CALIB_STAT_GYRO_SHIFT 4
73 #define BNO055_CALIB_STAT_SYS_SHIFT 6
74 #define BNO055_SYS_ERR_REG 0x3A
75 #define BNO055_POWER_MODE_REG 0x3E
76 #define BNO055_POWER_MODE_NORMAL 0
77 #define BNO055_SYS_TRIGGER_REG 0x3F
78 #define BNO055_SYS_TRIGGER_RST_SYS BIT(5)
79 #define BNO055_SYS_TRIGGER_CLK_SEL BIT(7)
80 #define BNO055_OPR_MODE_REG 0x3D
81 #define BNO055_OPR_MODE_CONFIG 0x0
82 #define BNO055_OPR_MODE_AMG 0x7
83 #define BNO055_OPR_MODE_FUSION_FMC_OFF 0xB
84 #define BNO055_OPR_MODE_FUSION 0xC
85 #define BNO055_UNIT_SEL_REG 0x3B
86 /* Android orientation mode means: pitch value decreases turning clockwise */
87 #define BNO055_UNIT_SEL_ANDROID BIT(7)
88 #define BNO055_UNIT_SEL_GYR_RPS BIT(1)
89 #define BNO055_CALDATA_START 0x55
90 #define BNO055_CALDATA_END 0x6A
91 #define BNO055_CALDATA_LEN 22
92
93 /*
94 * The difference in address between the register that contains the
95 * value and the register that contains the offset. This applies for
96 * accel, gyro and magn channels.
97 */
98 #define BNO055_REG_OFFSET_ADDR 0x4D
99
100 /* page 1 registers */
101 #define BNO055_PG1(x) ((x) | 0x80)
102 #define BNO055_ACC_CONFIG_REG BNO055_PG1(0x8)
103 #define BNO055_ACC_CONFIG_LPF_MASK GENMASK(4, 2)
104 #define BNO055_ACC_CONFIG_RANGE_MASK GENMASK(1, 0)
105 #define BNO055_MAG_CONFIG_REG BNO055_PG1(0x9)
106 #define BNO055_MAG_CONFIG_HIGHACCURACY 0x18
107 #define BNO055_MAG_CONFIG_ODR_MASK GENMASK(2, 0)
108 #define BNO055_GYR_CONFIG_REG BNO055_PG1(0xA)
109 #define BNO055_GYR_CONFIG_RANGE_MASK GENMASK(2, 0)
110 #define BNO055_GYR_CONFIG_LPF_MASK GENMASK(5, 3)
111 #define BNO055_GYR_AM_SET_REG BNO055_PG1(0x1F)
112 #define BNO055_UID_LOWER_REG BNO055_PG1(0x50)
113 #define BNO055_UID_HIGHER_REG BNO055_PG1(0x5F)
114 #define BNO055_UID_LEN 16
115
116 struct bno055_sysfs_attr {
117 const int *vals;
118 int len;
119 const int *fusion_vals;
120 const int *hw_xlate;
121 int hw_xlate_len;
122 int type;
123 };
124
125 static const int bno055_acc_lpf_vals[] = {
126 7, 810000, 15, 630000, 31, 250000, 62, 500000,
127 125, 0, 250, 0, 500, 0, 1000, 0,
128 };
129
130 static const struct bno055_sysfs_attr bno055_acc_lpf = {
131 .vals = bno055_acc_lpf_vals,
132 .len = ARRAY_SIZE(bno055_acc_lpf_vals),
133 .fusion_vals = (const int[]){62, 500000},
134 .type = IIO_VAL_INT_PLUS_MICRO,
135 };
136
137 static const int bno055_acc_range_vals[] = {
138 /* G: 2, 4, 8, 16 */
139 1962, 3924, 7848, 15696
140 };
141
142 static const struct bno055_sysfs_attr bno055_acc_range = {
143 .vals = bno055_acc_range_vals,
144 .len = ARRAY_SIZE(bno055_acc_range_vals),
145 .fusion_vals = (const int[]){3924}, /* 4G */
146 .type = IIO_VAL_INT,
147 };
148
149 /*
150 * Theoretically the IMU should return data in a given (i.e. fixed) unit
151 * regardless of the range setting. This happens for the accelerometer, but not
152 * for the gyroscope; the gyroscope range setting affects the scale.
153 * This is probably due to this[0] bug.
154 * For this reason we map the internal range setting onto the standard IIO scale
155 * attribute for gyro.
156 * Since the bug[0] may be fixed in future, we check for the IMU FW version and
157 * eventually warn the user.
158 * Currently we just don't care about "range" attributes for gyro.
159 *
160 * [0] https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BNO055-Wrong-sensitivity-resolution-in-datasheet/td-p/10266
161 */
162
163 /*
164 * dps = hwval * (dps_range/2^15)
165 * rps = hwval * (rps_range/2^15)
166 * = hwval * (dps_range/(2^15 * k))
167 * where k is rad-to-deg factor
168 */
169 static const int bno055_gyr_scale_vals[] = {
170 125, 1877467, 250, 1877467, 500, 1877467,
171 1000, 1877467, 2000, 1877467,
172 };
173
174 static const int bno055_gyr_scale_hw_xlate[] = {0, 1, 2, 3, 4};
175 static const struct bno055_sysfs_attr bno055_gyr_scale = {
176 .vals = bno055_gyr_scale_vals,
177 .len = ARRAY_SIZE(bno055_gyr_scale_vals),
178 .fusion_vals = (const int[]){1, 900},
179 .hw_xlate = bno055_gyr_scale_hw_xlate,
180 .hw_xlate_len = ARRAY_SIZE(bno055_gyr_scale_hw_xlate),
181 .type = IIO_VAL_FRACTIONAL,
182 };
183
184 static const int bno055_gyr_lpf_vals[] = {12, 23, 32, 47, 64, 116, 230, 523};
185 static const int bno055_gyr_lpf_hw_xlate[] = {5, 4, 7, 3, 6, 2, 1, 0};
186 static const struct bno055_sysfs_attr bno055_gyr_lpf = {
187 .vals = bno055_gyr_lpf_vals,
188 .len = ARRAY_SIZE(bno055_gyr_lpf_vals),
189 .fusion_vals = (const int[]){32},
190 .hw_xlate = bno055_gyr_lpf_hw_xlate,
191 .hw_xlate_len = ARRAY_SIZE(bno055_gyr_lpf_hw_xlate),
192 .type = IIO_VAL_INT,
193 };
194
195 static const int bno055_mag_odr_vals[] = {2, 6, 8, 10, 15, 20, 25, 30};
196 static const struct bno055_sysfs_attr bno055_mag_odr = {
197 .vals = bno055_mag_odr_vals,
198 .len = ARRAY_SIZE(bno055_mag_odr_vals),
199 .fusion_vals = (const int[]){20},
200 .type = IIO_VAL_INT,
201 };
202
203 struct bno055_priv {
204 struct regmap *regmap;
205 struct device *dev;
206 struct clk *clk;
207 int operation_mode;
208 int xfer_burst_break_thr;
209 struct mutex lock;
210 u8 uid[BNO055_UID_LEN];
211 struct gpio_desc *reset_gpio;
212 bool sw_reset;
213 struct {
214 __le16 chans[BNO055_SCAN_CH_COUNT];
215 aligned_s64 timestamp;
216 } buf;
217 struct dentry *debugfs;
218 };
219
bno055_regmap_volatile(struct device * dev,unsigned int reg)220 static bool bno055_regmap_volatile(struct device *dev, unsigned int reg)
221 {
222 /* data and status registers */
223 if (reg >= BNO055_ACC_DATA_X_LSB_REG && reg <= BNO055_SYS_ERR_REG)
224 return true;
225
226 /* when in fusion mode, config is updated by chip */
227 if (reg == BNO055_MAG_CONFIG_REG ||
228 reg == BNO055_ACC_CONFIG_REG ||
229 reg == BNO055_GYR_CONFIG_REG)
230 return true;
231
232 /* calibration data may be updated by the IMU */
233 if (reg >= BNO055_CALDATA_START && reg <= BNO055_CALDATA_END)
234 return true;
235
236 return false;
237 }
238
bno055_regmap_readable(struct device * dev,unsigned int reg)239 static bool bno055_regmap_readable(struct device *dev, unsigned int reg)
240 {
241 /* unnamed PG0 reserved areas */
242 if ((reg < BNO055_PG1(0) && reg > BNO055_CALDATA_END) ||
243 reg == 0x3C)
244 return false;
245
246 /* unnamed PG1 reserved areas */
247 if (reg > BNO055_PG1(BNO055_UID_HIGHER_REG) ||
248 (reg < BNO055_PG1(BNO055_UID_LOWER_REG) && reg > BNO055_PG1(BNO055_GYR_AM_SET_REG)) ||
249 reg == BNO055_PG1(0xE) ||
250 (reg < BNO055_PG1(BNO055_PAGESEL_REG) && reg >= BNO055_PG1(0x0)))
251 return false;
252 return true;
253 }
254
bno055_regmap_writeable(struct device * dev,unsigned int reg)255 static bool bno055_regmap_writeable(struct device *dev, unsigned int reg)
256 {
257 /*
258 * Unreadable registers are indeed reserved; there are no WO regs
259 * (except for a single bit in SYS_TRIGGER register)
260 */
261 if (!bno055_regmap_readable(dev, reg))
262 return false;
263
264 /* data and status registers */
265 if (reg >= BNO055_ACC_DATA_X_LSB_REG && reg <= BNO055_SYS_ERR_REG)
266 return false;
267
268 /* ID areas */
269 if (reg < BNO055_PAGESEL_REG ||
270 (reg <= BNO055_UID_HIGHER_REG && reg >= BNO055_UID_LOWER_REG))
271 return false;
272
273 return true;
274 }
275
276 static const struct regmap_range_cfg bno055_regmap_ranges[] = {
277 {
278 .range_min = 0,
279 .range_max = 0x7f * 2,
280 .selector_reg = BNO055_PAGESEL_REG,
281 .selector_mask = GENMASK(7, 0),
282 .selector_shift = 0,
283 .window_start = 0,
284 .window_len = 0x80,
285 },
286 };
287
288 const struct regmap_config bno055_regmap_config = {
289 .name = "bno055",
290 .reg_bits = 8,
291 .val_bits = 8,
292 .ranges = bno055_regmap_ranges,
293 .num_ranges = 1,
294 .volatile_reg = bno055_regmap_volatile,
295 .max_register = 0x80 * 2,
296 .writeable_reg = bno055_regmap_writeable,
297 .readable_reg = bno055_regmap_readable,
298 .cache_type = REGCACHE_MAPLE,
299 };
300 EXPORT_SYMBOL_NS_GPL(bno055_regmap_config, "IIO_BNO055");
301
302 /* must be called in configuration mode */
bno055_calibration_load(struct bno055_priv * priv,const u8 * data,int len)303 static int bno055_calibration_load(struct bno055_priv *priv, const u8 *data, int len)
304 {
305 if (len != BNO055_CALDATA_LEN) {
306 dev_dbg(priv->dev, "Invalid calibration file size %d (expected %d)",
307 len, BNO055_CALDATA_LEN);
308 return -EINVAL;
309 }
310
311 dev_dbg(priv->dev, "loading cal data: %*ph", BNO055_CALDATA_LEN, data);
312 return regmap_bulk_write(priv->regmap, BNO055_CALDATA_START,
313 data, BNO055_CALDATA_LEN);
314 }
315
bno055_operation_mode_do_set(struct bno055_priv * priv,int operation_mode)316 static int bno055_operation_mode_do_set(struct bno055_priv *priv,
317 int operation_mode)
318 {
319 int ret;
320
321 ret = regmap_write(priv->regmap, BNO055_OPR_MODE_REG,
322 operation_mode);
323 if (ret)
324 return ret;
325
326 /* Following datasheet specifications: sensor takes 7mS up to 19 mS to switch mode */
327 msleep(20);
328
329 return 0;
330 }
331
bno055_system_reset(struct bno055_priv * priv)332 static int bno055_system_reset(struct bno055_priv *priv)
333 {
334 int ret;
335
336 if (priv->reset_gpio) {
337 gpiod_set_value_cansleep(priv->reset_gpio, 0);
338 usleep_range(5000, 10000);
339 gpiod_set_value_cansleep(priv->reset_gpio, 1);
340 } else if (priv->sw_reset) {
341 ret = regmap_write(priv->regmap, BNO055_SYS_TRIGGER_REG,
342 BNO055_SYS_TRIGGER_RST_SYS);
343 if (ret)
344 return ret;
345 } else {
346 return 0;
347 }
348
349 regcache_drop_region(priv->regmap, 0x0, 0xff);
350 usleep_range(650000, 700000);
351
352 return 0;
353 }
354
bno055_init(struct bno055_priv * priv,const u8 * caldata,int len)355 static int bno055_init(struct bno055_priv *priv, const u8 *caldata, int len)
356 {
357 int ret;
358
359 ret = bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG);
360 if (ret)
361 return ret;
362
363 ret = regmap_write(priv->regmap, BNO055_POWER_MODE_REG,
364 BNO055_POWER_MODE_NORMAL);
365 if (ret)
366 return ret;
367
368 ret = regmap_write(priv->regmap, BNO055_SYS_TRIGGER_REG,
369 priv->clk ? BNO055_SYS_TRIGGER_CLK_SEL : 0);
370 if (ret)
371 return ret;
372
373 /* use standard SI units */
374 ret = regmap_write(priv->regmap, BNO055_UNIT_SEL_REG,
375 BNO055_UNIT_SEL_ANDROID | BNO055_UNIT_SEL_GYR_RPS);
376 if (ret)
377 return ret;
378
379 if (caldata) {
380 ret = bno055_calibration_load(priv, caldata, len);
381 if (ret)
382 dev_warn(priv->dev, "failed to load calibration data with error %d\n",
383 ret);
384 }
385
386 return 0;
387 }
388
bno055_operation_mode_set(struct bno055_priv * priv,int operation_mode)389 static ssize_t bno055_operation_mode_set(struct bno055_priv *priv,
390 int operation_mode)
391 {
392 u8 caldata[BNO055_CALDATA_LEN];
393 int ret;
394
395 mutex_lock(&priv->lock);
396
397 ret = bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG);
398 if (ret)
399 goto exit_unlock;
400
401 if (operation_mode == BNO055_OPR_MODE_FUSION ||
402 operation_mode == BNO055_OPR_MODE_FUSION_FMC_OFF) {
403 /* for entering fusion mode, reset the chip to clear the algo state */
404 ret = regmap_bulk_read(priv->regmap, BNO055_CALDATA_START, caldata,
405 BNO055_CALDATA_LEN);
406 if (ret)
407 goto exit_unlock;
408
409 ret = bno055_system_reset(priv);
410 if (ret)
411 goto exit_unlock;
412
413 ret = bno055_init(priv, caldata, BNO055_CALDATA_LEN);
414 if (ret)
415 goto exit_unlock;
416 }
417
418 ret = bno055_operation_mode_do_set(priv, operation_mode);
419 if (ret)
420 goto exit_unlock;
421
422 priv->operation_mode = operation_mode;
423
424 exit_unlock:
425 mutex_unlock(&priv->lock);
426 return ret;
427 }
428
bno055_uninit(void * arg)429 static void bno055_uninit(void *arg)
430 {
431 struct bno055_priv *priv = arg;
432
433 /* stop the IMU */
434 bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG);
435 }
436
437 #define BNO055_CHANNEL(_type, _axis, _index, _address, _sep, _sh, _avail) { \
438 .address = _address, \
439 .type = _type, \
440 .modified = 1, \
441 .channel2 = IIO_MOD_##_axis, \
442 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | (_sep), \
443 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | (_sh), \
444 .info_mask_shared_by_type_available = _avail, \
445 .scan_index = _index, \
446 .scan_type = { \
447 .sign = 's', \
448 .realbits = 16, \
449 .storagebits = 16, \
450 .endianness = IIO_LE, \
451 .repeat = IIO_MOD_##_axis == IIO_MOD_QUATERNION ? 4 : 0, \
452 }, \
453 }
454
455 /* scan indexes follow DATA register order */
456 enum bno055_scan_axis {
457 BNO055_SCAN_ACCEL_X,
458 BNO055_SCAN_ACCEL_Y,
459 BNO055_SCAN_ACCEL_Z,
460 BNO055_SCAN_MAGN_X,
461 BNO055_SCAN_MAGN_Y,
462 BNO055_SCAN_MAGN_Z,
463 BNO055_SCAN_GYRO_X,
464 BNO055_SCAN_GYRO_Y,
465 BNO055_SCAN_GYRO_Z,
466 BNO055_SCAN_YAW,
467 BNO055_SCAN_ROLL,
468 BNO055_SCAN_PITCH,
469 BNO055_SCAN_QUATERNION,
470 BNO055_SCAN_LIA_X,
471 BNO055_SCAN_LIA_Y,
472 BNO055_SCAN_LIA_Z,
473 BNO055_SCAN_GRAVITY_X,
474 BNO055_SCAN_GRAVITY_Y,
475 BNO055_SCAN_GRAVITY_Z,
476 BNO055_SCAN_TIMESTAMP,
477 _BNO055_SCAN_MAX
478 };
479
480 static const struct iio_chan_spec bno055_channels[] = {
481 /* accelerometer */
482 BNO055_CHANNEL(IIO_ACCEL, X, BNO055_SCAN_ACCEL_X,
483 BNO055_ACC_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
484 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY),
485 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)),
486 BNO055_CHANNEL(IIO_ACCEL, Y, BNO055_SCAN_ACCEL_Y,
487 BNO055_ACC_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
488 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY),
489 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)),
490 BNO055_CHANNEL(IIO_ACCEL, Z, BNO055_SCAN_ACCEL_Z,
491 BNO055_ACC_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
492 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY),
493 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)),
494 /* gyroscope */
495 BNO055_CHANNEL(IIO_ANGL_VEL, X, BNO055_SCAN_GYRO_X,
496 BNO055_GYR_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
497 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY),
498 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) |
499 BIT(IIO_CHAN_INFO_SCALE)),
500 BNO055_CHANNEL(IIO_ANGL_VEL, Y, BNO055_SCAN_GYRO_Y,
501 BNO055_GYR_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
502 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY),
503 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) |
504 BIT(IIO_CHAN_INFO_SCALE)),
505 BNO055_CHANNEL(IIO_ANGL_VEL, Z, BNO055_SCAN_GYRO_Z,
506 BNO055_GYR_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
507 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY),
508 BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) |
509 BIT(IIO_CHAN_INFO_SCALE)),
510 /* magnetometer */
511 BNO055_CHANNEL(IIO_MAGN, X, BNO055_SCAN_MAGN_X,
512 BNO055_MAG_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
513 BIT(IIO_CHAN_INFO_SAMP_FREQ), BIT(IIO_CHAN_INFO_SAMP_FREQ)),
514 BNO055_CHANNEL(IIO_MAGN, Y, BNO055_SCAN_MAGN_Y,
515 BNO055_MAG_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
516 BIT(IIO_CHAN_INFO_SAMP_FREQ), BIT(IIO_CHAN_INFO_SAMP_FREQ)),
517 BNO055_CHANNEL(IIO_MAGN, Z, BNO055_SCAN_MAGN_Z,
518 BNO055_MAG_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET),
519 BIT(IIO_CHAN_INFO_SAMP_FREQ), BIT(IIO_CHAN_INFO_SAMP_FREQ)),
520 /* euler angle */
521 BNO055_CHANNEL(IIO_ROT, YAW, BNO055_SCAN_YAW,
522 BNO055_EUL_DATA_X_LSB_REG, 0, 0, 0),
523 BNO055_CHANNEL(IIO_ROT, ROLL, BNO055_SCAN_ROLL,
524 BNO055_EUL_DATA_Y_LSB_REG, 0, 0, 0),
525 BNO055_CHANNEL(IIO_ROT, PITCH, BNO055_SCAN_PITCH,
526 BNO055_EUL_DATA_Z_LSB_REG, 0, 0, 0),
527 /* quaternion */
528 BNO055_CHANNEL(IIO_ROT, QUATERNION, BNO055_SCAN_QUATERNION,
529 BNO055_QUAT_DATA_W_LSB_REG, 0, 0, 0),
530
531 /* linear acceleration */
532 BNO055_CHANNEL(IIO_ACCEL, LINEAR_X, BNO055_SCAN_LIA_X,
533 BNO055_LIA_DATA_X_LSB_REG, 0, 0, 0),
534 BNO055_CHANNEL(IIO_ACCEL, LINEAR_Y, BNO055_SCAN_LIA_Y,
535 BNO055_LIA_DATA_Y_LSB_REG, 0, 0, 0),
536 BNO055_CHANNEL(IIO_ACCEL, LINEAR_Z, BNO055_SCAN_LIA_Z,
537 BNO055_LIA_DATA_Z_LSB_REG, 0, 0, 0),
538
539 /* gravity vector */
540 BNO055_CHANNEL(IIO_GRAVITY, X, BNO055_SCAN_GRAVITY_X,
541 BNO055_GRAVITY_DATA_X_LSB_REG, 0, 0, 0),
542 BNO055_CHANNEL(IIO_GRAVITY, Y, BNO055_SCAN_GRAVITY_Y,
543 BNO055_GRAVITY_DATA_Y_LSB_REG, 0, 0, 0),
544 BNO055_CHANNEL(IIO_GRAVITY, Z, BNO055_SCAN_GRAVITY_Z,
545 BNO055_GRAVITY_DATA_Z_LSB_REG, 0, 0, 0),
546
547 {
548 .type = IIO_TEMP,
549 .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
550 .scan_index = -1,
551 },
552 IIO_CHAN_SOFT_TIMESTAMP(BNO055_SCAN_TIMESTAMP),
553 };
554
bno055_get_regmask(struct bno055_priv * priv,int * val,int * val2,int reg,int mask,const struct bno055_sysfs_attr * attr)555 static int bno055_get_regmask(struct bno055_priv *priv, int *val, int *val2,
556 int reg, int mask,
557 const struct bno055_sysfs_attr *attr)
558 {
559 const int shift = __ffs(mask);
560 int hwval, idx;
561 int ret;
562 int i;
563
564 ret = regmap_read(priv->regmap, reg, &hwval);
565 if (ret)
566 return ret;
567
568 idx = (hwval & mask) >> shift;
569 if (attr->hw_xlate)
570 for (i = 0; i < attr->hw_xlate_len; i++)
571 if (attr->hw_xlate[i] == idx) {
572 idx = i;
573 break;
574 }
575 if (attr->type == IIO_VAL_INT) {
576 *val = attr->vals[idx];
577 } else { /* IIO_VAL_INT_PLUS_MICRO or IIO_VAL_FRACTIONAL */
578 *val = attr->vals[idx * 2];
579 *val2 = attr->vals[idx * 2 + 1];
580 }
581
582 return attr->type;
583 }
584
bno055_set_regmask(struct bno055_priv * priv,int val,int val2,int reg,int mask,const struct bno055_sysfs_attr * attr)585 static int bno055_set_regmask(struct bno055_priv *priv, int val, int val2,
586 int reg, int mask,
587 const struct bno055_sysfs_attr *attr)
588 {
589 const int shift = __ffs(mask);
590 int best_delta;
591 int req_val;
592 int tbl_val;
593 bool first;
594 int delta;
595 int hwval;
596 int ret;
597 int len;
598 int i;
599
600 /*
601 * The closest value the HW supports is only one in fusion mode,
602 * and it is autoselected, so don't do anything, just return OK,
603 * as the closest possible value has been (virtually) selected
604 */
605 if (priv->operation_mode != BNO055_OPR_MODE_AMG)
606 return 0;
607
608 len = attr->len;
609
610 /*
611 * We always get a request in INT_PLUS_MICRO, but we
612 * take care of the micro part only when we really have
613 * non-integer tables. This prevents 32-bit overflow with
614 * larger integers contained in integer tables.
615 */
616 req_val = val;
617 if (attr->type != IIO_VAL_INT) {
618 len /= 2;
619 req_val = min(val, 2147) * 1000000 + val2;
620 }
621
622 first = true;
623 for (i = 0; i < len; i++) {
624 switch (attr->type) {
625 case IIO_VAL_INT:
626 tbl_val = attr->vals[i];
627 break;
628 case IIO_VAL_INT_PLUS_MICRO:
629 WARN_ON(attr->vals[i * 2] > 2147);
630 tbl_val = attr->vals[i * 2] * 1000000 +
631 attr->vals[i * 2 + 1];
632 break;
633 case IIO_VAL_FRACTIONAL:
634 WARN_ON(attr->vals[i * 2] > 4294);
635 tbl_val = attr->vals[i * 2] * 1000000 /
636 attr->vals[i * 2 + 1];
637 break;
638 default:
639 return -EINVAL;
640 }
641 delta = abs(tbl_val - req_val);
642 if (first || delta < best_delta) {
643 best_delta = delta;
644 hwval = i;
645 first = false;
646 }
647 }
648
649 if (attr->hw_xlate)
650 hwval = attr->hw_xlate[hwval];
651
652 ret = bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG);
653 if (ret)
654 return ret;
655
656 ret = regmap_update_bits(priv->regmap, reg, mask, hwval << shift);
657 if (ret)
658 return ret;
659
660 return bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_AMG);
661 }
662
bno055_read_simple_chan(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int * val,int * val2,long mask)663 static int bno055_read_simple_chan(struct iio_dev *indio_dev,
664 struct iio_chan_spec const *chan,
665 int *val, int *val2, long mask)
666 {
667 struct bno055_priv *priv = iio_priv(indio_dev);
668 __le16 raw_val;
669 int ret;
670
671 switch (mask) {
672 case IIO_CHAN_INFO_RAW:
673 ret = regmap_bulk_read(priv->regmap, chan->address,
674 &raw_val, sizeof(raw_val));
675 if (ret < 0)
676 return ret;
677 *val = sign_extend32(le16_to_cpu(raw_val), 15);
678 return IIO_VAL_INT;
679 case IIO_CHAN_INFO_OFFSET:
680 if (priv->operation_mode != BNO055_OPR_MODE_AMG) {
681 *val = 0;
682 } else {
683 ret = regmap_bulk_read(priv->regmap,
684 chan->address +
685 BNO055_REG_OFFSET_ADDR,
686 &raw_val, sizeof(raw_val));
687 if (ret < 0)
688 return ret;
689 /*
690 * IMU reports sensor offsets; IIO wants correction
691 * offsets, thus we need the 'minus' here.
692 */
693 *val = -sign_extend32(le16_to_cpu(raw_val), 15);
694 }
695 return IIO_VAL_INT;
696 case IIO_CHAN_INFO_SCALE:
697 *val = 1;
698 switch (chan->type) {
699 case IIO_GRAVITY:
700 /* Table 3-35: 1 m/s^2 = 100 LSB */
701 case IIO_ACCEL:
702 /* Table 3-17: 1 m/s^2 = 100 LSB */
703 *val2 = 100;
704 break;
705 case IIO_MAGN:
706 /*
707 * Table 3-19: 1 uT = 16 LSB. But we need
708 * Gauss: 1G = 0.1 uT.
709 */
710 *val2 = 160;
711 break;
712 case IIO_ANGL_VEL:
713 /*
714 * Table 3-22: 1 Rps = 900 LSB
715 * .. but this is not exactly true. See comment at the
716 * beginning of this file.
717 */
718 if (priv->operation_mode != BNO055_OPR_MODE_AMG) {
719 *val = bno055_gyr_scale.fusion_vals[0];
720 *val2 = bno055_gyr_scale.fusion_vals[1];
721 return IIO_VAL_FRACTIONAL;
722 }
723
724 return bno055_get_regmask(priv, val, val2,
725 BNO055_GYR_CONFIG_REG,
726 BNO055_GYR_CONFIG_RANGE_MASK,
727 &bno055_gyr_scale);
728 break;
729 case IIO_ROT:
730 /* Table 3-28: 1 degree = 16 LSB */
731 *val2 = 16;
732 break;
733 default:
734 return -EINVAL;
735 }
736 return IIO_VAL_FRACTIONAL;
737
738 case IIO_CHAN_INFO_SAMP_FREQ:
739 if (chan->type != IIO_MAGN)
740 return -EINVAL;
741
742 return bno055_get_regmask(priv, val, val2,
743 BNO055_MAG_CONFIG_REG,
744 BNO055_MAG_CONFIG_ODR_MASK,
745 &bno055_mag_odr);
746
747 case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
748 switch (chan->type) {
749 case IIO_ANGL_VEL:
750 return bno055_get_regmask(priv, val, val2,
751 BNO055_GYR_CONFIG_REG,
752 BNO055_GYR_CONFIG_LPF_MASK,
753 &bno055_gyr_lpf);
754 case IIO_ACCEL:
755 return bno055_get_regmask(priv, val, val2,
756 BNO055_ACC_CONFIG_REG,
757 BNO055_ACC_CONFIG_LPF_MASK,
758 &bno055_acc_lpf);
759 default:
760 return -EINVAL;
761 }
762
763 default:
764 return -EINVAL;
765 }
766 }
767
bno055_sysfs_attr_avail(struct bno055_priv * priv,const struct bno055_sysfs_attr * attr,const int ** vals,int * length)768 static int bno055_sysfs_attr_avail(struct bno055_priv *priv,
769 const struct bno055_sysfs_attr *attr,
770 const int **vals, int *length)
771 {
772 if (priv->operation_mode != BNO055_OPR_MODE_AMG) {
773 /* locked when fusion enabled */
774 *vals = attr->fusion_vals;
775 if (attr->type == IIO_VAL_INT)
776 *length = 1;
777 else
778 *length = 2; /* IIO_VAL_INT_PLUS_MICRO or IIO_VAL_FRACTIONAL*/
779 } else {
780 *vals = attr->vals;
781 *length = attr->len;
782 }
783
784 return attr->type;
785 }
786
bno055_read_avail(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,const int ** vals,int * type,int * length,long mask)787 static int bno055_read_avail(struct iio_dev *indio_dev,
788 struct iio_chan_spec const *chan,
789 const int **vals, int *type, int *length,
790 long mask)
791 {
792 struct bno055_priv *priv = iio_priv(indio_dev);
793
794 switch (mask) {
795 case IIO_CHAN_INFO_SCALE:
796 switch (chan->type) {
797 case IIO_ANGL_VEL:
798 *type = bno055_sysfs_attr_avail(priv, &bno055_gyr_scale,
799 vals, length);
800 return IIO_AVAIL_LIST;
801 default:
802 return -EINVAL;
803 }
804 case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
805 switch (chan->type) {
806 case IIO_ANGL_VEL:
807 *type = bno055_sysfs_attr_avail(priv, &bno055_gyr_lpf,
808 vals, length);
809 return IIO_AVAIL_LIST;
810 case IIO_ACCEL:
811 *type = bno055_sysfs_attr_avail(priv, &bno055_acc_lpf,
812 vals, length);
813 return IIO_AVAIL_LIST;
814 default:
815 return -EINVAL;
816 }
817
818 break;
819 case IIO_CHAN_INFO_SAMP_FREQ:
820 switch (chan->type) {
821 case IIO_MAGN:
822 *type = bno055_sysfs_attr_avail(priv, &bno055_mag_odr,
823 vals, length);
824 return IIO_AVAIL_LIST;
825 default:
826 return -EINVAL;
827 }
828 default:
829 return -EINVAL;
830 }
831 }
832
bno055_read_temp_chan(struct iio_dev * indio_dev,int * val)833 static int bno055_read_temp_chan(struct iio_dev *indio_dev, int *val)
834 {
835 struct bno055_priv *priv = iio_priv(indio_dev);
836 unsigned int raw_val;
837 int ret;
838
839 ret = regmap_read(priv->regmap, BNO055_TEMP_REG, &raw_val);
840 if (ret < 0)
841 return ret;
842
843 /*
844 * Tables 3-36 and 3-37: one byte of priv, signed, 1 LSB = 1C.
845 * ABI wants milliC.
846 */
847 *val = raw_val * 1000;
848
849 return IIO_VAL_INT;
850 }
851
bno055_read_quaternion(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int size,int * vals,int * val_len,long mask)852 static int bno055_read_quaternion(struct iio_dev *indio_dev,
853 struct iio_chan_spec const *chan,
854 int size, int *vals, int *val_len,
855 long mask)
856 {
857 struct bno055_priv *priv = iio_priv(indio_dev);
858 __le16 raw_vals[4];
859 int i, ret;
860
861 switch (mask) {
862 case IIO_CHAN_INFO_RAW:
863 if (size < 4)
864 return -EINVAL;
865 ret = regmap_bulk_read(priv->regmap,
866 BNO055_QUAT_DATA_W_LSB_REG,
867 raw_vals, sizeof(raw_vals));
868 if (ret < 0)
869 return ret;
870 for (i = 0; i < 4; i++)
871 vals[i] = sign_extend32(le16_to_cpu(raw_vals[i]), 15);
872 *val_len = 4;
873 return IIO_VAL_INT_MULTIPLE;
874 case IIO_CHAN_INFO_SCALE:
875 /* Table 3-31: 1 quaternion = 2^14 LSB */
876 if (size < 2)
877 return -EINVAL;
878 vals[0] = 1;
879 vals[1] = 14;
880 return IIO_VAL_FRACTIONAL_LOG2;
881 default:
882 return -EINVAL;
883 }
884 }
885
bno055_is_chan_readable(struct iio_dev * indio_dev,struct iio_chan_spec const * chan)886 static bool bno055_is_chan_readable(struct iio_dev *indio_dev,
887 struct iio_chan_spec const *chan)
888 {
889 struct bno055_priv *priv = iio_priv(indio_dev);
890
891 if (priv->operation_mode != BNO055_OPR_MODE_AMG)
892 return true;
893
894 switch (chan->type) {
895 case IIO_GRAVITY:
896 case IIO_ROT:
897 return false;
898 case IIO_ACCEL:
899 if (chan->channel2 == IIO_MOD_LINEAR_X ||
900 chan->channel2 == IIO_MOD_LINEAR_Y ||
901 chan->channel2 == IIO_MOD_LINEAR_Z)
902 return false;
903 return true;
904 default:
905 return true;
906 }
907 }
908
_bno055_read_raw_multi(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int size,int * vals,int * val_len,long mask)909 static int _bno055_read_raw_multi(struct iio_dev *indio_dev,
910 struct iio_chan_spec const *chan,
911 int size, int *vals, int *val_len,
912 long mask)
913 {
914 if (!bno055_is_chan_readable(indio_dev, chan))
915 return -EBUSY;
916
917 switch (chan->type) {
918 case IIO_MAGN:
919 case IIO_ACCEL:
920 case IIO_ANGL_VEL:
921 case IIO_GRAVITY:
922 if (size < 2)
923 return -EINVAL;
924 *val_len = 2;
925 return bno055_read_simple_chan(indio_dev, chan,
926 &vals[0], &vals[1],
927 mask);
928 case IIO_TEMP:
929 *val_len = 1;
930 return bno055_read_temp_chan(indio_dev, &vals[0]);
931 case IIO_ROT:
932 /*
933 * Rotation is exposed as either a quaternion or three
934 * Euler angles.
935 */
936 if (chan->channel2 == IIO_MOD_QUATERNION)
937 return bno055_read_quaternion(indio_dev, chan,
938 size, vals,
939 val_len, mask);
940 if (size < 2)
941 return -EINVAL;
942 *val_len = 2;
943 return bno055_read_simple_chan(indio_dev, chan,
944 &vals[0], &vals[1],
945 mask);
946 default:
947 return -EINVAL;
948 }
949 }
950
bno055_read_raw_multi(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,int size,int * vals,int * val_len,long mask)951 static int bno055_read_raw_multi(struct iio_dev *indio_dev,
952 struct iio_chan_spec const *chan,
953 int size, int *vals, int *val_len,
954 long mask)
955 {
956 struct bno055_priv *priv = iio_priv(indio_dev);
957 int ret;
958
959 mutex_lock(&priv->lock);
960 ret = _bno055_read_raw_multi(indio_dev, chan, size,
961 vals, val_len, mask);
962 mutex_unlock(&priv->lock);
963 return ret;
964 }
965
_bno055_write_raw(struct iio_dev * iio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)966 static int _bno055_write_raw(struct iio_dev *iio_dev,
967 struct iio_chan_spec const *chan,
968 int val, int val2, long mask)
969 {
970 struct bno055_priv *priv = iio_priv(iio_dev);
971
972 switch (chan->type) {
973 case IIO_MAGN:
974 switch (mask) {
975 case IIO_CHAN_INFO_SAMP_FREQ:
976 return bno055_set_regmask(priv, val, val2,
977 BNO055_MAG_CONFIG_REG,
978 BNO055_MAG_CONFIG_ODR_MASK,
979 &bno055_mag_odr);
980 default:
981 return -EINVAL;
982 }
983 case IIO_ACCEL:
984 switch (mask) {
985 case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
986 return bno055_set_regmask(priv, val, val2,
987 BNO055_ACC_CONFIG_REG,
988 BNO055_ACC_CONFIG_LPF_MASK,
989 &bno055_acc_lpf);
990
991 default:
992 return -EINVAL;
993 }
994 case IIO_ANGL_VEL:
995 switch (mask) {
996 case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
997 return bno055_set_regmask(priv, val, val2,
998 BNO055_GYR_CONFIG_REG,
999 BNO055_GYR_CONFIG_LPF_MASK,
1000 &bno055_gyr_lpf);
1001 case IIO_CHAN_INFO_SCALE:
1002 return bno055_set_regmask(priv, val, val2,
1003 BNO055_GYR_CONFIG_REG,
1004 BNO055_GYR_CONFIG_RANGE_MASK,
1005 &bno055_gyr_scale);
1006 default:
1007 return -EINVAL;
1008 }
1009 default:
1010 return -EINVAL;
1011 }
1012 }
1013
bno055_write_raw(struct iio_dev * iio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)1014 static int bno055_write_raw(struct iio_dev *iio_dev,
1015 struct iio_chan_spec const *chan,
1016 int val, int val2, long mask)
1017 {
1018 struct bno055_priv *priv = iio_priv(iio_dev);
1019 int ret;
1020
1021 mutex_lock(&priv->lock);
1022 ret = _bno055_write_raw(iio_dev, chan, val, val2, mask);
1023 mutex_unlock(&priv->lock);
1024
1025 return ret;
1026 }
1027
in_accel_range_raw_available_show(struct device * dev,struct device_attribute * attr,char * buf)1028 static ssize_t in_accel_range_raw_available_show(struct device *dev,
1029 struct device_attribute *attr,
1030 char *buf)
1031 {
1032 struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev));
1033 int len = 0;
1034 int i;
1035
1036 if (priv->operation_mode != BNO055_OPR_MODE_AMG)
1037 return sysfs_emit(buf, "%d\n", bno055_acc_range.fusion_vals[0]);
1038
1039 for (i = 0; i < bno055_acc_range.len; i++)
1040 len += sysfs_emit_at(buf, len, "%d ", bno055_acc_range.vals[i]);
1041 buf[len - 1] = '\n';
1042
1043 return len;
1044 }
1045
fusion_enable_show(struct device * dev,struct device_attribute * attr,char * buf)1046 static ssize_t fusion_enable_show(struct device *dev,
1047 struct device_attribute *attr,
1048 char *buf)
1049 {
1050 struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev));
1051
1052 return sysfs_emit(buf, "%d\n",
1053 priv->operation_mode != BNO055_OPR_MODE_AMG);
1054 }
1055
fusion_enable_store(struct device * dev,struct device_attribute * attr,const char * buf,size_t len)1056 static ssize_t fusion_enable_store(struct device *dev,
1057 struct device_attribute *attr,
1058 const char *buf, size_t len)
1059 {
1060 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
1061 struct bno055_priv *priv = iio_priv(indio_dev);
1062 bool en;
1063 int ret;
1064
1065 if (indio_dev->active_scan_mask &&
1066 !bitmap_empty(indio_dev->active_scan_mask, _BNO055_SCAN_MAX))
1067 return -EBUSY;
1068
1069 ret = kstrtobool(buf, &en);
1070 if (ret)
1071 return -EINVAL;
1072
1073 if (!en)
1074 return bno055_operation_mode_set(priv, BNO055_OPR_MODE_AMG) ?: len;
1075
1076 /*
1077 * Coming from AMG means the FMC was off, just switch to fusion but
1078 * don't change anything that doesn't belong to us (i.e let FMC stay off).
1079 * Coming from any other fusion mode means we don't need to do anything.
1080 */
1081 if (priv->operation_mode == BNO055_OPR_MODE_AMG)
1082 return bno055_operation_mode_set(priv, BNO055_OPR_MODE_FUSION_FMC_OFF) ?: len;
1083
1084 return len;
1085 }
1086
in_magn_calibration_fast_enable_show(struct device * dev,struct device_attribute * attr,char * buf)1087 static ssize_t in_magn_calibration_fast_enable_show(struct device *dev,
1088 struct device_attribute *attr,
1089 char *buf)
1090 {
1091 struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev));
1092
1093 return sysfs_emit(buf, "%d\n",
1094 priv->operation_mode == BNO055_OPR_MODE_FUSION);
1095 }
1096
in_magn_calibration_fast_enable_store(struct device * dev,struct device_attribute * attr,const char * buf,size_t len)1097 static ssize_t in_magn_calibration_fast_enable_store(struct device *dev,
1098 struct device_attribute *attr,
1099 const char *buf, size_t len)
1100 {
1101 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
1102 struct bno055_priv *priv = iio_priv(indio_dev);
1103 int ret;
1104
1105 if (indio_dev->active_scan_mask &&
1106 !bitmap_empty(indio_dev->active_scan_mask, _BNO055_SCAN_MAX))
1107 return -EBUSY;
1108
1109 if (sysfs_streq(buf, "0")) {
1110 if (priv->operation_mode == BNO055_OPR_MODE_FUSION) {
1111 ret = bno055_operation_mode_set(priv, BNO055_OPR_MODE_FUSION_FMC_OFF);
1112 if (ret)
1113 return ret;
1114 }
1115 } else {
1116 if (priv->operation_mode == BNO055_OPR_MODE_AMG)
1117 return -EINVAL;
1118
1119 if (priv->operation_mode != BNO055_OPR_MODE_FUSION) {
1120 ret = bno055_operation_mode_set(priv, BNO055_OPR_MODE_FUSION);
1121 if (ret)
1122 return ret;
1123 }
1124 }
1125
1126 return len;
1127 }
1128
in_accel_range_raw_show(struct device * dev,struct device_attribute * attr,char * buf)1129 static ssize_t in_accel_range_raw_show(struct device *dev,
1130 struct device_attribute *attr,
1131 char *buf)
1132 {
1133 struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev));
1134 int val;
1135 int ret;
1136
1137 ret = bno055_get_regmask(priv, &val, NULL,
1138 BNO055_ACC_CONFIG_REG,
1139 BNO055_ACC_CONFIG_RANGE_MASK,
1140 &bno055_acc_range);
1141 if (ret < 0)
1142 return ret;
1143
1144 return sysfs_emit(buf, "%d\n", val);
1145 }
1146
in_accel_range_raw_store(struct device * dev,struct device_attribute * attr,const char * buf,size_t len)1147 static ssize_t in_accel_range_raw_store(struct device *dev,
1148 struct device_attribute *attr,
1149 const char *buf, size_t len)
1150 {
1151 struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev));
1152 unsigned long val;
1153 int ret;
1154
1155 ret = kstrtoul(buf, 10, &val);
1156 if (ret)
1157 return ret;
1158
1159 mutex_lock(&priv->lock);
1160 ret = bno055_set_regmask(priv, val, 0,
1161 BNO055_ACC_CONFIG_REG,
1162 BNO055_ACC_CONFIG_RANGE_MASK,
1163 &bno055_acc_range);
1164 mutex_unlock(&priv->lock);
1165
1166 return ret ?: len;
1167 }
1168
bno055_get_calib_status(struct device * dev,char * buf,int which)1169 static ssize_t bno055_get_calib_status(struct device *dev, char *buf, int which)
1170 {
1171 struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev));
1172 int calib;
1173 int ret;
1174 int val;
1175
1176 if (priv->operation_mode == BNO055_OPR_MODE_AMG ||
1177 (priv->operation_mode == BNO055_OPR_MODE_FUSION_FMC_OFF &&
1178 which == BNO055_CALIB_STAT_MAGN_SHIFT)) {
1179 calib = 0;
1180 } else {
1181 mutex_lock(&priv->lock);
1182 ret = regmap_read(priv->regmap, BNO055_CALIB_STAT_REG, &val);
1183 mutex_unlock(&priv->lock);
1184
1185 if (ret)
1186 return -EIO;
1187
1188 calib = ((val >> which) & GENMASK(1, 0)) + 1;
1189 }
1190
1191 return sysfs_emit(buf, "%d\n", calib);
1192 }
1193
serialnumber_show(struct device * dev,struct device_attribute * attr,char * buf)1194 static ssize_t serialnumber_show(struct device *dev,
1195 struct device_attribute *attr,
1196 char *buf)
1197 {
1198 struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev));
1199
1200 return sysfs_emit(buf, "%*ph\n", BNO055_UID_LEN, priv->uid);
1201 }
1202
calibration_data_read(struct file * filp,struct kobject * kobj,const struct bin_attribute * bin_attr,char * buf,loff_t pos,size_t count)1203 static ssize_t calibration_data_read(struct file *filp, struct kobject *kobj,
1204 const struct bin_attribute *bin_attr, char *buf,
1205 loff_t pos, size_t count)
1206 {
1207 struct bno055_priv *priv = iio_priv(dev_to_iio_dev(kobj_to_dev(kobj)));
1208 u8 data[BNO055_CALDATA_LEN];
1209 int ret;
1210
1211 /*
1212 * Calibration data is volatile; reading it in chunks will possibly
1213 * results in inconsistent data. We require the user to read the whole
1214 * blob in a single chunk
1215 */
1216 if (count < BNO055_CALDATA_LEN || pos)
1217 return -EINVAL;
1218
1219 mutex_lock(&priv->lock);
1220 ret = bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG);
1221 if (ret)
1222 goto exit_unlock;
1223
1224 ret = regmap_bulk_read(priv->regmap, BNO055_CALDATA_START, data,
1225 BNO055_CALDATA_LEN);
1226 if (ret)
1227 goto exit_unlock;
1228
1229 ret = bno055_operation_mode_do_set(priv, priv->operation_mode);
1230 if (ret)
1231 goto exit_unlock;
1232
1233 memcpy(buf, data, BNO055_CALDATA_LEN);
1234
1235 ret = BNO055_CALDATA_LEN;
1236 exit_unlock:
1237 mutex_unlock(&priv->lock);
1238 return ret;
1239 }
1240
sys_calibration_auto_status_show(struct device * dev,struct device_attribute * a,char * buf)1241 static ssize_t sys_calibration_auto_status_show(struct device *dev,
1242 struct device_attribute *a,
1243 char *buf)
1244 {
1245 return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_SYS_SHIFT);
1246 }
1247
in_accel_calibration_auto_status_show(struct device * dev,struct device_attribute * a,char * buf)1248 static ssize_t in_accel_calibration_auto_status_show(struct device *dev,
1249 struct device_attribute *a,
1250 char *buf)
1251 {
1252 return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_ACCEL_SHIFT);
1253 }
1254
in_gyro_calibration_auto_status_show(struct device * dev,struct device_attribute * a,char * buf)1255 static ssize_t in_gyro_calibration_auto_status_show(struct device *dev,
1256 struct device_attribute *a,
1257 char *buf)
1258 {
1259 return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_GYRO_SHIFT);
1260 }
1261
in_magn_calibration_auto_status_show(struct device * dev,struct device_attribute * a,char * buf)1262 static ssize_t in_magn_calibration_auto_status_show(struct device *dev,
1263 struct device_attribute *a,
1264 char *buf)
1265 {
1266 return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_MAGN_SHIFT);
1267 }
1268
bno055_debugfs_reg_access(struct iio_dev * iio_dev,unsigned int reg,unsigned int writeval,unsigned int * readval)1269 static int bno055_debugfs_reg_access(struct iio_dev *iio_dev, unsigned int reg,
1270 unsigned int writeval, unsigned int *readval)
1271 {
1272 struct bno055_priv *priv = iio_priv(iio_dev);
1273
1274 if (readval)
1275 return regmap_read(priv->regmap, reg, readval);
1276 else
1277 return regmap_write(priv->regmap, reg, writeval);
1278 }
1279
bno055_show_fw_version(struct file * file,char __user * userbuf,size_t count,loff_t * ppos)1280 static ssize_t bno055_show_fw_version(struct file *file, char __user *userbuf,
1281 size_t count, loff_t *ppos)
1282 {
1283 struct bno055_priv *priv = file->private_data;
1284 int rev, ver;
1285 char *buf;
1286 int ret;
1287
1288 ret = regmap_read(priv->regmap, BNO055_SW_REV_LSB_REG, &rev);
1289 if (ret)
1290 return ret;
1291
1292 ret = regmap_read(priv->regmap, BNO055_SW_REV_MSB_REG, &ver);
1293 if (ret)
1294 return ret;
1295
1296 buf = kasprintf(GFP_KERNEL, "ver: 0x%x, rev: 0x%x\n", ver, rev);
1297 if (!buf)
1298 return -ENOMEM;
1299
1300 ret = simple_read_from_buffer(userbuf, count, ppos, buf, strlen(buf));
1301 kfree(buf);
1302
1303 return ret;
1304 }
1305
1306 static const struct file_operations bno055_fw_version_ops = {
1307 .open = simple_open,
1308 .read = bno055_show_fw_version,
1309 .llseek = default_llseek,
1310 .owner = THIS_MODULE,
1311 };
1312
bno055_debugfs_remove(void * _priv)1313 static void bno055_debugfs_remove(void *_priv)
1314 {
1315 struct bno055_priv *priv = _priv;
1316
1317 debugfs_remove(priv->debugfs);
1318 priv->debugfs = NULL;
1319 }
1320
bno055_debugfs_init(struct iio_dev * iio_dev)1321 static void bno055_debugfs_init(struct iio_dev *iio_dev)
1322 {
1323 struct bno055_priv *priv = iio_priv(iio_dev);
1324
1325 priv->debugfs = debugfs_create_file("firmware_version", 0400,
1326 iio_get_debugfs_dentry(iio_dev),
1327 priv, &bno055_fw_version_ops);
1328 if (!IS_ERR(priv->debugfs))
1329 devm_add_action_or_reset(priv->dev, bno055_debugfs_remove,
1330 priv);
1331 if (IS_ERR_OR_NULL(priv->debugfs))
1332 dev_warn(priv->dev, "failed to setup debugfs");
1333 }
1334
1335 static IIO_DEVICE_ATTR_RW(fusion_enable, 0);
1336 static IIO_DEVICE_ATTR_RW(in_magn_calibration_fast_enable, 0);
1337 static IIO_DEVICE_ATTR_RW(in_accel_range_raw, 0);
1338
1339 static IIO_DEVICE_ATTR_RO(in_accel_range_raw_available, 0);
1340 static IIO_DEVICE_ATTR_RO(sys_calibration_auto_status, 0);
1341 static IIO_DEVICE_ATTR_RO(in_accel_calibration_auto_status, 0);
1342 static IIO_DEVICE_ATTR_RO(in_gyro_calibration_auto_status, 0);
1343 static IIO_DEVICE_ATTR_RO(in_magn_calibration_auto_status, 0);
1344 static IIO_DEVICE_ATTR_RO(serialnumber, 0);
1345
1346 static struct attribute *bno055_attrs[] = {
1347 &iio_dev_attr_in_accel_range_raw_available.dev_attr.attr,
1348 &iio_dev_attr_in_accel_range_raw.dev_attr.attr,
1349 &iio_dev_attr_fusion_enable.dev_attr.attr,
1350 &iio_dev_attr_in_magn_calibration_fast_enable.dev_attr.attr,
1351 &iio_dev_attr_sys_calibration_auto_status.dev_attr.attr,
1352 &iio_dev_attr_in_accel_calibration_auto_status.dev_attr.attr,
1353 &iio_dev_attr_in_gyro_calibration_auto_status.dev_attr.attr,
1354 &iio_dev_attr_in_magn_calibration_auto_status.dev_attr.attr,
1355 &iio_dev_attr_serialnumber.dev_attr.attr,
1356 NULL
1357 };
1358
1359 static const BIN_ATTR_RO(calibration_data, BNO055_CALDATA_LEN);
1360
1361 static const struct bin_attribute *const bno055_bin_attrs[] = {
1362 &bin_attr_calibration_data,
1363 NULL
1364 };
1365
1366 static const struct attribute_group bno055_attrs_group = {
1367 .attrs = bno055_attrs,
1368 .bin_attrs = bno055_bin_attrs,
1369 };
1370
1371 static const struct iio_info bno055_info = {
1372 .read_raw_multi = bno055_read_raw_multi,
1373 .read_avail = bno055_read_avail,
1374 .write_raw = bno055_write_raw,
1375 .attrs = &bno055_attrs_group,
1376 .debugfs_reg_access = bno055_debugfs_reg_access,
1377 };
1378
1379 /*
1380 * Reads len samples from the HW, stores them in buf starting from buf_idx,
1381 * and applies mask to cull (skip) unneeded samples.
1382 * Updates buf_idx incrementing with the number of stored samples.
1383 * Samples from HW are transferred into buf, then in-place copy on buf is
1384 * performed in order to cull samples that need to be skipped.
1385 * This avoids copies of the first samples until we hit the 1st sample to skip,
1386 * and also avoids having an extra bounce buffer.
1387 * buf must be able to contain len elements in spite of how many samples we are
1388 * going to cull.
1389 */
bno055_scan_xfer(struct bno055_priv * priv,int start_ch,int len,unsigned long mask,__le16 * buf,int * buf_idx)1390 static int bno055_scan_xfer(struct bno055_priv *priv,
1391 int start_ch, int len, unsigned long mask,
1392 __le16 *buf, int *buf_idx)
1393 {
1394 const int base = BNO055_ACC_DATA_X_LSB_REG;
1395 bool quat_in_read = false;
1396 int buf_base = *buf_idx;
1397 __le16 *dst, *src;
1398 int offs_fixup = 0;
1399 int xfer_len = len;
1400 int ret;
1401 int i, n;
1402
1403 if (!mask)
1404 return 0;
1405
1406 /*
1407 * All channels are made up 1 16-bit sample, except for quaternion that
1408 * is made up 4 16-bit values.
1409 * For us the quaternion CH is just like 4 regular CHs.
1410 * If our read starts past the quaternion make sure to adjust the
1411 * starting offset; if the quaternion is contained in our scan then make
1412 * sure to adjust the read len.
1413 */
1414 if (start_ch > BNO055_SCAN_QUATERNION) {
1415 start_ch += 3;
1416 } else if ((start_ch <= BNO055_SCAN_QUATERNION) &&
1417 ((start_ch + len) > BNO055_SCAN_QUATERNION)) {
1418 quat_in_read = true;
1419 xfer_len += 3;
1420 }
1421
1422 ret = regmap_bulk_read(priv->regmap,
1423 base + start_ch * sizeof(__le16),
1424 buf + buf_base,
1425 xfer_len * sizeof(__le16));
1426 if (ret)
1427 return ret;
1428
1429 for_each_set_bit(i, &mask, len) {
1430 if (quat_in_read && ((start_ch + i) > BNO055_SCAN_QUATERNION))
1431 offs_fixup = 3;
1432
1433 dst = buf + *buf_idx;
1434 src = buf + buf_base + offs_fixup + i;
1435
1436 n = (start_ch + i == BNO055_SCAN_QUATERNION) ? 4 : 1;
1437
1438 if (dst != src)
1439 memcpy(dst, src, n * sizeof(__le16));
1440
1441 *buf_idx += n;
1442 }
1443 return 0;
1444 }
1445
bno055_trigger_handler(int irq,void * p)1446 static irqreturn_t bno055_trigger_handler(int irq, void *p)
1447 {
1448 struct iio_poll_func *pf = p;
1449 struct iio_dev *iio_dev = pf->indio_dev;
1450 struct bno055_priv *priv = iio_priv(iio_dev);
1451 int xfer_start, start, end, prev_end;
1452 unsigned long mask;
1453 int quat_extra_len;
1454 bool first = true;
1455 int buf_idx = 0;
1456 bool thr_hit;
1457 int ret;
1458
1459 mutex_lock(&priv->lock);
1460
1461 /*
1462 * Walk the bitmap and eventually perform several transfers.
1463 * Bitmap ones-fields that are separated by gaps <= xfer_burst_break_thr
1464 * will be included in same transfer.
1465 * Every time the bitmap contains a gap wider than xfer_burst_break_thr
1466 * then we split the transfer, skipping the gap.
1467 */
1468 for_each_set_bitrange(start, end, iio_dev->active_scan_mask,
1469 iio_get_masklength(iio_dev)) {
1470 /*
1471 * First transfer will start from the beginning of the first
1472 * ones-field in the bitmap
1473 */
1474 if (first) {
1475 xfer_start = start;
1476 } else {
1477 /*
1478 * We found the next ones-field; check whether to
1479 * include it in * the current transfer or not (i.e.
1480 * let's perform the current * transfer and prepare for
1481 * another one).
1482 */
1483
1484 /*
1485 * In case the zeros-gap contains the quaternion bit,
1486 * then its length is actually 4 words instead of 1
1487 * (i.e. +3 wrt other channels).
1488 */
1489 quat_extra_len = ((start > BNO055_SCAN_QUATERNION) &&
1490 (prev_end <= BNO055_SCAN_QUATERNION)) ? 3 : 0;
1491
1492 /* If the gap is wider than xfer_burst_break_thr then.. */
1493 thr_hit = (start - prev_end + quat_extra_len) >
1494 priv->xfer_burst_break_thr;
1495
1496 /*
1497 * .. transfer all the data up to the gap. Then set the
1498 * next transfer start index at right after the gap
1499 * (i.e. at the start of this ones-field).
1500 */
1501 if (thr_hit) {
1502 mask = *iio_dev->active_scan_mask >> xfer_start;
1503 ret = bno055_scan_xfer(priv, xfer_start,
1504 prev_end - xfer_start,
1505 mask, priv->buf.chans, &buf_idx);
1506 if (ret)
1507 goto done;
1508 xfer_start = start;
1509 }
1510 }
1511 first = false;
1512 prev_end = end;
1513 }
1514
1515 /*
1516 * We finished walking the bitmap; no more gaps to check for. Just
1517 * perform the current transfer.
1518 */
1519 mask = *iio_dev->active_scan_mask >> xfer_start;
1520 ret = bno055_scan_xfer(priv, xfer_start,
1521 prev_end - xfer_start,
1522 mask, priv->buf.chans, &buf_idx);
1523
1524 if (!ret)
1525 iio_push_to_buffers_with_timestamp(iio_dev,
1526 &priv->buf, pf->timestamp);
1527 done:
1528 mutex_unlock(&priv->lock);
1529 iio_trigger_notify_done(iio_dev->trig);
1530 return IRQ_HANDLED;
1531 }
1532
bno055_buffer_preenable(struct iio_dev * indio_dev)1533 static int bno055_buffer_preenable(struct iio_dev *indio_dev)
1534 {
1535 struct bno055_priv *priv = iio_priv(indio_dev);
1536 const unsigned long fusion_mask =
1537 BIT(BNO055_SCAN_YAW) |
1538 BIT(BNO055_SCAN_ROLL) |
1539 BIT(BNO055_SCAN_PITCH) |
1540 BIT(BNO055_SCAN_QUATERNION) |
1541 BIT(BNO055_SCAN_LIA_X) |
1542 BIT(BNO055_SCAN_LIA_Y) |
1543 BIT(BNO055_SCAN_LIA_Z) |
1544 BIT(BNO055_SCAN_GRAVITY_X) |
1545 BIT(BNO055_SCAN_GRAVITY_Y) |
1546 BIT(BNO055_SCAN_GRAVITY_Z);
1547
1548 if (priv->operation_mode == BNO055_OPR_MODE_AMG &&
1549 bitmap_intersects(indio_dev->active_scan_mask, &fusion_mask,
1550 _BNO055_SCAN_MAX))
1551 return -EBUSY;
1552 return 0;
1553 }
1554
1555 static const struct iio_buffer_setup_ops bno055_buffer_setup_ops = {
1556 .preenable = bno055_buffer_preenable,
1557 };
1558
bno055_probe(struct device * dev,struct regmap * regmap,int xfer_burst_break_thr,bool sw_reset)1559 int bno055_probe(struct device *dev, struct regmap *regmap,
1560 int xfer_burst_break_thr, bool sw_reset)
1561 {
1562 const struct firmware *caldata = NULL;
1563 struct bno055_priv *priv;
1564 struct iio_dev *iio_dev;
1565 char *fw_name_buf;
1566 unsigned int val;
1567 int rev, ver;
1568 int ret;
1569
1570 iio_dev = devm_iio_device_alloc(dev, sizeof(*priv));
1571 if (!iio_dev)
1572 return -ENOMEM;
1573
1574 iio_dev->name = "bno055";
1575 priv = iio_priv(iio_dev);
1576 mutex_init(&priv->lock);
1577 priv->regmap = regmap;
1578 priv->dev = dev;
1579 priv->xfer_burst_break_thr = xfer_burst_break_thr;
1580 priv->sw_reset = sw_reset;
1581
1582 priv->reset_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_LOW);
1583 if (IS_ERR(priv->reset_gpio))
1584 return dev_err_probe(dev, PTR_ERR(priv->reset_gpio), "Failed to get reset GPIO\n");
1585
1586 priv->clk = devm_clk_get_optional_enabled(dev, "clk");
1587 if (IS_ERR(priv->clk))
1588 return dev_err_probe(dev, PTR_ERR(priv->clk), "Failed to get CLK\n");
1589
1590 if (priv->reset_gpio) {
1591 usleep_range(5000, 10000);
1592 gpiod_set_value_cansleep(priv->reset_gpio, 1);
1593 usleep_range(650000, 750000);
1594 } else if (!sw_reset) {
1595 dev_warn(dev, "No usable reset method; IMU may be unreliable\n");
1596 }
1597
1598 ret = regmap_read(priv->regmap, BNO055_CHIP_ID_REG, &val);
1599 if (ret)
1600 return ret;
1601
1602 if (val != BNO055_CHIP_ID_MAGIC)
1603 dev_warn(dev, "Unrecognized chip ID 0x%x\n", val);
1604
1605 /*
1606 * In case we haven't a HW reset pin, we can still reset the chip via
1607 * register write. This is probably nonsense in case we can't even
1608 * communicate with the chip or the chip isn't the one we expect (i.e.
1609 * we don't write to unknown chips), so we perform SW reset only after
1610 * chip magic ID check
1611 */
1612 if (!priv->reset_gpio) {
1613 ret = bno055_system_reset(priv);
1614 if (ret)
1615 return ret;
1616 }
1617
1618 ret = regmap_read(priv->regmap, BNO055_SW_REV_LSB_REG, &rev);
1619 if (ret)
1620 return ret;
1621
1622 ret = regmap_read(priv->regmap, BNO055_SW_REV_MSB_REG, &ver);
1623 if (ret)
1624 return ret;
1625
1626 /*
1627 * The stock FW version contains a bug (see comment at the beginning of
1628 * this file) that causes the anglvel scale to be changed depending on
1629 * the chip range setting. We workaround this, but we don't know what
1630 * other FW versions might do.
1631 */
1632 if (ver != 0x3 || rev != 0x11)
1633 dev_warn(dev, "Untested firmware version. Anglvel scale may not work as expected\n");
1634
1635 ret = regmap_bulk_read(priv->regmap, BNO055_UID_LOWER_REG,
1636 priv->uid, BNO055_UID_LEN);
1637 if (ret)
1638 return ret;
1639
1640 /* Sensor calibration data */
1641 fw_name_buf = kasprintf(GFP_KERNEL, BNO055_FW_UID_FMT,
1642 BNO055_UID_LEN, priv->uid);
1643 if (!fw_name_buf)
1644 return -ENOMEM;
1645
1646 ret = request_firmware(&caldata, fw_name_buf, dev);
1647 kfree(fw_name_buf);
1648 if (ret)
1649 ret = request_firmware(&caldata, BNO055_FW_GENERIC_NAME, dev);
1650 if (ret) {
1651 dev_notice(dev, "Calibration file load failed. See instruction in kernel Documentation/iio/bno055.rst\n");
1652 ret = bno055_init(priv, NULL, 0);
1653 } else {
1654 ret = bno055_init(priv, caldata->data, caldata->size);
1655 release_firmware(caldata);
1656 }
1657 if (ret)
1658 return ret;
1659
1660 priv->operation_mode = BNO055_OPR_MODE_FUSION;
1661 ret = bno055_operation_mode_do_set(priv, priv->operation_mode);
1662 if (ret)
1663 return ret;
1664
1665 ret = devm_add_action_or_reset(dev, bno055_uninit, priv);
1666 if (ret)
1667 return ret;
1668
1669 iio_dev->channels = bno055_channels;
1670 iio_dev->num_channels = ARRAY_SIZE(bno055_channels);
1671 iio_dev->info = &bno055_info;
1672 iio_dev->modes = INDIO_DIRECT_MODE;
1673
1674 ret = devm_iio_triggered_buffer_setup(dev, iio_dev,
1675 iio_pollfunc_store_time,
1676 bno055_trigger_handler,
1677 &bno055_buffer_setup_ops);
1678 if (ret)
1679 return ret;
1680
1681 ret = devm_iio_device_register(dev, iio_dev);
1682 if (ret)
1683 return ret;
1684
1685 bno055_debugfs_init(iio_dev);
1686
1687 return 0;
1688 }
1689 EXPORT_SYMBOL_NS_GPL(bno055_probe, "IIO_BNO055");
1690
1691 MODULE_AUTHOR("Andrea Merello <andrea.merello@iit.it>");
1692 MODULE_DESCRIPTION("Bosch BNO055 driver");
1693 MODULE_LICENSE("GPL");
1694