From 6aada2461e56077ede703ce53b7e21d1f1c3eb18 Mon Sep 17 00:00:00 2001
From: Cosmin Tanislav <demonsingur@gmail.com>
Date: Sun, 20 Feb 2022 19:34:44 +0200
Subject: [PATCH] sensors: Add udfps long press sensor

Co-authored-by: LuK1337 <priv.luk@gmail.com>
Change-Id: Ie78d7729201836bacd65a57f76e22adb61159192
---
 sensors/Sensor.cpp        | 142 ++++++++++++++++++++++++++++++++++++++
 sensors/Sensor.h          |  26 +++++++
 sensors/SensorsSubHal.cpp |   4 +-
 3 files changed, 171 insertions(+), 1 deletion(-)

diff --git a/sensors/Sensor.cpp b/sensors/Sensor.cpp
index dcf5b75..5fba482 100644
--- a/sensors/Sensor.cpp
+++ b/sensors/Sensor.cpp
@@ -17,10 +17,41 @@
 #include "Sensor.h"
 
 #include <hardware/sensors.h>
+#include <log/log.h>
 #include <utils/SystemClock.h>
 
 #include <cmath>
 
+namespace {
+
+static bool readFpState(int fd, int& screenX, int& screenY) {
+    char buffer[512];
+    int state = 0;
+    int rc;
+
+    rc = lseek(fd, 0, SEEK_SET);
+    if (rc) {
+        ALOGE("failed to seek: %d", rc);
+        return false;
+    }
+
+    rc = read(fd, &buffer, sizeof(buffer));
+    if (rc < 0) {
+        ALOGE("failed to read state: %d", rc);
+        return false;
+    }
+
+    rc = sscanf(buffer, "%d,%d,%d", &screenX, &screenY, &state);
+    if (rc < 0) {
+        ALOGE("failed to parse fp state: %d", rc);
+        return false;
+    }
+
+    return state > 0;
+}
+
+}  // anonymous namespace
+
 namespace android {
 namespace hardware {
 namespace sensors {
@@ -191,6 +222,117 @@ OneShotSensor::OneShotSensor(int32_t sensorHandle, ISensorsEventCallback* callba
     mSensorInfo.flags |= SensorFlagBits::ONE_SHOT_MODE;
 }
 
+UdfpsSensor::UdfpsSensor(int32_t sensorHandle, ISensorsEventCallback* callback)
+    : OneShotSensor(sensorHandle, callback) {
+    mSensorInfo.name = "UDFPS Sensor";
+    mSensorInfo.type =
+            static_cast<SensorType>(static_cast<int32_t>(SensorType::DEVICE_PRIVATE_BASE) + 1);
+    mSensorInfo.typeAsString = "org.lineageos.sensor.udfps";
+    mSensorInfo.maxRange = 2048.0f;
+    mSensorInfo.resolution = 1.0f;
+    mSensorInfo.power = 0;
+    mSensorInfo.flags |= SensorFlagBits::WAKE_UP;
+
+    int rc;
+
+    rc = pipe(mWaitPipeFd);
+    if (rc < 0) {
+        mWaitPipeFd[0] = -1;
+        mWaitPipeFd[1] = -1;
+        ALOGE("failed to open wait pipe: %d", rc);
+    }
+
+    mPollFd = open("/sys/kernel/oplus_display/fp_state", O_RDONLY);
+    if (mPollFd < 0) {
+        ALOGE("failed to open poll fd: %d", mPollFd);
+    }
+
+    if (mWaitPipeFd[0] < 0 || mWaitPipeFd[1] < 0 || mPollFd < 0) {
+        mStopThread = true;
+        return;
+    }
+
+    mPolls[0] = {
+            .fd = mWaitPipeFd[0],
+            .events = POLLIN,
+    };
+
+    mPolls[1] = {
+            .fd = mPollFd,
+            .events = POLLERR | POLLPRI,
+    };
+}
+
+UdfpsSensor::~UdfpsSensor() {
+    interruptPoll();
+}
+
+void UdfpsSensor::activate(bool enable) {
+    std::lock_guard<std::mutex> lock(mRunMutex);
+
+    if (mIsEnabled != enable) {
+        mIsEnabled = enable;
+
+        interruptPoll();
+        mWaitCV.notify_all();
+    }
+}
+
+void UdfpsSensor::setOperationMode(OperationMode mode) {
+    Sensor::setOperationMode(mode);
+    interruptPoll();
+}
+
+void UdfpsSensor::run() {
+    std::unique_lock<std::mutex> runLock(mRunMutex);
+
+    while (!mStopThread) {
+        if (!mIsEnabled || mMode == OperationMode::DATA_INJECTION) {
+            mWaitCV.wait(runLock, [&] {
+                return ((mIsEnabled && mMode == OperationMode::NORMAL) || mStopThread);
+            });
+        } else {
+            // Cannot hold lock while polling.
+            runLock.unlock();
+            int rc = poll(mPolls, 2, -1);
+            runLock.lock();
+
+            if (rc < 0) {
+                ALOGE("failed to poll: %d", rc);
+                mStopThread = true;
+                continue;
+            }
+
+            if (mPolls[1].revents == mPolls[1].events && readFpState(mPollFd, mScreenX, mScreenY)) {
+                mIsEnabled = false;
+                mCallback->postEvents(readEvents(), isWakeUpSensor());
+            } else if (mPolls[0].revents == mPolls[0].events) {
+                char buf;
+                read(mWaitPipeFd[0], &buf, sizeof(buf));
+            }
+        }
+    }
+}
+
+std::vector<Event> UdfpsSensor::readEvents() {
+    std::vector<Event> events;
+    Event event;
+    event.sensorHandle = mSensorInfo.sensorHandle;
+    event.sensorType = mSensorInfo.type;
+    event.timestamp = ::android::elapsedRealtimeNano();
+    event.u.data[0] = mScreenX;
+    event.u.data[1] = mScreenY;
+    events.push_back(event);
+    return events;
+}
+
+void UdfpsSensor::interruptPoll() {
+    if (mWaitPipeFd[1] < 0) return;
+
+    char c = '1';
+    write(mWaitPipeFd[1], &c, sizeof(c));
+}
+
 }  // namespace implementation
 }  // namespace subhal
 }  // namespace V2_1
