1 /*
2 * Copyright (C) 2012 Invensense, Inc.
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 //see also the EXTRA_VERBOSE define in the MPLSensor.h header file
19 
20 #include <fcntl.h>
21 #include <errno.h>
22 #include <math.h>
23 #include <float.h>
24 #include <poll.h>
25 #include <unistd.h>
26 #include <dirent.h>
27 #include <stdlib.h>
28 #include <sys/select.h>
29 #include <sys/syscall.h>
30 #include <dlfcn.h>
31 #include <pthread.h>
32 #include <cutils/log.h>
33 #include <utils/KeyedVector.h>
34 #include <utils/String8.h>
35 #include <string.h>
36 #include <linux/input.h>
37 #include <utils/Atomic.h>
38 
39 #include "MPLSensor.h"
40 #include "MPLSupport.h"
41 #include "sensor_params.h"
42 #include "local_log_def.h"
43 
44 #include "invensense.h"
45 #include "invensense_adv.h"
46 #include "ml_stored_data.h"
47 #include "ml_load_dmp.h"
48 #include "ml_sysfs_helper.h"
49 
50 // #define TESTING
51 
52 #ifdef THIRD_PARTY_ACCEL
53 #   warning "Third party accel"
54 #   define USE_THIRD_PARTY_ACCEL        (1)
55 #else
56 #   define USE_THIRD_PARTY_ACCEL        (0)
57 #endif
58 
59 #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
60 
61 /******************************************************************************/
62 /*  MPL interface misc.                                                       */
63 /******************************************************************************/
64 static int hertz_request = 200;
65 
66 #define DEFAULT_MPL_GYRO_RATE           (20000L)     //us
67 #define DEFAULT_MPL_COMPASS_RATE        (20000L)     //us
68 
69 #define DEFAULT_HW_GYRO_RATE            (100)        //Hz
70 #define DEFAULT_HW_ACCEL_RATE           (20)         //ms
71 #define DEFAULT_HW_COMPASS_RATE         (20000000L)  //ns
72 #define DEFAULT_HW_AKMD_COMPASS_RATE    (200000000L) //ns
73 
74 /* convert ns to hardware units */
75 #define HW_GYRO_RATE_NS                 (1000000000LL / rate_request) // to Hz
76 #define HW_ACCEL_RATE_NS                (rate_request / (1000000L))   // to ms
77 #define HW_COMPASS_RATE_NS              (rate_request)                // to ns
78 
79 /* convert Hz to hardware units */
80 #define HW_GYRO_RATE_HZ                 (hertz_request)
81 #define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
82 #define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)
83 
84 #define RATE_200HZ                      5000000LL
85 #define RATE_15HZ                       66667000LL
86 #define RATE_5HZ                        200000000LL
87 
88 static struct sensor_t sSensorList[] =
89 {
90     {"MPL Gyroscope", "Invensense", 1,
91      SENSORS_GYROSCOPE_HANDLE,
92      SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
93     {"MPL Raw Gyroscope", "Invensense", 1,
94      SENSORS_RAW_GYROSCOPE_HANDLE,
95      SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
96     {"MPL Accelerometer", "Invensense", 1,
97      SENSORS_ACCELERATION_HANDLE,
98      SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
99     {"MPL Magnetic Field", "Invensense", 1,
100      SENSORS_MAGNETIC_FIELD_HANDLE,
101      SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
102     {"MPL Orientation", "Invensense", 1,
103      SENSORS_ORIENTATION_HANDLE,
104      SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, {}},
105     {"MPL Rotation Vector", "Invensense", 1,
106      SENSORS_ROTATION_VECTOR_HANDLE,
107      SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
108     {"MPL Linear Acceleration", "Invensense", 1,
109      SENSORS_LINEAR_ACCEL_HANDLE,
110      SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
111     {"MPL Gravity", "Invensense", 1,
112      SENSORS_GRAVITY_HANDLE,
113      SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
114 
115 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
116     {"MPL Screen Orientation", "Invensense ", 1,
117      SENSORS_SCREEN_ORIENTATION_HANDLE,
118      SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}},
119 #endif
120 };
121 
122 MPLSensor *MPLSensor::gMPLSensor = NULL;
123 
124 extern "C" {
procData_cb_wrapper()125 void procData_cb_wrapper()
126 {
127     if(MPLSensor::gMPLSensor) {
128         MPLSensor::gMPLSensor->cbProcData();
129     }
130 }
131 
setCallbackObject(MPLSensor * gbpt)132 void setCallbackObject(MPLSensor* gbpt)
133 {
134     MPLSensor::gMPLSensor = gbpt;
135 }
136 
getCallbackObject()137 MPLSensor* getCallbackObject() {
138     return MPLSensor::gMPLSensor;
139 }
140 
141 } // end of extern C
142 
143 #ifdef INV_PLAYBACK_DBG
144 static FILE *logfile = NULL;
145 #endif
146 
147 pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER;
148 
149 /*******************************************************************************
150  * MPLSensor class implementation
151  ******************************************************************************/
152 
MPLSensor(CompassSensor * compass,int (* m_pt2AccelCalLoadFunc)(long *))153 MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
154                        : SensorBase(NULL, NULL),
155                          mNewData(0),
156                          mMasterSensorMask(INV_ALL_SENSORS),
157                          mLocalSensorMask(0),
158                          mPollTime(-1),
159                          mHaveGoodMpuCal(0),
160                          mGyroAccuracy(0),
161                          mAccelAccuracy(0),
162                          mCompassAccuracy(0),
163                          mSampleCount(0),
164                          dmp_orient_fd(-1),
165                          mDmpOrientationEnabled(0),
166                          mEnabled(0),
167                          mOldEnabledMask(0),
168                          mAccelInputReader(4),
169                          mGyroInputReader(32),
170                          mTempScale(0),
171                          mTempOffset(0),
172                          mTempCurrentTime(0),
173                          mAccelScale(2),
174                          mPendingMask(0),
175                          mSensorMask(0),
176                          mFeatureActiveMask(0) {
177     VFUNC_LOG;
178 
179     inv_error_t rv;
180     int i, fd;
181     char *port = NULL;
182     char *ver_str;
183     unsigned long mSensorMask;
184     int res;
185     FILE *fptr;
186 
187     mCompassSensor = compass;
188 
189     LOGV_IF(EXTRA_VERBOSE,
190             "HAL:MPLSensor constructor : numSensors = %d", numSensors);
191 
192     pthread_mutex_init(&mMplMutex, NULL);
193     pthread_mutex_init(&mHALMutex, NULL);
194     memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
195     memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
196 
197 #ifdef INV_PLAYBACK_DBG
198     LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
199     logfile = fopen("/data/playback.bin", "wb");
200     if (logfile)
201         inv_turn_on_data_logging(logfile);
202 #endif
203 
204     /* setup sysfs paths */
205     inv_init_sysfs_attributes();
206 
207     /* get chip name */
208     if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
209         LOGE("HAL:ERR- Failed to get chip ID\n");
210     } else {
211         LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
212     }
213 
214     enable_iio_sysfs();
215 
216     /* turn on power state */
217     onPower(1);
218 
219     /* reset driver master enable */
220     masterEnable(0);
221 
222     if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
223         /* Load DMP image if capable, ie. MPU6xxx/9xxx */
224         loadDMP();
225     }
226 
227     /* open temperature fd for temp comp */
228     LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
229     gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
230     if (gyro_temperature_fd == -1) {
231         LOGE("HAL:could not open temperature node");
232     } else {
233         LOGV_IF(EXTRA_VERBOSE,
234                 "HAL:temperature_fd opened: %s", mpu.temperature);
235     }
236 
237     /* read accel FSR to calcuate accel scale later */
238     if (!USE_THIRD_PARTY_ACCEL) {
239         char buf[3];
240         int count = 0;
241         LOGV_IF(SYSFS_VERBOSE,
242                 "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
243 
244         fd = open(mpu.accel_fsr, O_RDONLY);
245         if(fd < 0) {
246             LOGE("HAL:Error opening accel FSR");
247         } else {
248            memset(buf, 0, sizeof(buf));
249            count = read_attribute_sensor(fd, buf, sizeof(buf));
250            if(count < 1) {
251                LOGE("HAL:Error reading accel FSR");
252            } else {
253                count = sscanf(buf, "%d", &mAccelScale);
254                if(count)
255                    LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
256            }
257            close(fd);
258         }
259     }
260 
261     /* initialize sensor data */
262     memset(mPendingEvents, 0, sizeof(mPendingEvents));
263 
264     mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
265     mPendingEvents[RotationVector].sensor = ID_RV;
266     mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
267 
268     mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
269     mPendingEvents[LinearAccel].sensor = ID_LA;
270     mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
271 
272     mPendingEvents[Gravity].version = sizeof(sensors_event_t);
273     mPendingEvents[Gravity].sensor = ID_GR;
274     mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
275 
276     mPendingEvents[Gyro].version = sizeof(sensors_event_t);
277     mPendingEvents[Gyro].sensor = ID_GY;
278     mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
279 
280     mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
281     mPendingEvents[RawGyro].sensor = ID_RG;
282     mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
283 
284     mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
285     mPendingEvents[Accelerometer].sensor = ID_A;
286     mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
287 
288     /* Invensense compass calibration */
289     mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
290     mPendingEvents[MagneticField].sensor = ID_M;
291     mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
292     mPendingEvents[MagneticField].magnetic.status =
293         SENSOR_STATUS_ACCURACY_HIGH;
294 
295     mPendingEvents[Orientation].version = sizeof(sensors_event_t);
296     mPendingEvents[Orientation].sensor = ID_O;
297     mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
298     mPendingEvents[Orientation].orientation.status
299             = SENSOR_STATUS_ACCURACY_HIGH;
300 
301     mHandlers[RotationVector] = &MPLSensor::rvHandler;
302     mHandlers[LinearAccel] = &MPLSensor::laHandler;
303     mHandlers[Gravity] = &MPLSensor::gravHandler;
304     mHandlers[Gyro] = &MPLSensor::gyroHandler;
305     mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
306     mHandlers[Accelerometer] = &MPLSensor::accelHandler;
307     mHandlers[MagneticField] = &MPLSensor::compassHandler;
308     mHandlers[Orientation] = &MPLSensor::orienHandler;
309 
310     for (int i = 0; i < numSensors; i++) {
311         mDelays[i] = 0;
312     }
313 
314     (void)inv_get_version(&ver_str);
315     LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
316 
317     /* setup MPL */
318     inv_constructor_init();
319 
320     /* load calibration file from /data/inv_cal_data.bin */
321     rv = inv_load_calibration();
322     if(rv == INV_SUCCESS)
323         LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
324     else
325         LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
326 
327     /* Takes external Accel Calibration Load Method */
328     if( m_pt2AccelCalLoadFunc != NULL)
329     {
330         long accel_offset[3];
331         long tmp_offset[3];
332         int result = m_pt2AccelCalLoadFunc(accel_offset);
333         if(result)
334             LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result);
335         else
336         {
337             LOGW("HAL:Vendor accelerometer calibration file successfully loaded");
338             inv_get_accel_bias(tmp_offset, NULL);
339             LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n",
340                tmp_offset[0], tmp_offset[1], tmp_offset[2]);
341             inv_set_accel_bias(accel_offset, mAccelAccuracy);
342             inv_get_accel_bias(tmp_offset, NULL);
343             LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
344                tmp_offset[0], tmp_offset[1], tmp_offset[2]);
345         }
346     }
347     /* End of Accel Calibration Load Method */
348 
349     inv_set_device_properties();
350 
351     /* disable driver master enable the first sensor goes on */
352     masterEnable(0);
353     enableGyro(0);
354     enableAccel(0);
355     enableCompass(0);
356 
357     if (isLowPowerQuatEnabled()) {
358         enableLPQuaternion(0);
359     }
360 
361     onPower(0);
362 
363     if (isDmpDisplayOrientationOn()) {
364         enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
365     }
366 
367 }
368 
enable_iio_sysfs()369 void MPLSensor::enable_iio_sysfs()
370 {
371     VFUNC_LOG;
372 
373     char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
374     FILE *tempFp = NULL;
375 
376     /* ignore failures */
377     write_sysfs_int(mpu.in_timestamp_en, 1);
378 
379     LOGV_IF(SYSFS_VERBOSE,
380             "HAL:sysfs:cat %s (%lld)",
381             mpu.trigger_name, getTimestamp());
382     tempFp = fopen(mpu.trigger_name, "r");
383     if (tempFp == NULL) {
384         LOGE("HAL:could not open trigger name");
385     } else {
386         if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
387             LOGE("HAL:could not read trigger name");
388         }
389         fclose(tempFp);
390     }
391 
392     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
393             iio_trigger_name, mpu.current_trigger, getTimestamp());
394     tempFp = fopen(mpu.current_trigger, "w");
395     if (tempFp == NULL) {
396         LOGE("HAL:could not open current trigger");
397     } else {
398         if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) {
399             LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno);
400         }
401     }
402 
403     write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH);
404 
405     if (inv_get_iio_device_node(iio_device_node) < 0) {
406         LOGE("HAL:could retrive the iio device node");
407     }
408     iio_fd = open(iio_device_node, O_RDONLY);
409     if (iio_fd < 0) {
410         LOGE("HAL:could not open iio device node");
411     } else {
412         LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd);
413     }
414 }
415 
inv_constructor_init()416 int MPLSensor::inv_constructor_init()
417 {
418     VFUNC_LOG;
419 
420     inv_error_t result = inv_init_mpl();
421     if (result) {
422         LOGE("HAL:inv_init_mpl() failed");
423         return result;
424     }
425     result = inv_constructor_default_enable();
426     result = inv_start_mpl();
427     if (result) {
428         LOGE("HAL:inv_start_mpl() failed");
429         LOG_RESULT_LOCATION(result);
430         return result;
431     }
432 
433     return result;
434 }
435 
inv_constructor_default_enable()436 int MPLSensor::inv_constructor_default_enable()
437 {
438     VFUNC_LOG;
439 
440     inv_error_t result;
441 
442     result = inv_enable_quaternion();
443     if (result) {
444         LOGE("HAL:Cannot enable quaternion\n");
445         return result;
446     }
447 
448     result = inv_enable_in_use_auto_calibration();
449     if (result) {
450         return result;
451     }
452 
453     // result = inv_enable_motion_no_motion();
454     result = inv_enable_fast_nomot();
455     if (result) {
456         return result;
457     }
458 
459     result = inv_enable_gyro_tc();
460     if (result) {
461         return result;
462     }
463 
464     result = inv_enable_hal_outputs();
465     if (result) {
466         return result;
467     }
468 
469     if (!mCompassSensor->providesCalibration()) {
470         /* Invensense compass calibration */
471         LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
472         result = inv_enable_vector_compass_cal();
473         if (result) {
474             LOG_RESULT_LOCATION(result);
475             return result;
476         } else {
477             mFeatureActiveMask |= INV_COMPASS_CAL;
478         }
479 
480         // specify MPL's trust weight, used by compass algorithms
481         inv_vector_compass_cal_sensitivity(3);
482 
483         result = inv_enable_compass_bias_w_gyro();
484         if (result) {
485             LOG_RESULT_LOCATION(result);
486             return result;
487         }
488 
489         result = inv_enable_heading_from_gyro();
490         if (result) {
491             LOG_RESULT_LOCATION(result);
492             return result;
493         }
494 
495         result = inv_enable_magnetic_disturbance();
496         if (result) {
497             LOG_RESULT_LOCATION(result);
498             return result;
499         }
500     }
501 
502     result = inv_enable_9x_sensor_fusion();
503     if (result) {
504         LOG_RESULT_LOCATION(result);
505         return result;
506     } else {
507         // 9x sensor fusion enables Compass fit
508         mFeatureActiveMask |= INV_COMPASS_FIT;
509     }
510 
511     result = inv_enable_no_gyro_fusion();
512     if (result) {
513         LOG_RESULT_LOCATION(result);
514         return result;
515     }
516 
517     result = inv_enable_quat_accuracy_monitor();
518     if (result) {
519         LOG_RESULT_LOCATION(result);
520         return result;
521     }
522 
523     return result;
524 }
525 
526 /* TODO: create function pointers to calculate scale */
inv_set_device_properties()527 void MPLSensor::inv_set_device_properties()
528 {
529     VFUNC_LOG;
530 
531     unsigned short orient;
532 
533     inv_get_sensors_orientation();
534 
535     inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
536     inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
537 
538     /* gyro setup */
539     orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
540     inv_set_gyro_orientation_and_scale(orient, 2000L << 15);
541 
542     /* accel setup */
543     orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
544     /* use for third party accel input subsystem driver
545     inv_set_accel_orientation_and_scale(orient, 1LL << 22);
546     */
547     inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
548 
549     /* compass setup */
550     signed char orientMtx[9];
551     mCompassSensor->getOrientationMatrix(orientMtx);
552     orient =
553         inv_orientation_matrix_to_scalar(orientMtx);
554     long sensitivity;
555     sensitivity = mCompassSensor->getSensitivity();
556     inv_set_compass_orientation_and_scale(orient, sensitivity);
557 }
558 
loadDMP()559 void MPLSensor::loadDMP()
560 {
561     int res, fd;
562     FILE *fptr;
563 
564     if (isMpu3050()) {
565         //DMP support only for MPU6xxx/9xxx currently
566         return;
567     }
568 
569     /* load DMP firmware */
570     LOGV_IF(SYSFS_VERBOSE,
571             "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
572     fd = open(mpu.firmware_loaded, O_RDONLY);
573     if(fd < 0) {
574         LOGE("HAL:could not open dmp state");
575         return;
576     }
577     if(inv_read_dmp_state(fd)) {
578         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
579         return;
580     }
581 
582     LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
583     fptr = fopen(mpu.dmp_firmware, "w");
584     if(!fptr) {
585         LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno));
586         return;
587     }
588     res = inv_load_dmp(fptr);
589     if(res < 0) {
590         LOGE("HAL:load DMP failed");
591     } else {
592         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
593     }
594     fclose(fptr);
595 
596     // onDMP(1);                //Can't enable here. See note onDMP()
597 }
598 
inv_get_sensors_orientation()599 void MPLSensor::inv_get_sensors_orientation()
600 {
601     FILE *fptr;
602 
603     // get gyro orientation
604     LOGV_IF(SYSFS_VERBOSE,
605             "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
606     fptr = fopen(mpu.gyro_orient, "r");
607     if (fptr != NULL) {
608         int om[9];
609         fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
610                &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
611                &om[6], &om[7], &om[8]);
612         fclose(fptr);
613 
614         LOGV_IF(EXTRA_VERBOSE,
615                 "HAL:gyro mounting matrix: "
616                 "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
617                 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
618 
619         mGyroOrientation[0] = om[0];
620         mGyroOrientation[1] = om[1];
621         mGyroOrientation[2] = om[2];
622         mGyroOrientation[3] = om[3];
623         mGyroOrientation[4] = om[4];
624         mGyroOrientation[5] = om[5];
625         mGyroOrientation[6] = om[6];
626         mGyroOrientation[7] = om[7];
627         mGyroOrientation[8] = om[8];
628     } else {
629         LOGE("HAL:Couldn't read gyro mounting matrix");
630     }
631 
632     // get accel orientation
633     LOGV_IF(SYSFS_VERBOSE,
634             "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
635     fptr = fopen(mpu.accel_orient, "r");
636     if (fptr != NULL) {
637         int om[9];
638         fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
639                &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
640                &om[6], &om[7], &om[8]);
641         fclose(fptr);
642 
643         LOGV_IF(EXTRA_VERBOSE,
644                 "HAL:accel mounting matrix: "
645                 "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
646                 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
647 
648         mAccelOrientation[0] = om[0];
649         mAccelOrientation[1] = om[1];
650         mAccelOrientation[2] = om[2];
651         mAccelOrientation[3] = om[3];
652         mAccelOrientation[4] = om[4];
653         mAccelOrientation[5] = om[5];
654         mAccelOrientation[6] = om[6];
655         mAccelOrientation[7] = om[7];
656         mAccelOrientation[8] = om[8];
657     } else {
658         LOGE("HAL:Couldn't read accel mounting matrix");
659     }
660 }
661 
~MPLSensor()662 MPLSensor::~MPLSensor()
663 {
664     VFUNC_LOG;
665 
666     mCompassSensor = NULL;
667 
668     /* Close open fds */
669     if (iio_fd > 0)
670         close(iio_fd);
671     if( accel_fd > 0 )
672         close(accel_fd );
673     if (gyro_temperature_fd > 0)
674         close(gyro_temperature_fd);
675     if (sysfs_names_ptr)
676         free(sysfs_names_ptr);
677 
678     if (isDmpDisplayOrientationOn()) {
679         closeDmpOrientFd();
680     }
681 
682     /* Turn off Gyro master enable          */
683     /* A workaround until driver handles it */
684     /* TODO: Turn off and close all sensors */
685     if(write_sysfs_int(mpu.chip_enable, 0) < 0) {
686         LOGE("HAL:could not disable gyro master enable");
687     }
688 
689 #ifdef INV_PLAYBACK_DBG
690     inv_turn_off_data_logging();
691     fclose(logfile);
692 #endif
693 }
694 
695 #define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
696 #define A_ENABLED  ((1 << ID_A)  & enabled_sensors)
697 #define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
698 #define O_ENABLED  ((1 << ID_O)  & enabled_sensors)
699 #define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
700 #define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
701 #define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
702 
703 /* TODO: this step is optional, remove?  */
setGyroInitialState()704 int MPLSensor::setGyroInitialState()
705 {
706     VFUNC_LOG;
707 
708     int res = 0;
709 
710     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
711             HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
712     int fd = open(mpu.gyro_fifo_rate, O_RDWR);
713     res = errno;
714     if(fd < 0) {
715         LOGE("HAL:open of %s failed with '%s' (%d)",
716              mpu.gyro_fifo_rate, strerror(res), res);
717         return res;
718     }
719     res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
720     if(res < 0) {
721         LOGE("HAL:write_attribute_sensor : error writing %s with %d",
722              mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ);
723         return res;
724     }
725 
726     // Setting LPF is deprecated
727 
728     return 0;
729 }
730 
731 /* this applies to BMA250 Input Subsystem Driver only */
setAccelInitialState()732 int MPLSensor::setAccelInitialState()
733 {
734     VFUNC_LOG;
735 
736     struct input_absinfo absinfo_x;
737     struct input_absinfo absinfo_y;
738     struct input_absinfo absinfo_z;
739     float value;
740     if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
741         !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
742         !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
743         value = absinfo_x.value;
744         mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
745         value = absinfo_y.value;
746         mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
747         value = absinfo_z.value;
748         mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
749         //mHasPendingEvent = true;
750     }
751     return 0;
752 }
753 
onPower(int en)754 int MPLSensor::onPower(int en)
755 {
756     VFUNC_LOG;
757 
758     int res;
759 
760     int count, curr_power_state;
761 
762     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
763             en, mpu.power_state, getTimestamp());
764     res = read_sysfs_int(mpu.power_state, &curr_power_state);
765     if (res < 0) {
766         LOGE("HAL:Error reading power state");
767         // will set power_state anyway
768         curr_power_state = -1;
769     }
770     if (en != curr_power_state) {
771         if((res = write_sysfs_int(mpu.power_state, en)) < 0) {
772                 LOGE("HAL:Couldn't write power state");
773         }
774     } else {
775         LOGV_IF(EXTRA_VERBOSE,
776                 "HAL:Power state already enable/disable curr=%d new=%d",
777                 curr_power_state, en);
778     }
779     return res;
780 }
781 
onDMP(int en)782 int MPLSensor::onDMP(int en)
783 {
784     VFUNC_LOG;
785 
786     int res = -1;
787     int status;
788 
789     //Sequence to enable DMP
790     //1. Turn On power if not already on
791     //2. Load DMP image if not already loaded
792     //3. Either Gyro or Accel must be enabled/configured before next step
793     //4. Enable DMP
794 
795     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
796             mpu.firmware_loaded, getTimestamp());
797     res = read_sysfs_int(mpu.firmware_loaded, &status);
798     if (res < 0){
799         LOGE("HAL:ERR can't get firmware_loaded status");
800         return res;
801     }
802     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status);
803 
804     if (status) {
805         //Write only if curr DMP state <> request
806         LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
807                 mpu.dmp_on, getTimestamp());
808         if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
809             LOGE("HAL:ERR can't read DMP state");
810         } else if (status != en) {
811             res = write_sysfs_int(mpu.dmp_on, en);
812             //Enable DMP interrupt
813             if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
814                 LOGE("HAL:ERR can't en/dis DMP interrupt");
815             }
816         } else {
817             res = 0;    //DMP already set as requested
818         }
819     } else {
820         LOGE("HAL:ERR No DMP image");
821     }
822     return res;
823 }
824 
checkLPQuaternion(void)825 int MPLSensor::checkLPQuaternion(void)
826 {
827     VFUNC_LOG;
828 
829     return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
830 }
831 
enableLPQuaternion(int en)832 int MPLSensor::enableLPQuaternion(int en)
833 {
834     VFUNC_LOG;
835 
836     if (!en) {
837         enableQuaternionData(0);
838         onDMP(0);
839         mFeatureActiveMask &= ~INV_DMP_QUATERNION;
840         LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
841     } else {
842         if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
843             LOGE("HAL:ERR can't enable LP Quaternion");
844         } else {
845             mFeatureActiveMask |= INV_DMP_QUATERNION;
846             LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled");
847         }
848     }
849     return 0;
850 }
851 
enableQuaternionData(int en)852 int MPLSensor::enableQuaternionData(int en)
853 {
854     int res = 0;
855     VFUNC_LOG;
856 
857     // Enable DMP quaternion
858     res = write_sysfs_int(mpu.quaternion_on, en);
859 
860     if (!en) {
861         LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
862     } else {
863 
864         LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
865     }
866     write_sysfs_int(mpu.in_quat_r_en, en);
867     write_sysfs_int(mpu.in_quat_x_en, en);
868     write_sysfs_int(mpu.in_quat_y_en, en);
869     write_sysfs_int(mpu.in_quat_z_en, en);
870 
871     LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
872 
873     if (!en) {
874         inv_quaternion_sensor_was_turned_off();
875     }
876 
877     return res;
878 }
879 
enableTap(int)880 int MPLSensor::enableTap(int /*en*/)
881 {
882     VFUNC_LOG;
883 
884     return 0;
885 }
886 
enableFlick(int)887 int MPLSensor::enableFlick(int /*en*/)
888 {
889     VFUNC_LOG;
890 
891     return 0;
892 }
893 
enablePedometer(int)894 int MPLSensor::enablePedometer(int /*en*/)
895 {
896     VFUNC_LOG;
897 
898     return 0;
899 }
900 
masterEnable(int en)901 int MPLSensor::masterEnable(int en)
902 {
903     VFUNC_LOG;
904     return write_sysfs_int(mpu.chip_enable, en);
905 }
906 
enableGyro(int en)907 int MPLSensor::enableGyro(int en)
908 {
909     VFUNC_LOG;
910 
911     /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
912     int res;
913 
914     /* need to also turn on/off the master enable */
915     res = write_sysfs_int(mpu.gyro_enable, en);
916 
917     if (!en) {
918         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
919         inv_gyro_was_turned_off();
920     } else {
921         write_sysfs_int(mpu.gyro_x_fifo_enable, en);
922         write_sysfs_int(mpu.gyro_y_fifo_enable, en);
923         res = write_sysfs_int(mpu.gyro_z_fifo_enable, en);
924     }
925 
926     return res;
927 }
928 
enableAccel(int en)929 int MPLSensor::enableAccel(int en)
930 {
931     VFUNC_LOG;
932 
933     /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
934     int res;
935 
936     /* need to also turn on/off the master enable */
937     res = write_sysfs_int(mpu.accel_enable, en);
938 
939     if (!en) {
940         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
941         inv_accel_was_turned_off();
942     } else {
943         write_sysfs_int(mpu.accel_x_fifo_enable, en);
944         write_sysfs_int(mpu.accel_y_fifo_enable, en);
945         res = write_sysfs_int(mpu.accel_z_fifo_enable, en);
946     }
947 
948     return res;
949 }
950 
enableCompass(int en)951 int MPLSensor::enableCompass(int en)
952 {
953     VFUNC_LOG;
954 
955     int res = mCompassSensor->enable(ID_M, en);
956     if (!en) {
957         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
958         inv_compass_was_turned_off();
959     }
960     return res;
961 }
962 
computeLocalSensorMask(int enabled_sensors)963 void MPLSensor::computeLocalSensorMask(int enabled_sensors)
964 {
965     VFUNC_LOG;
966 
967     do {
968         if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
969             LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
970             mLocalSensorMask = ALL_MPL_SENSORS_NP;
971             break;
972         }
973 
974         if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
975             /* Invensense compass cal */
976             LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
977             mLocalSensorMask = 0;
978             break;
979         }
980 
981         if (GY_ENABLED) {
982             LOGV_IF(ENG_VERBOSE, "G ENABLED");
983             mLocalSensorMask |= INV_THREE_AXIS_GYRO;
984         } else {
985             LOGV_IF(ENG_VERBOSE, "G DISABLED");
986             mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
987         }
988 
989         if (A_ENABLED) {
990             LOGV_IF(ENG_VERBOSE, "A ENABLED");
991             mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
992         } else {
993             LOGV_IF(ENG_VERBOSE, "A DISABLED");
994             mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
995         }
996 
997         /* Invensense compass calibration */
998         if (M_ENABLED) {
999             LOGV_IF(ENG_VERBOSE, "M ENABLED");
1000             mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
1001         } else {
1002             LOGV_IF(ENG_VERBOSE, "M DISABLED");
1003             mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
1004         }
1005     } while (0);
1006 }
1007 
enableOneSensor(int en,const char * name,int (MPLSensor::* enabler)(int))1008 int MPLSensor::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) {
1009     LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name);
1010     return (this->*enabler)(en);
1011 }
1012 
enableSensors(unsigned long sensors,int,uint32_t changed)1013 int MPLSensor::enableSensors(unsigned long sensors, int /*en*/, uint32_t changed) {
1014     VFUNC_LOG;
1015 
1016     inv_error_t res = -1;
1017     bool store_cal = false;
1018     bool ext_compass_changed = false;
1019 
1020     // Sequence to enable or disable a sensor
1021     // 1. enable Power state
1022     // 2. reset master enable (=0)
1023     // 3. enable or disable a sensor
1024     // 4. set master enable (=1)
1025 
1026     pthread_mutex_lock(&GlobalHalMutex);
1027 
1028     uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer)
1029             | (1 << MagneticField);
1030     uint32_t all_integrated_changeables = all_changeables;
1031 
1032     if (!mCompassSensor->isIntegrated()) {
1033         ext_compass_changed = changed & (1 << MagneticField);
1034         all_integrated_changeables = all_changeables & ~(1 << MagneticField);
1035     }
1036 
1037     if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) {
1038         /* ensure power state is on */
1039         onPower(1);
1040 
1041         /* reset master enable */
1042         res = masterEnable(0);
1043         if(res < 0) {
1044             goto unlock_res;
1045         }
1046     }
1047 
1048     LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
1049 
1050     if (changed & ((1 << Gyro) | (1 << RawGyro))) {
1051         res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro);
1052         if(res < 0) {
1053             goto unlock_res;
1054         }
1055     }
1056 
1057     if (changed & (1 << Accelerometer)) {
1058         res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel);
1059         if(res < 0) {
1060             goto unlock_res;
1061         }
1062     }
1063 
1064     if (changed & (1 << MagneticField)) {
1065         /* Invensense compass calibration */
1066         res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass);
1067         if(res < 0) {
1068             goto unlock_res;
1069         }
1070     }
1071 
1072     if ( isLowPowerQuatEnabled() ) {
1073         // Enable LP Quat
1074         if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
1075                 (1 << LinearAccel) | (1 << Gravity)))) {
1076             if (!(changed & all_integrated_changeables)) {
1077                 /* ensure power state is on */
1078                 onPower(1);
1079                 /* reset master enable */
1080                 res = masterEnable(0);
1081                 if(res < 0) {
1082                     goto unlock_res;
1083                 }
1084             }
1085             if (!checkLPQuaternion()) {
1086                 enableLPQuaternion(1);
1087             } else {
1088                 LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
1089             }
1090         } else if (checkLPQuaternion()) {
1091             enableLPQuaternion(0);
1092         }
1093     }
1094 
1095     if (changed & all_integrated_changeables) {
1096         if (sensors &
1097             (INV_THREE_AXIS_GYRO
1098                 | INV_THREE_AXIS_ACCEL
1099                 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
1100             if ( isLowPowerQuatEnabled() ||
1101                     (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) {
1102                 // disable DMP event interrupt only (w/ data interrupt)
1103                 if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
1104                     res = -1;
1105                     LOGE("HAL:ERR can't disable DMP event interrupt");
1106                     goto unlock_res;
1107                 }
1108             }
1109 
1110             if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1111                 // enable DMP
1112                 onDMP(1);
1113 
1114                 res = enableAccel(1);
1115                 if(res < 0) {
1116                     goto unlock_res;
1117                 }
1118 
1119                 if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
1120                     res = turnOffAccelFifo();
1121                 }
1122                 if(res < 0) {
1123                     goto unlock_res;
1124                 }
1125             }
1126 
1127             res = masterEnable(1);
1128             if(res < 0) {
1129                 goto unlock_res;
1130             }
1131         } else { // all sensors idle -> reduce power
1132             if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1133                 // enable DMP
1134                 onDMP(1);
1135                 // enable DMP event interrupt only (no data interrupt)
1136                 if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
1137                     res = -1;
1138                     LOGE("HAL:ERR can't enable DMP event interrupt");
1139                 }
1140                 res = enableAccel(1);
1141                 if(res < 0) {
1142                     goto unlock_res;
1143                 }
1144                 if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
1145                     res = turnOffAccelFifo();
1146                 }
1147                 if(res < 0) {
1148                     goto unlock_res;
1149                 }
1150                 res = masterEnable(1);
1151                 if(res < 0) {
1152                     goto unlock_res;
1153                 }
1154             }
1155             else {
1156                 res = onPower(0);
1157                 if(res < 0) {
1158                     goto unlock_res;
1159                 }
1160             }
1161             store_cal = true;
1162         }
1163     } else if (ext_compass_changed &&
1164             !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
1165                 | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
1166             store_cal = true;
1167     }
1168 
1169     if (store_cal || ((changed & all_changeables) != all_changeables)) {
1170         storeCalibration();
1171     }
1172 
1173 unlock_res:
1174     pthread_mutex_unlock(&GlobalHalMutex);
1175     return res;
1176 }
1177 
1178 /* Store calibration file */
storeCalibration()1179 void MPLSensor::storeCalibration()
1180 {
1181     if(mHaveGoodMpuCal || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) {
1182        int res = inv_store_calibration();
1183        if (res) {
1184            LOGE("HAL:Cannot store calibration on file");
1185        } else {
1186            LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
1187        }
1188     }
1189 }
1190 
cbProcData()1191 void MPLSensor::cbProcData()
1192 {
1193     mNewData = 1;
1194     mSampleCount++;
1195     LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
1196 }
1197 
1198 /*  these handlers transform mpl data into one of the Android sensor types */
gyroHandler(sensors_event_t * s)1199 int MPLSensor::gyroHandler(sensors_event_t* s)
1200 {
1201     VHANDLER_LOG;
1202     int8_t status;
1203     int update;
1204     update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp);
1205     LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
1206             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
1207     return update;
1208 }
1209 
rawGyroHandler(sensors_event_t * s)1210 int MPLSensor::rawGyroHandler(sensors_event_t* s)
1211 {
1212     VHANDLER_LOG;
1213     int8_t status;
1214     int update;
1215     update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp);
1216     LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
1217             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
1218     return update;
1219 }
1220 
accelHandler(sensors_event_t * s)1221 int MPLSensor::accelHandler(sensors_event_t* s)
1222 {
1223     VHANDLER_LOG;
1224     int8_t status;
1225     int update;
1226     update = inv_get_sensor_type_accelerometer(
1227         s->acceleration.v, &status, &s->timestamp);
1228     LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
1229             s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
1230             s->timestamp, update);
1231     mAccelAccuracy = status;
1232     return update;
1233 }
1234 
compassHandler(sensors_event_t * s)1235 int MPLSensor::compassHandler(sensors_event_t* s)
1236 {
1237     VHANDLER_LOG;
1238     int update;
1239     update = inv_get_sensor_type_magnetic_field(
1240         s->magnetic.v, &s->magnetic.status, &s->timestamp);
1241     LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
1242             s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
1243     mCompassAccuracy = s->magnetic.status;
1244     return update;
1245 }
1246 
rvHandler(sensors_event_t * s)1247 int MPLSensor::rvHandler(sensors_event_t* s)
1248 {
1249     // rotation vector does not have an accuracy or status
1250     VHANDLER_LOG;
1251     int8_t status;
1252     int update;
1253     update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp);
1254     LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d",
1255             s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
1256     return update;
1257 }
1258 
laHandler(sensors_event_t * s)1259 int MPLSensor::laHandler(sensors_event_t* s)
1260 {
1261     VHANDLER_LOG;
1262     int8_t status;
1263     int update;
1264     update = inv_get_sensor_type_linear_acceleration(
1265             s->gyro.v, &status, &s->timestamp);
1266     LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
1267             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
1268     return update;
1269 }
1270 
gravHandler(sensors_event_t * s)1271 int MPLSensor::gravHandler(sensors_event_t* s)
1272 {
1273     VHANDLER_LOG;
1274     int8_t status;
1275     int update;
1276     update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp);
1277     LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
1278             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
1279     return update;
1280 }
1281 
orienHandler(sensors_event_t * s)1282 int MPLSensor::orienHandler(sensors_event_t* s)
1283 {
1284     VHANDLER_LOG;
1285     int update;
1286     update = inv_get_sensor_type_orientation(
1287             s->orientation.v, &s->orientation.status, &s->timestamp);
1288     LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
1289             s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update);
1290     return update;
1291 }
1292 
enable(int32_t handle,int en)1293 int MPLSensor::enable(int32_t handle, int en)
1294 {
1295     VFUNC_LOG;
1296 
1297     android::String8 sname;
1298     int what = -1, err = 0;
1299 
1300     switch (handle) {
1301     case ID_SO:
1302         sname = "Screen Orientation";
1303         LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
1304             (mDmpOrientationEnabled? "en": "dis"),
1305             (en? "en" : "dis"));
1306         enableDmpOrientation(en && isDmpDisplayOrientationOn());
1307         /* TODO: stop manually testing/using 0 and 1 instead of
1308          * false and true, but just use 0 and non-0.
1309          * This allows  passing 0 and non-0 ints around instead of
1310          * having to convert to 1 and test against 1.
1311          */
1312         mDmpOrientationEnabled = en;
1313         return 0;
1314     case ID_A:
1315         what = Accelerometer;
1316         sname = "Accelerometer";
1317         break;
1318     case ID_M:
1319         what = MagneticField;
1320         sname = "MagneticField";
1321         break;
1322     case ID_O:
1323         what = Orientation;
1324         sname = "Orientation";
1325         break;
1326     case ID_GY:
1327         what = Gyro;
1328         sname = "Gyro";
1329         break;
1330     case ID_RG:
1331         what = RawGyro;
1332         sname = "RawGyro";
1333         break;
1334     case ID_GR:
1335         what = Gravity;
1336         sname = "Gravity";
1337         break;
1338     case ID_RV:
1339         what = RotationVector;
1340         sname = "RotationVector";
1341         break;
1342     case ID_LA:
1343         what = LinearAccel;
1344         sname = "LinearAccel";
1345         break;
1346     default: //this takes care of all the gestures
1347         what = handle;
1348         sname = "Others";
1349         break;
1350     }
1351 
1352     if (uint32_t(what) >= numSensors)
1353         return -EINVAL;
1354 
1355     int newState = en ? 1 : 0;
1356     unsigned long sen_mask;
1357 
1358     LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
1359             ((mEnabled & (1 << what)) ? "en" : "dis"),
1360             ((uint32_t(newState) << what) ? "en" : "dis"));
1361     LOGV_IF(PROCESS_VERBOSE,
1362             "HAL:%s sensor state change what=%d", sname.string(), what);
1363 
1364     // pthread_mutex_lock(&mMplMutex);
1365     // pthread_mutex_lock(&mHALMutex);
1366 
1367     if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
1368         uint32_t sensor_type;
1369         short flags = newState;
1370         uint32_t lastEnabled = mEnabled, changed = 0;
1371 
1372         mEnabled &= ~(1 << what);
1373         mEnabled |= (uint32_t(flags) << what);
1374 
1375         LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
1376         LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
1377         computeLocalSensorMask(mEnabled);
1378         LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
1379         sen_mask = mLocalSensorMask & mMasterSensorMask;
1380         mSensorMask = sen_mask;
1381         LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
1382 
1383         switch (what) {
1384             case Gyro:
1385             case RawGyro:
1386             case Accelerometer:
1387             case MagneticField:
1388                 if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
1389                         (1 << LinearAccel) | (1 << Gravity))) &&
1390                         ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
1391                     changed |= (1 << what);
1392                 }
1393                 break;
1394 
1395             case Orientation:
1396             case RotationVector:
1397             case LinearAccel:
1398             case Gravity:
1399                 if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) |
1400                         (1 << LinearAccel) | (1 << Gravity)))) ||
1401                         (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
1402                         (1 << LinearAccel) | (1 << Gravity))))) {
1403                     for (int i = Gyro; i <= MagneticField; i++) {
1404                         if (!(mEnabled & (1 << i))) {
1405                             changed |= (1 << i);
1406                         }
1407                     }
1408                 }
1409                 break;
1410         }
1411         LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed);
1412         enableSensors(sen_mask, flags, changed);
1413     }
1414 
1415     // pthread_mutex_unlock(&mMplMutex);
1416     // pthread_mutex_unlock(&mHALMutex);
1417 
1418 #ifdef INV_PLAYBACK_DBG
1419     /* apparently the logging needs to be go through this sequence
1420        to properly flush the log file */
1421     inv_turn_off_data_logging();
1422     fclose(logfile);
1423     logfile = fopen("/data/playback.bin", "ab");
1424     if (logfile)
1425         inv_turn_on_data_logging(logfile);
1426 #endif
1427 
1428     return err;
1429 }
1430 
setDelay(int32_t handle,int64_t ns)1431 int MPLSensor::setDelay(int32_t handle, int64_t ns)
1432 {
1433     VFUNC_LOG;
1434 
1435     android::String8 sname;
1436     int what = -1;
1437 
1438     switch (handle) {
1439         case ID_SO:
1440             return update_delay();
1441         case ID_A:
1442             what = Accelerometer;
1443             sname = "Accelerometer";
1444             break;
1445         case ID_M:
1446             what = MagneticField;
1447             sname = "MagneticField";
1448             break;
1449         case ID_O:
1450             what = Orientation;
1451             sname = "Orientation";
1452             break;
1453         case ID_GY:
1454             what = Gyro;
1455             sname = "Gyro";
1456             break;
1457         case ID_RG:
1458             what = RawGyro;
1459             sname = "RawGyro";
1460             break;
1461         case ID_GR:
1462             what = Gravity;
1463             sname = "Gravity";
1464             break;
1465         case ID_RV:
1466             what = RotationVector;
1467             sname = "RotationVector";
1468             break;
1469         case ID_LA:
1470             what = LinearAccel;
1471             sname = "LinearAccel";
1472             break;
1473         default: // this takes care of all the gestures
1474             what = handle;
1475             sname = "Others";
1476             break;
1477     }
1478 
1479 #if 0
1480     // skip the 1st call for enalbing sensors called by ICS/JB sensor service
1481     static int counter_delay = 0;
1482     if (!(mEnabled & (1 << what))) {
1483         counter_delay = 0;
1484     } else {
1485         if (++counter_delay == 1) {
1486             return 0;
1487         }
1488         else {
1489             counter_delay = 0;
1490         }
1491     }
1492 #endif
1493 
1494     if (uint32_t(what) >= numSensors)
1495         return -EINVAL;
1496 
1497     if (ns < 0)
1498         return -EINVAL;
1499 
1500     LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
1501 
1502     // limit all rates to reasonable ones */
1503     if (ns < 5000000LL) {
1504         ns = 5000000LL;
1505     }
1506 
1507     /* store request rate to mDelays array for each sensor */
1508     mDelays[what] = ns;
1509 
1510     switch (what) {
1511         case Gyro:
1512         case RawGyro:
1513         case Accelerometer:
1514             for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
1515                     i++) {
1516                 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
1517                     LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
1518                     return 0;
1519                 }
1520             }
1521             break;
1522 
1523         case MagneticField:
1524             if (mCompassSensor->isIntegrated() &&
1525                     (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
1526                     ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
1527                     ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
1528                  LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
1529                  return 0;
1530             }
1531             break;
1532 
1533         case Orientation:
1534         case RotationVector:
1535         case LinearAccel:
1536         case Gravity:
1537             if (isLowPowerQuatEnabled()) {
1538                 LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ");
1539                 break;
1540             }
1541 
1542             for (int i = 0; i < numSensors; i++) {
1543                 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
1544                     LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
1545                     return 0;
1546                 }
1547             }
1548             break;
1549     }
1550 
1551     // pthread_mutex_lock(&mHALMutex);
1552     int res = update_delay();
1553     // pthread_mutex_unlock(&mHALMutex);
1554     return res;
1555 }
1556 
update_delay()1557 int MPLSensor::update_delay() {
1558     VHANDLER_LOG;
1559 
1560     int res = 0;
1561     int64_t got;
1562 
1563     pthread_mutex_lock(&GlobalHalMutex);
1564     if (mEnabled) {
1565         int64_t wanted = 1000000000;
1566         int64_t wanted_3rd_party_sensor = 1000000000;
1567 
1568         // Sequence to change sensor's FIFO rate
1569         // 1. enable Power state
1570         // 2. reset master enable
1571         // 3. Update delay
1572         // 4. set master enable
1573 
1574         // ensure power on
1575         onPower(1);
1576 
1577         // reset master enable
1578         masterEnable(0);
1579 
1580         /* search the minimum delay requested across all enabled sensors */
1581         for (int i = 0; i < numSensors; i++) {
1582             if (mEnabled & (1 << i)) {
1583                 int64_t ns = mDelays[i];
1584                 wanted = wanted < ns ? wanted : ns;
1585             }
1586         }
1587 
1588         // same delay for 3rd party Accel or Compass
1589         wanted_3rd_party_sensor = wanted;
1590 
1591         /* mpl rate in us in future maybe different for
1592            gyro vs compass vs accel */
1593         int rateInus = (int)wanted / 1000LL;
1594         int mplGyroRate = rateInus;
1595         int mplAccelRate = rateInus;
1596         int mplCompassRate = rateInus;
1597 
1598         LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
1599              "%llu ns, mpl rate: %d us, (%.2f Hz)",
1600              wanted, rateInus, 1000000000.f / wanted);
1601 
1602         /* set rate in MPL */
1603         /* compass can only do 100Hz max */
1604         inv_set_gyro_sample_rate(mplGyroRate);
1605         inv_set_accel_sample_rate(mplAccelRate);
1606         inv_set_compass_sample_rate(mplCompassRate);
1607 
1608         /* TODO: Test 200Hz */
1609         // inv_set_gyro_sample_rate(5000);
1610         LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
1611         LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate);
1612         LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate);
1613 
1614         int enabled_sensors = mEnabled;
1615         int tempFd = -1;
1616         if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
1617             if (isLowPowerQuatEnabled() ||
1618                     (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) {
1619                 bool setDMPrate= 0;
1620                 // Set LP Quaternion sample rate if enabled
1621                 if (checkLPQuaternion()) {
1622                     if (wanted < RATE_200HZ) {
1623                         enableLPQuaternion(0);
1624                     } else {
1625                         inv_set_quat_sample_rate(rateInus);
1626                         setDMPrate= 1;
1627                     }
1628                 }
1629 
1630                 if (checkDMPOrientation() || setDMPrate==1) {
1631                     getDmpRate(&wanted);
1632                 }
1633             }
1634 
1635             int64_t tempRate = wanted;
1636             LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
1637             //nsToHz
1638             LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1639                     1000000000.f / tempRate, mpu.gyro_fifo_rate,
1640                     getTimestamp());
1641             tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
1642             res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
1643             if(res < 0) {
1644                 LOGE("HAL:GYRO update delay error");
1645             }
1646 
1647             //nsToHz (BMA250)
1648             if(USE_THIRD_PARTY_ACCEL) {
1649                 LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
1650                         wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
1651                         getTimestamp());
1652                 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
1653                 res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
1654                 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
1655             }
1656 
1657             if (!mCompassSensor->isIntegrated()) {
1658                 LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor);
1659                 mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
1660                 got = mCompassSensor->getDelay(ID_M);
1661                 inv_set_compass_sample_rate(got / 1000);
1662             }
1663 
1664         } else {
1665 
1666             if (GY_ENABLED) {
1667                 wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
1668                     (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
1669                     (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
1670 
1671                 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1672                     getDmpRate(&wanted);
1673                 }
1674 
1675                 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1676                         1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
1677                 tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
1678                 res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
1679                 LOGE_IF(res < 0, "HAL:GYRO update delay error");
1680             }
1681 
1682             if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
1683                 if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
1684                     wanted = mDelays[Gyro];
1685                 }
1686                 else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
1687                     wanted = mDelays[RawGyro];
1688 
1689                 } else {
1690                     wanted = mDelays[Accelerometer];
1691                 }
1692 
1693                 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1694                     getDmpRate(&wanted);
1695                 }
1696 
1697                 /* TODO: use function pointers to calculate delay value specific to vendor */
1698                 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1699                         1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
1700                 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
1701                 if(USE_THIRD_PARTY_ACCEL) {
1702                     //BMA250 in ms
1703                     res = write_attribute_sensor(tempFd, wanted / 1000000L);
1704                 }
1705                 else {
1706                     //MPUxxxx in hz
1707                     res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
1708                 }
1709                 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
1710             }
1711 
1712             /* Invensense compass calibration */
1713             if (M_ENABLED) {
1714                 if (!mCompassSensor->isIntegrated()) {
1715                     wanted = mDelays[MagneticField];
1716                 } else {
1717                     if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
1718                         wanted = mDelays[Gyro];
1719                     }
1720                     else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
1721                         wanted = mDelays[RawGyro];
1722                     } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
1723                         wanted = mDelays[Accelerometer];
1724                     } else {
1725                         wanted = mDelays[MagneticField];
1726                     }
1727 
1728                     if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1729                         getDmpRate(&wanted);
1730                     }
1731                 }
1732 
1733                 mCompassSensor->setDelay(ID_M, wanted);
1734                 got = mCompassSensor->getDelay(ID_M);
1735                 inv_set_compass_sample_rate(got / 1000);
1736             }
1737 
1738         }
1739 
1740         unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
1741         if (sensors &
1742             (INV_THREE_AXIS_GYRO
1743                 | INV_THREE_AXIS_ACCEL
1744                 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
1745             res = masterEnable(1);
1746         } else { // all sensors idle -> reduce power
1747             res = onPower(0);
1748         }
1749     }
1750     pthread_mutex_unlock(&GlobalHalMutex);
1751     return res;
1752 }
1753 
1754 /* For Third Party Accel Input Subsystem Drivers only */
1755 /* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */
readAccelEvents(sensors_event_t *,int count)1756 int MPLSensor::readAccelEvents(sensors_event_t* /*data*/, int count)
1757 {
1758     VHANDLER_LOG;
1759 
1760     if (count < 1)
1761         return -EINVAL;
1762 
1763     ssize_t n = mAccelInputReader.fill(accel_fd);
1764     if (n < 0) {
1765         LOGE("HAL:missed accel events, exit");
1766         return n;
1767     }
1768 
1769     int numEventReceived = 0;
1770     input_event const* event;
1771     int nb, done = 0;
1772 
1773     while (!done && count && mAccelInputReader.readEvent(&event)) {
1774         int type = event->type;
1775         if (type == EV_ABS) {
1776             if (event->code == EVENT_TYPE_ACCEL_X) {
1777                 mPendingMask |= 1 << Accelerometer;
1778                 mCachedAccelData[0] = event->value;
1779             } else if (event->code == EVENT_TYPE_ACCEL_Y) {
1780                 mPendingMask |= 1 << Accelerometer;
1781                 mCachedAccelData[1] = event->value;
1782             } else if (event->code == EVENT_TYPE_ACCEL_Z) {
1783                 mPendingMask |= 1 << Accelerometer;
1784                 mCachedAccelData[2] =event-> value;
1785             }
1786         } else if (type == EV_SYN) {
1787             done = 1;
1788             if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
1789                 inv_build_accel(mCachedAccelData, 0, getTimestamp());
1790             }
1791         } else {
1792             LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
1793                     type, event->code);
1794         }
1795         mAccelInputReader.next();
1796     }
1797 
1798     return numEventReceived;
1799 }
1800 
1801 /**
1802  *  Should be called after reading at least one of gyro
1803  *  compass or accel data. (Also okay for handling all of them).
1804  *  @returns 0, if successful, error number if not.
1805  */
1806 /* TODO: This should probably be called "int readEvents(...)"
1807  *  and readEvents() called "void cacheData(void)".
1808  */
executeOnData(sensors_event_t * data,int count)1809 int MPLSensor::executeOnData(sensors_event_t* data, int count)
1810 {
1811     VFUNC_LOG;
1812 
1813     inv_execute_on_data();
1814 
1815     int numEventReceived = 0;
1816 
1817     long msg;
1818     msg = inv_get_message_level_0(1);
1819     if (msg) {
1820         if (msg & INV_MSG_MOTION_EVENT) {
1821             LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
1822         }
1823         if (msg & INV_MSG_NO_MOTION_EVENT) {
1824             LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
1825             /* after the first no motion, the gyro should be
1826                calibrated well */
1827             mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
1828             /* if gyros are on and we got a no motion, set a flag
1829                indicating that the cal file can be written. */
1830             mHaveGoodMpuCal = true;
1831         }
1832     }
1833 
1834     // load up virtual sensors
1835     for (int i = 0; i < numSensors; i++) {
1836         int update;
1837         if (mEnabled & (1 << i)) {
1838             update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
1839             mPendingMask |= (1 << i);
1840 
1841             if (update && (count > 0)) {
1842                 *data++ = mPendingEvents[i];
1843                 count--;
1844                 numEventReceived++;
1845             }
1846         }
1847     }
1848 
1849     return numEventReceived;
1850 }
1851 
1852 // collect data for MPL (but NOT sensor service currently), from driver layer
1853 /* TODO: FIX! data and count are not used, results is hardcoded to 0 */
1854 /* TODO: This should probably be called "void cacheEvents(void)"
1855  * And executeOnData() should be int readEvents(data,count)
1856  */
readEvents(sensors_event_t *,int)1857 int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) {
1858 
1859 
1860     int lp_quaternion_on = 0, nbyte;
1861     int i, nb, mask = 0, numEventReceived = 0,
1862         sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
1863             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
1864             (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0);
1865     char *rdata = mIIOBuffer;
1866 
1867     nbyte= (8 * sensors + 8) * 1;
1868 
1869     if (isLowPowerQuatEnabled()) {
1870         lp_quaternion_on = checkLPQuaternion();
1871         if (lp_quaternion_on) {
1872             nbyte += sizeof(mCachedQuaternionData);             //currently 16 bytes for Q data
1873         }
1874     }
1875 
1876     // pthread_mutex_lock(&mMplMutex);
1877     // pthread_mutex_lock(&mHALMutex);
1878 
1879     ssize_t rsize = read(iio_fd, rdata, nbyte);
1880     if (sensors == 0) {
1881         // read(iio_fd, rdata, nbyte);
1882         rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
1883     }
1884 
1885 #ifdef TESTING
1886     LOGI("get one sample of IIO data with size: %d", rsize);
1887     LOGI("sensors: %d", sensors);
1888 
1889     LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d",
1890         *((short *) (rdata + 0)), *((short *) (rdata + 2)),
1891         *((short *) (rdata + 4)));
1892     LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d",
1893         *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
1894         *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
1895         *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
1896 
1897     LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS &
1898         mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d",
1899         *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
1900             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
1901         *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
1902             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
1903         *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
1904             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
1905 #endif
1906 
1907     if (rsize < (nbyte - 8)) {
1908         LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)",
1909              rsize, nbyte, sensors, errno, strerror(errno));
1910         return -1;
1911     }
1912 
1913     if (isLowPowerQuatEnabled() && lp_quaternion_on) {
1914 
1915         for (i=0; i< 4; i++) {
1916             mCachedQuaternionData[i]= *(long*)rdata;
1917             rdata += sizeof(long);
1918         }
1919     }
1920 
1921     for (i = 0; i < 3; i++) {
1922         if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
1923             mCachedGyroData[i] = *((short *) (rdata + i * 2));
1924         }
1925         if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
1926             mCachedAccelData[i] = *((short *) (rdata + i * 2 +
1927                 ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
1928         }
1929         if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) {
1930             mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1)));
1931         }
1932     }
1933 
1934     mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
1935         ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
1936     if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
1937             (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
1938         mask |= 1 << MagneticField;
1939     }
1940 
1941     mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
1942     if (mCompassSensor->isIntegrated()) {
1943         mCompassTimestamp = mSensorTimestamp;
1944     }
1945 
1946     if (mask & (1 << Gyro)) {
1947         // send down temperature every 0.5 seconds
1948         // with timestamp measured in "driver" layer
1949         if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
1950             mTempCurrentTime = mSensorTimestamp;
1951             long long temperature[2];
1952             if(inv_read_temperature(temperature) == 0) {
1953                 LOGV_IF(INPUT_DATA,
1954                         "HAL:inv_read_temperature = %lld, timestamp= %lld",
1955                         temperature[0], temperature[1]);
1956                 inv_build_temp(temperature[0], temperature[1]);
1957             }
1958 #ifdef TESTING
1959             long bias[3], temp, temp_slope[3];
1960             inv_get_gyro_bias(bias, &temp);
1961             inv_get_gyro_ts(temp_slope);
1962 
1963             LOGI("T: %.3f "
1964                  "GB: %+13f %+13f %+13f "
1965                  "TS: %+13f %+13f %+13f "
1966                  "\n",
1967                  (float)temperature[0] / 65536.f,
1968                  (float)bias[0] / 65536.f / 16.384f,
1969                  (float)bias[1] / 65536.f / 16.384f,
1970                  (float)bias[2] / 65536.f / 16.384f,
1971                  temp_slope[0] / 65536.f,
1972                  temp_slope[1] / 65536.f,
1973                  temp_slope[2] / 65536.f);
1974 #endif
1975         }
1976 
1977         mPendingMask |= 1 << Gyro;
1978         mPendingMask |= 1 << RawGyro;
1979 
1980         if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
1981             inv_build_gyro(mCachedGyroData, mSensorTimestamp);
1982             LOGV_IF(INPUT_DATA,
1983                     "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
1984                     mCachedGyroData[0], mCachedGyroData[1],
1985                     mCachedGyroData[2], mSensorTimestamp);
1986         }
1987     }
1988 
1989     if (mask & (1 << Accelerometer)) {
1990         mPendingMask |= 1 << Accelerometer;
1991         if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
1992             inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
1993              LOGV_IF(INPUT_DATA,
1994                     "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
1995                     mCachedAccelData[0], mCachedAccelData[1],
1996                     mCachedAccelData[2], mSensorTimestamp);
1997         }
1998     }
1999 
2000     if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
2001         int status = 0;
2002         if (mCompassSensor->providesCalibration()) {
2003             status = mCompassSensor->getAccuracy();
2004             status |= INV_CALIBRATED;
2005         }
2006         if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
2007             inv_build_compass(mCachedCompassData, status,
2008                               mCompassTimestamp);
2009             LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
2010                     mCachedCompassData[0], mCachedCompassData[1],
2011                     mCachedCompassData[2], mCompassTimestamp);
2012         }
2013     }
2014 
2015     if (isLowPowerQuatEnabled() && lp_quaternion_on) {
2016 
2017         inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
2018         LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
2019                     mCachedQuaternionData[0], mCachedQuaternionData[1],
2020                     mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
2021     }
2022 
2023     // pthread_mutex_unlock(&mMplMutex);
2024     // pthread_mutex_unlock(&mHALMutex);
2025 
2026     return numEventReceived;
2027 }
2028 
2029 /* use for both MPUxxxx and third party compass */
readCompassEvents(sensors_event_t *,int count)2030 int MPLSensor::readCompassEvents(sensors_event_t* /*data*/, int count)
2031 {
2032     VHANDLER_LOG;
2033 
2034     if (count < 1)
2035         return -EINVAL;
2036 
2037     int numEventReceived = 0;
2038     int done = 0;
2039     int nb;
2040 
2041     // pthread_mutex_lock(&mMplMutex);
2042     // pthread_mutex_lock(&mHALMutex);
2043 
2044     done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
2045 #ifdef COMPASS_YAS53x
2046     if (mCompassSensor->checkCoilsReset()) {
2047        //Reset relevant compass settings
2048        resetCompass();
2049     }
2050 #endif
2051     if (done > 0) {
2052         int status = 0;
2053         if (mCompassSensor->providesCalibration()) {
2054             status = mCompassSensor->getAccuracy();
2055             status |= INV_CALIBRATED;
2056         }
2057         if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
2058             inv_build_compass(mCachedCompassData, status,
2059                               mCompassTimestamp);
2060             LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
2061                     mCachedCompassData[0], mCachedCompassData[1],
2062                     mCachedCompassData[2], mCompassTimestamp);
2063         }
2064     }
2065 
2066     // pthread_mutex_unlock(&mMplMutex);
2067     // pthread_mutex_unlock(&mHALMutex);
2068 
2069     return numEventReceived;
2070 }
2071 
2072 #ifdef COMPASS_YAS53x
resetCompass()2073 int MPLSensor::resetCompass()
2074 {
2075     VFUNC_LOG;
2076 
2077     //Reset compass cal if enabled
2078     if (mFeatureActiveMask & INV_COMPASS_CAL) {
2079        LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
2080        inv_init_vector_compass_cal();
2081     }
2082 
2083     //Reset compass fit if enabled
2084     if (mFeatureActiveMask & INV_COMPASS_FIT) {
2085        LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
2086        inv_init_compass_fit();
2087     }
2088 
2089     return 0;
2090 }
2091 #endif
2092 
getFd() const2093 int MPLSensor::getFd() const
2094 {
2095     VFUNC_LOG;
2096     LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
2097     return iio_fd;
2098 }
2099 
getAccelFd() const2100 int MPLSensor::getAccelFd() const
2101 {
2102     VFUNC_LOG;
2103     LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
2104     return accel_fd;
2105 }
2106 
getCompassFd() const2107 int MPLSensor::getCompassFd() const
2108 {
2109     VFUNC_LOG;
2110     int fd = mCompassSensor->getFd();
2111     LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
2112     return fd;
2113 }
2114 
turnOffAccelFifo()2115 int MPLSensor::turnOffAccelFifo() {
2116     int i, res, tempFd;
2117     char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
2118         mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
2119 
2120     for (i = 0; i < 3; i++) {
2121         res = write_sysfs_int(accel_fifo_enable[i], 0);
2122         if (res < 0) {
2123             return res;
2124         }
2125     }
2126     return 0;
2127 }
2128 
enableDmpOrientation(int en)2129 int MPLSensor::enableDmpOrientation(int en)
2130 {
2131     VFUNC_LOG;
2132     /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
2133     int res = 0;
2134     int enabled_sensors = mEnabled;
2135 
2136     if (isMpu3050())
2137         return res;
2138 
2139     pthread_mutex_lock(&GlobalHalMutex);
2140 
2141     // on power if not already On
2142     res = onPower(1);
2143     // reset master enable
2144     res = masterEnable(0);
2145 
2146     if (en) {
2147         //Enable DMP orientation
2148         if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
2149             LOGE("HAL:ERR can't enable Android orientation");
2150             res = -1; // indicate an err
2151         }
2152 
2153         // open DMP Orient Fd
2154         res = openDmpOrientFd();
2155 
2156         // enable DMP
2157         res = onDMP(1);
2158 
2159         // default DMP output rate to FIFO
2160         if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
2161             LOGE("HAL:ERR can't default DMP output rate");
2162         }
2163 
2164         // set DMP rate to 200Hz
2165         if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
2166             res = -1;
2167             LOGE("HAL:ERR can't set DMP rate to 200Hz");
2168         }
2169 
2170         // enable accel engine
2171         res = enableAccel(1);
2172 
2173         // disable accel FIFO
2174         if (!A_ENABLED) {
2175             res = turnOffAccelFifo();
2176         }
2177 
2178         mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
2179     } else {
2180         // disable DMP
2181         res = onDMP(0);
2182 
2183         // disable accel engine
2184         if (!A_ENABLED) {
2185             res = enableAccel(0);
2186         }
2187     }
2188 
2189     res = masterEnable(1);
2190     pthread_mutex_unlock(&GlobalHalMutex);
2191     return res;
2192 }
2193 
openDmpOrientFd()2194 int MPLSensor::openDmpOrientFd()
2195 {
2196     VFUNC_LOG;
2197 
2198     if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
2199         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
2200         return -1;
2201     }
2202 
2203     dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
2204     if (dmp_orient_fd < 0) {
2205         LOGE("HAL:ERR couldn't open dmpOrient node");
2206         return -1;
2207     } else {
2208         LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
2209         return 0;
2210     }
2211 }
2212 
closeDmpOrientFd()2213 int MPLSensor::closeDmpOrientFd()
2214 {
2215     VFUNC_LOG;
2216     if (dmp_orient_fd >= 0)
2217         close(dmp_orient_fd);
2218     return 0;
2219 }
2220 
dmpOrientHandler(int orient)2221 int MPLSensor::dmpOrientHandler(int orient)
2222 {
2223     VFUNC_LOG;
2224 
2225     LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
2226     return 0;
2227 }
2228 
readDmpOrientEvents(sensors_event_t * data,int count)2229 int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) {
2230     VFUNC_LOG;
2231 
2232     char dummy[4];
2233     int screen_orientation = 0;
2234     FILE *fp;
2235 
2236     fp = fopen(mpu.event_display_orientation, "r");
2237     if (fp == NULL) {
2238         LOGE("HAL:cannot open event_display_orientation");
2239         return 0;
2240     }
2241     fscanf(fp, "%d\n", &screen_orientation);
2242     fclose(fp);
2243 
2244     int numEventReceived = 0;
2245 
2246     if (mDmpOrientationEnabled && count > 0) {
2247         sensors_event_t temp;
2248 
2249         bzero(&temp, sizeof(temp));  /* Let's hope that SENSOR_TYPE_NONE is 0 */
2250         temp.version = sizeof(sensors_event_t);
2251         temp.sensor = ID_SO;
2252 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
2253         temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
2254         temp.screen_orientation = screen_orientation;
2255 #endif
2256         struct timespec ts;
2257         clock_gettime(CLOCK_MONOTONIC, &ts);
2258         temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
2259 
2260         *data++ = temp;
2261         count--;
2262         numEventReceived++;
2263     }
2264 
2265     // read dummy data per driver's request
2266     dmpOrientHandler(screen_orientation);
2267     read(dmp_orient_fd, dummy, 4);
2268 
2269     return numEventReceived;
2270 }
2271 
getDmpOrientFd()2272 int MPLSensor::getDmpOrientFd()
2273 {
2274     VFUNC_LOG;
2275 
2276     LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
2277     return dmp_orient_fd;
2278 
2279 }
2280 
checkDMPOrientation()2281 int MPLSensor::checkDMPOrientation()
2282 {
2283     VFUNC_LOG;
2284     return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
2285 }
2286 
getDmpRate(int64_t * wanted)2287 int MPLSensor::getDmpRate(int64_t *wanted)
2288 {
2289     if (checkDMPOrientation() || checkLPQuaternion()) {
2290         // set DMP output rate to FIFO
2291         write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
2292         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted);
2293 
2294         //DMP running rate must be @ 200Hz
2295         *wanted= RATE_200HZ;
2296         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
2297     }
2298     return 0;
2299 }
2300 
getPollTime()2301 int MPLSensor::getPollTime()
2302 {
2303     VHANDLER_LOG;
2304     return mPollTime;
2305 }
2306 
hasPendingEvents() const2307 bool MPLSensor::hasPendingEvents() const
2308 {
2309     VHANDLER_LOG;
2310     // if we are using the polling workaround, force the main
2311     // loop to check for data every time
2312     return (mPollTime != -1);
2313 }
2314 
2315 /* TODO: support resume suspend when we gain more info about them*/
sleepEvent()2316 void MPLSensor::sleepEvent()
2317 {
2318     VFUNC_LOG;
2319 }
2320 
wakeEvent()2321 void MPLSensor::wakeEvent()
2322 {
2323     VFUNC_LOG;
2324 }
2325 
inv_float_to_q16(float * fdata,long * ldata)2326 int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
2327 {
2328     VHANDLER_LOG;
2329 
2330     if (!fdata || !ldata)
2331         return -1;
2332     ldata[0] = (long)(fdata[0] * 65536.f);
2333     ldata[1] = (long)(fdata[1] * 65536.f);
2334     ldata[2] = (long)(fdata[2] * 65536.f);
2335     return 0;
2336 }
2337 
inv_long_to_q16(long * fdata,long * ldata)2338 int MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
2339 {
2340     VHANDLER_LOG;
2341 
2342     if (!fdata || !ldata)
2343         return -1;
2344     ldata[0] = (fdata[1] * 65536.f);
2345     ldata[1] = (fdata[2] * 65536.f);
2346     ldata[2] = (fdata[3] * 65536.f);
2347     return 0;
2348 }
2349 
inv_float_to_round(float * fdata,long * ldata)2350 int MPLSensor::inv_float_to_round(float *fdata, long *ldata)
2351 {
2352     VHANDLER_LOG;
2353 
2354     if (!fdata || !ldata)
2355             return -1;
2356     ldata[0] = (long)fdata[0];
2357     ldata[1] = (long)fdata[1];
2358     ldata[2] = (long)fdata[2];
2359     return 0;
2360 }
2361 
inv_float_to_round2(float * fdata,short * ldata)2362 int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
2363 {
2364     VHANDLER_LOG;
2365 
2366     if (!fdata || !ldata)
2367         return -1;
2368     ldata[0] = (short)fdata[0];
2369     ldata[1] = (short)fdata[1];
2370     ldata[2] = (short)fdata[2];
2371     return 0;
2372 }
2373 
inv_read_temperature(long long * data)2374 int MPLSensor::inv_read_temperature(long long *data)
2375 {
2376     VHANDLER_LOG;
2377 
2378     int count = 0;
2379     char raw_buf[40];
2380     long raw = 0;
2381 
2382     long long timestamp = 0;
2383 
2384     memset(raw_buf, 0, sizeof(raw_buf));
2385     count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
2386                                   sizeof(raw_buf));
2387     if(count < 1) {
2388         LOGE("HAL:error reading gyro temperature");
2389         return -1;
2390     }
2391 
2392     count = sscanf(raw_buf, "%ld%lld", &raw, &timestamp);
2393 
2394     if(count < 0) {
2395         return -1;
2396     }
2397 
2398     LOGV_IF(ENG_VERBOSE,
2399             "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
2400             raw, timestamp, count);
2401     data[0] = raw;
2402     data[1] = timestamp;
2403 
2404     return 0;
2405 }
2406 
inv_read_dmp_state(int fd)2407 int MPLSensor::inv_read_dmp_state(int fd)
2408 {
2409     VFUNC_LOG;
2410 
2411     if(fd < 0)
2412         return -1;
2413 
2414     int count = 0;
2415     char raw_buf[10];
2416     short raw = 0;
2417 
2418     memset(raw_buf, 0, sizeof(raw_buf));
2419     count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
2420     if(count < 1) {
2421         LOGE("HAL:error reading dmp state");
2422         close(fd);
2423         return -1;
2424     }
2425     count = sscanf(raw_buf, "%hd", &raw);
2426     if(count < 0) {
2427         LOGE("HAL:dmp state data is invalid");
2428         close(fd);
2429         return -1;
2430     }
2431     LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
2432     close(fd);
2433     return (int)raw;
2434 }
2435 
inv_read_sensor_bias(int fd,long * data)2436 int MPLSensor::inv_read_sensor_bias(int fd, long *data)
2437 {
2438     VFUNC_LOG;
2439 
2440     if(fd == -1) {
2441         return -1;
2442     }
2443 
2444     char buf[50];
2445     char x[15], y[15], z[15];
2446 
2447     memset(buf, 0, sizeof(buf));
2448     int count = read_attribute_sensor(fd, buf, sizeof(buf));
2449     if(count < 1) {
2450         LOGE("HAL:Error reading gyro bias");
2451         return -1;
2452     }
2453     count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
2454     if(count) {
2455         /* scale appropriately for MPL */
2456         LOGV_IF(ENG_VERBOSE,
2457                 "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
2458                 atol(x), atol(y), atol(z));
2459 
2460         data[0] = (long)(atol(x) / 10000 * (1L << 16));
2461         data[1] = (long)(atol(y) / 10000 * (1L << 16));
2462         data[2] = (long)(atol(z) / 10000 * (1L << 16));
2463 
2464         LOGV_IF(ENG_VERBOSE,
2465                 "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
2466                 data[0], data[1], data[2]);
2467     }
2468     return 0;
2469 }
2470 
2471 /** fill in the sensor list based on which sensors are configured.
2472  *  return the number of configured sensors.
2473  *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
2474  *  parameter len gives the length of the buffer pointed to by list
2475  */
populateSensorList(struct sensor_t * list,int len)2476 int MPLSensor::populateSensorList(struct sensor_t *list, int len)
2477 {
2478     VFUNC_LOG;
2479 
2480     int numsensors;
2481 
2482     if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
2483         LOGE("HAL:sensor list too small, not populating.");
2484         return -(sizeof(sSensorList) / sizeof(sensor_t));
2485     }
2486 
2487     /* fill in the base values */
2488     memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
2489 
2490     /* first add gyro, accel and compass to the list */
2491 
2492     /* fill in gyro/accel values */
2493     if(chip_ID == NULL) {
2494         LOGE("HAL:Can not get gyro/accel id");
2495     }
2496     fillGyro(chip_ID, list);
2497     fillAccel(chip_ID, list);
2498 
2499     // TODO: need fixes for unified HAL and 3rd-party solution
2500     mCompassSensor->fillList(&list[MagneticField]);
2501 
2502     if(1) {
2503         numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
2504         /* all sensors will be added to the list
2505            fill in orientation values */
2506         fillOrientation(list);
2507         /* fill in rotation vector values */
2508         fillRV(list);
2509         /* fill in gravity values */
2510         fillGravity(list);
2511         /* fill in Linear accel values */
2512         fillLinearAccel(list);
2513     } else {
2514         /* no 9-axis sensors, zero fill that part of the list */
2515         numsensors = 3;
2516         memset(list + 3, 0, 4 * sizeof(struct sensor_t));
2517     }
2518 
2519     return numsensors;
2520 }
2521 
fillAccel(const char * accel,struct sensor_t * list)2522 void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
2523 {
2524     VFUNC_LOG;
2525 
2526     if (accel) {
2527         if(accel != NULL && strcmp(accel, "BMA250") == 0) {
2528             list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
2529             list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
2530             list[Accelerometer].power = ACCEL_BMA250_POWER;
2531             list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
2532             return;
2533         } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
2534             list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
2535             list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
2536             list[Accelerometer].power = ACCEL_MPU6050_POWER;
2537             list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
2538             return;
2539         } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
2540             list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
2541             list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
2542             list[Accelerometer].power = ACCEL_MPU6500_POWER;
2543             list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
2544 
2545             return;
2546         } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
2547             list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
2548             list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
2549             list[Accelerometer].power = ACCEL_MPU9150_POWER;
2550             list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
2551             return;
2552         } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
2553             list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
2554             list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
2555             list[Accelerometer].power = ACCEL_BMA250_POWER;
2556             list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
2557             return;
2558         }
2559     }
2560 
2561     LOGE("HAL:unknown accel id %s -- "
2562          "params default to bma250 and might be wrong.",
2563          accel);
2564     list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
2565     list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
2566     list[Accelerometer].power = ACCEL_BMA250_POWER;
2567     list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
2568 }
2569 
fillGyro(const char * gyro,struct sensor_t * list)2570 void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
2571 {
2572     VFUNC_LOG;
2573 
2574     if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
2575         list[Gyro].maxRange = GYRO_MPU3050_RANGE;
2576         list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
2577         list[Gyro].power = GYRO_MPU3050_POWER;
2578         list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
2579     } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
2580         list[Gyro].maxRange = GYRO_MPU6050_RANGE;
2581         list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
2582         list[Gyro].power = GYRO_MPU6050_POWER;
2583         list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
2584     } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
2585         list[Gyro].maxRange = GYRO_MPU6500_RANGE;
2586         list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
2587         list[Gyro].power = GYRO_MPU6500_POWER;
2588         list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
2589     } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
2590         list[Gyro].maxRange = GYRO_MPU9150_RANGE;
2591         list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
2592         list[Gyro].power = GYRO_MPU9150_POWER;
2593         list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
2594     } else {
2595         LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
2596         LOGE("HAL:default to use mpu3050 params");
2597         list[Gyro].maxRange = GYRO_MPU3050_RANGE;
2598         list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
2599         list[Gyro].power = GYRO_MPU3050_POWER;
2600         list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
2601     }
2602 
2603     list[RawGyro].maxRange = list[Gyro].maxRange;
2604     list[RawGyro].resolution = list[Gyro].resolution;
2605     list[RawGyro].power = list[Gyro].power;
2606     list[RawGyro].minDelay = list[Gyro].minDelay;
2607 
2608     return;
2609 }
2610 
2611 /* fillRV depends on values of accel and compass in the list */
fillRV(struct sensor_t * list)2612 void MPLSensor::fillRV(struct sensor_t *list)
2613 {
2614     VFUNC_LOG;
2615 
2616     /* compute power on the fly */
2617     list[RotationVector].power = list[Gyro].power +
2618                                  list[Accelerometer].power +
2619                                  list[MagneticField].power;
2620     list[RotationVector].resolution = .00001;
2621     list[RotationVector].maxRange = 1.0;
2622     list[RotationVector].minDelay = 5000;
2623 
2624     return;
2625 }
2626 
fillOrientation(struct sensor_t * list)2627 void MPLSensor::fillOrientation(struct sensor_t *list)
2628 {
2629     VFUNC_LOG;
2630 
2631     list[Orientation].power = list[Gyro].power +
2632                               list[Accelerometer].power +
2633                               list[MagneticField].power;
2634     list[Orientation].resolution = .00001;
2635     list[Orientation].maxRange = 360.0;
2636     list[Orientation].minDelay = 5000;
2637 
2638     return;
2639 }
2640 
fillGravity(struct sensor_t * list)2641 void MPLSensor::fillGravity( struct sensor_t *list)
2642 {
2643     VFUNC_LOG;
2644 
2645     list[Gravity].power = list[Gyro].power +
2646                           list[Accelerometer].power +
2647                           list[MagneticField].power;
2648     list[Gravity].resolution = .00001;
2649     list[Gravity].maxRange = 9.81;
2650     list[Gravity].minDelay = 5000;
2651 
2652     return;
2653 }
2654 
fillLinearAccel(struct sensor_t * list)2655 void MPLSensor::fillLinearAccel(struct sensor_t *list)
2656 {
2657     VFUNC_LOG;
2658 
2659     list[LinearAccel].power = list[Gyro].power +
2660                           list[Accelerometer].power +
2661                           list[MagneticField].power;
2662     list[LinearAccel].resolution = list[Accelerometer].resolution;
2663     list[LinearAccel].maxRange = list[Accelerometer].maxRange;
2664     list[LinearAccel].minDelay = 5000;
2665 
2666     return;
2667 }
2668 
inv_init_sysfs_attributes(void)2669 int MPLSensor::inv_init_sysfs_attributes(void)
2670 {
2671     VFUNC_LOG;
2672 
2673     unsigned char i;
2674     char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
2675     char *sptr;
2676     char **dptr;
2677     int num;
2678 
2679     sysfs_names_ptr =
2680             (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
2681     sptr = sysfs_names_ptr;
2682     if (sptr == NULL) {
2683         LOGE("HAL:couldn't alloc mem for sysfs paths");
2684         return -1;
2685     }
2686 
2687     dptr = (char**)&mpu;
2688     i = 0;
2689     do {
2690         *dptr++ = sptr;
2691         sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
2692     } while (++i < MAX_SYSFS_ATTRB);
2693 
2694     // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
2695     // inv_get_sysfs_abs_path(sysfs_path);
2696     if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
2697         ALOGE("MPLSensor failed get sysfs path");
2698         return -1;
2699     }
2700 
2701     if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) {
2702         ALOGE("MPLSensor failed get iio trigger path");
2703         return -1;
2704     }
2705 
2706     sprintf(mpu.key, "%s%s", sysfs_path, "/key");
2707     sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
2708     sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
2709     sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
2710     sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
2711     sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name");
2712     sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
2713 
2714     sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware");
2715     sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
2716     sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
2717     sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
2718     sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on");
2719     sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
2720     sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
2721 
2722     // TODO: for self test
2723     sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
2724 
2725     sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
2726     sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
2727     sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
2728     sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
2729     sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en");
2730     sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
2731     sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");
2732 
2733     sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
2734     sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
2735     sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
2736 
2737 
2738 #ifndef THIRD_PARTY_ACCEL //MPUxxxx
2739     sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
2740     // TODO: for bias settings
2741     sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
2742 #endif
2743 
2744     sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
2745     sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
2746     sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
2747 
2748     sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
2749     sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
2750     sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
2751     sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
2752     sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
2753 
2754     sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
2755     sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
2756 
2757 #if SYSFS_VERBOSE
2758     // test print sysfs paths
2759     dptr = (char**)&mpu;
2760     for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
2761         LOGE("HAL:sysfs path: %s", *dptr++);
2762     }
2763 #endif
2764     return 0;
2765 }
2766 
2767 /* TODO: stop manually testing/using 0 and 1 instead of
2768  * false and true, but just use 0 and non-0.
2769  * This allows  passing 0 and non-0 ints around instead of
2770  * having to convert to 1 and test against 1.
2771  */
isMpu3050()2772 bool MPLSensor::isMpu3050()
2773 {
2774     return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050");
2775 }
2776 
isLowPowerQuatEnabled()2777 int MPLSensor::isLowPowerQuatEnabled()
2778 {
2779 #ifdef ENABLE_LP_QUAT_FEAT
2780     return !isMpu3050();
2781 #else
2782     return 0;
2783 #endif
2784 }
2785 
isDmpDisplayOrientationOn()2786 int MPLSensor::isDmpDisplayOrientationOn()
2787 {
2788 #ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
2789     return !isMpu3050();
2790 #else
2791     return 0;
2792 #endif
2793 }
2794