// возвращает рассчитанное значение CRC static uint64 crcCalculate(byte *data, uint32 datasize) { byte FieldSize = crcGetFieldSize(datasize); switch (FieldSize) { case 1: return CalcCRC8(data, datasize); case 2: return CalcCRC16(data, datasize); case 4: return CalcCRC32(data, datasize); case 8: return CalcCRC64(data, datasize); default: printf("Invalid CRCFieldSize size: %d\n", FieldSize); break; } return 0xFFFFFFFF; }
int CWLPrtclUDPServer::analyseProtocol(BYTE *out,DWORD &outlen) //从m_recvBuf包中,分析出命令 { //因为确定为UDP报文,确定一次只发一条报文,报文间有边界,无需(也不能,如:有多个客户端)拼包,故只做最简单的检查 TAPDUHeader * pHeader = NULL; TAPDUTailer * pTailer = NULL; WORD crc =0; int index = 0,cmdlen =0; BYTE tailer[sizeof(TAPDUTailer)] = {0}; outlen = NULL; LAB_HEAD: //检查数据长度是满足要求 if (m_recvbuflen < sizeof(TAPDUHeader) + sizeof(TAPDUTailer)) { m_recvbuflen = 0; return -1; //失败(不能与下一条报文拼接,因为下条报文可能来自另一个客户端) } //检查同步头是否合法 pHeader = (TAPDUHeader *) m_recvBuf; if (pHeader->syncheader != SYNC_HEADER) goto LAB_JUMP_TO_HEAD; //检查长度是否合法 cmdlen = pHeader->lc + sizeof(TAPDUHeader) + sizeof(TAPDUTailer); if (m_recvbuflen < cmdlen) goto LAB_JUMP_TO_HEAD; //检查同步尾是否合法 index = pHeader->lc + sizeof(TAPDUHeader); memcpy(tailer,&m_recvBuf[index],sizeof(TAPDUTailer)); pTailer = (TAPDUTailer *)tailer; if (pTailer->synctailer != SYNC_TAILER) goto LAB_JUMP_TO_HEAD; //检查CRC是否合法 crc = CalcCRC8(m_recvBuf,index); if (pTailer->crc != crc) goto LAB_JUMP_TO_HEAD; //成功验证出一条报文 m_recvbuflen = m_recvbuflen - cmdlen; outlen = cmdlen; memcpy(out,m_recvBuf,outlen); if (m_recvbuflen <= sizeof(TAPDUHeader) + sizeof(TAPDUTailer)) return 0; else return 1; LAB_JUMP_TO_HEAD: memcpy(&m_recvBuf[0],&m_recvBuf[1],m_recvbuflen-1); m_recvbuflen--; goto LAB_HEAD; }
/********************************************************************* 功能:发送响应 参数:pCmdIn,命令输入 pRemoteAddr,远程命令接收者 返回:返回:<0,错误;>=0,成功发送字节数 作者:叶文林 2012-11-30 *********************************************************************/ int CWLPrtclUDPServer::responseCmd(TRspCmdIn * pIn,struct sockaddr_in * pRemoteAddr) { BYTE cmd[UDPSERVER_MAX_CMD_LEN] = {0}; DWORD sendlen = 0,index = 0,len = 0; TAPDUHeader *pHeader = NULL; //报文头 BYTE *pBody = NULL; //报文体 TAPDUTailer tailer ; //报文尾 memset(cmd,0x00,sizeof(cmd)); pHeader = (TAPDUHeader * )cmd; //填充协议头 pHeader->syncheader = SYNC_HEADER; pHeader->cla = pIn->cla | CLA_RSP; pHeader->ins = pIn->ins; pHeader->p1 = pIn->p1; pHeader->p2 = pIn->p2; pHeader->sn = pIn->sn; pHeader->id = m_localID; pHeader->lc = pIn->lc; //填充协议体 index = sizeof(TAPDUHeader); pBody = cmd+index; memcpy(pBody,&(pIn->sw),2); memcpy(pBody+2,pIn->pData,pHeader->lc -2); //填充协议尾 index = sizeof(TAPDUHeader)+pHeader->lc; //报文尾位置 tailer.crc = CalcCRC8(cmd,index); tailer.synctailer = SYNC_TAILER; memcpy(cmd + index,&tailer,sizeof(TAPDUTailer)); //发送响应报文 len = sizeof(TAPDUHeader) + pHeader->lc + sizeof(TAPDUTailer); sendlen = m_pSocket->send(cmd,len,pRemoteAddr); //sendlen = send(cmd,len,pRemoteAddr); return sendlen; }
// создаёт поле CRC. Возвращает его размер static byte crcMakeField(byte *data, uint32 datasize, byte *crcbuffer) { byte FieldSize = crcGetFieldSize(datasize); switch (FieldSize) { case 1: crcbuffer[0] = CalcCRC8(data, datasize); break; case 2: { word crc16 = CalcCRC16(data, datasize); if (IsLittleEndian()) memrev(&crc16, sizeof(crc16)); memcpy(crcbuffer, &crc16, sizeof(crc16)); break; } case 4: { uint32 crc32 = CalcCRC32(data, datasize); if (IsLittleEndian()) memrev(&crc32, sizeof(crc32)); memcpy(crcbuffer, &crc32, sizeof(crc32)); break; } case 8: { uint64 crc64 = CalcCRC64(data, datasize); if (IsLittleEndian()) memrev(&crc64, sizeof(crc64)); memcpy(crcbuffer, &crc64, sizeof(crc64)); break; } default: printf("Invalid CRCFieldSize size: %d\n", FieldSize); break; } return FieldSize; }
/********************************************************************* 功能:处理命令发送消息并接收反馈消息 参数: 返回:TRUE,匹配 FALSE,不匹配 作者:唐小灿 2014-07-29 *********************************************************************/ WORD CWLPrtclUDPServer::cmd(TRqtCmdIn * pIn,BYTE * pOutData,int & outlen,int remoteID, struct sockaddr_in * pRemoteAddr) { BYTE sendbuf[UDPSERVER_MAX_CMD_LEN]; BYTE recvbuf[UDPSERVER_MAX_CMD_LEN]; TAPDUHeader * pHeader = NULL; //报文头 BYTE * pBody = NULL; //报文体 TAPDUTailer tailer = {0}; //报文尾 WORD sw; //响应状态字 static WORD sn = 0; WORD snCur = sn; //用于保存本次sn,多线程有漏洞,在snCur赋值前有线程修改sn的值则不正确 sn = (sn+1)%0xFFFF; snCur = sn; memset(sendbuf,0x00,sizeof(sendbuf)); pHeader = (TAPDUHeader * )sendbuf; //填充协议头 pHeader->syncheader = SYNC_HEADER; pHeader->cla = pIn->cla; pHeader->ins = pIn->ins; pHeader->p1 = pIn->p1; pHeader->p2 = pIn->p2; pHeader->sn = snCur;//pIn->sn; pHeader->id = m_localID; pHeader->remoteID = remoteID; pHeader->lc = pIn->lc; //填充协议体 pBody = sendbuf + sizeof(TAPDUHeader); memcpy(pBody,pIn->pData,pHeader->lc); //填充协议尾 int index = sizeof(TAPDUHeader)+pHeader->lc; //报文尾位置 tailer.crc = CalcCRC8(sendbuf,index); tailer.synctailer = SYNC_TAILER; memcpy(sendbuf + index,&tailer,sizeof(TAPDUTailer)); DWORD sendlen = sizeof(TAPDUHeader) + pHeader->lc + sizeof(TAPDUTailer); DWORD recvlen = sizeof(recvbuf); int ret = 0; int loop = 0; LAB_LOOP: ret = sendRqtRecvRsp(sendbuf,sendlen,recvbuf,recvlen,pRemoteAddr,m_defTimeoutMilSec); if (ret <= 0) { //WLOutDebug("发送报文失败,错误码 = %d",ret); WLOutDebug("发送报文失败,错误码 = %d,cla = 0x%.2X, ins = 0x%.2X",ret,pHeader->cla,pHeader->ins); return SW_NO_RSP; } //处理响应包 pHeader = (TAPDUHeader *)recvbuf; if (pHeader->sn != snCur) //非所请求的回复,重新请求 { loop++; if (loop > 3) { return SW_UNKNOWN_ERR; //未获得请求的内容 } WLOutDebug("重复请求 sn=%d,loop=%d",pHeader->sn,loop); goto LAB_LOOP; } pBody = recvbuf + sizeof(TAPDUHeader); memcpy(&sw,pBody,SW_LEN); //取状态字 if (pHeader->lc > SW_LEN) //取状态字之外的响应命令报文体 { //pOutData = pBody + SW_LEN; /********************************************************************** 修改:更新以下代码导致的内存溢出现象 作者:叶文林 2013-03-14 方试:新增代码 **********************************************************************/ outlen = pHeader->lc - SW_LEN; if (pOutData != NULL) memcpy(pOutData,pBody + SW_LEN,outlen); //******************************************************************* //以下代码有误,会有内存溢出现象,是因为将响应码和输出数据都copy入了 //输出数据区了 /********************************************************************** if (pOutData != NULL) memcpy(pOutData,pBody + SW_LEN,pHeader->lc); outlen = pHeader->lc - SW_LEN; **********************************************************************/ } else { pOutData = NULL; outlen = 0; } //填充输入头 pIn->cla = pHeader->cla; pIn->ins = pHeader->ins; pIn->p1 = pHeader->p1; pIn->p2 = pHeader->p2; pIn->sn = pHeader->sn; pIn->lc = pHeader->lc; pIn->pData = pOutData; return sw; }
int backup_or_restore_i2c(struct bma_callback *pCB) { int i2c_id_ii = 0x10; int flag_sensor_found = 0; unsigned char chip_id = 0; unsigned char crc8 = 0; unsigned char regs[BLOCK_SIZE + 1] = {0}; int ii = 0; int write_to_nvm = 0; int read_from_nvm = 0; if ((pCB == 0) || (pCB->i2c_read == 0) || (pCB->i2c_write == 0) || (pCB->fs_read == 0) || (pCB->fs_write == 0) || (pCB->msleep == 0) || (pCB->sensor_i2c_id == 0)) { pr_info("Bad parameter.\n"); return -1; } pCB->i2c_read(pCB->sensor_i2c_id, 0x00, 1, &chip_id, 1); pr_info("[BMA_INIT] -- 0X%02X : 0X%02X\n", pCB->sensor_i2c_id, chip_id); if ((chip_id & 0xFC) == 0xF8) { flag_sensor_found = 1; i2c_id_ii = pCB->sensor_i2c_id; } else { do { pCB->i2c_read(i2c_id_ii, 0x00, 1, &chip_id, 1); if ((chip_id & 0xFC) == 0xF8) { flag_sensor_found = 1; pr_info("Sensor found 2.\n"); } else { i2c_id_ii++; pr_info("[BMA_INIT] --i2c_id_ii: 0X%02X, ", i2c_id_ii); } } while ((!flag_sensor_found) && (i2c_id_ii <= 0x1F)); } if (!flag_sensor_found) { pr_info(" flag_sensor_found: %d, ", flag_sensor_found); return -1; } else { unsigned char buf = 0x00; pCB->i2c_write(i2c_id_ii, 0x11, 1, &buf, 1); pCB->msleep(20); buf = 0xAA; pCB->i2c_write(i2c_id_ii, 0x35, 1, &buf, 1); pCB->i2c_write(i2c_id_ii, 0x35, 1, &buf, 1); pCB->i2c_read(i2c_id_ii, 0x05, 1, &buf, 1); buf = (buf & 0x1F) | 0x80 ; pCB->i2c_write(i2c_id_ii, 0x05, 1, &buf, 1); pCB->fs_read("/efs/flag_sensor_backedup", &flag_sensor_backedup, 1); if (flag_sensor_backedup) { read_from_nvm = pCB->fs_read("/efs/reg_backedup", regs, BLOCK_SIZE+1); if (!read_from_nvm) { pr_info("cannot get saved data from NVM\n"); return -1; } for (ii = 0x00; ii <= 0x1A; ii++) pCB->i2c_write(pCB->sensor_i2c_id, ii, 1, ®s[ii], 1); buf = 0x0A; pCB->i2c_write(pCB->sensor_i2c_id, 0x35, 1, &buf, 1); buf = 0x0A; for (ii = 0x38; ii <= 0x3A; ii++) pCB->i2c_write(pCB->sensor_i2c_id, ii, 1, &buf, 1); } else { for (ii = 0x00; ii <= 0x1A; ii++) pCB->i2c_read(pCB->sensor_i2c_id, ii, 1, ®s[ii], 1); buf = 0x0A; pCB->i2c_write(pCB->sensor_i2c_id, 0x35, 1, &buf, 1); buf = 0x0A; for (ii = 0x38; ii <= 0x3A; ii++) pCB->i2c_write(pCB->sensor_i2c_id, ii, 1, &buf, 1); crc8 = CalcCRC8(regs, BLOCK_SIZE); if (crc8 == regs[0x1A]) { write_to_nvm = (pCB->fs_write( "/efs/reg_backedup", regs, BLOCK_SIZE+1) == BLOCK_SIZE+1); if (write_to_nvm) { flag_sensor_backedup = 1; pCB->fs_write( "/efs/flag_sensor_backedup", &flag_sensor_backedup, 1); } else { pr_info("backup to NVM failed\n"); return -1; } } else { pr_info( "CRCcheckfailed, crc8=0x%x regs[0x1A]=0x%x\n", crc8, regs[0x1A]); return -1; } } } return 0; }