diff --git a/mbtk/mbtk_gnssd/gnss_hd8122.c b/mbtk/mbtk_gnssd/gnss_hd8122.c
index 57b2e30..dff4599 100755
--- a/mbtk/mbtk_gnssd/gnss_hd8122.c
+++ b/mbtk/mbtk_gnssd/gnss_hd8122.c
@@ -29,30 +29,30 @@
 #define GNSS_POWER_GPIO 43
 #define GNSS_SET_TIMEOUT           3000     // 3s
 #define GNSS_PACK_BUFF_SIZE           1024
+#define GNSS_MSG_NUM_MAX 30
 
 
 static pthread_cond_t read_cond;
 static pthread_mutex_t read_mutex;
+static bool setting_waitting = FALSE;
 static bool setting_busy = FALSE;
 static void *gnss_set_rsp_ptr = NULL;
 static gnss_err_enum gnss_set_result = GNSS_ERR_OK;
+static hd8122_msg_id_t msg_array[GNSS_MSG_NUM_MAX];
 
 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;
+    uint32_t sum1 = 0;
+    uint32_t sum2 = 0;
     int index;
 
     for (index = 0; index < data_len; ++index ) {
-        sum1 = (sum1 + data[index]) % 0xff;
-        sum2 = (sum2 + sum1) % 0xff;
+        sum1 += data[index];
+        sum2 += sum1;
     }
 
-    // ???
-    sum2--;
-
-    return (sum2 << 8) | sum1;
+    return ((0xFF & sum2) << 8) | (0xFF & sum1);
 }
 
 static void gnss_set_timer_cb(int signo)
@@ -66,6 +66,77 @@
     return;
 }
 
+static void msg_init()
+{
+    int i = 0;
+    while(i < ARRAY_SIZE(msg_array)) {
+        msg_array[i].enable = FALSE;
+        i++;
+    }
+}
+
+
+static int msg_insert(uint8 gid, uint8 sid)
+{
+    int i = 0;
+    while(i < ARRAY_SIZE(msg_array)) {
+        if(!msg_array[i].enable)
+            break;
+        i++;
+    }
+
+    if(i == ARRAY_SIZE(msg_array)) {
+        LOGE("Msg full : %d", i);
+        return -1;
+    } else {
+        msg_array[i].enable = TRUE;
+        msg_array[i].gid = gid;
+        msg_array[i].sid = sid;
+        return 0;
+    }
+}
+
+static int msg_find(uint8 gid, uint8 sid)
+{
+    int i = 0;
+    while(i < ARRAY_SIZE(msg_array)) {
+        if(msg_array[i].enable && gid == msg_array[i].gid && sid == msg_array[i].sid)
+            break;
+        i++;
+    }
+
+    if(i == ARRAY_SIZE(msg_array)) {
+        LOGE("No found %d - %d", gid, sid);
+        return -1;
+    } else {
+        return i;
+    }
+}
+
+static int msg_remove(uint8 gid, uint8 sid)
+{
+    int i = msg_find(gid, sid);
+    if(i >= 0) {
+        msg_array[i].enable = FALSE;
+        msg_array[i].gid = 0;
+        msg_array[i].sid = 0;
+        return 0;
+    } else {
+        return -1;
+    }
+}
+
+static int msg_count()
+{
+    int i = 0;
+    int count = 0;
+    while(i < ARRAY_SIZE(msg_array)) {
+        if(msg_array[i].enable)
+            count++;
+        i++;
+    }
+    return count;
+}
 
 static int pack_create(hd8122_id_type_enum id_type, uint8 id, uint16 data_len, const uint8 *data, uint8 *pack, int pack_len)
 {
@@ -86,20 +157,81 @@
     return (data_ptr - pack);
 }
 
+// f1 d9 05 01 02 00 06 01 0f 38
+// or
+// f1 d9 05 00 02 00 06 01 0f 38
+static int msg_array_change(const uint8 *pack, int pack_len, hd8122_id_ack_enum *ack_nak)
+{
+    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) {
+        uint8 *ptr = pack + i * 10;
+        if(ptr[0] != 0xf1 || ptr[1] != 0xd9) {
+            LOGE("Pack head error : %02x %02x", ptr[0], ptr[1]);
+            return -1;
+        }
+
+        if(ptr[2] != 0x05) {
+            LOGE("Type not 0x05 : %02x", ptr[2]);
+            return -1;
+        }
+
+        int index = msg_find(ptr[6], ptr[7]);
+        if(index >= 0) {
+            if(ptr[3] == 0x01) {
+                msg_array[index].ack_nak = HD8122_ID_ACK_ACK;
+            } else if(ptr[3] == 0x00) {
+                msg_array[index].ack_nak = HD8122_ID_ACK_NAK;
+
+                // There is a nak as a failure.
+                *ack_nak = HD8122_ID_ACK_NAK;
+            } else {
+                LOGE("ID not 0x00 or 0x01 : %02x", ptr[3]);
+                return -1;
+            }
+
+            msg_array[index].enable = FALSE;
+        } else {
+            LOGE("Unknown gid - %d, sid - %d", ptr[6], ptr[7]);
+            return -1;
+        }
+        i++;
+    }
+
+    return 0;
+}
+
 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);
