forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2023-12-09 b22da3d8526a935aa31e086e63f60ff3246cb61c 
 kernel/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
....@@ -1,21 +1,15 @@
1
+// SPDX-License-Identifier: GPL-2.0-only
12 /*
23 * Copyright (C) 2012 Invensense, Inc.
3
-*
4
-* This software is licensed under the terms of the GNU General Public
5
-* License version 2, as published by the Free Software Foundation, and
6
-* may be copied, distributed, and modified under those terms.
7
-*
8
-* This program is distributed in the hope that it will be useful,
9
-* but WITHOUT ANY WARRANTY; without even the implied warranty of
10
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11
-* GNU General Public License for more details.
124 */
135 
6
+#include <linux/pm_runtime.h>
147 #include "inv_mpu_iio.h"
158 
16
-static void inv_scan_query(struct iio_dev *indio_dev)
9
+static unsigned int inv_scan_query_mpu6050(struct iio_dev *indio_dev)
1710 {
1811 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
12
+	unsigned int mask;
1913 
2014 	st->chip_config.gyro_fifo_enable =
2115 		test_bit(INV_MPU6050_SCAN_GYRO_X,
....@@ -32,6 +26,127 @@
3226 			 indio_dev->active_scan_mask) ||
3327 		test_bit(INV_MPU6050_SCAN_ACCL_Z,
3428 			 indio_dev->active_scan_mask);
29
+
30
+	st->chip_config.temp_fifo_enable =
31
+		test_bit(INV_MPU6050_SCAN_TEMP, indio_dev->active_scan_mask);
32
+
33
+	mask = 0;
34
+	if (st->chip_config.gyro_fifo_enable)
35
+		mask |= INV_MPU6050_SENSOR_GYRO;
36
+	if (st->chip_config.accl_fifo_enable)
37
+		mask |= INV_MPU6050_SENSOR_ACCL;
38
+	if (st->chip_config.temp_fifo_enable)
39
+		mask |= INV_MPU6050_SENSOR_TEMP;
40
+
41
+	return mask;
42
+}
43
+
44
+static unsigned int inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
45
+{
46
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
47
+	unsigned int mask;
48
+
49
+	mask = inv_scan_query_mpu6050(indio_dev);
50
+
51
+	/* no magnetometer if i2c auxiliary bus is used */
52
+	if (st->magn_disabled)
53
+		return mask;
54
+
55
+	st->chip_config.magn_fifo_enable =
56
+		test_bit(INV_MPU9X50_SCAN_MAGN_X,
57
+			 indio_dev->active_scan_mask) ||
58
+		test_bit(INV_MPU9X50_SCAN_MAGN_Y,
59
+			 indio_dev->active_scan_mask) ||
60
+		test_bit(INV_MPU9X50_SCAN_MAGN_Z,
61
+			 indio_dev->active_scan_mask);
62
+	if (st->chip_config.magn_fifo_enable)
63
+		mask |= INV_MPU6050_SENSOR_MAGN;
64
+
65
+	return mask;
66
+}
67
+
68
+static unsigned int inv_scan_query(struct iio_dev *indio_dev)
69
+{
70
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
71
+
72
+	switch (st->chip_type) {
73
+	case INV_MPU9150:
74
+	case INV_MPU9250:
75
+	case INV_MPU9255:
76
+		return inv_scan_query_mpu9x50(indio_dev);
77
+	default:
78
+		return inv_scan_query_mpu6050(indio_dev);
79
+	}
80
+}
81
+
82
+static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
83
+{
84
+	unsigned int gyro_skip = 0;
85
+	unsigned int magn_skip = 0;
86
+	unsigned int skip_samples;
87
+
88
+	/* gyro first sample is out of specs, skip it */
89
+	if (st->chip_config.gyro_fifo_enable)
90
+		gyro_skip = 1;
91
+
92
+	/* mag first sample is always not ready, skip it */
93
+	if (st->chip_config.magn_fifo_enable)
94
+		magn_skip = 1;
95
+
96
+	/* compute first samples to skip */
97
+	skip_samples = gyro_skip;
98
+	if (magn_skip > skip_samples)
99
+		skip_samples = magn_skip;
100
+
101
+	return skip_samples;
102
+}
103
+
104
+int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable)
105
+{
106
+	uint8_t d;
107
+	int ret;
108
+
109
+	if (enable) {
110
+		st->it_timestamp = 0;
111
+		/* reset FIFO */
112
+		d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
113
+		ret = regmap_write(st->map, st->reg->user_ctrl, d);
114
+		if (ret)
115
+			return ret;
116
+		/* enable sensor output to FIFO */
117
+		d = 0;
118
+		if (st->chip_config.gyro_fifo_enable)
119
+			d |= INV_MPU6050_BITS_GYRO_OUT;
120
+		if (st->chip_config.accl_fifo_enable)
121
+			d |= INV_MPU6050_BIT_ACCEL_OUT;
122
+		if (st->chip_config.temp_fifo_enable)
123
+			d |= INV_MPU6050_BIT_TEMP_OUT;
124
+		if (st->chip_config.magn_fifo_enable)
125
+			d |= INV_MPU6050_BIT_SLAVE_0;
126
+		ret = regmap_write(st->map, st->reg->fifo_en, d);
127
+		if (ret)
128
+			return ret;
129
+		/* enable FIFO reading */
130
+		d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
131
+		ret = regmap_write(st->map, st->reg->user_ctrl, d);
132
+		if (ret)
133
+			return ret;
134
+		/* enable interrupt */
135
+		ret = regmap_write(st->map, st->reg->int_enable,
136
+				   INV_MPU6050_BIT_DATA_RDY_EN);
137
+	} else {
138
+		ret = regmap_write(st->map, st->reg->int_enable, 0);
139
+		if (ret)
140
+			return ret;
141
+		ret = regmap_write(st->map, st->reg->fifo_en, 0);
142
+		if (ret)
143
+			return ret;
144
+		/* restore user_ctrl for disabling FIFO reading */
145
+		ret = regmap_write(st->map, st->reg->user_ctrl,
146
+				   st->chip_config.user_ctrl);
147
+	}
148
+
149
+	return ret;
35150 }
36151 
37152 /**
....@@ -42,72 +157,43 @@
42157 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
43158 {
44159 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
160
+	struct device *pdev = regmap_get_device(st->map);
161
+	unsigned int scan;
45162 	int result;
46163 
47164 	if (enable) {
48
-		result = inv_mpu6050_set_power_itg(st, true);
49
-		if (result)
165
+		scan = inv_scan_query(indio_dev);
166
+		result = pm_runtime_get_sync(pdev);
167
+		if (result < 0) {
168
+			pm_runtime_put_noidle(pdev);
50169 			return result;
51
-		inv_scan_query(indio_dev);
52
-		st->skip_samples = 0;
53
-		if (st->chip_config.gyro_fifo_enable) {
54
-			result = inv_mpu6050_switch_engine(st, true,
55
-					INV_MPU6050_BIT_PWR_GYRO_STBY);
56
-			if (result)
57
-				goto error_power_off;
58
-			/* gyro first sample is out of specs, skip it */
59
-			st->skip_samples = 1;
60170 		}
61
-		if (st->chip_config.accl_fifo_enable) {
62
-			result = inv_mpu6050_switch_engine(st, true,
63
-					INV_MPU6050_BIT_PWR_ACCL_STBY);
64
-			if (result)
65
-				goto error_gyro_off;
66
-		}
67
-		result = inv_reset_fifo(indio_dev);
68
-		if (result)
69
-			goto error_accl_off;
70
-	} else {
71
-		result = regmap_write(st->map, st->reg->fifo_en, 0);
72
-		if (result)
73
-			goto error_accl_off;
74
-
75
-		result = regmap_write(st->map, st->reg->int_enable, 0);
76
-		if (result)
77
-			goto error_accl_off;
78
-
79
-		result = regmap_write(st->map, st->reg->user_ctrl,
80
-				      st->chip_config.user_ctrl);
81
-		if (result)
82
-			goto error_accl_off;
83
-
84
-		result = inv_mpu6050_switch_engine(st, false,
85
-					INV_MPU6050_BIT_PWR_ACCL_STBY);
86
-		if (result)
87
-			goto error_accl_off;
88
-
89
-		result = inv_mpu6050_switch_engine(st, false,
90
-					INV_MPU6050_BIT_PWR_GYRO_STBY);
91
-		if (result)
92
-			goto error_gyro_off;
93
-
94
-		result = inv_mpu6050_set_power_itg(st, false);
171
+		/*
172
+		 * In case autosuspend didn't trigger, turn off first not
173
+		 * required sensors.
174
+		 */
175
+		result = inv_mpu6050_switch_engine(st, false, ~scan);
95176 		if (result)
96177 			goto error_power_off;
178
+		result = inv_mpu6050_switch_engine(st, true, scan);
179
+		if (result)
180
+			goto error_power_off;
181
+		st->skip_samples = inv_compute_skip_samples(st);
182
+		result = inv_mpu6050_prepare_fifo(st, true);
183
+		if (result)
184
+			goto error_power_off;
185
+	} else {
186
+		result = inv_mpu6050_prepare_fifo(st, false);
187
+		if (result)
188
+			goto error_power_off;
189
+		pm_runtime_mark_last_busy(pdev);
190
+		pm_runtime_put_autosuspend(pdev);
97191 	}
98192 
99193 	return 0;
100194 
101
-error_accl_off:
102
-	if (st->chip_config.accl_fifo_enable)
103
-		inv_mpu6050_switch_engine(st, false,
104
-					  INV_MPU6050_BIT_PWR_ACCL_STBY);
105
-error_gyro_off:
106
-	if (st->chip_config.gyro_fifo_enable)
107
-		inv_mpu6050_switch_engine(st, false,
108
-					  INV_MPU6050_BIT_PWR_GYRO_STBY);
109195 error_power_off:
110
-	inv_mpu6050_set_power_itg(st, false);
196
+	pm_runtime_put_autosuspend(pdev);
111197 	return result;
112198 }
113199