aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/misc/inv_mpu/mpu6050
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/misc/inv_mpu/mpu6050')
-rw-r--r--drivers/misc/inv_mpu/mpu6050/Makefile18
-rw-r--r--drivers/misc/inv_mpu/mpu6050/mldl_cfg.c1916
-rw-r--r--drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c138
-rw-r--r--drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c420
-rw-r--r--drivers/misc/inv_mpu/mpu6050/mpu-dev.c1309
5 files changed, 3801 insertions, 0 deletions
diff --git a/drivers/misc/inv_mpu/mpu6050/Makefile b/drivers/misc/inv_mpu/mpu6050/Makefile
new file mode 100644
index 00000000000..a93aa97a699
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/Makefile
@@ -0,0 +1,18 @@
1
2# Kernel makefile for motions sensors
3#
4#
5
6obj-$(CONFIG_MPU_SENSORS_MPU6050B1) += mpu6050b1.o
7
8ccflags-y := -DMPU_CURRENT_BUILD_MPU6050B1
9
10mpu6050b1-objs += mldl_cfg.o
11mpu6050b1-objs += mldl_print_cfg.o
12mpu6050b1-objs += mlsl-kernel.o
13mpu6050b1-objs += mpu-dev.o
14mpu6050b1-objs += ../accel/mpu6050.o
15
16EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
17EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
18EXTRA_CFLAGS += -DINV_CACHE_DMP=1
diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c
new file mode 100644
index 00000000000..22af0c20098
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c
@@ -0,0 +1,1916 @@
1/*
2 $License:
3 Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17 $
18 */
19
20/**
21 * @addtogroup MLDL
22 *
23 * @{
24 * @file mldl_cfg.c
25 * @brief The Motion Library Driver Layer.
26 */
27
28/* -------------------------------------------------------------------------- */
29#include <linux/delay.h>
30#include <linux/slab.h>
31
32#include <stddef.h>
33
34#include "mldl_cfg.h"
35#include <linux/mpu.h>
36#include "mpu6050b1.h"
37
38#include "mlsl.h"
39#include "mldl_print_cfg.h"
40#include "log.h"
41#undef MPL_LOG_TAG
42#define MPL_LOG_TAG "mldl_cfg:"
43
44/* -------------------------------------------------------------------------- */
45
46#define SLEEP 0
47#define WAKE_UP 7
48#define RESET 1
49#define STANDBY 1
50
51/* -------------------------------------------------------------------------- */
52
53/**
54 * @brief Stop the DMP running
55 *
56 * @return INV_SUCCESS or non-zero error code
57 */
58static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
59{
60 unsigned char user_ctrl_reg;
61 int result;
62
63 if (mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)
64 return INV_SUCCESS;
65
66 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
67 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
68 if (result) {
69 LOG_RESULT_LOCATION(result);
70 return result;
71 }
72 user_ctrl_reg = (user_ctrl_reg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
73 user_ctrl_reg = (user_ctrl_reg & (~BIT_DMP_EN)) | BIT_DMP_RST;
74
75 result = inv_serial_single_write(gyro_handle,
76 mldl_cfg->mpu_chip_info->addr,
77 MPUREG_USER_CTRL, user_ctrl_reg);
78 if (result) {
79 LOG_RESULT_LOCATION(result);
80 return result;
81 }
82 mldl_cfg->inv_mpu_state->status |= MPU_DMP_IS_SUSPENDED;
83
84 return result;
85}
86
87/**
88 * @brief Starts the DMP running
89 *
90 * @return INV_SUCCESS or non-zero error code
91 */
92static int dmp_start(struct mldl_cfg *mldl_cfg, void *mlsl_handle)
93{
94 unsigned char user_ctrl_reg;
95 int result;
96
97 if ((!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) &&
98 mldl_cfg->mpu_gyro_cfg->dmp_enable)
99 ||
100 ((mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) &&
101 !mldl_cfg->mpu_gyro_cfg->dmp_enable))
102 return INV_SUCCESS;
103
104 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
105 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
106 if (result) {
107 LOG_RESULT_LOCATION(result);
108 return result;
109 }
110
111 result = inv_serial_single_write(
112 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
113 MPUREG_USER_CTRL,
114 ((user_ctrl_reg & (~BIT_FIFO_EN))
115 | BIT_FIFO_RST));
116 if (result) {
117 LOG_RESULT_LOCATION(result);
118 return result;
119 }
120
121 result = inv_serial_single_write(
122 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
123 MPUREG_USER_CTRL, user_ctrl_reg);
124 if (result) {
125 LOG_RESULT_LOCATION(result);
126 return result;
127 }
128
129 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
130 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
131 if (result) {
132 LOG_RESULT_LOCATION(result);
133 return result;
134 }
135
136 user_ctrl_reg |= BIT_DMP_EN;
137
138 if (mldl_cfg->mpu_gyro_cfg->fifo_enable)
139 user_ctrl_reg |= BIT_FIFO_EN;
140 else
141 user_ctrl_reg &= ~BIT_FIFO_EN;
142
143 user_ctrl_reg |= BIT_DMP_RST;
144
145 result = inv_serial_single_write(
146 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
147 MPUREG_USER_CTRL, user_ctrl_reg);
148 if (result) {
149 LOG_RESULT_LOCATION(result);
150 return result;
151 }
152 mldl_cfg->inv_mpu_state->status &= ~MPU_DMP_IS_SUSPENDED;
153
154 return result;
155}
156
157/**
158 * @brief enables/disables the I2C bypass to an external device
159 * connected to MPU's secondary I2C bus.
160 * @param enable
161 * Non-zero to enable pass through.
162 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
163 */
164static int mpu6050b1_set_i2c_bypass(struct mldl_cfg *mldl_cfg,
165 void *mlsl_handle, unsigned char enable)
166{
167 unsigned char reg;
168 int result;
169 unsigned char status = mldl_cfg->inv_mpu_state->status;
170 if ((status & MPU_GYRO_IS_BYPASSED && enable) ||
171 (!(status & MPU_GYRO_IS_BYPASSED) && !enable))
172 return INV_SUCCESS;
173
174 /*---- get current 'USER_CTRL' into b ----*/
175 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
176 MPUREG_USER_CTRL, 1, &reg);
177 if (result) {
178 LOG_RESULT_LOCATION(result);
179 return result;
180 }
181
182 if (!enable) {
183 /* setting int_config with the property flag BIT_BYPASS_EN
184 should be done by the setup functions */
185 result = inv_serial_single_write(
186 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
187 MPUREG_INT_PIN_CFG,
188 (mldl_cfg->pdata->int_config & ~(BIT_BYPASS_EN)));
189 if (!(reg & BIT_I2C_MST_EN)) {
190 result =
191 inv_serial_single_write(
192 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
193 MPUREG_USER_CTRL,
194 (reg | BIT_I2C_MST_EN));
195 if (result) {
196 LOG_RESULT_LOCATION(result);
197 return result;
198 }
199 }
200 } else if (enable) {
201 if (reg & BIT_AUX_IF_EN) {
202 result =
203 inv_serial_single_write(
204 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
205 MPUREG_USER_CTRL,
206 (reg & (~BIT_I2C_MST_EN)));
207 if (result) {
208 LOG_RESULT_LOCATION(result);
209 return result;
210 }
211 /*****************************************
212 * To avoid hanging the bus we must sleep until all
213 * slave transactions have been completed.
214 * 24 bytes max slave reads
215 * +1 byte possible extra write
216 * +4 max slave address
217 * ---
218 * 33 Maximum bytes
219 * x9 Approximate bits per byte
220 * ---
221 * 297 bits.
222 * 2.97 ms minimum @ 100kbps
223 * 0.75 ms minimum @ 400kbps.
224 *****************************************/
225 msleep(3);
226 }
227 result = inv_serial_single_write(
228 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
229 MPUREG_INT_PIN_CFG,
230 (mldl_cfg->pdata->int_config | BIT_BYPASS_EN));
231 if (result) {
232 LOG_RESULT_LOCATION(result);
233 return result;
234 }
235 }
236 if (enable)
237 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
238 else
239 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
240
241 return result;
242}
243
244
245
246
247/**
248 * @brief enables/disables the I2C bypass to an external device
249 * connected to MPU's secondary I2C bus.
250 * @param enable
251 * Non-zero to enable pass through.
252 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
253 */
254static int mpu_set_i2c_bypass(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
255 unsigned char enable)
256{
257 return mpu6050b1_set_i2c_bypass(mldl_cfg, mlsl_handle, enable);
258}
259
260
261#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map))
262
263/* NOTE : when not indicated, product revision
264 is considered an 'npp'; non production part */
265
266/* produces an unique identifier for each device based on the
267 combination of product version and product revision */
268struct prod_rev_map_t {
269 unsigned short mpl_product_key;
270 unsigned char silicon_rev;
271 unsigned short gyro_trim;
272 unsigned short accel_trim;
273};
274
275/* NOTE: product entries are in chronological order */
276static struct prod_rev_map_t prod_rev_map[] = {
277 /* prod_ver = 0 */
278 {MPL_PROD_KEY(0, 1), MPU_SILICON_REV_A2, 131, 16384},
279 {MPL_PROD_KEY(0, 2), MPU_SILICON_REV_A2, 131, 16384},
280 {MPL_PROD_KEY(0, 3), MPU_SILICON_REV_A2, 131, 16384},
281 {MPL_PROD_KEY(0, 4), MPU_SILICON_REV_A2, 131, 16384},
282 {MPL_PROD_KEY(0, 5), MPU_SILICON_REV_A2, 131, 16384},
283 {MPL_PROD_KEY(0, 6), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/C2-1) */
284 /* prod_ver = 1, forced to 0 for MPU6050 A2 */
285 {MPL_PROD_KEY(0, 7), MPU_SILICON_REV_A2, 131, 16384},
286 {MPL_PROD_KEY(0, 8), MPU_SILICON_REV_A2, 131, 16384},
287 {MPL_PROD_KEY(0, 9), MPU_SILICON_REV_A2, 131, 16384},
288 {MPL_PROD_KEY(0, 10), MPU_SILICON_REV_A2, 131, 16384},
289 {MPL_PROD_KEY(0, 11), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/D2-1) */
290 {MPL_PROD_KEY(0, 12), MPU_SILICON_REV_A2, 131, 16384},
291 {MPL_PROD_KEY(0, 13), MPU_SILICON_REV_A2, 131, 16384},
292 {MPL_PROD_KEY(0, 14), MPU_SILICON_REV_A2, 131, 16384},
293 {MPL_PROD_KEY(0, 15), MPU_SILICON_REV_A2, 131, 16384},
294 {MPL_PROD_KEY(0, 27), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/D4) */
295 /* prod_ver = 1 */
296 {MPL_PROD_KEY(1, 16), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-1) */
297 {MPL_PROD_KEY(1, 17), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-2) */
298 {MPL_PROD_KEY(1, 18), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-3) */
299 {MPL_PROD_KEY(1, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-4) */
300 {MPL_PROD_KEY(1, 20), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-5) */
301 {MPL_PROD_KEY(1, 28), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D4) */
302 {MPL_PROD_KEY(1, 1), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-1) */
303 {MPL_PROD_KEY(1, 2), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-2) */
304 {MPL_PROD_KEY(1, 3), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-3) */
305 {MPL_PROD_KEY(1, 4), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-4) */
306 {MPL_PROD_KEY(1, 5), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-5) */
307 {MPL_PROD_KEY(1, 6), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-6) */
308 /* prod_ver = 2 */
309 {MPL_PROD_KEY(2, 7), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-1) */
310 {MPL_PROD_KEY(2, 8), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-2) */
311 {MPL_PROD_KEY(2, 9), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-3) */
312 {MPL_PROD_KEY(2, 10), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-4) */
313 {MPL_PROD_KEY(2, 11), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-5) */
314 {MPL_PROD_KEY(2, 12), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-6) */
315 {MPL_PROD_KEY(2, 29), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/D4) */
316 /* prod_ver = 3 */
317 {MPL_PROD_KEY(3, 30), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E2) */
318 /* prod_ver = 4 */
319 {MPL_PROD_KEY(4, 31), MPU_SILICON_REV_B1, 131, 8192}, /* (B2/F1) */
320 {MPL_PROD_KEY(4, 1), MPU_SILICON_REV_B1, 131, 8192}, /* (B3/F1) */
321 {MPL_PROD_KEY(4, 3), MPU_SILICON_REV_B1, 131, 8192}, /* (B4/F1) */
322 /* prod_ver = 5 */
323 {MPL_PROD_KEY(6, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */
324 /* prod_ver = 7 */
325 {MPL_PROD_KEY(7, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */
326 /* prod_ver = 8 */
327 {MPL_PROD_KEY(8, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */
328 /* prod_ver = 9 */
329 {MPL_PROD_KEY(9, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */
330 /* prod_ver = 10 */
331 {MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384} /* (B5/E2) */
332};
333
334/**
335 * @internal
336 * @brief Inverse lookup of the index of an MPL product key .
337 * @param key
338 * the MPL product indentifier also referred to as 'key'.
339 * @return the index position of the key in the array, -1 if not found.
340 */
341short index_of_key(unsigned short key)
342{
343 int i;
344 for (i = 0; i < NUM_OF_PROD_REVS; i++)
345 if (prod_rev_map[i].mpl_product_key == key)
346 return (short)i;
347 return -1;
348}
349
350/**
351 * @internal
352 * @brief Get the product revision and version for MPU6050 and
353 * extract all per-part specific information.
354 * The product version number is read from the PRODUCT_ID register in
355 * user space register map.
356 * The product revision number is in read from OTP bank 0, ADDR6[7:2].
357 * These 2 numbers, combined, provide an unique key to be used to
358 * retrieve some per-device information such as the silicon revision
359 * and the gyro and accel sensitivity trim values.
360 *
361 * @param mldl_cfg
362 * a pointer to the mldl config data structure.
363 * @param mlsl_handle
364 * an file handle to the serial communication device the
365 * device is connected to.
366 *
367 * @return 0 on success, a non-zero error code otherwise.
368 */
369static int inv_get_silicon_rev_mpu6050(
370 struct mldl_cfg *mldl_cfg, void *mlsl_handle)
371{
372 int result;
373 unsigned char prod_ver = 0x00, prod_rev = 0x00;
374 unsigned char bank =
375 (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
376 unsigned short memAddr = ((bank << 8) | 0x06);
377 unsigned short key;
378 short index;
379 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
380
381 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
382 MPUREG_PRODUCT_ID, 1, &prod_ver);
383 if (result) {
384 LOG_RESULT_LOCATION(result);
385 return result;
386 }
387 prod_ver &= 0xF;
388
389 result = inv_serial_read_mem(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
390 memAddr, 1, &prod_rev);
391 if (result) {
392 LOG_RESULT_LOCATION(result);
393 return result;
394 }
395 prod_rev >>= 2;
396
397 /* clean the prefetch and cfg user bank bits */
398 result = inv_serial_single_write(
399 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
400 MPUREG_BANK_SEL, 0);
401 if (result) {
402 LOG_RESULT_LOCATION(result);
403 return result;
404 }
405
406 key = MPL_PROD_KEY(prod_ver, prod_rev);
407 if (key == 0) {
408 MPL_LOGE("Product id read as 0 "
409 "indicates device is either "
410 "incompatible or an MPU3050\n");
411 return INV_ERROR_INVALID_MODULE;
412 }
413 index = index_of_key(key);
414 if (index == -1 || index >= NUM_OF_PROD_REVS) {
415 MPL_LOGE("Unsupported product key %d in MPL\n", key);
416 return INV_ERROR_INVALID_MODULE;
417 }
418 /* check MPL is compiled for this device */
419 if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1) {
420 MPL_LOGE("MPL compiled for MPU6050B1 support "
421 "but device is not MPU6050B1 (%d)\n", key);
422 return INV_ERROR_INVALID_MODULE;
423 }
424
425 mpu_chip_info->product_id = prod_ver;
426 mpu_chip_info->product_revision = prod_rev;
427 mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev;
428 mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim;
429 mpu_chip_info->accel_sens_trim = prod_rev_map[index].accel_trim;
430
431 return result;
432}
433#define inv_get_silicon_rev inv_get_silicon_rev_mpu6050
434
435
436/**
437 * @brief Enable / Disable the use MPU's secondary I2C interface level
438 * shifters.
439 * When enabled the secondary I2C interface to which the external
440 * device is connected runs at VDD voltage (main supply).
441 * When disabled the 2nd interface runs at VDDIO voltage.
442 * See the device specification for more details.
443 *
444 * @note using this API may produce unpredictable results, depending on how
445 * the MPU and slave device are setup on the target platform.
446 * Use of this API should entirely be restricted to system
447 * integrators. Once the correct value is found, there should be no
448 * need to change the level shifter at runtime.
449 *
450 * @pre Must be called after inv_serial_start().
451 * @note Typically called before inv_dmp_open().
452 *
453 * @param[in] enable:
454 * 0 to run at VDDIO (default),
455 * 1 to run at VDD.
456 *
457 * @return INV_SUCCESS if successfull, a non-zero error code otherwise.
458 */
459static int inv_mpu_set_level_shifter_bit(struct mldl_cfg *mldl_cfg,
460 void *mlsl_handle, unsigned char enable)
461{
462 int result;
463 unsigned char regval;
464
465 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
466 MPUREG_YG_OFFS_TC, 1, &regval);
467 if (result) {
468 LOG_RESULT_LOCATION(result);
469 return result;
470 }
471 if (enable)
472 regval |= BIT_I2C_MST_VDDIO;
473
474 result = inv_serial_single_write(
475 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
476 MPUREG_YG_OFFS_TC, regval);
477 if (result) {
478 LOG_RESULT_LOCATION(result);
479 return result;
480 }
481 return INV_SUCCESS;
482}
483
484
485/**
486 * @internal
487 * @brief MPU6050 B1 power management functions.
488 * @param mldl_cfg
489 * a pointer to the internal mldl_cfg data structure.
490 * @param mlsl_handle
491 * a file handle to the serial device used to communicate
492 * with the MPU6050 B1 device.
493 * @param reset
494 * 1 to reset hardware.
495 * @param sensors
496 * Bitfield of sensors to leave on
497 *
498 * @return 0 on success, a non-zero error code on error.
499 */
500static int mpu60xx_pwr_mgmt(struct mldl_cfg *mldl_cfg,
501 void *mlsl_handle,
502 unsigned int reset, unsigned long sensors)
503{
504 unsigned char pwr_mgmt[2];
505 unsigned char pwr_mgmt_prev[2];
506 int result;
507 int sleep = !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
508 | INV_DMP_PROCESSOR));
509
510 if (reset) {
511 MPL_LOGI("Reset MPU6050 B1\n");
512 result = inv_serial_single_write(
513 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
514 MPUREG_PWR_MGMT_1, BIT_H_RESET);
515 if (result) {
516 LOG_RESULT_LOCATION(result);
517 return result;
518 }
519 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
520 msleep(15);
521 }
522
523 /* NOTE : reading both PWR_MGMT_1 and PWR_MGMT_2 for efficiency because
524 they are accessible even when the device is powered off */
525 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
526 MPUREG_PWR_MGMT_1, 2, pwr_mgmt_prev);
527 if (result) {
528 LOG_RESULT_LOCATION(result);
529 return result;
530 }
531
532 pwr_mgmt[0] = pwr_mgmt_prev[0];
533 pwr_mgmt[1] = pwr_mgmt_prev[1];
534
535 if (sleep) {
536 mldl_cfg->inv_mpu_state->status |= MPU_DEVICE_IS_SUSPENDED;
537 pwr_mgmt[0] |= BIT_SLEEP;
538 } else {
539 mldl_cfg->inv_mpu_state->status &= ~MPU_DEVICE_IS_SUSPENDED;
540 pwr_mgmt[0] &= ~BIT_SLEEP;
541 }
542 if (pwr_mgmt[0] != pwr_mgmt_prev[0]) {
543 result = inv_serial_single_write(
544 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
545 MPUREG_PWR_MGMT_1, pwr_mgmt[0]);
546 if (result) {
547 LOG_RESULT_LOCATION(result);
548 return result;
549 }
550 }
551
552 pwr_mgmt[1] &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
553 if (!(sensors & INV_X_GYRO))
554 pwr_mgmt[1] |= BIT_STBY_XG;
555 if (!(sensors & INV_Y_GYRO))
556 pwr_mgmt[1] |= BIT_STBY_YG;
557 if (!(sensors & INV_Z_GYRO))
558 pwr_mgmt[1] |= BIT_STBY_ZG;
559
560 if (pwr_mgmt[1] != pwr_mgmt_prev[1]) {
561 result = inv_serial_single_write(
562 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
563 MPUREG_PWR_MGMT_2, pwr_mgmt[1]);
564 if (result) {
565 LOG_RESULT_LOCATION(result);
566 return result;
567 }
568 }
569
570 if ((pwr_mgmt[1] & (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
571 (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) {
572 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED;
573 } else {
574 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED;
575 }
576
577 return INV_SUCCESS;
578}
579
580
581/**
582 * @brief sets the clock source for the gyros.
583 * @param mldl_cfg
584 * a pointer to the struct mldl_cfg data structure.
585 * @param gyro_handle
586 * an handle to the serial device the gyro is assigned to.
587 * @return ML_SUCCESS if successful, a non-zero error code otherwise.
588 */
589static int mpu_set_clock_source(void *gyro_handle, struct mldl_cfg *mldl_cfg)
590{
591 int result;
592 unsigned char cur_clk_src;
593 unsigned char reg;
594
595 /* clock source selection */
596 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
597 MPUREG_PWR_MGM, 1, &reg);
598 if (result) {
599 LOG_RESULT_LOCATION(result);
600 return result;
601 }
602 cur_clk_src = reg & BITS_CLKSEL;
603 reg &= ~BITS_CLKSEL;
604
605
606 result = inv_serial_single_write(
607 gyro_handle, mldl_cfg->mpu_chip_info->addr,
608 MPUREG_PWR_MGM, mldl_cfg->mpu_gyro_cfg->clk_src | reg);
609 if (result) {
610 LOG_RESULT_LOCATION(result);
611 return result;
612 }
613
614 /* ERRATA:
615 workaroud to switch from any MPU_CLK_SEL_PLLGYROx to
616 MPU_CLK_SEL_INTERNAL and XGyro is powered up:
617 1) Select INT_OSC
618 2) PD XGyro
619 3) PU XGyro
620 */
621 if ((cur_clk_src == MPU_CLK_SEL_PLLGYROX
622 || cur_clk_src == MPU_CLK_SEL_PLLGYROY
623 || cur_clk_src == MPU_CLK_SEL_PLLGYROZ)
624 && mldl_cfg->mpu_gyro_cfg->clk_src == MPU_CLK_SEL_INTERNAL
625 && mldl_cfg->inv_mpu_cfg->requested_sensors & INV_X_GYRO) {
626 unsigned char first_result = INV_SUCCESS;
627 mldl_cfg->inv_mpu_cfg->requested_sensors &= ~INV_X_GYRO;
628 result = mpu60xx_pwr_mgmt(
629 mldl_cfg, gyro_handle,
630 false, mldl_cfg->inv_mpu_cfg->requested_sensors);
631 ERROR_CHECK_FIRST(first_result, result);
632 mldl_cfg->inv_mpu_cfg->requested_sensors |= INV_X_GYRO;
633 result = mpu60xx_pwr_mgmt(
634 mldl_cfg, gyro_handle,
635 false, mldl_cfg->inv_mpu_cfg->requested_sensors);
636 ERROR_CHECK_FIRST(first_result, result);
637 result = first_result;
638 }
639 return result;
640}
641
642/**
643 * Configures the MPU I2C Master
644 *
645 * @mldl_cfg Handle to the configuration data
646 * @gyro_handle handle to the gyro communictation interface
647 * @slave Can be Null if turning off the slave
648 * @slave_pdata Can be null if turning off the slave
649 * @slave_id enum ext_slave_type to determine which index to use
650 *
651 *
652 * This fucntion configures the slaves by:
653 * 1) Setting up the read
654 * a) Read Register
655 * b) Read Length
656 * 2) Set up the data trigger (MPU6050 only)
657 * a) Set trigger write register
658 * b) Set Trigger write value
659 * 3) Set up the divider (MPU6050 only)
660 * 4) Set the slave bypass mode depending on slave
661 *
662 * returns INV_SUCCESS or non-zero error code
663 */
664
665static int mpu_set_slave_mpu60xx(struct mldl_cfg *mldl_cfg,
666 void *gyro_handle,
667 struct ext_slave_descr *slave,
668 struct ext_slave_platform_data *slave_pdata,
669 int slave_id)
670{
671 int result;
672 unsigned char reg;
673 /* Slave values */
674 unsigned char slave_reg;
675 unsigned char slave_len;
676 unsigned char slave_endian;
677 unsigned char slave_address;
678 /* Which MPU6050 registers to use */
679 unsigned char addr_reg;
680 unsigned char reg_reg;
681 unsigned char ctrl_reg;
682 /* Which MPU6050 registers to use for the trigger */
683 unsigned char addr_trig_reg;
684 unsigned char reg_trig_reg;
685 unsigned char ctrl_trig_reg;
686
687 unsigned char bits_slave_delay = 0;
688 /* Divide down rate for the Slave, from the mpu rate */
689 unsigned char d0_trig_reg;
690 unsigned char delay_ctrl_orig;
691 unsigned char delay_ctrl;
692 long divider;
693
694 if (NULL == slave || NULL == slave_pdata) {
695 slave_reg = 0;
696 slave_len = 0;
697 slave_endian = 0;
698 slave_address = 0;
699 } else {
700 slave_reg = slave->read_reg;
701 slave_len = slave->read_len;
702 slave_endian = slave->endian;
703 slave_address = slave_pdata->address;
704 slave_address |= BIT_I2C_READ;
705 }
706
707 switch (slave_id) {
708 case EXT_SLAVE_TYPE_ACCEL:
709 addr_reg = MPUREG_I2C_SLV1_ADDR;
710 reg_reg = MPUREG_I2C_SLV1_REG;
711 ctrl_reg = MPUREG_I2C_SLV1_CTRL;
712 addr_trig_reg = 0;
713 reg_trig_reg = 0;
714 ctrl_trig_reg = 0;
715 bits_slave_delay = BIT_SLV1_DLY_EN;
716 break;
717 case EXT_SLAVE_TYPE_COMPASS:
718 addr_reg = MPUREG_I2C_SLV0_ADDR;
719 reg_reg = MPUREG_I2C_SLV0_REG;
720 ctrl_reg = MPUREG_I2C_SLV0_CTRL;
721 addr_trig_reg = MPUREG_I2C_SLV2_ADDR;
722 reg_trig_reg = MPUREG_I2C_SLV2_REG;
723 ctrl_trig_reg = MPUREG_I2C_SLV2_CTRL;
724 d0_trig_reg = MPUREG_I2C_SLV2_DO;
725 bits_slave_delay = BIT_SLV2_DLY_EN | BIT_SLV0_DLY_EN;
726 break;
727 case EXT_SLAVE_TYPE_PRESSURE:
728 addr_reg = MPUREG_I2C_SLV3_ADDR;
729 reg_reg = MPUREG_I2C_SLV3_REG;
730 ctrl_reg = MPUREG_I2C_SLV3_CTRL;
731 addr_trig_reg = MPUREG_I2C_SLV4_ADDR;
732 reg_trig_reg = MPUREG_I2C_SLV4_REG;
733 ctrl_trig_reg = MPUREG_I2C_SLV4_CTRL;
734 bits_slave_delay = BIT_SLV4_DLY_EN | BIT_SLV3_DLY_EN;
735 break;
736 default:
737 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
738 return INV_ERROR_INVALID_PARAMETER;
739 };
740
741 /* return if this slave has already been set */
742 if ((slave_address &&
743 ((mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay)
744 == bits_slave_delay)) ||
745 (!slave_address &&
746 (mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay) ==
747 0))
748 return 0;
749
750 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
751
752 /* Address */
753 result = inv_serial_single_write(gyro_handle,
754 mldl_cfg->mpu_chip_info->addr,
755 addr_reg, slave_address);
756 if (result) {
757 LOG_RESULT_LOCATION(result);
758 return result;
759 }
760 /* Register */
761 result = inv_serial_single_write(gyro_handle,
762 mldl_cfg->mpu_chip_info->addr,
763 reg_reg, slave_reg);
764 if (result) {
765 LOG_RESULT_LOCATION(result);
766 return result;
767 }
768
769 /* Length, byte swapping, grouping & enable */
770 if (slave_len > BITS_SLV_LENG) {
771 MPL_LOGW("Limiting slave burst read length to "
772 "the allowed maximum (15B, req. %d)\n", slave_len);
773 slave_len = BITS_SLV_LENG;
774 return INV_ERROR_INVALID_CONFIGURATION;
775 }
776 reg = slave_len;
777 if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN) {
778 reg |= BIT_SLV_BYTE_SW;
779 if (slave_reg & 1)
780 reg |= BIT_SLV_GRP;
781 }
782 if (slave_address)
783 reg |= BIT_SLV_ENABLE;
784
785 result = inv_serial_single_write(gyro_handle,
786 mldl_cfg->mpu_chip_info->addr,
787 ctrl_reg, reg);
788 if (result) {
789 LOG_RESULT_LOCATION(result);
790 return result;
791 }
792
793 /* Trigger */
794 if (addr_trig_reg) {
795 /* If slave address is 0 this clears the trigger */
796 result = inv_serial_single_write(gyro_handle,
797 mldl_cfg->mpu_chip_info->addr,
798 addr_trig_reg,
799 slave_address & ~BIT_I2C_READ);
800 if (result) {
801 LOG_RESULT_LOCATION(result);
802 return result;
803 }
804 }
805
806 if (slave && slave->trigger && reg_trig_reg) {
807 result = inv_serial_single_write(gyro_handle,
808 mldl_cfg->mpu_chip_info->addr,
809 reg_trig_reg,
810 slave->trigger->reg);
811 if (result) {
812 LOG_RESULT_LOCATION(result);
813 return result;
814 }
815 result = inv_serial_single_write(gyro_handle,
816 mldl_cfg->mpu_chip_info->addr,
817 ctrl_trig_reg,
818 BIT_SLV_ENABLE | 0x01);
819 if (result) {
820 LOG_RESULT_LOCATION(result);
821 return result;
822 }
823 result = inv_serial_single_write(gyro_handle,
824 mldl_cfg->mpu_chip_info->addr,
825 d0_trig_reg,
826 slave->trigger->value);
827 if (result) {
828 LOG_RESULT_LOCATION(result);
829 return result;
830 }
831 } else if (ctrl_trig_reg) {
832 result = inv_serial_single_write(gyro_handle,
833 mldl_cfg->mpu_chip_info->addr,
834 ctrl_trig_reg, 0x00);
835 if (result) {
836 LOG_RESULT_LOCATION(result);
837 return result;
838 }
839 }
840
841 /* Data rate */
842 if (slave) {
843 struct ext_slave_config config;
844 long data;
845 config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
846 config.len = sizeof(long);
847 config.apply = false;
848 config.data = &data;
849 if (!(slave->get_config))
850 return INV_ERROR_INVALID_CONFIGURATION;
851
852 result = slave->get_config(NULL, slave, slave_pdata, &config);
853 if (result) {
854 LOG_RESULT_LOCATION(result);
855 return result;
856 }
857 MPL_LOGI("Slave %d ODR: %ld Hz\n", slave_id, data / 1000);
858 divider = ((1000 * inv_mpu_get_sampling_rate_hz(
859 mldl_cfg->mpu_gyro_cfg))
860 / data) - 1;
861 } else {
862 divider = 0;
863 }
864
865 result = inv_serial_read(gyro_handle,
866 mldl_cfg->mpu_chip_info->addr,
867 MPUREG_I2C_MST_DELAY_CTRL,
868 1, &delay_ctrl_orig);
869 delay_ctrl = delay_ctrl_orig;
870 if (result) {
871 LOG_RESULT_LOCATION(result);
872 return result;
873 }
874
875 if (divider > 0 && divider <= MASK_I2C_MST_DLY) {
876 result = inv_serial_read(gyro_handle,
877 mldl_cfg->mpu_chip_info->addr,
878 MPUREG_I2C_SLV4_CTRL, 1, &reg);
879 if (result) {
880 LOG_RESULT_LOCATION(result);
881 return result;
882 }
883 if ((reg & MASK_I2C_MST_DLY) &&
884 ((long)(reg & MASK_I2C_MST_DLY) !=
885 (divider & MASK_I2C_MST_DLY))) {
886 MPL_LOGW("Changing slave divider: %ld to %ld\n",
887 (long)(reg & MASK_I2C_MST_DLY),
888 (divider & MASK_I2C_MST_DLY));
889
890 }
891 reg |= (unsigned char)(divider & MASK_I2C_MST_DLY);
892 result = inv_serial_single_write(gyro_handle,
893 mldl_cfg->mpu_chip_info->addr,
894 MPUREG_I2C_SLV4_CTRL,
895 reg);
896 if (result) {
897 LOG_RESULT_LOCATION(result);
898 return result;
899 }
900
901 delay_ctrl |= bits_slave_delay;
902 } else {
903 delay_ctrl &= ~(bits_slave_delay);
904 }
905 if (delay_ctrl != delay_ctrl_orig) {
906 result = inv_serial_single_write(
907 gyro_handle, mldl_cfg->mpu_chip_info->addr,
908 MPUREG_I2C_MST_DELAY_CTRL,
909 delay_ctrl);
910 if (result) {
911 LOG_RESULT_LOCATION(result);
912 return result;
913 }
914 }
915
916 if (slave_address)
917 mldl_cfg->inv_mpu_state->i2c_slaves_enabled |=
918 bits_slave_delay;
919 else
920 mldl_cfg->inv_mpu_state->i2c_slaves_enabled &=
921 ~bits_slave_delay;
922
923 return result;
924}
925
926static int mpu_set_slave(struct mldl_cfg *mldl_cfg,
927 void *gyro_handle,
928 struct ext_slave_descr *slave,
929 struct ext_slave_platform_data *slave_pdata,
930 int slave_id)
931{
932 return mpu_set_slave_mpu60xx(mldl_cfg, gyro_handle, slave,
933 slave_pdata, slave_id);
934}
935/**
936 * Check to see if the gyro was reset by testing a couple of registers known
937 * to change on reset.
938 *
939 * @mldl_cfg mldl configuration structure
940 * @gyro_handle handle used to communicate with the gyro
941 *
942 * @return INV_SUCCESS or non-zero error code
943 */
944static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
945{
946 int result = INV_SUCCESS;
947 unsigned char reg;
948
949 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
950 MPUREG_DMP_CFG_2, 1, &reg);
951 if (result) {
952 LOG_RESULT_LOCATION(result);
953 return result;
954 }
955
956 if (mldl_cfg->mpu_gyro_cfg->dmp_cfg2 != reg)
957 return true;
958
959 if (0 != mldl_cfg->mpu_gyro_cfg->dmp_cfg1)
960 return false;
961
962 /* Inconclusive assume it was reset */
963 return true;
964}
965
966
967int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
968 const unsigned char *data, int size)
969{
970 int bank, offset, write_size;
971 int result;
972 unsigned char read[MPU_MEM_BANK_SIZE];
973
974 if (mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) {
975#if INV_CACHE_DMP == 1
976 memcpy(mldl_cfg->mpu_ram->ram, data, size);
977 return INV_SUCCESS;
978#else
979 LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
980 return INV_ERROR_MEMORY_SET;
981#endif
982 }
983
984 if (!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)) {
985 LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
986 return INV_ERROR_MEMORY_SET;
987 }
988 /* Write and verify memory */
989 for (bank = 0; size > 0; bank++,
990 size -= write_size,
991 data += write_size) {
992 if (size > MPU_MEM_BANK_SIZE)
993 write_size = MPU_MEM_BANK_SIZE;
994 else
995 write_size = size;
996
997 result = inv_serial_write_mem(mlsl_handle,
998 mldl_cfg->mpu_chip_info->addr,
999 ((bank << 8) | 0x00),
1000 write_size,
1001 data);
1002 if (result) {
1003 LOG_RESULT_LOCATION(result);
1004 MPL_LOGE("Write mem error in bank %d\n", bank);
1005 return result;
1006 }
1007 result = inv_serial_read_mem(mlsl_handle,
1008 mldl_cfg->mpu_chip_info->addr,
1009 ((bank << 8) | 0x00),
1010 write_size,
1011 read);
1012 if (result) {
1013 LOG_RESULT_LOCATION(result);
1014 MPL_LOGE("Read mem error in bank %d\n", bank);
1015 return result;
1016 }
1017
1018#define ML_SKIP_CHECK 38
1019 for (offset = 0; offset < write_size; offset++) {
1020 /* skip the register memory locations */
1021 if (bank == 0 && offset < ML_SKIP_CHECK)
1022 continue;
1023 if (data[offset] != read[offset]) {
1024 result = INV_ERROR_SERIAL_WRITE;
1025 break;
1026 }
1027 }
1028 if (result != INV_SUCCESS) {
1029 LOG_RESULT_LOCATION(result);
1030 MPL_LOGE("Read data mismatch at bank %d, offset %d\n",
1031 bank, offset);
1032 return result;
1033 }
1034 }
1035 return INV_SUCCESS;
1036}
1037
1038static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle,
1039 unsigned long sensors)
1040{
1041 int result;
1042 int ii;
1043 unsigned char reg;
1044 unsigned char regs[7];
1045
1046 /* Wake up the part */
1047 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, sensors);
1048 if (result) {
1049 LOG_RESULT_LOCATION(result);
1050 return result;
1051 }
1052
1053 /* Always set the INT_ENABLE and DIVIDER as the Accel Only mode for 6050
1054 can set these too */
1055 result = inv_serial_single_write(
1056 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1057 MPUREG_INT_ENABLE, (mldl_cfg->mpu_gyro_cfg->int_config));
1058 if (result) {
1059 LOG_RESULT_LOCATION(result);
1060 return result;
1061 }
1062 result = inv_serial_single_write(
1063 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1064 MPUREG_SMPLRT_DIV, mldl_cfg->mpu_gyro_cfg->divider);
1065 if (result) {
1066 LOG_RESULT_LOCATION(result);
1067 return result;
1068 }
1069
1070 if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG) &&
1071 !mpu_was_reset(mldl_cfg, gyro_handle)) {
1072 return INV_SUCCESS;
1073 }
1074
1075 /* Configure the MPU */
1076 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1077 if (result) {
1078 LOG_RESULT_LOCATION(result);
1079 return result;
1080 }
1081 result = mpu_set_clock_source(gyro_handle, mldl_cfg);
1082 if (result) {
1083 LOG_RESULT_LOCATION(result);
1084 return result;
1085 }
1086
1087 reg = MPUREG_GYRO_CONFIG_VALUE(0, 0, 0,
1088 mldl_cfg->mpu_gyro_cfg->full_scale);
1089 result = inv_serial_single_write(
1090 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1091 MPUREG_GYRO_CONFIG, reg);
1092 reg = MPUREG_CONFIG_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync,
1093 mldl_cfg->mpu_gyro_cfg->lpf);
1094 result = inv_serial_single_write(
1095 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1096 MPUREG_CONFIG, reg);
1097 if (result) {
1098 LOG_RESULT_LOCATION(result);
1099 return result;
1100 }
1101 result = inv_serial_single_write(
1102 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1103 MPUREG_DMP_CFG_1, mldl_cfg->mpu_gyro_cfg->dmp_cfg1);
1104 if (result) {
1105 LOG_RESULT_LOCATION(result);
1106 return result;
1107 }
1108 result = inv_serial_single_write(
1109 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1110 MPUREG_DMP_CFG_2, mldl_cfg->mpu_gyro_cfg->dmp_cfg2);
1111 if (result) {
1112 LOG_RESULT_LOCATION(result);
1113 return result;
1114 }
1115
1116 /* Write and verify memory */
1117#if INV_CACHE_DMP != 0
1118 inv_mpu_set_firmware(mldl_cfg, gyro_handle,
1119 mldl_cfg->mpu_ram->ram, mldl_cfg->mpu_ram->length);
1120#endif
1121
1122 result = inv_serial_single_write(
1123 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1124 MPUREG_XG_OFFS_TC,
1125 ((mldl_cfg->mpu_offsets->tc[0] << 1) & BITS_XG_OFFS_TC));
1126 if (result) {
1127 LOG_RESULT_LOCATION(result);
1128 return result;
1129 }
1130 regs[0] = ((mldl_cfg->mpu_offsets->tc[1] << 1) & BITS_YG_OFFS_TC);
1131 result = inv_serial_single_write(
1132 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1133 MPUREG_YG_OFFS_TC, regs[0]);
1134 if (result) {
1135 LOG_RESULT_LOCATION(result);
1136 return result;
1137 }
1138 result = inv_serial_single_write(
1139 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1140 MPUREG_ZG_OFFS_TC,
1141 ((mldl_cfg->mpu_offsets->tc[2] << 1) & BITS_ZG_OFFS_TC));
1142 if (result) {
1143 LOG_RESULT_LOCATION(result);
1144 return result;
1145 }
1146 regs[0] = MPUREG_X_OFFS_USRH;
1147 for (ii = 0; ii < ARRAY_SIZE(mldl_cfg->mpu_offsets->gyro); ii++) {
1148 regs[1 + ii * 2] =
1149 (unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] >> 8)
1150 & 0xff;
1151 regs[1 + ii * 2 + 1] =
1152 (unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] & 0xff);
1153 }
1154 result = inv_serial_write(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1155 7, regs);
1156 if (result) {
1157 LOG_RESULT_LOCATION(result);
1158 return result;
1159 }
1160
1161 /* Configure slaves */
1162 result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
1163 mldl_cfg->pdata->level_shifter);
1164 if (result) {
1165 LOG_RESULT_LOCATION(result);
1166 return result;
1167 }
1168 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_CONFIG;
1169
1170 return result;
1171}
1172
1173int gyro_config(void *mlsl_handle,
1174 struct mldl_cfg *mldl_cfg,
1175 struct ext_slave_config *data)
1176{
1177 struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
1178 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
1179 struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
1180 int ii;
1181
1182 if (!data->data)
1183 return INV_ERROR_INVALID_PARAMETER;
1184
1185 switch (data->key) {
1186 case MPU_SLAVE_INT_CONFIG:
1187 mpu_gyro_cfg->int_config = *((__u8 *)data->data);
1188 break;
1189 case MPU_SLAVE_EXT_SYNC:
1190 mpu_gyro_cfg->ext_sync = *((__u8 *)data->data);
1191 break;
1192 case MPU_SLAVE_FULL_SCALE:
1193 mpu_gyro_cfg->full_scale = *((__u8 *)data->data);
1194 break;
1195 case MPU_SLAVE_LPF:
1196 mpu_gyro_cfg->lpf = *((__u8 *)data->data);
1197 break;
1198 case MPU_SLAVE_CLK_SRC:
1199 mpu_gyro_cfg->clk_src = *((__u8 *)data->data);
1200 break;
1201 case MPU_SLAVE_DIVIDER:
1202 mpu_gyro_cfg->divider = *((__u8 *)data->data);
1203 break;
1204 case MPU_SLAVE_DMP_ENABLE:
1205 mpu_gyro_cfg->dmp_enable = *((__u8 *)data->data);
1206 break;
1207 case MPU_SLAVE_FIFO_ENABLE:
1208 mpu_gyro_cfg->fifo_enable = *((__u8 *)data->data);
1209 break;
1210 case MPU_SLAVE_DMP_CFG1:
1211 mpu_gyro_cfg->dmp_cfg1 = *((__u8 *)data->data);
1212 break;
1213 case MPU_SLAVE_DMP_CFG2:
1214 mpu_gyro_cfg->dmp_cfg2 = *((__u8 *)data->data);
1215 break;
1216 case MPU_SLAVE_TC:
1217 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1218 mpu_offsets->tc[ii] = ((__u8 *)data->data)[ii];
1219 break;
1220 case MPU_SLAVE_GYRO:
1221 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1222 mpu_offsets->gyro[ii] = ((__u16 *)data->data)[ii];
1223 break;
1224 case MPU_SLAVE_ADDR:
1225 mpu_chip_info->addr = *((__u8 *)data->data);
1226 break;
1227 case MPU_SLAVE_PRODUCT_REVISION:
1228 mpu_chip_info->product_revision = *((__u8 *)data->data);
1229 break;
1230 case MPU_SLAVE_SILICON_REVISION:
1231 mpu_chip_info->silicon_revision = *((__u8 *)data->data);
1232 break;
1233 case MPU_SLAVE_PRODUCT_ID:
1234 mpu_chip_info->product_id = *((__u8 *)data->data);
1235 break;
1236 case MPU_SLAVE_GYRO_SENS_TRIM:
1237 mpu_chip_info->gyro_sens_trim = *((__u16 *)data->data);
1238 break;
1239 case MPU_SLAVE_ACCEL_SENS_TRIM:
1240 mpu_chip_info->accel_sens_trim = *((__u16 *)data->data);
1241 break;
1242 case MPU_SLAVE_RAM:
1243 if (data->len != mldl_cfg->mpu_ram->length)
1244 return INV_ERROR_INVALID_PARAMETER;
1245
1246 memcpy(mldl_cfg->mpu_ram->ram, data->data, data->len);
1247 break;
1248 default:
1249 LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
1250 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1251 };
1252 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG;
1253 return INV_SUCCESS;
1254}
1255
1256int gyro_get_config(void *mlsl_handle,
1257 struct mldl_cfg *mldl_cfg,
1258 struct ext_slave_config *data)
1259{
1260 struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
1261 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
1262 struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
1263 int ii;
1264
1265 if (!data->data)
1266 return INV_ERROR_INVALID_PARAMETER;
1267
1268 switch (data->key) {
1269 case MPU_SLAVE_INT_CONFIG:
1270 *((__u8 *)data->data) = mpu_gyro_cfg->int_config;
1271 break;
1272 case MPU_SLAVE_EXT_SYNC:
1273 *((__u8 *)data->data) = mpu_gyro_cfg->ext_sync;
1274 break;
1275 case MPU_SLAVE_FULL_SCALE:
1276 *((__u8 *)data->data) = mpu_gyro_cfg->full_scale;
1277 break;
1278 case MPU_SLAVE_LPF:
1279 *((__u8 *)data->data) = mpu_gyro_cfg->lpf;
1280 break;
1281 case MPU_SLAVE_CLK_SRC:
1282 *((__u8 *)data->data) = mpu_gyro_cfg->clk_src;
1283 break;
1284 case MPU_SLAVE_DIVIDER:
1285 *((__u8 *)data->data) = mpu_gyro_cfg->divider;
1286 break;
1287 case MPU_SLAVE_DMP_ENABLE:
1288 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_enable;
1289 break;
1290 case MPU_SLAVE_FIFO_ENABLE:
1291 *((__u8 *)data->data) = mpu_gyro_cfg->fifo_enable;
1292 break;
1293 case MPU_SLAVE_DMP_CFG1:
1294 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg1;
1295 break;
1296 case MPU_SLAVE_DMP_CFG2:
1297 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg2;
1298 break;
1299 case MPU_SLAVE_TC:
1300 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1301 ((__u8 *)data->data)[ii] = mpu_offsets->tc[ii];
1302 break;
1303 case MPU_SLAVE_GYRO:
1304 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1305 ((__u16 *)data->data)[ii] = mpu_offsets->gyro[ii];
1306 break;
1307 case MPU_SLAVE_ADDR:
1308 *((__u8 *)data->data) = mpu_chip_info->addr;
1309 break;
1310 case MPU_SLAVE_PRODUCT_REVISION:
1311 *((__u8 *)data->data) = mpu_chip_info->product_revision;
1312 break;
1313 case MPU_SLAVE_SILICON_REVISION:
1314 *((__u8 *)data->data) = mpu_chip_info->silicon_revision;
1315 break;
1316 case MPU_SLAVE_PRODUCT_ID:
1317 *((__u8 *)data->data) = mpu_chip_info->product_id;
1318 break;
1319 case MPU_SLAVE_GYRO_SENS_TRIM:
1320 *((__u16 *)data->data) = mpu_chip_info->gyro_sens_trim;
1321 break;
1322 case MPU_SLAVE_ACCEL_SENS_TRIM:
1323 *((__u16 *)data->data) = mpu_chip_info->accel_sens_trim;
1324 break;
1325 case MPU_SLAVE_RAM:
1326 if (data->len != mldl_cfg->mpu_ram->length)
1327 return INV_ERROR_INVALID_PARAMETER;
1328
1329 memcpy(data->data, mldl_cfg->mpu_ram->ram, data->len);
1330 break;
1331 default:
1332 LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
1333 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1334 };
1335
1336 return INV_SUCCESS;
1337}
1338
1339
1340/*******************************************************************************
1341 *******************************************************************************
1342 * Exported functions
1343 *******************************************************************************
1344 ******************************************************************************/
1345
1346/**
1347 * Initializes the pdata structure to defaults.
1348 *
1349 * Opens the device to read silicon revision, product id and whoami.
1350 *
1351 * @mldl_cfg
1352 * The internal device configuration data structure.
1353 * @mlsl_handle
1354 * The serial communication handle.
1355 *
1356 * @return INV_SUCCESS if silicon revision, product id and woami are supported
1357 * by this software.
1358 */
1359int inv_mpu_open(struct mldl_cfg *mldl_cfg,
1360 void *gyro_handle,
1361 void *accel_handle,
1362 void *compass_handle, void *pressure_handle)
1363{
1364 int result;
1365 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1366 int ii;
1367
1368 /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
1369 ii = 0;
1370 mldl_cfg->inv_mpu_cfg->ignore_system_suspend = false;
1371 mldl_cfg->mpu_gyro_cfg->int_config = BIT_DMP_INT_EN;
1372 mldl_cfg->mpu_gyro_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
1373 mldl_cfg->mpu_gyro_cfg->lpf = MPU_FILTER_42HZ;
1374 mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_2000DPS;
1375 mldl_cfg->mpu_gyro_cfg->divider = 4;
1376 mldl_cfg->mpu_gyro_cfg->dmp_enable = 1;
1377 mldl_cfg->mpu_gyro_cfg->fifo_enable = 1;
1378 mldl_cfg->mpu_gyro_cfg->ext_sync = 0;
1379 mldl_cfg->mpu_gyro_cfg->dmp_cfg1 = 0;
1380 mldl_cfg->mpu_gyro_cfg->dmp_cfg2 = 0;
1381 mldl_cfg->inv_mpu_state->status =
1382 MPU_DMP_IS_SUSPENDED |
1383 MPU_GYRO_IS_SUSPENDED |
1384 MPU_ACCEL_IS_SUSPENDED |
1385 MPU_COMPASS_IS_SUSPENDED |
1386 MPU_PRESSURE_IS_SUSPENDED |
1387 MPU_DEVICE_IS_SUSPENDED;
1388 mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
1389
1390 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1391 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1392 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1393 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1394
1395 if (mldl_cfg->mpu_chip_info->addr == 0) {
1396 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
1397 return INV_ERROR_INVALID_PARAMETER;
1398 }
1399
1400 /*
1401 * Reset,
1402 * Take the DMP out of sleep, and
1403 * read the product_id, sillicon rev and whoami
1404 */
1405 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
1406 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, true,
1407 INV_THREE_AXIS_GYRO);
1408 if (result) {
1409 LOG_RESULT_LOCATION(result);
1410 return result;
1411 }
1412
1413 result = inv_get_silicon_rev(mldl_cfg, gyro_handle);
1414 if (result) {
1415 LOG_RESULT_LOCATION(result);
1416 return result;
1417 }
1418
1419 /* Get the factory temperature compensation offsets */
1420 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1421 MPUREG_XG_OFFS_TC, 1,
1422 &mldl_cfg->mpu_offsets->tc[0]);
1423 if (result) {
1424 LOG_RESULT_LOCATION(result);
1425 return result;
1426 }
1427 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1428 MPUREG_YG_OFFS_TC, 1,
1429 &mldl_cfg->mpu_offsets->tc[1]);
1430 if (result) {
1431 LOG_RESULT_LOCATION(result);
1432 return result;
1433 }
1434 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1435 MPUREG_ZG_OFFS_TC, 1,
1436 &mldl_cfg->mpu_offsets->tc[2]);
1437 if (result) {
1438 LOG_RESULT_LOCATION(result);
1439 return result;
1440 }
1441
1442 /* Into bypass mode before sleeping and calling the slaves init */
1443 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
1444 if (result) {
1445 LOG_RESULT_LOCATION(result);
1446 return result;
1447 }
1448 result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
1449 mldl_cfg->pdata->level_shifter);
1450 if (result) {
1451 LOG_RESULT_LOCATION(result);
1452 return result;
1453 }
1454
1455 for (ii = 0; ii < GYRO_NUM_AXES; ii++) {
1456 mldl_cfg->mpu_offsets->tc[ii] =
1457 (mldl_cfg->mpu_offsets->tc[ii] & BITS_XG_OFFS_TC) >> 1;
1458 }
1459
1460#if INV_CACHE_DMP != 0
1461 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, 0);
1462#endif
1463 if (result) {
1464 LOG_RESULT_LOCATION(result);
1465 return result;
1466 }
1467
1468
1469 return result;
1470
1471}
1472
1473/**
1474 * Close the mpu interface
1475 *
1476 * @mldl_cfg pointer to the configuration structure
1477 * @mlsl_handle pointer to the serial layer handle
1478 *
1479 * @return INV_SUCCESS or non-zero error code
1480 */
1481int inv_mpu_close(struct mldl_cfg *mldl_cfg,
1482 void *gyro_handle,
1483 void *accel_handle,
1484 void *compass_handle,
1485 void *pressure_handle)
1486{
1487 return 0;
1488}
1489
1490/**
1491 * @brief resume the MPU device and all the other sensor
1492 * devices from their low power state.
1493 *
1494 * @mldl_cfg
1495 * pointer to the configuration structure
1496 * @gyro_handle
1497 * the main file handle to the MPU device.
1498 * @accel_handle
1499 * an handle to the accelerometer device, if sitting
1500 * onto a separate bus. Can match mlsl_handle if
1501 * the accelerometer device operates on the same
1502 * primary bus of MPU.
1503 * @compass_handle
1504 * an handle to the compass device, if sitting
1505 * onto a separate bus. Can match mlsl_handle if
1506 * the compass device operates on the same
1507 * primary bus of MPU.
1508 * @pressure_handle
1509 * an handle to the pressure sensor device, if sitting
1510 * onto a separate bus. Can match mlsl_handle if
1511 * the pressure sensor device operates on the same
1512 * primary bus of MPU.
1513 * @resume_gyro
1514 * whether resuming the gyroscope device is
1515 * actually needed (if the device supports low power
1516 * mode of some sort).
1517 * @resume_accel
1518 * whether resuming the accelerometer device is
1519 * actually needed (if the device supports low power
1520 * mode of some sort).
1521 * @resume_compass
1522 * whether resuming the compass device is
1523 * actually needed (if the device supports low power
1524 * mode of some sort).
1525 * @resume_pressure
1526 * whether resuming the pressure sensor device is
1527 * actually needed (if the device supports low power
1528 * mode of some sort).
1529 * @return INV_SUCCESS or a non-zero error code.
1530 */
1531int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
1532 void *gyro_handle,
1533 void *accel_handle,
1534 void *compass_handle,
1535 void *pressure_handle,
1536 unsigned long sensors)
1537{
1538 int result = INV_SUCCESS;
1539 int ii;
1540 bool resume_slave[EXT_SLAVE_NUM_TYPES];
1541 bool resume_dmp = sensors & INV_DMP_PROCESSOR;
1542 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1543 resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
1544 (sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
1545 resume_slave[EXT_SLAVE_TYPE_ACCEL] =
1546 sensors & INV_THREE_AXIS_ACCEL;
1547 resume_slave[EXT_SLAVE_TYPE_COMPASS] =
1548 sensors & INV_THREE_AXIS_COMPASS;
1549 resume_slave[EXT_SLAVE_TYPE_PRESSURE] =
1550 sensors & INV_THREE_AXIS_PRESSURE;
1551
1552 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1553 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1554 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1555 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1556
1557
1558 mldl_print_cfg(mldl_cfg);
1559
1560 /* Skip the Gyro since slave[EXT_SLAVE_TYPE_GYROSCOPE] is NULL */
1561 for (ii = EXT_SLAVE_TYPE_ACCEL; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1562 if (resume_slave[ii] &&
1563 ((!mldl_cfg->slave[ii]) ||
1564 (!mldl_cfg->slave[ii]->resume))) {
1565 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
1566 return INV_ERROR_INVALID_PARAMETER;
1567 }
1568 }
1569
1570 if ((resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] || resume_dmp)
1571 && ((mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED) ||
1572 (mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG))) {
1573 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1574 if (result) {
1575 LOG_RESULT_LOCATION(result);
1576 return result;
1577 }
1578 result = dmp_stop(mldl_cfg, gyro_handle);
1579 if (result) {
1580 LOG_RESULT_LOCATION(result);
1581 return result;
1582 }
1583 result = gyro_resume(mldl_cfg, gyro_handle, sensors);
1584 if (result) {
1585 LOG_RESULT_LOCATION(result);
1586 return result;
1587 }
1588 }
1589
1590 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1591 if (!mldl_cfg->slave[ii] ||
1592 !mldl_cfg->pdata_slave[ii] ||
1593 !resume_slave[ii] ||
1594 !(mldl_cfg->inv_mpu_state->status & (1 << ii)))
1595 continue;
1596
1597 if (EXT_SLAVE_BUS_SECONDARY ==
1598 mldl_cfg->pdata_slave[ii]->bus) {
1599 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle,
1600 true);
1601 if (result) {
1602 LOG_RESULT_LOCATION(result);
1603 return result;
1604 }
1605 }
1606 result = mldl_cfg->slave[ii]->resume(slave_handle[ii],
1607 mldl_cfg->slave[ii],
1608 mldl_cfg->pdata_slave[ii]);
1609 if (result) {
1610 LOG_RESULT_LOCATION(result);
1611 return result;
1612 }
1613 mldl_cfg->inv_mpu_state->status &= ~(1 << ii);
1614 }
1615
1616 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1617 if (resume_dmp &&
1618 !(mldl_cfg->inv_mpu_state->status & (1 << ii)) &&
1619 mldl_cfg->pdata_slave[ii] &&
1620 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata_slave[ii]->bus) {
1621 result = mpu_set_slave(mldl_cfg,
1622 gyro_handle,
1623 mldl_cfg->slave[ii],
1624 mldl_cfg->pdata_slave[ii],
1625 mldl_cfg->slave[ii]->type);
1626 if (result) {
1627 LOG_RESULT_LOCATION(result);
1628 return result;
1629 }
1630 }
1631 }
1632
1633 /* Turn on the master i2c iterface if necessary */
1634 if (resume_dmp) {
1635 result = mpu_set_i2c_bypass(
1636 mldl_cfg, gyro_handle,
1637 !(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
1638 if (result) {
1639 LOG_RESULT_LOCATION(result);
1640 return result;
1641 }
1642
1643 /* Now start */
1644 result = dmp_start(mldl_cfg, gyro_handle);
1645 if (result) {
1646 LOG_RESULT_LOCATION(result);
1647 return result;
1648 }
1649 }
1650 mldl_cfg->inv_mpu_cfg->requested_sensors = sensors;
1651
1652 return result;
1653}
1654
1655/**
1656 * @brief suspend the MPU device and all the other sensor
1657 * devices into their low power state.
1658 * @mldl_cfg
1659 * a pointer to the struct mldl_cfg internal data
1660 * structure.
1661 * @gyro_handle
1662 * the main file handle to the MPU device.
1663 * @accel_handle
1664 * an handle to the accelerometer device, if sitting
1665 * onto a separate bus. Can match gyro_handle if
1666 * the accelerometer device operates on the same
1667 * primary bus of MPU.
1668 * @compass_handle
1669 * an handle to the compass device, if sitting
1670 * onto a separate bus. Can match gyro_handle if
1671 * the compass device operates on the same
1672 * primary bus of MPU.
1673 * @pressure_handle
1674 * an handle to the pressure sensor device, if sitting
1675 * onto a separate bus. Can match gyro_handle if
1676 * the pressure sensor device operates on the same
1677 * primary bus of MPU.
1678 * @accel
1679 * whether suspending the accelerometer device is
1680 * actually needed (if the device supports low power
1681 * mode of some sort).
1682 * @compass
1683 * whether suspending the compass device is
1684 * actually needed (if the device supports low power
1685 * mode of some sort).
1686 * @pressure
1687 * whether suspending the pressure sensor device is
1688 * actually needed (if the device supports low power
1689 * mode of some sort).
1690 * @return INV_SUCCESS or a non-zero error code.
1691 */
1692int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
1693 void *gyro_handle,
1694 void *accel_handle,
1695 void *compass_handle,
1696 void *pressure_handle,
1697 unsigned long sensors)
1698{
1699 int result = INV_SUCCESS;
1700 int ii;
1701 struct ext_slave_descr **slave = mldl_cfg->slave;
1702 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
1703 bool suspend_dmp = ((sensors & INV_DMP_PROCESSOR) == INV_DMP_PROCESSOR);
1704 bool suspend_slave[EXT_SLAVE_NUM_TYPES];
1705 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1706
1707 suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
1708 ((sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO))
1709 == (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
1710 suspend_slave[EXT_SLAVE_TYPE_ACCEL] =
1711 ((sensors & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL);
1712 suspend_slave[EXT_SLAVE_TYPE_COMPASS] =
1713 ((sensors & INV_THREE_AXIS_COMPASS) == INV_THREE_AXIS_COMPASS);
1714 suspend_slave[EXT_SLAVE_TYPE_PRESSURE] =
1715 ((sensors & INV_THREE_AXIS_PRESSURE) ==
1716 INV_THREE_AXIS_PRESSURE);
1717
1718 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1719 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1720 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1721 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1722
1723 if (suspend_dmp) {
1724 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1725 if (result) {
1726 LOG_RESULT_LOCATION(result);
1727 return result;
1728 }
1729 result = dmp_stop(mldl_cfg, gyro_handle);
1730 if (result) {
1731 LOG_RESULT_LOCATION(result);
1732 return result;
1733 }
1734 }
1735
1736 /* Gyro */
1737 if (suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] &&
1738 !(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) {
1739 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false,
1740 ((~sensors) & INV_ALL_SENSORS));
1741 if (result) {
1742 LOG_RESULT_LOCATION(result);
1743 return result;
1744 }
1745 }
1746
1747 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1748 bool is_suspended = mldl_cfg->inv_mpu_state->status & (1 << ii);
1749 if (!slave[ii] || !pdata_slave[ii] ||
1750 is_suspended || !suspend_slave[ii])
1751 continue;
1752
1753 if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
1754 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1755 if (result) {
1756 LOG_RESULT_LOCATION(result);
1757 return result;
1758 }
1759 }
1760 result = slave[ii]->suspend(slave_handle[ii],
1761 slave[ii],
1762 pdata_slave[ii]);
1763 if (result) {
1764 LOG_RESULT_LOCATION(result);
1765 return result;
1766 }
1767 if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
1768 result = mpu_set_slave(mldl_cfg, gyro_handle,
1769 NULL, NULL,
1770 slave[ii]->type);
1771 if (result) {
1772 LOG_RESULT_LOCATION(result);
1773 return result;
1774 }
1775 }
1776 mldl_cfg->inv_mpu_state->status |= (1 << ii);
1777 }
1778
1779 /* Re-enable the i2c master if there are configured slaves and DMP */
1780 if (!suspend_dmp) {
1781 result = mpu_set_i2c_bypass(
1782 mldl_cfg, gyro_handle,
1783 !(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
1784 if (result) {
1785 LOG_RESULT_LOCATION(result);
1786 return result;
1787 }
1788 }
1789 mldl_cfg->inv_mpu_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
1790
1791 return result;
1792}
1793
1794int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
1795 void *gyro_handle,
1796 void *slave_handle,
1797 struct ext_slave_descr *slave,
1798 struct ext_slave_platform_data *pdata,
1799 unsigned char *data)
1800{
1801 int result;
1802 int bypass_result;
1803 int remain_bypassed = true;
1804
1805 if (NULL == slave || NULL == slave->read) {
1806 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1807 return INV_ERROR_INVALID_CONFIGURATION;
1808 }
1809
1810 if ((EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1811 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1812 remain_bypassed = false;
1813 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1814 if (result) {
1815 LOG_RESULT_LOCATION(result);
1816 return result;
1817 }
1818 }
1819
1820 result = slave->read(slave_handle, slave, pdata, data);
1821
1822 if (!remain_bypassed) {
1823 bypass_result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1824 if (bypass_result) {
1825 LOG_RESULT_LOCATION(bypass_result);
1826 return bypass_result;
1827 }
1828 }
1829 return result;
1830}
1831
1832int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
1833 void *gyro_handle,
1834 void *slave_handle,
1835 struct ext_slave_config *data,
1836 struct ext_slave_descr *slave,
1837 struct ext_slave_platform_data *pdata)
1838{
1839 int result;
1840 int remain_bypassed = true;
1841
1842 if (NULL == slave || NULL == slave->config) {
1843 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1844 return INV_ERROR_INVALID_CONFIGURATION;
1845 }
1846
1847 if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1848 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1849 remain_bypassed = false;
1850 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1851 if (result) {
1852 LOG_RESULT_LOCATION(result);
1853 return result;
1854 }
1855 }
1856
1857 result = slave->config(slave_handle, slave, pdata, data);
1858 if (result) {
1859 LOG_RESULT_LOCATION(result);
1860 return result;
1861 }
1862
1863 if (!remain_bypassed) {
1864 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1865 if (result) {
1866 LOG_RESULT_LOCATION(result);
1867 return result;
1868 }
1869 }
1870 return result;
1871}
1872
1873int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
1874 void *gyro_handle,
1875 void *slave_handle,
1876 struct ext_slave_config *data,
1877 struct ext_slave_descr *slave,
1878 struct ext_slave_platform_data *pdata)
1879{
1880 int result;
1881 int remain_bypassed = true;
1882
1883 if (NULL == slave || NULL == slave->get_config) {
1884 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1885 return INV_ERROR_INVALID_CONFIGURATION;
1886 }
1887
1888 if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1889 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1890 remain_bypassed = false;
1891 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1892 if (result) {
1893 LOG_RESULT_LOCATION(result);
1894 return result;
1895 }
1896 }
1897
1898 result = slave->get_config(slave_handle, slave, pdata, data);
1899 if (result) {
1900 LOG_RESULT_LOCATION(result);
1901 return result;
1902 }
1903
1904 if (!remain_bypassed) {
1905 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1906 if (result) {
1907 LOG_RESULT_LOCATION(result);
1908 return result;
1909 }
1910 }
1911 return result;
1912}
1913
1914/**
1915 * @}
1916 */
diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c
new file mode 100644
index 00000000000..7379a170761
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c
@@ -0,0 +1,138 @@
1/*
2 $License:
3 Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17 $
18 */
19
20/**
21 * @addtogroup MLDL
22 *
23 * @{
24 * @file mldl_print_cfg.c
25 * @brief The Motion Library Driver Layer.
26 */
27
28#include <stddef.h>
29#include "mldl_cfg.h"
30#include "mlsl.h"
31#include "linux/mpu.h"
32
33#include "log.h"
34#undef MPL_LOG_TAG
35#define MPL_LOG_TAG "mldl_print_cfg:"
36
37#undef MPL_LOG_NDEBUG
38#define MPL_LOG_NDEBUG 1
39
40void mldl_print_cfg(struct mldl_cfg *mldl_cfg)
41{
42 struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
43 struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
44 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
45 struct inv_mpu_cfg *inv_mpu_cfg = mldl_cfg->inv_mpu_cfg;
46 struct inv_mpu_state *inv_mpu_state = mldl_cfg->inv_mpu_state;
47 struct ext_slave_descr **slave = mldl_cfg->slave;
48 struct mpu_platform_data *pdata = mldl_cfg->pdata;
49 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
50 int ii;
51
52 /* mpu_gyro_cfg */
53 MPL_LOGV("int_config = %02x\n", mpu_gyro_cfg->int_config);
54 MPL_LOGV("ext_sync = %02x\n", mpu_gyro_cfg->ext_sync);
55 MPL_LOGV("full_scale = %02x\n", mpu_gyro_cfg->full_scale);
56 MPL_LOGV("lpf = %02x\n", mpu_gyro_cfg->lpf);
57 MPL_LOGV("clk_src = %02x\n", mpu_gyro_cfg->clk_src);
58 MPL_LOGV("divider = %02x\n", mpu_gyro_cfg->divider);
59 MPL_LOGV("dmp_enable = %02x\n", mpu_gyro_cfg->dmp_enable);
60 MPL_LOGV("fifo_enable = %02x\n", mpu_gyro_cfg->fifo_enable);
61 MPL_LOGV("dmp_cfg1 = %02x\n", mpu_gyro_cfg->dmp_cfg1);
62 MPL_LOGV("dmp_cfg2 = %02x\n", mpu_gyro_cfg->dmp_cfg2);
63 /* mpu_offsets */
64 MPL_LOGV("tc[0] = %02x\n", mpu_offsets->tc[0]);
65 MPL_LOGV("tc[1] = %02x\n", mpu_offsets->tc[1]);
66 MPL_LOGV("tc[2] = %02x\n", mpu_offsets->tc[2]);
67 MPL_LOGV("gyro[0] = %04x\n", mpu_offsets->gyro[0]);
68 MPL_LOGV("gyro[1] = %04x\n", mpu_offsets->gyro[1]);
69 MPL_LOGV("gyro[2] = %04x\n", mpu_offsets->gyro[2]);
70
71 /* mpu_chip_info */
72 MPL_LOGV("addr = %02x\n", mldl_cfg->mpu_chip_info->addr);
73
74 MPL_LOGV("silicon_revision = %02x\n", mpu_chip_info->silicon_revision);
75 MPL_LOGV("product_revision = %02x\n", mpu_chip_info->product_revision);
76 MPL_LOGV("product_id = %02x\n", mpu_chip_info->product_id);
77 MPL_LOGV("gyro_sens_trim = %02x\n", mpu_chip_info->gyro_sens_trim);
78 MPL_LOGV("accel_sens_trim = %02x\n", mpu_chip_info->accel_sens_trim);
79
80 MPL_LOGV("requested_sensors = %04x\n", inv_mpu_cfg->requested_sensors);
81 MPL_LOGV("ignore_system_suspend= %04x\n",
82 inv_mpu_cfg->ignore_system_suspend);
83 MPL_LOGV("status = %04x\n", inv_mpu_state->status);
84 MPL_LOGV("i2c_slaves_enabled= %04x\n",
85 inv_mpu_state->i2c_slaves_enabled);
86
87 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
88 if (!slave[ii])
89 continue;
90 MPL_LOGV("SLAVE %d:\n", ii);
91 MPL_LOGV(" suspend = %02x\n", (int)slave[ii]->suspend);
92 MPL_LOGV(" resume = %02x\n", (int)slave[ii]->resume);
93 MPL_LOGV(" read = %02x\n", (int)slave[ii]->read);
94 MPL_LOGV(" type = %02x\n", slave[ii]->type);
95 MPL_LOGV(" reg = %02x\n", slave[ii]->read_reg);
96 MPL_LOGV(" len = %02x\n", slave[ii]->read_len);
97 MPL_LOGV(" endian = %02x\n", slave[ii]->endian);
98 MPL_LOGV(" range.mantissa= %02x\n",
99 slave[ii]->range.mantissa);
100 MPL_LOGV(" range.fraction= %02x\n",
101 slave[ii]->range.fraction);
102 }
103
104 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
105 if (!pdata_slave[ii])
106 continue;
107 MPL_LOGV("PDATA_SLAVE[%d]\n", ii);
108 MPL_LOGV(" irq = %02x\n", pdata_slave[ii]->irq);
109 MPL_LOGV(" adapt_num = %02x\n", pdata_slave[ii]->adapt_num);
110 MPL_LOGV(" bus = %02x\n", pdata_slave[ii]->bus);
111 MPL_LOGV(" address = %02x\n", pdata_slave[ii]->address);
112 MPL_LOGV(" orientation=\n"
113 " %2d %2d %2d\n"
114 " %2d %2d %2d\n"
115 " %2d %2d %2d\n",
116 pdata_slave[ii]->orientation[0],
117 pdata_slave[ii]->orientation[1],
118 pdata_slave[ii]->orientation[2],
119 pdata_slave[ii]->orientation[3],
120 pdata_slave[ii]->orientation[4],
121 pdata_slave[ii]->orientation[5],
122 pdata_slave[ii]->orientation[6],
123 pdata_slave[ii]->orientation[7],
124 pdata_slave[ii]->orientation[8]);
125 }
126
127 MPL_LOGV("pdata->int_config = %02x\n", pdata->int_config);
128 MPL_LOGV("pdata->level_shifter = %02x\n", pdata->level_shifter);
129 MPL_LOGV("pdata->orientation =\n"
130 " %2d %2d %2d\n"
131 " %2d %2d %2d\n"
132 " %2d %2d %2d\n",
133 pdata->orientation[0], pdata->orientation[1],
134 pdata->orientation[2], pdata->orientation[3],
135 pdata->orientation[4], pdata->orientation[5],
136 pdata->orientation[6], pdata->orientation[7],
137 pdata->orientation[8]);
138}
diff --git a/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c b/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c
new file mode 100644
index 00000000000..f1c228f48fe
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c
@@ -0,0 +1,420 @@
1/*
2 $License:
3 Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17 $
18 */
19
20#include "mlsl.h"
21#include <linux/i2c.h>
22#include "log.h"
23#include "mpu6050b1.h"
24
25static int inv_i2c_write(struct i2c_adapter *i2c_adap,
26 unsigned char address,
27 unsigned int len, unsigned char const *data)
28{
29 struct i2c_msg msgs[1];
30 int res;
31
32 if (!data || !i2c_adap) {
33 LOG_RESULT_LOCATION(-EINVAL);
34 return -EINVAL;
35 }
36
37 msgs[0].addr = address;
38 msgs[0].flags = 0; /* write */
39 msgs[0].buf = (unsigned char *)data;
40 msgs[0].len = len;
41
42 res = i2c_transfer(i2c_adap, msgs, 1);
43 if (res < 1) {
44 if (res == 0)
45 res = -EIO;
46 LOG_RESULT_LOCATION(res);
47 return res;
48 } else
49 return 0;
50}
51
52static int inv_i2c_write_register(struct i2c_adapter *i2c_adap,
53 unsigned char address,
54 unsigned char reg, unsigned char value)
55{
56 unsigned char data[2];
57
58 data[0] = reg;
59 data[1] = value;
60 return inv_i2c_write(i2c_adap, address, 2, data);
61}
62
63static int inv_i2c_read(struct i2c_adapter *i2c_adap,
64 unsigned char address, unsigned char reg,
65 unsigned int len, unsigned char *data)
66{
67 struct i2c_msg msgs[2];
68 int res;
69
70 if (!data || !i2c_adap) {
71 LOG_RESULT_LOCATION(-EINVAL);
72 return -EINVAL;
73 }
74
75 msgs[0].addr = address;
76 msgs[0].flags = 0; /* write */
77 msgs[0].buf = &reg;
78 msgs[0].len = 1;
79
80 msgs[1].addr = address;
81 msgs[1].flags = I2C_M_RD;
82 msgs[1].buf = data;
83 msgs[1].len = len;
84
85 res = i2c_transfer(i2c_adap, msgs, 2);
86 if (res < 2) {
87 if (res >= 0)
88 res = -EIO;
89 LOG_RESULT_LOCATION(res);
90 return res;
91 } else
92 return 0;
93}
94
95static int mpu_memory_read(struct i2c_adapter *i2c_adap,
96 unsigned char mpu_addr,
97 unsigned short mem_addr,
98 unsigned int len, unsigned char *data)
99{
100 unsigned char bank[2];
101 unsigned char addr[2];
102 unsigned char buf;
103
104 struct i2c_msg msgs[4];
105 int res;
106
107 if (!data || !i2c_adap) {
108 LOG_RESULT_LOCATION(-EINVAL);
109 return -EINVAL;
110 }
111
112 bank[0] = MPUREG_BANK_SEL;
113 bank[1] = mem_addr >> 8;
114
115 addr[0] = MPUREG_MEM_START_ADDR;
116 addr[1] = mem_addr & 0xFF;
117
118 buf = MPUREG_MEM_R_W;
119
120 /* write message */
121 msgs[0].addr = mpu_addr;
122 msgs[0].flags = 0;
123 msgs[0].buf = bank;
124 msgs[0].len = sizeof(bank);
125
126 msgs[1].addr = mpu_addr;
127 msgs[1].flags = 0;
128 msgs[1].buf = addr;
129 msgs[1].len = sizeof(addr);
130
131 msgs[2].addr = mpu_addr;
132 msgs[2].flags = 0;
133 msgs[2].buf = &buf;
134 msgs[2].len = 1;
135
136 msgs[3].addr = mpu_addr;
137 msgs[3].flags = I2C_M_RD;
138 msgs[3].buf = data;
139 msgs[3].len = len;
140
141 res = i2c_transfer(i2c_adap, msgs, 4);
142 if (res != 4) {
143 if (res >= 0)
144 res = -EIO;
145 LOG_RESULT_LOCATION(res);
146 return res;
147 } else
148 return 0;
149}
150
151static int mpu_memory_write(struct i2c_adapter *i2c_adap,
152 unsigned char mpu_addr,
153 unsigned short mem_addr,
154 unsigned int len, unsigned char const *data)
155{
156 unsigned char bank[2];
157 unsigned char addr[2];
158 unsigned char buf[513];
159
160 struct i2c_msg msgs[3];
161 int res;
162
163 if (!data || !i2c_adap) {
164 LOG_RESULT_LOCATION(-EINVAL);
165 return -EINVAL;
166 }
167 if (len >= (sizeof(buf) - 1)) {
168 LOG_RESULT_LOCATION(-ENOMEM);
169 return -ENOMEM;
170 }
171
172 bank[0] = MPUREG_BANK_SEL;
173 bank[1] = mem_addr >> 8;
174
175 addr[0] = MPUREG_MEM_START_ADDR;
176 addr[1] = mem_addr & 0xFF;
177
178 buf[0] = MPUREG_MEM_R_W;
179 memcpy(buf + 1, data, len);
180
181 /* write message */
182 msgs[0].addr = mpu_addr;
183 msgs[0].flags = 0;
184 msgs[0].buf = bank;
185 msgs[0].len = sizeof(bank);
186
187 msgs[1].addr = mpu_addr;
188 msgs[1].flags = 0;
189 msgs[1].buf = addr;
190 msgs[1].len = sizeof(addr);
191
192 msgs[2].addr = mpu_addr;
193 msgs[2].flags = 0;
194 msgs[2].buf = (unsigned char *)buf;
195 msgs[2].len = len + 1;
196
197 res = i2c_transfer(i2c_adap, msgs, 3);
198 if (res != 3) {
199 if (res >= 0)
200 res = -EIO;
201 LOG_RESULT_LOCATION(res);
202 return res;
203 } else
204 return 0;
205}
206
207int inv_serial_single_write(
208 void *sl_handle,
209 unsigned char slave_addr,
210 unsigned char register_addr,
211 unsigned char data)
212{
213 return inv_i2c_write_register((struct i2c_adapter *)sl_handle,
214 slave_addr, register_addr, data);
215}
216EXPORT_SYMBOL(inv_serial_single_write);
217
218int inv_serial_write(
219 void *sl_handle,
220 unsigned char slave_addr,
221 unsigned short length,
222 unsigned char const *data)
223{
224 int result;
225 const unsigned short data_length = length - 1;
226 const unsigned char start_reg_addr = data[0];
227 unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
228 unsigned short bytes_written = 0;
229
230 while (bytes_written < data_length) {
231 unsigned short this_len = min(SERIAL_MAX_TRANSFER_SIZE,
232 data_length - bytes_written);
233 if (bytes_written == 0) {
234 result = inv_i2c_write((struct i2c_adapter *)
235 sl_handle, slave_addr,
236 1 + this_len, data);
237 } else {
238 /* manually increment register addr between chunks */
239 i2c_write[0] = start_reg_addr + bytes_written;
240 memcpy(&i2c_write[1], &data[1 + bytes_written],
241 this_len);
242 result = inv_i2c_write((struct i2c_adapter *)
243 sl_handle, slave_addr,
244 1 + this_len, i2c_write);
245 }
246 if (result) {
247 LOG_RESULT_LOCATION(result);
248 return result;
249 }
250 bytes_written += this_len;
251 }
252 return 0;
253}
254EXPORT_SYMBOL(inv_serial_write);
255
256int inv_serial_read(
257 void *sl_handle,
258 unsigned char slave_addr,
259 unsigned char register_addr,
260 unsigned short length,
261 unsigned char *data)
262{
263 int result;
264 unsigned short bytes_read = 0;
265
266 if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
267 && (register_addr == MPUREG_FIFO_R_W ||
268 register_addr == MPUREG_MEM_R_W)) {
269 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
270 return INV_ERROR_INVALID_PARAMETER;
271 }
272
273 while (bytes_read < length) {
274 unsigned short this_len =
275 min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
276 result = inv_i2c_read((struct i2c_adapter *)sl_handle,
277 slave_addr, register_addr + bytes_read,
278 this_len, &data[bytes_read]);
279 if (result) {
280 LOG_RESULT_LOCATION(result);
281 return result;
282 }
283 bytes_read += this_len;
284 }
285 return 0;
286}
287EXPORT_SYMBOL(inv_serial_read);
288
289int inv_serial_write_mem(
290 void *sl_handle,
291 unsigned char slave_addr,
292 unsigned short mem_addr,
293 unsigned short length,
294 unsigned char const *data)
295{
296 int result;
297 unsigned short bytes_written = 0;
298
299 if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
300 pr_err("memory read length (%d B) extends beyond its"
301 " limits (%d) if started at location %d\n", length,
302 MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
303 return INV_ERROR_INVALID_PARAMETER;
304 }
305 while (bytes_written < length) {
306 unsigned short this_len =
307 min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
308 result = mpu_memory_write((struct i2c_adapter *)sl_handle,
309 slave_addr, mem_addr + bytes_written,
310 this_len, &data[bytes_written]);
311 if (result) {
312 LOG_RESULT_LOCATION(result);
313 return result;
314 }
315 bytes_written += this_len;
316 }
317 return 0;
318}
319EXPORT_SYMBOL(inv_serial_write_mem);
320
321int inv_serial_read_mem(
322 void *sl_handle,
323 unsigned char slave_addr,
324 unsigned short mem_addr,
325 unsigned short length,
326 unsigned char *data)
327{
328 int result;
329 unsigned short bytes_read = 0;
330
331 if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
332 printk
333 ("memory read length (%d B) extends beyond its limits (%d) "
334 "if started at location %d\n", length,
335 MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
336 return INV_ERROR_INVALID_PARAMETER;
337 }
338 while (bytes_read < length) {
339 unsigned short this_len =
340 min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
341 result =
342 mpu_memory_read((struct i2c_adapter *)sl_handle,
343 slave_addr, mem_addr + bytes_read,
344 this_len, &data[bytes_read]);
345 if (result) {
346 LOG_RESULT_LOCATION(result);
347 return result;
348 }
349 bytes_read += this_len;
350 }
351 return 0;
352}
353EXPORT_SYMBOL(inv_serial_read_mem);
354
355int inv_serial_write_fifo(
356 void *sl_handle,
357 unsigned char slave_addr,
358 unsigned short length,
359 unsigned char const *data)
360{
361 int result;
362 unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
363 unsigned short bytes_written = 0;
364
365 if (length > FIFO_HW_SIZE) {
366 printk(KERN_ERR
367 "maximum fifo write length is %d\n", FIFO_HW_SIZE);
368 return INV_ERROR_INVALID_PARAMETER;
369 }
370 while (bytes_written < length) {
371 unsigned short this_len =
372 min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
373 i2c_write[0] = MPUREG_FIFO_R_W;
374 memcpy(&i2c_write[1], &data[bytes_written], this_len);
375 result = inv_i2c_write((struct i2c_adapter *)sl_handle,
376 slave_addr, this_len + 1, i2c_write);
377 if (result) {
378 LOG_RESULT_LOCATION(result);
379 return result;
380 }
381 bytes_written += this_len;
382 }
383 return 0;
384}
385EXPORT_SYMBOL(inv_serial_write_fifo);
386
387int inv_serial_read_fifo(
388 void *sl_handle,
389 unsigned char slave_addr,
390 unsigned short length,
391 unsigned char *data)
392{
393 int result;
394 unsigned short bytes_read = 0;
395
396 if (length > FIFO_HW_SIZE) {
397 printk(KERN_ERR
398 "maximum fifo read length is %d\n", FIFO_HW_SIZE);
399 return INV_ERROR_INVALID_PARAMETER;
400 }
401 while (bytes_read < length) {
402 unsigned short this_len =
403 min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
404 result = inv_i2c_read((struct i2c_adapter *)sl_handle,
405 slave_addr, MPUREG_FIFO_R_W, this_len,
406 &data[bytes_read]);
407 if (result) {
408 LOG_RESULT_LOCATION(result);
409 return result;
410 }
411 bytes_read += this_len;
412 }
413
414 return 0;
415}
416EXPORT_SYMBOL(inv_serial_read_fifo);
417
418/**
419 * @}
420 */
diff --git a/drivers/misc/inv_mpu/mpu6050/mpu-dev.c b/drivers/misc/inv_mpu/mpu6050/mpu-dev.c
new file mode 100644
index 00000000000..e171dc2a7ef
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mpu-dev.c
@@ -0,0 +1,1309 @@
1/*
2 $License:
3 Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17 $
18 */
19#include <linux/i2c.h>
20#include <linux/i2c-dev.h>
21#include <linux/interrupt.h>
22#include <linux/module.h>
23#include <linux/moduleparam.h>
24#include <linux/kernel.h>
25#include <linux/init.h>
26#include <linux/stat.h>
27#include <linux/irq.h>
28#include <linux/gpio.h>
29#include <linux/signal.h>
30#include <linux/miscdevice.h>
31#include <linux/slab.h>
32#include <linux/version.h>
33#include <linux/pm.h>
34#include <linux/mutex.h>
35#include <linux/suspend.h>
36#include <linux/poll.h>
37
38#include <linux/errno.h>
39#include <linux/fs.h>
40#include <linux/mm.h>
41#include <linux/sched.h>
42#include <linux/wait.h>
43#include <linux/uaccess.h>
44#include <linux/io.h>
45
46#include "mpuirq.h"
47#include "slaveirq.h"
48#include "mlsl.h"
49#include "mldl_cfg.h"
50#include <linux/mpu.h>
51
52#include "accel/mpu6050.h"
53
54/* Platform data for the MPU */
55struct mpu_private_data {
56 struct miscdevice dev;
57 struct i2c_client *client;
58
59 /* mldl_cfg data */
60 struct mldl_cfg mldl_cfg;
61 struct mpu_ram mpu_ram;
62 struct mpu_gyro_cfg mpu_gyro_cfg;
63 struct mpu_offsets mpu_offsets;
64 struct mpu_chip_info mpu_chip_info;
65 struct inv_mpu_cfg inv_mpu_cfg;
66 struct inv_mpu_state inv_mpu_state;
67
68 struct mutex mutex;
69 wait_queue_head_t mpu_event_wait;
70 struct completion completion;
71 struct timer_list timeout;
72 struct notifier_block nb;
73 struct mpuirq_data mpu_pm_event;
74 int response_timeout; /* In seconds */
75 unsigned long event;
76 int pid;
77 struct module *slave_modules[EXT_SLAVE_NUM_TYPES];
78};
79
80struct mpu_private_data *mpu_private_data;
81
82static void mpu_pm_timeout(u_long data)
83{
84 struct mpu_private_data *mpu = (struct mpu_private_data *)data;
85 struct i2c_client *client = mpu->client;
86 dev_dbg(&client->adapter->dev, "%s\n", __func__);
87 complete(&mpu->completion);
88}
89
90static int mpu_pm_notifier_callback(struct notifier_block *nb,
91 unsigned long event, void *unused)
92{
93 struct mpu_private_data *mpu =
94 container_of(nb, struct mpu_private_data, nb);
95 struct i2c_client *client = mpu->client;
96 struct timeval event_time;
97 dev_dbg(&client->adapter->dev, "%s: %ld\n", __func__, event);
98
99 /* Prevent the file handle from being closed before we initialize
100 the completion event */
101 mutex_lock(&mpu->mutex);
102 if (!(mpu->pid) ||
103 (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
104 mutex_unlock(&mpu->mutex);
105 return NOTIFY_OK;
106 }
107
108 if (event == PM_SUSPEND_PREPARE)
109 mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
110 if (event == PM_POST_SUSPEND)
111 mpu->event = MPU_PM_EVENT_POST_SUSPEND;
112
113 do_gettimeofday(&event_time);
114 mpu->mpu_pm_event.interruptcount++;
115 mpu->mpu_pm_event.irqtime =
116 (((long long)event_time.tv_sec) << 32) + event_time.tv_usec;
117 mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
118 mpu->mpu_pm_event.data = mpu->event;
119
120 if (mpu->response_timeout > 0) {
121 mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
122 add_timer(&mpu->timeout);
123 }
124 INIT_COMPLETION(mpu->completion);
125 mutex_unlock(&mpu->mutex);
126
127 wake_up_interruptible(&mpu->mpu_event_wait);
128 wait_for_completion(&mpu->completion);
129 del_timer_sync(&mpu->timeout);
130 dev_dbg(&client->adapter->dev, "%s: %ld DONE\n", __func__, event);
131 return NOTIFY_OK;
132}
133
134static int mpu_dev_open(struct inode *inode, struct file *file)
135{
136 struct mpu_private_data *mpu =
137 container_of(file->private_data, struct mpu_private_data, dev);
138 struct i2c_client *client = mpu->client;
139 int result;
140 int ii;
141 dev_dbg(&client->adapter->dev, "%s\n", __func__);
142 dev_dbg(&client->adapter->dev, "current->pid %d\n", current->pid);
143
144 result = mutex_lock_interruptible(&mpu->mutex);
145 if (mpu->pid) {
146 mutex_unlock(&mpu->mutex);
147 return -EBUSY;
148 }
149 mpu->pid = current->pid;
150
151 /* Reset the sensors to the default */
152 if (result) {
153 dev_err(&client->adapter->dev,
154 "%s: mutex_lock_interruptible returned %d\n",
155 __func__, result);
156 return result;
157 }
158
159 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
160 __module_get(mpu->slave_modules[ii]);
161
162 mutex_unlock(&mpu->mutex);
163 return 0;
164}
165
166/* close function - called when the "file" /dev/mpu is closed in userspace */
167static int mpu_release(struct inode *inode, struct file *file)
168{
169 struct mpu_private_data *mpu =
170 container_of(file->private_data, struct mpu_private_data, dev);
171 struct i2c_client *client = mpu->client;
172 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
173 int result = 0;
174 int ii;
175 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
176 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
177
178 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
179 if (!pdata_slave[ii])
180 slave_adapter[ii] = NULL;
181 else
182 slave_adapter[ii] =
183 i2c_get_adapter(pdata_slave[ii]->adapt_num);
184 }
185 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
186
187 mutex_lock(&mpu->mutex);
188 mldl_cfg->inv_mpu_cfg->requested_sensors = 0;
189 result = inv_mpu_suspend(mldl_cfg,
190 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
191 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
192 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
193 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
194 INV_ALL_SENSORS);
195 mpu->pid = 0;
196 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
197 module_put(mpu->slave_modules[ii]);
198
199 mutex_unlock(&mpu->mutex);
200 complete(&mpu->completion);
201 dev_dbg(&client->adapter->dev, "mpu_release\n");
202
203 return result;
204}
205
206/* read function called when from /dev/mpu is read. Read from the FIFO */
207static ssize_t mpu_read(struct file *file,
208 char __user *buf, size_t count, loff_t *offset)
209{
210 struct mpu_private_data *mpu =
211 container_of(file->private_data, struct mpu_private_data, dev);
212 struct i2c_client *client = mpu->client;
213 size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
214 int err;
215
216 if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
217 wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
218
219 if (!mpu->event || !buf
220 || count < sizeof(mpu->mpu_pm_event))
221 return 0;
222
223 err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
224 if (err) {
225 dev_err(&client->adapter->dev,
226 "Copy to user returned %d\n", err);
227 return -EFAULT;
228 }
229 mpu->event = 0;
230 return len;
231}
232
233static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
234{
235 struct mpu_private_data *mpu =
236 container_of(file->private_data, struct mpu_private_data, dev);
237 int mask = 0;
238
239 poll_wait(file, &mpu->mpu_event_wait, poll);
240 if (mpu->event)
241 mask |= POLLIN | POLLRDNORM;
242 return mask;
243}
244
245static int mpu_dev_ioctl_get_ext_slave_platform_data(
246 struct i2c_client *client,
247 struct ext_slave_platform_data __user *arg)
248{
249 struct mpu_private_data *mpu =
250 (struct mpu_private_data *)i2c_get_clientdata(client);
251 struct ext_slave_platform_data *pdata_slave;
252 struct ext_slave_platform_data local_pdata_slave;
253
254 if (copy_from_user(&local_pdata_slave, arg, sizeof(local_pdata_slave)))
255 return -EFAULT;
256
257 if (local_pdata_slave.type >= EXT_SLAVE_NUM_TYPES)
258 return -EINVAL;
259
260 pdata_slave = mpu->mldl_cfg.pdata_slave[local_pdata_slave.type];
261 /* All but private data and irq_data */
262 if (!pdata_slave)
263 return -ENODEV;
264 if (copy_to_user(arg, pdata_slave, sizeof(*pdata_slave)))
265 return -EFAULT;
266 return 0;
267}
268
269static int mpu_dev_ioctl_get_mpu_platform_data(
270 struct i2c_client *client,
271 struct mpu_platform_data __user *arg)
272{
273 struct mpu_private_data *mpu =
274 (struct mpu_private_data *)i2c_get_clientdata(client);
275 struct mpu_platform_data *pdata = mpu->mldl_cfg.pdata;
276
277 if (copy_to_user(arg, pdata, sizeof(*pdata)))
278 return -EFAULT;
279 return 0;
280}
281
282static int mpu_dev_ioctl_get_ext_slave_descr(
283 struct i2c_client *client,
284 struct ext_slave_descr __user *arg)
285{
286 struct mpu_private_data *mpu =
287 (struct mpu_private_data *)i2c_get_clientdata(client);
288 struct ext_slave_descr *slave;
289 struct ext_slave_descr local_slave;
290
291 if (copy_from_user(&local_slave, arg, sizeof(local_slave)))
292 return -EFAULT;
293
294 if (local_slave.type >= EXT_SLAVE_NUM_TYPES)
295 return -EINVAL;
296
297 slave = mpu->mldl_cfg.slave[local_slave.type];
298 /* All but private data and irq_data */
299 if (!slave)
300 return -ENODEV;
301 if (copy_to_user(arg, slave, sizeof(*slave)))
302 return -EFAULT;
303 return 0;
304}
305
306
307/**
308 * slave_config() - Pass a requested slave configuration to the slave sensor
309 *
310 * @adapter the adaptor to use to communicate with the slave
311 * @mldl_cfg the mldl configuration structuer
312 * @slave pointer to the slave descriptor
313 * @usr_config The configuration to pass to the slave sensor
314 *
315 * returns 0 or non-zero error code
316 */
317static int inv_mpu_config(struct mldl_cfg *mldl_cfg,
318 void *gyro_adapter,
319 struct ext_slave_config __user *usr_config)
320{
321 int retval = 0;
322 struct ext_slave_config config;
323
324 retval = copy_from_user(&config, usr_config, sizeof(config));
325 if (retval)
326 return -EFAULT;
327
328 if (config.len && config.data) {
329 void *data;
330 data = kmalloc(config.len, GFP_KERNEL);
331 if (!data)
332 return -ENOMEM;
333
334 retval = copy_from_user(data,
335 (void __user *)config.data, config.len);
336 if (retval) {
337 retval = -EFAULT;
338 kfree(data);
339 return retval;
340 }
341 config.data = data;
342 }
343 retval = gyro_config(gyro_adapter, mldl_cfg, &config);
344 kfree(config.data);
345 return retval;
346}
347
348static int inv_mpu_get_config(struct mldl_cfg *mldl_cfg,
349 void *gyro_adapter,
350 struct ext_slave_config __user *usr_config)
351{
352 int retval = 0;
353 struct ext_slave_config config;
354 void *user_data;
355
356 retval = copy_from_user(&config, usr_config, sizeof(config));
357 if (retval)
358 return -EFAULT;
359
360 user_data = config.data;
361 if (config.len && config.data) {
362 void *data;
363 data = kmalloc(config.len, GFP_KERNEL);
364 if (!data)
365 return -ENOMEM;
366
367 retval = copy_from_user(data,
368 (void __user *)config.data, config.len);
369 if (retval) {
370 retval = -EFAULT;
371 kfree(data);
372 return retval;
373 }
374 config.data = data;
375 }
376 retval = gyro_get_config(gyro_adapter, mldl_cfg, &config);
377 if (!retval)
378 retval = copy_to_user((unsigned char __user *)user_data,
379 config.data, config.len);
380 kfree(config.data);
381 return retval;
382}
383
384static int slave_config(struct mldl_cfg *mldl_cfg,
385 void *gyro_adapter,
386 void *slave_adapter,
387 struct ext_slave_descr *slave,
388 struct ext_slave_platform_data *pdata,
389 struct ext_slave_config __user *usr_config)
390{
391 int retval = 0;
392 struct ext_slave_config config;
393 if ((!slave) || (!slave->config))
394 return -ENODEV;
395
396 retval = copy_from_user(&config, usr_config, sizeof(config));
397 if (retval)
398 return -EFAULT;
399
400 if (config.len && config.data) {
401 void *data;
402 data = kmalloc(config.len, GFP_KERNEL);
403 if (!data)
404 return -ENOMEM;
405
406 retval = copy_from_user(data,
407 (void __user *)config.data, config.len);
408 if (retval) {
409 retval = -EFAULT;
410 kfree(data);
411 return retval;
412 }
413 config.data = data;
414 }
415 retval = inv_mpu_slave_config(mldl_cfg, gyro_adapter, slave_adapter,
416 &config, slave, pdata);
417 kfree(config.data);
418 return retval;
419}
420
421static int slave_get_config(struct mldl_cfg *mldl_cfg,
422 void *gyro_adapter,
423 void *slave_adapter,
424 struct ext_slave_descr *slave,
425 struct ext_slave_platform_data *pdata,
426 struct ext_slave_config __user *usr_config)
427{
428 int retval = 0;
429 struct ext_slave_config config;
430 void *user_data;
431 if (!(slave) || !(slave->get_config))
432 return -ENODEV;
433
434 retval = copy_from_user(&config, usr_config, sizeof(config));
435 if (retval)
436 return -EFAULT;
437
438 user_data = config.data;
439 if (config.len && config.data) {
440 void *data;
441 data = kmalloc(config.len, GFP_KERNEL);
442 if (!data)
443 return -ENOMEM;
444
445 retval = copy_from_user(data,
446 (void __user *)config.data, config.len);
447 if (retval) {
448 retval = -EFAULT;
449 kfree(data);
450 return retval;
451 }
452 config.data = data;
453 }
454 retval = inv_mpu_get_slave_config(mldl_cfg, gyro_adapter,
455 slave_adapter, &config, slave, pdata);
456 if (retval) {
457 kfree(config.data);
458 return retval;
459 }
460 retval = copy_to_user((unsigned char __user *)user_data,
461 config.data, config.len);
462 kfree(config.data);
463 return retval;
464}
465
466static int inv_slave_read(struct mldl_cfg *mldl_cfg,
467 void *gyro_adapter,
468 void *slave_adapter,
469 struct ext_slave_descr *slave,
470 struct ext_slave_platform_data *pdata,
471 void __user *usr_data)
472{
473 int retval;
474 unsigned char *data;
475 data = kzalloc(slave->read_len, GFP_KERNEL);
476 if (!data)
477 return -EFAULT;
478
479 retval = inv_mpu_slave_read(mldl_cfg, gyro_adapter, slave_adapter,
480 slave, pdata, data);
481
482 if ((!retval) &&
483 (copy_to_user((unsigned char __user *)usr_data,
484 data, slave->read_len)))
485 retval = -EFAULT;
486
487 kfree(data);
488 return retval;
489}
490
491static int mpu_handle_mlsl(void *sl_handle,
492 unsigned char addr,
493 unsigned int cmd,
494 struct mpu_read_write __user *usr_msg)
495{
496 int retval = 0;
497 struct mpu_read_write msg;
498 unsigned char *user_data;
499 retval = copy_from_user(&msg, usr_msg, sizeof(msg));
500 if (retval)
501 return -EFAULT;
502
503 user_data = msg.data;
504 if (msg.length && msg.data) {
505 unsigned char *data;
506 data = kmalloc(msg.length, GFP_KERNEL);
507 if (!data)
508 return -ENOMEM;
509
510 retval = copy_from_user(data,
511 (void __user *)msg.data, msg.length);
512 if (retval) {
513 retval = -EFAULT;
514 kfree(data);
515 return retval;
516 }
517 msg.data = data;
518 } else {
519 return -EPERM;
520 }
521
522 switch (cmd) {
523 case MPU_READ:
524 retval = inv_serial_read(sl_handle, addr,
525 msg.address, msg.length, msg.data);
526 break;
527 case MPU_WRITE:
528 retval = inv_serial_write(sl_handle, addr,
529 msg.length, msg.data);
530 break;
531 case MPU_READ_MEM:
532 retval = inv_serial_read_mem(sl_handle, addr,
533 msg.address, msg.length, msg.data);
534 break;
535 case MPU_WRITE_MEM:
536 retval = inv_serial_write_mem(sl_handle, addr,
537 msg.address, msg.length,
538 msg.data);
539 break;
540 case MPU_READ_FIFO:
541 retval = inv_serial_read_fifo(sl_handle, addr,
542 msg.length, msg.data);
543 break;
544 case MPU_WRITE_FIFO:
545 retval = inv_serial_write_fifo(sl_handle, addr,
546 msg.length, msg.data);
547 break;
548
549 };
550 if (retval) {
551 dev_err(&((struct i2c_adapter *)sl_handle)->dev,
552 "%s: i2c %d error %d\n",
553 __func__, cmd, retval);
554 kfree(msg.data);
555 return retval;
556 }
557 retval = copy_to_user((unsigned char __user *)user_data,
558 msg.data, msg.length);
559 kfree(msg.data);
560 return retval;
561}
562
563/* ioctl - I/O control */
564static long mpu_dev_ioctl(struct file *file,
565 unsigned int cmd, unsigned long arg)
566{
567 struct mpu_private_data *mpu =
568 container_of(file->private_data, struct mpu_private_data, dev);
569 struct i2c_client *client = mpu->client;
570 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
571 int retval = 0;
572 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
573 struct ext_slave_descr **slave = mldl_cfg->slave;
574 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
575 int ii;
576
577 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
578 if (!pdata_slave[ii])
579 slave_adapter[ii] = NULL;
580 else
581 slave_adapter[ii] =
582 i2c_get_adapter(pdata_slave[ii]->adapt_num);
583 }
584 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
585
586 retval = mutex_lock_interruptible(&mpu->mutex);
587 if (retval) {
588 dev_err(&client->adapter->dev,
589 "%s: mutex_lock_interruptible returned %d\n",
590 __func__, retval);
591 return retval;
592 }
593
594 switch (cmd) {
595 case MPU_GET_EXT_SLAVE_PLATFORM_DATA:
596 retval = mpu_dev_ioctl_get_ext_slave_platform_data(
597 client,
598 (struct ext_slave_platform_data __user *)arg);
599 break;
600 case MPU_GET_MPU_PLATFORM_DATA:
601 retval = mpu_dev_ioctl_get_mpu_platform_data(
602 client,
603 (struct mpu_platform_data __user *)arg);
604 break;
605 case MPU_GET_EXT_SLAVE_DESCR:
606 retval = mpu_dev_ioctl_get_ext_slave_descr(
607 client,
608 (struct ext_slave_descr __user *)arg);
609 break;
610 case MPU_READ:
611 case MPU_WRITE:
612 case MPU_READ_MEM:
613 case MPU_WRITE_MEM:
614 case MPU_READ_FIFO:
615 case MPU_WRITE_FIFO:
616 retval = mpu_handle_mlsl(
617 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
618 mldl_cfg->mpu_chip_info->addr, cmd,
619 (struct mpu_read_write __user *)arg);
620 break;
621 case MPU_CONFIG_GYRO:
622 retval = inv_mpu_config(
623 mldl_cfg,
624 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
625 (struct ext_slave_config __user *)arg);
626 break;
627 case MPU_CONFIG_ACCEL:
628 retval = slave_config(
629 mldl_cfg,
630 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
631 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
632 slave[EXT_SLAVE_TYPE_ACCEL],
633 pdata_slave[EXT_SLAVE_TYPE_ACCEL],
634 (struct ext_slave_config __user *)arg);
635 break;
636 case MPU_CONFIG_COMPASS:
637 retval = slave_config(
638 mldl_cfg,
639 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
640 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
641 slave[EXT_SLAVE_TYPE_COMPASS],
642 pdata_slave[EXT_SLAVE_TYPE_COMPASS],
643 (struct ext_slave_config __user *)arg);
644 break;
645 case MPU_CONFIG_PRESSURE:
646 retval = slave_config(
647 mldl_cfg,
648 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
649 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
650 slave[EXT_SLAVE_TYPE_PRESSURE],
651 pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
652 (struct ext_slave_config __user *)arg);
653 break;
654 case MPU_GET_CONFIG_GYRO:
655 retval = inv_mpu_get_config(
656 mldl_cfg,
657 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
658 (struct ext_slave_config __user *)arg);
659 break;
660 case MPU_GET_CONFIG_ACCEL:
661 retval = slave_get_config(
662 mldl_cfg,
663 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
664 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
665 slave[EXT_SLAVE_TYPE_ACCEL],
666 pdata_slave[EXT_SLAVE_TYPE_ACCEL],
667 (struct ext_slave_config __user *)arg);
668 break;
669 case MPU_GET_CONFIG_COMPASS:
670 retval = slave_get_config(
671 mldl_cfg,
672 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
673 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
674 slave[EXT_SLAVE_TYPE_COMPASS],
675 pdata_slave[EXT_SLAVE_TYPE_COMPASS],
676 (struct ext_slave_config __user *)arg);
677 break;
678 case MPU_GET_CONFIG_PRESSURE:
679 retval = slave_get_config(
680 mldl_cfg,
681 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
682 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
683 slave[EXT_SLAVE_TYPE_PRESSURE],
684 pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
685 (struct ext_slave_config __user *)arg);
686 break;
687 case MPU_SUSPEND:
688 retval = inv_mpu_suspend(
689 mldl_cfg,
690 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
691 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
692 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
693 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
694 arg);
695 break;
696 case MPU_RESUME:
697 retval = inv_mpu_resume(
698 mldl_cfg,
699 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
700 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
701 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
702 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
703 arg);
704 break;
705 case MPU_PM_EVENT_HANDLED:
706 dev_dbg(&client->adapter->dev, "%s: %d\n", __func__, cmd);
707 complete(&mpu->completion);
708 break;
709 case MPU_READ_ACCEL:
710 retval = inv_slave_read(
711 mldl_cfg,
712 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
713 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
714 slave[EXT_SLAVE_TYPE_ACCEL],
715 pdata_slave[EXT_SLAVE_TYPE_ACCEL],
716 (unsigned char __user *)arg);
717 break;
718 case MPU_READ_COMPASS:
719 retval = inv_slave_read(
720 mldl_cfg,
721 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
722 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
723 slave[EXT_SLAVE_TYPE_COMPASS],
724 pdata_slave[EXT_SLAVE_TYPE_COMPASS],
725 (unsigned char __user *)arg);
726 break;
727 case MPU_READ_PRESSURE:
728 retval = inv_slave_read(
729 mldl_cfg,
730 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
731 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
732 slave[EXT_SLAVE_TYPE_PRESSURE],
733 pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
734 (unsigned char __user *)arg);
735 break;
736 case MPU_GET_REQUESTED_SENSORS:
737 if (copy_to_user(
738 (__u32 __user *)arg,
739 &mldl_cfg->inv_mpu_cfg->requested_sensors,
740 sizeof(mldl_cfg->inv_mpu_cfg->requested_sensors)))
741 retval = -EFAULT;
742 break;
743 case MPU_SET_REQUESTED_SENSORS:
744 mldl_cfg->inv_mpu_cfg->requested_sensors = arg;
745 break;
746 case MPU_GET_IGNORE_SYSTEM_SUSPEND:
747 if (copy_to_user(
748 (unsigned char __user *)arg,
749 &mldl_cfg->inv_mpu_cfg->ignore_system_suspend,
750 sizeof(mldl_cfg->inv_mpu_cfg->ignore_system_suspend)))
751 retval = -EFAULT;
752 break;
753 case MPU_SET_IGNORE_SYSTEM_SUSPEND:
754 mldl_cfg->inv_mpu_cfg->ignore_system_suspend = arg;
755 break;
756 case MPU_GET_MLDL_STATUS:
757 if (copy_to_user(
758 (unsigned char __user *)arg,
759 &mldl_cfg->inv_mpu_state->status,
760 sizeof(mldl_cfg->inv_mpu_state->status)))
761 retval = -EFAULT;
762 break;
763 case MPU_GET_I2C_SLAVES_ENABLED:
764 if (copy_to_user(
765 (unsigned char __user *)arg,
766 &mldl_cfg->inv_mpu_state->i2c_slaves_enabled,
767 sizeof(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)))
768 retval = -EFAULT;
769 break;
770 default:
771 dev_err(&client->adapter->dev,
772 "%s: Unknown cmd %x, arg %lu\n",
773 __func__, cmd, arg);
774 retval = -EINVAL;
775 };
776
777 mutex_unlock(&mpu->mutex);
778 dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d\n",
779 __func__, cmd, arg, retval);
780
781 if (retval > 0)
782 retval = -retval;
783
784 return retval;
785}
786
787void mpu_shutdown(struct i2c_client *client)
788{
789 struct mpu_private_data *mpu =
790 (struct mpu_private_data *)i2c_get_clientdata(client);
791 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
792 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
793 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
794 int ii;
795
796 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
797 if (!pdata_slave[ii])
798 slave_adapter[ii] = NULL;
799 else
800 slave_adapter[ii] =
801 i2c_get_adapter(pdata_slave[ii]->adapt_num);
802 }
803 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
804
805 mutex_lock(&mpu->mutex);
806 (void)inv_mpu_suspend(mldl_cfg,
807 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
808 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
809 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
810 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
811 INV_ALL_SENSORS);
812 mutex_unlock(&mpu->mutex);
813 dev_dbg(&client->adapter->dev, "%s\n", __func__);
814}
815
816int mpu_dev_suspend(struct i2c_client *client, pm_message_t mesg)
817{
818 struct mpu_private_data *mpu =
819 (struct mpu_private_data *)i2c_get_clientdata(client);
820 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
821 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
822 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
823 int ii;
824
825 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
826 if (!pdata_slave[ii])
827 slave_adapter[ii] = NULL;
828 else
829 slave_adapter[ii] =
830 i2c_get_adapter(pdata_slave[ii]->adapt_num);
831 }
832 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
833
834 mutex_lock(&mpu->mutex);
835 if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
836 dev_dbg(&client->adapter->dev,
837 "%s: suspending on event %d\n", __func__, mesg.event);
838 (void)inv_mpu_suspend(mldl_cfg,
839 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
840 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
841 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
842 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
843 INV_ALL_SENSORS);
844 } else {
845 dev_dbg(&client->adapter->dev,
846 "%s: Already suspended %d\n", __func__, mesg.event);
847 }
848 mutex_unlock(&mpu->mutex);
849 return 0;
850}
851
852int mpu_dev_resume(struct i2c_client *client)
853{
854 struct mpu_private_data *mpu =
855 (struct mpu_private_data *)i2c_get_clientdata(client);
856 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
857 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
858 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
859 int ii;
860
861 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
862 if (!pdata_slave[ii])
863 slave_adapter[ii] = NULL;
864 else
865 slave_adapter[ii] =
866 i2c_get_adapter(pdata_slave[ii]->adapt_num);
867 }
868 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
869
870 mutex_lock(&mpu->mutex);
871 if (mpu->pid && !mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
872 (void)inv_mpu_resume(mldl_cfg,
873 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
874 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
875 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
876 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
877 mldl_cfg->inv_mpu_cfg->requested_sensors);
878 dev_dbg(&client->adapter->dev,
879 "%s for pid %d\n", __func__, mpu->pid);
880 }
881 mutex_unlock(&mpu->mutex);
882 return 0;
883}
884
885/* define which file operations are supported */
886static const struct file_operations mpu_fops = {
887 .owner = THIS_MODULE,
888 .read = mpu_read,
889 .poll = mpu_poll,
890 .unlocked_ioctl = mpu_dev_ioctl,
891 .open = mpu_dev_open,
892 .release = mpu_release,
893};
894
895int inv_mpu_register_slave(struct module *slave_module,
896 struct i2c_client *slave_client,
897 struct ext_slave_platform_data *slave_pdata,
898 struct ext_slave_descr *(*get_slave_descr)(void))
899{
900 struct mpu_private_data *mpu = mpu_private_data;
901 struct mldl_cfg *mldl_cfg;
902 struct ext_slave_descr *slave_descr;
903 struct ext_slave_platform_data **pdata_slave;
904 char *irq_name = NULL;
905 int result = 0;
906
907 if (!slave_client || !slave_pdata || !get_slave_descr)
908 return -EINVAL;
909
910 if (!mpu) {
911 dev_err(&slave_client->adapter->dev,
912 "%s: Null mpu_private_data\n", __func__);
913 return -EINVAL;
914 }
915 mldl_cfg = &mpu->mldl_cfg;
916 pdata_slave = mldl_cfg->pdata_slave;
917 slave_descr = get_slave_descr();
918
919 if (!slave_descr) {
920 dev_err(&slave_client->adapter->dev,
921 "%s: Null ext_slave_descr\n", __func__);
922 return -EINVAL;
923 }
924
925 mutex_lock(&mpu->mutex);
926 if (mpu->pid) {
927 mutex_unlock(&mpu->mutex);
928 return -EBUSY;
929 }
930
931 if (pdata_slave[slave_descr->type]) {
932 result = -EBUSY;
933 goto out_unlock_mutex;
934 }
935
936 slave_pdata->address = slave_client->addr;
937 slave_pdata->irq = slave_client->irq;
938 slave_pdata->adapt_num = i2c_adapter_id(slave_client->adapter);
939
940 dev_info(&slave_client->adapter->dev,
941 "%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
942 __func__,
943 slave_descr->name,
944 slave_descr->type,
945 slave_pdata->address,
946 slave_pdata->irq,
947 slave_pdata->adapt_num);
948
949 switch (slave_descr->type) {
950 case EXT_SLAVE_TYPE_ACCEL:
951 irq_name = "accelirq";
952 break;
953 case EXT_SLAVE_TYPE_COMPASS:
954 irq_name = "compassirq";
955 break;
956 case EXT_SLAVE_TYPE_PRESSURE:
957 irq_name = "pressureirq";
958 break;
959 default:
960 irq_name = "none";
961 };
962 if (slave_descr->init) {
963 result = slave_descr->init(slave_client->adapter,
964 slave_descr,
965 slave_pdata);
966 if (result) {
967 dev_err(&slave_client->adapter->dev,
968 "%s init failed %d\n",
969 slave_descr->name, result);
970 goto out_unlock_mutex;
971 }
972 }
973
974 if (slave_descr->type == EXT_SLAVE_TYPE_ACCEL &&
975 slave_descr->id == ACCEL_ID_MPU6050 &&
976 slave_descr->config) {
977 /* pass a reference to the mldl_cfg data
978 structure to the mpu6050 accel "class" */
979 struct ext_slave_config config;
980 config.key = MPU_SLAVE_CONFIG_INTERNAL_REFERENCE;
981 config.len = sizeof(struct mldl_cfg *);
982 config.apply = true;
983 config.data = mldl_cfg;
984 result = slave_descr->config(
985 slave_client->adapter, slave_descr,
986 slave_pdata, &config);
987 if (result) {
988 LOG_RESULT_LOCATION(result);
989 goto out_slavedescr_exit;
990 }
991 }
992 pdata_slave[slave_descr->type] = slave_pdata;
993 mpu->slave_modules[slave_descr->type] = slave_module;
994 mldl_cfg->slave[slave_descr->type] = slave_descr;
995
996 goto out_unlock_mutex;
997
998out_slavedescr_exit:
999 if (slave_descr->exit)
1000 slave_descr->exit(slave_client->adapter,
1001 slave_descr, slave_pdata);
1002out_unlock_mutex:
1003 mutex_unlock(&mpu->mutex);
1004
1005 if (!result && irq_name && (slave_pdata->irq > 0)) {
1006 int warn_result;
1007 dev_info(&slave_client->adapter->dev,
1008 "Installing %s irq using %d\n",
1009 irq_name,
1010 slave_pdata->irq);
1011 warn_result = slaveirq_init(slave_client->adapter,
1012 slave_pdata, irq_name);
1013 if (result)
1014 dev_WARN(&slave_client->adapter->dev,
1015 "%s irq assigned error: %d\n",
1016 slave_descr->name, warn_result);
1017 } else {
1018 dev_WARN(&slave_client->adapter->dev,
1019 "%s irq not assigned: %d %d %d\n",
1020 slave_descr->name,
1021 result, (int)irq_name, slave_pdata->irq);
1022 }
1023
1024 return result;
1025}
1026EXPORT_SYMBOL(inv_mpu_register_slave);
1027
1028void inv_mpu_unregister_slave(struct i2c_client *slave_client,
1029 struct ext_slave_platform_data *slave_pdata,
1030 struct ext_slave_descr *(*get_slave_descr)(void))
1031{
1032 struct mpu_private_data *mpu = mpu_private_data;
1033 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
1034 struct ext_slave_descr *slave_descr;
1035 int result;
1036
1037 dev_info(&slave_client->adapter->dev, "%s\n", __func__);
1038
1039 if (!slave_client || !slave_pdata || !get_slave_descr)
1040 return;
1041
1042 if (slave_pdata->irq)
1043 slaveirq_exit(slave_pdata);
1044
1045 slave_descr = get_slave_descr();
1046 if (!slave_descr)
1047 return;
1048
1049 mutex_lock(&mpu->mutex);
1050
1051 if (slave_descr->exit) {
1052 result = slave_descr->exit(slave_client->adapter,
1053 slave_descr,
1054 slave_pdata);
1055 if (result)
1056 dev_err(&slave_client->adapter->dev,
1057 "Accel exit failed %d\n", result);
1058 }
1059 mldl_cfg->slave[slave_descr->type] = NULL;
1060 mldl_cfg->pdata_slave[slave_descr->type] = NULL;
1061 mpu->slave_modules[slave_descr->type] = NULL;
1062
1063 mutex_unlock(&mpu->mutex);
1064
1065}
1066EXPORT_SYMBOL(inv_mpu_unregister_slave);
1067
1068static unsigned short normal_i2c[] = { I2C_CLIENT_END };
1069
1070static const struct i2c_device_id mpu_id[] = {
1071 {"mpu3050", 0},
1072 {"mpu6050", 0},
1073 {"mpu6050_no_accel", 0},
1074 {}
1075};
1076MODULE_DEVICE_TABLE(i2c, mpu_id);
1077
1078int mpu_probe(struct i2c_client *client, const struct i2c_device_id *devid)
1079{
1080 struct mpu_platform_data *pdata;
1081 struct mpu_private_data *mpu;
1082 struct mldl_cfg *mldl_cfg;
1083 int res = 0;
1084 int ii = 0;
1085 unsigned long irq_flags;
1086
1087 dev_info(&client->adapter->dev, "%s: %d\n", __func__, ii++);
1088
1089 if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
1090 res = -ENODEV;
1091 goto out_check_functionality_failed;
1092 }
1093
1094 mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
1095 if (!mpu) {
1096 res = -ENOMEM;
1097 goto out_alloc_data_failed;
1098 }
1099 mldl_cfg = &mpu->mldl_cfg;
1100 mldl_cfg->mpu_ram = &mpu->mpu_ram;
1101 mldl_cfg->mpu_gyro_cfg = &mpu->mpu_gyro_cfg;
1102 mldl_cfg->mpu_offsets = &mpu->mpu_offsets;
1103 mldl_cfg->mpu_chip_info = &mpu->mpu_chip_info;
1104 mldl_cfg->inv_mpu_cfg = &mpu->inv_mpu_cfg;
1105 mldl_cfg->inv_mpu_state = &mpu->inv_mpu_state;
1106
1107 mldl_cfg->mpu_ram->length = MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE;
1108 mldl_cfg->mpu_ram->ram = kzalloc(mldl_cfg->mpu_ram->length, GFP_KERNEL);
1109 if (!mldl_cfg->mpu_ram->ram) {
1110 res = -ENOMEM;
1111 goto out_alloc_ram_failed;
1112 }
1113 mpu_private_data = mpu;
1114 i2c_set_clientdata(client, mpu);
1115 mpu->client = client;
1116
1117 init_waitqueue_head(&mpu->mpu_event_wait);
1118 mutex_init(&mpu->mutex);
1119 init_completion(&mpu->completion);
1120
1121 mpu->response_timeout = 60; /* Seconds */
1122 mpu->timeout.function = mpu_pm_timeout;
1123 mpu->timeout.data = (u_long) mpu;
1124 init_timer(&mpu->timeout);
1125
1126 mpu->nb.notifier_call = mpu_pm_notifier_callback;
1127 mpu->nb.priority = 0;
1128 res = register_pm_notifier(&mpu->nb);
1129 if (res) {
1130 dev_err(&client->adapter->dev,
1131 "Unable to register pm_notifier %d\n", res);
1132 goto out_register_pm_notifier_failed;
1133 }
1134
1135 pdata = (struct mpu_platform_data *)client->dev.platform_data;
1136 if (!pdata) {
1137 dev_WARN(&client->adapter->dev,
1138 "Missing platform data for mpu\n");
1139 }
1140 mldl_cfg->pdata = pdata;
1141
1142 mldl_cfg->mpu_chip_info->addr = client->addr;
1143 res = inv_mpu_open(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
1144
1145 if (res) {
1146 dev_err(&client->adapter->dev,
1147 "Unable to open %s %d\n", MPU_NAME, res);
1148 res = -ENODEV;
1149 goto out_whoami_failed;
1150 }
1151
1152 mpu->dev.minor = MISC_DYNAMIC_MINOR;
1153 mpu->dev.name = "mpu";
1154 mpu->dev.fops = &mpu_fops;
1155 res = misc_register(&mpu->dev);
1156 if (res < 0) {
1157 dev_err(&client->adapter->dev,
1158 "ERROR: misc_register returned %d\n", res);
1159 goto out_misc_register_failed;
1160 }
1161
1162 if (client->irq) {
1163 dev_info(&client->adapter->dev,
1164 "Installing irq using %d\n", client->irq);
1165 if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL))
1166 irq_flags = IRQF_TRIGGER_FALLING;
1167 else
1168 irq_flags = IRQF_TRIGGER_RISING;
1169 res = mpuirq_init(client, mldl_cfg, irq_flags);
1170
1171 if (res)
1172 goto out_mpuirq_failed;
1173 } else {
1174 dev_WARN(&client->adapter->dev,
1175 "Missing %s IRQ\n", MPU_NAME);
1176 }
1177 if (!strcmp(mpu_id[1].name, devid->name)) {
1178 /* Special case to re-use the inv_mpu_register_slave */
1179 struct ext_slave_platform_data *slave_pdata;
1180 slave_pdata = kzalloc(sizeof(*slave_pdata), GFP_KERNEL);
1181 if (!slave_pdata) {
1182 res = -ENOMEM;
1183 goto out_slave_pdata_kzalloc_failed;
1184 }
1185 slave_pdata->bus = EXT_SLAVE_BUS_PRIMARY;
1186 for (ii = 0; ii < 9; ii++)
1187 slave_pdata->orientation[ii] = pdata->orientation[ii];
1188 res = inv_mpu_register_slave(
1189 NULL, client,
1190 slave_pdata,
1191 mpu6050_get_slave_descr);
1192 if (res) {
1193 /* if inv_mpu_register_slave fails there are no pointer
1194 references to the memory allocated to slave_pdata */
1195 kfree(slave_pdata);
1196 goto out_slave_pdata_kzalloc_failed;
1197 }
1198 }
1199 return res;
1200
1201out_slave_pdata_kzalloc_failed:
1202 if (client->irq)
1203 mpuirq_exit();
1204out_mpuirq_failed:
1205 misc_deregister(&mpu->dev);
1206out_misc_register_failed:
1207 inv_mpu_close(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
1208out_whoami_failed:
1209 unregister_pm_notifier(&mpu->nb);
1210out_register_pm_notifier_failed:
1211 kfree(mldl_cfg->mpu_ram->ram);
1212 mpu_private_data = NULL;
1213out_alloc_ram_failed:
1214 kfree(mpu);
1215out_alloc_data_failed:
1216out_check_functionality_failed:
1217 dev_err(&client->adapter->dev, "%s failed %d\n", __func__, res);
1218 return res;
1219
1220}
1221
1222static int mpu_remove(struct i2c_client *client)
1223{
1224 struct mpu_private_data *mpu = i2c_get_clientdata(client);
1225 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
1226 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
1227 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
1228 int ii;
1229
1230 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1231 if (!pdata_slave[ii])
1232 slave_adapter[ii] = NULL;
1233 else
1234 slave_adapter[ii] =
1235 i2c_get_adapter(pdata_slave[ii]->adapt_num);
1236 }
1237
1238 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
1239 dev_dbg(&client->adapter->dev, "%s\n", __func__);
1240
1241 inv_mpu_close(mldl_cfg,
1242 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
1243 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
1244 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
1245 slave_adapter[EXT_SLAVE_TYPE_PRESSURE]);
1246
1247 if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL] &&
1248 (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id ==
1249 ACCEL_ID_MPU6050)) {
1250 struct ext_slave_platform_data *slave_pdata =
1251 mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL];
1252 inv_mpu_unregister_slave(
1253 client,
1254 mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL],
1255 mpu6050_get_slave_descr);
1256 kfree(slave_pdata);
1257 }
1258
1259 if (client->irq)
1260 mpuirq_exit();
1261
1262 misc_deregister(&mpu->dev);
1263
1264 unregister_pm_notifier(&mpu->nb);
1265
1266 kfree(mpu->mldl_cfg.mpu_ram->ram);
1267 kfree(mpu);
1268
1269 return 0;
1270}
1271
1272static struct i2c_driver mpu_driver = {
1273 .class = I2C_CLASS_HWMON,
1274 .probe = mpu_probe,
1275 .remove = mpu_remove,
1276 .id_table = mpu_id,
1277 .driver = {
1278 .owner = THIS_MODULE,
1279 .name = MPU_NAME,
1280 },
1281 .address_list = normal_i2c,
1282 .shutdown = mpu_shutdown, /* optional */
1283 .suspend = mpu_dev_suspend, /* optional */
1284 .resume = mpu_dev_resume, /* optional */
1285
1286};
1287
1288static int __init mpu_init(void)
1289{
1290 int res = i2c_add_driver(&mpu_driver);
1291 pr_info("%s: Probe name %s\n", __func__, MPU_NAME);
1292 if (res)
1293 pr_err("%s failed\n", __func__);
1294 return res;
1295}
1296
1297static void __exit mpu_exit(void)
1298{
1299 pr_info("%s\n", __func__);
1300 i2c_del_driver(&mpu_driver);
1301}
1302
1303module_init(mpu_init);
1304module_exit(mpu_exit);
1305
1306MODULE_AUTHOR("Invensense Corporation");
1307MODULE_DESCRIPTION("User space character device interface for MPU");
1308MODULE_LICENSE("GPL");
1309MODULE_ALIAS(MPU_NAME);
generated by cgit v1.2.2 (git 2.25.0) at 2025-12-30 02:26:11 -0500