From: andreaskoepke Date: Fri, 20 Feb 2009 09:27:46 +0000 (+0000) Subject: be more careful with the stack when reading a packet X-Git-Tag: rc_6_tinyos_2_1_1~487 X-Git-Url: https://oss.titaniummirror.com/gitweb/?a=commitdiff_plain;h=04ea4f3a533d9fc080a8df849c752ece0022fcfb;hp=d380483a17df056bdb3539dba90181b30d5b6188;p=tinyos-2.x.git be more careful with the stack when reading a packet fix includes for g++ 4.3 --- diff --git a/support/sdk/cpp/sf/serialcomm.cpp b/support/sdk/cpp/sf/serialcomm.cpp index 4479e18f..3c1db5da 100644 --- a/support/sdk/cpp/sf/serialcomm.cpp +++ b/support/sdk/cpp/sf/serialcomm.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -42,6 +43,7 @@ #include #include #include +#include using namespace std; @@ -275,11 +277,11 @@ SerialComm::SerialComm(const char* pDevice, int pBaudrate, PacketBuffer &pReadBu newtio.c_oflag = 0; if ((tcflush(serialReadFD, TCIFLUSH) >= 0 && tcsetattr(serialReadFD, TCSANOW, &newtio) >= 0) - && (tcflush(serialWriteFD, TCIFLUSH) >= 0 && tcsetattr(serialWriteFD, TCSANOW, &newtio) >= 0) - && !errorReported) + && (tcflush(serialWriteFD, TCIFLUSH) >= 0 && tcsetattr(serialWriteFD, TCSANOW, &newtio) >= 0) + && !errorReported) { DEBUG("SerialComm::SerialComm : opened device "<< pDevice << " with baudrate = " << pBaudrate) - } + } else { close(serialReadFD); @@ -337,12 +339,12 @@ int SerialComm::writeFD(int fd, const char *buffer, int count, int *err) { int cnt = 0; /* - FD_SET(serialWriteFD, &wfds); - if(select(serialWriteFD + 1, NULL, &wfds, NULL, NULL) < 0) { - return -1; - } - FD_CLR(serialWriteFD, &wfds); - */ + FD_SET(serialWriteFD, &wfds); + if(select(serialWriteFD + 1, NULL, &wfds, NULL, NULL) < 0) { + return -1; + } + FD_CLR(serialWriteFD, &wfds); + */ int tmpCnt = BaseComm::writeFD(fd, buffer, count, err); if (tmpCnt < 0) { *err = errno; @@ -414,145 +416,86 @@ char SerialComm::nextRaw() { /* reads packet */ bool SerialComm::readPacket(SFPacket &pPacket) { - bool sync = false; - bool escape = false; - bool completePacket = false; + uint8_t buffer[maxMTU + 10]; int count = 0; - uint16_t crc = 0; - char buffer[maxMTU]; - while(!completePacket) - { - buffer[count] = nextRaw(); - - if(sync && (count == 1) && (buffer[count] == SYNC_BYTE)) { - DEBUG("SerialComm::readPacket double sync byte"); - sync = false; - escape = false; - count = 1; - crc = 0; - buffer[0] = SYNC_BYTE; - } - - if (!sync) - { - // wait for sync - if (buffer[0] == SYNC_BYTE) - { - sync = true; - escape = false; - count = 1; - crc = 0; + rx_states_t state = WAIT_FOR_SYNC; + for(;;) { + uint8_t nextByte = nextRaw(); + if(state == WAIT_FOR_SYNC) { + if(nextByte == SYNC_BYTE) { + count = 0; + state = IN_SYNC; } } - else if (count >= maxMTU) - { - DEBUG("SerialComm::readPacket : frame too long - size = " << count << " : resynchronising") - sync = false; - escape = false; - count = crc = 0; - badPacketCount++; - } - else if (escape) - { - if (buffer[count] == SYNC_BYTE) - { - DEBUG("SerialComm::readPacket : resynchronising") - sync = false; - escape = false; - count = crc = 0; - badPacketCount++; - } - else - { - buffer[count] ^= 0x20; - if (count > 3) - { - crc = SerialComm::byteCRC(buffer[count-3], crc); + else if(state == IN_SYNC) { + if(nextByte == SYNC_BYTE) { + if(count < minMTU) { + DEBUG("SerialComm::readPacket : frame too short - size = " << count << " : resynchronising "); + badPacketCount++; + count = 0; + } + else { + if(checkCrc(buffer, count)) { + pPacket.setType(buffer[typeOffset]); + pPacket.setSeqno(buffer[seqnoOffset]); + switch (buffer[typeOffset]) { + case SF_ACK: + break; + case SF_PACKET_NO_ACK: + case SF_PACKET_ACK: + pPacket.setPayload((char *)(&buffer[payloadOffset]-1), count+1+1 - serialHeaderBytes); + break; + default: + DEBUG("SerialComm::readPacket : unknown packet type = " \ + << static_cast(buffer[typeOffset] & 0xff)); + break; + } + break; // leave loop + } + else { + DEBUG("SerialComm::readPacket : bad crc"); + count = 0; + badPacketCount++; + } } - ++count; - escape = false; } - } - else if (buffer[count] == ESCAPE_BYTE) - { - // next byte is escaped - escape = true; - } - else if (buffer[count] == SYNC_BYTE) - { - // calculate last crc byte - if (count > 3) - { - crc = SerialComm::byteCRC(buffer[count-3], crc); + else if(nextByte == ESCAPE_BYTE) { + state = ESCAPED; } - uint16_t packetCRC = (buffer[count - 2] & 0xff) | ((buffer[count - 1] << 8) & 0xff00); - if (count < minMTU) - { - DEBUG("SerialComm::readPacket : frame too short - size = " << count << " : resynchronising ") - sync = false; - escape = false; - count = crc = 0; - badPacketCount++; + else { + buffer[count++] = nextByte; + if(count >= maxMTU) { + DEBUG("SerialComm::readPacket : packet too long, resynchronizing"); + count = 0; + badPacketCount++; + state = WAIT_FOR_SYNC; + } } - else if (crc != packetCRC) - { - DEBUG("SerialComm::readPacket : bad crc - calculated crc = " << crc << " packet crc = " << packetCRC << " : resynchronising " ) - sync = false; - escape = false; - count = crc = 0; - badPacketCount++; + } + else if(state == ESCAPED) { + if(nextByte == SYNC_BYTE) { + DEBUG("SerialComm::readPacket : state ESCAPED, packet got sync byte, resynchronizing"); + count = 0; + badPacketCount++; + state = IN_SYNC; } - else - { - pPacket.setType(buffer[typeOffset]); - pPacket.setSeqno(buffer[seqnoOffset]); - switch (buffer[typeOffset]) - { - case SF_ACK: - break; - case SF_PACKET_NO_ACK: - case SF_PACKET_ACK: - // buffer / payload - // FIXME: strange packet format!? because seqno is not really defined - missing :( - pPacket.setPayload(&buffer[payloadOffset]-1, count+1+1 - serialHeaderBytes); - break; - default: - DEBUG("SerialComm::readPacket : unknown packet type = " << static_cast(buffer[typeOffset] & 0xff)) - ; + else { + buffer[count++] = nextByte ^ 0x20; + if(count >= maxMTU) { + DEBUG("SerialComm::readPacket : state ESCAPED, packet too long, resynchronizing"); + count = 0; + badPacketCount++; + state = WAIT_FOR_SYNC; } - completePacket = true; -#ifdef DEBUG_RAW_SERIALCOMM - - DEBUG("SerialComm::readPacket : raw data >>") - for (int j=0; j <= count; j++) - { - cout << std::hex << static_cast(buffer[j] & 0xff) << " " << std::dec; - } - cout << endl; - cout << "as payload >> " << endl; - const char* ptr = pPacket.getPayload(); - for (int j=0; j < pPacket.getLength(); j++) - { - cout << std::hex << static_cast(ptr[j] & 0xff) << " " << std::dec; + else { + state = IN_SYNC; } - cout << endl; -#endif - } } - else - { - if (count > 3) - { - crc = SerialComm::byteCRC(buffer[count-3], crc); - } - ++count; - } } return true; } - /* writes packet */ bool SerialComm::writePacket(SFPacket &pPacket) { @@ -643,37 +586,37 @@ void SerialComm::readSerial() SFPacket packet; readPacket(packet); switch (packet.getType()) - { - case SF_ACK: + { + case SF_ACK: // successful delivery // FIXME: seqnos are not implemented on the node ! pthread_cond_signal(&ack.received); break; - case SF_PACKET_ACK: - { - // put ack in front of queue - SFPacket ack(SF_ACK, packet.getSeqno()); - writeBuffer.enqueueFront(ack); - } - case SF_PACKET_NO_ACK: + case SF_PACKET_ACK: + { + // put ack in front of queue + SFPacket ack(SF_ACK, packet.getSeqno()); + writeBuffer.enqueueFront(ack); + } + case SF_PACKET_NO_ACK: // do nothing - fall through - default: + default: if (!readBuffer.isFull()) - { + { ++readPacketCount; // put silently into buffer... readBuffer.enqueueBack(packet); - } + } else - { + { while(readBuffer.isFull()) { readBuffer.dequeue(); ++droppedReadPacketCount; } readBuffer.enqueueBack(packet); // DEBUG("SerialComm::readSerial : dropped packet") - } - } + } + } } } @@ -695,33 +638,34 @@ void SerialComm::writeSerial() while (true) { if (!retry) - { + { + cerr << " serial deqeue packet, empty: " << writeBuffer.isEmpty() << endl; packet = writeBuffer.dequeue(); - } + } switch (packet.getType()) - { - case SF_ACK: + { + case SF_ACK: // successful delivery if (!writePacket(packet)) - { + { DEBUG("SerialComm::writeSerial : writePacket failed (SF_ACK)") - reportError("SerialComm::writeSerial : writePacket(SF_ACK)", -1); - } + reportError("SerialComm::writeSerial : writePacket(SF_ACK)", -1); + } break; - case SF_PACKET_ACK: + case SF_PACKET_ACK: // do nothing - fall through - case SF_PACKET_NO_ACK: + case SF_PACKET_NO_ACK: // do nothing - fall through - default: + default: if (!retry) ++writtenPacketCount; // FIXME: this is the only currently supported type by the mote packet.setType(SF_PACKET_ACK); if (!writePacket(packet)) - { + { DEBUG("SerialComm::writeSerial : writePacket failed (SF_PACKET)") - reportError("SerialComm::writeSerial : writeFD(SF_PACKET)", -1); - } + reportError("SerialComm::writeSerial : writeFD(SF_PACKET)", -1); + } // wait for ack... struct timeval currentTime; struct timespec ackTime; @@ -734,24 +678,24 @@ void SerialComm::writeSerial() ackTime.tv_sec = currentTime.tv_sec; ackTime.tv_nsec = currentTime.tv_usec * 1000; - ackTime.tv_sec += timeout / (1000*1000*1000); - ackTime.tv_nsec += timeout % (1000*1000*1000); + ackTime.tv_sec += timeout / (1000*1000*1000); + ackTime.tv_nsec += timeout % (1000*1000*1000); pthread_cleanup_push((void(*)(void*)) pthread_mutex_unlock, (void *) &ack.lock); int retval = pthread_cond_timedwait(&ack.received, &ack.lock, &ackTime); if (!((retryCount < maxRetries) && (retval == ETIMEDOUT))) - { - if (retryCount >= maxRetries) ++droppedWritePacketCount; + { + if (retryCount >= maxRetries) ++droppedWritePacketCount; retry = false; retryCount = 0; - } + } else - { + { ++retryCount; retry = true; DEBUG("SerialComm::writeSerial : packet retryCount = " << retryCount); - ++sumRetries; - } + ++sumRetries; + } // removes the cleanup handler and executes it (unlock mutex) pthread_cleanup_pop(1); } } @@ -764,51 +708,51 @@ void SerialComm::cancel() if(readerThreadRunning && pthread_equal(callingThread, readerThread)) { DEBUG("SerialComm::cancel : by readerThread") - pthread_detach(readerThread); + pthread_detach(readerThread); if (writerThreadRunning) - { + { pthread_cancel(writerThread); DEBUG("SerialComm::cancel : writerThread canceled, joining") - pthread_join(writerThread, NULL); + pthread_join(writerThread, NULL); writerThreadRunning = false; - } + } readerThreadRunning = false; - pthread_cond_signal(&control.cancel); + pthread_cond_signal(&control.cancel); pthread_exit(NULL); } else if(writerThreadRunning && pthread_equal(callingThread, writerThread)) { DEBUG("SerialComm::cancel : by writerThread") - pthread_detach(writerThread); + pthread_detach(writerThread); if (readerThreadRunning) - { + { pthread_cancel(readerThread); DEBUG("SerialComm::cancel : readerThread canceled, joining") - pthread_join(readerThread, NULL); + pthread_join(readerThread, NULL); readerThreadRunning = false; - } + } writerThreadRunning = false; - pthread_cond_signal(&control.cancel); + pthread_cond_signal(&control.cancel); pthread_exit(NULL); } else { DEBUG("SerialComm::cancel : by other thread") - if (readerThreadRunning) - { - pthread_cancel(readerThread); - DEBUG("SerialComm::cancel : readerThread canceled, joining") - pthread_join(readerThread, NULL); - readerThreadRunning = false; - } + if (readerThreadRunning) + { + pthread_cancel(readerThread); + DEBUG("SerialComm::cancel : readerThread canceled, joining") + pthread_join(readerThread, NULL); + readerThreadRunning = false; + } if (writerThreadRunning) - { + { pthread_cancel(writerThread); DEBUG("SerialComm::cancel : writerThread canceled, joining") - pthread_join(writerThread, NULL); + pthread_join(writerThread, NULL); writerThreadRunning = false; - } - pthread_cond_signal(&control.cancel); + } + pthread_cond_signal(&control.cancel); } } @@ -818,10 +762,10 @@ int SerialComm::reportError(const char *msg, int result) if ((result < 0) && (!errorReported)) { errorMsg << "error : SF-Server ( SerialComm on device = " << device << " ) : " - << msg << " ( result = " << result << " )" << endl - << "error-description : " << strerror(errno) << endl; + << msg << " ( result = " << result << " )" << endl + << "error-description : " << strerror(errno) << endl; - cerr << errorMsg.str(); + cerr << errorMsg.str(); errorReported = true; cancel(); } diff --git a/support/sdk/cpp/sf/serialcomm.h b/support/sdk/cpp/sf/serialcomm.h index 97498477..af2a57cf 100644 --- a/support/sdk/cpp/sf/serialcomm.h +++ b/support/sdk/cpp/sf/serialcomm.h @@ -38,6 +38,7 @@ #include "packetbuffer.h" #include "sharedinfo.h" +#include #include #include #include @@ -66,13 +67,13 @@ protected: // min serial MTU static const int minMTU = 4; // byte count of serial header - static const int serialHeaderBytes = 6; + static const int serialHeaderBytes = 5; // byte offset of type field - static const int typeOffset = 1; + static const int typeOffset = 0; // byte offset of sequence number field - static const int seqnoOffset = 2; + static const int seqnoOffset = 1; // byte offset of payload field - static const int payloadOffset = 3; + static const int payloadOffset = 2; // timeout for acks in s static const int ackTimeout = 1000 * 1000 * 200; // max. reties for packets from pc to node @@ -80,6 +81,12 @@ protected: // how many bytes do we attempt to read from the serial line in one go? static const int rawReadBytes = 20; + + enum rx_states_t { + WAIT_FOR_SYNC, + IN_SYNC, + ESCAPED + }; /** Member vars */ protected: @@ -189,6 +196,24 @@ protected: crc ^= (crc & 0xff) << 5; return crc; } + + inline static uint16_t calcCRC(uint8_t *bytes, uint16_t len) { + uint16_t crc = 0; + for(unsigned i = 0; i < len; i++) { + crc = SerialComm::byteCRC(bytes[i], crc); + } + return crc; + } + + inline static uint16_t checkCrc(uint8_t *bytes, uint16_t count) { + bool crcOk = false; + if(count > 2) { + uint16_t crc = calcCRC(bytes, count - 2); + uint16_t packetCrc = (bytes[count-1] << 8) | bytes[count-2]; + if(crc == packetCrc) crcOk = true; + } + return crcOk; + } /* HDLC encode (byte stuff) count bytes from buffer from into buffer to. * to must be at least count * 2 bytes large. Returns the number of bytes diff --git a/support/sdk/cpp/sf/sfcontrol.cpp b/support/sdk/cpp/sf/sfcontrol.cpp index 4cfeac4b..8489d0b2 100644 --- a/support/sdk/cpp/sf/sfcontrol.cpp +++ b/support/sdk/cpp/sf/sfcontrol.cpp @@ -42,6 +42,8 @@ #include #include +#include +#include #include #include #include diff --git a/support/sdk/cpp/sf/sfpacket.cpp b/support/sdk/cpp/sf/sfpacket.cpp index c43059c6..0b066c8e 100644 --- a/support/sdk/cpp/sf/sfpacket.cpp +++ b/support/sdk/cpp/sf/sfpacket.cpp @@ -31,6 +31,7 @@ */ #include "sfpacket.h" +#include SFPacket::SFPacket(int pType, int pSeqno) { length = 0; diff --git a/support/sdk/cpp/sf/tcpcomm.cpp b/support/sdk/cpp/sf/tcpcomm.cpp index 27231a62..2e77a22c 100644 --- a/support/sdk/cpp/sf/tcpcomm.cpp +++ b/support/sdk/cpp/sf/tcpcomm.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -582,7 +583,7 @@ void TCPComm::reportStatus(ostream& os) void TCPComm::stuffPipe() { char info = 'n'; - write(pipeWriteFD, &info, 1); + if(write(pipeWriteFD, &info, 1) != 1) DEBUG("TCPComm::stuffPipe : lokal pipe is broken"); } void TCPComm::clearPipe() {