[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/connectivity/gps/flp_hal/inc/data_coder.h b/src/connectivity/gps/flp_hal/inc/data_coder.h
new file mode 100644
index 0000000..fc66d2f
--- /dev/null
+++ b/src/connectivity/gps/flp_hal/inc/data_coder.h
@@ -0,0 +1,32 @@
+#ifndef __DATA_CODER_H__
+#define __DATA_CODER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+char get_byte(char* buff, int* offset, int src_len);
+short get_short(char* buff, int* offset, int src_len);
+int get_int(char* buff, int* offset, int src_len);
+long long get_long(char* buff, int* offset, int src_len);
+float get_float(char* buff, int* offset, int src_len);
+double get_double(char* buff, int* offset, int src_len);
+char* get_string(char* buff, int* offset, int src_len);
+char* get_string2(char* buff, int* offset, int src_len);
+int get_binary(char* buff, int* offset, char* output, int src_len, int des_len);
+
+void put_byte(char* buff, int* offset, const char input);
+void put_short(char* buff, int* offset, const short input);
+void put_int(char* buff, int* offset, const int input);
+void put_long(char* buff, int* offset, const long long input);
+void put_float(char* buff, int* offset, const float input);
+void put_double(char* buff, int* offset, const double input);
+void put_string(char* buff, int* offset, const char* input);
+void put_binary(char* buff, int* offset, const char* input, int len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/src/connectivity/gps/flp_hal/inc/flphal_interface.h b/src/connectivity/gps/flp_hal/inc/flphal_interface.h
new file mode 100644
index 0000000..062e40a
--- /dev/null
+++ b/src/connectivity/gps/flp_hal/inc/flphal_interface.h
@@ -0,0 +1,114 @@
+#ifndef __HAL2FLP_INTERFACE_H__
+#define __HAL2FLP_INTERFACE_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <hardware/fused_location.h>
+
+//======================================================
+// FLP -> FLP HAL
+//======================================================
+#define FLP_TO_MNL "flp_to_mnl"
+#define MNL_TO_FLP "mnl_to_flp"
+
+/*****************************************************************************
+ * FLP Return Value for APIs
+ *****************************************************************************/
+#define MTK_FLP_SUCCESS (0)
+#define MTK_FLP_ERROR (-1)
+#define MTK_FLP_TIMEOUT (-2)
+
+#ifndef MTK_FLP_TRUE
+#define MTK_FLP_TRUE (1)
+#endif
+
+#ifndef MTK_FLP_FALSE
+#define MTK_FLP_FALSE (0)
+#endif
+
+#define HAL_FLP_BUFF_SIZE (1 * 1024)
+
+#ifdef __aarch64__
+#define MTK_64_PLATFORM
+#endif
+
+
+/*************************************************
+ Message Table for MTK FLP
+*************************************************/
+typedef enum{
+ MTK_FLP_MSG_SYS_FLPD_RESET_NTF = 100,
+
+ //HAL messages
+ MTK_FLP_MSG_HAL_INIT_CMD = 200,
+ MTK_FLP_MSG_HAL_START_CMD,
+ MTK_FLP_MSG_HAL_STOP_CMD,
+ MTK_FLP_MSG_HAL_STOP_RSP,
+ MTK_FLP_MSG_HAL_SET_OPTION_CMD,
+ MTK_FLP_MSG_HAL_INJECT_LOC_CMD,
+ MTK_FLP_MSG_HAL_DIAG_INJECT_DATA_NTF,
+ MTK_FLP_MSG_HAL_DIAG_REPORT_DATA_NTF,
+
+ MTK_FLP_MSG_HSB_REPORT_LOC_NTF = 300,
+ MTK_FLP_MSG_OFL_REPORT_LOC_NTF,
+
+ MTK_FLP_MSG_HAL_GEOFENCE_CALLBACK_NTF = 400,
+ MTK_FLP_MSG_HAL_REQUEST_LOC_NTF,
+ MTK_FLP_MSG_HAL_FLUSH_LOC_NTF,
+ MTK_FLP_MSG_HAL_REPORT_STATUS_NTF,
+ MTK_FLP_MSG_OFL_GEOFENCE_CALLBACK_NTF, //for Offload geofence use
+ MTK_FLP_MSG_OFL_GEOFENCE_CMD,
+
+ MTK_FLP_MSG_DC_START_CMD = 500,
+ MTK_FLP_MSG_DC_STOP_CMD,
+ MTK_FLP_MSG_CONN_SCREEN_STATUS, //for AP to connsys screen on/off status exchange
+ MTK_FLP_MSG_END,
+}MTK_FLP_MSG_TYPE;
+
+typedef enum {
+ MTK_FLP_IDLE_MODE = -1,
+ MTK_FLP_BATCH_SLEEP_ON_FIFO_FULL = 0,
+ MTK_FLP_BATCH_WAKE_ON_FIFO_FULL = 1,
+} MTK_FLP_BATCH_MODE_T;
+
+typedef struct {
+ int type;
+ int length;
+} MTK_FLP_MSG_T;
+
+typedef struct {
+ double max_power_allocation_mW;
+ unsigned int sources_to_use;
+ int flags;
+ int64_t period_ns;
+} MTK_FLP_BATCH_OPTION_T;
+
+
+typedef FlpDiagnosticCallbacks (*FlpDiagCB)();
+typedef void (* timer_callback)();
+
+int mnl2flphal_flp_init();
+int isflp_thread_exist() ;
+void flphal2mnl_flp_init();
+int flphal2mnl_flp_start(int id, FlpBatchOptions* options);
+int flphal2mnl_flp_reboot_done_ntf();
+int flphal2mnl_update_batching_options(int id, FlpBatchOptions* options);
+int flphal2mnl_stop_batching(int id) ;
+void flphal2mnl_get_batched_location(int last_n_locations);
+int flphal2mnl_inject_location(FlpLocation* location);
+int flphal2mnl_diag_inject_data(char* data, int length);
+void flphal2mnl_flush_batched_locations() ;
+int flp_send2mnl(const char* buff, int len);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/connectivity/gps/flp_hal/src/data_coder.c b/src/connectivity/gps/flp_hal/src/data_coder.c
new file mode 100644
index 0000000..ab67b38
--- /dev/null
+++ b/src/connectivity/gps/flp_hal/src/data_coder.c
@@ -0,0 +1,154 @@
+#include <string.h>
+
+#ifndef MAX
+#define MAX(A,B) ((A)>(B)?(A):(B))
+#endif
+#ifndef MIN
+#define MIN(A,B) ((A)<(B)?(A):(B))
+#endif
+
+//===============================================================
+// get
+char get_byte(char* buff, int* offset, int src_len) {
+ char ret = 0;
+ if (*offset >= 0 && *offset < src_len) {
+ ret = buff[*offset];
+ *offset += 1;
+ }
+ return ret;
+}
+
+short get_short(char* buff, int* offset, int src_len) {
+ short ret = 0;
+ ret |= get_byte(buff, offset, src_len) & 0xff;
+ ret |= (get_byte(buff, offset, src_len) << 8);
+ return ret;
+}
+
+int get_int(char* buff, int* offset, int src_len) {
+ int ret = 0;
+ ret |= get_short(buff, offset, src_len) & 0xffff;
+ ret |= (get_short(buff, offset, src_len) << 16);
+ return ret;
+}
+
+long long get_long(char* buff, int* offset, int src_len) {
+ long long ret = 0;
+ ret |= get_int(buff, offset, src_len) & 0xffffffffL;
+ ret |= ((long long)get_int(buff, offset, src_len) << 32);
+ return ret;
+}
+
+float get_float(char* buff, int* offset, int src_len) {
+ float ret;
+ int tmp = get_int(buff, offset, src_len);
+ ret = *((float*)&tmp);
+ return ret;
+}
+
+double get_double(char* buff, int* offset, int src_len) {
+ double ret;
+ long long tmp = get_long(buff, offset, src_len);
+ ret = *((double*)&tmp);
+ return ret;
+}
+
+char* get_string(char* buff, int* offset, int src_len) {
+ char ret = get_byte(buff, offset, src_len);
+ if (ret == 0) {
+ return NULL;
+ } else {
+ char* p = NULL;
+ int len = get_int(buff, offset, src_len);
+ if (*offset < 0 || *offset >= src_len) {
+ *offset = 0;
+ }
+ if ((len > src_len - *offset) || len < 0) {
+ len = src_len - *offset;
+ }
+ if (len <= 0) {
+ return NULL;
+ }
+ p = &buff[*offset];
+ p[len-1] = 0;
+ *offset += len;
+ return p;
+ }
+}
+
+char* get_string2(char* buff, int* offset, int src_len) {
+ char* output = get_string(buff, offset, src_len);
+ if (output == NULL) {
+ return "";
+ } else {
+ return output;
+ }
+}
+
+int get_binary(char* buff, int* offset, char* output, int src_len, int des_len) {
+ int len = 0;
+ if (*offset >= 0 && *offset <= src_len) {
+ len = get_int(buff, offset, src_len);
+ if (len > MIN((src_len-(*offset)), des_len)) {
+ len = MIN((src_len-(*offset)), des_len);
+ }
+ if (len > 0) {
+ memcpy(output, &buff[*offset], len);
+ *offset += len;
+ }
+ }
+ return len;
+}
+
+//===============================================================
+// put
+void put_byte(char* buff, int* offset, const char input) {
+ *((char*)&buff[*offset]) = input;
+ *offset += 1;
+}
+
+void put_short(char* buff, int* offset, const short input) {
+ put_byte(buff, offset, input & 0xff);
+ put_byte(buff, offset, (input >> 8) & 0xff);
+}
+
+void put_int(char* buff, int* offset, const int input) {
+ put_short(buff, offset, input & 0xffff);
+ put_short(buff, offset, (input >> 16) & 0xffff);
+}
+
+void put_long(char* buff, int* offset, const long long input) {
+ put_int(buff, offset, input & 0xffffffffL);
+ put_int(buff, offset, ((input >> 32L) & 0xffffffffL));
+}
+
+void put_float(char* buff, int* offset, const float input) {
+ int* data = (int*)&input;
+ put_int(buff, offset, *data);
+}
+
+void put_double(char* buff, int* offset, const double input) {
+ long long* data = (long long*)&input;
+ put_long(buff, offset, *data);
+}
+
+void put_string(char* buff, int* offset, const char* input) {
+ if (input == NULL) {
+ put_byte(buff, offset, 0);
+ } else {
+ int len = strlen(input) + 1;
+ put_byte(buff, offset, 1);
+ put_int(buff, offset, len);
+ memcpy(&buff[*offset], input, len);
+ *offset += len;
+ }
+}
+
+void put_binary(char* buff, int* offset, const char* input, const int len) {
+ put_int(buff, offset, len);
+ if (len > 0) {
+ memcpy(&buff[*offset], input, len);
+ *offset += len;
+ }
+}
+
diff --git a/src/connectivity/gps/flp_hal/src/flp2hal_interface.c b/src/connectivity/gps/flp_hal/src/flp2hal_interface.c
new file mode 100644
index 0000000..a69a698
--- /dev/null
+++ b/src/connectivity/gps/flp_hal/src/flp2hal_interface.c
@@ -0,0 +1,320 @@
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/un.h>
+#include "flphal_interface.h"
+#include "data_coder.h"
+#include "mtk_auto_log.h"
+
+#if defined(__ANDROID_OS__)
+#include <cutils/android_filesystem_config.h>
+#endif
+
+#ifdef LOG_TAG
+#undef LOG_TAG
+#define LOG_TAG "flphal"
+#endif
+
+
+/**********************************************************
+ * Define *
+ **********************************************************/
+typedef void*(*threadptr)(void*);
+#define MNLD_STRNCPY(dst,src,size) do{\
+ strncpy((char *)(dst), (src), (size - 1));\
+ (dst)[size - 1] = '\0';\
+ }while(0)
+
+/**********************************************************
+ * Function Declaration *
+ **********************************************************/
+void mnl2flphal_jni_thread(void);
+extern void mtk_mnl2flp_hal_response (MTK_FLP_MSG_T *prmsg);
+
+/**********************************************************
+ * Global vars *
+ **********************************************************/
+static pthread_t hal_jni_thread_id;
+static int g_ThreadExitJniSocket = 0, isflp_exist = 0;
+static char g_flp2mnl_path[128] = FLP_TO_MNL;
+int g_server_socket_fd;
+
+/**********************************************************
+ * Socket Function *
+ **********************************************************/
+static int safe_recvfrom(int sockfd, char* buf, int len) {
+ int ret = 0;
+ int retry = 10;
+
+ while ((ret = recvfrom(sockfd, buf, len, 0, NULL, NULL)) == -1) {
+ //LOGW("ret=%d len=%d\n", ret, len);
+ if (errno == EINTR) continue;
+ if (errno == EAGAIN) {
+ if (retry-- > 0) {
+ usleep(100 * 1000);
+ continue;
+ }
+ }
+ LOGE("sendto reason=[%s]\n", strerror(errno));
+ break;
+ }
+ return ret;
+}
+
+// -1 means failure
+static int safe_sendto(int sockfd, const char* dest, const char* buf, int size) {
+ int len = 0;
+ struct sockaddr_un soc_addr;
+ int retry = 10;
+
+ memset(&soc_addr, 0, sizeof(soc_addr));
+ soc_addr.sun_path[0] = 0;
+ MNLD_STRNCPY(soc_addr.sun_path + 1, dest,sizeof(soc_addr.sun_path) - 1);
+ soc_addr.sun_family = AF_UNIX;
+
+ while ((len = sendto(sockfd, buf, size, 0, (const struct sockaddr *)&soc_addr, sizeof(soc_addr))) == -1) {
+ if (errno == EINTR) continue;
+ if (errno == EAGAIN) {
+ if (retry-- > 0) {
+ usleep(100 * 1000);
+ continue;
+ }
+ }
+ LOGE("sendto dest=[%s] len=%d reason=[%s]\n",
+ dest, size, strerror(errno));
+ break;
+ }
+ return len;
+}
+
+int flp_send2mnl(const char* buff, int len) {
+ int ret = 0;
+ int sockfd = socket(PF_LOCAL, SOCK_DGRAM, 0);
+ if (safe_sendto(sockfd, g_flp2mnl_path, buff, len) < 0) {
+ LOGE("flp_send2mnl safe_sendto failed\n");
+ ret = -1;
+ }
+ close(sockfd);
+ return ret;
+}
+
+static int bind_udp_socket(char* path) {
+ int sockfd;
+ struct sockaddr_un soc_addr;
+
+ sockfd = socket(PF_LOCAL, SOCK_DGRAM, 0);
+ if (sockfd < 0) {
+ LOGE("socket failed reason=[%s]\n", strerror(errno));
+ return -1;
+ }
+
+ memset(&soc_addr, 0, sizeof(soc_addr));
+ soc_addr.sun_path[0] = 0;
+ MNLD_STRNCPY(soc_addr.sun_path + 1, path,sizeof(soc_addr.sun_path) - 1);
+ soc_addr.sun_family = AF_UNIX;
+ unlink(soc_addr.sun_path);
+ if (bind(sockfd, (struct sockaddr *)&soc_addr, sizeof(soc_addr)) < 0) {
+ LOGE("bind failed path=[%s] reason=[%s]\n", path, strerror(errno));
+ close(sockfd);
+ return -1;
+ }
+
+ return sockfd;
+}
+
+int isflp_thread_exist() {
+ return isflp_exist;
+}
+
+
+/*************************************************************/
+/* create thread to collect response from mnl to FLP HAL */
+/*************************************************************/
+int mnl2flphal_flp_init() {
+ int ret;
+
+ //Init client socket thread
+ if(isflp_exist) {
+ LOGE("flp thread exist");
+ return MTK_FLP_SUCCESS;
+ }
+ ret = pthread_create(&hal_jni_thread_id, NULL, (threadptr)mnl2flphal_jni_thread, NULL);
+ if(ret < 0) {
+ LOGE("Create client thread error\n");
+ return MTK_FLP_ERROR;
+ }
+ isflp_exist = 1;
+ return MTK_FLP_SUCCESS;
+}
+
+/*************************************************************/
+/* mnl to FLP HAL socket, -1 means failure */
+/*************************************************************/
+int create_mnl2flphal_fd() {
+ int fd = bind_udp_socket(MNL_TO_FLP);
+ return fd;
+}
+
+/*************************************************************/
+/* rearrange location structure in 64-bit platform */
+/*************************************************************/
+#ifdef MTK_64_PLATFORM
+void mnl2flphal_loc_rearrange(char *loc_in, FlpLocation *loc_out) {
+ char ratio = 2; //32 to 64-bits
+ char padding_diff = 4;
+ int sizeof_loc_in = sizeof(FlpLocation) - sizeof(size_t)/ratio - padding_diff;
+
+ if((loc_out == NULL)||(loc_in == NULL)) {
+ LOGE("mnl2flphal_loc_rearrange is NULL");
+ return;
+ }
+
+ memset(loc_out, 0, sizeof(FlpLocation));
+ loc_out->size = sizeof(FlpLocation);
+ memcpy(&loc_out->flags, loc_in + sizeof(size_t)/ratio, sizeof(uint16_t));
+ memcpy(&loc_out->latitude, loc_in + sizeof(size_t), sizeof_loc_in - sizeof(size_t));
+}
+#endif
+
+/*************************************************************/
+/* Handle message from mnl to FLP HAL */
+/*************************************************************/
+int mnl2flphal_hdlr (char *buff) {
+ char data[1024] ={0};
+ MTK_FLP_MSG_T *prmsg = NULL;
+ int offset = 0;
+ int ret = MTK_FLP_ERROR, len;
+ unsigned int cmd, msg_len;
+#ifdef MTK_64_PLATFORM
+ char loc_in[128] = {0};
+ FlpLocation loc_out;
+#else
+ FlpLocation loc_in = {0};
+#endif
+ MTK_FLP_MSG_T *prmsg_loc = NULL;
+
+ if(buff == NULL) {
+ LOGE("mnl2flphal_hdlr, recv prmsg is null pointer\r\n");
+ return MTK_FLP_ERROR;
+ }
+
+ len = get_binary(buff, &offset, data, HAL_FLP_BUFF_SIZE, sizeof(data));
+ if((len > 0) && (len<=1024)) {
+ prmsg = (MTK_FLP_MSG_T *)&data[0];
+ } else {
+ LOGE("len err:%d",len);
+ return ret;
+ }
+ cmd = prmsg->type;
+ msg_len = prmsg->length;
+
+ LOGD("msg_len, recv prmsg, type %u, len %d\r\n", cmd, msg_len);
+ switch (cmd) {
+ case MTK_FLP_MSG_SYS_FLPD_RESET_NTF:
+ case MTK_FLP_MSG_HAL_REQUEST_LOC_NTF:
+ case MTK_FLP_MSG_HAL_FLUSH_LOC_NTF:
+ case MTK_FLP_MSG_HAL_REPORT_STATUS_NTF:
+ //Send sys init to trigger flpd attach mnld
+ mtk_mnl2flp_hal_response((MTK_FLP_MSG_T *)prmsg);
+ break;
+ case MTK_FLP_MSG_HSB_REPORT_LOC_NTF:
+ case MTK_FLP_MSG_OFL_REPORT_LOC_NTF:
+#ifndef MTK_64_PLATFORM
+ if(msg_len % sizeof(FlpLocation) != 0) {
+ LOGE("REPORT_LOC_NTF: Data length ERROR, %d, %d",msg_len,sizeof(FlpLocation));
+ return MTK_FLP_ERROR;
+ }
+ memcpy( &loc_in, ((char *)prmsg + sizeof(MTK_FLP_MSG_T)), sizeof(FlpLocation));
+ if(loc_in.accuracy < 0.000001) {
+ LOGE("invalid_LOC_NTF: loc accuracy = %f",loc_in.accuracy);
+ return MTK_FLP_ERROR;
+ }
+ mtk_mnl2flp_hal_response((MTK_FLP_MSG_T *)prmsg);
+#else
+ memcpy( loc_in, ((char *)prmsg + sizeof(MTK_FLP_MSG_T)), 128*sizeof(char));
+ mnl2flphal_loc_rearrange(loc_in, &loc_out);
+ if(loc_out.accuracy < 0.000001) {
+ LOGE("invalid_LOC_NTF: loc accuracy = %f",loc_out.accuracy);
+ return MTK_FLP_ERROR;
+ }
+ //reform location ntf msg
+ prmsg_loc = malloc(sizeof(MTK_FLP_MSG_T)+ sizeof(FlpLocation));
+ if(prmsg_loc == NULL) {
+ LOGE("mnl2flphal_hdlr malloc failed");
+ return MTK_FLP_ERROR;
+ }
+ prmsg_loc->type = cmd;
+ prmsg_loc->length = sizeof(FlpLocation);
+ memcpy(((char*)prmsg_loc) + sizeof(MTK_FLP_MSG_T), &loc_out, sizeof(FlpLocation));
+ mtk_mnl2flp_hal_response(prmsg_loc);
+ free(prmsg_loc);
+#endif
+ break;
+ default:
+ LOGE("Uknown received type:0x%02x", cmd);
+ break;
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+/*************************************************************/
+/* mnl to FLP HAL main thread */
+/*************************************************************/
+void mnl2flphal_jni_thread(void) {
+ int ret = MTK_FLP_SUCCESS;
+ int offset = 0;
+ int read_len;
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+ MTK_FLP_MSG_T flp_header;
+
+ LOGD("mtk_flp_hal_jni_thread, Create\n");
+
+ g_server_socket_fd = create_mnl2flphal_fd();
+
+ ///TODO: add system timer function here
+ //mtk_flp_sys_timer_create(FLP_TIMER_ID_CHECKCNN);
+ //Send SYS_INIT to HAL
+ flp_header.type = MTK_FLP_MSG_HAL_INIT_CMD;
+ flp_header.length = 0;
+ put_binary(buff, &offset, (const char*)&flp_header, sizeof(MTK_FLP_MSG_T));
+ ret = flp_send2mnl(buff, offset);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ }
+
+ if (g_server_socket_fd >= 0) {
+ while(!g_ThreadExitJniSocket) {
+ // - recv msg from socket interface
+ read_len = safe_recvfrom(g_server_socket_fd, buff, sizeof(buff));
+ if ((read_len <= 0) || (read_len > HAL_FLP_BUFF_SIZE)) {
+ LOGE("mnl2flp safe_recvfrom failed read_len=%d", read_len);
+ continue;
+ }
+
+ if (!g_ThreadExitJniSocket) {
+ // Process received message
+ mnl2flphal_hdlr(buff);
+ } else {
+ LOGE("mtk_flp_hal_jni_thread, read msg fail,exit socket thread\n");
+ //read msg fail...
+ g_ThreadExitJniSocket = 1;
+ }
+ }
+ }
+
+ //close socket
+ LOGD("Closing server_fd,%d\r\n",g_server_socket_fd);
+ if(g_server_socket_fd >= 0) {
+ close(g_server_socket_fd);
+ }
+ LOGD("mtk_flp_hal_jni_thread, exit\n");
+ g_ThreadExitJniSocket = 1;
+ pthread_exit(NULL);
+
+ return;
+}
+
+
diff --git a/src/connectivity/gps/flp_hal/src/flpinf.c b/src/connectivity/gps/flp_hal/src/flpinf.c
new file mode 100644
index 0000000..52bea65
--- /dev/null
+++ b/src/connectivity/gps/flp_hal/src/flpinf.c
@@ -0,0 +1,560 @@
+#include <cutils/sockets.h>
+#include <sys/time.h>
+#include <time.h>
+#include <hardware/fused_location.h>
+#include "flphal_interface.h"
+#include "mtk_auto_log.h"
+
+#ifdef LOG_TAG
+#undef LOG_TAG
+#define LOG_TAG "flphal"
+#endif
+
+/**********************************************************
+ * Define *
+ **********************************************************/
+#define MTK_FLP_BATCH_SIZE 30
+#define UNICODE_BUF_LEN 2048
+#define FLP_BATCHED_LOCATION_FLUSH_HANDLER_TIMEOUT (200)
+
+
+/**********************************************************
+ * Global vars *
+ **********************************************************/
+static FlpCallbacks *mtkFlpCallbacks;
+static FlpDiagnosticCallbacks *mtkFlpDiagCallbacks;
+static FlpBatchOptions gFusedOptions;
+static int FlpCapability = 0;
+static int FlpLastStatus = 2;
+static FlpLocation FlpLocBuffer[MTK_FLP_BATCH_SIZE];
+static int FlpLocNum=0;
+static int FlpLocBegin=0;
+static int FlpLocEnd=0;
+static char fgtimerStarted = 0;
+static char fgtimerInit = 0;
+static timer_t batch_loc_timer;
+
+
+extern struct hw_module_t HAL_MODULE_INFO_SYM;
+static void mtk_flp_flush_loc();
+static void flp_timeout_handler();
+
+/*********************************************************/
+/* Timer Functions */
+/*********************************************************/
+// -1 means failure
+timer_t init_timer_id(timer_callback cb, int id) {
+ struct sigevent sevp;
+ timer_t timerid;
+
+ memset(&sevp, 0, sizeof(sevp));
+ sevp.sigev_value.sival_int = id;
+ sevp.sigev_notify = SIGEV_THREAD;
+ sevp.sigev_notify_function = cb;
+
+ if (timer_create(CLOCK_MONOTONIC, &sevp, &timerid) == -1) {
+ LOGE("timer_create failed reason=[%s]", strerror(errno));
+ return (timer_t)-1;
+ }
+ LOGD("timer create ok\n");
+ return timerid;
+}
+
+// -1 means failure
+timer_t init_timer(timer_callback cb) {
+ return init_timer_id(cb, 0);
+}
+
+// -1 means failure
+int start_timer(timer_t timerid, int milliseconds) {
+ struct itimerspec expire;
+ expire.it_interval.tv_sec = 0;
+ expire.it_interval.tv_nsec = 0;
+ expire.it_value.tv_sec = milliseconds/1000;
+ expire.it_value.tv_nsec = (milliseconds%1000)*1000000;
+ return timer_settime(timerid, 0, &expire, NULL);
+}
+
+// -1 means failure
+int stop_timer(timer_t timerid) {
+ return start_timer(timerid, 0);
+}
+
+/*********************************************************/
+/* FLP Location Diagnostic Interface implementation */
+/*********************************************************/
+void mtk_flp_diag_init(FlpDiagnosticCallbacks* callbacks) {
+ TRC();
+ if(callbacks == NULL) {
+ return;
+ }
+ mtkFlpDiagCallbacks = callbacks;
+}
+
+int mtk_flp_diag_inject_data(char* data, int length) {
+ TRC();
+ return flphal2mnl_diag_inject_data(data,length);
+}
+
+/*********************************************************/
+/* FLP Device Context Interface implementation */
+/*********************************************************/
+int mtk_flp_dev_inject_device_context(uint32_t enabledMask) {
+ UNUSED(enabledMask);
+ return MTK_FLP_SUCCESS;
+}
+
+/*********************************************************/
+/* FLP Location Interface implementation */
+/*********************************************************/
+int mtk_flp_init(FlpCallbacks* callbacks) {
+ int ret = MTK_FLP_ERROR;
+ TRC();
+ if(callbacks == NULL) {
+ return ret;
+ }
+ mtkFlpCallbacks = callbacks;
+ if(mtkFlpCallbacks == NULL) {
+ LOGE("mtkFlpCallbacks == NULL");
+ return ret;
+ }
+
+ if(mtkFlpCallbacks->set_thread_event_cb) {
+ LOGE("mtkFlpCallbacks.set_thread_event_cb");
+ mtkFlpCallbacks->set_thread_event_cb(ASSOCIATE_JVM);
+ } else {
+ LOGE("set_thread_event_cb not set!!");
+ }
+
+ if(mtkFlpCallbacks->flp_capabilities_cb) {
+ LOGE("mtkFlpCallbacks.flp_capabilities_cb");
+ FlpCapability = (int)(CAPABILITY_GNSS);
+ mtkFlpCallbacks->flp_capabilities_cb(FlpCapability);
+ } else {
+ LOGE("mtkFlpCallbacks.flp_capabilities_cb not set!!");
+ }
+ if(!fgtimerInit) {
+ batch_loc_timer = init_timer(flp_timeout_handler);
+ fgtimerInit = 1;
+ }
+
+ flphal2mnl_flp_init();
+ ret = mnl2flphal_flp_init();
+ return ret;
+}
+
+int mtk_flp_get_batch_size() {
+ TRC();
+ return MTK_FLP_BATCH_SIZE;
+}
+
+int mtk_flp_start_batching(int id, FlpBatchOptions* options) {
+ TRC();
+ memcpy(&gFusedOptions,options,sizeof(FlpBatchOptions));
+ return flphal2mnl_flp_start(id, options);
+}
+
+int mtk_flp_update_batching_options(int id, FlpBatchOptions* options) {
+ TRC();
+ return flphal2mnl_update_batching_options(id, options);
+}
+
+int mtk_flp_stop_batching(int id) {
+ TRC();
+ return flphal2mnl_stop_batching(id);
+}
+
+void mtk_flp_cleanup() {
+ TRC();
+ mtkFlpDiagCallbacks = NULL;
+ mtkFlpCallbacks = NULL;
+}
+
+void mtk_flp_get_batched_location(int last_n_locations) {
+ TRC();
+ flphal2mnl_get_batched_location(last_n_locations);
+}
+
+int mtk_flp_inject_location(FlpLocation* location) {
+ TRC();
+ return flphal2mnl_inject_location(location);
+}
+
+static const FlpDiagnosticInterface mtkFlpDiagnosticInterface = {
+ sizeof(FlpDiagnosticInterface),
+ mtk_flp_diag_init,
+ mtk_flp_diag_inject_data,
+};
+
+static const FlpDeviceContextInterface mtkFlpDeviceContextInterface = {
+ sizeof(FlpDeviceContextInterface),
+ mtk_flp_dev_inject_device_context,
+};
+
+const void* mtk_flp_get_extension(const char* name) {
+ TRC();
+ if(!strcmp(name, FLP_DIAGNOSTIC_INTERFACE)) {
+ return &mtkFlpDiagnosticInterface;
+ } else if(!strcmp(name, FLP_DEVICE_CONTEXT_INTERFACE)) {
+ return &mtkFlpDeviceContextInterface;
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+void mtk_flp_flush_batched_locations() {
+ TRC();
+ flphal2mnl_flush_batched_locations();
+}
+
+static void flp_timeout_handler() {
+ //flush location
+ mtk_flp_flush_loc();
+ stop_timer(batch_loc_timer);
+ fgtimerStarted =0;
+ LOGD("ofl_batch_timeout\n");
+}
+
+/*********************************************************/
+/* FLP Location check and debug print */
+/*********************************************************/
+
+static char mtk_flp_sys_dbg_check_loc(FlpLocation *loc) {
+ if(loc->size < sizeof(FlpLocation)) {
+ LOGE("Wrong location size:%zu, %zu", loc->size, sizeof(FlpLocation));
+ return 0;
+ }
+ if(loc->longitude>180 || loc->longitude < -180 || loc->latitude >90 || loc->latitude<-90) {
+ LOGE("Wrong location value, longitude:%f, latitude:%f", loc->longitude, loc->latitude);
+ return 0;
+ }
+ return 1;
+}
+
+static void mtk_flp_sys_dbg_dump_loc(FlpLocation *loc) {
+ #if 0
+ LOGD("Location(%x):LNG:%f LAT:%f ALT:%f ACC:%f SPD:%f BEARING:%f, FLAGS:%04X SOURCES:%08X Timestamp:%lld",
+ loc, loc->longitude, loc->latitude, loc->altitude, loc->accuracy,
+ loc->speed, loc->bearing, loc->flags, loc->sources_used, loc->timestamp);
+ #endif
+
+ if(mtk_flp_sys_dbg_check_loc(loc) != 1) {
+ LOGE("ERROR dumping location!!!!");
+ return;
+ }
+}
+
+static void mtk_flp_dump_locations(int n, FlpLocation** locs) {
+ int i;
+ TRC();
+ for(i = 0; i < n; i++){
+ mtk_flp_sys_dbg_dump_loc(locs[i]);
+ }
+}
+
+/*********************************************************/
+/* FLP Location ring buffer preparation */
+/*********************************************************/
+static int FlpLocRingIsFull(void) {
+ return (FlpLocNum==MTK_FLP_BATCH_SIZE);
+}
+
+static int FlpLocRingIsEmpty(void) {
+ return (FlpLocNum==0);
+}
+
+//Add one location to the Ring buffer. Remove oldest when full.
+static void FlpLocRingAdd(FlpLocation *p) {
+ if(p == NULL) {
+ LOGE("Adding NULL");
+ return;
+ }
+ mtk_flp_sys_dbg_dump_loc(p);
+ memcpy(&FlpLocBuffer[FlpLocBegin], p, sizeof(FlpLocation));
+
+ //LOGD("LocRing Add to %d(%x),%d", FlpLocBegin, &FlpLocBuffer[FlpLocBegin],FlpLocNum);
+
+ FlpLocBegin++;
+ if(FlpLocBegin == MTK_FLP_BATCH_SIZE) {
+ FlpLocBegin = 0;
+ }
+
+ if( FlpLocNum < MTK_FLP_BATCH_SIZE) { //if already not full yet.
+ FlpLocNum++;
+ } else { //if full. drop oldest
+ FlpLocEnd++;
+ if(FlpLocEnd == MTK_FLP_BATCH_SIZE) {
+ FlpLocEnd = 0;
+ }
+ }
+}
+
+//Remove a location from tail.
+static int FlpLocRingRemove(FlpLocation **p) {
+ if(FlpLocRingIsEmpty()) {
+ LOGE("Ring empty!!!");
+ return MTK_FLP_ERROR;
+ }
+ *p = &FlpLocBuffer[FlpLocEnd];
+
+ FlpLocNum--;
+ FlpLocEnd++;
+ if(FlpLocEnd == MTK_FLP_BATCH_SIZE) {
+ FlpLocEnd = 0;
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+//return the number of locations.
+static int FlpLocRingRemoveN(FlpLocation **p, int N) {
+ int idx=0;
+ if(N<=0) {
+ LOGE("Wrong N");
+ return 0;
+ }
+
+ while(!FlpLocRingIsEmpty()) {
+ FlpLocRingRemove(&p[idx]);
+ idx++;
+ if(idx == N) {
+ break;
+ }
+ }
+ return idx;
+}
+
+
+//get the value of last N location without removing it.
+static int FlpLocRingPeekLastN(FlpLocation **p, int N) {
+ int b,e,num,cur_num=0;
+
+ b = FlpLocBegin;
+ e = FlpLocEnd;
+ num = FlpLocNum;
+
+ while(cur_num < N && num > 0) {
+ b--;
+ if(b < 0) {
+ b = MTK_FLP_BATCH_SIZE-1;
+ }
+ mtk_flp_sys_dbg_dump_loc(&FlpLocBuffer[b]);
+ p[cur_num] = &FlpLocBuffer[b]; //copy pointer only. not content.
+ //LOGD("LocRing Peek to %d(%x)", b, &FlpLocBuffer[b]);
+ num--;
+ cur_num++;
+ }
+ return cur_num;
+}
+
+/*********************************************************/
+/* FLP Location, status & NTF Callback */
+/*********************************************************/
+static void mtk_flp_report_loc(MTK_FLP_MSG_T *prmsg) {
+ int loc_num = 0;
+ int i;
+ FlpLocation* ptr = NULL;
+ FlpLocation *locs[MTK_FLP_BATCH_SIZE];
+
+ //TRC();
+ //add locations to ring buffer every time location is reported
+ loc_num = prmsg->length/sizeof(FlpLocation);
+ if(prmsg->type == MTK_FLP_MSG_OFL_REPORT_LOC_NTF) {
+ for(i = 0; i < loc_num; i++) {
+ ptr = (FlpLocation* )((unsigned char*)prmsg + sizeof(MTK_FLP_MSG_T) + i*sizeof(FlpLocation));
+ LOGD("report loc %d/%d", i,loc_num);
+ FlpLocRingAdd(ptr);
+ }
+ if(!fgtimerStarted) {
+ start_timer(batch_loc_timer,FLP_BATCHED_LOCATION_FLUSH_HANDLER_TIMEOUT);
+ fgtimerStarted = 1;
+ LOGD("start timer\n");
+ }
+ } else if (prmsg->type == MTK_FLP_MSG_HSB_REPORT_LOC_NTF) {
+ for(i = 0; i < loc_num; i++) {
+ ptr = (FlpLocation* )((unsigned char*)prmsg + sizeof(MTK_FLP_MSG_T) + i*sizeof(FlpLocation));
+ LOGD("report loc %d/%d", i,loc_num);
+ FlpLocRingAdd(ptr);
+ }
+
+ //LOGD("report loc, flag:%x", gFusedOptions.flags);
+ if(!FlpLocRingIsFull()) {
+ //LOGD("Location Ring not FULL");
+ return;
+ }
+ if(gFusedOptions.flags == MTK_FLP_BATCH_WAKE_ON_FIFO_FULL) { //flush data when fifo full
+ loc_num = FlpLocRingPeekLastN(locs, MTK_FLP_BATCH_SIZE);
+ LOGD("wakeOnFifoFull_TRUE: %d", loc_num);
+ if(mtkFlpCallbacks!=NULL) {
+ mtkFlpCallbacks->location_cb(loc_num, locs);
+ }
+ loc_num = FlpLocRingRemoveN( locs, MTK_FLP_BATCH_SIZE);
+ FlpLocBegin = 0;
+ FlpLocEnd = 0;
+ FlpLocNum = 0;
+ }
+ }
+}
+
+static void mtk_flp_request_loc(int req_num) {
+ int loc_num = 0, i = 0;
+ FlpLocation *locs[MTK_FLP_BATCH_SIZE];
+
+ TRC();
+ if( req_num <0 ) {
+ LOGE("request_loc size error, %d",req_num);
+ return;
+ } else if(req_num>MTK_FLP_BATCH_SIZE) {
+ req_num = MTK_FLP_BATCH_SIZE;
+ }
+ loc_num = FlpLocRingPeekLastN(locs, req_num);
+ mtk_flp_dump_locations(loc_num, locs);
+ for (i = 0; i < loc_num; i++) {
+ if(mtkFlpCallbacks!=NULL) {
+ mtkFlpCallbacks->location_cb(1, &locs[i]);
+ }
+ }
+}
+
+static void mtk_flp_flush_loc() {
+ int loc_num = 0, i = 0;
+ FlpLocation *locs[MTK_FLP_BATCH_SIZE];
+
+ loc_num = FlpLocRingPeekLastN(locs, MTK_FLP_BATCH_SIZE);
+ if(loc_num < 1) {
+ LOGD("no loc out\n");
+ return;
+ } else {
+ LOGD("loc out %d\n",loc_num);
+ }
+
+ if(mtkFlpCallbacks!=NULL) {
+ mtkFlpCallbacks->location_cb(loc_num, locs);
+ }
+ loc_num = FlpLocRingRemoveN(locs, MTK_FLP_BATCH_SIZE);
+ FlpLocBegin = 0;
+ FlpLocEnd = 0;
+ FlpLocNum = 0;
+}
+
+static void mtk_flp_report_status(int status) {
+ //LOGD("report status: %d", status);
+ if(FlpLastStatus != status) {
+ LOGD("status change from: %d to %d", FlpLastStatus, status);
+ if(mtkFlpCallbacks!=NULL) {
+ mtkFlpCallbacks->flp_status_cb(status);
+ }
+ FlpLastStatus = status;
+ }
+}
+
+
+/*********************************************************/
+/* FLP to HAL msg handler */
+/*********************************************************/
+void mtk_mnl2flp_hal_response (MTK_FLP_MSG_T *prmsg) {
+ int *param=NULL;
+
+ if(prmsg == NULL) {
+ LOGE("mtk_mnl2flp_hal_response, recv prmsg is null pointer\r\n");
+ return;
+ }
+ switch (prmsg->type) {
+ case MTK_FLP_MSG_SYS_FLPD_RESET_NTF:
+ flphal2mnl_flp_reboot_done_ntf();
+ // AP/ofl switch, drop batched data
+ FlpLocBegin = 0;
+ FlpLocEnd = 0;
+ FlpLocNum = 0;
+ break;
+ case MTK_FLP_MSG_HAL_REQUEST_LOC_NTF:
+ if(prmsg->length == 0 ) {
+ LOGE("mtk_mnl2flp_hal_response msg error\r\n");
+ return;
+ }
+ param = (int*)((char*)prmsg+sizeof(MTK_FLP_MSG_T));
+ mtk_flp_request_loc( *param);
+ break;
+ case MTK_FLP_MSG_HAL_FLUSH_LOC_NTF:
+ mtk_flp_flush_loc();
+ break;
+ case MTK_FLP_MSG_HAL_REPORT_STATUS_NTF:
+ param = (int*)((char*)prmsg+sizeof(MTK_FLP_MSG_T));
+ mtk_flp_report_status(*param);
+ break;
+ case MTK_FLP_MSG_HSB_REPORT_LOC_NTF:
+ case MTK_FLP_MSG_OFL_REPORT_LOC_NTF:
+ mtk_flp_report_loc(prmsg);
+ break;
+ default:
+ break;
+ }
+}
+
+
+const FlpLocationInterface mtkFlpInterface = {
+ sizeof(FlpLocationInterface),
+ mtk_flp_init,
+ mtk_flp_get_batch_size,
+ mtk_flp_start_batching,
+ mtk_flp_update_batching_options,
+ mtk_flp_stop_batching,
+ mtk_flp_cleanup,
+ mtk_flp_get_batched_location,
+ mtk_flp_inject_location,
+ mtk_flp_get_extension,
+ mtk_flp_flush_batched_locations,
+};
+
+
+//=========================================================
+// Between
+// FLP Interface
+// Hardware Module Interface
+
+static const FlpLocationInterface* mtk_flp_get_flp_interface (
+ __unused struct flp_device_t* device) {
+ TRC();
+ UNUSED(device);
+ return &mtkFlpInterface;
+}
+
+static const struct flp_device_t flp_device = {
+ .common = { // hw_device_t
+ .tag = HARDWARE_DEVICE_TAG,
+ .version = 0,
+ .module = &HAL_MODULE_INFO_SYM,
+ .reserved = {0},
+ .close = NULL
+ },
+ .get_flp_interface = mtk_flp_get_flp_interface
+};
+
+//=========================================================
+// Implementation of
+// Hardware Module Interface
+
+static int open_flp (
+ __unused const struct hw_module_t* module,
+ __unused char const* id,
+ struct hw_device_t** device) {
+ *device = (struct hw_device_t*)&flp_device;
+ return 0;
+}
+
+static struct hw_module_methods_t flp_module_methods = {
+ .open = open_flp
+};
+
+struct hw_module_t HAL_MODULE_INFO_SYM = {
+ .tag = HARDWARE_MODULE_TAG,
+ .version_major = 1,
+ .version_minor = 0,
+ .id = FUSED_LOCATION_HARDWARE_MODULE_ID,
+ .name = "Hardware FLP Module",
+ .author = "The MTK FLP Source Project",
+ .methods = NULL,
+ .dso = NULL,
+ .reserved = {0}
+};
+
+
+
diff --git a/src/connectivity/gps/flp_hal/src/hal2flp_interface.c b/src/connectivity/gps/flp_hal/src/hal2flp_interface.c
new file mode 100644
index 0000000..ac2c243
--- /dev/null
+++ b/src/connectivity/gps/flp_hal/src/hal2flp_interface.c
@@ -0,0 +1,402 @@
+#include <inttypes.h>
+
+#include "flphal_interface.h"
+#include "data_coder.h"
+#include "mtk_auto_log.h"
+
+#ifdef LOG_TAG
+#undef LOG_TAG
+#define LOG_TAG "flphal"
+#endif
+
+/**********************************************************
+ * Define *
+ **********************************************************/
+#define FLP_OPTION_MAX 10
+#define GEOFENCE_INJECT_LOC_ENUM 600
+#define MTK_GFC2HAL "mtk_gfc2hal"
+
+/**********************************************************
+ * Global vars *
+ **********************************************************/
+static FlpBatchOptions gFlpOptions[FLP_OPTION_MAX];
+static FlpBatchOptions gFusedOptions;
+static int gFlpFirstBatch = 1;
+
+/**********************************************************
+ * Function Declaration *
+ **********************************************************/
+static void mtk_flp_dump_options(FlpBatchOptions* options);
+static void mtk_flp_set_options(int id, FlpBatchOptions* options);
+static void mtk_flp_update_options();
+
+/*********************************************************/
+/* FLP Location Interface implementation */
+/*********************************************************/
+void flphal2mnl_flp_init() {
+ memset(gFlpOptions, 0, FLP_OPTION_MAX*sizeof(FlpBatchOptions));
+ return;
+}
+
+int flphal2mnl_flp_start(int id, FlpBatchOptions* options) {
+ int ret, offset = 0;
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+ MTK_FLP_MSG_T *flp_header = NULL;
+
+ if((options == NULL) || (!isflp_thread_exist())) {
+ LOGE("isflp_thread_exist:%d",isflp_thread_exist());
+ return MTK_FLP_ERROR;
+ }
+
+ mtk_flp_set_options(id, options);
+ mtk_flp_update_options();
+ if(options->sources_to_use == 0) {
+ LOGD("mtk flp start zero sources");
+ return MTK_FLP_SUCCESS;
+ }
+ if(gFlpFirstBatch == 1) {
+ LOGD("first instance, add id = %d", id);
+ flp_header = malloc(sizeof(MTK_FLP_MSG_T) + sizeof(FlpBatchOptions));
+ if(flp_header== NULL) {
+ LOGE("flp start malloc failed");
+ return MTK_FLP_ERROR;
+ }
+ flp_header->type = MTK_FLP_MSG_HAL_START_CMD;
+ flp_header->length = sizeof(FlpBatchOptions);
+ memcpy((char *)flp_header +sizeof(MTK_FLP_MSG_T),&gFusedOptions, sizeof(FlpBatchOptions));
+ put_binary(buff, &offset, (const char*)flp_header, (flp_header->length+sizeof(MTK_FLP_MSG_T)));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_header);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ return MTK_FLP_ERROR;
+ }
+ gFlpFirstBatch = 0;
+ } else {
+ LOGD("update instance, add id = %d", id);
+ flp_header = malloc(sizeof(MTK_FLP_MSG_T) + sizeof(FlpBatchOptions));
+ if(flp_header== NULL) {
+ LOGE("flp start malloc failed");
+ return MTK_FLP_ERROR;
+ }
+ flp_header->type = MTK_FLP_MSG_HAL_SET_OPTION_CMD;
+ flp_header->length = sizeof(FlpBatchOptions);
+ memcpy((char *)flp_header +sizeof(MTK_FLP_MSG_T),&gFusedOptions, sizeof(FlpBatchOptions));
+ put_binary(buff, &offset, (const char*)flp_header, (flp_header->length+sizeof(MTK_FLP_MSG_T)));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_header);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ return MTK_FLP_ERROR;
+ }
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+int flphal2mnl_flp_reboot_done_ntf() {
+ int ret, offset = 0;
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+ MTK_FLP_MSG_T *flp_header = NULL;
+
+ if(!gFlpFirstBatch) {
+ LOGD("FLP HAL RECOVER: send start cmd");
+ flp_header = malloc(sizeof(MTK_FLP_MSG_T)+sizeof(FlpBatchOptions));
+ if(flp_header== NULL) {
+ LOGE("flp reboot malloc failed");
+ return MTK_FLP_ERROR;
+ }
+ flp_header->type = MTK_FLP_MSG_HAL_START_CMD;
+ flp_header->length = sizeof(FlpBatchOptions);
+ memcpy((char *)flp_header +sizeof(MTK_FLP_MSG_T),&gFusedOptions, sizeof(FlpBatchOptions));
+ put_binary(buff, &offset, (const char*)flp_header, (flp_header->length+sizeof(MTK_FLP_MSG_T)));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_header);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ return MTK_FLP_ERROR;
+ }
+ } else {
+ LOGD("FLP HAL RECOVER: No need send start flp cmd");
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+int flphal2mnl_update_batching_options(int id, FlpBatchOptions* options) {
+ int ret, offset = 0;
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+ MTK_FLP_MSG_T *flp_header = NULL;
+
+ if((options == NULL)||(!isflp_thread_exist())) {
+ LOGE("isflp_thread_exist:%d",isflp_thread_exist());
+ return MTK_FLP_ERROR;
+ }
+ mtk_flp_set_options(id, options);
+ mtk_flp_update_options();
+
+ //LOGD("update instance, update id = %d", id);
+ flp_header = malloc(sizeof(MTK_FLP_MSG_T)+sizeof(FlpBatchOptions));
+ if(flp_header== NULL) {
+ LOGE("flp update malloc failed");
+ return MTK_FLP_ERROR;
+ }
+ flp_header->type = MTK_FLP_MSG_HAL_SET_OPTION_CMD;
+ flp_header->length = sizeof(FlpBatchOptions);
+ memcpy((char *)flp_header +sizeof(MTK_FLP_MSG_T),&gFusedOptions, sizeof(FlpBatchOptions));
+ put_binary(buff, &offset, (const char*)flp_header, (flp_header->length+sizeof(MTK_FLP_MSG_T)));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_header);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ return MTK_FLP_ERROR;
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+int flphal2mnl_stop_batching(int id) {
+ int ret, offset = 0;
+ unsigned int msg_size=0;
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+ MTK_FLP_MSG_T *flp_header = NULL;
+
+ if( id >= FLP_OPTION_MAX || id <0 || (!isflp_thread_exist())) {
+ LOGD("Error batching id:%d, %d", id,isflp_thread_exist());
+ return MTK_FLP_SUCCESS;
+ }
+ memset(&gFlpOptions[id], 0, sizeof(FlpBatchOptions));
+ mtk_flp_update_options();
+
+ if(gFusedOptions.sources_to_use == 0) {
+ LOGD("stop all instance, last id = %d", id);
+ flp_header = malloc(sizeof(MTK_FLP_MSG_T));
+ if(flp_header== NULL) {
+ LOGE("flp stop malloc failed");
+ return MTK_FLP_ERROR;
+ }
+ flp_header->type = MTK_FLP_MSG_HAL_STOP_CMD;
+ flp_header->length = 0;
+ put_binary(buff, &offset, (const char*)flp_header, sizeof(MTK_FLP_MSG_T));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_header);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ return MTK_FLP_ERROR;
+ }
+ gFlpFirstBatch = 1;
+ } else {
+ LOGD("update instance, stop id = %d", id);
+ flp_header = malloc(sizeof(MTK_FLP_MSG_T)+sizeof(FlpBatchOptions));
+ if(flp_header== NULL) {
+ LOGE("flp stop malloc failed");
+ return MTK_FLP_ERROR;
+ }
+ flp_header->type = MTK_FLP_MSG_HAL_SET_OPTION_CMD;
+ flp_header->length = sizeof(FlpBatchOptions);
+ memcpy((char *)flp_header +sizeof(MTK_FLP_MSG_T),&gFusedOptions, sizeof(FlpBatchOptions));
+ put_binary(buff, &offset, (const char*)flp_header, (flp_header->length+sizeof(MTK_FLP_MSG_T)));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_header);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ return MTK_FLP_ERROR;
+ }
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+void flphal2mnl_get_batched_location(int last_n_locations) {
+ int ret, offset = 0;
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+ MTK_FLP_MSG_T *flp_header = NULL;
+
+ flp_header = malloc(sizeof(MTK_FLP_MSG_T)+sizeof(int));
+ if(flp_header== NULL) {
+ LOGE("flp get batch malloc failed");
+ return;
+ }
+
+ flp_header->type = MTK_FLP_MSG_HAL_REQUEST_LOC_NTF;
+ flp_header->length = sizeof(int);
+ memcpy((char *)flp_header +sizeof(MTK_FLP_MSG_T),&last_n_locations, sizeof(int));
+ put_binary(buff, &offset, (const char*)flp_header, (flp_header->length+sizeof(MTK_FLP_MSG_T)));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_header);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ }
+}
+
+int flphal2mnl_inject_location(FlpLocation* location) {
+ int ret, offset = 0;
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+ MTK_FLP_MSG_T *flp_header = NULL;
+
+ if(location == NULL) {
+ return MTK_FLP_ERROR;
+ }
+
+ flp_header = malloc(sizeof(MTK_FLP_MSG_T)+sizeof(FlpLocation));
+ if(flp_header== NULL) {
+ LOGE("flp inject loc malloc failed");
+ return MTK_FLP_ERROR;
+ }
+
+ flp_header->type = MTK_FLP_MSG_HAL_INJECT_LOC_CMD;
+ flp_header->length = sizeof(FlpLocation);
+ memcpy((char *)flp_header +sizeof(MTK_FLP_MSG_T),location, sizeof(FlpLocation));
+ put_binary(buff, &offset, (const char*)flp_header, (flp_header->length+sizeof(MTK_FLP_MSG_T)));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_header);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ return MTK_FLP_ERROR;
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+/*******************************************
+ unicode to ASCII conversion
+********************************************/
+int Unicode2Ascii(char* unicode, char * ascii, int len) {
+ int i;
+ for(i = 0; i < len; i++) {
+ ascii[i] = unicode[i*2];
+ }
+ return len;
+}
+
+
+/*********************************************************/
+/* FLP Location Diagnostic Interface implementation */
+/*********************************************************/
+int flphal2mnl_diag_inject_data(char* data, int length) {
+ unsigned int msg_size=0;
+ int i, ret, offset = 0;
+ char asciibuf[1024];
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+
+ //geofence info injection
+ float injectGeoInfo[4] = {0};
+ char* injectGeoField;
+ int injectGeoIndex = 0;
+ MTK_FLP_MSG_T *flp_msg = NULL;
+
+ if(data == NULL || length <= 0) {
+ return MTK_FLP_ERROR;
+ }
+ Unicode2Ascii(data, asciibuf, length);
+ asciibuf[length] = '\0';
+ //LOGD("DIAG_IN:%s\n", asciibuf);
+
+ if (strncmp(asciibuf, "RESET_FLP_DATA", 14) == 0) {
+ // stop FLP
+ if (!gFlpFirstBatch) {
+ //LOGD("Reset FLP\n");
+ for (i = 0; i < FLP_OPTION_MAX; i++) {
+ memset(&gFlpOptions[i], 0, sizeof(FlpBatchOptions));
+ }
+ memset(&gFusedOptions, 0, sizeof(FlpBatchOptions));
+
+ flp_msg = malloc(sizeof(MTK_FLP_MSG_T));
+ if(flp_msg == NULL) {
+ LOGE("diag inject alloc failed");
+ return MTK_FLP_ERROR;
+ }
+ flp_msg->type = MTK_FLP_MSG_HAL_STOP_CMD;
+ flp_msg->length = 0;
+ put_binary(buff, &offset, (const char*)flp_msg, sizeof(MTK_FLP_MSG_T));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_msg);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ return MTK_FLP_ERROR;
+ }
+ gFlpFirstBatch = 1;
+ }
+ } else if (strncmp(asciibuf, "GEO_COR", 7) == 0) {
+ injectGeoField = strtok(asciibuf, ",");
+ if (injectGeoField) {
+ injectGeoField = strtok(NULL, ",");
+ }
+ while (injectGeoField && injectGeoIndex < 4) {
+ injectGeoInfo[injectGeoIndex] = atof(injectGeoField);
+ injectGeoIndex++;
+ injectGeoField = strtok(NULL, ",");
+ }
+ flp_msg = malloc(sizeof(MTK_FLP_MSG_T)+3*sizeof(float));
+ if(flp_msg == NULL) {
+ LOGE("GEO_COR alloc failed");
+ return MTK_FLP_ERROR;
+ }
+ flp_msg->type = GEOFENCE_INJECT_LOC_ENUM;
+ flp_msg->length = 3*sizeof(float);
+ memcpy((char *)flp_msg+sizeof(MTK_FLP_MSG_T),&injectGeoInfo[0],3*sizeof(float));
+ put_binary(buff, &offset, (const char*)flp_msg, (flp_msg->length+sizeof(MTK_FLP_MSG_T)));
+ ret = flp_send2mnl(buff, offset);
+ free(flp_msg);
+ if(ret < 0) {
+ LOGE("GEOFENCE_INJECT_LOC_ENUM MTK_GFC2HAL send error return error");
+ return MTK_FLP_ERROR;
+ }
+ }
+ return MTK_FLP_SUCCESS;
+}
+
+void flphal2mnl_flush_batched_locations() {
+ int ret, offset = 0;
+ char buff[HAL_FLP_BUFF_SIZE] = {0};
+ MTK_FLP_MSG_T flp_header;
+
+ flp_header.type = MTK_FLP_MSG_HAL_FLUSH_LOC_NTF;
+ flp_header.length = 0;
+ put_binary(buff, &offset, (const char*)&flp_header, sizeof(MTK_FLP_MSG_T));
+ ret = flp_send2mnl(buff, offset);
+ if(ret < 0) {
+ LOGE("MTK_HAL2FLP send error return error");
+ }
+}
+
+static void mtk_flp_dump_options(FlpBatchOptions* options) {
+ LOGD("BatchOptions:flg %d,max_pow: %lf, period %" PRId64 ", source %d\n",
+ options->flags, options->max_power_allocation_mW, options->period_ns,
+ options->sources_to_use);
+}
+
+static void mtk_flp_set_options(int id, FlpBatchOptions* options) {
+ if( options == NULL) {
+ LOGE("WRONG USAGE of mtk_flp_set_options");
+ return;
+ }
+ //LOGD("mtk_flp_set_options id = %d",id);
+ memcpy( &gFlpOptions[id], options, sizeof(FlpBatchOptions));
+ mtk_flp_dump_options((FlpBatchOptions*)&gFlpOptions[id]);
+}
+
+static void mtk_flp_update_options() {
+ int i;
+
+ memset(&gFusedOptions, 0, sizeof(FlpBatchOptions));
+ gFusedOptions.period_ns = 1000000000000;
+
+ // update gFusedOptions
+ for(i = 0; i < FLP_OPTION_MAX; i++) {
+ if(gFlpOptions[i].sources_to_use == 0) {
+ continue;
+ }
+ gFusedOptions.sources_to_use |= gFlpOptions[i].sources_to_use;
+ if(gFusedOptions.period_ns > gFlpOptions[i].period_ns) {
+ gFusedOptions.period_ns = gFlpOptions[i].period_ns;
+ }
+ if(gFusedOptions.flags < gFlpOptions[i].flags) {
+ gFusedOptions.flags = gFlpOptions[i].flags;
+ }
+ gFusedOptions.max_power_allocation_mW = gFlpOptions[i].max_power_allocation_mW;
+ }
+ if(gFusedOptions.period_ns == 1000000000000) {
+ gFusedOptions.period_ns = 0;
+ }
+ //LOGD("mtk_flp_update_options");
+ mtk_flp_dump_options(&gFusedOptions);
+}
+
+
+