release-request-a84b3435-75fa-41e8-bd3c-ca0f4cbd5cc5-for-git_oc-m2-release-4352002 snap-temp-L88700000105039240android-8.1.0_r8android-8.1.0_r52android-8.1.0_r50android-8.1.0_r47android-8.1.0_r46android-8.1.0_r43android-8.1.0_r41android-8.1.0_r36android-8.1.0_r35android-8.1.0_r33android-8.1.0_r30android-8.1.0_r26android-8.1.0_r25android-8.1.0_r20android-8.1.0_r2oreo-m7-releaseoreo-m6-s4-releaseoreo-m6-s3-releaseoreo-m6-s2-releaseoreo-m2-s5-releaseoreo-m2-s4-releaseoreo-m2-s3-releaseoreo-m2-s2-releaseoreo-m2-s1-releaseoreo-m2-release

Change-Id: Ia5a1b811f0b0370a009f2e3a0b29978f17491da6

6 files changed, 89 insertions, 68 deletions

diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index 0a6d96d8..c7002535 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -180,6 +180,7 @@ void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th,
 
   acc->x_bias = acc->y_bias = acc->z_bias = 0;
   acc->x_bias_new = acc->y_bias_new = acc->z_bias_new = 0;
+  acc->average_temperature_celsius = 0;
 
 #ifdef IMU_TEMP_DBG_ENABLED
   acc->temp_time_nanos = 0;
@@ -422,7 +423,11 @@ static int accEigenTest(struct KasaFit *akf, struct AccelGoodData *agd) {
   float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
   evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
 
-  float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z);
+  float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
+
+  // Testing for negative number.
+  float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
+
   // Passing when evmin/evmax> EIGEN_RATIO.
   int eigen_pass = (evmin > evmax * EIGEN_RATIO) && (evmag > EIGEN_MAG);
 
@@ -526,10 +531,11 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
           // Eigen Ratio Test.
           if (accEigenTest(&acc->ac1[temp_gate].akf,
                            &acc->ac1[temp_gate].agd)) {
-            // Storing the new offsets.
+            // Storing the new offsets and average temperature.
             acc->x_bias_new = bias.x * KSCALE2;
             acc->y_bias_new = bias.y * KSCALE2;
             acc->z_bias_new = bias.z * KSCALE2;
+            acc->average_temperature_celsius = acc->ac1[temp_gate].agd.mean_t;
           }
 #ifdef ACCEL_CAL_DBG_ENABLED
           //// Debug ///////
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index e76abcd3..1cfef614 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -139,6 +139,9 @@ struct AccelCal {
   // to store a new offset, which gets updated during a power down event.
   float x_bias_new, y_bias_new, z_bias_new;
 
+  // Average temperature of the bias update.
+  float average_temperature_celsius;
+
   // Offset values that get subtracted from live data
   float x_bias, y_bias, z_bias;
 
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
index 1fb76302..7f8e563f 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -71,7 +71,10 @@ static int moc_eigen_test(struct KasaFit *kasa) {
   float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
   evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
 
-  float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z);
+  float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
+
+  // Testing for negative number.
+  float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
 
   int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) &&
                    (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG);
