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 | */ | ||
