src/connectivity/gps/2.0/geofenceinf_example/geofenceinf.c

#include <string.h> //strstr
#include <dirent.h>
#include <sys/stat.h> //mkdir
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <stddef.h>  // offsetof
#include <stdarg.h>
#include <unistd.h>  // usleep
#include <sys/socket.h>
#include <fcntl.h>
#include <arpa/inet.h>  // inet_addr
#include <sys/un.h>  // struct sockaddr_un
#include <pthread.h>
#include <sys/epoll.h>
#include <signal.h>
#include <semaphore.h>
#include <signal.h> //struct sigevent
#include <string.h> //memset, strncpy
#include <netdb.h> //struct addrinfo
#include <sys/types.h> //gettid
#include <netinet/in.h> //struct sockaddr_in
#include <netinet/tcp.h> //TCP_KEEPIDLE, TCP_KEEPINTVL, TCP_KEEPCNT
#include <netinet/ip.h> //IPTOS_LOWDELAY
#include <sys/epoll.h> //epoll_create
#include <semaphore.h> //sem_t
#include <inttypes.h> //PRId64
#include <stdbool.h>
#include "geofence.h"
//------------------MTK LBS Utility---------------------------------------------------
#ifndef LOGD
#define LOGD(...)   { printf(__VA_ARGS__); printf("\n"); fflush(stdout); }
#define LOGW(...)   { printf("\E[1;35;40m"); printf(__VA_ARGS__); printf("\E[0m"); printf("\n"); fflush(stdout); }
#define LOGE(...)   { printf("\E[1;31;40m"); printf(__VA_ARGS__); printf("\E[0m"); printf("\n"); fflush(stdout); }
#define LOGI(...)   { printf("\E[1;35;40m"); printf(__VA_ARGS__); printf("\E[0m"); printf("\n"); fflush(stdout); }
#endif

#define GEOFENCEADP_MNL_TCP_DATA      "mtk_geofenceadp_mnl_data"
#define GEOFENCEADP_MNL_TCP_CONTROL   "mtk_geofenceadp_mnl_control"

//#define AT_COMMAND_PRINT

#define MTK_ADD_GEOFENCE_SUCCESS   0
#define MTK_ADD_GEOFENCE_ERROR    -1
#define MTK_ADD_GEOFENCE_INSUFFICIENT_MEM -2
#define MTK_ADD_GEOFENCE_TOO_MANY -3

#define MNLD_STRNCPY(dst,src,size) do{\
                                       strncpy((char *)(dst), (src), (size - 1));\
                                      (dst)[size - 1] = '\0';\
                                     }while(0)

#define GEOFENCE_BUFF_SIZE   (1 * 1024)
#define GEOFENCE_DATAPATH_SIZE   (10*1024)

typedef struct {
    int type; //MTK_GFC_COMMAND_T
    unsigned int length;
} mtk_geofence_msg;

typedef struct {
    bool geofence_support;
    int server_data_fd;
}mtk_geofence_server_init_msg;

typedef enum {
    GEOFENCE_SERVER_CAP,//geofence server capability
    EXCUTE_RESULT,
    GEOFENCE_NUM,
    GEOFENCE_RESPONSE_INFO,
    GEOFENCE_ALERT_INFO,
    GNSS_TRACKING_STATUS,
} mtk_geofence_ret_command;

typedef struct mtk_geofence_create_status {
    int createstat;//  success : 0, error : -1, insufficient_memory : -2, too many fences  : -3
    int id;
}mtk_geofence_create_status;

typedef struct {
    int cmdtype;//mtk_geofence_command
    int result;
} mtk_geofence_result;

typedef struct cyclical_buffer{
    char *next_write;
    char *next_read;
    char *start_buffer;
    char *end_buffer;
    int buffer_size;
} cyclical_buffer_t;

static char geofence_raw_data[GEOFENCE_DATAPATH_SIZE] = {0};
static cyclical_buffer_t g_cyclical_buffer;    // io - cyclic buffer
int server_data_fd = -1;

//-1 means fail or serverfd is returned
static int geofenceinf_socket_tcp_client_connect_local(bool abstract, const char* name) {
    int fd;
    int size;
    struct sockaddr_un addr;

    memset(&addr, 0, sizeof(addr));
    addr.sun_family = AF_UNIX;
    size = strlen(name) + offsetof(struct sockaddr_un, sun_path) + 1;
    if(abstract) {
        addr.sun_path[0] = 0;
        memcpy(addr.sun_path + 1, name, strlen(name));
    } else {
        strncpy(addr.sun_path, name, sizeof(addr.sun_path));
        if(unlink(addr.sun_path) == -1) {
            LOGW("geofenceinf_socket_tcp_client_connect_local() unlink() failed, reason=[%s]%d",
                strerror(errno), errno);
        }
    }
    fd = socket(AF_UNIX, SOCK_STREAM, 0);
    if(fd == -1) {
        LOGE("geofenceinf_socket_tcp_client_connect_local() socket() failed, reason=[%s]%d",
            strerror(errno), errno);
        return -1;
    }
    if(connect(fd, (struct sockaddr*)&addr, size) == -1) {
        LOGE("geofenceinf_socket_tcp_client_connect_local() connect() failed, abstruct=%d name=[%s] reason=[%s]%d",
            abstract, name, strerror(errno), errno);
        close(fd);
        return -1;
    }

    return fd;
}

void geofenceinf_buffer_initialize
                ( cyclical_buffer_t *buffer,           // buffer to initialize
                  unsigned int buffer_size )     // size of buffer to create
{
   // Set up buffer manipulation pointers
   // end_buffer points to the next byte after the buffer
   buffer->end_buffer   = buffer->start_buffer + buffer_size;
   buffer->next_read    = buffer->start_buffer;
   buffer->next_write   = buffer->start_buffer;
   buffer->buffer_size = buffer_size;
   return;
}

