aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/misc/inv_mpu/mpu3050
diff options
context:
space:
mode:
authorJonathan Herman <hermanjl@cs.unc.edu>2013-01-22 10:38:37 -0500
committerJonathan Herman <hermanjl@cs.unc.edu>2013-01-22 10:38:37 -0500
commitfcc9d2e5a6c89d22b8b773a64fb4ad21ac318446 (patch)
treea57612d1888735a2ec7972891b68c1ac5ec8faea /drivers/misc/inv_mpu/mpu3050
parent8dea78da5cee153b8af9c07a2745f6c55057fe12 (diff)
Added missing tegra files.HEADmaster
Diffstat (limited to 'drivers/misc/inv_mpu/mpu3050')
-rw-r--r--drivers/misc/inv_mpu/mpu3050/Makefile17
-rw-r--r--drivers/misc/inv_mpu/mpu3050/mldl_cfg.c1765
-rw-r--r--drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c137
-rw-r--r--drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c420
-rw-r--r--drivers/misc/inv_mpu/mpu3050/mpu-dev.c1250
5 files changed, 3589 insertions, 0 deletions
diff --git a/drivers/misc/inv_mpu/mpu3050/Makefile b/drivers/misc/inv_mpu/mpu3050/Makefile
new file mode 100644
index 00000000000..9e573930238
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/Makefile
@@ -0,0 +1,17 @@
1
2# Kernel makefile for motions sensors
3#
4#
5
6obj-$(CONFIG_MPU_SENSORS_MPU3050) += mpu3050.o
7
8ccflags-y := -DMPU_CURRENT_BUILD_MPU3050
9
10mpu3050-objs += mldl_cfg.o
11mpu3050-objs += mldl_print_cfg.o
12mpu3050-objs += mlsl-kernel.o
13mpu3050-objs += mpu-dev.o
14
15EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
16EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
17EXTRA_CFLAGS += -DINV_CACHE_DMP=1
diff --git a/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c b/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c
new file mode 100644
index 00000000000..ccacc8ec0b5
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c
@@ -0,0 +1,1765 @@
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 "mpu3050.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 1
47#define WAKE_UP 0
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
159static int mpu3050_set_i2c_bypass(struct mldl_cfg *mldl_cfg,
160 void *mlsl_handle, unsigned char enable)
161{
162 unsigned char b;
163 int result;
164 unsigned char status = mldl_cfg->inv_mpu_state->status;
165
166 if ((status & MPU_GYRO_IS_BYPASSED && enable) ||
167 (!(status & MPU_GYRO_IS_BYPASSED) && !enable))
168 return INV_SUCCESS;
169
170 /*---- get current 'USER_CTRL' into b ----*/
171 result = inv_serial_read(
172 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
173 MPUREG_USER_CTRL, 1, &b);
174 if (result) {
175 LOG_RESULT_LOCATION(result);
176 return result;
177 }
178
179 b &= ~BIT_AUX_IF_EN;
180
181 if (!enable) {
182 result = inv_serial_single_write(
183 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
184 MPUREG_USER_CTRL,
185 (b | BIT_AUX_IF_EN));
186 if (result) {
187 LOG_RESULT_LOCATION(result);
188 return result;
189 }
190 } else {
191 /* Coming out of I2C is tricky due to several erratta. Do not
192 * modify this algorithm
193 */
194 /*
195 * 1) wait for the right time and send the command to change
196 * the aux i2c slave address to an invalid address that will
197 * get nack'ed
198 *
199 * 0x00 is broadcast. 0x7F is unlikely to be used by any aux.
200 */
201 result = inv_serial_single_write(
202 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
203 MPUREG_AUX_SLV_ADDR, 0x7F);
204 if (result) {
205 LOG_RESULT_LOCATION(result);
206 return result;
207 }
208 /*
209 * 2) wait enough time for a nack to occur, then go into
210 * bypass mode:
211 */
212 msleep(2);
213 result = inv_serial_single_write(
214 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
215 MPUREG_USER_CTRL, (b));
216 if (result) {
217 LOG_RESULT_LOCATION(result);
218 return result;
219 }
220 /*
221 * 3) wait for up to one MPU cycle then restore the slave
222 * address
223 */
224 msleep(inv_mpu_get_sampling_period_us(mldl_cfg->mpu_gyro_cfg)
225 / 1000);
226 result = inv_serial_single_write(
227 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
228 MPUREG_AUX_SLV_ADDR,
229 mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]
230 ->address);
231 if (result) {
232 LOG_RESULT_LOCATION(result);
233 return result;
234 }
235
236 /*
237 * 4) reset the ime interface
238 */
239
240 result = inv_serial_single_write(
241 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
242 MPUREG_USER_CTRL,
243 (b | BIT_AUX_IF_RST));
244 if (result) {
245 LOG_RESULT_LOCATION(result);
246 return result;
247 }
248 msleep(2);
249 }
250 if (enable)
251 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
252 else
253 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
254
255 return result;
256}
257
258
259/**
260 * @brief enables/disables the I2C bypass to an external device
261 * connected to MPU's secondary I2C bus.
262 * @param enable
263 * Non-zero to enable pass through.
264 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
265 */
266static int mpu_set_i2c_bypass(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
267 unsigned char enable)
268{
269 return mpu3050_set_i2c_bypass(mldl_cfg, mlsl_handle, enable);
270}
271
272
273#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map))
274
275/* NOTE : when not indicated, product revision
276 is considered an 'npp'; non production part */
277
278struct prod_rev_map_t {
279 unsigned char silicon_rev;
280 unsigned short gyro_trim;
281};
282
283#define OLDEST_PROD_REV_SUPPORTED 11
284static struct prod_rev_map_t prod_rev_map[] = {
285 {0, 0},
286 {MPU_SILICON_REV_A4, 131}, /* 1 A? OBSOLETED */
287 {MPU_SILICON_REV_A4, 131}, /* 2 | */
288 {MPU_SILICON_REV_A4, 131}, /* 3 | */
289 {MPU_SILICON_REV_A4, 131}, /* 4 | */
290 {MPU_SILICON_REV_A4, 131}, /* 5 | */
291 {MPU_SILICON_REV_A4, 131}, /* 6 | */
292 {MPU_SILICON_REV_A4, 131}, /* 7 | */
293 {MPU_SILICON_REV_A4, 131}, /* 8 | */
294 {MPU_SILICON_REV_A4, 131}, /* 9 | */
295 {MPU_SILICON_REV_A4, 131}, /* 10 V */
296 {MPU_SILICON_REV_B1, 131}, /* 11 B1 */
297 {MPU_SILICON_REV_B1, 131}, /* 12 | */
298 {MPU_SILICON_REV_B1, 131}, /* 13 | */
299 {MPU_SILICON_REV_B1, 131}, /* 14 V */
300 {MPU_SILICON_REV_B4, 131}, /* 15 B4 */
301 {MPU_SILICON_REV_B4, 131}, /* 16 | */
302 {MPU_SILICON_REV_B4, 131}, /* 17 | */
303 {MPU_SILICON_REV_B4, 131}, /* 18 | */
304 {MPU_SILICON_REV_B4, 115}, /* 19 | */
305 {MPU_SILICON_REV_B4, 115}, /* 20 V */
306 {MPU_SILICON_REV_B6, 131}, /* 21 B6 (B6/A9) */
307 {MPU_SILICON_REV_B4, 115}, /* 22 B4 (B7/A10) */
308 {MPU_SILICON_REV_B6, 0}, /* 23 B6 */
309 {MPU_SILICON_REV_B6, 0}, /* 24 | */
310 {MPU_SILICON_REV_B6, 0}, /* 25 | */
311 {MPU_SILICON_REV_B6, 131}, /* 26 V (B6/A11) */
312};
313
314/**
315 * @internal
316 * @brief Get the silicon revision ID from OTP for MPU3050.
317 * The silicon revision number is in read from OTP bank 0,
318 * ADDR6[7:2]. The corresponding ID is retrieved by lookup
319 * in a map.
320 *
321 * @param mldl_cfg
322 * a pointer to the mldl config data structure.
323 * @param mlsl_handle
324 * an file handle to the serial communication device the
325 * device is connected to.
326 *
327 * @return 0 on success, a non-zero error code otherwise.
328 */
329static int inv_get_silicon_rev_mpu3050(
330 struct mldl_cfg *mldl_cfg, void *mlsl_handle)
331{
332 int result;
333 unsigned char index = 0x00;
334 unsigned char bank =
335 (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
336 unsigned short mem_addr = ((bank << 8) | 0x06);
337 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
338
339 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
340 MPUREG_PRODUCT_ID, 1,
341 &mpu_chip_info->product_id);
342 if (result) {
343 LOG_RESULT_LOCATION(result);
344 return result;
345 }
346
347 result = inv_serial_read_mem(
348 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
349 mem_addr, 1, &index);
350 if (result) {
351 LOG_RESULT_LOCATION(result);
352 return result;
353 }
354 index >>= 2;
355
356 /* clean the prefetch and cfg user bank bits */
357 result = inv_serial_single_write(
358 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
359 MPUREG_BANK_SEL, 0);
360 if (result) {
361 LOG_RESULT_LOCATION(result);
362 return result;
363 }
364
365 if (index < OLDEST_PROD_REV_SUPPORTED || index >= NUM_OF_PROD_REVS) {
366 mpu_chip_info->silicon_revision = 0;
367 mpu_chip_info->gyro_sens_trim = 0;
368 MPL_LOGE("Unsupported Product Revision Detected : %d\n", index);
369 return INV_ERROR_INVALID_MODULE;
370 }
371
372 mpu_chip_info->product_revision = index;
373 mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev;
374 mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim;
375 if (mpu_chip_info->gyro_sens_trim == 0) {
376 MPL_LOGE("gyro sensitivity trim is 0"
377 " - unsupported non production part.\n");
378 return INV_ERROR_INVALID_MODULE;
379 }
380
381 return result;
382}
383#define inv_get_silicon_rev inv_get_silicon_rev_mpu3050
384
385
386/**
387 * @brief Enable / Disable the use MPU's secondary I2C interface level
388 * shifters.
389 * When enabled the secondary I2C interface to which the external
390 * device is connected runs at VDD voltage (main supply).
391 * When disabled the 2nd interface runs at VDDIO voltage.
392 * See the device specification for more details.
393 *
394 * @note using this API may produce unpredictable results, depending on how
395 * the MPU and slave device are setup on the target platform.
396 * Use of this API should entirely be restricted to system
397 * integrators. Once the correct value is found, there should be no
398 * need to change the level shifter at runtime.
399 *
400 * @pre Must be called after inv_serial_start().
401 * @note Typically called before inv_dmp_open().
402 *
403 * @param[in] enable:
404 * 0 to run at VDDIO (default),
405 * 1 to run at VDD.
406 *
407 * @return INV_SUCCESS if successfull, a non-zero error code otherwise.
408 */
409static int inv_mpu_set_level_shifter_bit(struct mldl_cfg *mldl_cfg,
410 void *mlsl_handle, unsigned char enable)
411{
412 int result;
413 unsigned char regval;
414
415 unsigned char reg;
416 unsigned char mask;
417
418 if (0 == mldl_cfg->mpu_chip_info->silicon_revision)
419 return INV_ERROR_INVALID_PARAMETER;
420
421 /*-- on parts before B6 the VDDIO bit is bit 7 of ACCEL_BURST_ADDR --
422 NOTE: this is incompatible with ST accelerometers where the VDDIO
423 bit MUST be set to enable ST's internal logic to autoincrement
424 the register address on burst reads --*/
425 if ((mldl_cfg->mpu_chip_info->silicon_revision & 0xf)
426 < MPU_SILICON_REV_B6) {
427 reg = MPUREG_ACCEL_BURST_ADDR;
428 mask = 0x80;
429 } else {
430 /*-- on B6 parts the VDDIO bit was moved to FIFO_EN2 =>
431 the mask is always 0x04 --*/
432 reg = MPUREG_FIFO_EN2;
433 mask = 0x04;
434 }
435
436 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
437 reg, 1, &regval);
438 if (result) {
439 LOG_RESULT_LOCATION(result);
440 return result;
441 }
442
443 if (enable)
444 regval |= mask;
445 else
446 regval &= ~mask;
447
448 result = inv_serial_single_write(
449 mlsl_handle, mldl_cfg->mpu_chip_info->addr, reg, regval);
450 if (result) {
451 LOG_RESULT_LOCATION(result);
452 return result;
453 }
454 return result;
455 return INV_SUCCESS;
456}
457
458
459/**
460 * @internal
461 * @brief This function controls the power management on the MPU device.
462 * The entire chip can be put to low power sleep mode, or individual
463 * gyros can be turned on/off.
464 *
465 * Putting the device into sleep mode depending upon the changing needs
466 * of the associated applications is a recommended method for reducing
467 * power consuption. It is a safe opearation in that sleep/wake up of
468 * gyros while running will not result in any interruption of data.
469 *
470 * Although it is entirely allowed to put the device into full sleep
471 * while running the DMP, it is not recomended because it will disrupt
472 * the ongoing calculations carried on inside the DMP and consequently
473 * the sensor fusion algorithm. Furthermore, while in sleep mode
474 * read & write operation from the app processor on both registers and
475 * memory are disabled and can only regained by restoring the MPU in
476 * normal power mode.
477 * Disabling any of the gyro axis will reduce the associated power
478 * consuption from the PLL but will not stop the DMP from running
479 * state.
480 *
481 * @param reset
482 * Non-zero to reset the device. Note that this setting
483 * is volatile and the corresponding register bit will
484 * clear itself right after being applied.
485 * @param sleep
486 * Non-zero to put device into full sleep.
487 * @param disable_gx
488 * Non-zero to disable gyro X.
489 * @param disable_gy
490 * Non-zero to disable gyro Y.
491 * @param disable_gz
492 * Non-zero to disable gyro Z.
493 *
494 * @return INV_SUCCESS if successfull; a non-zero error code otherwise.
495 */
496static int mpu3050_pwr_mgmt(struct mldl_cfg *mldl_cfg,
497 void *mlsl_handle,
498 unsigned char reset,
499 unsigned char sleep,
500 unsigned char disable_gx,
501 unsigned char disable_gy,
502 unsigned char disable_gz)
503{
504 unsigned char b;
505 int result;
506
507 result =
508 inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
509 MPUREG_PWR_MGM, 1, &b);
510 if (result) {
511 LOG_RESULT_LOCATION(result);
512 return result;
513 }
514
515 /* If we are awake, we need to put it in bypass before resetting */
516 if ((!(b & BIT_SLEEP)) && reset)
517 result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, 1);
518
519 /* Reset if requested */
520 if (reset) {
521 MPL_LOGV("Reset MPU3050\n");
522 result = inv_serial_single_write(
523 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
524 MPUREG_PWR_MGM, b | BIT_H_RESET);
525 if (result) {
526 LOG_RESULT_LOCATION(result);
527 return result;
528 }
529 msleep(5);
530 /* Some chips are awake after reset and some are asleep,
531 * check the status */
532 result = inv_serial_read(
533 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
534 MPUREG_PWR_MGM, 1, &b);
535 if (result) {
536 LOG_RESULT_LOCATION(result);
537 return result;
538 }
539 }
540
541 /* Update the suspended state just in case we return early */
542 if (b & BIT_SLEEP) {
543 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED;
544 mldl_cfg->inv_mpu_state->status |= MPU_DEVICE_IS_SUSPENDED;
545 } else {
546 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED;
547 mldl_cfg->inv_mpu_state->status &= ~MPU_DEVICE_IS_SUSPENDED;
548 }
549
550 /* if power status match requested, nothing else's left to do */
551 if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
552 (((sleep != 0) * BIT_SLEEP) |
553 ((disable_gx != 0) * BIT_STBY_XG) |
554 ((disable_gy != 0) * BIT_STBY_YG) |
555 ((disable_gz != 0) * BIT_STBY_ZG))) {
556 return INV_SUCCESS;
557 }
558
559 /*
560 * This specific transition between states needs to be reinterpreted:
561 * (1,1,1,1) -> (0,1,1,1) has to become
562 * (1,1,1,1) -> (1,0,0,0) -> (0,1,1,1)
563 * where
564 * (1,1,1,1) is (sleep=1,disable_gx=1,disable_gy=1,disable_gz=1)
565 */
566 if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
567 (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
568 && ((!sleep) && disable_gx && disable_gy && disable_gz)) {
569 result = mpu3050_pwr_mgmt(mldl_cfg, mlsl_handle, 0, 1, 0, 0, 0);
570 if (result)
571 return result;
572 b |= BIT_SLEEP;
573 b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
574 }
575
576 if ((b & BIT_SLEEP) != ((sleep != 0) * BIT_SLEEP)) {
577 if (sleep) {
578 result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, 1);
579 if (result) {
580 LOG_RESULT_LOCATION(result);
581 return result;
582 }
583 b |= BIT_SLEEP;
584 result =
585 inv_serial_single_write(
586 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
587 MPUREG_PWR_MGM, b);
588 if (result) {
589 LOG_RESULT_LOCATION(result);
590 return result;
591 }
592 mldl_cfg->inv_mpu_state->status |=
593 MPU_GYRO_IS_SUSPENDED;
594 mldl_cfg->inv_mpu_state->status |=
595 MPU_DEVICE_IS_SUSPENDED;
596 } else {
597 b &= ~BIT_SLEEP;
598 result =
599 inv_serial_single_write(
600 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
601 MPUREG_PWR_MGM, b);
602 if (result) {
603 LOG_RESULT_LOCATION(result);
604 return result;
605 }
606 mldl_cfg->inv_mpu_state->status &=
607 ~MPU_GYRO_IS_SUSPENDED;
608 mldl_cfg->inv_mpu_state->status &=
609 ~MPU_DEVICE_IS_SUSPENDED;
610 msleep(5);
611 }
612 }
613 /*---
614 WORKAROUND FOR PUTTING GYRO AXIS in STAND-BY MODE
615 1) put one axis at a time in stand-by
616 ---*/
617 if ((b & BIT_STBY_XG) != ((disable_gx != 0) * BIT_STBY_XG)) {
618 b ^= BIT_STBY_XG;
619 result = inv_serial_single_write(
620 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
621 MPUREG_PWR_MGM, b);
622 if (result) {
623 LOG_RESULT_LOCATION(result);
624 return result;
625 }
626 }
627 if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) {
628 b ^= BIT_STBY_YG;
629 result = inv_serial_single_write(
630 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
631 MPUREG_PWR_MGM, b);
632 if (result) {
633 LOG_RESULT_LOCATION(result);
634 return result;
635 }
636 }
637 if ((b & BIT_STBY_ZG) != ((disable_gz != 0) * BIT_STBY_ZG)) {
638 b ^= BIT_STBY_ZG;
639 result = inv_serial_single_write(
640 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
641 MPUREG_PWR_MGM, b);
642 if (result) {
643 LOG_RESULT_LOCATION(result);
644 return result;
645 }
646 }
647
648 return INV_SUCCESS;
649}
650
651
652/**
653 * @brief sets the clock source for the gyros.
654 * @param mldl_cfg
655 * a pointer to the struct mldl_cfg data structure.
656 * @param gyro_handle
657 * an handle to the serial device the gyro is assigned to.
658 * @return ML_SUCCESS if successful, a non-zero error code otherwise.
659 */
660static int mpu_set_clock_source(void *gyro_handle, struct mldl_cfg *mldl_cfg)
661{
662 int result;
663 unsigned char cur_clk_src;
664 unsigned char reg;
665
666 /* clock source selection */
667 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
668 MPUREG_PWR_MGM, 1, &reg);
669 if (result) {
670 LOG_RESULT_LOCATION(result);
671 return result;
672 }
673 cur_clk_src = reg & BITS_CLKSEL;
674 reg &= ~BITS_CLKSEL;
675
676
677 result = inv_serial_single_write(
678 gyro_handle, mldl_cfg->mpu_chip_info->addr,
679 MPUREG_PWR_MGM, mldl_cfg->mpu_gyro_cfg->clk_src | reg);
680 if (result) {
681 LOG_RESULT_LOCATION(result);
682 return result;
683 }
684 /* TODO : workarounds to be determined and implemented */
685
686 return result;
687}
688
689/**
690 * Configures the MPU I2C Master
691 *
692 * @mldl_cfg Handle to the configuration data
693 * @gyro_handle handle to the gyro communictation interface
694 * @slave Can be Null if turning off the slave
695 * @slave_pdata Can be null if turning off the slave
696 * @slave_id enum ext_slave_type to determine which index to use
697 *
698 *
699 * This fucntion configures the slaves by:
700 * 1) Setting up the read
701 * a) Read Register
702 * b) Read Length
703 * 2) Set up the data trigger (MPU6050 only)
704 * a) Set trigger write register
705 * b) Set Trigger write value
706 * 3) Set up the divider (MPU6050 only)
707 * 4) Set the slave bypass mode depending on slave
708 *
709 * returns INV_SUCCESS or non-zero error code
710 */
711static int mpu_set_slave_mpu3050(struct mldl_cfg *mldl_cfg,
712 void *gyro_handle,
713 struct ext_slave_descr *slave,
714 struct ext_slave_platform_data *slave_pdata,
715 int slave_id)
716{
717 int result;
718 unsigned char reg;
719 unsigned char slave_reg;
720 unsigned char slave_len;
721 unsigned char slave_endian;
722 unsigned char slave_address;
723
724 if (slave_id != EXT_SLAVE_TYPE_ACCEL)
725 return 0;
726
727 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
728
729 if (NULL == slave || NULL == slave_pdata) {
730 slave_reg = 0;
731 slave_len = 0;
732 slave_endian = 0;
733 slave_address = 0;
734 mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
735 } else {
736 slave_reg = slave->read_reg;
737 slave_len = slave->read_len;
738 slave_endian = slave->endian;
739 slave_address = slave_pdata->address;
740 mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 1;
741 }
742
743 /* Address */
744 result = inv_serial_single_write(gyro_handle,
745 mldl_cfg->mpu_chip_info->addr,
746 MPUREG_AUX_SLV_ADDR, slave_address);
747 if (result) {
748 LOG_RESULT_LOCATION(result);
749 return result;
750 }
751 /* Register */
752 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
753 MPUREG_ACCEL_BURST_ADDR, 1, &reg);
754 if (result) {
755 LOG_RESULT_LOCATION(result);
756 return result;
757 }
758 reg = ((reg & 0x80) | slave_reg);
759 result = inv_serial_single_write(gyro_handle,
760 mldl_cfg->mpu_chip_info->addr,
761 MPUREG_ACCEL_BURST_ADDR, reg);
762 if (result) {
763 LOG_RESULT_LOCATION(result);
764 return result;
765 }
766
767 /* Length */
768 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
769 MPUREG_USER_CTRL, 1, &reg);
770 if (result) {
771 LOG_RESULT_LOCATION(result);
772 return result;
773 }
774 reg = (reg & ~BIT_AUX_RD_LENG);
775 result = inv_serial_single_write(
776 gyro_handle, mldl_cfg->mpu_chip_info->addr,
777 MPUREG_USER_CTRL, reg);
778 if (result) {
779 LOG_RESULT_LOCATION(result);
780 return result;
781 }
782
783 return result;
784}
785
786
787static int mpu_set_slave(struct mldl_cfg *mldl_cfg,
788 void *gyro_handle,
789 struct ext_slave_descr *slave,
790 struct ext_slave_platform_data *slave_pdata,
791 int slave_id)
792{
793 return mpu_set_slave_mpu3050(mldl_cfg, gyro_handle, slave,
794 slave_pdata, slave_id);
795}
796/**
797 * Check to see if the gyro was reset by testing a couple of registers known
798 * to change on reset.
799 *
800 * @mldl_cfg mldl configuration structure
801 * @gyro_handle handle used to communicate with the gyro
802 *
803 * @return INV_SUCCESS or non-zero error code
804 */
805static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
806{
807 int result = INV_SUCCESS;
808 unsigned char reg;
809
810 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
811 MPUREG_DMP_CFG_2, 1, &reg);
812 if (result) {
813 LOG_RESULT_LOCATION(result);
814 return result;
815 }
816
817 if (mldl_cfg->mpu_gyro_cfg->dmp_cfg2 != reg)
818 return true;
819
820 if (0 != mldl_cfg->mpu_gyro_cfg->dmp_cfg1)
821 return false;
822
823 /* Inconclusive assume it was reset */
824 return true;
825}
826
827
828int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
829 const unsigned char *data, int size)
830{
831 int bank, offset, write_size;
832 int result;
833 unsigned char read[MPU_MEM_BANK_SIZE];
834
835 if (mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) {
836#if INV_CACHE_DMP == 1
837 memcpy(mldl_cfg->mpu_ram->ram, data, size);
838 return INV_SUCCESS;
839#else
840 LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
841 return INV_ERROR_MEMORY_SET;
842#endif
843 }
844
845 if (!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)) {
846 LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
847 return INV_ERROR_MEMORY_SET;
848 }
849 /* Write and verify memory */
850 for (bank = 0; size > 0; bank++,
851 size -= write_size,
852 data += write_size) {
853 if (size > MPU_MEM_BANK_SIZE)
854 write_size = MPU_MEM_BANK_SIZE;
855 else
856 write_size = size;
857
858 result = inv_serial_write_mem(mlsl_handle,
859 mldl_cfg->mpu_chip_info->addr,
860 ((bank << 8) | 0x00),
861 write_size,
862 data);
863 if (result) {
864 LOG_RESULT_LOCATION(result);
865 MPL_LOGE("Write mem error in bank %d\n", bank);
866 return result;
867 }
868 result = inv_serial_read_mem(mlsl_handle,
869 mldl_cfg->mpu_chip_info->addr,
870 ((bank << 8) | 0x00),
871 write_size,
872 read);
873 if (result) {
874 LOG_RESULT_LOCATION(result);
875 MPL_LOGE("Read mem error in bank %d\n", bank);
876 return result;
877 }
878
879#define ML_SKIP_CHECK 20
880 for (offset = 0; offset < write_size; offset++) {
881 /* skip the register memory locations */
882 if (bank == 0 && offset < ML_SKIP_CHECK)
883 continue;
884 if (data[offset] != read[offset]) {
885 result = INV_ERROR_SERIAL_WRITE;
886 break;
887 }
888 }
889 if (result != INV_SUCCESS) {
890 LOG_RESULT_LOCATION(result);
891 MPL_LOGE("Read data mismatch at bank %d, offset %d\n",
892 bank, offset);
893 return result;
894 }
895 }
896 return INV_SUCCESS;
897}
898
899static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle,
900 unsigned long sensors)
901{
902 int result;
903 int ii;
904 unsigned char reg;
905 unsigned char regs[7];
906
907 /* Wake up the part */
908 result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, false, false,
909 !(sensors & INV_X_GYRO),
910 !(sensors & INV_Y_GYRO),
911 !(sensors & INV_Z_GYRO));
912
913 if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG) &&
914 !mpu_was_reset(mldl_cfg, gyro_handle)) {
915 return INV_SUCCESS;
916 }
917
918 if (result) {
919 LOG_RESULT_LOCATION(result);
920 return result;
921 }
922 result = inv_serial_single_write(
923 gyro_handle, mldl_cfg->mpu_chip_info->addr,
924 MPUREG_INT_CFG,
925 (mldl_cfg->mpu_gyro_cfg->int_config |
926 mldl_cfg->pdata->int_config));
927 if (result) {
928 LOG_RESULT_LOCATION(result);
929 return result;
930 }
931 result = inv_serial_single_write(
932 gyro_handle, mldl_cfg->mpu_chip_info->addr,
933 MPUREG_SMPLRT_DIV, mldl_cfg->mpu_gyro_cfg->divider);
934 if (result) {
935 LOG_RESULT_LOCATION(result);
936 return result;
937 }
938 result = mpu_set_clock_source(gyro_handle, mldl_cfg);
939 if (result) {
940 LOG_RESULT_LOCATION(result);
941 return result;
942 }
943
944 reg = DLPF_FS_SYNC_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync,
945 mldl_cfg->mpu_gyro_cfg->full_scale,
946 mldl_cfg->mpu_gyro_cfg->lpf);
947 result = inv_serial_single_write(
948 gyro_handle, mldl_cfg->mpu_chip_info->addr,
949 MPUREG_DLPF_FS_SYNC, reg);
950 if (result) {
951 LOG_RESULT_LOCATION(result);
952 return result;
953 }
954 result = inv_serial_single_write(
955 gyro_handle, mldl_cfg->mpu_chip_info->addr,
956 MPUREG_DMP_CFG_1, mldl_cfg->mpu_gyro_cfg->dmp_cfg1);
957 if (result) {
958 LOG_RESULT_LOCATION(result);
959 return result;
960 }
961 result = inv_serial_single_write(
962 gyro_handle, mldl_cfg->mpu_chip_info->addr,
963 MPUREG_DMP_CFG_2, mldl_cfg->mpu_gyro_cfg->dmp_cfg2);
964 if (result) {
965 LOG_RESULT_LOCATION(result);
966 return result;
967 }
968
969 /* Write and verify memory */
970#if INV_CACHE_DMP != 0
971 inv_mpu_set_firmware(mldl_cfg, gyro_handle,
972 mldl_cfg->mpu_ram->ram, mldl_cfg->mpu_ram->length);
973#endif
974
975 result = inv_serial_single_write(
976 gyro_handle, mldl_cfg->mpu_chip_info->addr,
977 MPUREG_XG_OFFS_TC, mldl_cfg->mpu_offsets->tc[0]);
978 if (result) {
979 LOG_RESULT_LOCATION(result);
980 return result;
981 }
982 result = inv_serial_single_write(
983 gyro_handle, mldl_cfg->mpu_chip_info->addr,
984 MPUREG_YG_OFFS_TC, mldl_cfg->mpu_offsets->tc[1]);
985 if (result) {
986 LOG_RESULT_LOCATION(result);
987 return result;
988 }
989 result = inv_serial_single_write(
990 gyro_handle, mldl_cfg->mpu_chip_info->addr,
991 MPUREG_ZG_OFFS_TC, mldl_cfg->mpu_offsets->tc[2]);
992 if (result) {
993 LOG_RESULT_LOCATION(result);
994 return result;
995 }
996 regs[0] = MPUREG_X_OFFS_USRH;
997 for (ii = 0; ii < ARRAY_SIZE(mldl_cfg->mpu_offsets->gyro); ii++) {
998 regs[1 + ii * 2] =
999 (unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] >> 8)
1000 & 0xff;
1001 regs[1 + ii * 2 + 1] =
1002 (unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] & 0xff);
1003 }
1004 result = inv_serial_write(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1005 7, regs);
1006 if (result) {
1007 LOG_RESULT_LOCATION(result);
1008 return result;
1009 }
1010
1011 /* Configure slaves */
1012 result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
1013 mldl_cfg->pdata->level_shifter);
1014 if (result) {
1015 LOG_RESULT_LOCATION(result);
1016 return result;
1017 }
1018 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_CONFIG;
1019
1020 return result;
1021}
1022
1023int gyro_config(void *mlsl_handle,
1024 struct mldl_cfg *mldl_cfg,
1025 struct ext_slave_config *data)
1026{
1027 struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
1028 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
1029 struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
1030 int ii;
1031
1032 if (!data->data)
1033 return INV_ERROR_INVALID_PARAMETER;
1034
1035 switch (data->key) {
1036 case MPU_SLAVE_INT_CONFIG:
1037 mpu_gyro_cfg->int_config = *((__u8 *)data->data);
1038 break;
1039 case MPU_SLAVE_EXT_SYNC:
1040 mpu_gyro_cfg->ext_sync = *((__u8 *)data->data);
1041 break;
1042 case MPU_SLAVE_FULL_SCALE:
1043 mpu_gyro_cfg->full_scale = *((__u8 *)data->data);
1044 break;
1045 case MPU_SLAVE_LPF:
1046 mpu_gyro_cfg->lpf = *((__u8 *)data->data);
1047 break;
1048 case MPU_SLAVE_CLK_SRC:
1049 mpu_gyro_cfg->clk_src = *((__u8 *)data->data);
1050 break;
1051 case MPU_SLAVE_DIVIDER:
1052 mpu_gyro_cfg->divider = *((__u8 *)data->data);
1053 break;
1054 case MPU_SLAVE_DMP_ENABLE:
1055 mpu_gyro_cfg->dmp_enable = *((__u8 *)data->data);
1056 break;
1057 case MPU_SLAVE_FIFO_ENABLE:
1058 mpu_gyro_cfg->fifo_enable = *((__u8 *)data->data);
1059 break;
1060 case MPU_SLAVE_DMP_CFG1:
1061 mpu_gyro_cfg->dmp_cfg1 = *((__u8 *)data->data);
1062 break;
1063 case MPU_SLAVE_DMP_CFG2:
1064 mpu_gyro_cfg->dmp_cfg2 = *((__u8 *)data->data);
1065 break;
1066 case MPU_SLAVE_TC:
1067 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1068 mpu_offsets->tc[ii] = ((__u8 *)data->data)[ii];
1069 break;
1070 case MPU_SLAVE_GYRO:
1071 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1072 mpu_offsets->gyro[ii] = ((__u16 *)data->data)[ii];
1073 break;
1074 case MPU_SLAVE_ADDR:
1075 mpu_chip_info->addr = *((__u8 *)data->data);
1076 break;
1077 case MPU_SLAVE_PRODUCT_REVISION:
1078 mpu_chip_info->product_revision = *((__u8 *)data->data);
1079 break;
1080 case MPU_SLAVE_SILICON_REVISION:
1081 mpu_chip_info->silicon_revision = *((__u8 *)data->data);
1082 break;
1083 case MPU_SLAVE_PRODUCT_ID:
1084 mpu_chip_info->product_id = *((__u8 *)data->data);
1085 break;
1086 case MPU_SLAVE_GYRO_SENS_TRIM:
1087 mpu_chip_info->gyro_sens_trim = *((__u16 *)data->data);
1088 break;
1089 case MPU_SLAVE_ACCEL_SENS_TRIM:
1090 mpu_chip_info->accel_sens_trim = *((__u16 *)data->data);
1091 break;
1092 case MPU_SLAVE_RAM:
1093 if (data->len != mldl_cfg->mpu_ram->length)
1094 return INV_ERROR_INVALID_PARAMETER;
1095
1096 memcpy(mldl_cfg->mpu_ram->ram, data->data, data->len);
1097 break;
1098 default:
1099 LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
1100 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1101 };
1102 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG;
1103 return INV_SUCCESS;
1104}
1105
1106int gyro_get_config(void *mlsl_handle,
1107 struct mldl_cfg *mldl_cfg,
1108 struct ext_slave_config *data)
1109{
1110 struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
1111 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
1112 struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
1113 int ii;
1114
1115 if (!data->data)
1116 return INV_ERROR_INVALID_PARAMETER;
1117
1118 switch (data->key) {
1119 case MPU_SLAVE_INT_CONFIG:
1120 *((__u8 *)data->data) = mpu_gyro_cfg->int_config;
1121 break;
1122 case MPU_SLAVE_EXT_SYNC:
1123 *((__u8 *)data->data) = mpu_gyro_cfg->ext_sync;
1124 break;
1125 case MPU_SLAVE_FULL_SCALE:
1126 *((__u8 *)data->data) = mpu_gyro_cfg->full_scale;
1127 break;
1128 case MPU_SLAVE_LPF:
1129 *((__u8 *)data->data) = mpu_gyro_cfg->lpf;
1130 break;
1131 case MPU_SLAVE_CLK_SRC:
1132 *((__u8 *)data->data) = mpu_gyro_cfg->clk_src;
1133 break;
1134 case MPU_SLAVE_DIVIDER:
1135 *((__u8 *)data->data) = mpu_gyro_cfg->divider;
1136 break;
1137 case MPU_SLAVE_DMP_ENABLE:
1138 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_enable;
1139 break;
1140 case MPU_SLAVE_FIFO_ENABLE:
1141 *((__u8 *)data->data) = mpu_gyro_cfg->fifo_enable;
1142 break;
1143 case MPU_SLAVE_DMP_CFG1:
1144 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg1;
1145 break;
1146 case MPU_SLAVE_DMP_CFG2:
1147 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg2;
1148 break;
1149 case MPU_SLAVE_TC:
1150 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1151 ((__u8 *)data->data)[ii] = mpu_offsets->tc[ii];
1152 break;
1153 case MPU_SLAVE_GYRO:
1154 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1155 ((__u16 *)data->data)[ii] = mpu_offsets->gyro[ii];
1156 break;
1157 case MPU_SLAVE_ADDR:
1158 *((__u8 *)data->data) = mpu_chip_info->addr;
1159 break;
1160 case MPU_SLAVE_PRODUCT_REVISION:
1161 *((__u8 *)data->data) = mpu_chip_info->product_revision;
1162 break;
1163 case MPU_SLAVE_SILICON_REVISION:
1164 *((__u8 *)data->data) = mpu_chip_info->silicon_revision;
1165 break;
1166 case MPU_SLAVE_PRODUCT_ID:
1167 *((__u8 *)data->data) = mpu_chip_info->product_id;
1168 break;
1169 case MPU_SLAVE_GYRO_SENS_TRIM:
1170 *((__u16 *)data->data) = mpu_chip_info->gyro_sens_trim;
1171 break;
1172 case MPU_SLAVE_ACCEL_SENS_TRIM:
1173 *((__u16 *)data->data) = mpu_chip_info->accel_sens_trim;
1174 break;
1175 case MPU_SLAVE_RAM:
1176 if (data->len != mldl_cfg->mpu_ram->length)
1177 return INV_ERROR_INVALID_PARAMETER;
1178
1179 memcpy(data->data, mldl_cfg->mpu_ram->ram, data->len);
1180 break;
1181 default:
1182 LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
1183 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1184 };
1185
1186 return INV_SUCCESS;
1187}
1188
1189
1190/*******************************************************************************
1191 *******************************************************************************
1192 * Exported functions
1193 *******************************************************************************
1194 ******************************************************************************/
1195
1196/**
1197 * Initializes the pdata structure to defaults.
1198 *
1199 * Opens the device to read silicon revision, product id and whoami.
1200 *
1201 * @mldl_cfg
1202 * The internal device configuration data structure.
1203 * @mlsl_handle
1204 * The serial communication handle.
1205 *
1206 * @return INV_SUCCESS if silicon revision, product id and woami are supported
1207 * by this software.
1208 */
1209int inv_mpu_open(struct mldl_cfg *mldl_cfg,
1210 void *gyro_handle,
1211 void *accel_handle,
1212 void *compass_handle, void *pressure_handle)
1213{
1214 int result;
1215 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1216 int ii;
1217
1218 /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
1219 ii = 0;
1220 mldl_cfg->inv_mpu_cfg->ignore_system_suspend = false;
1221 mldl_cfg->mpu_gyro_cfg->int_config = BIT_DMP_INT_EN;
1222 mldl_cfg->mpu_gyro_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
1223 mldl_cfg->mpu_gyro_cfg->lpf = MPU_FILTER_42HZ;
1224 mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_2000DPS;
1225 mldl_cfg->mpu_gyro_cfg->divider = 4;
1226 mldl_cfg->mpu_gyro_cfg->dmp_enable = 1;
1227 mldl_cfg->mpu_gyro_cfg->fifo_enable = 1;
1228 mldl_cfg->mpu_gyro_cfg->ext_sync = 0;
1229 mldl_cfg->mpu_gyro_cfg->dmp_cfg1 = 0;
1230 mldl_cfg->mpu_gyro_cfg->dmp_cfg2 = 0;
1231 mldl_cfg->inv_mpu_state->status =
1232 MPU_DMP_IS_SUSPENDED |
1233 MPU_GYRO_IS_SUSPENDED |
1234 MPU_ACCEL_IS_SUSPENDED |
1235 MPU_COMPASS_IS_SUSPENDED |
1236 MPU_PRESSURE_IS_SUSPENDED |
1237 MPU_DEVICE_IS_SUSPENDED;
1238 mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
1239
1240 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1241 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1242 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1243 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1244
1245 if (mldl_cfg->mpu_chip_info->addr == 0) {
1246 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
1247 return INV_ERROR_INVALID_PARAMETER;
1248 }
1249
1250 /*
1251 * Reset,
1252 * Take the DMP out of sleep, and
1253 * read the product_id, sillicon rev and whoami
1254 */
1255 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
1256 result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, RESET, 0, 0, 0, 0);
1257 if (result) {
1258 LOG_RESULT_LOCATION(result);
1259 return result;
1260 }
1261
1262 result = inv_get_silicon_rev(mldl_cfg, gyro_handle);
1263 if (result) {
1264 LOG_RESULT_LOCATION(result);
1265 return result;
1266 }
1267
1268 /* Get the factory temperature compensation offsets */
1269 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1270 MPUREG_XG_OFFS_TC, 1,
1271 &mldl_cfg->mpu_offsets->tc[0]);
1272 if (result) {
1273 LOG_RESULT_LOCATION(result);
1274 return result;
1275 }
1276 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1277 MPUREG_YG_OFFS_TC, 1,
1278 &mldl_cfg->mpu_offsets->tc[1]);
1279 if (result) {
1280 LOG_RESULT_LOCATION(result);
1281 return result;
1282 }
1283 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1284 MPUREG_ZG_OFFS_TC, 1,
1285 &mldl_cfg->mpu_offsets->tc[2]);
1286 if (result) {
1287 LOG_RESULT_LOCATION(result);
1288 return result;
1289 }
1290
1291 /* Into bypass mode before sleeping and calling the slaves init */
1292 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
1293 if (result) {
1294 LOG_RESULT_LOCATION(result);
1295 return result;
1296 }
1297 result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
1298 mldl_cfg->pdata->level_shifter);
1299 if (result) {
1300 LOG_RESULT_LOCATION(result);
1301 return result;
1302 }
1303
1304
1305#if INV_CACHE_DMP != 0
1306 result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, 0, SLEEP, 0, 0, 0);
1307#endif
1308 if (result) {
1309 LOG_RESULT_LOCATION(result);
1310 return result;
1311 }
1312
1313
1314 return result;
1315
1316}
1317
1318/**
1319 * Close the mpu interface
1320 *
1321 * @mldl_cfg pointer to the configuration structure
1322 * @mlsl_handle pointer to the serial layer handle
1323 *
1324 * @return INV_SUCCESS or non-zero error code
1325 */
1326int inv_mpu_close(struct mldl_cfg *mldl_cfg,
1327 void *gyro_handle,
1328 void *accel_handle,
1329 void *compass_handle,
1330 void *pressure_handle)
1331{
1332 return 0;
1333}
1334
1335/**
1336 * @brief resume the MPU device and all the other sensor
1337 * devices from their low power state.
1338 *
1339 * @mldl_cfg
1340 * pointer to the configuration structure
1341 * @gyro_handle
1342 * the main file handle to the MPU device.
1343 * @accel_handle
1344 * an handle to the accelerometer device, if sitting
1345 * onto a separate bus. Can match mlsl_handle if
1346 * the accelerometer device operates on the same
1347 * primary bus of MPU.
1348 * @compass_handle
1349 * an handle to the compass device, if sitting
1350 * onto a separate bus. Can match mlsl_handle if
1351 * the compass device operates on the same
1352 * primary bus of MPU.
1353 * @pressure_handle
1354 * an handle to the pressure sensor device, if sitting
1355 * onto a separate bus. Can match mlsl_handle if
1356 * the pressure sensor device operates on the same
1357 * primary bus of MPU.
1358 * @resume_gyro
1359 * whether resuming the gyroscope device is
1360 * actually needed (if the device supports low power
1361 * mode of some sort).
1362 * @resume_accel
1363 * whether resuming the accelerometer device is
1364 * actually needed (if the device supports low power
1365 * mode of some sort).
1366 * @resume_compass
1367 * whether resuming the compass device is
1368 * actually needed (if the device supports low power
1369 * mode of some sort).
1370 * @resume_pressure
1371 * whether resuming the pressure sensor device is
1372 * actually needed (if the device supports low power
1373 * mode of some sort).
1374 * @return INV_SUCCESS or a non-zero error code.
1375 */
1376int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
1377 void *gyro_handle,
1378 void *accel_handle,
1379 void *compass_handle,
1380 void *pressure_handle,
1381 unsigned long sensors)
1382{
1383 int result = INV_SUCCESS;
1384 int ii;
1385 bool resume_slave[EXT_SLAVE_NUM_TYPES];
1386 bool resume_dmp = sensors & INV_DMP_PROCESSOR;
1387 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1388 resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
1389 (sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
1390 resume_slave[EXT_SLAVE_TYPE_ACCEL] =
1391 sensors & INV_THREE_AXIS_ACCEL;
1392 resume_slave[EXT_SLAVE_TYPE_COMPASS] =
1393 sensors & INV_THREE_AXIS_COMPASS;
1394 resume_slave[EXT_SLAVE_TYPE_PRESSURE] =
1395 sensors & INV_THREE_AXIS_PRESSURE;
1396
1397 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1398 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1399 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1400 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1401
1402
1403 mldl_print_cfg(mldl_cfg);
1404
1405 /* Skip the Gyro since slave[EXT_SLAVE_TYPE_GYROSCOPE] is NULL */
1406 for (ii = EXT_SLAVE_TYPE_ACCEL; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1407 if (resume_slave[ii] &&
1408 ((!mldl_cfg->slave[ii]) ||
1409 (!mldl_cfg->slave[ii]->resume))) {
1410 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
1411 return INV_ERROR_INVALID_PARAMETER;
1412 }
1413 }
1414
1415 if ((resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] || resume_dmp)
1416 && ((mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED) ||
1417 (mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG))) {
1418 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1419 if (result) {
1420 LOG_RESULT_LOCATION(result);
1421 return result;
1422 }
1423 result = dmp_stop(mldl_cfg, gyro_handle);
1424 if (result) {
1425 LOG_RESULT_LOCATION(result);
1426 return result;
1427 }
1428 result = gyro_resume(mldl_cfg, gyro_handle, sensors);
1429 if (result) {
1430 LOG_RESULT_LOCATION(result);
1431 return result;
1432 }
1433 }
1434
1435 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1436 if (!mldl_cfg->slave[ii] ||
1437 !mldl_cfg->pdata_slave[ii] ||
1438 !resume_slave[ii] ||
1439 !(mldl_cfg->inv_mpu_state->status & (1 << ii)))
1440 continue;
1441
1442 if (EXT_SLAVE_BUS_SECONDARY ==
1443 mldl_cfg->pdata_slave[ii]->bus) {
1444 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle,
1445 true);
1446 if (result) {
1447 LOG_RESULT_LOCATION(result);
1448 return result;
1449 }
1450 }
1451 result = mldl_cfg->slave[ii]->resume(slave_handle[ii],
1452 mldl_cfg->slave[ii],
1453 mldl_cfg->pdata_slave[ii]);
1454 if (result) {
1455 LOG_RESULT_LOCATION(result);
1456 return result;
1457 }
1458 mldl_cfg->inv_mpu_state->status &= ~(1 << ii);
1459 }
1460
1461 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1462 if (resume_dmp &&
1463 !(mldl_cfg->inv_mpu_state->status & (1 << ii)) &&
1464 mldl_cfg->pdata_slave[ii] &&
1465 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata_slave[ii]->bus) {
1466 result = mpu_set_slave(mldl_cfg,
1467 gyro_handle,
1468 mldl_cfg->slave[ii],
1469 mldl_cfg->pdata_slave[ii],
1470 mldl_cfg->slave[ii]->type);
1471 if (result) {
1472 LOG_RESULT_LOCATION(result);
1473 return result;
1474 }
1475 }
1476 }
1477
1478 /* Turn on the master i2c iterface if necessary */
1479 if (resume_dmp) {
1480 result = mpu_set_i2c_bypass(
1481 mldl_cfg, gyro_handle,
1482 !(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
1483 if (result) {
1484 LOG_RESULT_LOCATION(result);
1485 return result;
1486 }
1487
1488 /* Now start */
1489 result = dmp_start(mldl_cfg, gyro_handle);
1490 if (result) {
1491 LOG_RESULT_LOCATION(result);
1492 return result;
1493 }
1494 }
1495 mldl_cfg->inv_mpu_cfg->requested_sensors = sensors;
1496
1497 return result;
1498}
1499
1500/**
1501 * @brief suspend the MPU device and all the other sensor
1502 * devices into their low power state.
1503 * @mldl_cfg
1504 * a pointer to the struct mldl_cfg internal data
1505 * structure.
1506 * @gyro_handle
1507 * the main file handle to the MPU device.
1508 * @accel_handle
1509 * an handle to the accelerometer device, if sitting
1510 * onto a separate bus. Can match gyro_handle if
1511 * the accelerometer device operates on the same
1512 * primary bus of MPU.
1513 * @compass_handle
1514 * an handle to the compass device, if sitting
1515 * onto a separate bus. Can match gyro_handle if
1516 * the compass device operates on the same
1517 * primary bus of MPU.
1518 * @pressure_handle
1519 * an handle to the pressure sensor device, if sitting
1520 * onto a separate bus. Can match gyro_handle if
1521 * the pressure sensor device operates on the same
1522 * primary bus of MPU.
1523 * @accel
1524 * whether suspending the accelerometer device is
1525 * actually needed (if the device supports low power
1526 * mode of some sort).
1527 * @compass
1528 * whether suspending the compass device is
1529 * actually needed (if the device supports low power
1530 * mode of some sort).
1531 * @pressure
1532 * whether suspending the pressure sensor device is
1533 * actually needed (if the device supports low power
1534 * mode of some sort).
1535 * @return INV_SUCCESS or a non-zero error code.
1536 */
1537int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
1538 void *gyro_handle,
1539 void *accel_handle,
1540 void *compass_handle,
1541 void *pressure_handle,
1542 unsigned long sensors)
1543{
1544 int result = INV_SUCCESS;
1545 int ii;
1546 struct ext_slave_descr **slave = mldl_cfg->slave;
1547 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
1548 bool suspend_dmp = ((sensors & INV_DMP_PROCESSOR) == INV_DMP_PROCESSOR);
1549 bool suspend_slave[EXT_SLAVE_NUM_TYPES];
1550 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1551
1552 suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
1553 ((sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO))
1554 == (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
1555 suspend_slave[EXT_SLAVE_TYPE_ACCEL] =
1556 ((sensors & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL);
1557 suspend_slave[EXT_SLAVE_TYPE_COMPASS] =
1558 ((sensors & INV_THREE_AXIS_COMPASS) == INV_THREE_AXIS_COMPASS);
1559 suspend_slave[EXT_SLAVE_TYPE_PRESSURE] =
1560 ((sensors & INV_THREE_AXIS_PRESSURE) ==
1561 INV_THREE_AXIS_PRESSURE);
1562
1563 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1564 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1565 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1566 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1567
1568 if (suspend_dmp) {
1569 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1570 if (result) {
1571 LOG_RESULT_LOCATION(result);
1572 return result;
1573 }
1574 result = dmp_stop(mldl_cfg, gyro_handle);
1575 if (result) {
1576 LOG_RESULT_LOCATION(result);
1577 return result;
1578 }
1579 }
1580
1581 /* Gyro */
1582 if (suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] &&
1583 !(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) {
1584 result = mpu3050_pwr_mgmt(
1585 mldl_cfg, gyro_handle, 0,
1586 suspend_dmp && suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE],
1587 (unsigned char)(sensors & INV_X_GYRO),
1588 (unsigned char)(sensors & INV_Y_GYRO),
1589 (unsigned char)(sensors & INV_Z_GYRO));
1590 if (result) {
1591 LOG_RESULT_LOCATION(result);
1592 return result;
1593 }
1594 }
1595
1596 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1597 bool is_suspended = mldl_cfg->inv_mpu_state->status & (1 << ii);
1598 if (!slave[ii] || !pdata_slave[ii] ||
1599 is_suspended || !suspend_slave[ii])
1600 continue;
1601
1602 if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
1603 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1604 if (result) {
1605 LOG_RESULT_LOCATION(result);
1606 return result;
1607 }
1608 }
1609 result = slave[ii]->suspend(slave_handle[ii],
1610 slave[ii],
1611 pdata_slave[ii]);
1612 if (result) {
1613 LOG_RESULT_LOCATION(result);
1614 return result;
1615 }
1616 if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
1617 result = mpu_set_slave(mldl_cfg, gyro_handle,
1618 NULL, NULL,
1619 slave[ii]->type);
1620 if (result) {
1621 LOG_RESULT_LOCATION(result);
1622 return result;
1623 }
1624 }
1625 mldl_cfg->inv_mpu_state->status |= (1 << ii);
1626 }
1627
1628 /* Re-enable the i2c master if there are configured slaves and DMP */
1629 if (!suspend_dmp) {
1630 result = mpu_set_i2c_bypass(
1631 mldl_cfg, gyro_handle,
1632 !(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
1633 if (result) {
1634 LOG_RESULT_LOCATION(result);
1635 return result;
1636 }
1637 }
1638 mldl_cfg->inv_mpu_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
1639
1640 return result;
1641}
1642
1643int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
1644 void *gyro_handle,
1645 void *slave_handle,
1646 struct ext_slave_descr *slave,
1647 struct ext_slave_platform_data *pdata,
1648 unsigned char *data)
1649{
1650 int result;
1651 int bypass_result;
1652 int remain_bypassed = true;
1653
1654 if (NULL == slave || NULL == slave->read) {
1655 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1656 return INV_ERROR_INVALID_CONFIGURATION;
1657 }
1658
1659 if ((EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1660 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1661 remain_bypassed = false;
1662 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1663 if (result) {
1664 LOG_RESULT_LOCATION(result);
1665 return result;
1666 }
1667 }
1668
1669 result = slave->read(slave_handle, slave, pdata, data);
1670
1671 if (!remain_bypassed) {
1672 bypass_result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1673 if (bypass_result) {
1674 LOG_RESULT_LOCATION(bypass_result);
1675 return bypass_result;
1676 }
1677 }
1678 return result;
1679}
1680
1681int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
1682 void *gyro_handle,
1683 void *slave_handle,
1684 struct ext_slave_config *data,
1685 struct ext_slave_descr *slave,
1686 struct ext_slave_platform_data *pdata)
1687{
1688 int result;
1689 int remain_bypassed = true;
1690
1691 if (NULL == slave || NULL == slave->config) {
1692 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1693 return INV_ERROR_INVALID_CONFIGURATION;
1694 }
1695
1696 if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1697 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1698 remain_bypassed = false;
1699 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1700 if (result) {
1701 LOG_RESULT_LOCATION(result);
1702 return result;
1703 }
1704 }
1705
1706 result = slave->config(slave_handle, slave, pdata, data);
1707 if (result) {
1708 LOG_RESULT_LOCATION(result);
1709 return result;
1710 }
1711
1712 if (!remain_bypassed) {
1713 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1714 if (result) {
1715 LOG_RESULT_LOCATION(result);
1716 return result;
1717 }
1718 }
1719 return result;
1720}
1721
1722int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
1723 void *gyro_handle,
1724 void *slave_handle,
1725 struct ext_slave_config *data,
1726 struct ext_slave_descr *slave,
1727 struct ext_slave_platform_data *pdata)
1728{
1729 int result;
1730 int remain_bypassed = true;
1731
1732 if (NULL == slave || NULL == slave->get_config) {
1733 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1734 return INV_ERROR_INVALID_CONFIGURATION;
1735 }
1736
1737 if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1738 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1739 remain_bypassed = false;
1740 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1741 if (result) {
1742 LOG_RESULT_LOCATION(result);
1743 return result;
1744 }
1745 }
1746
1747 result = slave->get_config(slave_handle, slave, pdata, data);
1748 if (result) {
1749 LOG_RESULT_LOCATION(result);
1750 return result;
1751 }
1752
1753 if (!remain_bypassed) {
1754 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1755 if (result) {
1756 LOG_RESULT_LOCATION(result);
1757 return result;
1758 }
1759 }
1760 return result;
1761}
1762
1763/**
1764 * @}
1765 */
diff --git a/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c b/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c
new file mode 100644
index 00000000000..e2b8d30ceba
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c
@@ -0,0 +1,137 @@
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
79 MPL_LOGV("requested_sensors = %04x\n", inv_mpu_cfg->requested_sensors);
80 MPL_LOGV("ignore_system_suspend= %04x\n",
81 inv_mpu_cfg->ignore_system_suspend);
82 MPL_LOGV("status = %04x\n", inv_mpu_state->status);
83 MPL_LOGV("i2c_slaves_enabled= %04x\n",
84 inv_mpu_state->i2c_slaves_enabled);
85
86 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
87 if (!slave[ii])
88 continue;
89 MPL_LOGV("SLAVE %d:\n", ii);
90 MPL_LOGV(" suspend = %02x\n", (int)slave[ii]->suspend);
91 MPL_LOGV(" resume = %02x\n", (int)slave[ii]->resume);
92 MPL_LOGV(" read = %02x\n", (int)slave[ii]->read);
93 MPL_LOGV(" type = %02x\n", slave[ii]->type);
94 MPL_LOGV(" reg = %02x\n", slave[ii]->read_reg);
95 MPL_LOGV(" len = %02x\n", slave[ii]->read_len);
96 MPL_LOGV(" endian = %02x\n", slave[ii]->endian);
97 MPL_LOGV(" range.mantissa= %02x\n",
98 slave[ii]->range.mantissa);
99 MPL_LOGV(" range.fraction= %02x\n",
100 slave[ii]->range.fraction);
101 }
102
103 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
104 if (!pdata_slave[ii])
105 continue;
106 MPL_LOGV("PDATA_SLAVE[%d]\n", ii);
107 MPL_LOGV(" irq = %02x\n", pdata_slave[ii]->irq);
108 MPL_LOGV(" adapt_num = %02x\n", pdata_slave[ii]->adapt_num);
109 MPL_LOGV(" bus = %02x\n", pdata_slave[ii]->bus);
110 MPL_LOGV(" address = %02x\n", pdata_slave[ii]->address);
111 MPL_LOGV(" orientation=\n"
112 " %2d %2d %2d\n"
113 " %2d %2d %2d\n"
114 " %2d %2d %2d\n",
115 pdata_slave[ii]->orientation[0],
116 pdata_slave[ii]->orientation[1],
117 pdata_slave[ii]->orientation[2],
118 pdata_slave[ii]->orientation[3],
119 pdata_slave[ii]->orientation[4],
120 pdata_slave[ii]->orientation[5],
121 pdata_slave[ii]->orientation[6],
122 pdata_slave[ii]->orientation[7],
123 pdata_slave[ii]->orientation[8]);
124 }
125
126 MPL_LOGV("pdata->int_config = %02x\n", pdata->int_config);
127 MPL_LOGV("pdata->level_shifter = %02x\n", pdata->level_shifter);
128 MPL_LOGV("pdata->orientation =\n"
129 " %2d %2d %2d\n"
130 " %2d %2d %2d\n"
131 " %2d %2d %2d\n",
132 pdata->orientation[0], pdata->orientation[1],
133 pdata->orientation[2], pdata->orientation[3],
134 pdata->orientation[4], pdata->orientation[5],
135 pdata->orientation[6], pdata->orientation[7],
136 pdata->orientation[8]);
137}
diff --git a/drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c b/drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c
new file mode 100644
index 00000000000..19adf5182c0
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/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 "mpu3050.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/mpu3050/mpu-dev.c b/drivers/misc/inv_mpu/mpu3050/mpu-dev.c
new file mode 100644
index 00000000000..c98ed3b4a80
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mpu-dev.c
@@ -0,0 +1,1250 @@
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
53/* Platform data for the MPU */
54struct mpu_private_data {
55 struct miscdevice dev;
56 struct i2c_client *client;
57
58 /* mldl_cfg data */
59 struct mldl_cfg mldl_cfg;
60 struct mpu_ram mpu_ram;
61 struct mpu_gyro_cfg mpu_gyro_cfg;
62 struct mpu_offsets mpu_offsets;
63 struct mpu_chip_info mpu_chip_info;
64 struct inv_mpu_cfg inv_mpu_cfg;
65 struct inv_mpu_state inv_mpu_state;
66
67 struct mutex mutex;
68 wait_queue_head_t mpu_event_wait;
69 struct completion completion;
70 struct timer_list timeout;
71 struct notifier_block nb;
72 struct mpuirq_data mpu_pm_event;
73 int response_timeout; /* In seconds */
74 unsigned long event;
75 int pid;
76 struct module *slave_modules[EXT_SLAVE_NUM_TYPES];
77};
78
79struct mpu_private_data *mpu_private_data;
80
81static void mpu_pm_timeout(u_long data)
82{
83 struct mpu_private_data *mpu = (struct mpu_private_data *)data;
84 struct i2c_client *client = mpu->client;
85 dev_dbg(&client->adapter->dev, "%s\n", __func__);
86 complete(&mpu->completion);
87}
88
89static int mpu_pm_notifier_callback(struct notifier_block *nb,
90 unsigned long event, void *unused)
91{
92 struct mpu_private_data *mpu =
93 container_of(nb, struct mpu_private_data, nb);
94 struct i2c_client *client = mpu->client;
95 struct timeval event_time;
96 dev_dbg(&client->adapter->dev, "%s: %ld\n", __func__, event);
97
98 /* Prevent the file handle from being closed before we initialize
99 the completion event */
100 mutex_lock(&mpu->mutex);
101 if (!(mpu->pid) ||
102 (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
103 mutex_unlock(&mpu->mutex);
104 return NOTIFY_OK;
105 }
106
107 if (event == PM_SUSPEND_PREPARE)
108 mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
109 if (event == PM_POST_SUSPEND)
110 mpu->event = MPU_PM_EVENT_POST_SUSPEND;
111
112 do_gettimeofday(&event_time);
113 mpu->mpu_pm_event.interruptcount++;
114 mpu->mpu_pm_event.irqtime =
115 (((long long)event_time.tv_sec) << 32) + event_time.tv_usec;
116 mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
117 mpu->mpu_pm_event.data = mpu->event;
118
119 if (mpu->response_timeout > 0) {
120 mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
121 add_timer(&mpu->timeout);
122 }
123 INIT_COMPLETION(mpu->completion);
124 mutex_unlock(&mpu->mutex);
125
126 wake_up_interruptible(&mpu->mpu_event_wait);
127 wait_for_completion(&mpu->completion);
128 del_timer_sync(&mpu->timeout);
129 dev_dbg(&client->adapter->dev, "%s: %ld DONE\n", __func__, event);
130 return NOTIFY_OK;
131}
132
133static int mpu_dev_open(struct inode *inode, struct file *file)
134{
135 struct mpu_private_data *mpu =
136 container_of(file->private_data, struct mpu_private_data, dev);
137 struct i2c_client *client = mpu->client;
138 int result;
139 int ii;
140 dev_dbg(&client->adapter->dev, "%s\n", __func__);
141 dev_dbg(&client->adapter->dev, "current->pid %d\n", current->pid);
142
143 result = mutex_lock_interruptible(&mpu->mutex);
144 if (mpu->pid) {
145 mutex_unlock(&mpu->mutex);
146 return -EBUSY;
147 }
148 mpu->pid = current->pid;
149
150 /* Reset the sensors to the default */
151 if (result) {
152 dev_err(&client->adapter->dev,
153 "%s: mutex_lock_interruptible returned %d\n",
154 __func__, result);
155 return result;
156 }
157
158 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
159 __module_get(mpu->slave_modules[ii]);
160
161 mutex_unlock(&mpu->mutex);
162 return 0;
163}
164
165/* close function - called when the "file" /dev/mpu is closed in userspace */
166static int mpu_release(struct inode *inode, struct file *file)
167{
168 struct mpu_private_data *mpu =
169 container_of(file->private_data, struct mpu_private_data, dev);
170 struct i2c_client *client = mpu->client;
171 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
172 int result = 0;
173 int ii;
174 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
175 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
176
177 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
178 if (!pdata_slave[ii])
179 slave_adapter[ii] = NULL;
180 else
181 slave_adapter[ii] =
182 i2c_get_adapter(pdata_slave[ii]->adapt_num);
183 }
184 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
185
186 mutex_lock(&mpu->mutex);
187 mldl_cfg->inv_mpu_cfg->requested_sensors = 0;
188 result = inv_mpu_suspend(mldl_cfg,
189 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
190 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
191 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
192 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
193 INV_ALL_SENSORS);
194 mpu->pid = 0;
195 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
196 module_put(mpu->slave_modules[ii]);
197
198 mutex_unlock(&mpu->mutex);
199 complete(&mpu->completion);
200 dev_dbg(&client->adapter->dev, "mpu_release\n");
201
202 return result;
203}
204
205/* read function called when from /dev/mpu is read. Read from the FIFO */
206static ssize_t mpu_read(struct file *file,
207 char __user *buf, size_t count, loff_t *offset)
208{
209 struct mpu_private_data *mpu =
210 container_of(file->private_data, struct mpu_private_data, dev);
211 struct i2c_client *client = mpu->client;
212 size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
213 int err;
214
215 if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
216 wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
217
218 if (!mpu->event || !buf
219 || count < sizeof(mpu->mpu_pm_event))
220 return 0;
221
222 err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
223 if (err) {
224 dev_err(&client->adapter->dev,
225 "Copy to user returned %d\n", err);
226 return -EFAULT;
227 }
228 mpu->event = 0;
229 return len;
230}
231
232static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
233{
234 struct mpu_private_data *mpu =
235 container_of(file->private_data, struct mpu_private_data, dev);
236 int mask = 0;
237
238 poll_wait(file, &mpu->mpu_event_wait, poll);
239 if (mpu->event)
240 mask |= POLLIN | POLLRDNORM;
241 return mask;
242}
243
244static int mpu_dev_ioctl_get_ext_slave_platform_data(
245 struct i2c_client *client,
246 struct ext_slave_platform_data __user *arg)
247{
248 struct mpu_private_data *mpu =
249 (struct mpu_private_data *)i2c_get_clientdata(client);
250 struct ext_slave_platform_data *pdata_slave;
251 struct ext_slave_platform_data local_pdata_slave;
252
253 if (copy_from_user(&local_pdata_slave, arg, sizeof(local_pdata_slave)))
254 return -EFAULT;
255
256 if (local_pdata_slave.type >= EXT_SLAVE_NUM_TYPES)
257 return -EINVAL;
258
259 pdata_slave = mpu->mldl_cfg.pdata_slave[local_pdata_slave.type];
260 /* All but private data and irq_data */
261 if (!pdata_slave)
262 return -ENODEV;
263 if (copy_to_user(arg, pdata_slave, sizeof(*pdata_slave)))
264 return -EFAULT;
265 return 0;
266}
267
268static int mpu_dev_ioctl_get_mpu_platform_data(
269 struct i2c_client *client,
270 struct mpu_platform_data __user *arg)
271{
272 struct mpu_private_data *mpu =
273 (struct mpu_private_data *)i2c_get_clientdata(client);
274 struct mpu_platform_data *pdata = mpu->mldl_cfg.pdata;
275
276 if (copy_to_user(arg, pdata, sizeof(*pdata)))
277 return -EFAULT;
278 return 0;
279}
280
281static int mpu_dev_ioctl_get_ext_slave_descr(
282 struct i2c_client *client,
283 struct ext_slave_descr __user *arg)
284{
285 struct mpu_private_data *mpu =
286 (struct mpu_private_data *)i2c_get_clientdata(client);
287 struct ext_slave_descr *slave;
288 struct ext_slave_descr local_slave;
289
290 if (copy_from_user(&local_slave, arg, sizeof(local_slave)))
291 return -EFAULT;
292
293 if (local_slave.type >= EXT_SLAVE_NUM_TYPES)
294 return -EINVAL;
295
296 slave = mpu->mldl_cfg.slave[local_slave.type];
297 /* All but private data and irq_data */
298 if (!slave)
299 return -ENODEV;
300 if (copy_to_user(arg, slave, sizeof(*slave)))
301 return -EFAULT;
302 return 0;
303}
304
305
306/**
307 * slave_config() - Pass a requested slave configuration to the slave sensor
308 *
309 * @adapter the adaptor to use to communicate with the slave
310 * @mldl_cfg the mldl configuration structuer
311 * @slave pointer to the slave descriptor
312 * @usr_config The configuration to pass to the slave sensor
313 *
314 * returns 0 or non-zero error code
315 */
316static int inv_mpu_config(struct mldl_cfg *mldl_cfg,
317 void *gyro_adapter,
318 struct ext_slave_config __user *usr_config)
319{
320 int retval = 0;
321 struct ext_slave_config config;
322
323 retval = copy_from_user(&config, usr_config, sizeof(config));
324 if (retval)
325 return -EFAULT;
326
327 if (config.len && config.data) {
328 void *data;
329 data = kmalloc(config.len, GFP_KERNEL);
330 if (!data)
331 return -ENOMEM;
332
333 retval = copy_from_user(data,
334 (void __user *)config.data, config.len);
335 if (retval) {
336 retval = -EFAULT;
337 kfree(data);
338 return retval;
339 }
340 config.data = data;
341 }
342 retval = gyro_config(gyro_adapter, mldl_cfg, &config);
343 kfree(config.data);
344 return retval;
345}
346
347static int inv_mpu_get_config(struct mldl_cfg *mldl_cfg,
348 void *gyro_adapter,
349 struct ext_slave_config __user *usr_config)
350{
351 int retval = 0;
352 struct ext_slave_config config;
353 void *user_data;
354
355 retval = copy_from_user(&config, usr_config, sizeof(config));
356 if (retval)
357 return -EFAULT;
358
359 user_data = config.data;
360 if (config.len && config.data) {
361 void *data;
362 data = kmalloc(config.len, GFP_KERNEL);
363 if (!data)
364 return -ENOMEM;
365
366 retval = copy_from_user(data,
367 (void __user *)config.data, config.len);
368 if (retval) {
369 retval = -EFAULT;
370 kfree(data);
371 return retval;
372 }
373 config.data = data;
374 }
375 retval = gyro_get_config(gyro_adapter, mldl_cfg, &config);
376 if (!retval)
377 retval = copy_to_user((unsigned char __user *)user_data,
378 config.data, config.len);
379 kfree(config.data);
380 return retval;
381}
382
383static int slave_config(struct mldl_cfg *mldl_cfg,
384 void *gyro_adapter,
385 void *slave_adapter,
386 struct ext_slave_descr *slave,
387 struct ext_slave_platform_data *pdata,
388 struct ext_slave_config __user *usr_config)
389{
390 int retval = 0;
391 struct ext_slave_config config;
392 if ((!slave) || (!slave->config))
393 return -ENODEV;
394
395 retval = copy_from_user(&config, usr_config, sizeof(config));
396 if (retval)
397 return -EFAULT;
398
399 if (config.len && config.data) {
400 void *data;
401 data = kmalloc(config.len, GFP_KERNEL);
402 if (!data)
403 return -ENOMEM;
404
405 retval = copy_from_user(data,
406 (void __user *)config.data, config.len);
407 if (retval) {
408 retval = -EFAULT;
409 kfree(data);
410 return retval;
411 }
412 config.data = data;
413 }
414 retval = inv_mpu_slave_config(mldl_cfg, gyro_adapter, slave_adapter,
415 &config, slave, pdata);
416 kfree(config.data);
417 return retval;
418}
419
420static int slave_get_config(struct mldl_cfg *mldl_cfg,
421 void *gyro_adapter,
422 void *slave_adapter,
423 struct ext_slave_descr *slave,
424 struct ext_slave_platform_data *pdata,
425 struct ext_slave_config __user *usr_config)
426{
427 int retval = 0;
428 struct ext_slave_config config;
429 void *user_data;
430 if (!(slave) || !(slave->get_config))
431 return -ENODEV;
432
433 retval = copy_from_user(&config, usr_config, sizeof(config));
434 if (retval)
435 return -EFAULT;
436
437 user_data = config.data;
438 if (config.len && config.data) {
439 void *data;
440 data = kmalloc(config.len, GFP_KERNEL);
441 if (!data)
442 return -ENOMEM;
443
444 retval = copy_from_user(data,
445 (void __user *)config.data, config.len);
446 if (retval) {
447 retval = -EFAULT;
448 kfree(data);
449 return retval;
450 }
451 config.data = data;
452 }
453 retval = inv_mpu_get_slave_config(mldl_cfg, gyro_adapter,
454 slave_adapter, &config, slave, pdata);
455 if (retval) {
456 kfree(config.data);
457 return retval;
458 }
459 retval = copy_to_user((unsigned char __user *)user_data,
460 config.data, config.len);
461 kfree(config.data);
462 return retval;
463}
464
465static int inv_slave_read(struct mldl_cfg *mldl_cfg,
466 void *gyro_adapter,
467 void *slave_adapter,
468 struct ext_slave_descr *slave,
469 struct ext_slave_platform_data *pdata,
470 void __user *usr_data)
471{
472 int retval;
473 unsigned char *data;
474 data = kzalloc(slave->read_len, GFP_KERNEL);
475 if (!data)
476 return -EFAULT;
477
478 retval = inv_mpu_slave_read(mldl_cfg, gyro_adapter, slave_adapter,
479 slave, pdata, data);
480
481 if ((!retval) &&
482 (copy_to_user((unsigned char __user *)usr_data,
483 data, slave->read_len)))
484 retval = -EFAULT;
485
486 kfree(data);
487 return retval;
488}
489
490static int mpu_handle_mlsl(void *sl_handle,
491 unsigned char addr,
492 unsigned int cmd,
493 struct mpu_read_write __user *usr_msg)
494{
495 int retval = 0;
496 struct mpu_read_write msg;
497 unsigned char *user_data;
498 retval = copy_from_user(&msg, usr_msg, sizeof(msg));
499 if (retval)
500 return -EFAULT;
501
502 user_data = msg.data;
503 if (msg.length && msg.data) {
504 unsigned char *data;
505 data = kmalloc(msg.length, GFP_KERNEL);
506 if (!data)
507 return -ENOMEM;
508
509 retval = copy_from_user(data,
510 (void __user *)msg.data, msg.length);
511 if (retval) {
512 retval = -EFAULT;
513 kfree(data);
514 return retval;
515 }
516 msg.data = data;
517 } else {
518 return -EPERM;
519 }
520
521 switch (cmd) {
522 case MPU_READ:
523 retval = inv_serial_read(sl_handle, addr,
524 msg.address, msg.length, msg.data);
525 break;
526 case MPU_WRITE:
527 retval = inv_serial_write(sl_handle, addr,
528 msg.length, msg.data);
529 break;
530 case MPU_READ_MEM:
531 retval = inv_serial_read_mem(sl_handle, addr,
532 msg.address, msg.length, msg.data);
533 break;
534 case MPU_WRITE_MEM:
535 retval = inv_serial_write_mem(sl_handle, addr,
536 msg.address, msg.length,
537 msg.data);
538 break;
539 case MPU_READ_FIFO:
540 retval = inv_serial_read_fifo(sl_handle, addr,
541 msg.length, msg.data);
542 break;
543 case MPU_WRITE_FIFO:
544 retval = inv_serial_write_fifo(sl_handle, addr,
545 msg.length, msg.data);
546 break;
547
548 };
549 if (retval) {
550 dev_err(&((struct i2c_adapter *)sl_handle)->dev,
551 "%s: i2c %d error %d\n",
552 __func__, cmd, retval);
553 kfree(msg.data);
554 return retval;
555 }
556 retval = copy_to_user((unsigned char __user *)user_data,
557 msg.data, msg.length);
558 kfree(msg.data);
559 return retval;
560}
561
562/* ioctl - I/O control */
563static long mpu_dev_ioctl(struct file *file,
564 unsigned int cmd, unsigned long arg)
565{
566 struct mpu_private_data *mpu =
567 container_of(file->private_data, struct mpu_private_data, dev);
568 struct i2c_client *client = mpu->client;
569 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
570 int retval = 0;
571 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
572 struct ext_slave_descr **slave = mldl_cfg->slave;
573 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
574 int ii;
575
576 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
577 if (!pdata_slave[ii])
578 slave_adapter[ii] = NULL;
579 else
580 slave_adapter[ii] =
581 i2c_get_adapter(pdata_slave[ii]->adapt_num);
582 }
583 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
584
585 retval = mutex_lock_interruptible(&mpu->mutex);
586 if (retval) {
587 dev_err(&client->adapter->dev,
588 "%s: mutex_lock_interruptible returned %d\n",
589 __func__, retval);
590 return retval;
591 }
592
593 switch (cmd) {
594 case MPU_GET_EXT_SLAVE_PLATFORM_DATA:
595 retval = mpu_dev_ioctl_get_ext_slave_platform_data(
596 client,
597 (struct ext_slave_platform_data __user *)arg);
598 break;
599 case MPU_GET_MPU_PLATFORM_DATA:
600 retval = mpu_dev_ioctl_get_mpu_platform_data(
601 client,
602 (struct mpu_platform_data __user *)arg);
603 break;
604 case MPU_GET_EXT_SLAVE_DESCR:
605 retval = mpu_dev_ioctl_get_ext_slave_descr(
606 client,
607 (struct ext_slave_descr __user *)arg);
608 break;
609 case MPU_READ:
610 case MPU_WRITE:
611 case MPU_READ_MEM:
612 case MPU_WRITE_MEM:
613 case MPU_READ_FIFO:
614 case MPU_WRITE_FIFO:
615 retval = mpu_handle_mlsl(
616 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
617 mldl_cfg->mpu_chip_info->addr, cmd,
618 (struct mpu_read_write __user *)arg);
619 break;
620 case MPU_CONFIG_GYRO:
621 retval = inv_mpu_config(
622 mldl_cfg,
623 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
624 (struct ext_slave_config __user *)arg);
625 break;
626 case MPU_CONFIG_ACCEL:
627 retval = slave_config(
628 mldl_cfg,
629 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
630 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
631 slave[EXT_SLAVE_TYPE_ACCEL],
632 pdata_slave[EXT_SLAVE_TYPE_ACCEL],
633 (struct ext_slave_config __user *)arg);
634 break;
635 case MPU_CONFIG_COMPASS:
636 retval = slave_config(
637 mldl_cfg,
638 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
639 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
640 slave[EXT_SLAVE_TYPE_COMPASS],
641 pdata_slave[EXT_SLAVE_TYPE_COMPASS],
642 (struct ext_slave_config __user *)arg);
643 break;
644 case MPU_CONFIG_PRESSURE:
645 retval = slave_config(
646 mldl_cfg,
647 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
648 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
649 slave[EXT_SLAVE_TYPE_PRESSURE],
650 pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
651 (struct ext_slave_config __user *)arg);
652 break;
653 case MPU_GET_CONFIG_GYRO:
654 retval = inv_mpu_get_config(
655 mldl_cfg,
656 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
657 (struct ext_slave_config __user *)arg);
658 break;
659 case MPU_GET_CONFIG_ACCEL:
660 retval = slave_get_config(
661 mldl_cfg,
662 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
663 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
664 slave[EXT_SLAVE_TYPE_ACCEL],
665 pdata_slave[EXT_SLAVE_TYPE_ACCEL],
666 (struct ext_slave_config __user *)arg);
667 break;
668 case MPU_GET_CONFIG_COMPASS:
669 retval = slave_get_config(
670 mldl_cfg,
671 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
672 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
673 slave[EXT_SLAVE_TYPE_COMPASS],
674 pdata_slave[EXT_SLAVE_TYPE_COMPASS],
675 (struct ext_slave_config __user *)arg);
676 break;
677 case MPU_GET_CONFIG_PRESSURE:
678 retval = slave_get_config(
679 mldl_cfg,
680 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
681 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
682 slave[EXT_SLAVE_TYPE_PRESSURE],
683 pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
684 (struct ext_slave_config __user *)arg);
685 break;
686 case MPU_SUSPEND:
687 retval = inv_mpu_suspend(
688 mldl_cfg,
689 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
690 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
691 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
692 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
693 arg);
694 break;
695 case MPU_RESUME:
696 retval = inv_mpu_resume(
697 mldl_cfg,
698 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
699 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
700 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
701 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
702 arg);
703 break;
704 case MPU_PM_EVENT_HANDLED:
705 dev_dbg(&client->adapter->dev, "%s: %d\n", __func__, cmd);
706 complete(&mpu->completion);
707 break;
708 case MPU_READ_ACCEL:
709 retval = inv_slave_read(
710 mldl_cfg,
711 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
712 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
713 slave[EXT_SLAVE_TYPE_ACCEL],
714 pdata_slave[EXT_SLAVE_TYPE_ACCEL],
715 (unsigned char __user *)arg);
716 break;
717 case MPU_READ_COMPASS:
718 retval = inv_slave_read(
719 mldl_cfg,
720 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
721 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
722 slave[EXT_SLAVE_TYPE_COMPASS],
723 pdata_slave[EXT_SLAVE_TYPE_COMPASS],
724 (unsigned char __user *)arg);
725 break;
726 case MPU_READ_PRESSURE:
727 retval = inv_slave_read(
728 mldl_cfg,
729 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
730 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
731 slave[EXT_SLAVE_TYPE_PRESSURE],
732 pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
733 (unsigned char __user *)arg);
734 break;
735 case MPU_GET_REQUESTED_SENSORS:
736 if (copy_to_user(
737 (__u32 __user *)arg,
738 &mldl_cfg->inv_mpu_cfg->requested_sensors,
739 sizeof(mldl_cfg->inv_mpu_cfg->requested_sensors)))
740 retval = -EFAULT;
741 break;
742 case MPU_SET_REQUESTED_SENSORS:
743 mldl_cfg->inv_mpu_cfg->requested_sensors = arg;
744 break;
745 case MPU_GET_IGNORE_SYSTEM_SUSPEND:
746 if (copy_to_user(
747 (unsigned char __user *)arg,
748 &mldl_cfg->inv_mpu_cfg->ignore_system_suspend,
749 sizeof(mldl_cfg->inv_mpu_cfg->ignore_system_suspend)))
750 retval = -EFAULT;
751 break;
752 case MPU_SET_IGNORE_SYSTEM_SUSPEND:
753 mldl_cfg->inv_mpu_cfg->ignore_system_suspend = arg;
754 break;
755 case MPU_GET_MLDL_STATUS:
756 if (copy_to_user(
757 (unsigned char __user *)arg,
758 &mldl_cfg->inv_mpu_state->status,
759 sizeof(mldl_cfg->inv_mpu_state->status)))
760 retval = -EFAULT;
761 break;
762 case MPU_GET_I2C_SLAVES_ENABLED:
763 if (copy_to_user(
764 (unsigned char __user *)arg,
765 &mldl_cfg->inv_mpu_state->i2c_slaves_enabled,
766 sizeof(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)))
767 retval = -EFAULT;
768 break;
769 default:
770 dev_err(&client->adapter->dev,
771 "%s: Unknown cmd %x, arg %lu\n",
772 __func__, cmd, arg);
773 retval = -EINVAL;
774 };
775
776 mutex_unlock(&mpu->mutex);
777 dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d\n",
778 __func__, cmd, arg, retval);
779
780 if (retval > 0)
781 retval = -retval;
782
783 return retval;
784}
785
786void mpu_shutdown(struct i2c_client *client)
787{
788 struct mpu_private_data *mpu =
789 (struct mpu_private_data *)i2c_get_clientdata(client);
790 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
791 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
792 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
793 int ii;
794
795 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
796 if (!pdata_slave[ii])
797 slave_adapter[ii] = NULL;
798 else
799 slave_adapter[ii] =
800 i2c_get_adapter(pdata_slave[ii]->adapt_num);
801 }
802 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
803
804 mutex_lock(&mpu->mutex);
805 (void)inv_mpu_suspend(mldl_cfg,
806 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
807 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
808 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
809 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
810 INV_ALL_SENSORS);
811 mutex_unlock(&mpu->mutex);
812 dev_dbg(&client->adapter->dev, "%s\n", __func__);
813}
814
815int mpu_dev_suspend(struct i2c_client *client, pm_message_t mesg)
816{
817 struct mpu_private_data *mpu =
818 (struct mpu_private_data *)i2c_get_clientdata(client);
819 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
820 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
821 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
822 int ii;
823
824 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
825 if (!pdata_slave[ii])
826 slave_adapter[ii] = NULL;
827 else
828 slave_adapter[ii] =
829 i2c_get_adapter(pdata_slave[ii]->adapt_num);
830 }
831 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
832
833 mutex_lock(&mpu->mutex);
834 if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
835 dev_dbg(&client->adapter->dev,
836 "%s: suspending on event %d\n", __func__, mesg.event);
837 (void)inv_mpu_suspend(mldl_cfg,
838 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
839 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
840 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
841 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
842 INV_ALL_SENSORS);
843 } else {
844 dev_dbg(&client->adapter->dev,
845 "%s: Already suspended %d\n", __func__, mesg.event);
846 }
847 mutex_unlock(&mpu->mutex);
848 return 0;
849}
850
851int mpu_dev_resume(struct i2c_client *client)
852{
853 struct mpu_private_data *mpu =
854 (struct mpu_private_data *)i2c_get_clientdata(client);
855 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
856 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
857 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
858 int ii;
859
860 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
861 if (!pdata_slave[ii])
862 slave_adapter[ii] = NULL;
863 else
864 slave_adapter[ii] =
865 i2c_get_adapter(pdata_slave[ii]->adapt_num);
866 }
867 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
868
869 mutex_lock(&mpu->mutex);
870 if (mpu->pid && !mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
871 (void)inv_mpu_resume(mldl_cfg,
872 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
873 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
874 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
875 slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
876 mldl_cfg->inv_mpu_cfg->requested_sensors);
877 dev_dbg(&client->adapter->dev,
878 "%s for pid %d\n", __func__, mpu->pid);
879 }
880 mutex_unlock(&mpu->mutex);
881 return 0;
882}
883
884/* define which file operations are supported */
885static const struct file_operations mpu_fops = {
886 .owner = THIS_MODULE,
887 .read = mpu_read,
888 .poll = mpu_poll,
889 .unlocked_ioctl = mpu_dev_ioctl,
890 .open = mpu_dev_open,
891 .release = mpu_release,
892};
893
894int inv_mpu_register_slave(struct module *slave_module,
895 struct i2c_client *slave_client,
896 struct ext_slave_platform_data *slave_pdata,
897 struct ext_slave_descr *(*get_slave_descr)(void))
898{
899 struct mpu_private_data *mpu = mpu_private_data;
900 struct mldl_cfg *mldl_cfg;
901 struct ext_slave_descr *slave_descr;
902 struct ext_slave_platform_data **pdata_slave;
903 char *irq_name = NULL;
904 int result = 0;
905
906 if (!slave_client || !slave_pdata || !get_slave_descr)
907 return -EINVAL;
908
909 if (!mpu) {
910 dev_err(&slave_client->adapter->dev,
911 "%s: Null mpu_private_data\n", __func__);
912 return -EINVAL;
913 }
914 mldl_cfg = &mpu->mldl_cfg;
915 pdata_slave = mldl_cfg->pdata_slave;
916 slave_descr = get_slave_descr();
917
918 if (!slave_descr) {
919 dev_err(&slave_client->adapter->dev,
920 "%s: Null ext_slave_descr\n", __func__);
921 return -EINVAL;
922 }
923
924 mutex_lock(&mpu->mutex);
925 if (mpu->pid) {
926 mutex_unlock(&mpu->mutex);
927 return -EBUSY;
928 }
929
930 if (pdata_slave[slave_descr->type]) {
931 result = -EBUSY;
932 goto out_unlock_mutex;
933 }
934
935 slave_pdata->address = slave_client->addr;
936 slave_pdata->irq = slave_client->irq;
937 slave_pdata->adapt_num = i2c_adapter_id(slave_client->adapter);
938
939 dev_info(&slave_client->adapter->dev,
940 "%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
941 __func__,
942 slave_descr->name,
943 slave_descr->type,
944 slave_pdata->address,
945 slave_pdata->irq,
946 slave_pdata->adapt_num);
947
948 switch (slave_descr->type) {
949 case EXT_SLAVE_TYPE_ACCEL:
950 irq_name = "accelirq";
951 break;
952 case EXT_SLAVE_TYPE_COMPASS:
953 irq_name = "compassirq";
954 break;
955 case EXT_SLAVE_TYPE_PRESSURE:
956 irq_name = "pressureirq";
957 break;
958 default:
959 irq_name = "none";
960 };
961 if (slave_descr->init) {
962 result = slave_descr->init(slave_client->adapter,
963 slave_descr,
964 slave_pdata);
965 if (result) {
966 dev_err(&slave_client->adapter->dev,
967 "%s init failed %d\n",
968 slave_descr->name, result);
969 goto out_unlock_mutex;
970 }
971 }
972
973 pdata_slave[slave_descr->type] = slave_pdata;
974 mpu->slave_modules[slave_descr->type] = slave_module;
975 mldl_cfg->slave[slave_descr->type] = slave_descr;
976
977 goto out_unlock_mutex;
978
979out_unlock_mutex:
980 mutex_unlock(&mpu->mutex);
981
982 if (!result && irq_name && (slave_pdata->irq > 0)) {
983 int warn_result;
984 dev_info(&slave_client->adapter->dev,
985 "Installing %s irq using %d\n",
986 irq_name,
987 slave_pdata->irq);
988 warn_result = slaveirq_init(slave_client->adapter,
989 slave_pdata, irq_name);
990 if (result)
991 dev_WARN(&slave_client->adapter->dev,
992 "%s irq assigned error: %d\n",
993 slave_descr->name, warn_result);
994 } else {
995 dev_info(&slave_client->adapter->dev,
996 "%s irq not assigned: %d %d %d\n",
997 slave_descr->name,
998 result, (int)irq_name, slave_pdata->irq);
999 }
1000
1001 return result;
1002}
1003EXPORT_SYMBOL(inv_mpu_register_slave);
1004
1005void inv_mpu_unregister_slave(struct i2c_client *slave_client,
1006 struct ext_slave_platform_data *slave_pdata,
1007 struct ext_slave_descr *(*get_slave_descr)(void))
1008{
1009 struct mpu_private_data *mpu = mpu_private_data;
1010 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
1011 struct ext_slave_descr *slave_descr;
1012 int result;
1013
1014 dev_info(&slave_client->adapter->dev, "%s\n", __func__);
1015
1016 if (!slave_client || !slave_pdata || !get_slave_descr)
1017 return;
1018
1019 if (slave_pdata->irq)
1020 slaveirq_exit(slave_pdata);
1021
1022 slave_descr = get_slave_descr();
1023 if (!slave_descr)
1024 return;
1025
1026 mutex_lock(&mpu->mutex);
1027
1028 if (slave_descr->exit) {
1029 result = slave_descr->exit(slave_client->adapter,
1030 slave_descr,
1031 slave_pdata);
1032 if (result)
1033 dev_err(&slave_client->adapter->dev,
1034 "Accel exit failed %d\n", result);
1035 }
1036 mldl_cfg->slave[slave_descr->type] = NULL;
1037 mldl_cfg->pdata_slave[slave_descr->type] = NULL;
1038 mpu->slave_modules[slave_descr->type] = NULL;
1039
1040 mutex_unlock(&mpu->mutex);
1041
1042}
1043EXPORT_SYMBOL(inv_mpu_unregister_slave);
1044
1045static unsigned short normal_i2c[] = { I2C_CLIENT_END };
1046
1047static const struct i2c_device_id mpu_id[] = {
1048 {"mpu3050", 0},
1049 {"mpu6050", 0},
1050 {"mpu6050_no_accel", 0},
1051 {}
1052};
1053MODULE_DEVICE_TABLE(i2c, mpu_id);
1054
1055int mpu_probe(struct i2c_client *client, const struct i2c_device_id *devid)
1056{
1057 struct mpu_platform_data *pdata;
1058 struct mpu_private_data *mpu;
1059 struct mldl_cfg *mldl_cfg;
1060 int res = 0;
1061 int ii = 0;
1062 unsigned long irq_flags;
1063
1064 dev_info(&client->adapter->dev, "%s: %d\n", __func__, ii++);
1065
1066 if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
1067 res = -ENODEV;
1068 goto out_check_functionality_failed;
1069 }
1070
1071 mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
1072 if (!mpu) {
1073 res = -ENOMEM;
1074 goto out_alloc_data_failed;
1075 }
1076 mldl_cfg = &mpu->mldl_cfg;
1077 mldl_cfg->mpu_ram = &mpu->mpu_ram;
1078 mldl_cfg->mpu_gyro_cfg = &mpu->mpu_gyro_cfg;
1079 mldl_cfg->mpu_offsets = &mpu->mpu_offsets;
1080 mldl_cfg->mpu_chip_info = &mpu->mpu_chip_info;
1081 mldl_cfg->inv_mpu_cfg = &mpu->inv_mpu_cfg;
1082 mldl_cfg->inv_mpu_state = &mpu->inv_mpu_state;
1083
1084 mldl_cfg->mpu_ram->length = MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE;
1085 mldl_cfg->mpu_ram->ram = kzalloc(mldl_cfg->mpu_ram->length, GFP_KERNEL);
1086 if (!mldl_cfg->mpu_ram->ram) {
1087 res = -ENOMEM;
1088 goto out_alloc_ram_failed;
1089 }
1090 mpu_private_data = mpu;
1091 i2c_set_clientdata(client, mpu);
1092 mpu->client = client;
1093
1094 init_waitqueue_head(&mpu->mpu_event_wait);
1095 mutex_init(&mpu->mutex);
1096 init_completion(&mpu->completion);
1097
1098 mpu->response_timeout = 60; /* Seconds */
1099 mpu->timeout.function = mpu_pm_timeout;
1100 mpu->timeout.data = (u_long) mpu;
1101 init_timer(&mpu->timeout);
1102
1103 mpu->nb.notifier_call = mpu_pm_notifier_callback;
1104 mpu->nb.priority = 0;
1105 res = register_pm_notifier(&mpu->nb);
1106 if (res) {
1107 dev_err(&client->adapter->dev,
1108 "Unable to register pm_notifier %d\n", res);
1109 goto out_register_pm_notifier_failed;
1110 }
1111
1112 pdata = (struct mpu_platform_data *)client->dev.platform_data;
1113 if (!pdata) {
1114 dev_WARN(&client->adapter->dev,
1115 "Missing platform data for mpu\n");
1116 }
1117 mldl_cfg->pdata = pdata;
1118
1119 mldl_cfg->mpu_chip_info->addr = client->addr;
1120 res = inv_mpu_open(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
1121
1122 if (res) {
1123 dev_err(&client->adapter->dev,
1124 "Unable to open %s %d\n", MPU_NAME, res);
1125 res = -ENODEV;
1126 goto out_whoami_failed;
1127 }
1128
1129 mpu->dev.minor = MISC_DYNAMIC_MINOR;
1130 mpu->dev.name = "mpu";
1131 mpu->dev.fops = &mpu_fops;
1132 res = misc_register(&mpu->dev);
1133 if (res < 0) {
1134 dev_err(&client->adapter->dev,
1135 "ERROR: misc_register returned %d\n", res);
1136 goto out_misc_register_failed;
1137 }
1138
1139 if (client->irq) {
1140 dev_info(&client->adapter->dev,
1141 "Installing irq using %d\n", client->irq);
1142 if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL))
1143 irq_flags = IRQF_TRIGGER_FALLING;
1144 else
1145 irq_flags = IRQF_TRIGGER_RISING;
1146 res = mpuirq_init(client, mldl_cfg, irq_flags);
1147
1148 if (res)
1149 goto out_mpuirq_failed;
1150 } else {
1151 dev_WARN(&client->adapter->dev,
1152 "Missing %s IRQ\n", MPU_NAME);
1153 }
1154 return res;
1155
1156out_mpuirq_failed:
1157 misc_deregister(&mpu->dev);
1158out_misc_register_failed:
1159 inv_mpu_close(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
1160out_whoami_failed:
1161 unregister_pm_notifier(&mpu->nb);
1162out_register_pm_notifier_failed:
1163 kfree(mldl_cfg->mpu_ram->ram);
1164 mpu_private_data = NULL;
1165out_alloc_ram_failed:
1166 kfree(mpu);
1167out_alloc_data_failed:
1168out_check_functionality_failed:
1169 dev_err(&client->adapter->dev, "%s failed %d\n", __func__, res);
1170 return res;
1171
1172}
1173
1174static int mpu_remove(struct i2c_client *client)
1175{
1176 struct mpu_private_data *mpu = i2c_get_clientdata(client);
1177 struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
1178 struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
1179 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
1180 int ii;
1181
1182 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1183 if (!pdata_slave[ii])
1184 slave_adapter[ii] = NULL;
1185 else
1186 slave_adapter[ii] =
1187 i2c_get_adapter(pdata_slave[ii]->adapt_num);
1188 }
1189
1190 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
1191 dev_dbg(&client->adapter->dev, "%s\n", __func__);
1192
1193 inv_mpu_close(mldl_cfg,
1194 slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
1195 slave_adapter[EXT_SLAVE_TYPE_ACCEL],
1196 slave_adapter[EXT_SLAVE_TYPE_COMPASS],
1197 slave_adapter[EXT_SLAVE_TYPE_PRESSURE]);
1198
1199
1200 if (client->irq)
1201 mpuirq_exit();
1202
1203 misc_deregister(&mpu->dev);
1204
1205 unregister_pm_notifier(&mpu->nb);
1206
1207 kfree(mpu->mldl_cfg.mpu_ram->ram);
1208 kfree(mpu);
1209
1210 return 0;
1211}
1212
1213static struct i2c_driver mpu_driver = {
1214 .class = I2C_CLASS_HWMON,
1215 .probe = mpu_probe,
1216 .remove = mpu_remove,
1217 .id_table = mpu_id,
1218 .driver = {
1219 .owner = THIS_MODULE,
1220 .name = MPU_NAME,
1221 },
1222 .address_list = normal_i2c,
1223 .shutdown = mpu_shutdown, /* optional */
1224 .suspend = mpu_dev_suspend, /* optional */
1225 .resume = mpu_dev_resume, /* optional */
1226
1227};
1228
1229static int __init mpu_init(void)
1230{
1231 int res = i2c_add_driver(&mpu_driver);
1232 pr_info("%s: Probe name %s\n", __func__, MPU_NAME);
1233 if (res)
1234 pr_err("%s failed\n", __func__);
1235 return res;
1236}
1237
1238static void __exit mpu_exit(void)
1239{
1240 pr_info("%s\n", __func__);
1241 i2c_del_driver(&mpu_driver);
1242}
1243
1244module_init(mpu_init);
1245module_exit(mpu_exit);
1246
1247MODULE_AUTHOR("Invensense Corporation");
1248MODULE_DESCRIPTION("User space character device interface for MPU");
1249MODULE_LICENSE("GPL");
1250MODULE_ALIAS(MPU_NAME);
generated by cgit v1.2.2 (git 2.25.0) at 2026-01-20 00:26:44 -0500