1/* 2 $License: 3 Copyright 2011 InvenSense, Inc. 4 5 Licensed under the Apache License, Version 2.0 (the "License"); 6 you may not use this file except in compliance with the License. 7 You may obtain a copy of the License at 8 9 http://www.apache.org/licenses/LICENSE-2.0 10 11 Unless required by applicable law or agreed to in writing, software 12 distributed under the License is distributed on an "AS IS" BASIS, 13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 See the License for the specific language governing permissions and 15 limitations under the License. 16 $ 17 */ 18 19/****************************************************************************** 20 * 21 * $Id: mldl_cfg_mpu.c 5653 2011-06-16 21:06:55Z nroyer $ 22 * 23 *****************************************************************************/ 24 25/** 26 * @addtogroup MLDL 27 * 28 * @{ 29 * @file mldl_cfg_mpu.c 30 * @brief The Motion Library Driver Layer. 31 */ 32 33/* ------------------ */ 34/* - Include Files. - */ 35/* ------------------ */ 36 37#include <stddef.h> 38#include "mldl_cfg.h" 39#include "mlsl.h" 40#include "mpu.h" 41 42#ifdef LINUX 43#include <sys/ioctl.h> 44#endif 45 46#include "log.h" 47#undef MPL_LOG_TAG 48#define MPL_LOG_TAG "MPL-mldl_cfg_mpu:" 49 50/* --------------------- */ 51/* - Variables. - */ 52/* --------------------- */ 53 54 55/* ---------------------- */ 56/* - Static Functions. - */ 57/* ---------------------- */ 58void mpu_print_cfg(struct mldl_cfg * mldl_cfg) 59{ 60 struct mpu_platform_data *pdata = mldl_cfg->pdata; 61 struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel; 62 struct ext_slave_platform_data *compass = &mldl_cfg->pdata->compass; 63 struct ext_slave_platform_data *pressure = &mldl_cfg->pdata->pressure; 64 65 MPL_LOGD("mldl_cfg.addr = %02x\n", mldl_cfg->addr); 66 MPL_LOGD("mldl_cfg.int_config = %02x\n", mldl_cfg->int_config); 67 MPL_LOGD("mldl_cfg.ext_sync = %02x\n", mldl_cfg->ext_sync); 68 MPL_LOGD("mldl_cfg.full_scale = %02x\n", mldl_cfg->full_scale); 69 MPL_LOGD("mldl_cfg.lpf = %02x\n", mldl_cfg->lpf); 70 MPL_LOGD("mldl_cfg.clk_src = %02x\n", mldl_cfg->clk_src); 71 MPL_LOGD("mldl_cfg.divider = %02x\n", mldl_cfg->divider); 72 MPL_LOGD("mldl_cfg.dmp_enable = %02x\n", mldl_cfg->dmp_enable); 73 MPL_LOGD("mldl_cfg.fifo_enable = %02x\n", mldl_cfg->fifo_enable); 74 MPL_LOGD("mldl_cfg.dmp_cfg1 = %02x\n", mldl_cfg->dmp_cfg1); 75 MPL_LOGD("mldl_cfg.dmp_cfg2 = %02x\n", mldl_cfg->dmp_cfg2); 76 MPL_LOGD("mldl_cfg.offset_tc[0] = %02x\n", mldl_cfg->offset_tc[0]); 77 MPL_LOGD("mldl_cfg.offset_tc[1] = %02x\n", mldl_cfg->offset_tc[1]); 78 MPL_LOGD("mldl_cfg.offset_tc[2] = %02x\n", mldl_cfg->offset_tc[2]); 79 MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", mldl_cfg->silicon_revision); 80 MPL_LOGD("mldl_cfg.product_id = %02x\n", mldl_cfg->product_id); 81 MPL_LOGD("mldl_cfg.gyro_sens_trim = %02x\n", mldl_cfg->gyro_sens_trim); 82#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 83 MPL_LOGD("mldl_cfg.accel_sens_trim = %02x\n", mldl_cfg->accel_sens_trim); 84#endif 85 86 if (mldl_cfg->accel) { 87 MPL_LOGD("slave_accel->suspend = %p\n", mldl_cfg->accel->suspend); 88 MPL_LOGD("slave_accel->resume = %p\n", mldl_cfg->accel->resume); 89 MPL_LOGD("slave_accel->read = %p\n", mldl_cfg->accel->read); 90 MPL_LOGD("slave_accel->type = %02x\n", mldl_cfg->accel->type); 91 MPL_LOGD("slave_accel->read_reg = %02x\n", 92 mldl_cfg->accel->read_reg); 93 MPL_LOGD("slave_accel->read_len = %02x\n", 94 mldl_cfg->accel->read_len); 95 MPL_LOGD("slave_accel->endian = %02x\n", mldl_cfg->accel->endian); 96 MPL_LOGD("slave_accel->range.mantissa= %02x\n", (int)mldl_cfg->accel->range.mantissa); 97 MPL_LOGD("slave_accel->range.fraction= %02x\n", (int)mldl_cfg->accel->range.fraction); 98 } else { 99 MPL_LOGD("slave_accel = NULL\n"); 100 } 101 102 if (mldl_cfg->compass) { 103 MPL_LOGD("slave_compass->suspend = %p\n", mldl_cfg->compass->suspend); 104 MPL_LOGD("slave_compass->resume = %p\n", mldl_cfg->compass->resume); 105 MPL_LOGD("slave_compass->read = %p\n", mldl_cfg->compass->read); 106 MPL_LOGD("slave_compass->type = %02x\n", mldl_cfg->compass->type); 107 MPL_LOGD("slave_compass->read_reg = %02x\n", 108 mldl_cfg->compass->read_reg); 109 MPL_LOGD("slave_compass->read_len = %02x\n", 110 mldl_cfg->compass->read_len); 111 MPL_LOGD("slave_compass->endian = %02x\n", mldl_cfg->compass->endian); 112 MPL_LOGD("slave_compass->range.mantissa= %02x\n", (int)mldl_cfg->compass->range.mantissa); 113 MPL_LOGD("slave_compass->range.fraction= %02x\n", (int)mldl_cfg->compass->range.fraction); 114 } else { 115 MPL_LOGD("slave_compass = NULL\n"); 116 } 117 118 if (mldl_cfg->pressure) { 119 MPL_LOGD("slave_pressure->suspend = %p\n", mldl_cfg->pressure->suspend); 120 MPL_LOGD("slave_pressure->resume = %p\n", mldl_cfg->pressure->resume); 121 MPL_LOGD("slave_pressure->read = %p\n", mldl_cfg->pressure->read); 122 MPL_LOGD("slave_pressure->type = %02x\n", mldl_cfg->pressure->type); 123 MPL_LOGD("slave_pressure->read_reg = %02x\n", 124 mldl_cfg->pressure->read_reg); 125 MPL_LOGD("slave_pressure->read_len = %02x\n", 126 mldl_cfg->pressure->read_len); 127 MPL_LOGD("slave_pressure->endian = %02x\n", mldl_cfg->pressure->endian); 128 MPL_LOGD("slave_pressure->range.mantissa= %02x\n", (int)mldl_cfg->pressure->range.mantissa); 129 MPL_LOGD("slave_pressure->range.fraction= %02x\n", (int)mldl_cfg->pressure->range.fraction); 130 } else { 131 MPL_LOGD("slave_pressure = NULL\n"); 132 } 133 134 MPL_LOGD("accel->get_slave_descr = %p\n",accel->get_slave_descr); 135 MPL_LOGD("accel->adapt_num = %02x\n", accel->adapt_num); 136 MPL_LOGD("accel->bus = %02x\n", accel->bus); 137 MPL_LOGD("accel->address = %02x\n", accel->address); 138 MPL_LOGD("accel->orientation = \n" 139 " %2d %2d %2d\n" 140 " %2d %2d %2d\n" 141 " %2d %2d %2d\n", 142 accel->orientation[0],accel->orientation[1],accel->orientation[2], 143 accel->orientation[3],accel->orientation[4],accel->orientation[5], 144 accel->orientation[6],accel->orientation[7],accel->orientation[8]); 145 MPL_LOGD("compass->get_slave_descr = %p\n",compass->get_slave_descr); 146 MPL_LOGD("compass->adapt_num = %02x\n", compass->adapt_num); 147 MPL_LOGD("compass->bus = %02x\n", compass->bus); 148 MPL_LOGD("compass->address = %02x\n", compass->address); 149 MPL_LOGD("compass->orientation = \n" 150 " %2d %2d %2d\n" 151 " %2d %2d %2d\n" 152 " %2d %2d %2d\n", 153 compass->orientation[0],compass->orientation[1],compass->orientation[2], 154 compass->orientation[3],compass->orientation[4],compass->orientation[5], 155 compass->orientation[6],compass->orientation[7],compass->orientation[8]); 156 MPL_LOGD("pressure->get_slave_descr = %p\n",pressure->get_slave_descr); 157 MPL_LOGD("pressure->adapt_num = %02x\n", pressure->adapt_num); 158 MPL_LOGD("pressure->bus = %02x\n", pressure->bus); 159 MPL_LOGD("pressure->address = %02x\n", pressure->address); 160 MPL_LOGD("pressure->orientation = \n" 161 " %2d %2d %2d\n" 162 " %2d %2d %2d\n" 163 " %2d %2d %2d\n", 164 pressure->orientation[0],pressure->orientation[1],pressure->orientation[2], 165 pressure->orientation[3],pressure->orientation[4],pressure->orientation[5], 166 pressure->orientation[6],pressure->orientation[7],pressure->orientation[8]); 167 168 MPL_LOGD("pdata->int_config = %02x\n", pdata->int_config); 169 MPL_LOGD("pdata->level_shifter = %02x\n", pdata->level_shifter); 170 MPL_LOGD("pdata->orientation = \n" 171 " %2d %2d %2d\n" 172 " %2d %2d %2d\n" 173 " %2d %2d %2d\n", 174 pdata->orientation[0],pdata->orientation[1],pdata->orientation[2], 175 pdata->orientation[3],pdata->orientation[4],pdata->orientation[5], 176 pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]); 177 178 MPL_LOGD("Struct sizes: mldl_cfg: %zu, " 179 "ext_slave_descr:%zu, mpu_platform_data:%zu: RamOffset: %zu\n", 180 sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr), 181 sizeof(struct mpu_platform_data), 182 offsetof(struct mldl_cfg, ram)); 183} 184 185/****************************************************************************** 186 ****************************************************************************** 187 * Exported functions 188 ****************************************************************************** 189 *****************************************************************************/ 190 191/** 192 * Initializes the pdata structure to defaults. 193 * 194 * Opens the device to read silicon revision, product id and whoami. Leaves 195 * the device in suspended state for low power. 196 * 197 * @param mldl_cfg handle to the config structure 198 * @param mlsl_handle handle to the mpu serial layer 199 * @param accel_handle handle to the accel serial layer 200 * @param compass_handle handle to the compass serial layer 201 * @param pressure_handle handle to the pressure serial layer 202 * 203 * @return INV_SUCCESS if silicon revision, product id and woami are supported 204 * by this software. 205 */ 206int inv_mpu_open(struct mldl_cfg *mldl_cfg, 207 void *mlsl_handle, 208 void *accel_handle, 209 void *compass_handle, 210 void *pressure_handle) 211{ 212 int result; 213 result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); 214 if (result) { 215 LOG_RESULT_LOCATION(result); 216 return result; 217 } 218 219 result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL, 220 INV_ALL_SENSORS); 221 if (result) { 222 LOG_RESULT_LOCATION(result); 223 return result; 224 } 225 return result; 226} 227 228/** 229 * Stub for driver close. Just verify that the devices are suspended 230 * 231 * @param mldl_cfg handle to the config structure 232 * @param mlsl_handle handle to the mpu serial layer 233 * @param accel_handle handle to the accel serial layer 234 * @param compass_handle handle to the compass serial layer 235 * @param pressure_handle handle to the compass serial layer 236 * 237 * @return INV_SUCCESS or non-zero error code 238 */ 239int inv_mpu_close(struct mldl_cfg *mldl_cfg, 240 void *mlsl_handle, 241 void *accel_handle, 242 void *compass_handle, 243 void *pressure_handle) 244{ 245 int result = INV_SUCCESS; 246 247 result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL, 248 INV_ALL_SENSORS); 249 return result; 250} 251 252int inv_mpu_resume(struct mldl_cfg* mldl_cfg, 253 void *mlsl_handle, 254 void *accel_handle, 255 void *compass_handle, 256 void *pressure_handle, 257 unsigned long sensors) 258{ 259 int result; 260 261 mldl_cfg->requested_sensors = sensors; 262 result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg); 263 if (result) { 264 LOG_RESULT_LOCATION(result); 265 return result; 266 } 267 result = ioctl((int)(uintptr_t)mlsl_handle, MPU_RESUME, NULL); 268 if (result) { 269 LOG_RESULT_LOCATION(result); 270 return result; 271 } 272 result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); 273 if (result) { 274 LOG_RESULT_LOCATION(result); 275 return result; 276 } 277 //MPL_LOGI("%s: Resuming to %04lx\n", __func__, mldl_cfg->requested_sensors); 278 279 return result; 280} 281 282 283int inv_mpu_suspend(struct mldl_cfg *mldl_cfg, 284 void *mlsl_handle, 285 void *accel_handle, 286 void *compass_handle, 287 void *pressure_handle, 288 unsigned long sensors) 289{ 290 int result; 291 unsigned long requested = mldl_cfg->requested_sensors; 292 293 mldl_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS; 294 //MPL_LOGI("%s: suspending sensors to %04lx\n", __func__, 295 // mldl_cfg->requested_sensors); 296 297 result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg); 298 if (result) { 299 LOG_RESULT_LOCATION(result); 300 return result; 301 } 302 result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SUSPEND, NULL); 303 if (result) { 304 LOG_RESULT_LOCATION(result); 305 return result; 306 } 307 result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); 308 if (result) { 309 LOG_RESULT_LOCATION(result); 310 return result; 311 } 312 313 mldl_cfg->requested_sensors = requested; 314 //MPL_LOGI("%s: Will resume next to %04lx\n", __func__, requested); 315 316 return result; 317} 318 319/** 320 * Send slave configuration information 321 * 322 * @param mldl_cfg pointer to the mldl configuration structure 323 * @param gyro_handle handle to the gyro sensor 324 * @param slave_handle handle to the slave sensor 325 * @param slave slave description 326 * @param pdata slave platform data 327 * @param data where to store the read data 328 * 329 * @return 0 or non-zero error code 330 */ 331int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg, 332 void *gyro_handle, 333 void *slave_handle, 334 struct ext_slave_descr *slave, 335 struct ext_slave_platform_data *pdata, 336 unsigned char *data) 337{ 338 int result; 339 if (!mldl_cfg || !gyro_handle || !data || !slave) { 340 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); 341 return INV_ERROR_INVALID_PARAMETER; 342 } 343 344 switch (slave->type) { 345 case EXT_SLAVE_TYPE_ACCELEROMETER: 346 result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_ACCEL, data); 347 break; 348 case EXT_SLAVE_TYPE_COMPASS: 349 result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_COMPASS, data); 350 break; 351 case EXT_SLAVE_TYPE_PRESSURE: 352 result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_PRESSURE, data); 353 break; 354 default: 355 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); 356 return INV_ERROR_INVALID_PARAMETER; 357 break; 358 } 359 360 return result; 361} 362 363/** 364 * Send slave configuration information 365 * 366 * @param mldl_cfg pointer to the mldl configuration structure 367 * @param gyro_handle handle to the gyro sensor 368 * @param slave_handle handle to the slave sensor 369 * @param data the data being sent 370 * @param slave slave description 371 * @param pdata slave platform data 372 * 373 * @return 0 or non-zero error code 374 */ 375int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg, 376 void *gyro_handle, 377 void *slave_handle, 378 struct ext_slave_config *data, 379 struct ext_slave_descr *slave, 380 struct ext_slave_platform_data *pdata) 381{ 382 int result; 383 if (!mldl_cfg || !data || !slave || !pdata || !slave) { 384 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); 385 return INV_ERROR_INVALID_PARAMETER; 386 } 387 388 switch (slave->type) { 389 case EXT_SLAVE_TYPE_ACCELEROMETER: 390 result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_ACCEL, data); 391 if (result) { 392 LOG_RESULT_LOCATION(result); 393 return result; 394 } 395 break; 396 case EXT_SLAVE_TYPE_COMPASS: 397 result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_COMPASS, data); 398 if (result) { 399 LOG_RESULT_LOCATION(result); 400 return result; 401 } 402 break; 403 case EXT_SLAVE_TYPE_PRESSURE: 404 result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_PRESSURE, data); 405 if (result) { 406 LOG_RESULT_LOCATION(result); 407 return result; 408 } 409 break; 410 default: 411 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); 412 return INV_ERROR_INVALID_PARAMETER; 413 break; 414 } 415 416 return result; 417} 418 419/** 420 * Request slave configuration information 421 * 422 * Use this specifically after requesting a slave configuration to see what the 423 * slave accually accepted. 424 * 425 * @param mldl_cfg pointer to the mldl configuration structure 426 * @param gyro_handle handle to the gyro sensor 427 * @param slave_handle handle to the slave sensor 428 * @param data the data being requested. 429 * @param slave slave description 430 * @param pdata slave platform data 431 * 432 * @return 0 or non-zero error code 433 */ 434int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg, 435 void *gyro_handle, 436 void *slave_handle, 437 struct ext_slave_config *data, 438 struct ext_slave_descr *slave, 439 struct ext_slave_platform_data *pdata) 440{ 441 int result; 442 if (!mldl_cfg || !data || !slave) { 443 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); 444 return INV_ERROR_INVALID_PARAMETER; 445 } 446 switch (slave->type) { 447 case EXT_SLAVE_TYPE_ACCELEROMETER: 448 result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_ACCEL, data); 449 if (result) { 450 LOG_RESULT_LOCATION(result); 451 return result; 452 } 453 break; 454 case EXT_SLAVE_TYPE_COMPASS: 455 result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_COMPASS, data); 456 if (result) { 457 LOG_RESULT_LOCATION(result); 458 return result; 459 } 460 break; 461 case EXT_SLAVE_TYPE_PRESSURE: 462 result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_PRESSURE, data); 463 if (result) { 464 LOG_RESULT_LOCATION(result); 465 return result; 466 } 467 break; 468 default: 469 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); 470 return INV_ERROR_INVALID_PARAMETER; 471 break; 472 } 473 return result; 474} 475/** 476 *@} 477 */ 478