gnssd: add 8122 agps func
Change-Id: Ie87dcafc1da9cf5cffd9f5397f1640f19aa36aab
diff --git a/mbtk/mbtk_gnssd/agps/8122_agnss.c b/mbtk/mbtk_gnssd/agps/8122_agnss.c
new file mode 100755
index 0000000..4acbe33
--- /dev/null
+++ b/mbtk/mbtk_gnssd/agps/8122_agnss.c
@@ -0,0 +1,418 @@
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <stdint.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <string.h>
+#include <unistd.h>
+
+
+#include "8122_agnss.h"
+#include "mbtk_log.h"
+
+#define BUFFER_SIZE 1024
+
+#define INJECT_RESULT_SUCCESS 0
+#define INJECT_RESULT_FAIL -1
+
+static int hd_fd = -1;
+static hd_eph_inject_status_type eph_inject_status = HD_EPH_INJECT_STATUS_SUCCESS;
+
+static void hd_agnss_delay(uint32_t nms)
+{
+ usleep(nms * 1000);
+}
+
+static int hd_agnss_uart_write(uint8_t *buf, uint32_t len)
+{
+ int TxLen;
+ if(hd_fd > 0)
+ {
+ TxLen = write(hd_fd,buf,len);
+ if(TxLen != (int)len)
+ {
+ LOGE("hd_agnss_uart_write fail.ret = [%d], len = [%d]", TxLen, len);
+ return INJECT_RESULT_FAIL;
+ }
+ }
+ else
+ {
+ LOGE("8122 agps fd error.");
+ }
+ return TxLen;
+}
+
+static int32_t message_package(uint8_t *dst, uint8_t *src, int32_t src_len)
+{
+ int32_t i = 0;
+ int8_t checksum1 = 0;
+ int8_t checksum2 = 0;
+
+ /* message header */
+ for (i = 0; i < 4; i++)
+ {
+ dst[i] = *(src + i);
+ }
+
+ /* message len */
+ dst[4] = src_len;
+ dst[5] = 0;
+
+ /* payload */
+ for( i = 0 ; i < src_len; i++)
+ {
+ dst[6 + i] = *(src + 4) ;
+ src++;
+ }
+
+ /* check sum */
+ for(i = 2; i < (6 + dst[4]); i++)
+ {
+ checksum1 += dst[i];
+ checksum2 += checksum1;
+ }
+
+ dst[src_len + 6] = checksum1;
+ dst[src_len + 7] = checksum2;
+
+ return src_len + 8;
+}
+
+/************************************time inject*************************************/
+static int gnss_inject_time(HD_AGNSS_UTC_TIME_TYPE *time)
+{
+ uint8_t cmd[24] = {0}; /* packet head+payload */
+ uint8_t message[30] = {0}; /* payload : 20, packet pad : 8 */
+ int32_t len = 0;
+
+ memset(cmd, 0, sizeof(cmd));
+
+ cmd[0] = 0xF1;
+ cmd[1] = 0xD9;
+ cmd[2] = 0x0B;
+ cmd[3] = 0x11;
+
+ cmd[4] = 0x00; /* UTC */
+ cmd[5] = 0x00; /* reserved */
+ cmd[6] = 0x12; /* leap cnt, hd8030 not used */
+
+ cmd[7] = time->year & 0xFF;
+ cmd[8] = ((time->year >> 8) & 0xFF); /* year */
+ cmd[9] = time->month; /* month */
+ cmd[10] = time->day; /* day */
+ cmd[11] = time->hour; /* hour */
+ cmd[12] = time->min; /* minu */
+ cmd[13] = time->second; /* sec */
+
+ cmd[18] = 0x00;
+ cmd[19] = 0x00;
+ for (int i = 0; i < 4; i++)
+ {
+ cmd[20 + i] = 0x00;
+ }
+
+ len = message_package(message, cmd, sizeof(cmd) - 4);
+
+ LOGE("%02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X",
+ message[0],message[1],message[2],message[3],message[4],message[5],message[6],message[7],
+ message[8],message[9],message[10],message[11],message[12],message[13],message[14],
+ message[15],message[16],message[17],message[18],message[19],message[20],
+ message[21],message[22],message[23],message[24],message[25],message[26],
+ message[27],message[28],message[29]);
+
+ hd_agnss_uart_write(message, len);
+ hd_agnss_delay(3); // just example, use wait 3ms instead of ACK
+
+ return 0;
+}
+
+/************************************location inject*************************************/
+static int gnss_inject_location(int32_t latitude, int32_t longitude, float altitude, float accuracy)
+{
+ uint8_t cmd[21] = { 0 }; /* packet head+payload */
+ uint32_t acc = (uint32_t)(fabs(accuracy));
+ uint32_t alti = (uint32_t)(altitude * 100); // m--->cm
+ uint32_t i = 0;
+ uint8_t message[25] = {0}; /* payload:17 + packet pad:8 */
+
+ memset(cmd, 0, sizeof(cmd));
+ cmd[0] = 0xF1;
+ cmd[1] = 0xD9;
+ cmd[2] = 0x0B;
+ cmd[3] = 0x10;
+
+ /* LLA : 1 */
+ cmd[4] = 0x01;
+
+ /* lat : 4 */
+ for (i = 0; i < 4; i++)
+ {
+ cmd[5 + i] = (latitude >> (i * 8)) & 0xff;
+ }
+
+ /* long : 4 */
+ for (i = 0; i < 4; i++)
+ {
+ cmd[9 + i] = (longitude >> (i * 8)) & 0xff;
+ }
+
+ /* alti : 4 */
+ for (i = 0; i < 4; i++)
+ {
+ cmd[13 + i] = (alti >> (i * 8)) & 0xff;
+ }
+
+ /* accuracy : 4 */
+ for (i = 0; i < 4; i++)
+ {
+ cmd[17 + i] = (acc >> (i * 8)) & 0xff;
+ }
+
+
+ int32_t len = message_package(message, cmd, sizeof(cmd) - 4);
+
+ LOGE("%02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X",
+ message[0],message[1],message[2],message[3],message[4],message[5],message[6],message[7],
+ message[8],message[9],message[10],message[11],message[12],message[13],message[14],
+ message[15],message[16],message[17],message[18],message[19],message[20],
+ message[21],message[22],message[23],message[24]);
+
+ hd_agnss_uart_write(message, len);
+ hd_agnss_delay(3); // just example, use wait 3ms instead of ACK
+
+ return 0;
+}
+
+/************************************eph data inject*************************************/
+/**
+* @brief get the eph frame and send to HD80xx
+* @param data: the pointer of the eph data file
+* @param len : the total length of eph data file(eg:the length of HD_GPS_BD.hdb )
+* @retval 0: successs -1 error
+*/
+static int gnss_eph_inject_data(const char *eph_file_path)
+{
+ int packet_length;
+ int eph_file_fd = -1;
+ int size = 0;
+ int left_size = 0;
+ int total_size = 0;
+ int write_size = 0;
+ int wait_time = 0;
+ uint8_t *databuf = (uint8_t *)malloc(BUFFER_SIZE);
+ if(databuf == NULL)
+ {
+ LOGE("malloc fail");
+ return INJECT_RESULT_FAIL;
+ }
+
+ eph_file_fd = open(eph_file_path, O_RDWR);
+ if (eph_file_fd <= 0)
+ {
+ LOGE("%s open file FAIL. errno:%d\n", __FUNCTION__, errno);
+ goto error;
+ }
+ while(0 < (size = read(eph_file_fd, (databuf + left_size), BUFFER_SIZE)))
+ {
+ total_size = size + left_size;
+ left_size = 0;
+ for(int i=0; i < total_size;)
+ {
+ if((databuf[i] == 0xF1) && (databuf[i + 1] == 0xD9))
+ {
+ packet_length = (databuf[i + 4] | (databuf[i + 5] << 8));
+ if (i + packet_length + 8 <= total_size)
+ {
+ hd_set_eph_inject_status(HD_EPH_INJECT_STATUS_WAIT_RETURN);
+ write_size = hd_agnss_uart_write(databuf + i, packet_length + 8);
+ if(write_size < 0)
+ {
+ LOGE("hd_agnss_uart_write fail");
+ goto error;
+ }
+ LOGD("%s Write[%d]\r\n", __FUNCTION__, write_size);
+ wait_time = 0;
+ while(1)
+ {
+ wait_time += 100;
+ hd_agnss_delay(wait_time);
+ if(wait_time < 1000)
+ {
+ if(hd_get_eph_inject_status() == HD_EPH_INJECT_STATUS_SUCCESS)
+ {
+ break;
+ }
+ else if(hd_get_eph_inject_status() == HD_EPH_INJECT_STATUS_FAIL)
+ {
+ LOGE("8122 return fail");
+ goto error;
+ }
+ else
+ {
+ LOGE("eph inject wait 8122 return");
+ }
+ }
+ else
+ {
+ LOGE("8122 wait 1s return timeout");
+ goto error;
+ }
+ }
+ i = i + packet_length + 8;
+ }
+ else
+ {
+ left_size = total_size - i;
+ uint8_t *tmp = databuf;
+ databuf = (uint8_t *)malloc(BUFFER_SIZE + left_size);
+ memcpy(databuf, tmp + i ,left_size);
+ free(tmp);
+ break;
+ }
+ }
+ else
+ {
+ i++;
+ }
+ }
+ }
+
+ if(databuf)
+ {
+ free(databuf);
+ databuf = NULL;
+ }
+ if(eph_file_fd > 0)
+ {
+ close(eph_file_fd);
+ eph_file_fd = -1;
+ }
+ hd_set_eph_inject_status(HD_EPH_INJECT_STATUS_SUCCESS);
+ return INJECT_RESULT_SUCCESS;
+error:
+ if(databuf)
+ {
+ free(databuf);
+ databuf = NULL;
+ }
+ if(eph_file_fd > 0)
+ {
+ close(eph_file_fd);
+ eph_file_fd = -1;
+ }
+ hd_set_eph_inject_status(HD_EPH_INJECT_STATUS_SUCCESS);
+ return INJECT_RESULT_FAIL;
+}
+
+/************************************choice func inject*************************************/
+/**
+ * @brief AGNSS星历注入、位置注入、时间注入。星历、位置、时间可选择性注入。
+ * @param eph_file_path：从服务器下载的星历数据文件路径；若无则填NULL
+
+ latitude：纬度，乘以10^7；若无则填0
+ longitude：经度，乘以10^7；若无则填0
+ altitude：高程，浮点，单位为m；若无则填0
+ accuracy：精度；若无则填0
+
+ UTC_time：UTC时间；若无则填NULL
+ * @retval VOID
+**/
+int hd_agnss_inject(const char *eph_file_path, int32_t latitude, int32_t longitude, float altitude, float accuracy, /****latitude和longitude由double型乘以10^7得到*****/
+ HD_AGNSS_UTC_TIME_TYPE *UTC_time)
+{
+ int ret = INJECT_RESULT_SUCCESS;
+ if (UTC_time != NULL)
+ {
+ gnss_inject_time(UTC_time);
+ }
+
+ if ((latitude != 0) && (longitude != 0))
+ {
+ gnss_inject_location(latitude, longitude, altitude, accuracy);
+ }
+
+ if (eph_file_path != NULL)
+ {
+ ret = gnss_eph_inject_data(eph_file_path);
+ if(ret < 0)
+ {
+ LOGE("gnss_eph_inject_data fail");
+ return INJECT_RESULT_FAIL;
+ }
+ }
+ return INJECT_RESULT_SUCCESS;
+}
+
+
+/************************************set fd*************************************/
+void hd_set_gnss_dev_fd(int fd)
+{
+ hd_fd = fd;
+}
+
+/************************************check return*************************************/
+//gps f1 d9 05 01 02 00 0b 32 46 74
+//bds f1 d9 05 01 02 00 0b 33 46 74
+//glo f1 d9 05 01 02 00 0b 34 46 74
+
+int hd_eph_inject_result_check(const uint8_t *pack, int pack_len)
+{
+ if(pack_len == 0 || pack_len % 10)
+ {
+ LOGE("pack_len(%d) error.", pack_len);
+ return -1;
+ }
+ int count = pack_len / 10;
+ int i = 0;
+ while(i < count)
+ {
+ const uint8_t *ptr = pack + i * 10;
+ if(ptr[0] != 0xf1 || ptr[1] != 0xd9)
+ {
+ LOGE("Pack head error : %02x %02x", ptr[0], ptr[1]);
+ return INJECT_RESULT_FAIL;
+ }
+
+ if(ptr[2] != 0x05)
+ {
+ LOGE("Type not 0x05 : %02x", ptr[2]);
+ return INJECT_RESULT_FAIL;
+ }
+
+ if(ptr[6] != 0x0b || (ptr[7] < 0x32 && ptr[7] > 0x36))
+ {
+ LOGE("Unknown gid - %d, sid - %d", ptr[6], ptr[7]);
+ return INJECT_RESULT_FAIL;
+ }
+
+ if(ptr[3] == 0x01)
+ {
+ return INJECT_RESULT_SUCCESS;
+ }
+ else if(ptr[3] == 0x00)
+ {
+ return INJECT_RESULT_FAIL;
+ }
+ else
+ {
+ LOGE("ID not 0x00 or 0x01 : %02x", ptr[3]);
+ return INJECT_RESULT_FAIL;
+ }
+ i++;
+ }
+
+ return INJECT_RESULT_SUCCESS;
+}
+/************************************get or set status*************************************/
+void hd_set_eph_inject_status(hd_eph_inject_status_type status)
+{
+ eph_inject_status = status;
+}
+
+hd_eph_inject_status_type hd_get_eph_inject_status(void)
+{
+ return eph_inject_status;
+}
+