Fix gnss 8122 and IPC socket.
Change-Id: Id413efb4dd8df3e5a52333cc0c1400db7f84ad9f
diff --git a/mbtk/mbtk_gnssd/gnss_hd8122.c b/mbtk/mbtk_gnssd/gnss_hd8122.c
index dc36267..57b2e30 100755
--- a/mbtk/mbtk_gnssd/gnss_hd8122.c
+++ b/mbtk/mbtk_gnssd/gnss_hd8122.c
@@ -27,10 +27,91 @@
#define UART_BITRATE_NMEA_DEF_FW 115200 // Default bitrate.
#define GNSS_POWER_GPIO 43
+#define GNSS_SET_TIMEOUT 3000 // 3s
+#define GNSS_PACK_BUFF_SIZE 1024
+
static pthread_cond_t read_cond;
static pthread_mutex_t read_mutex;
+static bool setting_busy = FALSE;
+static void *gnss_set_rsp_ptr = NULL;
+static gnss_err_enum gnss_set_result = GNSS_ERR_OK;
+int gnss_write(int fd, const void *data, int data_len);
+
+static uint16 fletcher16(const uint8_t* data, int data_len) {
+ uint16_t sum1 = 0;
+ uint16_t sum2 = 0;
+ int index;
+
+ for (index = 0; index < data_len; ++index ) {
+ sum1 = (sum1 + data[index]) % 0xff;
+ sum2 = (sum2 + sum1) % 0xff;
+ }
+
+ // ???
+ sum2--;
+
+ return (sum2 << 8) | sum1;
+}
+
+static void gnss_set_timer_cb(int signo)
+{
+ if(setting_busy) {
+ pthread_mutex_lock(&read_mutex);
+ pthread_cond_signal(&read_cond);
+ pthread_mutex_unlock(&read_mutex);
+ gnss_set_result = GNSS_ERR_TIMEOUT;
+ }
+ return;
+}
+
+
+static int pack_create(hd8122_id_type_enum id_type, uint8 id, uint16 data_len, const uint8 *data, uint8 *pack, int pack_len)
+{
+ if(pack == NULL || pack_len < HD8122_PACK_LEN_MIN) {
+ return -1;
+ }
+ memset(pack, 0, pack_len);
+ uint8 *data_ptr = pack;
+ data_ptr += uint16_2_byte(HD8122_PACK_HEAD, data_ptr, false);
+ *data_ptr++ = (uint8)id_type;
+ *data_ptr++ = id;
+ data_ptr += uint16_2_byte(data_len, data_ptr, false);
+ if(data_len > 0) {
+ memcpy(data_ptr, data, data_len);
+ data_ptr += data_len;
+ }
+ data_ptr += uint16_2_byte(fletcher16(pack + 2, 4 + data_len), data_ptr, false);
+ return (data_ptr - pack);
+}
+
+static void gnss_cmd_rsp_process(const void *data, int data_len) {
+ const char *ptr = (const char*)data;
+ log_hex("RSP", data, data_len);
+ if(0)
+ {
+ mbtk_timer_clear();
+
+ pthread_mutex_lock(&read_mutex);
+ pthread_cond_signal(&read_cond);
+ pthread_mutex_unlock(&read_mutex);
+ }
+}
+
+static gnss_err_enum gnss_8122_reset(int fd, int reset)
+{
+ uint8 buff[GNSS_PACK_BUFF_SIZE];
+ LOGD("RESET");
+ int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_SIMPLERST, 1, (uint8*)(&reset), buff, sizeof(buff));
+ if(len <= 0) {
+ LOGE("pack_create() fail.");
+ return GNSS_ERR_ARG;
+ }
+ log_hex("PACK", buff, len);
+ gnss_write(fd, buff, len);
+ return GNSS_ERR_OK;
+}
int gnss_8122_dev_open()
{
@@ -61,9 +142,55 @@
return 0;
}
+void gnss_8122_set_cb(const void *data, int data_len)
+{
+ const char *buff = (const char*)data;
+ if(setting_busy) {
+ gnss_cmd_rsp_process(data, data_len);
+ }
+}
gnss_err_enum gnss_8122_set(int fd, const char *cmd, void *cmd_rsp, int cmd_rsp_len)
{
- return GNSS_ERR_OK;
+ if(setting_busy) {
+ return GNSS_ERR_SET_BUSY;
+ } else {
+ bool should_wait_rsp = TRUE;
+ setting_busy = TRUE;
+ gnss_set_rsp_ptr = cmd_rsp;
+ gnss_set_result = GNSS_ERR_OK;
+ mbtk_timer_set(gnss_set_timer_cb, GNSS_SET_TIMEOUT);
+
+ if(memcmp(cmd, "reset", 5) == 0) {
+ gnss_set_result = gnss_8122_reset(fd, atoi(cmd + 6));
+ if(gnss_set_result != GNSS_ERR_OK) {
+ goto set_fail;
+ }
+ should_wait_rsp = FALSE;
+ }
+ else
+ {
+ LOGW("Unknown cmd:%s", cmd);
+ gnss_set_result = GNSS_ERR_UNSUPPORT;
+ goto set_fail;
+ }
+
+set_success:
+ if(should_wait_rsp) {
+ pthread_mutex_lock(&read_mutex);
+ pthread_cond_wait(&read_cond, &read_mutex);
+ pthread_mutex_unlock(&read_mutex);
+ } else {
+ mbtk_timer_clear();
+ }
+
+ setting_busy = FALSE;
+ return gnss_set_result;
+set_fail:
+ setting_busy = FALSE;
+ mbtk_timer_clear();
+ return gnss_set_result;
+ }
}
+