1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * Copyright (C) 2026 InvenSense, Inc.
4 */
5
6 #include <linux/bitfield.h>
7 #include <linux/cleanup.h>
8 #include <linux/delay.h>
9 #include <linux/device.h>
10 #include <linux/err.h>
11 #include <linux/iio/iio.h>
12 #include <linux/ktime.h>
13 #include <linux/module.h>
14 #include <linux/mutex.h>
15 #include <linux/pm_runtime.h>
16 #include <linux/regmap.h>
17 #include <linux/regulator/consumer.h>
18 #include <linux/time.h>
19 #include <linux/timekeeping.h>
20 #include <linux/types.h>
21
22 #include <asm/byteorder.h>
23
24 #include "inv_icm42607.h"
25
inv_icm42607_is_readable_reg(struct device * dev,unsigned int reg)26 static bool inv_icm42607_is_readable_reg(struct device *dev, unsigned int reg)
27 {
28 switch (reg) {
29 case INV_ICM42607_REG_MCLK_RDY ... INV_ICM42607_REG_INT_CONFIG:
30 case INV_ICM42607_REG_TEMP_DATA1 ... INV_ICM42607_REG_TMST_FSYNCL:
31 case INV_ICM42607_REG_APEX_DATA4 ... INV_ICM42607_REG_INTF_CONFIG1:
32 case INV_ICM42607_REG_INT_STATUS_DRDY ... INV_ICM42607_REG_FIFO_DATA:
33 case INV_ICM42607_REG_WHOAMI:
34 return true;
35 }
36
37 return false;
38 }
39
inv_icm42607_is_writeable_reg(struct device * dev,unsigned int reg)40 static bool inv_icm42607_is_writeable_reg(struct device *dev, unsigned int reg)
41 {
42 switch (reg) {
43 case INV_ICM42607_REG_DEVICE_CONFIG ... INV_ICM42607_REG_INT_CONFIG:
44 case INV_ICM42607_REG_PWR_MGMT0 ... INV_ICM42607_REG_INT_SOURCE4:
45 case INV_ICM42607_REG_INTF_CONFIG0 ... INV_ICM42607_REG_INTF_CONFIG1:
46 return true;
47 }
48
49 return false;
50 }
51
inv_icm42607_is_volatile_reg(struct device * dev,unsigned int reg)52 static bool inv_icm42607_is_volatile_reg(struct device *dev, unsigned int reg)
53 {
54 switch (reg) {
55 case INV_ICM42607_REG_MCLK_RDY:
56 case INV_ICM42607_REG_SIGNAL_PATH_RESET:
57 case INV_ICM42607_REG_TEMP_DATA1 ... INV_ICM42607_REG_APEX_DATA5:
58 case INV_ICM42607_REG_APEX_CONFIG0:
59 case INV_ICM42607_REG_FIFO_LOST_PKT0 ... INV_ICM42607_REG_APEX_DATA3:
60 case INV_ICM42607_REG_INT_STATUS_DRDY:
61 case INV_ICM42607_REG_INT_STATUS ... INV_ICM42607_REG_FIFO_DATA:
62 return true;
63 }
64
65 return false;
66 }
67
68 const struct regmap_config inv_icm42607_regmap_config = {
69 .reg_bits = 8,
70 .val_bits = 8,
71 .writeable_reg = inv_icm42607_is_writeable_reg,
72 .readable_reg = inv_icm42607_is_readable_reg,
73 .volatile_reg = inv_icm42607_is_volatile_reg,
74 .max_register = INV_ICM42607_REG_WHOAMI,
75 .cache_type = REGCACHE_MAPLE,
76 };
77 EXPORT_SYMBOL_NS_GPL(inv_icm42607_regmap_config, "IIO_ICM42607");
78
79 /* chip initial default configuration */
80 static const struct inv_icm42607_conf inv_icm42607_default_conf = {
81 .gyro = {
82 .mode = INV_ICM42607_SENSOR_MODE_OFF,
83 .fs = INV_ICM42607_GYRO_FS_1000DPS,
84 .odr = INV_ICM42607_ODR_100HZ,
85 .filter = INV_ICM42607_FILTER_BW_25HZ,
86 },
87 .accel = {
88 .mode = INV_ICM42607_SENSOR_MODE_OFF,
89 .fs = INV_ICM42607_ACCEL_FS_4G,
90 .odr = INV_ICM42607_ODR_100HZ,
91 .filter = INV_ICM42607_FILTER_BW_25HZ,
92 },
93 };
94
95 const struct inv_icm42607_hw inv_icm42607_hw_data = {
96 .whoami = INV_ICM42607_WHOAMI,
97 .name = "icm42607",
98 .conf = &inv_icm42607_default_conf,
99 };
100 EXPORT_SYMBOL_NS_GPL(inv_icm42607_hw_data, "IIO_ICM42607");
101
102 const struct inv_icm42607_hw inv_icm42607p_hw_data = {
103 .whoami = INV_ICM42607P_WHOAMI,
104 .name = "icm42607p",
105 .conf = &inv_icm42607_default_conf,
106 };
107 EXPORT_SYMBOL_NS_GPL(inv_icm42607p_hw_data, "IIO_ICM42607");
108
109 const struct iio_mount_matrix *
inv_icm42607_get_mount_matrix(struct iio_dev * indio_dev,const struct iio_chan_spec * chan)110 inv_icm42607_get_mount_matrix(struct iio_dev *indio_dev,
111 const struct iio_chan_spec *chan)
112 {
113 const struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
114
115 return &st->orientation;
116 }
117
inv_icm42607_odr_to_period_us(enum inv_icm42607_odr odr)118 static u32 inv_icm42607_odr_to_period_us(enum inv_icm42607_odr odr)
119 {
120 static const u32 odr_periods[INV_ICM42607_ODR_NB] = {
121 /* Reserved values */
122 0, 0, 0, 0, 0,
123 /* 1600Hz */
124 625,
125 /* 800Hz */
126 1250,
127 /* 400Hz */
128 2500,
129 /* 200Hz */
130 5000,
131 /* 100 Hz */
132 10000,
133 /* 50Hz */
134 20000,
135 /* 25Hz */
136 40000,
137 /* 12.5Hz */
138 80000,
139 /* 6.25Hz */
140 160000,
141 /* 3.125Hz */
142 320000,
143 /* 1.5625Hz */
144 640000,
145 };
146
147 odr = clamp(odr, INV_ICM42607_ODR_1600HZ, INV_ICM42607_ODR_1_5625HZ_LP);
148
149 return odr_periods[odr];
150 }
151
inv_icm42607_get_pwr_mgmt0(struct inv_icm42607_state * st,enum inv_icm42607_sensor_mode * gyro,enum inv_icm42607_sensor_mode * accel)152 int inv_icm42607_get_pwr_mgmt0(struct inv_icm42607_state *st,
153 enum inv_icm42607_sensor_mode *gyro,
154 enum inv_icm42607_sensor_mode *accel)
155 {
156 unsigned int val;
157 int ret;
158
159 ret = regmap_read(st->map, INV_ICM42607_REG_PWR_MGMT0, &val);
160 if (ret)
161 return ret;
162
163 *gyro = FIELD_GET(INV_ICM42607_PWR_MGMT0_GYRO_MODE_MASK, val);
164 *accel = FIELD_GET(INV_ICM42607_PWR_MGMT0_ACCEL_MODE_MASK, val);
165
166 return 0;
167 }
168
inv_icm42607_set_pwr_mgmt0(struct inv_icm42607_state * st,enum inv_icm42607_sensor_mode gyro,enum inv_icm42607_sensor_mode accel)169 static int inv_icm42607_set_pwr_mgmt0(struct inv_icm42607_state *st,
170 enum inv_icm42607_sensor_mode gyro,
171 enum inv_icm42607_sensor_mode accel)
172 {
173 enum inv_icm42607_sensor_mode oldaccel, oldgyro;
174 unsigned int sleepval_us, odr_us;
175 unsigned int val;
176 s64 disable_wait;
177 int ret;
178
179 ret = inv_icm42607_get_pwr_mgmt0(st, &oldgyro, &oldaccel);
180 if (ret)
181 return ret;
182
183 if (gyro == oldgyro && accel == oldaccel)
184 return 0;
185
186 /*
187 * Datasheet on page 14.26 says we need to ensure the gyro sensor is on
188 * for a minimum of 45ms. So if we transition from an on state to an
189 * off state make sure at least 45ms have passed before power off and
190 * wait if it hasn't. In case some platforms don't respond well to a
191 * sleep of 0, make sure the fsleep duration is > 0.
192 */
193 if (!gyro && oldgyro) {
194 disable_wait = clamp(ktime_us_delta(st->conf.gyro_stop, ktime_get()),
195 0, INV_ICM42607_GYRO_STOP_TIME_US);
196
197 if (disable_wait > 0)
198 fsleep(disable_wait);
199 }
200
201 val = FIELD_PREP(INV_ICM42607_PWR_MGMT0_GYRO_MODE_MASK, gyro) |
202 FIELD_PREP(INV_ICM42607_PWR_MGMT0_ACCEL_MODE_MASK, accel);
203 ret = regmap_write(st->map, INV_ICM42607_REG_PWR_MGMT0, val);
204 if (ret)
205 return ret;
206
207 /*
208 * If a state change occurs from off to on, sleep for the startup time
209 * of the sensor. Since more than one sensor can be transitioned from
210 * off to on, select the maximum time from each of the sensors changing
211 * from off to on. Also account for the time it takes for the first
212 * sample to be ready by looking up the odr value and adding it to the
213 * sleep time. The startup time for the temp sensor is considerably
214 * smaller than the startup time for the other sensors and one or more
215 * are required to be on for the temp sensor to function, so any start
216 * delay should be enough.
217 */
218 sleepval_us = 0;
219 odr_us = 0;
220 if (accel && !oldaccel) {
221 sleepval_us = max(sleepval_us, INV_ICM42607_ACCEL_STARTUP_TIME_US);
222 odr_us = max(odr_us, inv_icm42607_odr_to_period_us(st->conf.accel.odr));
223 }
224
225 if (gyro && !oldgyro) {
226 sleepval_us = max(sleepval_us, INV_ICM42607_GYRO_STARTUP_TIME_US);
227 odr_us = max(odr_us, inv_icm42607_odr_to_period_us(st->conf.gyro.odr));
228 /* Track the earliest we can turn off the gyroscope. */
229 st->conf.gyro_stop = ktime_add_us(ktime_get(),
230 INV_ICM42607_GYRO_STOP_TIME_US);
231 }
232
233 sleepval_us += odr_us;
234
235 /*
236 * Only sleep if sleepval_us is greater than 0 in case some platforms
237 * have issues with a 0 delay. The 0 delay can happen if one or both
238 * sensors is shut down.
239 */
240 if (sleepval_us > 0)
241 fsleep(sleepval_us);
242
243 return 0;
244 }
245
246 /*
247 * Sanity test between old and new config values, and note if we need
248 * to update register config0 or config1.
249 */
inv_icm42607_update_config(struct inv_icm42607_sensor_conf * conf,struct inv_icm42607_sensor_conf * oldconf,bool * config0,bool * config1)250 static void inv_icm42607_update_config(struct inv_icm42607_sensor_conf *conf,
251 struct inv_icm42607_sensor_conf *oldconf,
252 bool *config0, bool *config1)
253 {
254 if (conf->mode < 0)
255 conf->mode = oldconf->mode;
256 if (conf->fs < 0)
257 conf->fs = oldconf->fs;
258 if (conf->odr < 0)
259 conf->odr = oldconf->odr;
260 if (conf->filter < 0)
261 conf->filter = oldconf->filter;
262
263 *config0 = (conf->fs != oldconf->fs) || (conf->odr != oldconf->odr);
264
265 *config1 = (conf->filter != oldconf->filter);
266 }
267
inv_icm42607_set_sensor_conf(struct inv_icm42607_state * st,struct inv_icm42607_sensor_conf * conf,enum iio_chan_type chan_type)268 int inv_icm42607_set_sensor_conf(struct inv_icm42607_state *st,
269 struct inv_icm42607_sensor_conf *conf,
270 enum iio_chan_type chan_type)
271 {
272 enum inv_icm42607_sensor_mode accel_mode, gyro_mode;
273 struct inv_icm42607_sensor_conf *oldconf;
274 bool config0, config1;
275 unsigned int val;
276 int ret;
277
278 switch (chan_type) {
279 case IIO_ACCEL:
280 oldconf = &st->conf.accel;
281 break;
282 case IIO_ANGL_VEL:
283 oldconf = &st->conf.gyro;
284 break;
285 default:
286 return -EINVAL;
287 }
288
289 /*
290 * Since we are only making one-shot calls today, we can avoid needless
291 * on/off cycles if we rely on the runtime power management to handle
292 * shutting off the sensors when not in use. The downside is if we
293 * enable both sensors then only read from one, we have to wait for
294 * runtime PM to turn it off. So just keep the mode of the sensor we
295 * are not actively using to whatever it's already set as.
296 */
297 ret = inv_icm42607_get_pwr_mgmt0(st, &gyro_mode, &accel_mode);
298 if (ret)
299 return ret;
300
301 inv_icm42607_update_config(conf, oldconf, &config0, &config1);
302
303 if (config0) {
304 if (chan_type == IIO_ANGL_VEL) {
305 val = FIELD_PREP(INV_ICM42607_GYRO_CONFIG0_FS_SEL_MASK, conf->fs);
306 val |= FIELD_PREP(INV_ICM42607_GYRO_CONFIG0_ODR_MASK, conf->odr);
307 ret = regmap_write(st->map, INV_ICM42607_REG_GYRO_CONFIG0, val);
308 } else {
309 val = FIELD_PREP(INV_ICM42607_ACCEL_CONFIG0_FS_SEL_MASK, conf->fs);
310 val |= FIELD_PREP(INV_ICM42607_ACCEL_CONFIG0_ODR_MASK, conf->odr);
311 ret = regmap_write(st->map, INV_ICM42607_REG_ACCEL_CONFIG0, val);
312 }
313 if (ret)
314 return ret;
315
316 oldconf->fs = conf->fs;
317 oldconf->odr = conf->odr;
318 }
319
320 if (config1) {
321 if (chan_type == IIO_ANGL_VEL) {
322 val = FIELD_PREP(INV_ICM42607_GYRO_CONFIG1_FILTER_MASK,
323 conf->filter);
324 ret = regmap_update_bits(st->map, INV_ICM42607_REG_GYRO_CONFIG1,
325 INV_ICM42607_GYRO_CONFIG1_FILTER_MASK, val);
326 } else {
327 val = FIELD_PREP(INV_ICM42607_ACCEL_CONFIG1_FILTER_MASK,
328 conf->filter);
329 ret = regmap_update_bits(st->map, INV_ICM42607_REG_ACCEL_CONFIG1,
330 INV_ICM42607_ACCEL_CONFIG1_FILTER_MASK, val);
331 }
332 if (ret)
333 return ret;
334
335 oldconf->filter = conf->filter;
336 }
337
338 oldconf->mode = conf->mode;
339
340 switch (chan_type) {
341 case IIO_ACCEL:
342 return inv_icm42607_set_pwr_mgmt0(st, gyro_mode, conf->mode);
343 case IIO_ANGL_VEL:
344 return inv_icm42607_set_pwr_mgmt0(st, conf->mode, accel_mode);
345 default:
346 return -EINVAL;
347 }
348 }
349
inv_icm42607_read_sensor(struct iio_dev * indio_dev,struct iio_chan_spec const * chan,s16 * val)350 int inv_icm42607_read_sensor(struct iio_dev *indio_dev,
351 struct iio_chan_spec const *chan,
352 s16 *val)
353 {
354 struct inv_icm42607_sensor_conf conf = INV_ICM42607_SENSOR_CONF_INIT;
355 struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
356 struct inv_icm42607_sensor_state *sensor_st = iio_priv(indio_dev);
357 struct device *dev = regmap_get_device(st->map);
358 unsigned int reg;
359 __be16 data;
360 int ret;
361
362 if ((chan->type != IIO_ANGL_VEL) && (chan->type != IIO_ACCEL))
363 return -EINVAL;
364
365 switch (chan->channel2) {
366 case IIO_MOD_X:
367 if (chan->type == IIO_ANGL_VEL)
368 reg = INV_ICM42607_REG_GYRO_DATA_X1;
369 else
370 reg = INV_ICM42607_REG_ACCEL_DATA_X1;
371 break;
372 case IIO_MOD_Y:
373 if (chan->type == IIO_ANGL_VEL)
374 reg = INV_ICM42607_REG_GYRO_DATA_Y1;
375 else
376 reg = INV_ICM42607_REG_ACCEL_DATA_Y1;
377 break;
378 case IIO_MOD_Z:
379 if (chan->type == IIO_ANGL_VEL)
380 reg = INV_ICM42607_REG_GYRO_DATA_Z1;
381 else
382 reg = INV_ICM42607_REG_ACCEL_DATA_Z1;
383 break;
384 default:
385 return -EINVAL;
386 }
387
388 PM_RUNTIME_ACQUIRE_AUTOSUSPEND(dev, pm);
389 ret = PM_RUNTIME_ACQUIRE_ERR(&pm);
390 if (ret)
391 return ret;
392
393 guard(mutex)(&st->lock);
394
395 /* enable sensor */
396 conf.mode = sensor_st->power_mode;
397 conf.filter = sensor_st->filter;
398 ret = inv_icm42607_set_sensor_conf(st, &conf, chan->type);
399 if (ret)
400 return ret;
401
402 /* read sensor register data */
403 ret = regmap_bulk_read(st->map, reg, &data, sizeof(data));
404 if (ret)
405 return ret;
406
407 *val = be16_to_cpu(data);
408 if (*val == INV_ICM42607_DATA_INVALID)
409 return -EINVAL;
410
411 return 0;
412 }
413
inv_icm42607_set_init_conf(struct inv_icm42607_state * st,const struct inv_icm42607_conf * conf)414 static int inv_icm42607_set_init_conf(struct inv_icm42607_state *st,
415 const struct inv_icm42607_conf *conf)
416 {
417 unsigned int val;
418 int ret;
419
420 val = FIELD_PREP(INV_ICM42607_PWR_MGMT0_GYRO_MODE_MASK, conf->gyro.mode);
421 val |= FIELD_PREP(INV_ICM42607_PWR_MGMT0_ACCEL_MODE_MASK, conf->accel.mode);
422 ret = regmap_write(st->map, INV_ICM42607_REG_PWR_MGMT0, val);
423 if (ret)
424 return ret;
425
426 val = FIELD_PREP(INV_ICM42607_GYRO_CONFIG0_FS_SEL_MASK, conf->gyro.fs);
427 val |= FIELD_PREP(INV_ICM42607_GYRO_CONFIG0_ODR_MASK, conf->gyro.odr);
428 ret = regmap_write(st->map, INV_ICM42607_REG_GYRO_CONFIG0, val);
429 if (ret)
430 return ret;
431
432 val = FIELD_PREP(INV_ICM42607_ACCEL_CONFIG0_FS_SEL_MASK, conf->accel.fs);
433 val |= FIELD_PREP(INV_ICM42607_ACCEL_CONFIG0_ODR_MASK, conf->accel.odr);
434 ret = regmap_write(st->map, INV_ICM42607_REG_ACCEL_CONFIG0, val);
435 if (ret)
436 return ret;
437
438 val = FIELD_PREP(INV_ICM42607_GYRO_CONFIG1_FILTER_MASK, conf->gyro.filter);
439 ret = regmap_update_bits(st->map, INV_ICM42607_REG_GYRO_CONFIG1,
440 INV_ICM42607_GYRO_CONFIG1_FILTER_MASK, val);
441 if (ret)
442 return ret;
443
444 val = FIELD_PREP(INV_ICM42607_ACCEL_CONFIG1_FILTER_MASK, conf->accel.filter);
445 ret = regmap_update_bits(st->map, INV_ICM42607_REG_ACCEL_CONFIG1,
446 INV_ICM42607_ACCEL_CONFIG1_FILTER_MASK, val);
447 if (ret)
448 return ret;
449
450 val = FIELD_PREP(INV_ICM42607_TEMP_CONFIG0_FILTER_MASK,
451 INV_ICM42607_TEMP_FILTER_BW_34HZ);
452 ret = regmap_update_bits(st->map, INV_ICM42607_REG_TEMP_CONFIG0,
453 INV_ICM42607_TEMP_CONFIG0_FILTER_MASK, val);
454 if (ret)
455 return ret;
456
457 st->conf = *conf;
458
459 return 0;
460 }
461
inv_icm42607_setup(struct inv_icm42607_state * st,inv_icm42607_bus_setup inv_icm42607_bus_setup)462 static int inv_icm42607_setup(struct inv_icm42607_state *st,
463 inv_icm42607_bus_setup inv_icm42607_bus_setup)
464 {
465 const struct device *dev = regmap_get_device(st->map);
466 unsigned int val;
467 int ret;
468
469 ret = regmap_read(st->map, INV_ICM42607_REG_WHOAMI, &val);
470 if (ret)
471 return ret;
472
473 /* Warn, but don't fail. */
474 if (val != st->hw->whoami)
475 dev_warn(dev, "Unknown whoami %#02x expected %#02x (%s)\n",
476 val, st->hw->whoami, st->hw->name);
477
478 ret = regmap_write(st->map, INV_ICM42607_REG_SIGNAL_PATH_RESET,
479 INV_ICM42607_SIGNAL_PATH_RESET_SOFT_RESET);
480 if (ret)
481 return ret;
482
483 fsleep(1 * USEC_PER_MSEC);
484
485 /*
486 * No polling interval specified in datasheet, so use reset time as
487 * polling interval and 10x reset time as timeout period.
488 */
489 ret = regmap_read_poll_timeout(st->map, INV_ICM42607_REG_INT_STATUS,
490 val, val & INV_ICM42607_INT_STATUS_RESET_DONE,
491 1 * USEC_PER_MSEC, 10 * USEC_PER_MSEC);
492 if (ret)
493 return dev_err_probe(dev, ret,
494 "reset error, reset done bit not set\n");
495
496 /* Sync the regcache again after a reset. */
497 regcache_mark_dirty(st->map);
498 ret = regcache_sync(st->map);
499 if (ret)
500 return ret;
501
502 ret = inv_icm42607_bus_setup(st);
503 if (ret)
504 return ret;
505
506 ret = regmap_set_bits(st->map, INV_ICM42607_REG_INTF_CONFIG0,
507 INV_ICM42607_INTF_CONFIG0_SENSOR_DATA_ENDIAN);
508 if (ret)
509 return ret;
510
511 val = FIELD_PREP(INV_ICM42607_INTF_CONFIG1_CLKSEL_MASK,
512 INV_ICM42607_INTF_CONFIG1_CLKSEL_PLL);
513 ret = regmap_update_bits(st->map, INV_ICM42607_REG_INTF_CONFIG1,
514 INV_ICM42607_INTF_CONFIG1_CLKSEL_MASK,
515 val);
516 if (ret)
517 return ret;
518
519 return inv_icm42607_set_init_conf(st, st->hw->conf);
520 }
521
inv_icm42607_enable_vddio_reg(struct inv_icm42607_state * st)522 static int inv_icm42607_enable_vddio_reg(struct inv_icm42607_state *st)
523 {
524 int ret;
525
526 if (st->vddio_en)
527 return 0;
528
529 ret = regulator_enable(st->vddio_supply);
530 if (ret)
531 return ret;
532
533 fsleep(INV_ICM42607_POWER_UP_TIME_US);
534
535 st->vddio_en = true;
536
537 return 0;
538 }
539
inv_icm42607_sensors_off(struct inv_icm42607_state * st)540 static int inv_icm42607_sensors_off(struct inv_icm42607_state *st)
541 {
542 const struct device *dev = regmap_get_device(st->map);
543 int ret;
544
545 guard(mutex)(&st->lock);
546
547 st->conf.gyro.mode = INV_ICM42607_SENSOR_MODE_OFF;
548 st->conf.accel.mode = INV_ICM42607_SENSOR_MODE_OFF;
549
550 ret = inv_icm42607_set_pwr_mgmt0(st, st->conf.gyro.mode,
551 st->conf.accel.mode);
552 if (ret)
553 dev_err(dev, "Unable to turn off sensors\n");
554
555 return ret;
556 }
557
inv_icm42607_sensors_off_action(void * data)558 static void inv_icm42607_sensors_off_action(void *data)
559 {
560 inv_icm42607_sensors_off(data);
561 }
562
inv_icm42607_disable_vddio_reg(void * _data)563 static void inv_icm42607_disable_vddio_reg(void *_data)
564 {
565 struct inv_icm42607_state *st = _data;
566
567 if (!st->vddio_en)
568 return;
569
570 regulator_disable(st->vddio_supply);
571
572 st->vddio_en = false;
573 }
574
inv_icm42607_core_probe(struct regmap * regmap,const struct inv_icm42607_hw * hw,inv_icm42607_bus_setup inv_icm42607_bus_setup)575 int inv_icm42607_core_probe(struct regmap *regmap,
576 const struct inv_icm42607_hw *hw,
577 inv_icm42607_bus_setup inv_icm42607_bus_setup)
578 {
579 struct device *dev = regmap_get_device(regmap);
580 struct inv_icm42607_state *st;
581 int ret;
582
583 st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
584 if (!st)
585 return -ENOMEM;
586
587 dev_set_drvdata(dev, st);
588
589 ret = devm_mutex_init(dev, &st->lock);
590 if (ret)
591 return ret;
592
593 st->hw = hw;
594 st->map = regmap;
595
596 ret = iio_read_mount_matrix(dev, &st->orientation);
597 if (ret)
598 return dev_err_probe(dev, ret,
599 "failed to retrieve mounting matrix\n");
600
601 ret = devm_regulator_get_enable(dev, "vdd");
602 if (ret)
603 return dev_err_probe(dev, ret,
604 "Failed to get vdd regulator\n");
605
606 st->vddio_supply = devm_regulator_get(dev, "vddio");
607 if (IS_ERR(st->vddio_supply))
608 return dev_err_probe(dev, PTR_ERR(st->vddio_supply),
609 "Failed to get vddio regulator\n");
610
611 ret = inv_icm42607_enable_vddio_reg(st);
612 if (ret)
613 return ret;
614
615 ret = devm_add_action_or_reset(dev, inv_icm42607_disable_vddio_reg, st);
616 if (ret)
617 return ret;
618
619 /* Setup chip registers (includes WHOAMI check, reset check, bus setup) */
620 ret = inv_icm42607_setup(st, inv_icm42607_bus_setup);
621 if (ret)
622 return ret;
623
624 /*
625 * Ensure if sensors get turned on at some point, they're turned off
626 * as part of teardown.
627 */
628 ret = devm_add_action_or_reset(dev, inv_icm42607_sensors_off_action, st);
629 if (ret)
630 return ret;
631
632 ret = devm_pm_runtime_set_active_enabled(dev);
633 if (ret)
634 return ret;
635
636 pm_runtime_set_autosuspend_delay(dev, INV_ICM42607_SUSPEND_DELAY_MS);
637 pm_runtime_use_autosuspend(dev);
638
639 /* Initialize IIO device for Accel */
640 st->indio_accel = inv_icm42607_accel_init(st);
641 if (IS_ERR(st->indio_accel))
642 return PTR_ERR(st->indio_accel);
643
644 /* Initialize IIO device for Gyro */
645 st->indio_gyro = inv_icm42607_gyro_init(st);
646 if (IS_ERR(st->indio_gyro))
647 return PTR_ERR(st->indio_gyro);
648
649 return 0;
650 }
651 EXPORT_SYMBOL_NS_GPL(inv_icm42607_core_probe, "IIO_ICM42607");
652
inv_icm42607_suspend(struct device * dev)653 static int inv_icm42607_suspend(struct device *dev)
654 {
655 struct inv_icm42607_state *st = dev_get_drvdata(dev);
656 int ret;
657
658 ret = pm_runtime_force_suspend(dev);
659 if (ret)
660 return ret;
661
662 inv_icm42607_disable_vddio_reg(st);
663
664 return 0;
665 }
666
inv_icm42607_resume_core(struct inv_icm42607_state * st)667 static int inv_icm42607_resume_core(struct inv_icm42607_state *st)
668 {
669 int ret;
670
671 ret = inv_icm42607_enable_vddio_reg(st);
672 if (ret)
673 return ret;
674
675 /* Sync the regcache again after regulator shutdown. */
676 regcache_mark_dirty(st->map);
677
678 return regcache_sync(st->map);
679 }
680
inv_icm42607_resume(struct device * dev)681 static int inv_icm42607_resume(struct device *dev)
682 {
683 struct inv_icm42607_state *st = dev_get_drvdata(dev);
684 int ret;
685
686 ret = inv_icm42607_resume_core(st);
687 if (ret) {
688 int rc;
689
690 rc = pm_runtime_force_resume(dev);
691 if (rc)
692 dev_warn(dev, "Failed to restore runtime PM state: %d\n", rc);
693
694 return ret;
695 }
696
697 return pm_runtime_force_resume(dev);
698 }
699
inv_icm42607_runtime_suspend(struct device * dev)700 static int inv_icm42607_runtime_suspend(struct device *dev)
701 {
702 struct inv_icm42607_state *st = dev_get_drvdata(dev);
703
704 /*
705 * Set sensors state to off. Since we only support one-shot
706 * today we can use runtime PM to turn sensors off when not
707 * in use, and then when needed the reads/writes will
708 * re-enable the sensors as needed. This reduces complexity,
709 * however the tradeoff is that an unused sensor won't be
710 * turned off until the entire chip is no longer in use.
711 */
712 return inv_icm42607_sensors_off(st);
713 }
714
715 EXPORT_NS_GPL_DEV_PM_OPS(inv_icm42607_pm_ops, IIO_ICM42607) = {
716 SYSTEM_SLEEP_PM_OPS(inv_icm42607_suspend, inv_icm42607_resume)
717 RUNTIME_PM_OPS(inv_icm42607_runtime_suspend, NULL, NULL)
718 };
719
720 MODULE_AUTHOR("InvenSense, Inc.");
721 MODULE_DESCRIPTION("InvenSense ICM-42607 device driver");
722 MODULE_LICENSE("GPL");
723