int geofenceinf_check_fence_vadility(mtk_geofence_property *fence) {
    if ((fence->latest_state != GEOFENCE_ENTERED)
     && (fence->latest_state != GEOFENCE_EXITED)
     && (fence->latest_state != GEOFENCE_UNKNOWN)){
        LOGE("geofenceinf_check_fence_vadility latest_state:%d fail", fence->latest_state);
        return MTK_GFC_ERROR;
    }

    if (!(fence->monitor_transition & MTK_GEOFENCE_ENTER) && !(fence->monitor_transition & MTK_GEOFENCE_EXIT)){
        LOGE("geofenceinf_check_fence_vadility monitor_transition:%d fail", fence->monitor_transition);
        return MTK_GFC_ERROR;
    }

    return MTK_GFC_SUCCESS;
}

int geofenceinf_socket_set_blocking(int fd, int blocking) {
    if (fd < 0) {
        LOGE("geofenceinf_socket_set_blocking() invalid fd=%d", fd);
        return -1;
    }

    int flags = fcntl(fd, F_GETFL, 0);
    if (flags == -1) {
        LOGE("geofenceinf_socket_set_blocking() fcntl() failed invalid flags=%d reason=[%s]%d",
            flags, strerror(errno), errno);
        return -1;
    }

    flags = blocking ? (flags&~O_NONBLOCK) : (flags|O_NONBLOCK);
    return (fcntl(fd, F_SETFL, flags) == 0) ? 0 : -1;
}

int geofenceinf_epoll_add_fd2(int epfd, int fd, uint32_t events) {
    struct epoll_event ev;
    memset(&ev, 0, sizeof(ev));
    ev.data.fd = fd;
    ev.events = events;
    // don't set the fd to edge trigger
    // the some event like accept may be lost if two or more clients are connecting to server at the same time
    // level trigger is preferred to avoid event lost
    // do not set EPOLLOUT due to it will always trigger when write is available
    if (epoll_ctl(epfd, EPOLL_CTL_ADD, fd, &ev) == -1) {
        LOGE("epoll_add_fd2() epoll_ctl() failed reason=[%s]%d epfd=%d fd=%d",
            strerror(errno), errno, epfd, fd);
        return -1;
    }
    return 0;
}

int geofenceinf_socket_bind_udp(const char* path) {
    int fd;
    struct sockaddr_un addr;

    fd = socket(PF_LOCAL, SOCK_DGRAM, 0);
    if (fd < 0) {
        LOGE("socket_bind_udp() socket() failed reason=[%s]%d",
            strerror(errno), errno);
        return -1;
    }
    LOGD("fd=%d,path=%s", fd, path);

    memset(&addr, 0, sizeof(addr));
    addr.sun_path[0] = 0;
    MNLD_STRNCPY(addr.sun_path + 1, path,sizeof(addr.sun_path) - 1);
    addr.sun_family = AF_UNIX;
    unlink(addr.sun_path);
    if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        LOGE("socket_bind_udp() bind() failed path=[%s] reason=[%s]%d",
            path, strerror(errno), errno);
        close(fd);
        return -1;
    }
    return fd;
}


// -1 means failure
int geofenceinf_safe_sendto(const char* path, const char* buff, int len) {
    int ret = 0;
    struct sockaddr_un addr;
    int retry = 10;
    int fd = socket(PF_LOCAL, SOCK_DGRAM, 0);
    if (fd < 0) {
        LOGE("safe_sendto() socket() failed reason=[%s]%d",
            strerror(errno), errno);
        return -1;
    }

    int flags = fcntl(fd, F_GETFL, 0);
    if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) == -1){
        LOGE("fcntl failed reason=[%s]%d",
                    strerror(errno), errno);

        close(fd);
        return -1;
    }

    memset(&addr, 0, sizeof(addr));
    addr.sun_path[0] = 0;
    MNLD_STRNCPY(addr.sun_path + 1, path,sizeof(addr.sun_path) - 1);
    addr.sun_family = AF_UNIX;

    while ((ret = sendto(fd, buff, len, 0,
        (const struct sockaddr *)&addr, sizeof(addr))) == -1) {
        if (errno == EINTR) continue;
        if (errno == EAGAIN) {
            if (retry-- > 0) {
                usleep(100 * 1000);
                continue;
            }
        }
        LOGE("safe_sendto() sendto() failed path=[%s] ret=%d reason=[%s]%d",
            path, ret, strerror(errno), errno);
        break;
    }

    close(fd);
    return ret;
}
void geofenceinf_client_init(void)
{
    g_cyclical_buffer.start_buffer = &geofence_raw_data[0];
    geofenceinf_buffer_initialize(&g_cyclical_buffer, GEOFENCE_DATAPATH_SIZE);
}

