diff options
author | Jonathan Herman <hermanjl@cs.unc.edu> | 2013-01-22 10:38:37 -0500 |
---|---|---|
committer | Jonathan Herman <hermanjl@cs.unc.edu> | 2013-01-22 10:38:37 -0500 |
commit | fcc9d2e5a6c89d22b8b773a64fb4ad21ac318446 (patch) | |
tree | a57612d1888735a2ec7972891b68c1ac5ec8faea /drivers/misc/inv_mpu/accel/mpu6050.c | |
parent | 8dea78da5cee153b8af9c07a2745f6c55057fe12 (diff) |
Diffstat (limited to 'drivers/misc/inv_mpu/accel/mpu6050.c')
-rw-r--r-- | drivers/misc/inv_mpu/accel/mpu6050.c | 695 |
1 files changed, 695 insertions, 0 deletions
diff --git a/drivers/misc/inv_mpu/accel/mpu6050.c b/drivers/misc/inv_mpu/accel/mpu6050.c new file mode 100644 index 00000000000..c5bb6784a41 --- /dev/null +++ b/drivers/misc/inv_mpu/accel/mpu6050.c | |||
@@ -0,0 +1,695 @@ | |||
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 ACCELDL | ||
22 | * @brief Provides the interface to setup and handle an accelerometer. | ||
23 | * | ||
24 | * @{ | ||
25 | * @file mpu6050.c | ||
26 | * @brief Accelerometer setup and handling methods for Invensense MPU6050 | ||
27 | */ | ||
28 | |||
29 | /* -------------------------------------------------------------------------- */ | ||
30 | |||
31 | #include <linux/i2c.h> | ||
32 | #include <linux/module.h> | ||
33 | #include <linux/moduleparam.h> | ||
34 | #include <linux/kernel.h> | ||
35 | #include <linux/errno.h> | ||
36 | #include <linux/delay.h> | ||
37 | #include <linux/slab.h> | ||
38 | #include "mpu-dev.h" | ||
39 | |||
40 | #include <log.h> | ||
41 | #include <linux/mpu.h> | ||
42 | #include "mpu6050b1.h" | ||
43 | #include "mlsl.h" | ||
44 | #include "mldl_cfg.h" | ||
45 | #undef MPL_LOG_TAG | ||
46 | #define MPL_LOG_TAG "MPL-acc" | ||
47 | |||
48 | /* -------------------------------------------------------------------------- */ | ||
49 | |||
50 | struct mpu6050_config { | ||
51 | unsigned int odr; /**< output data rate 1/1000 Hz */ | ||
52 | unsigned int fsr; /**< full scale range mg */ | ||
53 | unsigned int ths; /**< mot/no-mot thseshold mg */ | ||
54 | unsigned int dur; /**< mot/no-mot duration ms */ | ||
55 | unsigned int irq_type; /**< irq type */ | ||
56 | }; | ||
57 | |||
58 | struct mpu6050_private_data { | ||
59 | struct mpu6050_config suspend; | ||
60 | struct mpu6050_config resume; | ||
61 | struct mldl_cfg *mldl_cfg_ref; | ||
62 | }; | ||
63 | |||
64 | /* -------------------------------------------------------------------------- */ | ||
65 | |||
66 | static int mpu6050_set_mldl_cfg_ref(void *mlsl_handle, | ||
67 | struct ext_slave_platform_data *pdata, | ||
68 | struct mldl_cfg *mldl_cfg_ref) | ||
69 | { | ||
70 | struct mpu6050_private_data *private_data = | ||
71 | (struct mpu6050_private_data *)pdata->private_data; | ||
72 | private_data->mldl_cfg_ref = mldl_cfg_ref; | ||
73 | return 0; | ||
74 | } | ||
75 | static int mpu6050_set_lp_mode(void *mlsl_handle, | ||
76 | struct ext_slave_platform_data *pdata, | ||
77 | unsigned char lpa_freq) | ||
78 | { | ||
79 | unsigned char b = 0; | ||
80 | /* Reducing the duration setting for lp mode */ | ||
81 | b = 1; | ||
82 | inv_serial_single_write(mlsl_handle, pdata->address, | ||
83 | MPUREG_ACCEL_MOT_DUR, b); | ||
84 | /* Setting the cycle bit and LPA wake up freq */ | ||
85 | inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1, | ||
86 | &b); | ||
87 | b |= BIT_CYCLE | BIT_PD_PTAT; | ||
88 | inv_serial_single_write(mlsl_handle, pdata->address, | ||
89 | MPUREG_PWR_MGMT_1, | ||
90 | b); | ||
91 | inv_serial_read(mlsl_handle, pdata->address, | ||
92 | MPUREG_PWR_MGMT_2, 1, &b); | ||
93 | b |= lpa_freq & BITS_LPA_WAKE_CTRL; | ||
94 | inv_serial_single_write(mlsl_handle, pdata->address, | ||
95 | MPUREG_PWR_MGMT_2, b); | ||
96 | |||
97 | return INV_SUCCESS; | ||
98 | } | ||
99 | |||
100 | static int mpu6050_set_fp_mode(void *mlsl_handle, | ||
101 | struct ext_slave_platform_data *pdata) | ||
102 | { | ||
103 | unsigned char b; | ||
104 | struct mpu6050_private_data *private_data = | ||
105 | (struct mpu6050_private_data *)pdata->private_data; | ||
106 | /* Resetting the cycle bit and LPA wake up freq */ | ||
107 | inv_serial_read(mlsl_handle, pdata->address, | ||
108 | MPUREG_PWR_MGMT_1, 1, &b); | ||
109 | b &= ~BIT_CYCLE & ~BIT_PD_PTAT; | ||
110 | inv_serial_single_write(mlsl_handle, pdata->address, | ||
111 | MPUREG_PWR_MGMT_1, b); | ||
112 | inv_serial_read(mlsl_handle, pdata->address, | ||
113 | MPUREG_PWR_MGMT_2, 1, &b); | ||
114 | b &= ~BITS_LPA_WAKE_CTRL; | ||
115 | inv_serial_single_write(mlsl_handle, pdata->address, | ||
116 | MPUREG_PWR_MGMT_2, b); | ||
117 | /* Resetting the duration setting for fp mode */ | ||
118 | b = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB; | ||
119 | inv_serial_single_write(mlsl_handle, pdata->address, | ||
120 | MPUREG_ACCEL_MOT_DUR, b); | ||
121 | |||
122 | return INV_SUCCESS; | ||
123 | } | ||
124 | /** | ||
125 | * Record the odr for use in computing duration values. | ||
126 | * | ||
127 | * @param config Config to set, suspend or resume structure | ||
128 | * @param odr output data rate in 1/1000 hz | ||
129 | */ | ||
130 | static int mpu6050_set_odr(void *mlsl_handle, | ||
131 | struct ext_slave_platform_data *pdata, | ||
132 | struct mpu6050_config *config, long apply, long odr) | ||
133 | { | ||
134 | int result; | ||
135 | unsigned char b; | ||
136 | unsigned char lpa_freq = 1; /* Default value */ | ||
137 | long base; | ||
138 | int total_divider; | ||
139 | struct mpu6050_private_data *private_data = | ||
140 | (struct mpu6050_private_data *)pdata->private_data; | ||
141 | struct mldl_cfg *mldl_cfg_ref = | ||
142 | (struct mldl_cfg *)private_data->mldl_cfg_ref; | ||
143 | |||
144 | if (mldl_cfg_ref) { | ||
145 | base = 1000 * | ||
146 | inv_mpu_get_sampling_rate_hz(mldl_cfg_ref->mpu_gyro_cfg) | ||
147 | * (mldl_cfg_ref->mpu_gyro_cfg->divider + 1); | ||
148 | } else { | ||
149 | /* have no reference to mldl_cfg => assume base rate is 1000 */ | ||
150 | base = 1000000L; | ||
151 | } | ||
152 | |||
153 | if (odr != 0) { | ||
154 | total_divider = (base / odr) - 1; | ||
155 | /* final odr MAY be different from requested odr due to | ||
156 | integer truncation */ | ||
157 | config->odr = base / (total_divider + 1); | ||
158 | } else { | ||
159 | config->odr = 0; | ||
160 | return 0; | ||
161 | } | ||
162 | |||
163 | /* if the DMP and/or gyros are on, don't set the ODR => | ||
164 | the DMP/gyro mldl_cfg->divider setting will handle it */ | ||
165 | if (apply | ||
166 | && (mldl_cfg_ref && | ||
167 | !(mldl_cfg_ref->inv_mpu_cfg->requested_sensors & | ||
168 | INV_DMP_PROCESSOR))) { | ||
169 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
170 | MPUREG_SMPLRT_DIV, | ||
171 | (unsigned char)total_divider); | ||
172 | if (result) { | ||
173 | LOG_RESULT_LOCATION(result); | ||
174 | return result; | ||
175 | } | ||
176 | MPL_LOGI("ODR : %d mHz\n", config->odr); | ||
177 | } | ||
178 | /* Decide whether to put accel in LP mode or pull out of LP mode | ||
179 | based on the odr. */ | ||
180 | switch (odr) { | ||
181 | case 1000: | ||
182 | lpa_freq = BITS_LPA_WAKE_1HZ; | ||
183 | break; | ||
184 | case 2000: | ||
185 | lpa_freq = BITS_LPA_WAKE_2HZ; | ||
186 | break; | ||
187 | case 10000: | ||
188 | lpa_freq = BITS_LPA_WAKE_10HZ; | ||
189 | break; | ||
190 | case 40000: | ||
191 | lpa_freq = BITS_LPA_WAKE_40HZ; | ||
192 | break; | ||
193 | default: | ||
194 | inv_serial_read(mlsl_handle, pdata->address, | ||
195 | MPUREG_PWR_MGMT_1, 1, &b); | ||
196 | b &= BIT_CYCLE; | ||
197 | if (b == BIT_CYCLE) { | ||
198 | MPL_LOGI(" Accel LP - > FP mode. \n "); | ||
199 | mpu6050_set_fp_mode(mlsl_handle, pdata); | ||
200 | } | ||
201 | } | ||
202 | /* If lpa_freq default value was changed, set into LP mode */ | ||
203 | if (lpa_freq != 1) { | ||
204 | MPL_LOGI(" Accel FP - > LP mode. \n "); | ||
205 | mpu6050_set_lp_mode(mlsl_handle, pdata, lpa_freq); | ||
206 | } | ||
207 | return 0; | ||
208 | } | ||
209 | |||
210 | static int mpu6050_set_fsr(void *mlsl_handle, | ||
211 | struct ext_slave_platform_data *pdata, | ||
212 | struct mpu6050_config *config, long apply, long fsr) | ||
213 | { | ||
214 | unsigned char fsr_mask; | ||
215 | int result; | ||
216 | |||
217 | if (fsr <= 2000) { | ||
218 | config->fsr = 2000; | ||
219 | fsr_mask = 0x00; | ||
220 | } else if (fsr <= 4000) { | ||
221 | config->fsr = 4000; | ||
222 | fsr_mask = 0x08; | ||
223 | } else if (fsr <= 8000) { | ||
224 | config->fsr = 8000; | ||
225 | fsr_mask = 0x10; | ||
226 | } else { /* fsr = [8001, oo) */ | ||
227 | config->fsr = 16000; | ||
228 | fsr_mask = 0x18; | ||
229 | } | ||
230 | |||
231 | if (apply) { | ||
232 | unsigned char reg; | ||
233 | result = inv_serial_read(mlsl_handle, pdata->address, | ||
234 | MPUREG_ACCEL_CONFIG, 1, ®); | ||
235 | if (result) { | ||
236 | LOG_RESULT_LOCATION(result); | ||
237 | return result; | ||
238 | } | ||
239 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
240 | MPUREG_ACCEL_CONFIG, | ||
241 | reg | fsr_mask); | ||
242 | if (result) { | ||
243 | LOG_RESULT_LOCATION(result); | ||
244 | return result; | ||
245 | } | ||
246 | MPL_LOGV("FSR: %d\n", config->fsr); | ||
247 | } | ||
248 | return 0; | ||
249 | } | ||
250 | |||
251 | static int mpu6050_set_irq(void *mlsl_handle, | ||
252 | struct ext_slave_platform_data *pdata, | ||
253 | struct mpu6050_config *config, long apply, | ||
254 | long irq_type) | ||
255 | { | ||
256 | int result; | ||
257 | unsigned char reg_int_cfg; | ||
258 | |||
259 | switch (irq_type) { | ||
260 | case MPU_SLAVE_IRQ_TYPE_DATA_READY: | ||
261 | config->irq_type = irq_type; | ||
262 | reg_int_cfg = BIT_RAW_RDY_EN; | ||
263 | break; | ||
264 | /* todo: add MOTION, NO_MOTION, and FREEFALL */ | ||
265 | case MPU_SLAVE_IRQ_TYPE_NONE: | ||
266 | /* Do nothing, not even set the interrupt because it is | ||
267 | shared with the gyro */ | ||
268 | config->irq_type = irq_type; | ||
269 | return 0; | ||
270 | default: | ||
271 | return INV_ERROR_INVALID_PARAMETER; | ||
272 | } | ||
273 | |||
274 | if (apply) { | ||
275 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
276 | MPUREG_INT_ENABLE, | ||
277 | reg_int_cfg); | ||
278 | if (result) { | ||
279 | LOG_RESULT_LOCATION(result); | ||
280 | return result; | ||
281 | } | ||
282 | MPL_LOGV("irq_type: %d\n", config->irq_type); | ||
283 | } | ||
284 | |||
285 | return 0; | ||
286 | } | ||
287 | |||
288 | static int mpu6050_set_ths(void *mlsl_handle, | ||
289 | struct ext_slave_platform_data *slave, | ||
290 | struct mpu6050_config *config, long apply, long ths) | ||
291 | { | ||
292 | if (ths < 0) | ||
293 | ths = 0; | ||
294 | |||
295 | config->ths = ths; | ||
296 | MPL_LOGV("THS: %d\n", config->ths); | ||
297 | return 0; | ||
298 | } | ||
299 | |||
300 | static int mpu6050_set_dur(void *mlsl_handle, | ||
301 | struct ext_slave_platform_data *slave, | ||
302 | struct mpu6050_config *config, long apply, long dur) | ||
303 | { | ||
304 | if (dur < 0) | ||
305 | dur = 0; | ||
306 | |||
307 | config->dur = dur; | ||
308 | MPL_LOGV("DUR: %d\n", config->dur); | ||
309 | return 0; | ||
310 | } | ||
311 | |||
312 | |||
313 | static int mpu6050_init(void *mlsl_handle, | ||
314 | struct ext_slave_descr *slave, | ||
315 | struct ext_slave_platform_data *pdata) | ||
316 | { | ||
317 | int result; | ||
318 | struct mpu6050_private_data *private_data; | ||
319 | |||
320 | |||
321 | private_data = kzalloc(sizeof(*private_data), GFP_KERNEL); | ||
322 | |||
323 | if (!private_data) | ||
324 | return INV_ERROR_MEMORY_EXAUSTED; | ||
325 | |||
326 | pdata->private_data = private_data; | ||
327 | |||
328 | result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->suspend, | ||
329 | false, 0); | ||
330 | if (result) { | ||
331 | LOG_RESULT_LOCATION(result); | ||
332 | return result; | ||
333 | } | ||
334 | result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->resume, | ||
335 | false, 200000); | ||
336 | if (result) { | ||
337 | LOG_RESULT_LOCATION(result); | ||
338 | return result; | ||
339 | } | ||
340 | result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->suspend, | ||
341 | false, 2000); | ||
342 | if (result) { | ||
343 | LOG_RESULT_LOCATION(result); | ||
344 | return result; | ||
345 | } | ||
346 | result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->resume, | ||
347 | false, 2000); | ||
348 | if (result) { | ||
349 | LOG_RESULT_LOCATION(result); | ||
350 | return result; | ||
351 | } | ||
352 | |||
353 | result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->suspend, | ||
354 | false, MPU_SLAVE_IRQ_TYPE_NONE); | ||
355 | if (result) { | ||
356 | LOG_RESULT_LOCATION(result); | ||
357 | return result; | ||
358 | } | ||
359 | result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->resume, | ||
360 | false, MPU_SLAVE_IRQ_TYPE_NONE); | ||
361 | if (result) { | ||
362 | LOG_RESULT_LOCATION(result); | ||
363 | return result; | ||
364 | } | ||
365 | |||
366 | result = mpu6050_set_ths(mlsl_handle, pdata, &private_data->suspend, | ||
367 | false, 80); | ||
368 | if (result) { | ||
369 | LOG_RESULT_LOCATION(result); | ||
370 | return result; | ||
371 | } | ||
372 | result = mpu6050_set_ths(mlsl_handle, pdata, &private_data->resume, | ||
373 | false, 40); | ||
374 | if (result) { | ||
375 | LOG_RESULT_LOCATION(result); | ||
376 | return result; | ||
377 | } | ||
378 | result = mpu6050_set_dur(mlsl_handle, pdata, &private_data->suspend, | ||
379 | false, 1000); | ||
380 | if (result) { | ||
381 | LOG_RESULT_LOCATION(result); | ||
382 | return result; | ||
383 | } | ||
384 | result = mpu6050_set_dur(mlsl_handle, pdata, &private_data->resume, | ||
385 | false, 2540); | ||
386 | if (result) { | ||
387 | LOG_RESULT_LOCATION(result); | ||
388 | return result; | ||
389 | } | ||
390 | |||
391 | return 0; | ||
392 | } | ||
393 | |||
394 | static int mpu6050_exit(void *mlsl_handle, | ||
395 | struct ext_slave_descr *slave, | ||
396 | struct ext_slave_platform_data *pdata) | ||
397 | { | ||
398 | kfree(pdata->private_data); | ||
399 | pdata->private_data = NULL; | ||
400 | return 0; | ||
401 | } | ||
402 | |||
403 | static int mpu6050_suspend(void *mlsl_handle, | ||
404 | struct ext_slave_descr *slave, | ||
405 | struct ext_slave_platform_data *pdata) | ||
406 | { | ||
407 | unsigned char reg; | ||
408 | int result; | ||
409 | struct mpu6050_private_data *private_data = | ||
410 | (struct mpu6050_private_data *)pdata->private_data; | ||
411 | |||
412 | result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->suspend, | ||
413 | true, private_data->suspend.odr); | ||
414 | if (result) { | ||
415 | LOG_RESULT_LOCATION(result); | ||
416 | return result; | ||
417 | } | ||
418 | |||
419 | result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->suspend, | ||
420 | true, private_data->suspend.irq_type); | ||
421 | if (result) { | ||
422 | LOG_RESULT_LOCATION(result); | ||
423 | return result; | ||
424 | } | ||
425 | |||
426 | result = inv_serial_read(mlsl_handle, pdata->address, | ||
427 | MPUREG_PWR_MGMT_2, 1, ®); | ||
428 | if (result) { | ||
429 | LOG_RESULT_LOCATION(result); | ||
430 | return result; | ||
431 | } | ||
432 | reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA); | ||
433 | |||
434 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
435 | MPUREG_PWR_MGMT_2, reg); | ||
436 | if (result) { | ||
437 | LOG_RESULT_LOCATION(result); | ||
438 | return result; | ||
439 | } | ||
440 | |||
441 | return 0; | ||
442 | } | ||
443 | |||
444 | static int mpu6050_resume(void *mlsl_handle, | ||
445 | struct ext_slave_descr *slave, | ||
446 | struct ext_slave_platform_data *pdata) | ||
447 | { | ||
448 | int result; | ||
449 | unsigned char reg; | ||
450 | struct mpu6050_private_data *private_data = | ||
451 | (struct mpu6050_private_data *)pdata->private_data; | ||
452 | |||
453 | result = inv_serial_read(mlsl_handle, pdata->address, | ||
454 | MPUREG_PWR_MGMT_1, 1, ®); | ||
455 | if (result) { | ||
456 | LOG_RESULT_LOCATION(result); | ||
457 | return result; | ||
458 | } | ||
459 | |||
460 | if (reg & BIT_SLEEP) { | ||
461 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
462 | MPUREG_PWR_MGMT_1, reg & ~BIT_SLEEP); | ||
463 | if (result) { | ||
464 | LOG_RESULT_LOCATION(result); | ||
465 | return result; | ||
466 | } | ||
467 | } | ||
468 | msleep(2); | ||
469 | |||
470 | result = inv_serial_read(mlsl_handle, pdata->address, | ||
471 | MPUREG_PWR_MGMT_2, 1, ®); | ||
472 | if (result) { | ||
473 | LOG_RESULT_LOCATION(result); | ||
474 | return result; | ||
475 | } | ||
476 | reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA); | ||
477 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
478 | MPUREG_PWR_MGMT_2, reg); | ||
479 | if (result) { | ||
480 | LOG_RESULT_LOCATION(result); | ||
481 | return result; | ||
482 | } | ||
483 | |||
484 | /* settings */ | ||
485 | |||
486 | result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->resume, | ||
487 | true, private_data->resume.fsr); | ||
488 | if (result) { | ||
489 | LOG_RESULT_LOCATION(result); | ||
490 | return result; | ||
491 | } | ||
492 | result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->resume, | ||
493 | true, private_data->resume.odr); | ||
494 | if (result) { | ||
495 | LOG_RESULT_LOCATION(result); | ||
496 | return result; | ||
497 | } | ||
498 | result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->resume, | ||
499 | true, private_data->resume.irq_type); | ||
500 | |||
501 | /* motion, no_motion */ | ||
502 | /* TODO : port these in their respective _set_thrs and _set_dur | ||
503 | functions and use the APPLY paremeter to apply just like | ||
504 | _set_odr, _set_irq, and _set_fsr. */ | ||
505 | reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_THR_LSB; | ||
506 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
507 | MPUREG_ACCEL_MOT_THR, reg); | ||
508 | if (result) { | ||
509 | LOG_RESULT_LOCATION(result); | ||
510 | return result; | ||
511 | } | ||
512 | reg = (unsigned char) | ||
513 | ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths); | ||
514 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
515 | MPUREG_ACCEL_ZRMOT_THR, reg); | ||
516 | if (result) { | ||
517 | LOG_RESULT_LOCATION(result); | ||
518 | return result; | ||
519 | } | ||
520 | reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB; | ||
521 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
522 | MPUREG_ACCEL_MOT_DUR, reg); | ||
523 | if (result) { | ||
524 | LOG_RESULT_LOCATION(result); | ||
525 | return result; | ||
526 | } | ||
527 | reg = (unsigned char)private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB; | ||
528 | result = inv_serial_single_write(mlsl_handle, pdata->address, | ||
529 | MPUREG_ACCEL_ZRMOT_DUR, reg); | ||
530 | if (result) { | ||
531 | LOG_RESULT_LOCATION(result); | ||
532 | return result; | ||
533 | } | ||
534 | return 0; | ||
535 | } | ||
536 | |||
537 | static int mpu6050_read(void *mlsl_handle, | ||
538 | struct ext_slave_descr *slave, | ||
539 | struct ext_slave_platform_data *pdata, | ||
540 | unsigned char *data) | ||
541 | { | ||
542 | int result; | ||
543 | result = inv_serial_read(mlsl_handle, pdata->address, | ||
544 | slave->read_reg, slave->read_len, data); | ||
545 | return result; | ||
546 | } | ||
547 | |||
548 | static int mpu6050_config(void *mlsl_handle, | ||
549 | struct ext_slave_descr *slave, | ||
550 | struct ext_slave_platform_data *pdata, | ||
551 | struct ext_slave_config *data) | ||
552 | { | ||
553 | struct mpu6050_private_data *private_data = | ||
554 | (struct mpu6050_private_data *)pdata->private_data; | ||
555 | if (!data->data) | ||
556 | return INV_ERROR_INVALID_PARAMETER; | ||
557 | |||
558 | switch (data->key) { | ||
559 | case MPU_SLAVE_CONFIG_ODR_SUSPEND: | ||
560 | return mpu6050_set_odr(mlsl_handle, pdata, | ||
561 | &private_data->suspend, | ||
562 | data->apply, *((long *)data->data)); | ||
563 | case MPU_SLAVE_CONFIG_ODR_RESUME: | ||
564 | return mpu6050_set_odr(mlsl_handle, pdata, | ||
565 | &private_data->resume, | ||
566 | data->apply, *((long *)data->data)); | ||
567 | case MPU_SLAVE_CONFIG_FSR_SUSPEND: | ||
568 | return mpu6050_set_fsr(mlsl_handle, pdata, | ||
569 | &private_data->suspend, | ||
570 | data->apply, *((long *)data->data)); | ||
571 | case MPU_SLAVE_CONFIG_FSR_RESUME: | ||
572 | return mpu6050_set_fsr(mlsl_handle, pdata, | ||
573 | &private_data->resume, | ||
574 | data->apply, *((long *)data->data)); | ||
575 | case MPU_SLAVE_CONFIG_MOT_THS: | ||
576 | return mpu6050_set_ths(mlsl_handle, pdata, | ||
577 | &private_data->suspend, | ||
578 | data->apply, *((long *)data->data)); | ||
579 | case MPU_SLAVE_CONFIG_NMOT_THS: | ||
580 | return mpu6050_set_ths(mlsl_handle, pdata, | ||
581 | &private_data->resume, | ||
582 | data->apply, *((long *)data->data)); | ||
583 | case MPU_SLAVE_CONFIG_MOT_DUR: | ||
584 | return mpu6050_set_dur(mlsl_handle, pdata, | ||
585 | &private_data->suspend, | ||
586 | data->apply, *((long *)data->data)); | ||
587 | case MPU_SLAVE_CONFIG_NMOT_DUR: | ||
588 | return mpu6050_set_dur(mlsl_handle, pdata, | ||
589 | &private_data->resume, | ||
590 | data->apply, *((long *)data->data)); | ||
591 | case MPU_SLAVE_CONFIG_IRQ_SUSPEND: | ||
592 | return mpu6050_set_irq(mlsl_handle, pdata, | ||
593 | &private_data->suspend, | ||
594 | data->apply, *((long *)data->data)); | ||
595 | break; | ||
596 | case MPU_SLAVE_CONFIG_IRQ_RESUME: | ||
597 | return mpu6050_set_irq(mlsl_handle, pdata, | ||
598 | &private_data->resume, | ||
599 | data->apply, *((long *)data->data)); | ||
600 | case MPU_SLAVE_CONFIG_INTERNAL_REFERENCE: | ||
601 | return mpu6050_set_mldl_cfg_ref(mlsl_handle, pdata, | ||
602 | (struct mldl_cfg *)data->data); | ||
603 | break; | ||
604 | |||
605 | default: | ||
606 | return INV_ERROR_FEATURE_NOT_IMPLEMENTED; | ||
607 | }; | ||
608 | |||
609 | return 0; | ||
610 | } | ||
611 | |||
612 | static int mpu6050_get_config(void *mlsl_handle, | ||
613 | struct ext_slave_descr *slave, | ||
614 | struct ext_slave_platform_data *pdata, | ||
615 | struct ext_slave_config *data) | ||
616 | { | ||
617 | struct mpu6050_private_data *private_data = | ||
618 | (struct mpu6050_private_data *)pdata->private_data; | ||
619 | if (!data->data) | ||
620 | return INV_ERROR_INVALID_PARAMETER; | ||
621 | |||
622 | switch (data->key) { | ||
623 | case MPU_SLAVE_CONFIG_ODR_SUSPEND: | ||
624 | (*(unsigned long *)data->data) = | ||
625 | (unsigned long)private_data->suspend.odr; | ||
626 | break; | ||
627 | case MPU_SLAVE_CONFIG_ODR_RESUME: | ||
628 | (*(unsigned long *)data->data) = | ||
629 | (unsigned long)private_data->resume.odr; | ||
630 | break; | ||
631 | case MPU_SLAVE_CONFIG_FSR_SUSPEND: | ||
632 | (*(unsigned long *)data->data) = | ||
633 | (unsigned long)private_data->suspend.fsr; | ||
634 | break; | ||
635 | case MPU_SLAVE_CONFIG_FSR_RESUME: | ||
636 | (*(unsigned long *)data->data) = | ||
637 | (unsigned long)private_data->resume.fsr; | ||
638 | break; | ||
639 | case MPU_SLAVE_CONFIG_MOT_THS: | ||
640 | (*(unsigned long *)data->data) = | ||
641 | (unsigned long)private_data->suspend.ths; | ||
642 | break; | ||
643 | case MPU_SLAVE_CONFIG_NMOT_THS: | ||
644 | (*(unsigned long *)data->data) = | ||
645 | (unsigned long)private_data->resume.ths; | ||
646 | break; | ||
647 | case MPU_SLAVE_CONFIG_MOT_DUR: | ||
648 | (*(unsigned long *)data->data) = | ||
649 | (unsigned long)private_data->suspend.dur; | ||
650 | break; | ||
651 | case MPU_SLAVE_CONFIG_NMOT_DUR: | ||
652 | (*(unsigned long *)data->data) = | ||
653 | (unsigned long)private_data->resume.dur; | ||
654 | break; | ||
655 | case MPU_SLAVE_CONFIG_IRQ_SUSPEND: | ||
656 | (*(unsigned long *)data->data) = | ||
657 | (unsigned long)private_data->suspend.irq_type; | ||
658 | break; | ||
659 | case MPU_SLAVE_CONFIG_IRQ_RESUME: | ||
660 | (*(unsigned long *)data->data) = | ||
661 | (unsigned long)private_data->resume.irq_type; | ||
662 | break; | ||
663 | default: | ||
664 | return INV_ERROR_FEATURE_NOT_IMPLEMENTED; | ||
665 | }; | ||
666 | |||
667 | return 0; | ||
668 | } | ||
669 | |||
670 | static struct ext_slave_descr mpu6050_descr = { | ||
671 | .init = mpu6050_init, | ||
672 | .exit = mpu6050_exit, | ||
673 | .suspend = mpu6050_suspend, | ||
674 | .resume = mpu6050_resume, | ||
675 | .read = mpu6050_read, | ||
676 | .config = mpu6050_config, | ||
677 | .get_config = mpu6050_get_config, | ||
678 | .name = "mpu6050", | ||
679 | .type = EXT_SLAVE_TYPE_ACCEL, | ||
680 | .id = ACCEL_ID_MPU6050, | ||
681 | .read_reg = 0x3B, | ||
682 | .read_len = 6, | ||
683 | .endian = EXT_SLAVE_BIG_ENDIAN, | ||
684 | .range = {2, 0}, | ||
685 | .trigger = NULL, | ||
686 | }; | ||
687 | |||
688 | struct ext_slave_descr *mpu6050_get_slave_descr(void) | ||
689 | { | ||
690 | return &mpu6050_descr; | ||
691 | } | ||
692 | |||
693 | /** | ||
694 | * @} | ||
695 | */ | ||