forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-01-04 1543e317f1da31b75942316931e8f491a8920811 
 kernel/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
....@@ -1,14 +1,6 @@
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 
146 #include <linux/module.h>
....@@ -21,7 +13,6 @@
2113 #include <linux/interrupt.h>
2214 #include <linux/poll.h>
2315 #include <linux/math64.h>
24
-#include <asm/unaligned.h>
2516 #include "inv_mpu_iio.h"
2617 
2718 /**
....@@ -98,58 +89,14 @@
9889 	return ts;
9990 }
10091 
101
-int inv_reset_fifo(struct iio_dev *indio_dev)
92
+static int inv_reset_fifo(struct iio_dev *indio_dev)
10293 {
10394 	int result;
104
-	u8 d;
10595 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
10696 
107
-	/* reset it timestamp validation */
108
-	st->it_timestamp = 0;
109
-
110
-	/* disable interrupt */
111
-	result = regmap_write(st->map, st->reg->int_enable, 0);
112
-	if (result) {
113
-		dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
114
-			result);
115
-		return result;
116
-	}
117
-	/* disable the sensor output to FIFO */
118
-	result = regmap_write(st->map, st->reg->fifo_en, 0);
119
-	if (result)
120
-		goto reset_fifo_fail;
121
-	/* disable fifo reading */
122
-	result = regmap_write(st->map, st->reg->user_ctrl,
123
-			      st->chip_config.user_ctrl);
124
-	if (result)
125
-		goto reset_fifo_fail;
126
-
127
-	/* reset FIFO*/
128
-	d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
129
-	result = regmap_write(st->map, st->reg->user_ctrl, d);
130
-	if (result)
131
-		goto reset_fifo_fail;
132
-
133
-	/* enable interrupt */
134
-	if (st->chip_config.accl_fifo_enable ||
135
-	    st->chip_config.gyro_fifo_enable) {
136
-		result = regmap_write(st->map, st->reg->int_enable,
137
-				      INV_MPU6050_BIT_DATA_RDY_EN);
138
-		if (result)
139
-			return result;
140
-	}
141
-	/* enable FIFO reading */
142
-	d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
143
-	result = regmap_write(st->map, st->reg->user_ctrl, d);
144
-	if (result)
145
-		goto reset_fifo_fail;
146
-	/* enable sensor output to FIFO */
147
-	d = 0;
148
-	if (st->chip_config.gyro_fifo_enable)
149
-		d |= INV_MPU6050_BITS_GYRO_OUT;
150
-	if (st->chip_config.accl_fifo_enable)
151
-		d |= INV_MPU6050_BIT_ACCEL_OUT;
152
-	result = regmap_write(st->map, st->reg->fifo_en, d);
97
+	/* disable fifo and reenable it */
98
+	inv_mpu6050_prepare_fifo(st, false);
99
+	result = inv_mpu6050_prepare_fifo(st, true);
153100 	if (result)
154101 		goto reset_fifo_fail;
155102 
....@@ -163,7 +110,7 @@
163110 	return result;
164111 }
165112 
166
-/**
113
+/*
167114  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
168115  */
169116 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
....@@ -173,7 +120,6 @@
173120 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
174121 	size_t bytes_per_datum;
175122 	int result;
176
-	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
177123 	u16 fifo_count;
178124 	s64 timestamp;
179125 	int int_status;
....@@ -188,14 +134,12 @@
188134 			"failed to ack interrupt\n");
189135 		goto flush_fifo;
190136 	}
191
-	if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) {
192
-		dev_warn(regmap_get_device(st->map),
193
-			"spurious interrupt with status 0x%x\n", int_status);
137
+	if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
194138 		goto end_session;
195
-	}
196139 
197140 	if (!(st->chip_config.accl_fifo_enable |
198
-		st->chip_config.gyro_fifo_enable))
141
+		st->chip_config.gyro_fifo_enable |
142
+		st->chip_config.magn_fifo_enable))
199143 		goto end_session;
200144 	bytes_per_datum = 0;
201145 	if (st->chip_config.accl_fifo_enable)
....@@ -204,18 +148,21 @@
204148 	if (st->chip_config.gyro_fifo_enable)
205149 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
206150 
207
-	if (st->chip_type == INV_ICM20602)
208
-		bytes_per_datum += INV_ICM20602_BYTES_PER_TEMP_SENSOR;
151
+	if (st->chip_config.temp_fifo_enable)
152
+		bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
153
+
154
+	if (st->chip_config.magn_fifo_enable)
155
+		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
209156 
210157 	/*
211158 	 * read fifo_count register to know how many bytes are inside the FIFO
212159 	 * right now
213160 	 */
214
-	result = regmap_bulk_read(st->map, st->reg->fifo_count_h, data,
215
-				  INV_MPU6050_FIFO_COUNT_BYTE);
161
+	result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
162
+				  st->data, INV_MPU6050_FIFO_COUNT_BYTE);
216163 	if (result)
217164 		goto end_session;
218
-	fifo_count = get_unaligned_be16(&data[0]);
165
+	fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
219166 
220167 	/*
221168 	 * Handle fifo overflow by resetting fifo.
....@@ -232,8 +179,8 @@
232179 	nb = fifo_count / bytes_per_datum;
233180 	inv_mpu6050_update_period(st, pf->timestamp, nb);
234181 	for (i = 0; i < nb; ++i) {
235
-		result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
236
-					  data, bytes_per_datum);
182
+		result = regmap_noinc_read(st->map, st->reg->fifo_r_w,
183
+					   st->data, bytes_per_datum);
237184 		if (result)
238185 			goto flush_fifo;
239186 		/* skip first samples if needed */
....@@ -242,7 +189,7 @@
242189 			continue;
243190 		}
244191 		timestamp = inv_mpu6050_get_timestamp(st);
245
-		iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
192
+		iio_push_to_buffers_with_timestamp(indio_dev, st->data, timestamp);
246193 	}
247194 
248195 end_session: