修复factorytest getversion 失败bug

diff --git a/code/sdk/application/wearable/service/factorytest_service/factorytestService.c b/code/sdk/application/wearable/service/factorytest_service/factorytestService.c
index 8d3ec2d..e54ec84 100644 (file)
--- a/code/sdk/application/wearable/service/factorytest_service/factorytestService.c
+++ b/code/sdk/application/wearable/service/factorytest_service/factorytestService.c
@@ -215,7 +215,7 @@ extern void factorytest_protocol_decode(uint8_t *buf, uint16_t event_size);
 void factory_recv_cmd_data(uint8_t *buf, uint16_t event_size,uint16_t handle)\r
 {\r
     if(buf != NULL)\r
-        printf("factory_recv_cmd_data len %d , handle 0x%X ,*buf 0x02X\r\n",event_size,handle,*buf);\r
+        printf("factory_recv_cmd_data len %d , handle 0x%X ,*buf 0x%02X\r\n",event_size,handle,*buf);\r
     /****此处为接收指令,处理后反馈结果的参考**/\r
     factorytest_protocol_decode(buf,event_size);\r
     /*****参考结束****/\r

diff --git a/code/sdk/application/wearable/service/factorytest_service/factorytest_protocol.c b/code/sdk/application/wearable/service/factorytest_service/factorytest_protocol.c
index 6bafd53..d6b2e0d 100644 (file)
--- a/code/sdk/application/wearable/service/factorytest_service/factorytest_protocol.c
+++ b/code/sdk/application/wearable/service/factorytest_service/factorytest_protocol.c
@@ -18,25 +18,21 @@ typedef struct factorytest_cmd_header_t
 }factorytest_cmd_header_t;\r
 #pragma pack()\r
 \r
