1 /* 2 * Copyright (C) 2012 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 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]); 112 fclose(fptr); 113 114 LOGV_IF(EXTRA_VERBOSE, 115 "HAL:compass mounting matrix: " 116 "%+d %+d %+d %+d %+d %+d %+d %+d %+d", 117 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); 118 119 mCompassOrientation[0] = om[0]; 120 mCompassOrientation[1] = om[1]; 121 mCompassOrientation[2] = om[2]; 122 mCompassOrientation[3] = om[3]; 123 mCompassOrientation[4] = om[4]; 124 mCompassOrientation[5] = om[5]; 125 mCompassOrientation[6] = om[6]; 126 mCompassOrientation[7] = om[7]; 127 mCompassOrientation[8] = om[8]; 128 } else { 129 LOGE("HAL:Couldn't read compass mounting matrix"); 130 } 131 132 if(mYasCompass) { 133 mCoilsResetFd = fopen(compassSysFs.compass_attr_1, "r+"); 134 if (fptr == NULL) { 135 LOGE("HAL:Couldn't read compass overunderflow"); 136 } 137 } 138 } 139 140 void CompassSensor::enable_iio_sysfs() 141 { 142 VFUNC_LOG; 143 144 int tempFd = 0; 145 char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN]; 146 FILE *tempFp = NULL; 147 const char* compass = dev_full_name; 148 149 write_sysfs_int(compassSysFs.in_timestamp_en, 1); 150 151 LOGV_IF(SYSFS_VERBOSE, 152 "HAL:sysfs:cat %s (%lld)", 153 compassSysFs.trigger_name, getTimestamp()); 154 tempFp = fopen(compassSysFs.trigger_name, "r"); 155 if (tempFp == NULL) { 156 LOGE("HAL:could not open %s trigger name", compass); 157 } else { 158 if (fscanf(tempFp, "%s", iio_trigger_name) < 0) { 159 LOGE("HAL:could not read trigger name"); 160 } 161 fclose(tempFp); 162 } 163 164 LOGV_IF(SYSFS_VERBOSE, 165 "HAL:sysfs:echo %s > %s (%lld)", 166 iio_trigger_name, compassSysFs.current_trigger, getTimestamp()); 167 tempFp = fopen(compassSysFs.current_trigger, "w"); 168 if (tempFp == NULL) { 169 LOGE("HAL:could not open current trigger"); 170 } else { 171 if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) { 172 LOGE("HAL:could not write current trigger"); 173 } 174 } 175 176 LOGV_IF(SYSFS_VERBOSE, 177 "HAL:sysfs:echo %d > %s (%lld)", 178 IIO_BUFFER_LENGTH, compassSysFs.buffer_length, getTimestamp()); 179 tempFp = fopen(compassSysFs.buffer_length, "w"); 180 if (tempFp == NULL) { 181 LOGE("HAL:could not open buffer length"); 182 } else { 183 if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) { 184 LOGE("HAL:could not write buffer length"); 185 } 186 } 187 188 sprintf(iio_device_node, "%s%d", "/dev/iio:device", 189 find_type_by_name(compass, "iio:device")); 190 compass_fd = open(iio_device_node, O_RDONLY); 191 int res = errno; 192 if (compass_fd < 0) { 193 LOGE("HAL:could not open '%s' iio device node in path '%s' - " 194 "error '%s' (%d)", 195 compass, iio_device_node, strerror(res), res); 196 } else { 197 LOGV_IF(EXTRA_VERBOSE, 198 "HAL:iio %s, compass_fd opened : %d", compass, compass_fd); 199 } 200 201 /* TODO: need further tests for optimization to reduce context-switch 202 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 203 compassSysFs.compass_x_fifo_enable, getTimestamp()); 204 tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR); 205 res = errno; 206 if (tempFd > 0) { 207 res = enable_sysfs_sensor(tempFd, 1); 208 } else { 209 LOGE("HAL:open of %s failed with '%s' (%d)", 210 compassSysFs.compass_x_fifo_enable, strerror(res), res); 211 } 212 213 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 214 compassSysFs.compass_y_fifo_enable, getTimestamp()); 215 tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR); 216 res = errno; 217 if (tempFd > 0) { 218 res = enable_sysfs_sensor(tempFd, 1); 219 } else { 220 LOGE("HAL:open of %s failed with '%s' (%d)", 221 compassSysFs.compass_y_fifo_enable, strerror(res), res); 222 } 223 224 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 225 compassSysFs.compass_z_fifo_enable, getTimestamp()); 226 tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR); 227 res = errno; 228 if (tempFd > 0) { 229 res = enable_sysfs_sensor(tempFd, 1); 230 } else { 231 LOGE("HAL:open of %s failed with '%s' (%d)", 232 compassSysFs.compass_z_fifo_enable, strerror(res), res); 233 } 234 */ 235 } 236 237 CompassSensor::~CompassSensor() 238 { 239 VFUNC_LOG; 240 241 free(pathP); 242 if( compass_fd > 0) 243 close(compass_fd); 244 if(mYasCompass) { 245 if( mCoilsResetFd != NULL ) 246 fclose(mCoilsResetFd); 247 } 248 } 249 250 int CompassSensor::getFd(void) const 251 { 252 VHANDLER_LOG; 253 LOGI_IF(0, "HAL:compass_fd=%d", compass_fd); 254 return compass_fd; 255 } 256 257 /** 258 * @brief This function will enable/disable sensor. 259 * @param[in] handle 260 * which sensor to enable/disable. 261 * @param[in] en 262 * en=1, enable; 263 * en=0, disable 264 * @return if the operation is successful. 265 */ 266 int CompassSensor::enable(int32_t handle, int en) 267 { 268 VFUNC_LOG; 269 270 mEnable = en; 271 int tempFd; 272 int res = 0; 273 274 /* reset master enable */ 275 res = masterEnable(0); 276 if (res < 0) { 277 return res; 278 } 279 280 if (en) { 281 res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en); 282 res += write_sysfs_int(compassSysFs.compass_y_fifo_enable, en); 283 res += write_sysfs_int(compassSysFs.compass_z_fifo_enable, en); 284 285 res = masterEnable(en); 286 if (res < en) { 287 return res; 288 } 289 } 290 291 return res; 292 } 293 294 int CompassSensor::masterEnable(int en) { 295 VFUNC_LOG; 296 return write_sysfs_int(compassSysFs.chip_enable, en); 297 } 298 299 int CompassSensor::setDelay(int32_t handle, int64_t ns) 300 { 301 VFUNC_LOG; 302 int tempFd; 303 int res; 304 305 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 306 1000000000.f / ns, compassSysFs.compass_rate, getTimestamp()); 307 mDelay = ns; 308 if (ns == 0) 309 return -1; 310 tempFd = open(compassSysFs.compass_rate, O_RDWR); 311 res = write_attribute_sensor(tempFd, 1000000000.f / ns); 312 if(res < 0) { 313 LOGE("HAL:Compass update delay error"); 314 } 315 return res; 316 } 317 318 /** 319 @brief This function will return the state of the sensor. 320 @return 1=enabled; 0=disabled 321 **/ 322 int CompassSensor::getEnable(int32_t handle) 323 { 324 VFUNC_LOG; 325 return mEnable; 326 } 327 328 /* use for Invensense compass calibration */ 329 #define COMPASS_EVENT_DEBUG (0) 330 void CompassSensor::processCompassEvent(const input_event *event) 331 { 332 VHANDLER_LOG; 333 334 switch (event->code) { 335 case EVENT_TYPE_ICOMPASS_X: 336 LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n"); 337 mCachedCompassData[0] = event->value; 338 break; 339 case EVENT_TYPE_ICOMPASS_Y: 340 LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n"); 341 mCachedCompassData[1] = event->value; 342 break; 343 case EVENT_TYPE_ICOMPASS_Z: 344 LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n"); 345 mCachedCompassData[2] = event->value; 346 break; 347 } 348 349 mCompassTimestamp = 350 (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L; 351 } 352 353 void CompassSensor::getOrientationMatrix(signed char *orient) 354 { 355 VFUNC_LOG; 356 memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation)); 357 } 358 359 long CompassSensor::getSensitivity() 360 { 361 VFUNC_LOG; 362 363 long sensitivity; 364 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 365 compassSysFs.compass_scale, getTimestamp()); 366 inv_read_data(compassSysFs.compass_scale, &sensitivity); 367 return sensitivity; 368 } 369 370 /** 371 @brief This function is called by sensors_mpl.cpp 372 to read sensor data from the driver. 373 @param[out] data sensor data is stored in this variable. Scaled such that 374 1 uT = 2^16 375 @para[in] timestamp data's timestamp 376 @return 1, if 1 sample read, 0, if not, negative if error 377 */ 378 int CompassSensor::readSample(long *data, int64_t *timestamp) { 379 VFUNC_LOG; 380 381 int i; 382 char *rdata = mIIOBuffer; 383 384 size_t rsize = read(compass_fd, rdata, (8 * mEnable + 8) * 1); 385 386 if (!mEnable) { 387 rsize = read(compass_fd, rdata, (8 + 8) * IIO_BUFFER_LENGTH); 388 // LOGI("clear buffer with size: %d", rsize); 389 } 390 /* 391 LOGI("get one sample of AMI IIO data with size: %d", rsize); 392 LOGI_IF(mEnable, "compass x/y/z: %d/%d/%d", *((short *) (rdata + 0)), 393 *((short *) (rdata + 2)), *((short *) (rdata + 4))); 394 */ 395 if (mEnable) { 396 for (i = 0; i < 3; i++) { 397 data[i] = *((short *) (rdata + i * 2)); 398 } 399 *timestamp = *((long long *) (rdata + 8 * mEnable)); 400 } 401 402 return mEnable; 403 } 404 405 /** 406 * @brief This function will return the current delay for this sensor. 407 * @return delay in nanoseconds. 408 */ 409 int64_t CompassSensor::getDelay(int32_t handle) 410 { 411 VFUNC_LOG; 412 return mDelay; 413 } 414 415 void CompassSensor::fillList(struct sensor_t *list) 416 { 417 VFUNC_LOG; 418 419 const char *compass = dev_full_name; 420 421 if (compass) { 422 if(!strcmp(compass, "INV_COMPASS")) { 423 list->maxRange = COMPASS_MPU9150_RANGE; 424 list->resolution = COMPASS_MPU9150_RESOLUTION; 425 list->power = COMPASS_MPU9150_POWER; 426 list->minDelay = COMPASS_MPU9150_MINDELAY; 427 mMinDelay = list->minDelay; 428 return; 429 } 430 if(!strcmp(compass, "compass") 431 || !strcmp(compass, "INV_AK8975") 432 || !strncmp(compass, "ak89xx", 2)) { 433 list->maxRange = COMPASS_AKM8975_RANGE; 434 list->resolution = COMPASS_AKM8975_RESOLUTION; 435 list->power = COMPASS_AKM8975_POWER; 436 list->minDelay = COMPASS_AKM8975_MINDELAY; 437 mMinDelay = list->minDelay; 438 return; 439 } 440 if(!strcmp(compass, "ami306")) { 441 list->maxRange = COMPASS_AMI306_RANGE; 442 list->resolution = COMPASS_AMI306_RESOLUTION; 443 list->power = COMPASS_AMI306_POWER; 444 list->minDelay = COMPASS_AMI306_MINDELAY; 445 mMinDelay = list->minDelay; 446 return; 447 } 448 if(!strcmp(compass, "yas530") 449 || !strcmp(compass, "yas532") 450 || !strcmp(compass, "yas533")) { 451 list->maxRange = COMPASS_YAS53x_RANGE; 452 list->resolution = COMPASS_YAS53x_RESOLUTION; 453 list->power = COMPASS_YAS53x_POWER; 454 list->minDelay = COMPASS_YAS53x_MINDELAY; 455 mMinDelay = list->minDelay; 456 return; 457 } 458 } 459 460 LOGE("HAL:unknown compass id %s -- " 461 "params default to ak8975 and might be wrong.", 462 compass); 463 list->maxRange = COMPASS_AKM8975_RANGE; 464 list->resolution = COMPASS_AKM8975_RESOLUTION; 465 list->power = COMPASS_AKM8975_POWER; 466 list->minDelay = COMPASS_AKM8975_MINDELAY; 467 mMinDelay = list->minDelay; 468 } 469 470 /* Read sysfs entry to determine whether overflow had happend 471 then write to sysfs to reset to zero */ 472 int CompassSensor::checkCoilsReset() 473 { 474 int result=-1; 475 VFUNC_LOG; 476 477 if(mCoilsResetFd != NULL) { 478 int attr; 479 rewind(mCoilsResetFd); 480 fscanf(mCoilsResetFd, "%d", &attr); 481 if(attr == 0) 482 return 0; 483 else { 484 LOGV_IF(SYSFS_VERBOSE, "HAL:overflow detected"); 485 rewind(mCoilsResetFd); 486 if(fprintf(mCoilsResetFd, "%d", 0) < 0) 487 LOGE("HAL:could not write overunderflow"); 488 else 489 return 1; 490 } 491 } else { 492 LOGE("HAL:could not read overunderflow"); 493 } 494 return result; 495 } 496 497 int CompassSensor::inv_init_sysfs_attributes(void) 498 { 499 VFUNC_LOG; 500 501 unsigned char i = 0; 502 char sysfs_path[MAX_SYSFS_NAME_LEN], 503 iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2]; 504 char *sptr; 505 char **dptr; 506 int num; 507 const char* compass = dev_full_name; 508 509 pathP = (char*)malloc( 510 sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); 511 sptr = pathP; 512 dptr = (char**)&compassSysFs; 513 if (sptr == NULL) 514 return -1; 515 516 do { 517 *dptr++ = sptr; 518 sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); 519 } while (++i < COMPASS_MAX_SYSFS_ATTRB); 520 521 // get proper (in absolute/relative) IIO path & build sysfs paths 522 sprintf(sysfs_path, "%s%d", "/sys/bus/iio/devices/iio:device", 523 find_type_by_name(compass, "iio:device")); 524 sprintf(iio_trigger_path, "%s%d", "/sys/bus/iio/devices/trigger", 525 find_type_by_name(compass, "iio:device")); 526 527 #if defined COMPASS_AK8975 528 inv_get_input_number(compass, &num); 529 tbuf[0] = num + 0x30; 530 tbuf[1] = 0; 531 sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf); 532 strcat(sysfs_path, "/ak8975"); 533 534 sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable"); 535 sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate"); 536 sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale"); 537 sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); 538 #else /* IIO */ 539 sprintf(compassSysFs.chip_enable, "%s%s", sysfs_path, "/buffer/enable"); 540 sprintf(compassSysFs.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en"); 541 sprintf(compassSysFs.trigger_name, "%s%s", iio_trigger_path, "/name"); 542 sprintf(compassSysFs.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger"); 543 sprintf(compassSysFs.buffer_length, "%s%s", sysfs_path, "/buffer/length"); 544 545 sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en"); 546 sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en"); 547 sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en"); 548 sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency"); 549 sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale"); 550 sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); 551 552 if(mYasCompass) { 553 sprintf(compassSysFs.compass_attr_1, "%s%s", sysfs_path, "/overunderflow"); 554 } 555 #endif 556 557 #if 0 558 // test print sysfs paths 559 dptr = (char**)&compassSysFs; 560 LOGI("sysfs path base: %s", sysfs_path); 561 LOGI("trigger sysfs path base: %s", iio_trigger_path); 562 for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) { 563 LOGE("HAL:sysfs path: %s", *dptr++); 564 } 565 #endif 566 return 0; 567 } 568 569 int CompassSensor::isYasCompass(void) 570 { 571 return mYasCompass; 572 } 573
