1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * Copyright (C) 2020 Invensense, Inc.
4 */
5
6 #include <linux/kernel.h>
7 #include <linux/device.h>
8 #include <linux/module.h>
9 #include <linux/slab.h>
10 #include <linux/delay.h>
11 #include <linux/mutex.h>
12 #include <linux/interrupt.h>
13 #include <linux/irq.h>
14 #include <linux/regulator/consumer.h>
15 #include <linux/pm_runtime.h>
16 #include <linux/property.h>
17 #include <linux/regmap.h>
18 #include <linux/iio/iio.h>
19
20 #include "inv_icm42600.h"
21 #include "inv_icm42600_buffer.h"
22 #include "inv_icm42600_timestamp.h"
23
24 static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
25 {
26 .name = "user banks",
27 .range_min = 0x0000,
28 .range_max = 0x4FFF,
29 .selector_reg = INV_ICM42600_REG_BANK_SEL,
30 .selector_mask = INV_ICM42600_BANK_SEL_MASK,
31 .selector_shift = 0,
32 .window_start = 0,
33 .window_len = 0x1000,
34 },
35 };
36
37 const struct regmap_config inv_icm42600_regmap_config = {
38 .reg_bits = 8,
39 .val_bits = 8,
40 .max_register = 0x4FFF,
41 .ranges = inv_icm42600_regmap_ranges,
42 .num_ranges = ARRAY_SIZE(inv_icm42600_regmap_ranges),
43 };
44 EXPORT_SYMBOL_GPL(inv_icm42600_regmap_config);
45
46 struct inv_icm42600_hw {
47 uint8_t whoami;
48 const char *name;
49 const struct inv_icm42600_conf *conf;
50 };
51
52 /* chip initial default configuration */
53 static const struct inv_icm42600_conf inv_icm42600_default_conf = {
54 .gyro = {
55 .mode = INV_ICM42600_SENSOR_MODE_OFF,
56 .fs = INV_ICM42600_GYRO_FS_2000DPS,
57 .odr = INV_ICM42600_ODR_50HZ,
58 .filter = INV_ICM42600_FILTER_BW_ODR_DIV_2,
59 },
60 .accel = {
61 .mode = INV_ICM42600_SENSOR_MODE_OFF,
62 .fs = INV_ICM42600_ACCEL_FS_16G,
63 .odr = INV_ICM42600_ODR_50HZ,
64 .filter = INV_ICM42600_FILTER_BW_ODR_DIV_2,
65 },
66 .temp_en = false,
67 };
68
69 static const struct inv_icm42600_hw inv_icm42600_hw[INV_CHIP_NB] = {
70 [INV_CHIP_ICM42600] = {
71 .whoami = INV_ICM42600_WHOAMI_ICM42600,
72 .name = "icm42600",
73 .conf = &inv_icm42600_default_conf,
74 },
75 [INV_CHIP_ICM42602] = {
76 .whoami = INV_ICM42600_WHOAMI_ICM42602,
77 .name = "icm42602",
78 .conf = &inv_icm42600_default_conf,
79 },
80 [INV_CHIP_ICM42605] = {
81 .whoami = INV_ICM42600_WHOAMI_ICM42605,
82 .name = "icm42605",
83 .conf = &inv_icm42600_default_conf,
84 },
85 [INV_CHIP_ICM42622] = {
86 .whoami = INV_ICM42600_WHOAMI_ICM42622,
87 .name = "icm42622",
88 .conf = &inv_icm42600_default_conf,
89 },
90 };
91
92 const struct iio_mount_matrix *
inv_icm42600_get_mount_matrix(const struct iio_dev * indio_dev,const struct iio_chan_spec * chan)93 inv_icm42600_get_mount_matrix(const struct iio_dev *indio_dev,
94 const struct iio_chan_spec *chan)
95 {
96 const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
97
98 return &st->orientation;
99 }
100
inv_icm42600_odr_to_period(enum inv_icm42600_odr odr)101 uint32_t inv_icm42600_odr_to_period(enum inv_icm42600_odr odr)
102 {
103 static uint32_t odr_periods[INV_ICM42600_ODR_NB] = {
104 /* reserved values */
105 0, 0, 0,
106 /* 8kHz */
107 125000,
108 /* 4kHz */
109 250000,
110 /* 2kHz */
111 500000,
112 /* 1kHz */
113 1000000,
114 /* 200Hz */
115 5000000,
116 /* 100Hz */
117 10000000,
118 /* 50Hz */
119 20000000,
120 /* 25Hz */
121 40000000,
122 /* 12.5Hz */
123 80000000,
124 /* 6.25Hz */
125 160000000,
126 /* 3.125Hz */
127 320000000,
128 /* 1.5625Hz */
129 640000000,
130 /* 500Hz */
131 2000000,
132 };
133
134 return odr_periods[odr];
135 }
136
inv_icm42600_set_pwr_mgmt0(struct inv_icm42600_state * st,enum inv_icm42600_sensor_mode gyro,enum inv_icm42600_sensor_mode accel,bool temp,unsigned int * sleep_ms)137 static int inv_icm42600_set_pwr_mgmt0(struct inv_icm42600_state *st,
138 enum inv_icm42600_sensor_mode gyro,
139 enum inv_icm42600_sensor_mode accel,
140 bool temp, unsigned int *sleep_ms)
141 {
142 enum inv_icm42600_sensor_mode oldgyro = st->conf.gyro.mode;
143 enum inv_icm42600_sensor_mode oldaccel = st->conf.accel.mode;
144 bool oldtemp = st->conf.temp_en;
145 unsigned int sleepval;
146 unsigned int val;
147 int ret;
148
149 /* if nothing changed, exit */
150 if (gyro == oldgyro && accel == oldaccel && temp == oldtemp)
151 return 0;
152
153 val = INV_ICM42600_PWR_MGMT0_GYRO(gyro) |
154 INV_ICM42600_PWR_MGMT0_ACCEL(accel);
155 if (!temp)
156 val |= INV_ICM42600_PWR_MGMT0_TEMP_DIS;
157 ret = regmap_write(st->map, INV_ICM42600_REG_PWR_MGMT0, val);
158 if (ret)
159 return ret;
160
161 st->conf.gyro.mode = gyro;
162 st->conf.accel.mode = accel;
163 st->conf.temp_en = temp;
164
165 /* compute required wait time for sensors to stabilize */
166 sleepval = 0;
167 /* temperature stabilization time */
168 if (temp && !oldtemp) {
169 if (sleepval < INV_ICM42600_TEMP_STARTUP_TIME_MS)
170 sleepval = INV_ICM42600_TEMP_STARTUP_TIME_MS;
171 }
172 /* accel startup time */
173 if (accel != oldaccel && oldaccel == INV_ICM42600_SENSOR_MODE_OFF) {
174 /* block any register write for at least 200 µs */
175 usleep_range(200, 300);
176 if (sleepval < INV_ICM42600_ACCEL_STARTUP_TIME_MS)
177 sleepval = INV_ICM42600_ACCEL_STARTUP_TIME_MS;
178 }
179 if (gyro != oldgyro) {
180 /* gyro startup time */
181 if (oldgyro == INV_ICM42600_SENSOR_MODE_OFF) {
182 /* block any register write for at least 200 µs */
183 usleep_range(200, 300);
184 if (sleepval < INV_ICM42600_GYRO_STARTUP_TIME_MS)
185 sleepval = INV_ICM42600_GYRO_STARTUP_TIME_MS;
186 /* gyro stop time */
187 } else if (gyro == INV_ICM42600_SENSOR_MODE_OFF) {
188 if (sleepval < INV_ICM42600_GYRO_STOP_TIME_MS)
189 sleepval = INV_ICM42600_GYRO_STOP_TIME_MS;
190 }
191 }
192
193 /* deferred sleep value if sleep pointer is provided or direct sleep */
194 if (sleep_ms)
195 *sleep_ms = sleepval;
196 else if (sleepval)
197 msleep(sleepval);
198
199 return 0;
200 }
201
inv_icm42600_set_accel_conf(struct inv_icm42600_state * st,struct inv_icm42600_sensor_conf * conf,unsigned int * sleep_ms)202 int inv_icm42600_set_accel_conf(struct inv_icm42600_state *st,
203 struct inv_icm42600_sensor_conf *conf,
204 unsigned int *sleep_ms)
205 {
206 struct inv_icm42600_sensor_conf *oldconf = &st->conf.accel;
207 unsigned int val;
208 int ret;
209
210 /* Sanitize missing values with current values */
211 if (conf->mode < 0)
212 conf->mode = oldconf->mode;
213 if (conf->fs < 0)
214 conf->fs = oldconf->fs;
215 if (conf->odr < 0)
216 conf->odr = oldconf->odr;
217 if (conf->filter < 0)
218 conf->filter = oldconf->filter;
219
220 /* set ACCEL_CONFIG0 register (accel fullscale & odr) */
221 if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) {
222 val = INV_ICM42600_ACCEL_CONFIG0_FS(conf->fs) |
223 INV_ICM42600_ACCEL_CONFIG0_ODR(conf->odr);
224 ret = regmap_write(st->map, INV_ICM42600_REG_ACCEL_CONFIG0, val);
225 if (ret)
226 return ret;
227 oldconf->fs = conf->fs;
228 oldconf->odr = conf->odr;
229 }
230
231 /* set GYRO_ACCEL_CONFIG0 register (accel filter) */
232 if (conf->filter != oldconf->filter) {
233 val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(conf->filter) |
234 INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(st->conf.gyro.filter);
235 ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val);
236 if (ret)
237 return ret;
238 oldconf->filter = conf->filter;
239 }
240
241 /* set PWR_MGMT0 register (accel sensor mode) */
242 return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode, conf->mode,
243 st->conf.temp_en, sleep_ms);
244 }
245
inv_icm42600_set_gyro_conf(struct inv_icm42600_state * st,struct inv_icm42600_sensor_conf * conf,unsigned int * sleep_ms)246 int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
247 struct inv_icm42600_sensor_conf *conf,
248 unsigned int *sleep_ms)
249 {
250 struct inv_icm42600_sensor_conf *oldconf = &st->conf.gyro;
251 unsigned int val;
252 int ret;
253
254 /* sanitize missing values with current values */
255 if (conf->mode < 0)
256 conf->mode = oldconf->mode;
257 if (conf->fs < 0)
258 conf->fs = oldconf->fs;
259 if (conf->odr < 0)
260 conf->odr = oldconf->odr;
261 if (conf->filter < 0)
262 conf->filter = oldconf->filter;
263
264 /* set GYRO_CONFIG0 register (gyro fullscale & odr) */
265 if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) {
266 val = INV_ICM42600_GYRO_CONFIG0_FS(conf->fs) |
267 INV_ICM42600_GYRO_CONFIG0_ODR(conf->odr);
268 ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_CONFIG0, val);
269 if (ret)
270 return ret;
271 oldconf->fs = conf->fs;
272 oldconf->odr = conf->odr;
273 }
274
275 /* set GYRO_ACCEL_CONFIG0 register (gyro filter) */
276 if (conf->filter != oldconf->filter) {
277 val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(st->conf.accel.filter) |
278 INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(conf->filter);
279 ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val);
280 if (ret)
281 return ret;
282 oldconf->filter = conf->filter;
283 }
284
285 /* set PWR_MGMT0 register (gyro sensor mode) */
286 return inv_icm42600_set_pwr_mgmt0(st, conf->mode, st->conf.accel.mode,
287 st->conf.temp_en, sleep_ms);
288
289 return 0;
290 }
291
inv_icm42600_set_temp_conf(struct inv_icm42600_state * st,bool enable,unsigned int * sleep_ms)292 int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
293 unsigned int *sleep_ms)
294 {
295 return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode,
296 st->conf.accel.mode, enable,
297 sleep_ms);
298 }
299
inv_icm42600_debugfs_reg(struct iio_dev * indio_dev,unsigned int reg,unsigned int writeval,unsigned int * readval)300 int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg,
301 unsigned int writeval, unsigned int *readval)
302 {
303 struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
304 int ret;
305
306 mutex_lock(&st->lock);
307
308 if (readval)
309 ret = regmap_read(st->map, reg, readval);
310 else
311 ret = regmap_write(st->map, reg, writeval);
312
313 mutex_unlock(&st->lock);
314
315 return ret;
316 }
317
inv_icm42600_set_conf(struct inv_icm42600_state * st,const struct inv_icm42600_conf * conf)318 static int inv_icm42600_set_conf(struct inv_icm42600_state *st,
319 const struct inv_icm42600_conf *conf)
320 {
321 unsigned int val;
322 int ret;
323
324 /* set PWR_MGMT0 register (gyro & accel sensor mode, temp enabled) */
325 val = INV_ICM42600_PWR_MGMT0_GYRO(conf->gyro.mode) |
326 INV_ICM42600_PWR_MGMT0_ACCEL(conf->accel.mode);
327 if (!conf->temp_en)
328 val |= INV_ICM42600_PWR_MGMT0_TEMP_DIS;
329 ret = regmap_write(st->map, INV_ICM42600_REG_PWR_MGMT0, val);
330 if (ret)
331 return ret;
332
333 /* set GYRO_CONFIG0 register (gyro fullscale & odr) */
334 val = INV_ICM42600_GYRO_CONFIG0_FS(conf->gyro.fs) |
335 INV_ICM42600_GYRO_CONFIG0_ODR(conf->gyro.odr);
336 ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_CONFIG0, val);
337 if (ret)
338 return ret;
339
340 /* set ACCEL_CONFIG0 register (accel fullscale & odr) */
341 val = INV_ICM42600_ACCEL_CONFIG0_FS(conf->accel.fs) |
342 INV_ICM42600_ACCEL_CONFIG0_ODR(conf->accel.odr);
343 ret = regmap_write(st->map, INV_ICM42600_REG_ACCEL_CONFIG0, val);
344 if (ret)
345 return ret;
346
347 /* set GYRO_ACCEL_CONFIG0 register (gyro & accel filters) */
348 val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(conf->accel.filter) |
349 INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(conf->gyro.filter);
350 ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val);
351 if (ret)
352 return ret;
353
354 /* update internal conf */
355 st->conf = *conf;
356
357 return 0;
358 }
359
360 /**
361 * inv_icm42600_setup() - check and setup chip
362 * @st: driver internal state
363 * @bus_setup: callback for setting up bus specific registers
364 *
365 * Returns 0 on success, a negative error code otherwise.
366 */
inv_icm42600_setup(struct inv_icm42600_state * st,inv_icm42600_bus_setup bus_setup)367 static int inv_icm42600_setup(struct inv_icm42600_state *st,
368 inv_icm42600_bus_setup bus_setup)
369 {
370 const struct inv_icm42600_hw *hw = &inv_icm42600_hw[st->chip];
371 const struct device *dev = regmap_get_device(st->map);
372 unsigned int val;
373 int ret;
374
375 /* check chip self-identification value */
376 ret = regmap_read(st->map, INV_ICM42600_REG_WHOAMI, &val);
377 if (ret)
378 return ret;
379 if (val != hw->whoami) {
380 dev_err(dev, "invalid whoami %#02x expected %#02x (%s)\n",
381 val, hw->whoami, hw->name);
382 return -ENODEV;
383 }
384 st->name = hw->name;
385
386 /* reset to make sure previous state are not there */
387 ret = regmap_write(st->map, INV_ICM42600_REG_DEVICE_CONFIG,
388 INV_ICM42600_DEVICE_CONFIG_SOFT_RESET);
389 if (ret)
390 return ret;
391 msleep(INV_ICM42600_RESET_TIME_MS);
392
393 ret = regmap_read(st->map, INV_ICM42600_REG_INT_STATUS, &val);
394 if (ret)
395 return ret;
396 if (!(val & INV_ICM42600_INT_STATUS_RESET_DONE)) {
397 dev_err(dev, "reset error, reset done bit not set\n");
398 return -ENODEV;
399 }
400
401 /* set chip bus configuration */
402 ret = bus_setup(st);
403 if (ret)
404 return ret;
405
406 /* sensor data in big-endian (default) */
407 ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
408 INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN,
409 INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN);
410 if (ret)
411 return ret;
412
413 return inv_icm42600_set_conf(st, hw->conf);
414 }
415
inv_icm42600_irq_timestamp(int irq,void * _data)416 static irqreturn_t inv_icm42600_irq_timestamp(int irq, void *_data)
417 {
418 struct inv_icm42600_state *st = _data;
419
420 st->timestamp.gyro = iio_get_time_ns(st->indio_gyro);
421 st->timestamp.accel = iio_get_time_ns(st->indio_accel);
422
423 return IRQ_WAKE_THREAD;
424 }
425
inv_icm42600_irq_handler(int irq,void * _data)426 static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
427 {
428 struct inv_icm42600_state *st = _data;
429 struct device *dev = regmap_get_device(st->map);
430 unsigned int status;
431 int ret;
432
433 mutex_lock(&st->lock);
434
435 ret = regmap_read(st->map, INV_ICM42600_REG_INT_STATUS, &status);
436 if (ret)
437 goto out_unlock;
438
439 /* FIFO full */
440 if (status & INV_ICM42600_INT_STATUS_FIFO_FULL)
441 dev_warn(dev, "FIFO full data lost!\n");
442
443 /* FIFO threshold reached */
444 if (status & INV_ICM42600_INT_STATUS_FIFO_THS) {
445 ret = inv_icm42600_buffer_fifo_read(st, 0);
446 if (ret) {
447 dev_err(dev, "FIFO read error %d\n", ret);
448 goto out_unlock;
449 }
450 ret = inv_icm42600_buffer_fifo_parse(st);
451 if (ret)
452 dev_err(dev, "FIFO parsing error %d\n", ret);
453 }
454
455 out_unlock:
456 mutex_unlock(&st->lock);
457 return IRQ_HANDLED;
458 }
459
460 /**
461 * inv_icm42600_irq_init() - initialize int pin and interrupt handler
462 * @st: driver internal state
463 * @irq: irq number
464 * @irq_type: irq trigger type
465 * @open_drain: true if irq is open drain, false for push-pull
466 *
467 * Returns 0 on success, a negative error code otherwise.
468 */
inv_icm42600_irq_init(struct inv_icm42600_state * st,int irq,int irq_type,bool open_drain)469 static int inv_icm42600_irq_init(struct inv_icm42600_state *st, int irq,
470 int irq_type, bool open_drain)
471 {
472 struct device *dev = regmap_get_device(st->map);
473 unsigned int val;
474 int ret;
475
476 /* configure INT1 interrupt: default is active low on edge */
477 switch (irq_type) {
478 case IRQF_TRIGGER_RISING:
479 case IRQF_TRIGGER_HIGH:
480 val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_HIGH;
481 break;
482 default:
483 val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW;
484 break;
485 }
486
487 switch (irq_type) {
488 case IRQF_TRIGGER_LOW:
489 case IRQF_TRIGGER_HIGH:
490 val |= INV_ICM42600_INT_CONFIG_INT1_LATCHED;
491 break;
492 default:
493 break;
494 }
495
496 if (!open_drain)
497 val |= INV_ICM42600_INT_CONFIG_INT1_PUSH_PULL;
498
499 ret = regmap_write(st->map, INV_ICM42600_REG_INT_CONFIG, val);
500 if (ret)
501 return ret;
502
503 /* Deassert async reset for proper INT pin operation (cf datasheet) */
504 ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_CONFIG1,
505 INV_ICM42600_INT_CONFIG1_ASYNC_RESET, 0);
506 if (ret)
507 return ret;
508
509 return devm_request_threaded_irq(dev, irq, inv_icm42600_irq_timestamp,
510 inv_icm42600_irq_handler, irq_type,
511 "inv_icm42600", st);
512 }
513
inv_icm42600_enable_regulator_vddio(struct inv_icm42600_state * st)514 static int inv_icm42600_enable_regulator_vddio(struct inv_icm42600_state *st)
515 {
516 int ret;
517
518 ret = regulator_enable(st->vddio_supply);
519 if (ret)
520 return ret;
521
522 /* wait a little for supply ramp */
523 usleep_range(3000, 4000);
524
525 return 0;
526 }
527
inv_icm42600_disable_vdd_reg(void * _data)528 static void inv_icm42600_disable_vdd_reg(void *_data)
529 {
530 struct inv_icm42600_state *st = _data;
531 const struct device *dev = regmap_get_device(st->map);
532 int ret;
533
534 ret = regulator_disable(st->vdd_supply);
535 if (ret)
536 dev_err(dev, "failed to disable vdd error %d\n", ret);
537 }
538
inv_icm42600_disable_vddio_reg(void * _data)539 static void inv_icm42600_disable_vddio_reg(void *_data)
540 {
541 struct inv_icm42600_state *st = _data;
542 const struct device *dev = regmap_get_device(st->map);
543 int ret;
544
545 ret = regulator_disable(st->vddio_supply);
546 if (ret)
547 dev_err(dev, "failed to disable vddio error %d\n", ret);
548 }
549
inv_icm42600_disable_pm(void * _data)550 static void inv_icm42600_disable_pm(void *_data)
551 {
552 struct device *dev = _data;
553
554 pm_runtime_put_sync(dev);
555 pm_runtime_disable(dev);
556 }
557
inv_icm42600_core_probe(struct regmap * regmap,int chip,int irq,inv_icm42600_bus_setup bus_setup)558 int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
559 inv_icm42600_bus_setup bus_setup)
560 {
561 struct device *dev = regmap_get_device(regmap);
562 struct inv_icm42600_state *st;
563 struct irq_data *irq_desc;
564 int irq_type;
565 bool open_drain;
566 int ret;
567
568 if (chip < 0 || chip >= INV_CHIP_NB) {
569 dev_err(dev, "invalid chip = %d\n", chip);
570 return -ENODEV;
571 }
572
573 /* get irq properties, set trigger falling by default */
574 irq_desc = irq_get_irq_data(irq);
575 if (!irq_desc) {
576 dev_err(dev, "could not find IRQ %d\n", irq);
577 return -EINVAL;
578 }
579
580 irq_type = irqd_get_trigger_type(irq_desc);
581 if (!irq_type)
582 irq_type = IRQF_TRIGGER_FALLING;
583
584 open_drain = device_property_read_bool(dev, "drive-open-drain");
585
586 st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
587 if (!st)
588 return -ENOMEM;
589
590 dev_set_drvdata(dev, st);
591 mutex_init(&st->lock);
592 st->chip = chip;
593 st->map = regmap;
594
595 ret = iio_read_mount_matrix(dev, &st->orientation);
596 if (ret) {
597 dev_err(dev, "failed to retrieve mounting matrix %d\n", ret);
598 return ret;
599 }
600
601 st->vdd_supply = devm_regulator_get(dev, "vdd");
602 if (IS_ERR(st->vdd_supply))
603 return PTR_ERR(st->vdd_supply);
604
605 st->vddio_supply = devm_regulator_get(dev, "vddio");
606 if (IS_ERR(st->vddio_supply))
607 return PTR_ERR(st->vddio_supply);
608
609 ret = regulator_enable(st->vdd_supply);
610 if (ret)
611 return ret;
612 msleep(INV_ICM42600_POWER_UP_TIME_MS);
613
614 ret = devm_add_action_or_reset(dev, inv_icm42600_disable_vdd_reg, st);
615 if (ret)
616 return ret;
617
618 ret = inv_icm42600_enable_regulator_vddio(st);
619 if (ret)
620 return ret;
621
622 ret = devm_add_action_or_reset(dev, inv_icm42600_disable_vddio_reg, st);
623 if (ret)
624 return ret;
625
626 /* setup chip registers */
627 ret = inv_icm42600_setup(st, bus_setup);
628 if (ret)
629 return ret;
630
631 ret = inv_icm42600_timestamp_setup(st);
632 if (ret)
633 return ret;
634
635 ret = inv_icm42600_buffer_init(st);
636 if (ret)
637 return ret;
638
639 st->indio_gyro = inv_icm42600_gyro_init(st);
640 if (IS_ERR(st->indio_gyro))
641 return PTR_ERR(st->indio_gyro);
642
643 st->indio_accel = inv_icm42600_accel_init(st);
644 if (IS_ERR(st->indio_accel))
645 return PTR_ERR(st->indio_accel);
646
647 ret = inv_icm42600_irq_init(st, irq, irq_type, open_drain);
648 if (ret)
649 return ret;
650
651 /* setup runtime power management */
652 ret = pm_runtime_set_active(dev);
653 if (ret)
654 return ret;
655 pm_runtime_get_noresume(dev);
656 pm_runtime_enable(dev);
657 pm_runtime_set_autosuspend_delay(dev, INV_ICM42600_SUSPEND_DELAY_MS);
658 pm_runtime_use_autosuspend(dev);
659 pm_runtime_put(dev);
660
661 return devm_add_action_or_reset(dev, inv_icm42600_disable_pm, dev);
662 }
663 EXPORT_SYMBOL_GPL(inv_icm42600_core_probe);
664
665 /*
666 * Suspend saves sensors state and turns everything off.
667 * Check first if runtime suspend has not already done the job.
668 */
inv_icm42600_suspend(struct device * dev)669 static int __maybe_unused inv_icm42600_suspend(struct device *dev)
670 {
671 struct inv_icm42600_state *st = dev_get_drvdata(dev);
672 int ret;
673
674 mutex_lock(&st->lock);
675
676 st->suspended.gyro = st->conf.gyro.mode;
677 st->suspended.accel = st->conf.accel.mode;
678 st->suspended.temp = st->conf.temp_en;
679 if (pm_runtime_suspended(dev)) {
680 ret = 0;
681 goto out_unlock;
682 }
683
684 /* disable FIFO data streaming */
685 if (st->fifo.on) {
686 ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
687 INV_ICM42600_FIFO_CONFIG_BYPASS);
688 if (ret)
689 goto out_unlock;
690 }
691
692 ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
693 INV_ICM42600_SENSOR_MODE_OFF, false,
694 NULL);
695 if (ret)
696 goto out_unlock;
697
698 regulator_disable(st->vddio_supply);
699
700 out_unlock:
701 mutex_unlock(&st->lock);
702 return ret;
703 }
704
705 /*
706 * System resume gets the system back on and restores the sensors state.
707 * Manually put runtime power management in system active state.
708 */
inv_icm42600_resume(struct device * dev)709 static int __maybe_unused inv_icm42600_resume(struct device *dev)
710 {
711 struct inv_icm42600_state *st = dev_get_drvdata(dev);
712 int ret;
713
714 mutex_lock(&st->lock);
715
716 ret = inv_icm42600_enable_regulator_vddio(st);
717 if (ret)
718 goto out_unlock;
719
720 pm_runtime_disable(dev);
721 pm_runtime_set_active(dev);
722 pm_runtime_enable(dev);
723
724 /* restore sensors state */
725 ret = inv_icm42600_set_pwr_mgmt0(st, st->suspended.gyro,
726 st->suspended.accel,
727 st->suspended.temp, NULL);
728 if (ret)
729 goto out_unlock;
730
731 /* restore FIFO data streaming */
732 if (st->fifo.on)
733 ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
734 INV_ICM42600_FIFO_CONFIG_STREAM);
735
736 out_unlock:
737 mutex_unlock(&st->lock);
738 return ret;
739 }
740
741 /* Runtime suspend will turn off sensors that are enabled by iio devices. */
inv_icm42600_runtime_suspend(struct device * dev)742 static int __maybe_unused inv_icm42600_runtime_suspend(struct device *dev)
743 {
744 struct inv_icm42600_state *st = dev_get_drvdata(dev);
745 int ret;
746
747 mutex_lock(&st->lock);
748
749 /* disable all sensors */
750 ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
751 INV_ICM42600_SENSOR_MODE_OFF, false,
752 NULL);
753 if (ret)
754 goto error_unlock;
755
756 regulator_disable(st->vddio_supply);
757
758 error_unlock:
759 mutex_unlock(&st->lock);
760 return ret;
761 }
762
763 /* Sensors are enabled by iio devices, no need to turn them back on here. */
inv_icm42600_runtime_resume(struct device * dev)764 static int __maybe_unused inv_icm42600_runtime_resume(struct device *dev)
765 {
766 struct inv_icm42600_state *st = dev_get_drvdata(dev);
767 int ret;
768
769 mutex_lock(&st->lock);
770
771 ret = inv_icm42600_enable_regulator_vddio(st);
772
773 mutex_unlock(&st->lock);
774 return ret;
775 }
776
777 const struct dev_pm_ops inv_icm42600_pm_ops = {
778 SET_SYSTEM_SLEEP_PM_OPS(inv_icm42600_suspend, inv_icm42600_resume)
779 SET_RUNTIME_PM_OPS(inv_icm42600_runtime_suspend,
780 inv_icm42600_runtime_resume, NULL)
781 };
782 EXPORT_SYMBOL_GPL(inv_icm42600_pm_ops);
783
784 MODULE_AUTHOR("InvenSense, Inc.");
785 MODULE_DESCRIPTION("InvenSense ICM-426xx device driver");
786 MODULE_LICENSE("GPL");
787