int geofenceinf_get_server_init_msg(int fd, mtk_geofence_callback *callback)
{
    int ret;
    int recv_len = 0;
    unsigned int read_count = 0, msg_len = 0;
    char buffer[GEOFENCE_BUFF_SIZE] = {0};
    char data[GEOFENCE_BUFF_SIZE] = {0};
    mtk_geofence_msg *prmsg = NULL;
    mtk_geofence_server_init_msg server_init_msg;
    mtk_geofence_server_capability cap;

    memset(&server_init_msg, 0, sizeof(mtk_geofence_server_init_msg));
    //receive geofence server ack capability & server data fd
    do {
        ret = read(fd, buffer, GEOFENCE_BUFF_SIZE);
        if(ret == -1) {
            LOGW("geofenceinf_get_server_init_msg() read() fd=[%d] failed, reason=[%s]%d", fd, strerror(errno), errno);
            return ret;
        } else if(ret == 0) {
            LOGW("geofenceinf_get_server_init_msg() read() fd=[%d] find the remote side has closed the session", fd);
            return ret;
        } else {
            memcpy((char *)(data + recv_len), buffer, ret);
            recv_len += ret;
            read_count++;
            if ((recv_len >= sizeof(mtk_geofence_msg)) && (msg_len == 0)){
                prmsg = (mtk_geofence_msg *)&buffer[0];
                msg_len = prmsg->length;
            }
            if (read_count >= 10){
                LOGW("geofenceinf_get_server_init_msg() read() fd:%d too much counts:%d", fd, read_count);
                return MTK_GFC_ERROR;
            }
        }
        memset(buffer, 0, GEOFENCE_BUFF_SIZE);
    } while((recv_len < msg_len) || (msg_len == 0));

    msg_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_server_init_msg);
    prmsg = (mtk_geofence_msg *)&data[0];
    if (prmsg->length == msg_len){
        memcpy(&server_init_msg, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_server_init_msg));
    } else if (prmsg->length > msg_len) {
        memcpy(&server_init_msg, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_server_init_msg));
        LOGD("mnl msg len:%d > struct len:%d", prmsg->length, msg_len);
    } else {
        memset(buffer, 0, GEOFENCE_BUFF_SIZE);
        memcpy(buffer, (char*)prmsg, prmsg->length);
        memset((char *)(buffer + prmsg->length), 0, (msg_len - prmsg->length));
        memcpy(&server_init_msg, (((char*)buffer)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_server_init_msg));
        LOGD("mnl msg len:%d < struct len:%d", prmsg->length, msg_len);
    }

    if (server_init_msg.server_data_fd >= 0){
        server_data_fd = server_init_msg.server_data_fd;
        LOGD("geofenceinf_get_server_init_msg:server data fd:%d", server_data_fd);
    }

    if (callback->geofence_capability_update != NULL){
        memset(&cap, 0, sizeof(mtk_geofence_server_capability));
        cap.geofence_support = server_init_msg.geofence_support;
        callback->geofence_capability_update(&cap);
    }
    return MTK_GFC_SUCCESS;
}

int geofenceinf_client_register(const char* name, mtk_geofence_callback *callback)
{
    int fd;

    if ( (fd = geofenceinf_socket_tcp_client_connect_local(true, GEOFENCEADP_MNL_TCP_DATA)) < 0){
        LOGE("Geofence connect server failed:%d", fd);
    } else {
        LOGD("Geofence new client [%s], fd:%d", name, fd);
        geofenceinf_client_init();
        if (geofenceinf_get_server_init_msg(fd, callback) < 0){
            close(fd);
            return MTK_GFC_ERROR;
        }
        geofenceinf_socket_set_blocking(fd, 0);
    }

    return fd;
}

int geofenceinf_send2mnl(int fd, const char* buff, int len) {
    int ret = write(fd, buff, len);
    LOGD("geofence_send2mnl() write():length:%d", len);
    if(ret == -1) {
        LOGE("geofence_send2mnl() write() failed, reason=[%s]%d", strerror(errno), errno);
    }
    return ret;
}

int geofenceinf_safe_recv(int fd, char* buff, int len) {
    int ret = 0;
    int retry = 10;

    while ((ret = read(fd, buff, len)) == -1) {
        LOGW("geofenceinf_safe_recv() ret=%d len=%d", ret, len);
        if (errno == EINTR) continue;
        if (errno == EAGAIN) {
            if (retry-- > 0) {
                usleep(100 * 1000);
                continue;
            }
        }
        LOGE("geofenceinf_safe_recv() recvfrom() failed reason=[%s]%d",
            strerror(errno), errno);
        break;
    }
    return ret;
}

int geofenceinf_control_path_safe_recv(int fd, char* buff, int len, int expect_len) {
    int ret = 0, ret1 = 0;
    int retry = 10;
    char data[GEOFENCE_BUFF_SIZE] = {0};

    ret = read(fd, buff, len);
    if(ret == -1) {
        LOGW("geofenceinf_control_path_safe_recv() read() fd=[%d] failed, reason=[%s]%d",
            fd, strerror(errno), errno);
        return ret;
    } else if(ret == 0) {
        LOGW("geofenceinf_control_path_safe_recv() read() fd=[%d] find the remote side has closed the session", fd);
        return ret;
    }

    if (ret < expect_len){
        //set fd non-blocking, try it again.
        geofenceinf_socket_set_blocking(fd, 0);
        while ((ret1 = read(fd, data, GEOFENCE_BUFF_SIZE)) == -1) {
            LOGW("geofenceinf_control_path_safe_recv() ret=%d", ret1);
            if (errno == EINTR) continue;
            if (errno == EAGAIN) {
                if (retry-- > 0) {
                    usleep(100 * 1000);
                    continue;
                }
            }
            LOGE("geofenceinf_safe_recv() recvfrom() failed reason=[%s]%d",
                strerror(errno), errno);
            break;
        }
        if ((ret1 > 0) && ((ret + ret1) < len)){
            memcpy((char *)(buff + ret), data, ret1);
            ret = ret + ret1;
        }
    }

    return ret;
}

