1 /* 2 * Copyright (C) 2014 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #define LOG_NDEBUG 0 18 19 #include <fcntl.h> 20 #include <errno.h> 21 #include <math.h> 22 #include <unistd.h> 23 #include <dirent.h> 24 #include <sys/select.h> 25 #include <cutils/log.h> 26 #include <linux/input.h> 27 #include <string.h> 28 29 #include "CompassSensor.IIO.primary.h" 30 #include "sensors.h" 31 #include "MPLSupport.h" 32 #include "sensor_params.h" 33 #include "ml_sysfs_helper.h" 34 35 #define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*) 36 #define COMPASS_NAME "USE_SYSFS" 37 38 #if defined COMPASS_AK8975 39 #pragma message("HAL:build Invensense compass cal with AK8975 on primary bus") 40 #define USE_MPL_COMPASS_HAL (1) 41 #define COMPASS_NAME "INV_AK8975" 42 #endif 43 44 /******************************************************************************/ 45 46 CompassSensor::CompassSensor() 47 : SensorBase(COMPASS_NAME, NULL), 48 mCompassTimestamp(0), 49 mCompassInputReader(8), 50 mCoilsResetFd(0) 51 { 52 FILE *fptr; 53 54 VFUNC_LOG; 55 56 mYasCompass = false; 57 if(!strcmp(dev_name, "USE_SYSFS")) { 58 char sensor_name[20]; 59 find_name_by_sensor_type("in_magn_x_raw", "iio:device", sensor_name); 60 strncpy(dev_full_name, sensor_name, 61 sizeof(dev_full_name) / sizeof(dev_full_name[0])); 62 if(!strncmp(dev_full_name, "yas", 3)) { 63 mYasCompass = true; 64 } 65 } else { 66 67 #ifdef COMPASS_YAS53x 68 /* for YAS53x compasses, dev_name is just a prefix, 69 we need to find the actual name */ 70 if (fill_dev_full_name_by_prefix(dev_name, 71 dev_full_name, sizeof(dev_full_name) / sizeof(dev_full_name[0]))) { 72 LOGE("Cannot find Yamaha device with prefix name '%s' - " 73 "magnetometer will likely not work.", dev_name); 74 } else { 75 mYasCompass = true; 76 } 77 #else 78 strncpy(dev_full_name, dev_name, 79 sizeof(dev_full_name) / sizeof(dev_full_name[0])); 80 #endif 81 82 } 83 84 if (inv_init_sysfs_attributes()) { 85 LOGE("Error Instantiating Compass\n"); 86 return; 87 } 88 89 if (!strcmp(dev_full_name, "INV_COMPASS")) { 90 mI2CBus = COMPASS_BUS_SECONDARY; 91 } else { 92 mI2CBus = COMPASS_BUS_PRIMARY; 93 } 94 95 memset(mCachedCompassData, 0, sizeof(mCachedCompassData)); 96 97 if (!isIntegrated()) { 98 enable(ID_M, 0); 99 } 100 101 LOGV_IF(SYSFS_VERBOSE, "HAL:compass name: %s", dev_full_name); 102 enable_iio_sysfs(); 103 104 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 105 compassSysFs.compass_orient, getTimestamp()); 106 fptr = fopen(compassSysFs.compass_orient, "r"); 107 if (fptr != NULL) { 108 int om[9]; 109 if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 110 &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], 111 &om[6], &om[7], &om[8]) < 0 || fclose(fptr)) { 112 LOGE("HAL:could not read compass mounting matrix"); 113 } else { 114 115 LOGV_IF(EXTRA_VERBOSE, 116 "HAL:compass mounting matrix: " 117 "%+d %+d %+d %+d %+d %+d %+d %+d %+d", 118 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); 119 120 mCompassOrientation[0] = om[0]; 121 mCompassOrientation[1] = om[1]; 122 mCompassOrientation[2] = om[2]; 123 mCompassOrientation[3] = om[3]; 124 mCompassOrientation[4] = om[4]; 125 mCompassOrientation[5] = om[5]; 126 mCompassOrientation[6] = om[6]; 127 mCompassOrientation[7] = om[7]; 128 mCompassOrientation[8] = om[8]; 129 } 130 } 131 132 if(mYasCompass) { 133 mCoilsResetFd = fopen(compassSysFs.compass_attr_1, "r+"); 134 if (fptr == NULL) { 135 LOGE("HAL:Could not open compass overunderflow"); 136 } 137 } 138 } 139 140 void CompassSensor::enable_iio_sysfs() 141 { 142 VFUNC_LOG; 143 144 int tempFd = 0; 145 char iio_device_node[MAX_CHIP_ID_LEN]; 146 FILE *tempFp = NULL; 147 const char* compass = dev_full_name; 148 149 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 150 1, compassSysFs.in_timestamp_en, getTimestamp()); 151 write_sysfs_int(compassSysFs.in_timestamp_en, 1); 152 153 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 154 IIO_BUFFER_LENGTH, compassSysFs.buffer_length, getTimestamp()); 155 tempFp = fopen(compassSysFs.buffer_length, "w"); 156 if (tempFp == NULL) { 157 LOGE("HAL:could not open buffer length"); 158 } else { 159 if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) { 160 LOGE("HAL:could not write buffer length"); 161 } 162 } 163 164 sprintf(iio_device_node, "%s%d", "/dev/iio:device", 165 find_type_by_name(compass, "iio:device")); 166 compass_fd = open(iio_device_node, O_RDONLY); 167 int res = errno; 168 if (compass_fd < 0) { 169 LOGE("HAL:could not open '%s' iio device node in path '%s' - " 170 "error '%s' (%d)", 171 compass, iio_device_node, strerror(res), res); 172 } else { 173 LOGV_IF(EXTRA_VERBOSE, 174 "HAL:iio %s, compass_fd opened : %d", compass, compass_fd); 175 } 176 177 /* TODO: need further tests for optimization to reduce context-switch 178 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 179 compassSysFs.compass_x_fifo_enable, getTimestamp()); 180 tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR); 181 res = errno; 182 if (tempFd > 0) { 183 res = enable_sysfs_sensor(tempFd, 1); 184 } else { 185 LOGE("HAL:open of %s failed with '%s' (%d)", 186 compassSysFs.compass_x_fifo_enable, strerror(res), res); 187 } 188 189 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 190 compassSysFs.compass_y_fifo_enable, getTimestamp()); 191 tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR); 192 res = errno; 193 if (tempFd > 0) { 194 res = enable_sysfs_sensor(tempFd, 1); 195 } else { 196 LOGE("HAL:open of %s failed with '%s' (%d)", 197 compassSysFs.compass_y_fifo_enable, strerror(res), res); 198 } 199 200 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 201 compassSysFs.compass_z_fifo_enable, getTimestamp()); 202 tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR); 203 res = errno; 204 if (tempFd > 0) { 205 res = enable_sysfs_sensor(tempFd, 1); 206 } else { 207 LOGE("HAL:open of %s failed with '%s' (%d)", 208 compassSysFs.compass_z_fifo_enable, strerror(res), res); 209 } 210 */ 211 } 212 213 CompassSensor::~CompassSensor() 214 { 215 VFUNC_LOG; 216 217 free(pathP); 218 if( compass_fd > 0) 219 close(compass_fd); 220 if(mYasCompass) { 221 if( mCoilsResetFd != NULL ) 222 fclose(mCoilsResetFd); 223 } 224 } 225 226 int CompassSensor::getFd(void) const 227 { 228 VHANDLER_LOG; 229 LOGI_IF(0, "HAL:compass_fd=%d", compass_fd); 230 return compass_fd; 231 } 232 233 /** 234 * @brief This function will enable/disable sensor. 235 * @param[in] handle 236 * which sensor to enable/disable. 237 * @param[in] en 238 * en=1, enable; 239 * en=0, disable 240 * @return if the operation is successful. 241 */ 242 int CompassSensor::enable(int32_t handle, int en) 243 { 244 VFUNC_LOG; 245 246 mEnable = en; 247 int tempFd; 248 int res = 0; 249 250 /* reset master enable */ 251 res = masterEnable(0); 252 if (res < 0) { 253 return res; 254 } 255 256 if (en) { 257 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 258 en, compassSysFs.compass_x_fifo_enable, getTimestamp()); 259 res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en); 260 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 261 en, compassSysFs.compass_y_fifo_enable, getTimestamp()); 262 res += write_sysfs_int(compassSysFs.compass_y_fifo_enable, en); 263 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 264 en, compassSysFs.compass_z_fifo_enable, getTimestamp()); 265 res += write_sysfs_int(compassSysFs.compass_z_fifo_enable, en); 266 267 res = masterEnable(en); 268 if (res < en) { 269 return res; 270 } 271 } 272 273 return res; 274 } 275 276 int CompassSensor::masterEnable(int en) 277 { 278 VFUNC_LOG; 279 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 280 en, compassSysFs.chip_enable, getTimestamp()); 281 return write_sysfs_int(compassSysFs.chip_enable, en); 282 } 283 284 int CompassSensor::setDelay(int32_t handle, int64_t ns) 285 { 286 VFUNC_LOG; 287 int tempFd; 288 int res; 289 290 mDelay = ns; 291 if (ns == 0) 292 return -1; 293 tempFd = open(compassSysFs.compass_rate, O_RDWR); 294 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 295 1000000000.f / ns, compassSysFs.compass_rate, getTimestamp()); 296 res = write_attribute_sensor(tempFd, 1000000000.f / ns); 297 if(res < 0) { 298 LOGE("HAL:Compass update delay error"); 299 } 300 return res; 301 } 302 303 /** 304 @brief This function will return the state of the sensor. 305 @return 1=enabled; 0=disabled 306 **/ 307 int CompassSensor::getEnable(int32_t handle) 308 { 309 VFUNC_LOG; 310 return mEnable; 311 } 312 313 /* use for Invensense compass calibration */ 314 #define COMPASS_EVENT_DEBUG (0) 315 void CompassSensor::processCompassEvent(const input_event *event) 316 { 317 VHANDLER_LOG; 318 319 switch (event->code) { 320 case EVENT_TYPE_ICOMPASS_X: 321 LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n"); 322 mCachedCompassData[0] = event->value; 323 break; 324 case EVENT_TYPE_ICOMPASS_Y: 325 LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n"); 326 mCachedCompassData[1] = event->value; 327 break; 328 case EVENT_TYPE_ICOMPASS_Z: 329 LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n"); 330 mCachedCompassData[2] = event->value; 331 break; 332 } 333 334 mCompassTimestamp = 335 (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L; 336 } 337 338 void CompassSensor::getOrientationMatrix(signed char *orient) 339 { 340 VFUNC_LOG; 341 memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation)); 342 } 343 344 long CompassSensor::getSensitivity() 345 { 346 VFUNC_LOG; 347 348 long sensitivity; 349 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 350 compassSysFs.compass_scale, getTimestamp()); 351 inv_read_data(compassSysFs.compass_scale, &sensitivity); 352 return sensitivity; 353 } 354 355 /** 356 @brief This function is called by sensors_mpl.cpp 357 to read sensor data from the driver. 358 @param[out] data sensor data is stored in this variable. Scaled such that 359 1 uT = 2^16 360 @para[in] timestamp data's timestamp 361 @return 1, if 1 sample read, 0, if not, negative if error 362 */ 363 int CompassSensor::readSample(long *data, int64_t *timestamp) { 364 VFUNC_LOG; 365 366 int i; 367 char *rdata = mIIOBuffer; 368 369 size_t rsize = read(compass_fd, rdata, (8 * mEnable + 8) * 1); 370 371 if (!mEnable) { 372 rsize = read(compass_fd, rdata, (8 + 8) * IIO_BUFFER_LENGTH); 373 // LOGI("clear buffer with size: %d", rsize); 374 } 375 /* 376 LOGI("get one sample of AMI IIO data with size: %d", rsize); 377 LOGI_IF(mEnable, "compass x/y/z: %d/%d/%d", *((short *) (rdata + 0)), 378 *((short *) (rdata + 2)), *((short *) (rdata + 4))); 379 */ 380 if (mEnable) { 381 for (i = 0; i < 3; i++) { 382 data[i] = *((short *) (rdata + i * 2)); 383 } 384 *timestamp = *((long long *) (rdata + 8 * mEnable)); 385 } 386 387 return mEnable; 388 } 389 390 /** 391 * @brief This function will return the current delay for this sensor. 392 * @return delay in nanoseconds. 393 */ 394 int64_t CompassSensor::getDelay(int32_t handle) 395 { 396 VFUNC_LOG; 397 return mDelay; 398 } 399 400 void CompassSensor::fillList(struct sensor_t *list) 401 { 402 VFUNC_LOG; 403 404 const char *compass = dev_full_name; 405 406 if (compass) { 407 if(!strcmp(compass, "INV_COMPASS")) { 408 list->maxRange = COMPASS_MPU9150_RANGE; 409 list->resolution = COMPASS_MPU9150_RESOLUTION; 410 list->power = COMPASS_MPU9150_POWER; 411 list->minDelay = COMPASS_MPU9150_MINDELAY; 412 mMinDelay = list->minDelay; 413 return; 414 } 415 if(!strcmp(compass, "compass") 416 || !strcmp(compass, "INV_AK8975") 417 || !strcmp(compass, "AKM8975") 418 || !strcmp(compass, "akm8975")) { 419 list->maxRange = COMPASS_AKM8975_RANGE; 420 list->resolution = COMPASS_AKM8975_RESOLUTION; 421 list->power = COMPASS_AKM8975_POWER; 422 list->minDelay = COMPASS_AKM8975_MINDELAY; 423 mMinDelay = list->minDelay; 424 return; 425 } 426 if(!strcmp(compass, "compass") 427 || !strcmp(compass, "INV_AK8963") 428 || !strcmp(compass, "AKM8963") 429 || !strcmp(compass, "akm8963")) { 430 list->maxRange = COMPASS_AKM8963_RANGE; 431 list->resolution = COMPASS_AKM8963_RESOLUTION; 432 list->power = COMPASS_AKM8963_POWER; 433 list->minDelay = COMPASS_AKM8963_MINDELAY; 434 mMinDelay = list->minDelay; 435 return; 436 } 437 if(!strcmp(compass, "compass") 438 || !strcmp(compass, "INV_AK09911") 439 || !strcmp(compass, "AK09911") 440 || !strcmp(compass, "ak09911")) { 441 list->maxRange = COMPASS_AKM9911_RANGE; 442 list->resolution = COMPASS_AKM9911_RESOLUTION; 443 list->power = COMPASS_AKM9911_POWER; 444 list->minDelay = COMPASS_AKM9911_MINDELAY; 445 mMinDelay = list->minDelay; 446 return; 447 } 448 if(!strcmp(compass, "compass") 449 || !strcmp(compass, "INV_AK09912") 450 || !strcmp(compass, "AK09912") 451 || !strcmp(compass, "ak09912")) { 452 list->maxRange = COMPASS_AKM9912_RANGE; 453 list->resolution = COMPASS_AKM9912_RESOLUTION; 454 list->power = COMPASS_AKM9912_POWER; 455 list->minDelay = COMPASS_AKM9912_MINDELAY; 456 mMinDelay = list->minDelay; 457 return; 458 } 459 if(!strcmp(compass, "ami306")) { 460 list->maxRange = COMPASS_AMI306_RANGE; 461 list->resolution = COMPASS_AMI306_RESOLUTION; 462 list->power = COMPASS_AMI306_POWER; 463 list->minDelay = COMPASS_AMI306_MINDELAY; 464 mMinDelay = list->minDelay; 465 return; 466 } 467 if(!strcmp(compass, "yas530") 468 || !strcmp(compass, "yas532") 469 || !strcmp(compass, "yas533")) { 470 list->maxRange = COMPASS_YAS53x_RANGE; 471 list->resolution = COMPASS_YAS53x_RESOLUTION; 472 list->power = COMPASS_YAS53x_POWER; 473 list->minDelay = COMPASS_YAS53x_MINDELAY; 474 mMinDelay = list->minDelay; 475 return; 476 } 477 } 478 479 LOGE("HAL:unknown compass id %s -- " 480 "params default to ak8975 and might be wrong.", 481 compass); 482 list->maxRange = COMPASS_AKM8975_RANGE; 483 list->resolution = COMPASS_AKM8975_RESOLUTION; 484 list->power = COMPASS_AKM8975_POWER; 485 list->minDelay = COMPASS_AKM8975_MINDELAY; 486 mMinDelay = list->minDelay; 487 } 488 489 /* Read sysfs entry to determine whether overflow had happend 490 then write to sysfs to reset to zero */ 491 int CompassSensor::checkCoilsReset() 492 { 493 int result=-1; 494 VFUNC_LOG; 495 496 if(mCoilsResetFd != NULL) { 497 int attr; 498 rewind(mCoilsResetFd); 499 fscanf(mCoilsResetFd, "%d", &attr); 500 if(attr == 0) 501 return 0; 502 else { 503 LOGV_IF(SYSFS_VERBOSE, "HAL:overflow detected"); 504 rewind(mCoilsResetFd); 505 if(fprintf(mCoilsResetFd, "%d", 0) < 0) 506 LOGE("HAL:could not write overunderflow"); 507 else 508 return 1; 509 } 510 } else { 511 LOGE("HAL:could not read overunderflow"); 512 } 513 return result; 514 } 515 516 int CompassSensor::inv_init_sysfs_attributes(void) 517 { 518 VFUNC_LOG; 519 520 unsigned char i = 0; 521 char sysfs_path[MAX_SYSFS_NAME_LEN], tbuf[2]; 522 char *sptr; 523 char **dptr; 524 int num; 525 const char* compass = dev_full_name; 526 527 pathP = (char*)malloc( 528 sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); 529 sptr = pathP; 530 dptr = (char**)&compassSysFs; 531 if (sptr == NULL) 532 return -1; 533 534 do { 535 *dptr++ = sptr; 536 sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); 537 } while (++i < COMPASS_MAX_SYSFS_ATTRB); 538 539 // get proper (in absolute/relative) IIO path & build sysfs paths 540 sprintf(sysfs_path, "%s%d", "/sys/bus/iio/devices/iio:device", 541 find_type_by_name(compass, "iio:device")); 542 543 #if defined COMPASS_AK8975 544 inv_get_input_number(compass, &num); 545 tbuf[0] = num + 0x30; 546 tbuf[1] = 0; 547 sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf); 548 strcat(sysfs_path, "/ak8975"); 549 550 sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable"); 551 sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate"); 552 sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale"); 553 sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); 554 #else /* IIO */ 555 sprintf(compassSysFs.chip_enable, "%s%s", sysfs_path, "/buffer/enable"); 556 sprintf(compassSysFs.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en"); 557 sprintf(compassSysFs.buffer_length, "%s%s", sysfs_path, "/buffer/length"); 558 559 sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en"); 560 sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en"); 561 sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en"); 562 sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency"); 563 sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale"); 564 sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); 565 566 if(mYasCompass) { 567 sprintf(compassSysFs.compass_attr_1, "%s%s", sysfs_path, "/overunderflow"); 568 } 569 #endif 570 571 #if 0 572 // test print sysfs paths 573 dptr = (char**)&compassSysFs; 574 LOGI("sysfs path base: %s", sysfs_path); 575 for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) { 576 LOGE("HAL:sysfs path: %s", *dptr++); 577 } 578 #endif 579 return 0; 580 } 581 582 int CompassSensor::isYasCompass(void) 583 { 584 return mYasCompass; 585 } 586
