C++ readPacket函数代码示例

TEST(Packets, PacketParameterReadTest)
{
    std::istringstream inputStream;
    Packet* actualPacket;
    DigitalOutputPacket expectedPacket1(7, true);
    DigitalOutputPacket expectedPacket2(4, false);

    inputStream.str("\xFF\x02\x07\x02\xFF");
    actualPacket = readPacket(inputStream);

    CHECK(actualPacket != NULL);
    CHECK_EQUAL(expectedPacket1, *actualPacket);
    CHECK(expectedPacket2 != *actualPacket);

    delete actualPacket;

    inputStream.str("\xFF\x02\x04\x01\xFF");
    actualPacket = readPacket(inputStream);

    CHECK(actualPacket != NULL);
    CHECK_EQUAL(expectedPacket2, *actualPacket);
    CHECK(expectedPacket1 != *actualPacket);

    delete actualPacket;
}

uint32_t oggAudio::read(uint32_t size, uint8_t *data)
{
uint32_t i;

	if(_wavheader->encoding==WAV_OGG)
	{

		if(!_inBuffer)
			readPacket(&_inBuffer,_buffer,&i);
		if(!_inBuffer) return 0;
		if(size<_inBuffer)
		{
			memcpy(data,_buffer,size);
			memmove(_buffer,_buffer+size,_inBuffer-size);
			_inBuffer-=size;
			aprintf("Ogg:This round read %lu bytes from buffer (asked) \n",size);
			return size;
		}
		memcpy(data,_buffer,_inBuffer);
		i=_inBuffer;
		_inBuffer=0;
		aprintf("This round read %lu bytes asked %lu \n",i,size);
		return i;
	}
	// It is not vorbis audio
	// We have to skip 1 byte header in case of fresh packet
	// Ask the full page
	uint32_t cursize=0;
	uint32_t flags=0;
	uint64_t stamp=0;
	uint8_t *frags,frag;
	uint32_t ssize=0;
	uint32_t red=0;
	uint8_t lenbyte;

	//
	ssize=readPacket(&cursize,data,&flags);
	// First byte is len stuff
	lenbyte=data[0];
	lenbyte=(lenbyte>>6)+((lenbyte&2)<<1);
	lenbyte=1+lenbyte;
	if(ssize<lenbyte)
	{
		printf("Oops:ssize %lu, lenbyte %d\n",ssize,lenbyte);
		return MINUS_ONE;
		
	}
	ssize-=lenbyte;
	memmove(data,data+lenbyte,ssize);
	return ssize;
		
	printf("OGM : Failed reading non vorbis\n");
	return 0;
	


}

void eraseBaseband(int fd, unsigned int begin, unsigned int end) {
    // correct the end address as the boot loader does, but still give it the
    // 'wrong' value
    unsigned int end2 = end;
    if (begin == 0xa0020000) end = 0xa0310000;

    LOG(LOGLEVEL_INFO, "Erasing flash range 0x%08x-0x%08x...\n", begin, end);

    unsigned int base = end - begin;
    if (base == 0) base = 1; // no div by 0, rather wrong values

    ErasePacket packet = {
        .begin = begin,
        .end = end2
    };
    writePacket(fd, 0x805, &packet, ERASE_PACKET_SIZE);
    char buffer[PACKET_SIZE(sizeof(unsigned short))];
    size_t length = readPacket(fd, WRITE_TIMEOUT, buffer, sizeof(buffer));
    unsigned short *job = verifyPacket(buffer, length);
    LOG(LOGLEVEL_DEBUG, "Erase returns: %d\n", job ? *job : 0);

    if (job) {
        EraseStatusReplyPacket *reply;
        int previous = -1;
        do {
            writePacket(fd, 0x806, job, sizeof(*job));
            char buffer2[PACKET_SIZE(ERASE_STATUS_REPLY_PACKET_SIZE)];
            length = readPacket(fd, DEFAULT_TIMEOUT, buffer2, sizeof(buffer2));
            reply = verifyPacket(buffer2, length);
            if (reply) {
                LOG(LOGLEVEL_DEBUG, "Erase status returns: done=%s current=0x%08x w00=%d\n", reply->done ? "yes" : "no", reply->current, reply->w00);
                if (reply->current >= begin && reply->current <= end) {
                    int percent = (reply->current - begin) * 100 / base;
                    if (percent != previous) {
                        LOG(LOGLEVEL_INFO, "Current progress: %u%%\n", percent);
                        previous = percent;
                    }
                } else if (reply->current == 0xa0000000 && reply->done) {
                    LOG(LOGLEVEL_ERROR, "Looks like the erase command failed due to an invalid secpack.\n");
                } else {
                    LOG(LOGLEVEL_INFO, "Current position: 0x%08x\n", reply->current);
                }
            }
        } while (reply && !reply->done);
    } else {
        LOG(LOGLEVEL_ERROR, "Erase command failed!\n");
    }
}