int geofenceinf_geofence_add_result(mtk_geofence_msg *prmsg, mtk_geofence_create_status *fence_ret) {
    unsigned int msg_len;
    #ifdef AT_COMMAND_PRINT
    char data[GEOFENCE_BUFF_SIZE] = {0};
    #endif

    msg_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_create_status);
    if (prmsg->length == msg_len){
        memcpy(fence_ret, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_create_status));
    } else {
        //LOGE("geofenceinf_geofence_add_result:message len error %d %d",prmsg->length, msg_len);
        return MTK_GFC_ERROR;
    }

    #ifdef AT_COMMAND_PRINT
    /*Construct AT command */
    if (fence_ret->createstat == MTK_ADD_GEOFENCE_SUCCESS){
        sprintf(data, "+EGEOADDCIRCLE:%d,%d/r/n", fence_ret->createstat, fence_ret->id);
        //Send AT command

    } else {
        sprintf(data, "+EGEOADDCIRCLE:%d/r/n", fence_ret->createstat);
        //Send AT command
    }
    LOGD("AT command:%s",data);
    #endif

    return MTK_GFC_SUCCESS;
}

int geofenceinf_geofence_remove_result(mtk_geofence_msg *prmsg, mtk_geofence_result *remove_result) {
    unsigned int msg_len;
    #ifdef AT_COMMAND_PRINT
    char data[GEOFENCE_BUFF_SIZE] = {0};
    #endif

    msg_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_result);
    if (prmsg->length == msg_len){
        memcpy(remove_result, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_result));
    } else {
        //LOGE("geofenceinf_get_remove_result:message len error %d %d",prmsg->length, msg_len);
        return MTK_GFC_ERROR;
    }

    #ifdef AT_COMMAND_PRINT
    /*Construct AT command */
    if (remove_result->result == 0){
        sprintf(data, "OK/r/n");
    } else {
        sprintf(data, "FAIL/r/n");
    }
    LOGD("AT command:%s",data);
    #endif

    return MTK_GFC_SUCCESS;
}

int geofenceinf_geofence_clear_result(mtk_geofence_msg *prmsg, mtk_geofence_result *clear_result) {
    unsigned int msg_len;
    #ifdef AT_COMMAND_PRINT
    char data[GEOFENCE_BUFF_SIZE] = {0};
    #endif

    msg_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_result);
    if (prmsg->length == msg_len){
        memcpy(clear_result, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_result));
    } else {
        //LOGE("geofenceinf_geofence_clear_result:message len error %d %d",prmsg->length, msg_len);
        return MTK_GFC_ERROR;
    }

    #ifdef AT_COMMAND_PRINT
    /*Construct AT command */
    if (clear_result->result == 0){
        sprintf(data, "OK/r/n");
    } else {
        sprintf(data, "FAIL/r/n");
    }
    LOGD("AT command:%s",data);
    #endif

    return MTK_GFC_SUCCESS;
}

int geofenceinf_geofence_get_geofences_num(mtk_geofence_msg *prmsg, int *fence_nums) {
    unsigned int msg_len;
    #ifdef AT_COMMAND_PRINT
    char data[GEOFENCE_BUFF_SIZE] = {0};
    #endif

    msg_len = sizeof(mtk_geofence_msg) + sizeof(int);
    if (prmsg->length == msg_len){
        memcpy(fence_nums, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(int));
    } else {
        //LOGE("geofenceinf_geofence_get_geofences_numgeofenceinf_geofence_get_geofences_numgeofenceinf_geofence_get_geofences_num:message len error %d %d",prmsg->length, msg_len);
        return MTK_GFC_ERROR;
    }

    #ifdef AT_COMMAND_PRINT
    /*Construct AT command */
    sprintf(data, "+EGEOMAX: :%d/r/n", *fence_nums);
    LOGD("AT command:%s",data);
    #endif

    return MTK_GFC_SUCCESS;
}

// GNSS Adaptor --> MNLD Message
int geofenceinf_add_geofence(mtk_geofence_property *fence) {
    int ret;
    mtk_geofence_msg *geo_header = NULL;
    unsigned int msg_len;
    int recv_len, expect_len;
    char buffer[GEOFENCE_BUFF_SIZE] = {0};
    char geo_data[GEOFENCE_BUFF_SIZE] = {0};
    mtk_geofence_create_status fence_status;
    int fd = -1, temp_server_data_fd = -1;
    mtk_geofence_msg *prmsg = NULL;

    LOGD("geofence_add_geofences");
    if (server_data_fd < 0){
        LOGE("geofenceinf_add_geofence:wait server data fd");
        return -1;
    } else {
        temp_server_data_fd = server_data_fd;
    }
    /* For geofence recover mechanism */
    ret = geofenceinf_check_fence_vadility(fence);
    if(ret < 0) {
        LOGE("geofenceinf_check_fence_vadility fail");
        return -1;
    }

    //construct add fence cmd
    msg_len = sizeof(mtk_geofence_msg) + sizeof(int) + sizeof(mtk_geofence_property);
    geo_header = (mtk_geofence_msg *)&geo_data[0];
    if (msg_len > GEOFENCE_BUFF_SIZE){
        LOGE("geofenceinf message length too long:%d", msg_len);
        return MTK_GFC_ERROR;
    }
    //construct total message
    //construct header
    geo_header->type = ADD_GEOFENCE_AREA;
    geo_header->length = msg_len;
    //input server data fd
    memcpy( ((char*)geo_header) + sizeof(mtk_geofence_msg), &temp_server_data_fd, sizeof(int));
    //input geofence property payload
    memcpy( ((char*)geo_header) + sizeof(mtk_geofence_msg) + sizeof(int), fence, sizeof(mtk_geofence_property));

    //Create TCP client fd, and connect tcp server
    if ( (fd = geofenceinf_socket_tcp_client_connect_local(true, GEOFENCEADP_MNL_TCP_CONTROL)) < 0){
        LOGE("geofenceinf_add_geofence:create client fd failed:%d", fd);
        return -1;
    }

    ret = geofenceinf_send2mnl(fd, (char *)geo_header, msg_len);
    if(ret < 0) {
        LOGE("geofenceinf_add_geofence:send message to mnl failed");
        close(fd);
        return -1;
    }

    //wait ack
    expect_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_create_status);
    if ( (recv_len = geofenceinf_control_path_safe_recv(fd, buffer, GEOFENCE_BUFF_SIZE, expect_len)) <= 0){
        LOGE("geofenceinf_add_geofence:don't recv any data");
        close(fd);
        return -1;
    } else {
        prmsg = (mtk_geofence_msg *)&buffer[0];
        ret = geofenceinf_geofence_add_result(prmsg, &fence_status);
        if (ret == MTK_GFC_ERROR){
            LOGE("geofenceinf_add_geofence:message data don't match struct %d %d", recv_len, expect_len);
            close(fd);
            return -1;
        }
    }

    if (fence_status.createstat == 0){
        close(fd);
        return fence_status.id;
    } else {
        close(fd);
        return fence_status.createstat;
    }
}

