uint16_t mqtt_parse_msg_id(const uint8_t* buf) {
uint8_t type = MQTTParseMessageType(buf);
uint8_t qos = MQTTParseMessageQos(buf);
uint16_t id = 0;
//printf("mqtt_parse_msg_id\n");
if(type >= MQTT_MSG_PUBLISH && type <= MQTT_MSG_UNSUBACK) {
if(type == MQTT_MSG_PUBLISH) {
if(qos != 0) {
// fixed header length + Topic (UTF encoded)
// = 1 for "flags" byte + rlb for length bytes + topic size
uint8_t rlb = mqtt_num_rem_len_bytes(buf);
uint8_t offset = *(buf+1+rlb)<<8; // topic UTF MSB
offset |= *(buf+1+rlb+1); // topic UTF LSB
offset += (1+rlb+2); // fixed header + topic size
id = *(buf+offset)<<8; // id MSB
id |= *(buf+offset+1); // id LSB
}
} else {
// fixed header length
// 1 for "flags" byte + rlb for length bytes
uint8_t rlb = mqtt_num_rem_len_bytes(buf);
id = *(buf+1+rlb)<<8; // id MSB
id |= *(buf+1+rlb+1); // id LSB
}
}
return id;
}
uint16_t mqtt_parse_pub_msg_ptr(const uint8_t* buf, const uint8_t **msg_ptr) {
uint16_t len = 0;
//printf("mqtt_parse_pub_msg_ptr\n");
if(MQTTParseMessageType(buf) == MQTT_MSG_PUBLISH) {
// message starts at
// fixed header length + Topic (UTF encoded) + msg id (if QoS>0)
uint8_t rlb = mqtt_num_rem_len_bytes(buf);
uint8_t offset = (*(buf+1+rlb))<<8; // topic UTF MSB
offset |= *(buf+1+rlb+1); // topic UTF LSB
offset += (1+rlb+2); // fixed header + topic size
if(MQTTParseMessageQos(buf)) {
offset += 2; // add two bytes of msg id
}
*msg_ptr = (buf + offset);
// offset is now pointing to start of message
// length of the message is remaining length - variable header
// variable header is offset - fixed header
// fixed header is 1 + rlb
// so, lom = remlen - (offset - (1+rlb))
len = mqtt_parse_rem_len(buf) - (offset-(rlb+1));
} else {
*msg_ptr = NULL;
}
return len;
}
int32 MQTT_readPacket( int32 socketid,ppacket pbuf,int32 bufferLen )
{
int32 bytes_rcvd;
uint8_t *pData;
int32 messageLen;
int32 varLen;
int32 packet_length;
memset(pbuf->phead, 0, bufferLen);
pData = pbuf->phead;
bytes_rcvd = recv(socketid, pData , bufferLen, 0);
if((bytes_rcvd) <= 0)
{
MQTTclose_socket( &g_stMQTTBroker );
return -1;
}
//pData = packetBuffer + 0;
pbuf->pend=pbuf->phead+bytes_rcvd;
messageLen = mqtt_parse_rem_len(pData);
varLen = mqtt_num_rem_len_bytes(pData);
if(varLen<1 || varLen>4)
{
return -3;
}
packet_length = varLen + messageLen + 1;
if (bytes_rcvd < packet_length)
{
GAgent_Printf(GAGENT_INFO, " packet length too long %s:%d ", __FUNCTION__, __LINE__);
return -3;
}
return bytes_rcvd;
}
int read_packet(int timeout)
{
if(timeout > 0)
{
fd_set readfds;
struct timeval tmv;
// Initialize the file descriptor set
FD_ZERO (&readfds);
FD_SET (socket_id, &readfds);
// Initialize the timeout data structure
tmv.tv_sec = timeout;
tmv.tv_usec = 0;
// select returns 0 if timeout, 1 if input available, -1 if error
if(select(1, &readfds, NULL, NULL, &tmv))
return -2;
}
int total_bytes = 0, bytes_rcvd, packet_length;
memset(packet_buffer, 0, sizeof(packet_buffer));
if((bytes_rcvd = recv(socket_id, (packet_buffer+total_bytes), RCVBUFSIZE, 0)) <= 0) {
return -1;
}
total_bytes += bytes_rcvd; // Keep tally of total bytes
if (total_bytes < 2)
return -1;
// now we have the full fixed header in packet_buffer
// parse it for remaining length and number of bytes
uint16_t rem_len = mqtt_parse_rem_len(packet_buffer);
uint8_t rem_len_bytes = mqtt_num_rem_len_bytes(packet_buffer);
//packet_length = packet_buffer[1] + 2; // Remaining length + fixed header length
// total packet length = remaining length + byte 1 of fixed header + remaning length part of fixed header
packet_length = rem_len + rem_len_bytes + 1;
while(total_bytes < packet_length) // Reading the packet
{
if((bytes_rcvd = recv(socket_id, (packet_buffer+total_bytes), RCVBUFSIZE, 0)) <= 0)
return -1;
total_bytes += bytes_rcvd; // Keep tally of total bytes
}
return packet_length;
}
uint16_t mqtt_parse_pub_topic_ptr(const uint8_t* buf, const uint8_t **topic_ptr) {
uint16_t len = 0;
//printf("mqtt_parse_pub_topic_ptr\n");
if(MQTTParseMessageType(buf) == MQTT_MSG_PUBLISH) {
// fixed header length = 1 for "flags" byte + rlb for length bytes
uint8_t rlb = mqtt_num_rem_len_bytes(buf);
len = *(buf+1+rlb)<<8; // MSB of topic UTF
len |= *(buf+1+rlb+1); // LSB of topic UTF
// start of topic = add 1 for "flags", rlb for remaining length, 2 for UTF
*topic_ptr = (buf + (1+rlb+2));
} else {
*topic_ptr = NULL;
}
return len;
}
int MQTT_readPacket(uint8_t *packetBuffer, int bufferLen)
{
int bytes_rcvd;
fd_set readfds;
struct timeval tmv;
uint8_t *pData;
int messageLen;
int varLen;
int packet_length;
tmv.tv_sec = 0;
tmv.tv_usec = 0;
memset(packetBuffer, 0, bufferLen);
FD_ZERO (&readfds);
FD_SET (g_stMQTTBroker.socketid, &readfds);
if(select((g_stMQTTBroker.socketid+1), &readfds, NULL, NULL, &tmv)==-1)
{
return -2;
}
if(FD_ISSET(g_stMQTTBroker.socketid, &readfds))
{
bytes_rcvd = recv(g_stMQTTBroker.socketid, packetBuffer, bufferLen, 0);
if((bytes_rcvd) <= 0)
{
return -1;
}
pData = packetBuffer + 0; /*去掉头*/
messageLen = mqtt_parse_rem_len(pData);
varLen = mqtt_num_rem_len_bytes(pData);
packet_length = varLen + messageLen + 1;
if (bytes_rcvd < packet_length)
{
/*目前这种情况不可能发生，增加调试信息*/
GAgent_Printf(GAGENT_INFO, "goto %s:%d ", __FUNCTION__, __LINE__);
return -3;
}
return bytes_rcvd;
}
return 0;
}
err_t MqttClient::onReceive(pbuf *buf)
{
if (buf == NULL)
{
// Disconnected, close it
TcpClient::onReceive(buf);
}
else
{
if (buf->len < 1)
{
// Bad packet?
debugf("> MQTT WRONG PACKET? (len: %d)", buf->len);
close();
return ERR_OK;
}
int received = 0;
while (received < buf->tot_len)
{
int type = 0;
if (waitingSize == 0)
{
// It's begining of new packet
int pos = received;
if (posHeader == 0)
{
//debugf("start posHeader");
pbuf_copy_partial(buf, &buffer[posHeader], 1, pos);
pos++;
posHeader = 1;
}
while (posHeader > 0 && pos < buf->tot_len)
{
//debugf("add posHeader");
pbuf_copy_partial(buf, &buffer[posHeader], 1, pos);
if ((buffer[posHeader] & 128) == 0)
posHeader = 0; // Remaining Length ended
else
posHeader++;
pos++;
}
if (posHeader == 0)
{
//debugf("start len calc");
// Remaining Length field processed
uint16_t rem_len = mqtt_parse_rem_len(buffer);
uint8_t rem_len_bytes = mqtt_num_rem_len_bytes(buffer);
// total packet length = remaining length + byte 1 of fixed header + remaning length part of fixed header
waitingSize = rem_len + rem_len_bytes + 1;
type = MQTTParseMessageType(buffer);
debugPrintResponseType(type, waitingSize);
// Prevent overflow
if (waitingSize < MQTT_MAX_BUFFER_SIZE)
{
current = buffer;
buffer[waitingSize] = 0;
}
else
current = NULL;
}
else
continue;
}
int available = min(waitingSize, buf->tot_len - received);
waitingSize -= available;
if (current != NULL)
{
pbuf_copy_partial(buf, current, available, received);
current += available;
if (waitingSize == 0)
{
// Full packet received
if(type == MQTT_MSG_PUBLISH)
{
const uint8_t *ptrTopic, *ptrMsg;
uint16_t lenTopic, lenMsg;
lenTopic = mqtt_parse_pub_topic_ptr(buffer, &ptrTopic);
lenMsg = mqtt_parse_pub_msg_ptr(buffer, &ptrMsg);
// Additional check for wrong packet/parsing error
if (lenTopic + lenMsg < MQTT_MAX_BUFFER_SIZE)
{
debugf("%d: %d\n", lenTopic, lenMsg);
String topic, msg;
topic.setString((char*)ptrTopic, lenTopic);
msg.setString((char*)ptrMsg, lenMsg);
if (callback)
callback(topic, msg);
}
else
{
debugf("WRONG SIZES: %d: %d", lenTopic, lenMsg);
}
}
}
}
else
debugf("SKIP: %d (%d)", available, waitingSize + available); // To large!
received += available;
}
// Fire ReadyToSend callback
TcpClient::onReceive(buf);
}
return ERR_OK;
}
int32 Mqtt_DispatchPublishPacket( pgcontext pgc,u8 *packetBuffer,int32 packetLen )
{
u8 topic[128];
int32 topiclen;
u8 *pHiP0Data;
int32 HiP0DataLen;
int32 i;
u8 varlen=0;
u8 clientid[PHONECLIENTID + 1];
//int32 clientidlen = 0;
u8 *pTemp;
u16 cmd;
int32 sn;
u16 *pcmd=NULL;
topiclen = mqtt_parse_pub_topic(packetBuffer, topic);
//HiP0DataLen = packetLen - topiclen;
topic[topiclen] = '\0';
HiP0DataLen = mqtt_parse_publish_msg(packetBuffer, &pHiP0Data);
if(strncmp((const int8*)topic,"app2dev/",strlen("app2dev/"))==0)
{
varlen = mqtt_num_rem_len_bytes( pHiP0Data+3 );
if(varlen<1 || varlen>4)
{
return 0;
}
pcmd = (u16*)&pHiP0Data[4+varlen+1];
cmd = ntohs( *pcmd );
pTemp = &topic[strlen("app2dev/")];
i = 0;
while (*pTemp != '/')
{
i++;
pTemp++;
}
pTemp ++; /* 跳过\/ */
i=0;
while (*pTemp != '\0' && i <= PHONECLIENTID)
{
clientid[i] = *pTemp;
i++;
pTemp++;
}
if(i > PHONECLIENTID)
{
/* should handle invalid phone client id.don't ack the cmd */
i = PHONECLIENTID;
}
clientid[i]= '\0';
strcpy( pgc->rtinfo.waninfo.phoneClientId ,(const int8*)clientid );
pgc->rtinfo.waninfo.srcAttrs.cmd = cmd;
memcpy( packetBuffer,pHiP0Data,HiP0DataLen );
GAgent_Printf( GAGENT_INFO,"Cloud CMD =%04X",cmd );
if( cmd==0x0093 )
{
sn = *(int32 *)&pHiP0Data[4+varlen+1 + sizeof(cmd)];
sn = ntohl(sn);
Cloud_SetClientAttrs(pgc, clientid, cmd, sn);
}
else if( cmd == 0x0090 )
{
sn = 0;
Cloud_SetClientAttrs(pgc, clientid, cmd, sn);
}
return HiP0DataLen;
}
// 订阅最新固件响应
else if(strncmp((const int8*)topic,"ser2cli_res/",strlen("ser2cli_res/"))==0)
{
pcmd = (u16*)&pHiP0Data[4];
cmd = ntohs( *pcmd );
// pHiP0Data消息体的指针
// HiP0DataLen消息体的长度 packetBuffer
switch(cmd)
{
/* V4.1 Don't use this cmd */
case 0x020e:
break;
// wan client on line numbers res.
case 0x0210:
Mqtt_ResOnlineClient( pgc,(int8*)pHiP0Data, HiP0DataLen);
break;
case 0x0211:
//todo MCU OTA.
GAgent_Printf( GAGENT_DEBUG,"M2M cmd to check OTA!!! ");
GAgent_SetCloudConfigStatus( pgc,CLOUD_RES_GET_SOFTVER);
break;
default:
break;
}
return 0;
}
return 0;
}
void DispatchTCPData( int nSocket, u8 *pData,int datalength)
{
int varlen;
int datalen;
u8 cmd;
u8 TcpCmd[2];
int i;
int sendLen;
unsigned char *pP0Data;
if( !(pData[0]==0x00&&pData[1]==0x00&&pData[2]==0x00&&pData[3]==0x03)) return ;
/*根据报文中的报文长度确定报文是否是一个有效的报文*/
varlen = mqtt_num_rem_len_bytes(pData+4);
/*这个地方+3是因为MQTT库里面实现把 UDP flag算到messagelen里面，这里为了跟mqtt库保持一致所以加3*/
datalen = mqtt_parse_rem_len(pData+3);
cmd = pData[7+varlen-1];
GAgent_Printf(GAGENT_INFO,"LAN_TCP Receive Data cmd:%2X\n", cmd);
switch(cmd)//get cmd
{
case 0x0006: //user bind passcode
if( g_passcodeEnable==1 )
{
handlePasscode( nSocket,pData, datalength);
}
break;
case 0x0008: //user login
handleLogin( nSocket,pData, datalength);
break;
case 0x0090: // send p0 to uart
for( i=0;i<8;i++ )
{
if( nSocket==g_SocketLogin[i] )
{
pP0Data = pData+(7+varlen);
//datalen-3 才是P0的长度因为：datalen后面有falg(1B)+cmd(2B);
LAN_handleMCUData( nSocket,pP0Data, datalen-3);
break;
}
}
break;
case 0x000A:
GetWifiVersion( nSocket,pData,datalength );
break;
case 0x000C:
GetWifiHotspots( nSocket,pData,datalength );
break;
//串口配置
case 0x000E:
break;
//日志设置
case 0x0010:
break;
//请求wifi模组信息
case 0X0013:
GetWifiInfo( nSocket );
break;
//心跳包
case 0X0015:
LanAppTick_Ack( nSocket );
break;
default:
GAgent_Printf(GAGENT_WARNING,"DispatchTCPData invalid cmd:%2x\n", cmd);
break;
}
return;
}
/***************************************************
FunctionName : ParsePacket.
Description : set the source phead ppayload
pend.
pbug : data source struct.
return : 0 ok other fail.
Add by Alex.lin --2015-03-21
***************************************************/
uint32 ParsePacket( ppacket pRxBuf )
{
int32 varlen=0;
int32 datalen=0;
uint16 cmd=0;
uint16 *pcmd=NULL;
GAgent_Printf(GAGENT_DEBUG,"\r\n");
GAgent_Printf(GAGENT_DEBUG,"IN %s packet type : %04x",__FUNCTION__ ,pRxBuf->type );
if( ((pRxBuf->type)&(CLOUD_DATA_IN)) == CLOUD_DATA_IN )
{
datalen = mqtt_parse_rem_len( pRxBuf->phead+3 );
varlen = mqtt_num_rem_len_bytes( pRxBuf->phead+3 );
pcmd = (u16*)&(pRxBuf->phead[4+varlen+1]);
cmd = ntohs( *pcmd );
GAgent_Printf( GAGENT_INFO,"CLOUD_DATA_IN cmd : %04X", cmd );
if( cmd == 0x0090 )
{
pRxBuf->ppayload = pRxBuf->phead+4+varlen+1+2;
}
if( cmd ==0x0093 )
{//with sn.
pRxBuf->ppayload = pRxBuf->phead+4+varlen+1+2+4;
}
pRxBuf->pend = pRxBuf->phead+4+varlen+datalen;
GAgent_Printf( GAGENT_DEBUG," ReSet Data Type : %04X - CLOUD_DATA_IN", pRxBuf->type );
pRxBuf->type = SetPacketType( pRxBuf->type,CLOUD_DATA_IN,0 );
pRxBuf->type = SetPacketType( pRxBuf->type,LOCAL_DATA_OUT,1 );
GAgent_Printf( GAGENT_DEBUG," Set Data Type : %04X - LOCAL_DATA_OUT", pRxBuf->type );
}
else if( ((pRxBuf->type)&(LOCAL_DATA_IN)) == LOCAL_DATA_IN )
{
/* head(0xffff)| len(2B) | cmd(1B) | sn(1B) | flag(2B) | payload(xB) | checksum(1B) */
pRxBuf->ppayload = pRxBuf->phead+8; /* head + len + cmd + sn + flag */
datalen = ( (int32)ntohs( *(uint16 *)(pRxBuf->phead + 2) ) ) & 0xffff;
pRxBuf->pend = (pRxBuf->phead )+( datalen+4 ); /* datalen + head + len */
GAgent_Printf( GAGENT_DEBUG," ReSet Data Type : %04X - LOCAL_DATA_IN", pRxBuf->type );
pRxBuf->type = SetPacketType( pRxBuf->type,LOCAL_DATA_IN,0 );
pRxBuf->type = SetPacketType( pRxBuf->type,CLOUD_DATA_OUT,1 );
pRxBuf->type = SetPacketType( pRxBuf->type,LAN_TCP_DATA_OUT,1 );
GAgent_Printf( GAGENT_DEBUG," Set Data Type : %04X - CLOUD_DATA_OUT & LAN_TCP_DATA_OUT ",pRxBuf->type );
}
else if( ((pRxBuf->type)&(LAN_TCP_DATA_IN)) == LAN_TCP_DATA_IN )
{
datalen = mqtt_parse_rem_len( pRxBuf->phead+3 );
varlen = mqtt_num_rem_len_bytes( pRxBuf->phead+3 );
pRxBuf->ppayload = pRxBuf->phead + LAN_PROTOCOL_HEAD_LEN + varlen + LAN_PROTOCOL_FLAG_LEN + LAN_PROTOCOL_CMD_LEN;
pRxBuf->pend = pRxBuf->phead + LAN_PROTOCOL_HEAD_LEN + varlen + datalen;
GAgent_Printf( GAGENT_DEBUG," ReSet Data Type : %04X - LAN_TCP_DATA_IN", pRxBuf->type );
pRxBuf->type = SetPacketType( pRxBuf->type,LAN_TCP_DATA_IN,0 );
pRxBuf->type = SetPacketType( pRxBuf->type,LOCAL_DATA_OUT,1 );
GAgent_Printf( GAGENT_DEBUG," Set Data Type : %04X - LOCAL_DATA_OUT", pRxBuf->type );
}
else
{
GAgent_Printf( GAGENT_DEBUG,"Data Type error,wite :%04X ", pRxBuf->type );
return 1;
}
GAgent_Printf( GAGENT_DEBUG,"OUT packet type : %04X\r\n",pRxBuf->type );
return 0;
}
void GAgent_Socket_DoUDPServer(void)
{
int readnum;
int ret;
fd_set readfds, exceptfds;
struct sockaddr_t addr;
socklen_t addrLen = sizeof(struct sockaddr_t);
int messagelen; /*报文长度*/
int varlen; /*可变数据长度字段的长度*/
struct timeval_t t;
t.tv_sec = 0;//秒
t.tv_usec = 0;//微秒
if( g_GAgent_UDPServerFd <=0 )
{
return ;
}
FD_ZERO(&readfds);
FD_SET(g_GAgent_UDPServerFd, &readfds);
ret = select((g_GAgent_UDPServerFd+1), &readfds, NULL, &exceptfds, &t);
if(ret<0)
{
return ;
}
memset(g_GAgent_stSocketRecBuffer, 0x0, SOCKET_RECBUFFER_LEN);
if( FD_ISSET(g_GAgent_UDPServerFd, &readfds) )
{
readnum = GAgent_Socket_recvfrom(g_GAgent_UDPServerFd, g_GAgent_stSocketRecBuffer, SOCKET_RECBUFFER_LEN, &addr, &addrLen);
if (readnum <= 0)
{
return;
}
/*根据报文中的报文长度确定报文是否是一个有效的报文*/
varlen = mqtt_num_rem_len_bytes(g_GAgent_stSocketRecBuffer+4);
//这个地方+3是因为MQTT库里面实现把 UDP flag算到messagelen里面，这里为了跟mqtt库保持一致所以加3
messagelen = mqtt_parse_rem_len(g_GAgent_stSocketRecBuffer+3);
if ((messagelen+varlen+XPG_PACKET_VERSIONLEN) != readnum)
{
/*报文长度错误*/
GAgent_Printf(GAGENT_WARNING, "Invalid UDP packet length");
return;
}
if (readnum < SOCKET_RECBUFFER_LEN)
{
GAgent_Socket_DispatchUDPRecvData(g_GAgent_stSocketRecBuffer, varlen, messagelen, addr, addrLen);
return;
}
if (readnum >= SOCKET_RECBUFFER_LEN)
{
/*根据目前的情况，不可能出现这个问题。增加调试信息*/
GAgent_Printf(GAGENT_WARNING, "TOO LENGTH OF UDP Packet Size.");
;
}
}
return;
}