int PubSubClient::loop() {
   if (connected()) {
      long t = millis();
      if (t - lastActivity > KEEPALIVE) {
         _client.write(MQTTPINGREQ);
         _client.write((uint8_t)0);
         lastActivity = t;
      }
      if (_client.available()) {
         uint8_t len = readPacket();
         if (len > 0) {
            uint8_t type = buffer[0]&0xF0;
            if (type == MQTTPUBLISH) {
               if (callback) {
                  uint8_t tl = (buffer[2]<<3)+buffer[3];
                  char topic[tl+1];
                  for (int i=0;i<tl;i++) {
                     topic[i] = buffer[4+i];
                  }
                  topic[tl] = 0;
                  // ignore msgID - only support QoS 0 subs
                  uint8_t *payload = buffer+4+tl;
                  callback(topic,payload,len-4-tl);
               }
            } else if (type == MQTTPINGREQ) {
               _client.write(MQTTPINGRESP);
               _client.write((uint8_t)0);
               lastActivity = t;
            }
         }
      }
      return 1;
   }
   return 0;
}

int PubSubClient::connect(char *id, char* willTopic, uint8_t willQos, uint8_t willRetain, char* willMessage) {
   if (!connected()) {
      if (_client.connect()) {
         nextMsgId = 1;
         uint8_t d[9] = {0x00,0x06,'M','Q','I','s','d','p',MQTTPROTOCOLVERSION};
         uint8_t length = 0;
         int j;
         for (j = 0;j<9;j++) {
            buffer[length++] = d[j];
         }
         if (willTopic) {
            buffer[length++] = 0x06|(willQos<<3)|(willRetain<<5);
         } else {
            buffer[length++] = 0x02;
         }
         buffer[length++] = 0;
         buffer[length++] = (KEEPALIVE/1000);
         length = writeString(id,buffer,length);
         if (willTopic) {
            length = writeString(willTopic,buffer,length);
            length = writeString(willMessage,buffer,length);
         }
         write(MQTTCONNECT,buffer,length);
         while (!_client.available()) {}
         uint8_t len = readPacket();
         
         if (len == 4 && buffer[3] == 0) {
            lastActivity = millis();
            return 1;
         }
      }
      _client.stop();
   }
   return 0;
}

void SerialServer::processTransfer(QByteArray& header)
{
    QDataStream headerStream(&header, QIODevice::ReadOnly);
    
    quint16 startWord;
    quint16 packetCount;
    
    headerStream >> startWord;
    headerStream >> packetCount;
    
    if(startWord != SERIAL_START)
        return;
    qWarning("Got start...");
    qWarning("packetcount=%d", packetCount);
    
    QByteArray compressedData;
    
    for(quint16 i = 0;i < packetCount;i++) {
        QByteArray data;
        if(!readPacket(&data))
            return;
        compressedData += data;
    }
    
    
    QByteArray data = qUncompress(compressedData);
    compressedData.clear();
    compressedData.squeeze();
    
    processData(data);
}

int AudioStreamInstance::readBuffer(int16 *buffer, const int numSamples) {
	debugC(5, kDebugAudio, "readBuffer(buffer, %d)", numSamples);

	if (_stopped)
		return 0;

	handleFade(numSamples);
	int32 leftSamples = numSamples;
	int32 destOffset = 0;
	if ((_bufferOffset + leftSamples) * 2 >= _bufferSize) {
		if (_bufferSize - _bufferOffset * 2 > 0) {
			memcpy(buffer, &_buffer[_bufferOffset], _bufferSize - _bufferOffset * 2);
			leftSamples -= (_bufferSize - _bufferOffset * 2) / 2;
			destOffset += (_bufferSize - _bufferOffset * 2) / 2;
		}
		if (!readPacket())
			return 0;

		_bufferOffset = 0;
	}

	if (leftSamples >= 0) {
		memcpy(buffer + destOffset, &_buffer[_bufferOffset], MIN(leftSamples * 2, _bufferSize));
		_bufferOffset += leftSamples;
	}

	_playedSamples += numSamples;

	return numSamples;
}

AudioStreamInstance::AudioStreamInstance(AudioManager *man, Audio::Mixer *mixer, Common::SeekableReadStream *stream , bool looping, bool deleteFileStreamAtEnd) {
	_compBufferSize = 0;
	_buffer = NULL;
	_bufferSize = 0;
	_bufferMaxSize = 0;
	_mixer = mixer;
	_compBuffer = NULL;
	_bufferOffset = 0;
	_lastSample = 0;
	_lastStepIndex = 0;
	_file = stream;
	_fadingIn = false;
	_fadingOut = false;
	_fadeTime = 0;
	_stopped = false;
	_volume = 255;
	_totalSize = stream->size();
	_currentReadSize = 8;
	_man = man;
	_looping = looping;
	_musicAttenuation = 1000;
	_deleteFileStream = deleteFileStreamAtEnd;
	_playedSamples = 0;

	// preload one packet
	if (_totalSize > 0) {
		_file->skip(8);
		readPacket();
	} else {
		stopNow();
	}

	_soundType = Audio::Mixer::kPlainSoundType;
}