int geofenceinf_remove_geofence(const int geofence_id) {
    int ret;
    mtk_geofence_msg *geo_header=NULL;
    unsigned int msg_len;
    int recv_len;
    int expect_len;
    char buffer[GEOFENCE_BUFF_SIZE] = {0};
    char geo_data[GEOFENCE_BUFF_SIZE] = {0};
    int fd = -1, temp_server_data_fd = -1;
    mtk_geofence_msg *prmsg = NULL;
    mtk_geofence_result remove_result;

    LOGD("geofence_remove_geofences,fence id:%d", geofence_id);
    if (server_data_fd < 0){
        LOGE("geofenceinf_remove_geofence:wait server data fd");
        return MTK_GFC_ERROR;
    } else {
        temp_server_data_fd = server_data_fd;
    }

    //construct remove fence cmd
    msg_len = sizeof(mtk_geofence_msg) + sizeof(int) + sizeof(int);
    geo_header = (mtk_geofence_msg *)&geo_data[0];
    if (msg_len > GEOFENCE_BUFF_SIZE){
        LOGE("geofenceinf message length too long:%d", msg_len);
        return MTK_GFC_ERROR;
    }

    //construct total message
    //construct header
    geo_header->type = REMOVE_GEOFENCE;
    geo_header->length = msg_len;
    //input server data fd
    memcpy( ((char*)geo_header) + sizeof(mtk_geofence_msg), &temp_server_data_fd, sizeof(int));
    //input geofence property payload
    memcpy( ((char*)geo_header) + sizeof(mtk_geofence_msg) + sizeof(int), &geofence_id, sizeof(int));

    //Create TCP client fd, and connect tcp server
    if ( (fd = geofenceinf_socket_tcp_client_connect_local(true, GEOFENCEADP_MNL_TCP_CONTROL)) < 0){
        LOGE("geofenceinf_remove_geofence:create client fd failed:%d", fd);
        return MTK_GFC_ERROR;
    }

    ret = geofenceinf_send2mnl(fd, (char *)geo_header, msg_len);
    if(ret < 0) {
        LOGE("geofenceinf_remove_geofence:send message to mnl failed");
        close(fd);
        return MTK_GFC_ERROR;
    }

    //wait ack
    expect_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_result);
    if ( (recv_len = geofenceinf_control_path_safe_recv(fd, buffer, GEOFENCE_BUFF_SIZE, expect_len)) <= 0){
        LOGE("geofenceinf_remove_geofence:don't recv any data");
        close(fd);
        return MTK_GFC_ERROR;
    } else {
        prmsg = (mtk_geofence_msg *)&buffer[0];
        ret = geofenceinf_geofence_remove_result(prmsg, &remove_result);
        if (ret == MTK_GFC_ERROR){
            LOGE("geofenceinf_remove_geofence:message data don't match struct %d %d", recv_len, expect_len);
            close(fd);
            return MTK_GFC_ERROR;
        }
    }

    if (remove_result.result == 0){
        close(fd);
        return MTK_GFC_SUCCESS;
    } else {
        close(fd);
        return MTK_GFC_ERROR;
    }
}

int geofenceinf_clear_geofences(void) {
    int ret;
    mtk_geofence_msg *geo_header=NULL;
    unsigned int msg_len;
    int recv_len;
    int expect_len;
    char buffer[GEOFENCE_BUFF_SIZE] = {0};
    char geo_data[GEOFENCE_BUFF_SIZE] = {0};
    int fd = -1, temp_server_data_fd = -1;
    mtk_geofence_msg *prmsg = NULL;
    mtk_geofence_result clear_result;

    LOGD("geofence_clear_geofences");
    if (server_data_fd < 0){
        LOGE("geofenceinf_clear_geofences:wait server data fd");
        return MTK_GFC_ERROR;
    } else {
        temp_server_data_fd = server_data_fd;
    }

    //construct remove fence cmd
    msg_len = sizeof(mtk_geofence_msg) + sizeof(int);
    geo_header = (mtk_geofence_msg *)&geo_data[0];
    if (msg_len > GEOFENCE_BUFF_SIZE){
        LOGE("geofenceinf message length too long:%d", msg_len);
        return MTK_GFC_ERROR;
    }

    //construct total message
    //construct header
    geo_header->type = CLEAR_GEOFENCE;
    geo_header->length = msg_len;
    //input server data fd
    memcpy( ((char*)geo_header) + sizeof(mtk_geofence_msg), &temp_server_data_fd, sizeof(int));

    //Create TCP client fd, and connect tcp server
    if ( (fd = geofenceinf_socket_tcp_client_connect_local(true, GEOFENCEADP_MNL_TCP_CONTROL)) < 0){
        LOGE("geofenceinf_clear_geofences:create client fd failed:%d", fd);
        return MTK_GFC_ERROR;
    }

    ret = geofenceinf_send2mnl(fd, (char *)geo_header, msg_len);
    if(ret < 0) {
        LOGE("geofenceinf_clear_geofences:send message to mnl failed");
        close(fd);
        return MTK_GFC_ERROR;
    }

    //wait ack
    expect_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_result);
    if ( (recv_len = geofenceinf_control_path_safe_recv(fd, buffer, GEOFENCE_BUFF_SIZE, expect_len)) <= 0){
        LOGE("geofenceinf_clear_geofences:don't recv any data");
        close(fd);
        return MTK_GFC_ERROR;
    } else {
        prmsg = (mtk_geofence_msg *)&buffer[0];
        ret = geofenceinf_geofence_clear_result(prmsg, &clear_result);
        if (ret == MTK_GFC_ERROR){
            LOGE("geofenceinf_clear_geofences:message data don't match struct %d %d", recv_len, expect_len);
            close(fd);
            return MTK_GFC_ERROR;
        }
    }

    if (clear_result.result == 0){
        close(fd);
        return MTK_GFC_SUCCESS;
    } else {
        close(fd);
        return MTK_GFC_ERROR;
    }
}

