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
CompassSensor()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
enable_iio_sysfs()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
~CompassSensor()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
getFd(void) const226 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 */
enable(int32_t handle,int en)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
masterEnable(int en)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
setDelay(int32_t handle,int64_t ns)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 **/
getEnable(int32_t handle)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)
processCompassEvent(const input_event * event)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
getOrientationMatrix(signed char * orient)338 void CompassSensor::getOrientationMatrix(signed char *orient)
339 {
340 VFUNC_LOG;
341 memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation));
342 }
343
getSensitivity()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 */
readSample(long * data,int64_t * timestamp)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 */
getDelay(int32_t handle)394 int64_t CompassSensor::getDelay(int32_t handle)
395 {
396 VFUNC_LOG;
397 return mDelay;
398 }
399
fillList(struct sensor_t * list)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 */
checkCoilsReset()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
inv_init_sysfs_attributes(void)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
isYasCompass(void)582 int CompassSensor::isYasCompass(void)
583 {
584 return mYasCompass;
585 }
586