From: SnickerSun <snickersun@163.com>
Date: Thu, 28 Mar 2024 08:10:18 +0000 (+0800)
Subject: 修复factorytest getversion 失败bug
X-Git-Tag: release_V1.2.70_20241020_135402~1241^2~21^2~3^2
X-Git-Url: http://git.snicker.com.cn/Users/apple/Desktop/static/gitweb.css?a=commitdiff_plain;h=df1e57951b5cb8681b929f19b6fce7dc157077b5;p=mirrors%2FFD230.git

修复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 8d3ec2dff..e54ec84d0 100644
--- 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)
 {
     if(buf != NULL)
-        printf("factory_recv_cmd_data len %d , handle 0x%X ,*buf 0x02X\r\n",event_size,handle,*buf);
+        printf("factory_recv_cmd_data len %d , handle 0x%X ,*buf 0x%02X\r\n",event_size,handle,*buf);
     /****此处为接收指令,处理后反馈结果的参考**/
     factorytest_protocol_decode(buf,event_size);
     /*****参考结束****/
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 6bafd5345..d6b2e0de7 100644
--- 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;
 #pragma pack()
 
-uint16_t modbus_crc16(uint8_t *data, uint16_t length) {  
-    uint8_t crc_hi = 0xFF;  
-    uint8_t crc_lo = 0xFF;  
-    int i;  
-  
-    while (length--) {  
-        crc_hi ^= *data++;
-        for (i = 0; i < 8; i++) {  
-            if (crc_hi & 0x80) {  
-                crc_hi = (crc_hi << 1) ^ 0x8005;  
-            } else {  
-                crc_hi <<= 1;  
-            }  
-        }  
-    }  
-  
-    return (crc_hi << 8) | crc_lo;  
-}  
- 
+static int CRC16_XMODEM(uint8_t *bytes,uint16_t length) {
+    int crc = 0x0000;          // initial value
+    int polynomial = 0x1021;
+    for (int index = 0; index < length; index++) {
+        uint8_t b = bytes[index];
+        for (int i = 0; i < 8; i++) {
+            uint8_t bit = ((b >> (7 - i) & 1) == 1);
+            uint8_t c15 = ((crc >> 15 & 1) == 1);
+            crc <<= 1;
+            if (c15 ^ bit) crc ^= polynomial;
+        }
+    }
+    crc &= 0xffff;
+    return crc;
+}
 errcode_t factorytest_protocol_replymsg(uint8_t mode, uint8_t submode,uint8_t testitem,
                                 uint8_t *input_data,uint16_t input_len)
 {
@@ -45,21 +41,35 @@ errcode_t factorytest_protocol_replymsg(uint8_t mode, uint8_t submode,uint8_t te
     errcode_t ret;
     uint16_t total_len = input_len +1+2+1+1+1+2;//头+长度+mode+submod+testitem+crc
     replybuf = malloc(total_len);
-    if(replybuf == NULL) return ERRCODE_MALLOC;
-    factorytest_cmd_header_t *pcmdheader = (factorytest_cmd_header_t*)replybuf;
-    pcmdheader->header = BLE_TEST_PROTOCOL_HEADER;
-    pcmdheader->len = total_len - 2 -1 - 2; //去掉头,长度,crc
-    pcmdheader->mode = mode;
-    pcmdheader->submod = submode;
-    pcmdheader->testitem = testitem;
+    printf("input_len 0x%02X, total_len 0x%02X\n",input_len,total_len);
+    if(replybuf == NULL) 
+    {
+        printf("malloc buf failed!\r\n");
+        return ERRCODE_MALLOC;
+    }
+    factorytest_cmd_header_t pcmdheader ={0};
+    pcmdheader.header = BLE_TEST_PROTOCOL_HEADER;
+    pcmdheader.len = total_len - 2 -1 - 2; //去掉头,长度,crc
+    pcmdheader.mode = mode;
+    pcmdheader.submod = submode;
+    pcmdheader.testitem = testitem;
+    replybuf[0] = pcmdheader.header;
+    replybuf[1] = ((uint8_t*)&pcmdheader.len)[1];
+    replybuf[2] = ((uint8_t*)&pcmdheader.len)[0];
+    replybuf[3] = pcmdheader.mode;
+    replybuf[4] = pcmdheader.submod;
+    replybuf[5] = pcmdheader.testitem;
     if (input_len>0)
         memcpy_s(replybuf+1+2+1+1+1/*header+len+mode+submod+testitem*/,input_len,input_data,input_len);
     //计算除了crc外的所有数据
-    uint16_t crc16 = modbus_crc16(replybuf,total_len - 2);
-    
-    *(replybuf+(total_len-1)) = ((uint8_t *) &crc16 )[1];
-    *(replybuf+(total_len-2)) = ((uint8_t *) &crc16 )[0];
+    uint16_t crc16 = CRC16_XMODEM(replybuf,total_len - 2);
     
+    replybuf[total_len-1] = ((uint8_t *) &crc16 )[0];
+    replybuf[total_len-2] = ((uint8_t *) &crc16 )[1];
+    for ( int i = 0; i<total_len;i++){
+        printf("%02X ",replybuf[i]);
+    }
+    printf("\r\n");
     ret = ble_factorytest_server_send_input_report(replybuf,total_len);
     free(replybuf);
     return ret;
@@ -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)
 {
     errcode_t ret;
+    printf("submode 0x%02X testitem 0x%02X\r\n",submode,testitem);
     switch(submode)
     {
-        printf("submode 0x%02X testitem 0x%02X\r\n",submode,testitem);
         case BLE_TEST_CMD_COMMON_SUB_MODE_SYSTEM_VERSION:
             if (testitem == 0x02)
             {
 
-                uint8_t buildversion[64]={0};
-                uint16_t versionlen = 0;
-                ret = uapi_get_buildversion(buildversion,&versionlen);
+                uint8_t buildversion[128]={0};
+                uint16_t versionlen = 128;
+                ret = uapi_get_buildversion(buildversion,versionlen);
                 factorytest_protocol_replymsg(BLE_TEST_CMD_MODE_COMMON,
                         BLE_TEST_CMD_COMMON_SUB_MODE_SYSTEM_VERSION,
                         0x02,
                         buildversion,
-                        versionlen);
+                        strlen((char*)buildversion));
             }
             break;
 
@@ -116,14 +126,14 @@ errcode_t factorytest_protocol_decode(uint8_t *buf, uint16_t event_size)
                 printf("Error 数据长度大于eventsize\r\n");
                 return ERRCODE_FAIL;
             }
-            uint16_t crc16 = modbus_crc16(buf,header.len+1+2);
+            uint16_t crc16 = CRC16_XMODEM(buf,header.len+1+2);
             uint16_t crc16_2 = 0;
             ((uint8_t *)&crc16_2)[1] = buf[event_size-2];
             ((uint8_t *)&crc16_2)[0] = buf[event_size-1];
                 
             printf("crc16 0x%04X vs 0x%04X\n",crc16,crc16_2);
 
-            // if( crc16 == crc16_2) //校验通过,开始正式解析
+            if( crc16 == crc16_2) //校验通过,开始正式解析
             {
                 printf("mode %d\n",header.mode);
                 switch(header.mode)