int geofenceinf_query_geofences_num(void) {
    int ret;
    mtk_geofence_msg *geo_header=NULL;
    unsigned int msg_len;
    int recv_len;
    int expect_len;
    char buffer[GEOFENCE_BUFF_SIZE] = {0};
    char geo_data[GEOFENCE_BUFF_SIZE] = {0};
    int fence_nums;
    int fd = -1, temp_server_data_fd = -1;
    mtk_geofence_msg *prmsg = NULL;

    LOGD("geofence_query_geofences_num");
    if (server_data_fd < 0){
        LOGE("geofence_query_geofences_num:wait server data fd");
        return -1;
    } else {
        temp_server_data_fd = server_data_fd;
    }

    //construct remove fence cmd
    msg_len = sizeof(mtk_geofence_msg) + sizeof(int);
    geo_header = (mtk_geofence_msg *)&geo_data[0];
    if (msg_len > GEOFENCE_BUFF_SIZE){
        LOGE("geofenceinf message length too long:%d", msg_len);
        return MTK_GFC_ERROR;
    }

    //construct total message
    //construct header
    geo_header->type = QUERY_GEOFENCE_NUM;
    geo_header->length = msg_len;
    //input server data fd
    memcpy( ((char*)geo_header) + sizeof(mtk_geofence_msg), &temp_server_data_fd, sizeof(int));

    //Create TCP client fd, and connect tcp server
    if ( (fd = geofenceinf_socket_tcp_client_connect_local(true, GEOFENCEADP_MNL_TCP_CONTROL)) < 0){
        LOGE("geofence_query_geofences_num:create client fd failed:%d", fd);
        return -1;
    }

    ret = geofenceinf_send2mnl(fd, (char *)geo_header, msg_len);
    if(ret < 0) {
        LOGE("geofence_query_geofences_num:send message to mnl failed");
        close(fd);
        return MTK_GFC_ERROR;
    }

    //wait ack
    expect_len = sizeof(mtk_geofence_msg) + sizeof(fence_nums);
    if ( (recv_len = geofenceinf_control_path_safe_recv(fd, buffer, GEOFENCE_BUFF_SIZE, expect_len)) <= 0){
        LOGE("geofence_query_geofences_num:don't recv any data");
        close(fd);
        return MTK_GFC_ERROR;
    } else {
        prmsg = (mtk_geofence_msg *)&buffer[0];
        ret = geofenceinf_geofence_get_geofences_num(prmsg, &fence_nums);
        if (ret == MTK_GFC_ERROR){
            LOGE("geofence_query_geofences_num:message data don't match struct %d %d", recv_len, expect_len);
            close(fd);
            return MTK_GFC_ERROR;
        }
    }

    if (fence_nums > 0){
        close(fd);
        return fence_nums;
    } else {
        close(fd);
        return MTK_GFC_ERROR;
    }
}

// GNSS Adaptor --> MNLD Message
int geofenceinf_client_capability_config(mtk_geofence_client_capability *cap) {
    int ret;
    mtk_geofence_msg *geo_header=NULL;
    char geo_data[GEOFENCE_BUFF_SIZE] = {0};
    unsigned int msg_len;
    int fd = -1, temp_server_data_fd = -1;

    LOGD("geofenceinf_client_capability_config");
    if (server_data_fd < 0){
        LOGE("geofenceinf_client_capability_config:wait server data fd");
        return -1;
    } else {
        temp_server_data_fd = server_data_fd;
    }

    //construct remove fence cmd
    msg_len = sizeof(mtk_geofence_msg) + sizeof(int) + sizeof(mtk_geofence_client_capability);
    geo_header = (mtk_geofence_msg *)&geo_data[0];
    if (msg_len > GEOFENCE_BUFF_SIZE){
        LOGE("geofenceinf message length too long:%d", msg_len);
        return MTK_GFC_ERROR;
    }

    //construct total message
    //construct header
    geo_header->type = GEOFENCE_CLIENT_CAP;
    geo_header->length = msg_len;
    //input server data fd
    memcpy( ((char*)geo_header) + sizeof(mtk_geofence_msg), &temp_server_data_fd, sizeof(int));
    memcpy( ((char*)geo_header) + sizeof(mtk_geofence_msg) + sizeof(int), cap, sizeof(mtk_geofence_client_capability));

    //Create TCP client fd, and connect tcp server
    if ( (fd = geofenceinf_socket_tcp_client_connect_local(true, GEOFENCEADP_MNL_TCP_CONTROL)) < 0){
        LOGE("geofenceinf_client_capability_config:create client fd failed:%d", fd);
        return -1;
    }

    ret = geofenceinf_send2mnl(fd, (char *)geo_header, msg_len);
    if(ret < 0) {
        LOGE("geofenceinf_client_capability_config:send message to mnl failed");
        close(fd);
        return MTK_GFC_ERROR;
    }
    return MTK_GFC_SUCCESS;
}