-uint16_t modbus_crc16(uint8_t *data, uint16_t length) {  \r
-    uint8_t crc_hi = 0xFF;  \r
-    uint8_t crc_lo = 0xFF;  \r
-    int i;  \r
-  \r
-    while (length--) {  \r
-        crc_hi ^= *data++;\r
-        for (i = 0; i < 8; i++) {  \r
-            if (crc_hi & 0x80) {  \r
-                crc_hi = (crc_hi << 1) ^ 0x8005;  \r
-            } else {  \r
-                crc_hi <<= 1;  \r
-            }  \r
-        }  \r
-    }  \r
-  \r
-    return (crc_hi << 8) | crc_lo;  \r
-}  \r
- \r
+static int CRC16_XMODEM(uint8_t *bytes,uint16_t length) {\r
+    int crc = 0x0000;          // initial value\r
+    int polynomial = 0x1021;\r
+    for (int index = 0; index < length; index++) {\r
+        uint8_t b = bytes[index];\r
+        for (int i = 0; i < 8; i++) {\r
+            uint8_t bit = ((b >> (7 - i) & 1) == 1);\r
+            uint8_t c15 = ((crc >> 15 & 1) == 1);\r
+            crc <<= 1;\r
+            if (c15 ^ bit) crc ^= polynomial;\r
+        }\r
+    }\r
+    crc &= 0xffff;\r
+    return crc;\r
+}\r
 errcode_t factorytest_protocol_replymsg(uint8_t mode, uint8_t submode,uint8_t testitem,\r
                                 uint8_t *input_data,uint16_t input_len)\r
 {\r
@@ -45,21 +41,35 @@ errcode_t factorytest_protocol_replymsg(uint8_t mode, uint8_t submode,uint8_t te
     errcode_t ret;\r
     uint16_t total_len = input_len +1+2+1+1+1+2;//头+长度+mode+submod+testitem+crc\r
     replybuf = malloc(total_len);\r
-    if(replybuf == NULL) return ERRCODE_MALLOC;\r
-    factorytest_cmd_header_t *pcmdheader = (factorytest_cmd_header_t*)replybuf;\r
-    pcmdheader->header = BLE_TEST_PROTOCOL_HEADER;\r
-    pcmdheader->len = total_len - 2 -1 - 2; //去掉头,长度,crc\r
-    pcmdheader->mode = mode;\r
-    pcmdheader->submod = submode;\r
-    pcmdheader->testitem = testitem;\r
+    printf("input_len 0x%02X, total_len 0x%02X\n",input_len,total_len);\r
+    if(replybuf == NULL) \r
+    {\r
+        printf("malloc buf failed!\r\n");\r
+        return ERRCODE_MALLOC;\r
+    }\r
+    factorytest_cmd_header_t pcmdheader ={0};\r
+    pcmdheader.header = BLE_TEST_PROTOCOL_HEADER;\r
+    pcmdheader.len = total_len - 2 -1 - 2; //去掉头,长度,crc\r
+    pcmdheader.mode = mode;\r
+    pcmdheader.submod = submode;\r
+    pcmdheader.testitem = testitem;\r
+    replybuf[0] = pcmdheader.header;\r
+    replybuf[1] = ((uint8_t*)&pcmdheader.len)[1];\r
+    replybuf[2] = ((uint8_t*)&pcmdheader.len)[0];\r
+    replybuf[3] = pcmdheader.mode;\r
+    replybuf[4] = pcmdheader.submod;\r
+    replybuf[5] = pcmdheader.testitem;\r
     if (input_len>0)\r
         memcpy_s(replybuf+1+2+1+1+1/*header+len+mode+submod+testitem*/,input_len,input_data,input_len);\r
     //计算除了crc外的所有数据\r
-    uint16_t crc16 = modbus_crc16(replybuf,total_len - 2);\r
-    \r
-    *(replybuf+(total_len-1)) = ((uint8_t *) &crc16 )[1];\r
-    *(replybuf+(total_len-2)) = ((uint8_t *) &crc16 )[0];\r
+    uint16_t crc16 = CRC16_XMODEM(replybuf,total_len - 2);\r
     \r
+    replybuf[total_len-1] = ((uint8_t *) &crc16 )[0];\r
+    replybuf[total_len-2] = ((uint8_t *) &crc16 )[1];\r
+    for ( int i = 0; i<total_len;i++){\r
+        printf("%02X ",replybuf[i]);\r
+    }\r
+    printf("\r\n");\r
     ret = ble_factorytest_server_send_input_report(replybuf,total_len);\r
     free(replybuf);\r
     return ret;\r
@@ -71,21 +81,21 @@ errcode_t factorytest_protocol_replymsg(uint8_t mode, uint8_t submode,uint8_t te
 void factorytest_protocol_common_decode(uint8_t submode,uint8_t testitem,uint8_t* data,uint16_t len)\r
 {\r
     errcode_t ret;\r
+    printf("submode 0x%02X testitem 0x%02X\r\n",submode,testitem);\r
     switch(submode)\r
     {\r
-        printf("submode 0x%02X testitem 0x%02X\r\n",submode,testitem);\r
         case BLE_TEST_CMD_COMMON_SUB_MODE_SYSTEM_VERSION:\r
             if (testitem == 0x02)\r
             {\r
 \r
-                uint8_t buildversion[64]={0};\r
-                uint16_t versionlen = 0;\r
-                ret = uapi_get_buildversion(buildversion,&versionlen);\r
+                uint8_t buildversion[128]={0};\r
+                uint16_t versionlen = 128;\r
+                ret = uapi_get_buildversion(buildversion,versionlen);\r
                 factorytest_protocol_replymsg(BLE_TEST_CMD_MODE_COMMON,\r
                         BLE_TEST_CMD_COMMON_SUB_MODE_SYSTEM_VERSION,\r
                         0x02,\r
                         buildversion,\r
-                        versionlen);\r
+                        strlen((char*)buildversion));\r
             }\r
             break;\r
 \r
@@ -116,14 +126,14 @@ errcode_t factorytest_protocol_decode(uint8_t *buf, uint16_t event_size)
                 printf("Error 数据长度大于eventsize\r\n");\r
                 return ERRCODE_FAIL;\r
             }\r
-            uint16_t crc16 = modbus_crc16(buf,header.len+1+2);\r
+            uint16_t crc16 = CRC16_XMODEM(buf,header.len+1+2);\r
             uint16_t crc16_2 = 0;\r
             ((uint8_t *)&crc16_2)[1] = buf[event_size-2];\r
             ((uint8_t *)&crc16_2)[0] = buf[event_size-1];\r
                 \r
             printf("crc16 0x%04X vs 0x%04X\n",crc16,crc16_2);\r
 \r
-            // if( crc16 == crc16_2) //校验通过,开始正式解析\r
+            if( crc16 == crc16_2) //校验通过,开始正式解析\r
             {\r
                 printf("mode %d\n",header.mode);\r
                 switch(header.mode)\r