int main(int argc, char **argv)
{
int sockfd, new_fd,numbytes; // listen on sock_fd, new connection on new_fd
char buf[MAXDATASIZE];
struct addrinfo hints, *servinfo, *p;
struct sockaddr_storage their_addr; // connector's address information
socklen_t sin_size;
struct sigaction sa;
int yes=1;
char s[INET6_ADDRSTRLEN];
int rv,tempType=0;
pid_t tempPid;
PACKET tempPacket;
if (argc==2 && (strcmp("-d",argv[1])==0)) {
printf("== Daemon mode selected.==\n");
daemonFlag=1;
}
if (daemonFlag) {
// Become a daemon:
switch (fork ())
{
case -1: // can't fork
perror ("fork()");
exit (0);
case 0: // child, process becomes a daemon:
close (STDIN_FILENO);
close (STDOUT_FILENO);
close (STDERR_FILENO);
if (setsid () == -1) // request a new session (job control)
{
exit (0);
}
break;
default: // parent returns to calling process:
return 0;
}
// Establish signal handler to clean up before termination:
if (signal (SIGTERM, termination_handler) == SIG_IGN)
signal (SIGTERM, SIG_IGN);
signal (SIGINT, SIG_IGN);
signal (SIGHUP, SIG_IGN);
}
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE; // use my IP
if ((rv = getaddrinfo(NULL, PORT, &hints, &servinfo)) != 0) {
fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
return 1;
}
// loop through all the results and bind to the first we can
for(p = servinfo; p != NULL; p = p->ai_next) {
if ((sockfd = socket(p->ai_family, p->ai_socktype,
p->ai_protocol)) == -1) {
perror("server: socket");
continue;
}
if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &yes,
sizeof(int)) == -1) {
perror("setsockopt");
exit(1);
}
if (bind(sockfd, p->ai_addr, p->ai_addrlen) == -1) {
close(sockfd);
perror("server: bind");
continue;
}
break;
}
if (p == NULL) {
fprintf(stderr, "server: failed to bind\n");
return 2;
}
freeaddrinfo(servinfo); // all done with this structure
if (listen(sockfd, BACKLOG) == -1) {
perror("listen");
exit(1);
}
sa.sa_handler = sigchld_handler; // reap all dead processes
sigemptyset(&sa.sa_mask);
sa.sa_flags = SA_RESTART;
if (sigaction(SIGCHLD, &sa, NULL) == -1) {
perror("sigaction");
exit(1);
}
printf("server(%d): waiting for connections...\n",getpid());
while(keep_going)
{ // main accept() loop
sin_size = sizeof their_addr;
new_fd = accept(sockfd, (struct sockaddr *)&their_addr, &sin_size);
if (new_fd == -1) {
perror("accept");
continue;
}
inet_ntop(their_addr.ss_family,
get_in_addr((struct sockaddr *)&their_addr),
s, sizeof s);
printf("server(%d): got connection from %s\n", getpid(),s);
if (!fork()) { // this is the child process
close(sockfd); // child doesn't need this descriptor
openlog("DaemonServer",LOG_PID,LOG_SYSLOG);
while (tempType!=2) {//child stops when client deregisters
//recv packet
if (recv(new_fd, (void*)&tempPacket,
sizeof(tempPacket), 0) == -1) {
perror("recv");
break;
}
tempType=tempPacket.packet_head.type;
tempPid=tempPacket.packet_head.sender_pid;
if (tempType<=3) {
printf("server(%d): received msg type:'%d' from %s-client(%d) \n",getpid(),
tempType,tempPacket.packet_head.mid,tempPid);
syslog(LOG_NOTICE, "server(%d): received msg type:'%d' from %s-client(%d) \n",getpid(),
tempType,tempPacket.packet_head.mid,tempPid);
if (tempType==MSG_HEAD_NORMAL) {
printf("server(%d): received following msg string from %s-client(%d): %s \n",getpid(),tempPacket.packet_head.mid,tempPid,tempPacket.packet_body.charbody);
syslog(LOG_NOTICE, "server(%d): received following msg string from %s-client(%d): %s \n",getpid(),tempPacket.packet_head.mid,tempPid,tempPacket.packet_body.charbody);
}
//generate ACKs
printf("server(%d): sent ACK type:'%d' to %s-client(%d)\n",getpid(),
tempType+3,tempPacket.packet_head.mid,tempPid);
syslog(LOG_NOTICE, "server(%d): received following msg string from %s-client(%d): %s \n",getpid(),tempPacket.packet_head.mid,tempPid,tempPacket.packet_body.charbody);
generatePacket(&tempPacket, tempType+3, getpid(), "Server");
//send acks
if (send(new_fd, (void*)&tempPacket, sizeof(tempPacket), 0) == -1) {
perror("send");
break;
}
}
}
closelog();
close(new_fd);
exit(0);
}
close(new_fd); // parent doesn't need this
}
return 0;
}
int main(int argc, char *argv[])
{
int sockfd;
struct sockaddr_in theiraddr;
struct hostent *he;
uint8_t mac[6] = {0, 0, 0, 0, 0, 0};
uint8_t packet[102] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
int broadcast = 1;
if (argc != 3)
{
fprintf(stderr, "Usage: %s <host> <mac>\n", argv[0]);
exit(1);
}
if ((he = gethostbyname(argv[1])) == NULL)
{
perror("gethostbyname");
exit(1);
}
if ((sockfd = socket(PF_INET, SOCK_DGRAM, 0)) == -1)
{
perror("socket");
exit(1);
}
if (setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) == -1)
{
perror("setsockopt (SO_BROADCAST)");
exit(1);
}
memset(&theiraddr, 0, sizeof(theiraddr));
theiraddr.sin_family = PF_INET;
theiraddr.sin_port = htons(PORT);
theiraddr.sin_addr = *((struct in_addr *) he->h_addr);
sscanf(argv[2], "%02"SCNx8":%02"SCNx8":%02"SCNx8":%02"SCNx8":%02"SCNx8":%02"SCNx8, &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5]);
generatePacket(mac, packet);
if ((sendto(sockfd, packet, sizeof(packet), 0, (struct sockaddr *)&theiraddr, sizeof(struct sockaddr))) == -1)
{
perror("sendto");
exit(1);
}
printf("Sent a wakeup call to %s.\n", inet_ntoa(theiraddr.sin_addr));
close(sockfd);
return 0;
}
void SSLWorld::sendVisionBuffer()
{
int t = timer->elapsed();
sendQueue.push_back(new SendingPacket(generatePacket(),t));
while (t - sendQueue.front()->t>=cfg->sendDelay())
{
SSL_WrapperPacket *packet = sendQueue.front()->packet;
delete sendQueue.front();
sendQueue.pop_front();
visionServer->send(*packet);
delete packet;
if (sendQueue.isEmpty()) break;
}
}
DS2Packet& DS2Packet::setData(const uint8_t* data, uint16_t length)
{
if(length)
{
delete[] dataPtr;
dataPtr = new uint8_t[length];
memcpy(dataPtr, data, length);
}
this->dataLength = length;
if(type == DS2_8BIT)
this->packetLength = length + 3;
else if(type == DS2_16BIT)
this->packetLength = length + 4;
else
throw;
generatePacket();
return *this;
}
//数据段转为data
_xai_packet* generatePacketFromParamDataList(_xai_packet_param_data* ctrl_param_data , int count){
_xai_packet* packet = generatePacket();
void* data_load = malloc(1000);
memset(data_load, 0, 1000);
int dataPos = 0;
for (int i = 0; i < count; i++) {
_xai_packet_param_data* ctrl_data = paramDataAtIndex(ctrl_param_data, i);
if (NULL == ctrl_data) {
free(data_load);
purgePacket(packet);
printf("CTRL DATA NULL");
//abort();
return NULL;
}
_xai_packet* ctrl_data_packet = generatePacketFromeDataOne(ctrl_data);
memcpy(data_load + dataPos, ctrl_data_packet->all_load, ctrl_data_packet->size);
dataPos += ctrl_data_packet->size;
purgePacket(ctrl_data_packet);
}
packet->all_load = data_load;
packet->size = dataPos;
return packet;
}
_xai_packet* generatePacketFromeDataOne(_xai_packet_param_data* ctrl_param_data){
if (ctrl_param_data == NULL) return NULL;
_xai_packet* ctrl_data = generatePacket();
char* payload = malloc(1000);
memset(payload,0,1000);
if(!payload) return NULL;
//big
uint16_t big_len = CFSwapInt16(ctrl_param_data->data_len);
//存入固定格式
param_to_packet_helper(payload, &ctrl_param_data->data_type, _XPP_CD_TYPE_START, _XPP_CD_TYPE_END);
param_to_packet_helper(payload, &big_len, _XPP_CD_LEN_START, _XPP_CD_LEN_END);
ctrl_data->pre_load = malloc(_XPPS_CD_FIXED_ALL);
memcpy(ctrl_data->pre_load, payload, _XPPS_CD_FIXED_ALL);
int pos = _XPPS_CD_FIXED_ALL;
if (NULL != ctrl_param_data->data) {
/*type 大端 小端转化*/
void* in_data = malloc(ctrl_param_data->data_len);
memset(in_data, 0, ctrl_param_data->data_len);
memcpy(in_data, ctrl_param_data->data, ctrl_param_data->data_len);
if (XAI_DATA_TYPE_BIN_DIGITAL_UNSIGN == ctrl_param_data->data_type) {
SwapBytes(in_data, ctrl_param_data->data_len);
}
if (XAI_DATA_TYPE_BIN_APSN == ctrl_param_data->data_type) {
SwapBytes(in_data, ctrl_param_data->data_len);
}
if (XAI_DATA_TYPE_BIN_LUID == ctrl_param_data->data_type) {
SwapBytes(in_data, ctrl_param_data->data_len);
}
ctrl_data->data_load = malloc(ctrl_param_data->data_len);
memset(ctrl_data->data_load, 0, ctrl_param_data->data_len);
memcpy(ctrl_data->data_load, in_data, ctrl_param_data->data_len);
//param_to_packet_helper(payload, in_data, 0 , 0 + ctrl_param_data->data_len);
memcpy(payload+pos, in_data, ctrl_param_data->data_len);
pos += ctrl_param_data->data_len;
free(in_data);
in_data = NULL;
}else{
ctrl_data->data_load = NULL;
}
ctrl_data->all_load = malloc(pos);
memcpy(ctrl_data->all_load, payload, pos);
ctrl_data->size = pos;
free(payload);
return ctrl_data;
}
int main(int argc, char *argv[])
{
int sockfd, numbytes;
char buf[MAXDATASIZE];
struct addrinfo hints, *servinfo, *p;
int rv;
char s[INET6_ADDRSTRLEN];
PACKET tempPacket;
if (argc != 3) {
fprintf(stderr,"usage: client [hostname] [mnemonic name]\n");
exit(1);
}
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
if ((rv = getaddrinfo(argv[1], PORT, &hints, &servinfo)) != 0) {
fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
return 1;
}
// loop through all the results and connect to the first we can
for(p = servinfo; p != NULL; p = p->ai_next) {
if ((sockfd = socket(p->ai_family, p->ai_socktype,
p->ai_protocol)) == -1) {
perror("client: socket");
continue;
}
if (connect(sockfd, p->ai_addr, p->ai_addrlen) == -1) {
close(sockfd);
perror("client: connect");
continue;
}
break;
}
if (p == NULL) {
fprintf(stderr, "client: failed to connect\n");
return 2;
}
inet_ntop(p->ai_family, get_in_addr((struct sockaddr *)p->ai_addr),
s, sizeof s);
printf("client: connecting to %s\n", s);
freeaddrinfo(servinfo); // all done with this structure
//send reg
generatePacket(&tempPacket, MSG_HEAD_REG, getpid(), argv[2]);
if (send(sockfd, (void*)&tempPacket, sizeof(tempPacket), 0) == -1)
perror("send");
//recv reg_ack
if ((numbytes = recv(sockfd, (void*)&tempPacket, sizeof(tempPacket), 0)) == -1) {
perror("recv");
exit(1);
}
if (tempPacket.packet_head.type==MSG_HEAD_REG_ACK) {
printf("client(%d): received ACK for reg from server(%d)\n",getpid(),tempPacket.packet_head.sender_pid);
}
printf("Press any key to send msg: 'Hello Server!'");
getchar();
//Send msg
generatePacket(&tempPacket, MSG_HEAD_NORMAL, getpid(), argv[2]);
addMsg(&tempPacket,"Hello Server!");
if (send(sockfd, (void*)&tempPacket, sizeof(tempPacket), 0) == -1)
perror("send");
//recv msg_ack
if ((numbytes = recv(sockfd, (void*)&tempPacket, sizeof(tempPacket), 0)) == -1) {
perror("recv");
exit(1);
}
if (tempPacket.packet_head.type==MSG_HEAD_NORMAL_ACK) {
printf("client(%d): received ACK for msg from server(%d)\n",getpid(),tempPacket.packet_head.sender_pid);
}
printf("Press any key to deregister.");
getchar();
//send dereg
generatePacket(&tempPacket, MSG_HEAD_DEREG, getpid(), argv[2]);
if (send(sockfd, (void*)&tempPacket, sizeof(tempPacket), 0) == -1)
perror("send");
//recv dereg_ack
if ((numbytes = recv(sockfd, (void*)&tempPacket, sizeof(tempPacket), 0)) == -1) {
perror("recv");
exit(1);
}
if (tempPacket.packet_head.type==MSG_HEAD_DEREG_ACK) {
printf("client(%d): received ACK for dereg from server(%d)\n",getpid(),tempPacket.packet_head.sender_pid);
}
close(sockfd);
return 0;
}
void SimThread::run()
{
init = true;
currentLocationPacket=0;
currentLocationInfoPacket=0;
QFile logfile(m_filename);
if (!logfile.open(QIODevice::ReadOnly))
{
qDebug() << "Unable to open file. Exiting sim thread";
return;
}
QByteArray logbytes = logfile.readAll();
nextPacketIsLocation = false;
QByteArray qbuffer;
QList<QByteArray> m_queuedMessages;
QString byteoutofpacket;
bool m_inpacket = false;
bool m_inescape = false;
for (int i=0;i<logbytes.size();i++)
{
if ((unsigned char)logbytes[i] == 0xAA)
{
if (m_inpacket)
{
//Start byte in the middle of a packet
//Clear out the buffer and start fresh
m_inescape = false;
qbuffer.clear();
}
//qbuffer.append(buffer[i]);
//qDebug() << "Start of packet";
//Start of packet
m_inpacket = true;
}
else if ((unsigned char)logbytes[i] == 0xCC && m_inpacket)
{
//qDebug() << "End of packet. Size:" << qbuffer.size();
//End of packet
m_inpacket = false;
//qbuffer.append(buffer[i]);
//m_logFile->flush();
//emit parseBuffer(qbuffer);
//New Location of checksum
unsigned char sum = 0;
for (int i=0;i<qbuffer.size()-1;i++)
{
sum += qbuffer[i];
}
//qDebug() << "Payload sum:" << QString::number(sum);
//qDebug() << "Checksum sum:" << QString::number((unsigned char)currPacket[currPacket.length()-1]);
if (sum != (unsigned char)qbuffer[qbuffer.size()-1])
{
qDebug() << "BAD CHECKSUM!";
//return QPair<QByteArray,QByteArray>();
}
else
{
m_queuedMessages.append(qbuffer.mid(0,qbuffer.length()-1));
}
//return qbuffer;
QString output;
for (int i=0;i<qbuffer.size();i++)
{
int num = (unsigned char)qbuffer[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
//qDebug() << "Full packet:";
//qDebug() << output;
qbuffer.clear();
}
else
{
if (m_inpacket && !m_inescape)
{
if ((unsigned char)logbytes[i] == 0xBB)
{
//Need to escape the next byte
//retval = logfile.read(1);
m_inescape = true;
}
else
{
qbuffer.append(logbytes[i]);
}
}
else if (m_inpacket && m_inescape)
{
if ((unsigned char)logbytes[i] == 0x55)
{
qbuffer.append((char)0xAA);
}
else if ((unsigned char)logbytes[i] == 0x44)
{
qbuffer.append((char)0xBB);
}
else if ((unsigned char)logbytes[i] == 0x33)
{
qbuffer.append((char)0xCC);
}
else
{
qDebug() << "Error, escaped character is not valid!:" << QString::number(logbytes[i],16);
}
m_inescape = false;
}
else
{
//qDebug() << "Byte out of a packet:" << QString::number(buffer[i],16);
byteoutofpacket += QString::number(logbytes[i],16) + " ";
}
}
}
//qDebug() << "Bytes out of a packet:" << byteoutofpacket;
//Done.
qDebug() << m_queuedMessages.size() << "Messages read in";
for (int i=0;i<m_queuedMessages.size();i++)
{
Packet parsedPacket = parseBuffer(m_queuedMessages[i]);
packetList.append(parsedPacket);
//parsePacket(parsedPacket);
}
int fd=0;
#ifdef HAVE_POSIX_OPENPT
fd = posix_openpt(O_RDWR | O_NOCTTY);
#else
fd = open("/dev/ptmx",O_RDWR | O_NOCTTY);
#endif //HAVE_POSIX_OPENPT
if(-1 == fd) {
#ifdef HAVE_POSIX_OPENPT
perror("Error in posix_openpt");
#else
perror("Error opening /dev/ptmx");
#endif //HAVE_POSIX_OPENPT
return;
}
grantpt(fd);
unlockpt(fd);
struct termios oldtio;
if(0 != tcgetattr(fd,&oldtio)) {
perror("tcgetattr openpt warning");
}
//bzero(&newtio,sizeof(newtio));
oldtio.c_cflag = CS8 | CLOCAL | CREAD; // CBAUD
oldtio.c_iflag = IGNPAR | ICRNL;
oldtio.c_oflag = 0;
oldtio.c_lflag = ICANON & (~ECHO);
oldtio.c_cc[VEOL] = '\r';
// oldtio.c_cc[VEOL2] = 0; /* '\0' */
tcflush(fd,TCIFLUSH);
if(0 != tcsetattr(fd,TCSANOW,&oldtio)) {
perror("tcsetattr warning");
}
#ifdef HAVE_PTSNAME_R
if(0 != ptsname_r(fd, portname, sizeof(portname))) {
perror("Couldn't get pty slave name");
}
#else
char *ptsname_val = ptsname(fd);
if(NULL == ptsname_val) {
perror("Couldn't get pty slave name");
}
QString portname = QString(ptsname_val);
//strncpy(portname, ptsname_val, sizeof(portname));
#endif //HAVE_PTSNAME_R
fcntl(fd,F_SETFL,O_NONBLOCK); // O_NONBLOCK + fdopen/stdio == bad
qDebug() << "Port:" << portname << "opened for sim!";
unsigned char buffer[1024];
unsigned short nextlocationid;
bool nextislocation = false;
init = false;
while (true)
{
int readlen = read(fd,buffer,1024);
if (readlen < 0)
{
//Nothing on the port
usleep(10000);
}
if (readlen == 0)
{
usleep(10000);
}
for (int i=0;i<readlen;i++)
{
if (buffer[i] == 0xAA)
{
if (m_inpacket)
{
//Start byte in the middle of a packet
//Clear out the buffer and start fresh
m_inescape = false;
qbuffer.clear();
}
//qbuffer.append(buffer[i]);
//qDebug() << "Start of packet";
//Start of packet
m_inpacket = true;
}
else if (buffer[i] == 0xCC && m_inpacket)
{
//qDebug() << "End of packet. Size:" << qbuffer.size();
//End of packet
m_inpacket = false;
//qbuffer.append(buffer[i]);
//m_logFile->flush();
//emit parseBuffer(qbuffer);
//New Location of checksum
unsigned char sum = 0;
for (int i=0;i<qbuffer.size()-1;i++)
{
sum += qbuffer[i];
}
//qDebug() << "Payload sum:" << QString::number(sum);
//qDebug() << "Checksum sum:" << QString::number((unsigned char)currPacket[currPacket.length()-1]);
if (sum != (unsigned char)qbuffer[qbuffer.size()-1])
{
qDebug() << "BAD CHECKSUM!";
//return QPair<QByteArray,QByteArray>();
}
else
{
QByteArray fullmsg = qbuffer.mid(0,qbuffer.length()-1);
Packet p = parseBuffer(fullmsg);
if (p.payloadid == 0x0104 || p.payloadid == 0x0106)
{
Packet p2 = locationidList[currentLocationPacket++];
QByteArray tosend = generatePacket(p2.header,p2.payload);
write(fd,tosend.data(),tosend.length());
}
else if (p.payloadid == 0xF8E0)
{
Packet p2 = locationidInfoList[currentLocationInfoPacket++];
QByteArray tosend = generatePacket(p2.header,p2.payload);
write(fd,tosend.data(),tosend.length());
}
/*if (p.payloadid == 0x0106 || p.payloadid == 0x0108)
{
//Request for ram.
if (nextislocation)
{
nextislocation = false;
unsigned short lookforpayload = p.payloadid+1;
for (int i=0;i<packetList.size();i++)
{
if (packetList[i].payloadid == lookforpayload)
{
unsigned short tmp = (((unsigned char)p.payload[0]) << 8) + (unsigned char)p.payload[1];
//if (tmp == nextlocationid)
//{
QByteArray tosend = generatePacket(packetList[i].header,packetList[i].payload);
write(fd,tosend.data(),tosend.length());
break;
//}
}
}
}
}*/
else
{
unsigned short lookforpayload = p.payloadid+1;
for (int i=0;i<packetList.size();i++)
{
if (packetList[i].payloadid == lookforpayload)
{
QByteArray tosend = generatePacket(packetList[i].header,packetList[i].payload);
write(fd,tosend.data(),tosend.length());
break;
}
}
}
}
//return qbuffer;
QString output;
for (int i=0;i<qbuffer.size();i++)
{
int num = (unsigned char)qbuffer[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
//qDebug() << "Full packet:";
//qDebug() << output;
qbuffer.clear();
}
else
{
if (m_inpacket && !m_inescape)
{
if (buffer[i] == 0xBB)
{
//Need to escape the next byte
//retval = logfile.read(1);
m_inescape = true;
}
else
{
qbuffer.append(buffer[i]);
}
}
else if (m_inpacket && m_inescape)
{
if (buffer[i] == 0x55)
{
qbuffer.append((char)0xAA);
}
else if (buffer[i] == 0x44)
{
qbuffer.append((char)0xBB);
}
else if (buffer[i] == 0x33)
{
qbuffer.append((char)0xCC);
}
else
{
qDebug() << "Error, escaped character is not valid!:" << QString::number(buffer[i],16);
}
m_inescape = false;
}
else
{
//qDebug() << "Byte out of a packet:" << QString::number(buffer[i],16);
byteoutofpacket += QString::number(buffer[i],16) + " ";
}
}
}
//qDebug() << "Bytes out of a packet:" << byteoutofpacket;
}
}
_xai_packet* generatePacketFromParamNormal(_xai_packet_param_normal* normal_param){
_xai_packet* packet = generatePacket();
uint8_t* payload = malloc(1000);
memset(payload,0,1000);
if(!payload){
return NULL;
}
uint16_t big_msgid = CFSwapInt16(normal_param->msgid);
uint16_t big_magic_number = CFSwapInt16(normal_param->magic_number);
uint16_t big_length = CFSwapInt16(normal_param->length);
void* big_from = generateSwapGUID(normal_param->from_guid);
void* big_to = generateSwapGUID(normal_param->to_guid);
//存入固定格式
param_to_packet_helper(payload, big_from,_XPP_N_FROM_GUID_START,_XPP_N_FROM_GUID_END);
param_to_packet_helper(payload, big_to ,_XPP_N_TO_GUID_START,_XPP_N_TO_GUID_END);
param_to_packet_helper(payload, &normal_param->flag, _XPP_N_FLAG_START, _XPP_N_FLAG_END);
param_to_packet_helper(payload, &big_msgid, _XPP_N_MSGID_START, _XPP_N_MSGID_END);
param_to_packet_helper(payload, &big_magic_number, _XPP_N_MAGIC_NUMBER_START, _XPP_N_MAGIC_NUMBER_END);
param_to_packet_helper(payload, &big_length, _XPP_N_LENGTH_START, _XPP_N_LENGTH_END);
purgeGUID(big_from);
purgeGUID(big_to);
/*fixed size */
packet->fix_pos = _XPPS_N_FIXED_ALL; //固定格式位置
unsigned int pos = _XPPS_N_FIXED_ALL;
/*preload --- fixed*/
packet->pre_load = malloc(pos);
memset(packet->pre_load, 0, pos);
memcpy(packet->pre_load, payload, pos);
/*allload*/
if (NULL != normal_param->data && 0 != normal_param->length){
packet->data_load = malloc(normal_param->length);
memset(packet->data_load, 0, normal_param->length);
memcpy(packet->data_load, normal_param->data, normal_param->length);
param_to_packet_helper(payload, normal_param->data, _XPPS_N_FIXED_ALL, _XPPS_N_FIXED_ALL+normal_param->length);
memcpy(payload+pos, normal_param->data, normal_param->length);
pos += normal_param->length;
}else{
packet->data_load = NULL;
}
//复制内存到all
packet->all_load = malloc(pos);
memset(packet->all_load, 0, pos);
memcpy(packet->all_load,payload,pos);
packet->size = pos;
free(payload);
return packet;
}
DS2PStatus DS2Packet::packetFromRawPacket(const uint8_t* rawPacketData, uint16_t length, DS2PacketType type)
{
int16_t dataLength;
uint8_t checksum;
this->type = type;
if(type == DS2_8BIT)
{
// DEBUG("8 bit: size: 0x%x at 0x%x: ", length, rawPacketData);
if(length < 4)
{
// DEBUG("insufficient data for 8 bit packet\r\n");
return DS2_PSTATUS_INSUFFICIENT_DATA;
}
packetLength = rawPacketData[1];
dataLength = packetLength - 3;
}
else if(type == DS2_16BIT)
{
// DEBUG("16 bit: size: 0x%x at 0x%x: ", length, rawPacketData);
if(rawPacketData[1] != 0x00)
{
// DEBUG("high address byte is not 0x00: %x %x %x %x %x\r\n", rawPacketData[0], rawPacketData[1], rawPacketData[2], rawPacketData[3], rawPacketData[4]);
return DS2_PSTATUS_INVALID;
}
if(length < 5)
{
// DEBUG("insufficient data for 16 bit packet\r\n");
return DS2_PSTATUS_INSUFFICIENT_DATA;
}
packetLength = rawPacketData[2];
packetLength += rawPacketData[1] << 8;
dataLength = packetLength - 4;
}
else
{
throw;
}
if(packetLength == 0)
{
// DEBUG("bad packet: zero packet length: %x %x %x %x %x\r\n", rawPacketData[0], rawPacketData[1], rawPacketData[2], rawPacketData[3], rawPacketData[4]);
return DS2_PSTATUS_INVALID;
}
if(packetLength > MAX_PACKET_LENGTH)
{
// DEBUG("bad packet: packet length > 0x%x : %x %x %x %x %x\r\n", MAX_PACKET_LENGTH, rawPacketData[0], rawPacketData[1], rawPacketData[2], rawPacketData[3], rawPacketData[4]);
return DS2_PSTATUS_INVALID;
}
if(dataLength <= 0)
{
// DEBUG("bad packet: zero data length: %x %x %x %x %x\r\n", rawPacketData[0], rawPacketData[1], rawPacketData[2], rawPacketData[3], rawPacketData[4]);
return DS2_PSTATUS_INVALID;
}
if(length < packetLength)
{
// DEBUG("bad packet: insufficient data (want 0x%x)\r\n", packetLength);
return DS2_PSTATUS_INSUFFICIENT_DATA;
}
if(type == DS2_8BIT)
{
setData(rawPacketData+2, dataLength);
checksum = rawPacketData[dataLength + 2];
}
else if(type == DS2_16BIT)
{
setData(rawPacketData+3, dataLength);
checksum = rawPacketData[dataLength + 3];
}
else
{
throw;
}
setAddress(rawPacketData[0]);
//return true if packet has a valid checksum
generatePacket();
if(checksum == this->checksum)
return DS2_PSTATUS_OK;
else
return DS2_PSTATUS_CHECKSUM_ERROR;
}
DS2Packet& DS2Packet::setAddress(int address)
{
this->address = address;
generatePacket();
return *this;
}
int main(int argc, char *argv[])
{
/*
int sockfd, portno; //sockfd:portno -> server socket
socklen_t clilen;
struct sockaddr_in serv_addr, cli_addr;
if (argc < 2) {
fprintf(stderr,"ERROR, no port provided\n");
exit(1);
}
sockfd = socket(AF_INET, SOCK_STREAM, 0); //create socket
if (sockfd < 0)
error("ERROR opening socket");
memset((char *) &serv_addr, 0, sizeof(serv_addr)); //reset memory
//fill in address info
portno = atoi(argv[1]);
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(portno);
if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
error("ERROR on binding");
listen(sockfd,10); //5 simultaneous connection at most
fd_set active_fd_set, read_fd_set; //set for holding sockets
int newsockfd; //socket representing client
// Initialize the set of active sockets
FD_ZERO(&active_fd_set);
FD_SET(sockfd, &active_fd_set); //put sock to the set to monitor new connections
while(1){
int i = 0;
read_fd_set = active_fd_set;
if(select(FD_SETSIZE,&read_fd_set,NULL,NULL,NULL)<0){error("ERROR select()");}//FAIL with exit status 1 on error
for(i =0; i < FD_SETSIZE; i++){
if(FD_ISSET(i, &read_fd_set)) {
if(i == sockfd)//new connection request
{
newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
if(newsockfd < 0) { error("ERROR accept()");}
FD_SET(newsockfd, &active_fd_set);
}
else{ // Data on connected socket
if (read_socket(i) < 0){
close(i);//close connection
FD_CLR(i, &active_fd_set);
}
}
}
}
}
close(sockfd);
*/
/*
uint32_t test1[1] = {0x00000001};
uint16_t cs1 = 0;
cs1 = checksum((uint8_t *)test1, sizeof(uint32_t)*1);
printf("given:%x, checksum:%x\n",test1[0],cs1);
int i = 0;
uint32_t seqNum = 0;
uint32_t ACKNum = 0;
bool ACK = 0;
bool last = 0;
uint16_t size = 0;
uint16_t cs = 0;
char data[MAX_BODY_SIZE];
char * data_r;
char *packetTest = malloc(sizeof(char)*PACKET_SIZE);
memset(packetTest, 0, sizeof(char)*PACKET_SIZE);
seqNum = 0x12345678;
setSeqNum(packetTest, seqNum);
seqNum = getSeqNum(packetTest);
printf("seqNumTest set?=%x\n", seqNum);
ACKNum =0x9ABCDEF0;
setACKNum(packetTest, ACKNum);
ACKNum = getACKNum(packetTest);
printf("ACKNumTest set?=%x\n", ACKNum);
ACK = true;
setACK(packetTest, ACK);
ACK = getACK(packetTest);
printf("ack set?=%x\n",ACK);
ACK = false;
setACK(packetTest, ACK);
ACK = getACK(packetTest);
printf("ack set?=%x\n",ACK);
last = true;
setLast(packetTest, last);
last = getLast(packetTest);
printf("last set?=%x\n", last);
last = false;
setLast(packetTest, last);
last = getLast(packetTest);
printf("last set?=%x\n", last);
setACK(packetTest, true);
setLast(packetTest, true);
size = 990;
setSize(packetTest, size);
size = getSize(packetTest);
printf("size set?=%x\n",size);
cs = checksum(packetTest, PACKET_SIZE);
setChecksum(packetTest, cs);
cs = getChecksum(packetTest);
printf("checksum set?=%x\n", cs);
//check that the sum is 1111
cs = checksum(packetTest, PACKET_SIZE);
printf("checksum zero?=%x\n", cs);
strcpy(data, "Who wants to eat marshmellows?!");
setData(packetTest, data, strlen(data));
data_r = getData(packetTest);
printf("data set?=\n%s\n",data_r);
free(data_r);
*/
int i = 0;
char data[MAX_BODY_SIZE];
char * data_r;
strcpy(data, "Who wants to eat marshmellows?!");
char * packetTest = generatePacket(0x12345678,
0x9ABCDEF0,
false,
false,
data,
strlen(data));
printPacket(packetTest);
printf("test pkt bits\n");
for(i = 0; i < PACKET_SIZE/32; i+=8)
{
printf("testbits, %08x %08x %08x %08x ",((uint32_t *)packetTest)[i], ((uint32_t *)packetTest)[i+1], ((uint32_t *)packetTest)[i+2], ((uint32_t *)packetTest)[i+3]);
printf("%08x %08x %08x %08x\n",((uint32_t *)packetTest)[i+4], ((uint32_t *)packetTest)[i+5], ((uint32_t *)packetTest)[i+6], ((uint32_t *)packetTest)[i+7]);
}
free(packetTest);
return 0;
}
