// возвращает рассчитанное значение 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;
}