+    hd8122_id_ack_enum ack_nak = HD8122_ID_ACK_ACK;
+    if(!msg_array_change((const uint8*)data, data_len, &ack_nak)) {
+        if(setting_waitting && msg_count() == 0)
+        {
+            if(ack_nak == HD8122_ID_ACK_ACK) {
+                gnss_set_result = GNSS_ERR_OK;
+            } else {
+                gnss_set_result = GNSS_ERR_UNKNOWN;
+            }
+
+            mbtk_timer_clear();
+
+            pthread_mutex_lock(&read_mutex);
+            pthread_cond_signal(&read_cond);
+            pthread_mutex_unlock(&read_mutex);
+            setting_waitting = FALSE;
+        }
+    } else {
+        LOGW("Unknown rsp data.");
     }
 }
 
-static gnss_err_enum gnss_8122_reset(int fd, int reset)
+static gnss_err_enum gnss_8122_reset(int fd, uint8 reset)
 {
     uint8 buff[GNSS_PACK_BUFF_SIZE];
     LOGD("RESET");
@@ -113,6 +245,72 @@
     return GNSS_ERR_OK;
 }
 
+static gnss_err_enum gnss_8122_syscfg(int fd, uint32 mode)
+{
+    uint8 buff[GNSS_PACK_BUFF_SIZE];
+    LOGD("SYSCFG");
+    //uint8 mode_str[4];
+    //uint32_2_byte(mode, mode_str, TRUE);
+    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NAVSAT, 4, (uint8*)(&mode), buff, sizeof(buff));
+    if(len <= 0) {
+        LOGE("pack_create() fail.");
+        return GNSS_ERR_ARG;
+    }
+    log_hex("PACK", buff, len);
+    gnss_write(fd, buff, len);
+    msg_insert(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NAVSAT);
+    return GNSS_ERR_OK;
+}
+
+static gnss_err_enum gnss_8122_msgcfg(int fd, uint8 type, uint8 id, uint8 period)
+{
+    uint8 buff[GNSS_PACK_BUFF_SIZE];
+    LOGD("MSGCFG");
+    uint8 data[3];
+    data[0] = type;
+    data[1] = id;
+    data[2] = period;
+    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_MSG, 3, data, buff, sizeof(buff));
+    if(len <= 0) {
+        LOGE("pack_create() fail.");
+        return GNSS_ERR_ARG;
+    }
+    log_hex("PACK", buff, len);
+    gnss_write(fd, buff, len);
+    msg_insert(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_MSG);
+    return GNSS_ERR_OK;
+}
+
+static gnss_err_enum gnss_8122_minel(int fd, float *elev)
+{
+    uint8 buff[GNSS_PACK_BUFF_SIZE];
+    LOGD("ELEV");
+    //uint8 elev_buff[4];
+    //uint32_2_byte((uint32)elev, elev_buff, TRUE);
+    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_ELEV, 8, (uint8*)elev, 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;
+}
+
+static gnss_err_enum gnss_8122_nmeaver(int fd, uint8 ver)
+{
+    uint8 buff[GNSS_PACK_BUFF_SIZE];
+    LOGD("NMEA-VER");
+    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NMEAVER, 1, (uint8*)(&ver), 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()
 {
     return mbtk_gpio_value_set(GNSS_POWER_GPIO, MBTK_GPIO_DIRECT_OUT, 1);
@@ -145,7 +343,7 @@
 void gnss_8122_set_cb(const void *data, int data_len)
 {
     const char *buff = (const char*)data;
-    if(setting_busy) {
+    if(setting_busy) { // Has setting cmd process.
         gnss_cmd_rsp_process(data, data_len);
     }
 }
@@ -159,14 +357,187 @@
         setting_busy = TRUE;
         gnss_set_rsp_ptr = cmd_rsp;
         gnss_set_result = GNSS_ERR_OK;
+        msg_init();
         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(memcmp(cmd, "$RESET", 6) == 0) { // $RESET,<mode>
+            gnss_reset_type_enum mode = (gnss_reset_type_enum)atoi(cmd + 7);
+            if(mode == GNSS_RESET_TYPE_HOT) {
+                gnss_set_result = gnss_8122_reset(fd, 3);
+            } else if(mode == GNSS_RESET_TYPE_WARM) {
+                gnss_set_result = gnss_8122_reset(fd, 2);
+            } else if(mode == GNSS_RESET_TYPE_COLD) {
+                gnss_set_result = gnss_8122_reset(fd, 1);
+            } else {
+                gnss_set_result = GNSS_ERR_ARG;
+                goto set_fail;
+            }
             if(gnss_set_result != GNSS_ERR_OK) {
                 goto set_fail;
             }
             should_wait_rsp = FALSE;
+        } else if(memcmp(cmd, "$SYSCFG", 7) == 0) { // $SYSCFG,<mode>
+            uint32 mode = 0;
+            mode = (uint32)atoi(cmd + 8);
+            uint32 new_mode = 0;
+            if(((GNSS_SET_SYSCFG_GPS | GNSS_SET_SYSCFG_BDS | GNSS_SET_SYSCFG_GLO | GNSS_SET_SYSCFG_GAL) & mode) != mode) {
+                gnss_set_result = GNSS_ERR_ARG;
+                goto set_fail;
+            }
+
+            if(mode & GNSS_SET_SYSCFG_GPS) { // GPS
+                new_mode |= 0x00000001;
+            }
+            if(mode & GNSS_SET_SYSCFG_BDS) { // BDS
+                new_mode |= 0x00000002;
+            }
+            if(mode & GNSS_SET_SYSCFG_GLO) { // GLO
+                new_mode |= 0x00000004;
+            }
+            if(mode & GNSS_SET_SYSCFG_GAL) { // GAL
+                new_mode |= 0x00000010;
+            }
+
+            gnss_set_result = gnss_8122_syscfg(fd, new_mode);
+            if(gnss_set_result != GNSS_ERR_OK) {
+                goto set_fail;
+            }
+            should_wait_rsp = TRUE;
+        } else if(memcmp(cmd, "$MSGCFG", 7) == 0) { // $MSGCFG,<mode>,<rate>
+            uint32 mode;
+            int rate;
+            if(2 == sscanf(cmd, "$MSGCFG,%d,%d", &mode, &rate)) {
+                int time = rate / 1000; // s
+                if(time < 0) {
+                    gnss_set_result = GNSS_ERR_ARG;
+                    goto set_fail;
+                }
+
+                if(((GNSS_SET_MSGCFG_RMC | GNSS_SET_MSGCFG_VTG | GNSS_SET_MSGCFG_GGA | GNSS_SET_MSGCFG_GSA
+                    | GNSS_SET_MSGCFG_GRS | GNSS_SET_MSGCFG_GSV | GNSS_SET_MSGCFG_GLL | GNSS_SET_MSGCFG_ZDA
+                    | GNSS_SET_MSGCFG_GST | GNSS_SET_MSGCFG_TXT) & mode) != mode) {
+                    gnss_set_result = GNSS_ERR_ARG;
+                    goto set_fail;
+                }
+
+                if(mode & GNSS_SET_MSGCFG_RMC) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF8, 0x05, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_VTG) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x06, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_GGA) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x00, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_GSA) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x02, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_GRS) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x03, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_GSV) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x04, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_GLL) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x01, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_ZDA) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x07, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_GST) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x08, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+
+                if(mode & GNSS_SET_MSGCFG_TXT) {
+                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x20, time);
+                    if(gnss_set_result != GNSS_ERR_OK) {
+                        goto set_fail;
+                    }
+                }
+            } else {
+                gnss_set_result = GNSS_ERR_ARG;
+                goto set_fail;
+            }
+
+            should_wait_rsp = TRUE;
+        } else if(memcmp(cmd, "$MINEL", 6) == 0) { // $MINEL,<elev>
+#if 0
+            float elev = 0.0f;
+            elev = (float)atof(cmd + 7);
+//            LOGD("ELEV : %f", elev);
+//            log_hex("ELEV", &elev, sizeof(double));
+            if(elev < -90.0f || elev > 90.0f) {
+                gnss_set_result = GNSS_ERR_ARG;
+                goto set_fail;
+            }
+            float elevs[2];
+            elevs[0] = elev;
+            elevs[1] = elev;
+            gnss_set_result = gnss_8122_minel(fd, elevs);
+            if(gnss_set_result != GNSS_ERR_OK) {
+                goto set_fail;
+            }
+            should_wait_rsp = FALSE;
+#else
+            gnss_set_result = GNSS_ERR_UNSUPPORT;
+            goto set_fail;
+#endif
+        } else if(memcmp(cmd, "$NMEACFG", 8) == 0) { // $NMEACFG,<ver>
+#if 0
+            gnss_memaver_type_enum version = (gnss_memaver_type_enum)atoi(cmd + 9);
+            if(version == GNSS_MEMAVER_TYPE_3_0) {
+                gnss_set_result = gnss_8122_nmeaver(fd, 1);
+            } else if(version == GNSS_MEMAVER_TYPE_4_0) {
+                gnss_set_result = gnss_8122_nmeaver(fd, 2);
+            } else if(version == GNSS_MEMAVER_TYPE_4_1) {
+                gnss_set_result = gnss_8122_nmeaver(fd, 3);
+            } else {
+                gnss_set_result = GNSS_ERR_ARG;
+                goto set_fail;
+            }
+            if(gnss_set_result != GNSS_ERR_OK) {
+                goto set_fail;
+            }
+            should_wait_rsp = FALSE;
+#else
+            gnss_set_result = GNSS_ERR_UNSUPPORT;
+            goto set_fail;
+#endif
         }
         else
         {
@@ -177,6 +548,7 @@
 
 set_success:
         if(should_wait_rsp) {
+            setting_waitting = TRUE;
             pthread_mutex_lock(&read_mutex);
             pthread_cond_wait(&read_cond, &read_mutex);
             pthread_mutex_unlock(&read_mutex);