AVFrame *FFMpegDecoder::getFrameAtSec(double sec)
{
	if (videoStream < 0)
	{
		errinfo("no video stream!");
		return NULL;
	}

	AVFrame *pic = NULL;
	seekToSec(sec);
	while (true)
	{
		AVPacket *pkt = readPacket();
		if (pkt == NULL)
			break;

		if (pkt->stream_index != videoStream)
			continue;

		pic = decodeVideo();
		if (pic != NULL)
			break;
	}
	return pic;
}

void prepareFlash(int fd) {
    LOG(LOGLEVEL_INFO, "Preparing flash access...\n");

    short param = 0;
    writePacket(fd, 0x84, &param, sizeof(param));
    char buffer[PACKET_SIZE(CFI1_PACKET_SIZE)];
    size_t length = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer));
    LOG(LOGLEVEL_DEBUG, "CFI Stage 1 returns:\n");
    LOGDO(LOGLEVEL_DEBUG, printBuffer(verifyPacket(buffer, length), CFI1_PACKET_SIZE));

    writePacket(fd, 0x85, NULL, 0);
    length = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer));
    short *unknown = verifyPacket(buffer, length);
    LOG(LOGLEVEL_DEBUG, "CFI Stage 2 returns: %d\n", unknown ? *unknown : 0);
    //LOGDO(LOGLEVEL_DEBUG, printBuffer(buffer, length));
}

SendFileDataPacket::SendFileDataPacket(const char *buffer, size_t bufferSize) :
	DataPacket(DataPacketHeader(buffer, bufferSize))
{
	assert(m_header.getDataSize() + DataPacketHeader::getHeaderSize() <= bufferSize);

	readPacket(buffer, bufferSize);
}

void
service_net ()
{
	while (interrupts > 0)
	{
		NRF_CE_lo;
		uint8_t status = NRF_ReadRegister(NRF_STATUS);
		RXEN_lo;
	        // received a packet
	        if (status & 0x40)
     	   	{                
			printf("Radio received a packet\n");
		 	readPacket();
        	} else if (status & 0x20)
        	{ // packet was sent
                	printf("Radio successfully sent the packet\n");
         		NRF_WriteRegister(NRF_STATUS, 0x10);
		} else if (status & 0x10)
         	{ // packet failed to send              
			printf("Radio failed to send the packet\n");
			NRF_WriteRegister(NRF_STATUS, 0x20);
         	}
		RXEN_hi;
		NRF_CE_hi;
		INT_Disable();
		interrupts--;
		INT_Enable();
	}
}	

 void Demuxer::feedStream(Stream& stream)
 {
     sf::Lock l(m_synchronized);
     
     while ((!didReachEndOfFile() || hasPendingDataForStream(stream)) && stream.needsMoreData())
     {
         AVPacket* pkt = NULL;
         
         pkt = gatherQueuedPacketForStream(stream);
         
         if (!pkt)
             pkt = readPacket();
         
         if (!pkt)
         {
             m_eofReached = true;
         }
         else
         {
             if (!distributePacket(pkt, stream))
             {
                 av_free_packet(pkt);
                 av_free(pkt);
             }
         }
     }
 }

	void PacketReader::onDataReady(Uint8* buf, Uint32 size)
	{
		if (error)
			return;

		mutex.lock();
		if (packet_queue.size() == 0)
		{
			Uint32 ret = 0;
			while (ret < size && !error)
			{
				ret += newPacket(buf + ret, size - ret);
			}
		}
		else
		{
			Uint32 ret = 0;
			IncomingPacket::Ptr pck = packet_queue.back();
			if (pck->read == pck->size) // last packet in queue is fully read
				ret = newPacket(buf, size);
			else
				ret = readPacket(buf, size);

			while (ret < size  && !error)
			{
				ret += newPacket(buf + ret, size - ret);
			}
		}
		mutex.unlock();
	}

void WANDongleSerialPort::rxHandler()
{
  if (((USBEndpoint *) bulk_in)->getState() == USB_TYPE_IDLE) //Success
  {
    buf_in_read_pos = 0;
    buf_in_len = ((USBEndpoint *) bulk_in)->getLengthTransferred(); //Update length
    //lock_rx.unlock();
    rx_mtx.lock();
    lock_rx = false; //Transmission complete
    if(cb_rx_en)
    {
      rx_mtx.unlock();
      listener->readable(); //Call handler from the IRQ context
      //readPacket() should be called by the handler subsequently once the buffer has been emptied
    }
    else
    {
      cb_rx_pending = true; //Queue the callback
      rx_mtx.unlock();
    }

  }
  else //Error, try reading again
  {
    //lock_rx.unlock();
    DBG("Trying again");
    readPacket();
  }
}