From 7450d7c4a3cd327a3092ff8eb51d64b2a856b8dc Mon Sep 17 00:00:00 2001
From: Jan Altensen <info@stricted.net>
Date: Thu, 3 Oct 2019 18:35:54 +0200
Subject: [PATCH] sdm710-common: kang camera motor from guacamole

Change-Id: Ic4dfc1b7d3716d2154be1364aa93400be2a116ff
---
 camera_motor/CameraMotor.cpp | 47 +++++++++++++++++++++++++++++++++---
 1 file changed, 43 insertions(+), 4 deletions(-)

diff --git a/camera_motor/CameraMotor.cpp b/camera_motor/CameraMotor.cpp
index 76bfb27..227cee6 100644
--- a/camera_motor/CameraMotor.cpp
+++ b/camera_motor/CameraMotor.cpp
@@ -19,15 +19,20 @@
 #include "CameraMotor.h"
 #include <android-base/logging.h>
 #include <hidl/HidlTransportSupport.h>
+#include <chrono>
 #include <fstream>
+#include <thread>
 
-#define CAMERA_MOTOR_ENABLE "/sys/bus/platform/devices/vendor:motor_pl/enable"
-#define CAMERA_MOTOR_DIRECTION "/sys/bus/platform/devices/vendor:motor_pl/direction"
+#define CAMERA_MOTOR_ENABLE "/sys/devices/platform/vendor/vendor:motor_pl/enable"
+#define CAMERA_MOTOR_DIRECTION "/sys/devices/platform/vendor/vendor:motor_pl/direction"
 #define CAMERA_MOTOR_HALL_CALIBRATION "/sys/bus/platform/devices/vendor:motor_pl/hall_calibration"
+#define CAMERA_MOTOR_POSITION "/sys/devices/platform/vendor/vendor:motor_pl/position"
 #define CAMERA_PERSIST_HALL_CALIBRATION "/mnt/vendor/persist/engineermode/hall_calibration"
 #define DIRECTION_DOWN "0"
 #define DIRECTION_UP "1"
 #define HALL_CALIBRATION_DEFAULT "170,170,480,0,0,480,500,0,0,500,1500"
+#define POSITION_DOWN "1"
+#define POSITION_UP "0"
 #define ENABLED "1"
 #define CAMERA_ID_FRONT "1"
 
@@ -38,6 +43,8 @@ namespace motor {
 namespace V1_0 {
 namespace implementation {
 
+using namespace std::chrono_literals;
+
 /*
  * Write value to path and close file.
  */
@@ -56,6 +63,26 @@ static T get(const std::string& path, const T& def) {
     return file.fail() ? def : result;
 }
 
+static void waitUntilFileChange(const std::string& path, const std::string &val,
+        const std::chrono::milliseconds relativeTimeout) {
+    auto startTime = std::chrono::steady_clock::now();
+
+    while (true) {
+        if (get<std::string>(path, "") == val) {
+            return;
+        }
+
+        std::this_thread::sleep_for(50ms);
+
+        auto now = std::chrono::steady_clock::now();
+        auto timeElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - startTime);
+
+        if (timeElapsed > relativeTimeout) {
+            return;
+        }
+    }
+}
+
 CameraMotor::CameraMotor() {
     // Load motor hall calibration data
     set(CAMERA_MOTOR_HALL_CALIBRATION,
@@ -63,22 +90,34 @@ CameraMotor::CameraMotor() {
 }
 
 Return<void> CameraMotor::onConnect(const hidl_string& cameraId) {
-    if (cameraId == CAMERA_ID_FRONT) {
+    auto motorPosition = get<std::string>(CAMERA_MOTOR_POSITION, "");
+
+    if (cameraId == CAMERA_ID_FRONT && motorPosition == POSITION_DOWN) {
         LOG(INFO) << "Popping out front camera";
 
         set(CAMERA_MOTOR_DIRECTION, DIRECTION_UP);
         set(CAMERA_MOTOR_ENABLE, ENABLED);
+        waitUntilFileChange(CAMERA_MOTOR_POSITION, POSITION_UP, 5s);
+    } else if (cameraId != CAMERA_ID_FRONT && motorPosition == POSITION_UP) {
+        LOG(INFO) << "Retracting front camera";
+
+        set(CAMERA_MOTOR_DIRECTION, DIRECTION_DOWN);
+        set(CAMERA_MOTOR_ENABLE, ENABLED);
+        waitUntilFileChange(CAMERA_MOTOR_POSITION, POSITION_DOWN, 5s);
     }
 
     return Void();
 }
 
 Return<void> CameraMotor::onDisconnect(const hidl_string& cameraId) {
-    if (cameraId == CAMERA_ID_FRONT) {
+    auto motorPosition = get<std::string>(CAMERA_MOTOR_POSITION, "");
+
+    if (cameraId == CAMERA_ID_FRONT && motorPosition == POSITION_UP) {
         LOG(INFO) << "Retracting front camera";
 
         set(CAMERA_MOTOR_DIRECTION, DIRECTION_DOWN);
         set(CAMERA_MOTOR_ENABLE, ENABLED);
+        waitUntilFileChange(CAMERA_MOTOR_POSITION, POSITION_DOWN, 5s);
     }
 
     return Void();