void geofenceinf_server_capability_sync(mtk_geofence_msg *prmsg, mtk_geofence_callback *at_callback) {
    mtk_geofence_server_capability cap;
    unsigned int msg_len;
    char data[GEOFENCE_BUFF_SIZE] = {0};

    msg_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_server_capability);
    if (prmsg->length == msg_len){
        memcpy(&cap, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_server_capability));
    } else if (prmsg->length > msg_len) {
        memcpy(&cap, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_server_capability));
        LOGD("mnl msg len:%d > struct len:%d", prmsg->length, msg_len);
    } else {
        memcpy(data, (char*)prmsg, prmsg->length);
        memset((char *)(data + prmsg->length), 0, (msg_len - prmsg->length));
        memcpy(&cap, (((char*)data)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_server_capability));
        LOGD("mnl msg len:%d < struct len:%d", prmsg->length, msg_len);
    }

    if (at_callback->geofence_capability_update != NULL){
        at_callback->geofence_capability_update(&cap);
    }

    return;
}

void geofenceinf_get_server_data_fd(mtk_geofence_msg *prmsg) {
    int data_fd;
    unsigned int msg_len;
    char data[GEOFENCE_BUFF_SIZE] = {0};

    msg_len = sizeof(mtk_geofence_msg) + sizeof(int);
    if (prmsg->length == msg_len){
        memcpy(&data_fd, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(int));
    } else if (prmsg->length > msg_len) {
        memcpy(&data_fd, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(int));
        LOGD("mnl msg len:%d > struct len:%d", prmsg->length, msg_len);
    } else {
        memcpy(data, (char*)prmsg, prmsg->length);
        memset((char *)(data + prmsg->length), 0, (msg_len - prmsg->length));
        memcpy(&data_fd, (((char*)data)+sizeof(mtk_geofence_msg)), sizeof(int));
        LOGD("mnl msg len:%d < struct len:%d", prmsg->length, msg_len);
    }

    if (data_fd < 0){
        LOGD("geofenceinf_get_server_data_fd error");
    } else {
        server_data_fd = data_fd;
        LOGD("geofenceinf_get_server_data_fd:%d", server_data_fd);
    }

    return;
}

void geofenceinf_geofence_alert(mtk_geofence_msg *prmsg, mtk_geofence_callback *at_callback) {
    mtk_geofence_alert fence_alert;
    unsigned int msg_len;
    char data[GEOFENCE_BUFF_SIZE] = {0};

    msg_len = sizeof(mtk_geofence_msg) + sizeof(mtk_geofence_alert);
    if (prmsg->length == msg_len){
        memcpy(&fence_alert, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_alert));
    } else if (prmsg->length > msg_len) {
        memcpy(&fence_alert, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_alert));
        LOGD("mnl msg len:%d > struct len:%d", prmsg->length, msg_len);
    } else {
        memcpy(data, (char*)prmsg, prmsg->length);
        memset((char *)(data + prmsg->length), 0, (msg_len - prmsg->length));
        memcpy(&fence_alert, (((char*)data)+sizeof(mtk_geofence_msg)), sizeof(mtk_geofence_alert));
        LOGD("mnl msg len:%d < struct len:%d", prmsg->length, msg_len);
    }

    #ifdef AT_COMMAND_PRINT
    /*Construct AT command */
    sprintf(data, "+EGEORESP:%d,%d,%.6f,%.6f,%.2f,%.2f,%.2f,%d,%d,%d,%d,%d,%.2f,%.2f,%.2f/r/n",
            fence_alert.id,
            fence_alert.alert_state,
            fence_alert.latitude,
            fence_alert.longitude,
            fence_alert.altitude,
            fence_alert.speed,
            fence_alert.heading,
            fence_alert.h_acc,
            fence_alert.h_err_majoraxis,
            fence_alert.h_err_minoraxis,
            fence_alert.h_err_angle,
            fence_alert.hor_conf,
            fence_alert.pdop,
            fence_alert.hdop,
            fence_alert.vdop);
    LOGD("AT command:%s",data);
    #endif
    //Send AT command
    if (at_callback->geofence_fence_alert_callback != NULL){
        at_callback->geofence_fence_alert_callback(&fence_alert);
    }

    return;
}

void geofenceinf_gnss_tracking_status(mtk_geofence_msg *prmsg, mtk_geofence_callback *at_callback) {
    mtk_gnss_tracking_status tracking_Status;
    unsigned int msg_len;
    char data[GEOFENCE_BUFF_SIZE] = {0};

    msg_len = sizeof(mtk_geofence_msg) + sizeof(mtk_gnss_tracking_status);
    if (prmsg->length == msg_len){
        memcpy(&tracking_Status, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_gnss_tracking_status));
    } else if (prmsg->length > msg_len) {
        memcpy(&tracking_Status, (((char*)prmsg)+sizeof(mtk_geofence_msg)), sizeof(mtk_gnss_tracking_status));
        LOGD("mnl msg len:%d > struct len:%d", prmsg->length, msg_len);
    } else {
        memcpy(data, (char*)prmsg, prmsg->length);
        memset((char *)(data + prmsg->length), 0, (msg_len - prmsg->length));
        memcpy(&tracking_Status, (((char*)data)+sizeof(mtk_geofence_msg)), sizeof(mtk_gnss_tracking_status));
        LOGD("mnl msg len:%d < struct len:%d", prmsg->length, msg_len);
    }

    #ifdef AT_COMMAND_PRINT
    /*Construct AT command */
    sprintf(data, "+EGEOTRACK:%d,\"%d/%d/%d,%d:%d:%d\"",
            tracking_Status.status,
            tracking_Status.year,
            tracking_Status.month,
            tracking_Status.day,
            tracking_Status.hour,
            tracking_Status.minute,
            tracking_Status.second
            );
    LOGD("AT command:%s",data);
    #endif
    //Send AT command
    if (at_callback->geofence_tracking_status_callback != NULL){
        at_callback->geofence_tracking_status_callback(&tracking_Status);
    }

    return;
}