diff --git a/sensors/Sensor.h b/sensors/Sensor.h
index b7cd4a5..aa1f194 100644
--- a/sensors/Sensor.h
+++ b/sensors/Sensor.h
@@ -17,6 +17,9 @@
 #pragma once
 
 #include <android/hardware/sensors/2.1/types.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <unistd.h>
 
 #include <condition_variable>
 #include <memory>
@@ -88,6 +91,29 @@ class OneShotSensor : public Sensor {
     virtual Result flush() override { return Result::BAD_VALUE; }
 };
 
+class UdfpsSensor : public OneShotSensor {
+  public:
+    UdfpsSensor(int32_t sensorHandle, ISensorsEventCallback* callback);
+    virtual ~UdfpsSensor() override;
+
+    virtual void activate(bool enable) override;
+    virtual void setOperationMode(OperationMode mode) override;
+
+  protected:
+    virtual void run() override;
+    virtual std::vector<Event> readEvents();
+
+  private:
+    void interruptPoll();
+
+    struct pollfd mPolls[2];
+    int mWaitPipeFd[2];
+    int mPollFd;
+
+    int mScreenX;
+    int mScreenY;
+};
+
 }  // namespace implementation
 }  // namespace subhal
 }  // namespace V2_1
diff --git a/sensors/SensorsSubHal.cpp b/sensors/SensorsSubHal.cpp
index 6cbcb56..9306b98 100644
--- a/sensors/SensorsSubHal.cpp
+++ b/sensors/SensorsSubHal.cpp
@@ -32,7 +32,9 @@ namespace implementation {
 using ::android::hardware::Void;
 using ::android::hardware::sensors::V2_0::implementation::ScopedWakelock;
 
-SensorsSubHal::SensorsSubHal() : mCallback(nullptr), mNextHandle(1) {}
+SensorsSubHal::SensorsSubHal() : mCallback(nullptr), mNextHandle(1) {
+    AddSensor<UdfpsSensor>();
+}
 
 Return<void> SensorsSubHal::getSensorsList_2_1(ISensors::getSensorsList_2_1_cb _hidl_cb) {
     std::vector<SensorInfo> sensors;