@@ -119,7 +122,8 @@ int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) {
   initVec3(&v, out.x, out.y, out.z);
   vec3ScalarMul(&v, -0.5f);
 
-  float r = sqrtf(vec3Dot(&v, &v) - out.w);
+  float r_square = vec3Dot(&v, &v) - out.w;
+  float r = (r_square > 0) ? sqrtf(r_square) : 0;
 
   initVec3(bias, v.x, v.y, v.z);
   *radius = r;
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index 251dcb9e..ba87c152 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -114,6 +114,21 @@ static bool isActivitySensor(int sensorIndex) {
         && sensorIndex <= COMMS_SENSOR_ACTIVITY_LAST;
 }
 
+static bool isWakeEvent(int32_t sensor)
+{
+    switch (sensor) {
+    case COMMS_SENSOR_DOUBLE_TOUCH:
+    case COMMS_SENSOR_DOUBLE_TWIST:
+    case COMMS_SENSOR_GESTURE:
+    case COMMS_SENSOR_PROXIMITY:
+    case COMMS_SENSOR_SIGNIFICANT_MOTION:
+    case COMMS_SENSOR_TILT:
+        return true;
+    default:
+        return false;
+    }
+}
+
 HubConnection::HubConnection()
     : Thread(false /* canCallJava */),
       mRing(10 *1024),
@@ -143,6 +158,7 @@ HubConnection::HubConnection()
 
     mWakelockHeld = false;
     mWakeEventCount = 0;
+    mWriteFailures = 0;
 
     initNanohubLock();
 
@@ -616,31 +632,20 @@ sensors_event_t *HubConnection::initEv(sensors_event_t *ev, uint64_t timestamp,
     return ev;
 }
 
-ssize_t HubConnection::getWakeEventCount()
+ssize_t HubConnection::decrementIfWakeEventLocked(int32_t sensor)
 {
-    return mWakeEventCount;
-}
-
-ssize_t HubConnection::decrementWakeEventCount()
-{
-    return --mWakeEventCount;
-}
-
-bool HubConnection::isWakeEvent(int32_t sensor)
-{
-    switch (sensor) {
-    case COMMS_SENSOR_PROXIMITY:
-    case COMMS_SENSOR_SIGNIFICANT_MOTION:
-    case COMMS_SENSOR_TILT:
-    case COMMS_SENSOR_DOUBLE_TWIST:
-    case COMMS_SENSOR_GESTURE:
-        return true;
-    default:
-        return false;
+    if (isWakeEvent(sensor)) {
+        if (mWakeEventCount > 0)
+            mWakeEventCount--;
+        else
+            ALOGW("%s: sensor=%d, unexpected count=%d, no-op",
+                  __FUNCTION__, sensor, mWakeEventCount);
     }
+
+    return mWakeEventCount;
 }
 
-void HubConnection::protectIfWakeEvent(int32_t sensor)
+void HubConnection::protectIfWakeEventLocked(int32_t sensor)
 {
     if (isWakeEvent(sensor)) {
         if (mWakelockHeld == false) {
@@ -653,6 +658,8 @@ void HubConnection::protectIfWakeEvent(int32_t sensor)
 
 void HubConnection::releaseWakeLockIfAppropriate()
 {
+    Mutex::Autolock autoLock(mLock);
+
     if (mWakelockHeld && (mWakeEventCount == 0)) {
         mWakelockHeld = false;
         release_wake_lock(WAKELOCK_NAME);
@@ -728,11 +735,8 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         break;
     }
 
-    if (cnt > 0) {
-        // If event is a wake event, protect it with a wakelock
-        protectIfWakeEvent(sensor);
+    if (cnt > 0)
         write(nev, cnt);
-    }
 }
 
 uint8_t HubConnection::magAccuracyUpdate(sensors_vec_t *sv)
@@ -830,11 +834,8 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         break;
     }
 
-    if (cnt > 0) {
-        // If event is a wake event, protect it with a wakelock
-        protectIfWakeEvent(sensor);
+    if (cnt > 0)
         write(nev, cnt);
-    }
 }
 
 void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t sensor, struct ThreeAxisSample *sample, bool highAccuracy)
@@ -1021,11 +1022,8 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         break;
     }
 
-    if (cnt > 0) {
-        // If event is a wake event, protect it with a wakelock
-        protectIfWakeEvent(sensor);
+    if (cnt > 0)
         write(nev, cnt);
-    }
 }
 
 void HubConnection::discardInotifyEvent() {
@@ -1659,10 +1657,6 @@ bool HubConnection::threadLoop() {
     return false;
 }
 
-ssize_t HubConnection::read(sensors_event_t *ev, size_t size) {
-    return mRing.read(ev, size);
-}
-
 void HubConnection::setActivityCallback(ActivityEventHandler *eventHandler)
 {
     Mutex::Autolock autoLock(mLock);
@@ -1912,8 +1906,44 @@ void HubConnection::initNanohubLock() {
     }
 }
 
+ssize_t HubConnection::read(sensors_event_t *ev, size_t size) {
+    ssize_t n = mRing.read(ev, size);
+
+    Mutex::Autolock autoLock(mLock);
+
+    // We log the first failure in write, so only log 2+ errors
+    if (mWriteFailures > 1) {
+        ALOGW("%s: mRing.write failed %d times",
+              __FUNCTION__, mWriteFailures);
+        mWriteFailures = 0;
+    }
+
+    for (ssize_t i = 0; i < n; i++)
+        decrementIfWakeEventLocked(ev[i].sensor);
+
+    return n;
+}
+
+
 ssize_t HubConnection::write(const sensors_event_t *ev, size_t n) {
-    return mRing.write(ev, n);
+    ssize_t ret = 0;
+
+    Mutex::Autolock autoLock(mLock);
+
+    for (size_t i=0; i<n; i++) {
+        if (mRing.write(&ev[i], 1) == 1) {
+            ret++;
+            // If event is a wake event, protect it with a wakelock
+            protectIfWakeEventLocked(ev[i].sensor);
+        } else {
+            if (mWriteFailures++ == 0)
+                ALOGW("%s: mRing.write failed @ %zu/%zu",
+                      __FUNCTION__, i, n);
+            break;
+        }
+    }
+
+    return ret;
 }
 
 #ifdef USB_MAG_BIAS_REPORTING_ENABLED
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
index 4dbd64e0..388660cf 100644
--- a/sensorhal/hubconnection.h
+++ b/sensorhal/hubconnection.h
@@ -79,10 +79,7 @@ struct HubConnection : public Thread {
 
     void setOperationParameter(const additional_info_event_t &info);
 
-    bool isWakeEvent(int32_t sensor);
     void releaseWakeLockIfAppropriate();
-    ssize_t getWakeEventCount();
-    ssize_t decrementWakeEventCount();
 
     //TODO: factor out event ring buffer functionality into a separate class
     ssize_t read(sensors_event_t *ev, size_t size);
@@ -111,7 +108,8 @@ private:
     bool mWakelockHeld;
     int32_t mWakeEventCount;
 
-    void protectIfWakeEvent(int32_t sensor);
+    void protectIfWakeEventLocked(int32_t sensor);
+    ssize_t decrementIfWakeEventLocked(int32_t sensor);
 
     static inline uint64_t period_ns_to_frequency_q10(nsecs_t period_ns) {
         return 1024000000000ULL / period_ns;
@@ -244,6 +242,7 @@ private:
     Mutex mLock;
 
     RingBuffer mRing;
+    int32_t mWriteFailures;
 
     ActivityEventHandler *mActivityEventHandler;
 
diff --git a/sensorhal/sensors.cpp b/sensorhal/sensors.cpp
index 0ddb263b..234f7da3 100644
--- a/sensorhal/sensors.cpp
+++ b/sensorhal/sensors.cpp
@@ -105,28 +105,7 @@ int SensorContext::poll(sensors_event_t *data, int count) {
     // Release wakelock if held and no more events in ring buffer
     mHubConnection->releaseWakeLockIfAppropriate();
 
-    ssize_t n = mHubConnection->read(data, count);
-
-    if (n < 0) {
-        return -1;
-    }
-
-    // If we have wake events in the queue, determine how many we're sending
-    // up this round and decrement that count now so that when we get called back,
-    // we'll have an accurate count of how many wake events are STILL in the HAL queue
-    // to be able to determine whether we can release our wakelock if held.
-    if (mHubConnection->getWakeEventCount() != 0) {
-        for (ssize_t i = 0; i < n; i++) {
-            if (mHubConnection->isWakeEvent(data[i].sensor)) {
-                ssize_t count = mHubConnection->decrementWakeEventCount();
-                if (count == 0) {
-                    break;
-                }
-            }
-        }
-    }
-
-    return n;
+    return mHubConnection->read(data, count);
 }
 
 int SensorContext::batch(