modify sin led gpio

hc

2024-05-13 9d77db3c730780c8ef5ccd4b66403ff5675cfe4e

 kernel/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
..
..
@@ -1,14 +1,6 @@
1
+// SPDX-License-Identifier: GPL-2.0-only
1
2
 /*
2
3
 * 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.
12
4
 */
13
5
 
14
6
 #include <linux/module.h>
..
..
@@ -21,7 +13,6 @@
21
13
 #include <linux/interrupt.h>
22
14
 #include <linux/poll.h>
23
15
 #include <linux/math64.h>
24
-#include <asm/unaligned.h>
25
16
 #include "inv_mpu_iio.h"
26
17
 
27
18
 /**
..
..
@@ -98,58 +89,14 @@
98
89
 	return ts;
99
90
 }
100
91
 
101
-int inv_reset_fifo(struct iio_dev *indio_dev)
92
+static int inv_reset_fifo(struct iio_dev *indio_dev)
102
93
 {
103
94
 	int result;
104
-	u8 d;
105
95
 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
106
96
 
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);
153
100
 	if (result)
154
101
 		goto reset_fifo_fail;
155
102
 
..
..
@@ -163,7 +110,7 @@
163
110
 	return result;
164
111
 }
165
112
 
166
-/**
113
+/*
167
114
  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
168
115
  */
169
116
 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
..
..
@@ -173,7 +120,6 @@
173
120
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
174
121
 	size_t bytes_per_datum;
175
122
 	int result;
176
-	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
177
123
 	u16 fifo_count;
178
124
 	s64 timestamp;
179
125
 	int int_status;
..
..
@@ -188,14 +134,12 @@
188
134
 			"failed to ack interrupt\n");
189
135
 		goto flush_fifo;
190
136
 	}
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))
194
138
 		goto end_session;
195
-	}
196
139
 
197
140
 	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))
199
143
 		goto end_session;
200
144
 	bytes_per_datum = 0;
201
145
 	if (st->chip_config.accl_fifo_enable)
..
..
@@ -204,18 +148,21 @@
204
148
 	if (st->chip_config.gyro_fifo_enable)
205
149
 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
206
150
 
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;
209
156
 
210
157
 	/*
211
158
 	 * read fifo_count register to know how many bytes are inside the FIFO
212
159
 	 * right now
213
160
 	 */
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);
216
163
 	if (result)
217
164
 		goto end_session;
218
-	fifo_count = get_unaligned_be16(&data[0]);
165
+	fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
219
166
 
220
167
 	/*
221
168
 	 * Handle fifo overflow by resetting fifo.
..
..
@@ -232,8 +179,8 @@
232
179
 	nb = fifo_count / bytes_per_datum;
233
180
 	inv_mpu6050_update_period(st, pf->timestamp, nb);
234
181
 	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);
237
184
 		if (result)
238
185
 			goto flush_fifo;
239
186
 		/* skip first samples if needed */
..
..
@@ -242,7 +189,7 @@
242
189
 			continue;
243
190
 		}
244
191
 		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);
246
193
 	}
247
194
 
248
195
 end_session: