veux: Merge common tree to veux

- Ref:
  https://github.com/xiaomi-sm6375-devs/android_device_xiaomi_sm6375-common

Signed-off-by: kleidione <kleidione@gmail.com>

1 files changed, 351 insertions, 0 deletions

diff --git a/gps/utils/loc_misc_utils.cpp b/gps/utils/loc_misc_utils.cpp
new file mode 100644
index 0000000..fbcaef4
--- /dev/null
+++ b/gps/utils/loc_misc_utils.cpp
@@ -0,0 +1,351 @@
+/* Copyright (c) 2014, 2020 The Linux Foundation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above
+ *       copyright notice, this list of conditions and the following
+ *       disclaimer in the documentation and/or other materials provided
+ *       with the distribution.
+ *     * Neither the name of The Linux Foundation, nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#define LOG_NDEBUG 0
+#define LOG_TAG "LocSvc_misc_utils"
+#include <stdio.h>
+#include <string.h>
+#include <inttypes.h>
+#include <dlfcn.h>
+#include <math.h>
+#include <log_util.h>
+#include <loc_misc_utils.h>
+#include <ctype.h>
+#include <fcntl.h>
+#include <inttypes.h>
+
+#ifndef MSEC_IN_ONE_SEC
+#define MSEC_IN_ONE_SEC 1000ULL
+#endif
+#define GET_MSEC_FROM_TS(ts) ((ts.tv_sec * MSEC_IN_ONE_SEC) + (ts.tv_nsec + 500000)/1000000)
+
+int loc_util_split_string(char *raw_string, char **split_strings_ptr,
+                          int max_num_substrings, char delimiter)
+{
+    int raw_string_index=0;
+    int num_split_strings=0;
+    unsigned char end_string=0;
+    int raw_string_length=0;
+
+    if(!raw_string || !split_strings_ptr) {
+        LOC_LOGE("%s:%d]: NULL parameters", __func__, __LINE__);
+        num_split_strings = -1;
+        goto err;
+    }
+    LOC_LOGD("%s:%d]: raw string: %s\n", __func__, __LINE__, raw_string);
+    raw_string_length = strlen(raw_string) + 1;
+    split_strings_ptr[num_split_strings] = &raw_string[raw_string_index];
+    for(raw_string_index=0; raw_string_index < raw_string_length; raw_string_index++) {
+        if(raw_string[raw_string_index] == '\0')
+            end_string=1;
+        if((raw_string[raw_string_index] == delimiter) || end_string) {
+            raw_string[raw_string_index] = '\0';
+            if (num_split_strings < max_num_substrings) {
+                LOC_LOGD("%s:%d]: split string: %s\n",
+                         __func__, __LINE__, split_strings_ptr[num_split_strings]);
+            }
+            num_split_strings++;
+            if(((raw_string_index + 1) < raw_string_length) &&
+               (num_split_strings < max_num_substrings)) {
+                split_strings_ptr[num_split_strings] = &raw_string[raw_string_index+1];
+            }
+            else {
+                break;
+            }
+        }
+        if(end_string)
+            break;
+    }
+err:
+    LOC_LOGD("%s:%d]: num_split_strings: %d\n", __func__, __LINE__, num_split_strings);
+    return num_split_strings;
+}
+
+void loc_util_trim_space(char *org_string)
+{
+    char *scan_ptr, *write_ptr;
+    char *first_nonspace = NULL, *last_nonspace = NULL;
+
+    if(org_string == NULL) {
+        LOC_LOGE("%s:%d]: NULL parameter", __func__, __LINE__);
+        goto err;
+    }
+
+    scan_ptr = write_ptr = org_string;
+
+    while (*scan_ptr) {
+        //Find the first non-space character
+        if ( !isspace(*scan_ptr) && first_nonspace == NULL) {
+            first_nonspace = scan_ptr;
+        }
+        //Once the first non-space character is found in the
+        //above check, keep shifting the characters to the left
+        //to replace the spaces
+        if (first_nonspace != NULL) {
+            *(write_ptr++) = *scan_ptr;
+            //Keep track of which was the last non-space character
+            //encountered
+            //last_nonspace will not be updated in the case where
+            //the string ends with spaces
+            if ( !isspace(*scan_ptr)) {
+                last_nonspace = write_ptr;
+            }
+        }
+        scan_ptr++;
+    }
+    //Add NULL terminator after the last non-space character
+    if (last_nonspace) { *last_nonspace = '\0'; }
+err:
+    return;
+}
+
+inline void logDlError(const char* failedCall) {
+    const char * err = dlerror();
+    LOC_LOGe("%s error: %s", failedCall, (nullptr == err) ? "unknown" : err);
+}
+
+void* dlGetSymFromLib(void*& libHandle, const char* libName, const char* symName)
+{
+    void* sym = nullptr;
+    if ((nullptr != libHandle || nullptr != libName) && nullptr != symName) {
+        if (nullptr == libHandle) {
+            libHandle = dlopen(libName, RTLD_NOW);
+            if (nullptr == libHandle) {
+                logDlError("dlopen");
+            }
+        }
+        // NOT else, as libHandle gets assigned 5 line above
+        if (nullptr != libHandle) {
+            sym = dlsym(libHandle, symName);
+            if (nullptr == sym) {
+                logDlError("dlsym");
+            }
+        }
+    } else {
+        LOC_LOGe("Either libHandle (%p) or libName (%p) must not be null; "
+                 "symName (%p) can not be null.", libHandle, libName, symName);
+    }
+
+    return sym;
+}
+
+uint64_t getQTimerTickCount()
+{
+    uint64_t qTimerCount = 0;
+#if __aarch64__
+    asm volatile("mrs %0, cntvct_el0" : "=r" (qTimerCount));
+#elif defined (__i386__) || defined (__x86_64__)
+    /* Qtimer not supported in x86 architecture */
+    qTimerCount = 0;
+#else
+    asm volatile("mrrc p15, 1, %Q0, %R0, c14" : "=r" (qTimerCount));
+#endif
+
+    return qTimerCount;
+}
+
+uint64_t getQTimerDeltaNanos()
+{
+    char qtimer_val_string[100];
+    char *temp;
+    uint64_t local_qtimer = 0, remote_qtimer = 0;
+    int mdm_fd = -1, wlan_fd = -1, ret = 0;
+    uint64_t delta = 0;
+
+    memset(qtimer_val_string, '\0', sizeof(qtimer_val_string));
+
+    char devNode[] = "/sys/bus/mhi/devices/0306_00.01.00/time_us";
+    for (; devNode[27] < 3 && mdm_fd < 0; devNode[27]++) {
+        mdm_fd = ::open(devNode, O_RDONLY);
+        if (mdm_fd < 0) {
+            LOC_LOGe("MDM open file: %s error: %s", devNode, strerror(errno));
+        }
+    }
+    if (mdm_fd > 0) {
+        ret = read(mdm_fd, qtimer_val_string, sizeof(qtimer_val_string)-1);
+        ::close(mdm_fd);
+        if (ret < 0) {
+            LOC_LOGe("MDM read time_us file error: %s", strerror(errno));
+        } else {
+            temp = qtimer_val_string;
+            temp = strchr(temp, ':');
+            temp = temp + 2;
+            local_qtimer = atoll(temp);
+
+            temp = strchr(temp, ':');
+            temp = temp + 2;
+            remote_qtimer = atoll(temp);
+
+            if (local_qtimer >= remote_qtimer) {
+                delta = (local_qtimer - remote_qtimer) * 1000;
+            }
+            LOC_LOGv("qtimer values in microseconds: local:%" PRIi64 " remote:%" PRIi64 ""
+                     " delta in nanoseconds:%" PRIi64 "",
+                     local_qtimer, remote_qtimer, delta);
+        }
+    }
+    return delta;
+}
+
+uint64_t getQTimerFreq()
+{
+#if __aarch64__
+    uint64_t val = 0;
+    asm volatile("mrs %0, cntfrq_el0" : "=r" (val));
+#elif defined (__i386__) || defined (__x86_64__)
+    /* Qtimer not supported in x86 architecture */
+    uint64_t val = 0;
+#else
+    uint32_t val = 0;
+    asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (val));
+#endif
+    return val;
+}
+
+uint64_t getBootTimeMilliSec()
+{
+    struct timespec curTs;
+    clock_gettime(CLOCK_BOOTTIME, &curTs);
+    return (uint64_t)GET_MSEC_FROM_TS(curTs);
+}
+
+// Used for convert position/velocity from GSNS antenna based to VRP based
+void Matrix_MxV(float a[3][3],  float b[3], float c[3]) {
+    int i, j;
+
+    for (i=0; i<3; i++) {
+        c[i] = 0.0f;
+        for (j=0; j<3; j++)
+            c[i] += a[i][j] * b[j];
+    }
+}
+
+// Used for convert position/velocity from GNSS antenna based to VRP based
+void Matrix_Skew(float a[3], float c[3][3]) {
+    c[0][0] = 0.0f;
+    c[0][1] = -a[2];
+    c[0][2] = a[1];
+    c[1][0] = a[2];
+    c[1][1] = 0.0f;
+    c[1][2] = -a[0];
+    c[2][0] = -a[1];
+    c[2][1] = a[0];
+    c[2][2] = 0.0f;
+}
+
+// Used for convert position/velocity from GNSS antenna based to VRP based
+void Euler2Dcm(float euler[3], float dcm[3][3]) {
+    float cr = 0.0, sr = 0.0, cp = 0.0, sp = 0.0, ch = 0.0, sh = 0.0;
+
+    cr = cosf(euler[0]);
+    sr = sinf(euler[0]);
+    cp = cosf(euler[1]);
+    sp = sinf(euler[1]);
+    ch = cosf(euler[2]);
+    sh = sinf(euler[2]);
+
+    dcm[0][0] = cp * ch;
+    dcm[0][1] = (sp*sr*ch) - (cr*sh);
+    dcm[0][2] = (cr*sp*ch) + (sh*sr);
+
+    dcm[1][0] = cp * sh;
+    dcm[1][1] = (sr*sp*sh) + (cr*ch);
+    dcm[1][2] = (cr*sp*sh) - (sr*ch);
+
+    dcm[2][0] = -sp;
+    dcm[2][1] = sr * cp;
+    dcm[2][2] = cr * cp;
+}
+
+// Used for convert position from GSNS based to VRP based
+// The converted position will be stored in the llaInfo parameter.
+#define A6DOF_WGS_A (6378137.0f)
+#define A6DOF_WGS_B (6335439.0f)
+#define A6DOF_WGS_E2 (0.00669437999014f)
+void loc_convert_lla_gnss_to_vrp(double lla[3], float rollPitchYaw[3],
+                                 float leverArm[3]) {
+    LOC_LOGv("lla: %f, %f, %f, lever arm: %f %f %f, "
+             "rollpitchyaw: %f %f %f",
+             lla[0], lla[1], lla[2],
+             leverArm[0], leverArm[1], leverArm[2],
+             rollPitchYaw[0], rollPitchYaw[1], rollPitchYaw[2]);
+
+    float cnb[3][3];
+    memset(cnb, 0, sizeof(cnb));
+    Euler2Dcm(rollPitchYaw, cnb);
+
+    float sl = sin(lla[0]);
+    float cl = cos(lla[0]);
+    float sf = 1.0f / (1.0f - A6DOF_WGS_E2 * sl* sl);
+    float sfr = sqrtf(sf);
+
+    float rn = A6DOF_WGS_B * sf * sfr + lla[2];
+    float re = A6DOF_WGS_A * sfr + lla[2];
+
+    float deltaNEU[3];
+
+    // gps_pos_lla = imu_pos_lla + Cbn*la_b .* [1/geo.Rn; 1/(geo.Re*geo.cL); -1];
+    Matrix_MxV(cnb, leverArm, deltaNEU);
+
+    // NED to lla conversion
+    lla[0] = lla[0] + deltaNEU[0] / rn;
+    lla[1] = lla[1] + deltaNEU[1] / (re * cl);
+    lla[2] = lla[2] + deltaNEU[2];
+}
+
+// Used for convert velocity from GSNS based to VRP based
+// The converted velocity will be stored in the enuVelocity parameter.
+void loc_convert_velocity_gnss_to_vrp(float enuVelocity[3], float rollPitchYaw[3],
+                                      float rollPitchYawRate[3], float leverArm[3]) {
+
+    LOC_LOGv("enu velocity: %f, %f, %f, lever arm: %f %f %f, roll pitch yaw: %f %f %f,"
+             "rollpitchyawRate: %f %f %f",
+             enuVelocity[0], enuVelocity[1], enuVelocity[2],
+             leverArm[0], leverArm[1], leverArm[2],
+             rollPitchYaw[0], rollPitchYaw[1], rollPitchYaw[2],
+             rollPitchYawRate[0], rollPitchYawRate[1], rollPitchYawRate[2]);
+
+    float cnb[3][3];
+    memset(cnb, 0, sizeof(cnb));
+    Euler2Dcm(rollPitchYaw, cnb);
+
+    float skewLA[3][3];
+    memset(skewLA, 0, sizeof(skewLA));
+    Matrix_Skew(leverArm, skewLA);
+
+    float tmp[3];
+    float deltaEnuVelocity[3];
+    memset(tmp, 0, sizeof(tmp));
+    memset(deltaEnuVelocity, 0, sizeof(deltaEnuVelocity));
+    Matrix_MxV(skewLA, rollPitchYawRate, tmp);
+    Matrix_MxV(cnb, tmp, deltaEnuVelocity);
+
+    enuVelocity[0] = enuVelocity[0] - deltaEnuVelocity[0];
+    enuVelocity[1] = enuVelocity[1] - deltaEnuVelocity[1];
+    enuVelocity[2] = enuVelocity[2] - deltaEnuVelocity[2];
+}