int geofenceinf_add_rawdata_to_buffer(char *data, int length)
{
    int i;
    LOGD("geofenceadp rev raw data:%d", length);
    for (i = 0; i < length; i++)
    {
        *(g_cyclical_buffer.next_write++) = data[i];
        if (g_cyclical_buffer.next_write == g_cyclical_buffer.end_buffer){
            g_cyclical_buffer.next_write = g_cyclical_buffer.start_buffer;
        }

        if (g_cyclical_buffer.next_write == g_cyclical_buffer.next_read){
            LOGE("geofence ring_buffer overflow\r\n");
            return -1;
        }
    }

    return 0;
}

int geofenceinf_get_one_message(char *data, unsigned int len)
{
    char *next_write, *next_read;
    unsigned int data_size, i, header_len;
    char buffer[GEOFENCE_BUFF_SIZE] = {0};
    unsigned int return_len = 0;
    mtk_geofence_msg geo_header;

    next_write = g_cyclical_buffer.next_write;
    next_read = g_cyclical_buffer.next_read;

    if (g_cyclical_buffer.next_read == next_write)
    {
        //buffer empty
        return -1;
    }

    header_len = sizeof(mtk_geofence_msg);
    /*Compute data length*/
    if (g_cyclical_buffer.next_read < next_write)
    {
        data_size = (unsigned long)next_write - (unsigned long)g_cyclical_buffer.next_read;
    }
    else
    {
        data_size = (unsigned long)g_cyclical_buffer.end_buffer - (unsigned long)g_cyclical_buffer.next_read +
                     (unsigned long)next_write - (unsigned long)g_cyclical_buffer.start_buffer;
    }

    /*Copy data header to buffer*/
    if (data_size >= header_len)
    {
        for (i = 0; i < header_len; i++)
        {
            buffer[i] = *(next_read++);
            if (next_read == g_cyclical_buffer.end_buffer){
                next_read = g_cyclical_buffer.start_buffer;
            }
        }

        memset(&geo_header, 0, sizeof(mtk_geofence_msg));
        memcpy(&geo_header, buffer, sizeof(mtk_geofence_msg));
        if (geo_header.length <= data_size){
            for (i = 0; (i < geo_header.length)&&(i < len); i++)
            {
                data[i] = *(g_cyclical_buffer.next_read++);
                return_len++;
                if (g_cyclical_buffer.next_read == g_cyclical_buffer.end_buffer){
                    g_cyclical_buffer.next_read = g_cyclical_buffer.start_buffer;
                }
            }
        }
        else {
            //no enough data
            return -2;
        }
    }
    else
    {
        //no enough data
        return -2;
    }

    return return_len;
}

int mnl2geofence_hdlr (int fd, mtk_geofence_callback *callback) {
    char data[GEOFENCE_BUFF_SIZE] = {0};
    char buff[GEOFENCE_BUFF_SIZE] = {0};
    int len;
    int read_len;
    int msg_len;
    mtk_geofence_ret_command cmd;
    mtk_geofence_msg *prmsg = NULL;

    read_len = geofenceinf_safe_recv(fd, buff, sizeof(buff));
    if (read_len <= 0) {
        close(fd);
        server_data_fd = -1;
        if(callback->geofence_connection_broken) {
            LOGW("Connection broken...");
            callback->geofence_connection_broken();
        }
        LOGE("mnl2geofence_hdlr() geofenceinf_safe_recv() failed read_len=%d, %s", read_len, strerror(errno));
        return MTK_GFC_ERROR;
    }

    if (geofenceinf_add_rawdata_to_buffer(buff, read_len) < 0){
        //error handle
        LOGE("geofenceinf_add_rawdata_to_buffer() overflow\r\n");
    }

    while ((len = geofenceinf_get_one_message(data, GEOFENCE_BUFF_SIZE)) > 0)
    {
        if((len > 0) && (len <= GEOFENCE_BUFF_SIZE)) {
            prmsg = (mtk_geofence_msg *)&data[0];
        } else {
            LOGE("len err:%d",len);
            return MTK_GFC_ERROR;
        }

        cmd = prmsg->type;
        msg_len = prmsg->length;
        if (msg_len < 0){
            LOGE("mnl2geofence_hdlr() message length error:%d", msg_len);
            return MTK_GFC_ERROR;
        }
        //LOGD("command %d, len %d", cmd, msg_len);
        switch (cmd) {
            case GEOFENCE_ALERT_INFO:
                geofenceinf_geofence_alert(prmsg, callback);
                break;
            case GNSS_TRACKING_STATUS:
                geofenceinf_gnss_tracking_status(prmsg, callback);
                break;
            case GEOFENCE_SERVER_CAP:
                geofenceinf_server_capability_sync(prmsg, callback);
                break;
            default:
                LOGE("invalid geofence cmd:%d",cmd);
                break;
        }

        memset(data, 0, GEOFENCE_BUFF_SIZE);
        len = 0;
    }
    return MTK_GFC_SUCCESS;
}