/** * \brief 接收指令到缓冲区 * \param pstrCmd 指令缓冲区 * \param nCmdLen 指令缓冲区的大小 * \param wait 当套接口数据没有准备好的时候,是否需要等待 * \return 实际接收的指令大小 * 返回-1,表示接收错误 * 返回0,表示接收超时 * 返回整数,表示实际接收的字节数 */ int CSocket::recvToCmd(void *pstrCmd,const int nCmdLen,const bool wait) { //printf("CSocket::recvToCmd\n"); //够一个完整的纪录,不需要向套接口接收数据 do { if (_rcv_raw_size >= packetMinSize()/* && _rcv_queue.rd_size() >= packetMinSize()*/) { DWORD nRecordLen = packetSize(&_rcv_queue.rd_buf()[PACKHEADSIZE]); if (_rcv_raw_size >= nRecordLen/* && _rcv_queue.rd_size() >= nRecordLen*/) { int retval = packetUnpack(_rcv_queue.rd_buf(),nRecordLen,(BYTE*)pstrCmd); _rcv_queue.rd_flip(nRecordLen + PACKHEADLASTSIZE); InterlockedExchangeAdd( (LONG*)&_rcv_raw_size, -( nRecordLen + PACKHEADLASTSIZE)); return retval; } } else { break; } } while(0); do { int retval = recvToBuf(); if (-1 == retval || (0 == retval && !wait)) return retval; //够一个完整的纪录,不需要向套接口接收数据 do { if (_rcv_raw_size >= packetMinSize()/* && _rcv_queue.rd_size() >= packetMinSize()*/) { DWORD nRecordLen = packetSize(&_rcv_queue.rd_buf()[PACKHEADSIZE]); if (_rcv_raw_size >= nRecordLen/* && _rcv_queue.rd_size() >= nRecordLen*/) { int retval = packetUnpack(_rcv_queue.rd_buf(),nRecordLen,(BYTE*)pstrCmd); _rcv_queue.rd_flip(nRecordLen + PACKHEADLASTSIZE); InterlockedExchangeAdd((LONG*)&_rcv_raw_size, -( nRecordLen + PACKHEADLASTSIZE )); return retval; } } else { break; } } while(0); } while(true); return 0; }
void ApplePS2CypressTouchPad::packetReady() { while (_ringBuffer.count() >= kPacketLengthLarge) { UInt8 *packet = _ringBuffer.tail(); this->updatePacketSize(packet[0]); UInt8 size = packetSize(); if (_ringBuffer.count() < size) return ; // should be deleted once communication with PS2 cypress trackpad stable // v34 and later seems to send 0x04 header with 4 fingers touch (??), so lets make a little trick ... if (_touchPadVersion >= 34 && (packet[0] == 0x04)) packet[0] = 0x20; // 4 fingers should be 0x20 ... if (_touchPadVersion >= 34 && (packet[0] == 0x44)) packet[0] = 0xa0; // 5 fingers should be 0xa0 ... #ifdef DEBUG if (packet[0] || packet[1] || packet[2] || packet[3] || packet[4] || packet[5] || packet[6] || packet[7]) DEBUG_LOG("CYPRESS: %s: packet dump { 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x }\n", getName(), packet[0], packet[1], packet[2], packet[3], packet[4], packet[5], packet[6], packet[7]); #endif if (this->cypressCheckPacketEndFrame(packet)) continue ; if (this->cypressCheckPacketValidity(packet) == false) return ; this->cypressProcessPacket(packet); _ringBuffer.advanceTail(kPacketLengthLarge); } }
int vtSendPacket(VtSession *z, Packet *p) { IOchunk ioc; int n; uint8_t buf[2]; /* add framing */ n = packetSize(p); if(n >= (1<<16)) { vtSetError(EBigPacket); packetFree(p); return 0; } buf[0] = n>>8; buf[1] = n; packetPrefix(p, buf, 2); for(;;) { n = packetFragments(p, &ioc, 1, 0); if(n == 0) break; if(!vtFdWrite(z->fd, ioc.addr, ioc.len)) { packetFree(p); return 0; } packetConsume(p, nil, n); } packetFree(p); return 1; }
void CClientNet::sendProc() { TS_PEER_MESSAGE *pmsg = new TS_PEER_MESSAGE(); memset(pmsg, 0, sizeof(TS_PEER_MESSAGE)); iop_usleep(10); while (isRunning()) { ReadOut(*pmsg); if (getType(pmsg->msg) > PACKETCONTROL) m_agent->send(pmsg->msg.Body, packetSize(pmsg->msg)); else m_Connect->send(pmsg->msg.Body, packetSize(pmsg->msg)); } delete pmsg; #ifdef _DEBUG_INFO_ cout << "send thread exit" << endl; #endif }
void FifoReadCallback::readDataAvailable(size_t len) noexcept { try { readBuffer_.postallocate(len); // Process any pending packet headers. if (pendingHeader_.packetSize() > 0) { if (readBuffer_.chainLength() < pendingHeader_.packetSize()) { return; } feedParser(pendingHeader_, readBuffer_.split(pendingHeader_.packetSize())); pendingHeader_.setPacketSize(0); } while (readBuffer_.chainLength() >= std::max(sizeof(MessageHeader), sizeof(PacketHeader))) { if (isMessageHeader(readBuffer_)) { auto msgHeader = parseMessageHeader(readBuffer_); auto fromAddr = msgHeader.getLocalAddress(); auto toAddr = msgHeader.getPeerAddress(); if (msgHeader.direction() == MessageDirection::Received) { std::swap(fromAddr, toAddr); } parserMap_.fetch(msgHeader.msgId()).setAddresses(fromAddr, toAddr); continue; } auto packetHeader = parsePacketHeader( readBuffer_.split(sizeof(PacketHeader))->coalesce()); if (packetHeader.packetSize() > readBuffer_.chainLength()) { // Wait for more data. pendingHeader_ = std::move(packetHeader); return; } feedParser(packetHeader, readBuffer_.split(packetHeader.packetSize())); } } catch (const std::exception& ex) { CHECK(false) << "Unexpected exception: " << ex.what(); } }
void FifoReadCallback::readDataAvailable(size_t len) noexcept { try { readBuffer_.postallocate(len); // Process any pending packet headers. if (pendingHeader_) { if (readBuffer_.chainLength() < pendingHeader_->packetSize()) { return; } forwardMessage(pendingHeader_.value(), readBuffer_.split(pendingHeader_->packetSize())); pendingHeader_.clear(); } while (readBuffer_.chainLength() >= kHeaderMagicSize) { if (isMessageHeader(readBuffer_)) { if (readBuffer_.chainLength() < sizeof(MessageHeader)) { // Wait for more data return; } handleMessageHeader(parseMessageHeader(readBuffer_)); } if (readBuffer_.chainLength() < sizeof(PacketHeader)) { // Wait for more data return; } auto packetHeader = parsePacketHeader(readBuffer_); if (packetHeader.packetSize() > readBuffer_.chainLength()) { // Wait for more data. pendingHeader_.assign(std::move(packetHeader)); return; } forwardMessage(packetHeader, readBuffer_.split(packetHeader.packetSize())); } } catch (const std::exception& ex) { CHECK(false) << "Unexpected exception: " << ex.what(); } }
int vtHello(VtSession *z) { Packet *p; uint8_t buf[10]; char *sid; int crypto, codec; sid = nil; p = packetAlloc(); if(!vtAddString(p, vtGetVersion(z))) goto Err; if(!vtAddString(p, vtGetUid(z))) goto Err; buf[0] = vtGetCryptoStrength(z); buf[1] = 0; buf[2] = 0; packetAppend(p, buf, 3); p = vtRPC(z, VtQHello, p); if(p == nil) return 0; if(!vtGetString(p, &sid)) goto Err; if(!packetConsume(p, buf, 2)) goto Err; if(packetSize(p) != 0) { vtSetError(EProtocolBotch); goto Err; } crypto = buf[0]; codec = buf[1]; USED(crypto); USED(codec); packetFree(p); vtLock(z->lk); z->sid = sid; z->auth.state = VtAuthOK; vtSha1Free(z->inHash); z->inHash = nil; vtSha1Free(z->outHash); z->outHash = nil; vtUnlock(z->lk); return 1; Err: packetFree(p); vtMemFree(sid); return 0; }
int vtRead(VtSession *z, uint8_t score[VtScoreSize], int type, uint8_t *buf, int n) { Packet *p; p = vtReadPacket(z, score, type, n); if(p == nil) return -1; n = packetSize(p); packetCopy(p, buf, 0, n); packetFree(p); return n; }
// throooooough the NAT, type must < PACKETCONTROL void CClientNet::sendConnectionMsg() { ts_msg msg; TS_MESSAGE_HEAD *head = (TS_MESSAGE_HEAD*) &msg; head->type = CONNECTION; head->time = getClientTime(); head->size = sizeof(TS_MESSAGE_HEAD); head->sequence = 0; head->UID = m_uid; head->version = VersionNumber; head->subSeq = 0; for (int i = 0; i < 5; i++) m_Connect->send(msg.Body, packetSize(msg)); }
DWORD Socket::successUnpack(void *cmd) { DWORD retval = 0; //有消息还没有读出来 CheckConditonReturn(m_recvRawSize >= packetMinSize(),0); //理论上消息的长度 DWORD recordLen = packetSize(m_recvQueue.rdBuffer()); if(m_recvRawSize >= recordLen) { retval = packetUnpack(m_recvQueue.rdBuffer(),recordLen,(BYTE*)cmd); m_recvQueue.rdFlip(recordLen); m_recvRawSize -= recordLen; } return retval; }
int vtWritePacket(VtSession *z, uint8_t score[VtScoreSize], int type, Packet *p) { int n = packetSize(p); uint8_t *hdr; if(n > VtMaxLumpSize || n < 0) { vtSetError(ELumpSize); goto Err; } if(n == 0) { memmove(score, vtZeroScore, VtScoreSize); return 1; } hdr = packetHeader(p, 4); hdr[0] = type; hdr[1] = 0; /* pad */ hdr[2] = 0; /* pad */ hdr[3] = 0; /* pad */ p = vtRPC(z, VtQWrite, p); if(p == nil) return 0; if(!packetConsume(p, score, VtScoreSize)) goto Err; if(packetSize(p) != 0) { vtSetError(EProtocolBotch); goto Err; } packetFree(p); return 1; Err: packetFree(p); return 0; }
Packet* vtRecvPacket(VtSession *z) { uint8_t buf[10], *b; int n; Packet *p; int size, len; if(z->cstate != VtStateConnected) { vtSetError("session not connected"); return 0; } vtLock(z->inLock); p = z->part; /* get enough for head size */ size = packetSize(p); while(size < 2) { b = packetTrailer(p, MaxFragSize); assert(b != nil); n = vtFdRead(z->fd, b, MaxFragSize); if(n <= 0) goto Err; size += n; packetTrim(p, 0, size); } if(!packetConsume(p, buf, 2)) goto Err; len = (buf[0] << 8) | buf[1]; size -= 2; while(size < len) { n = len - size; if(n > MaxFragSize) n = MaxFragSize; b = packetTrailer(p, n); if(!vtFdReadFully(z->fd, b, n)) goto Err; size += n; } p = packetSplit(p, len); vtUnlock(z->inLock); return p; Err: vtUnlock(z->inLock); return nil; }
uint32_t Packet::CalcPacketSize() const { uint32_t packetSize(0); if (command == CommandId::CMDID_TEXT || command == CommandId::CMDID_WORKSPACE) { packetSize = sizeof(Text); } else { BEHAVIAC_ASSERT(!"Unknown command"); } packetSize += sizeof(command); return packetSize; }
void CServer::sendProc() { TS_PEER_MESSAGE pmsg; memset(&pmsg, 0, sizeof(TS_PEER_MESSAGE)); CPeerConnection* peer = new CPeerConnection(pConnect->getSocket()); while (isRunning()) { ReadOut(pmsg); peer->setPeer(pmsg.peeraddr); peer->send(pmsg.msg.Body, packetSize(pmsg.msg)); #ifdef _DEBUG_INFO_ // cout << "port:" << pConnect->getPeer()->sin_port << endl; #endif } delete peer; }
int vtSync(VtSession *z) { Packet *p = packetAlloc(); p = vtRPC(z, VtQSync, p); if(p == nil) return 0; if(packetSize(p) != 0){ vtSetError(EProtocolBotch); goto Err; } packetFree(p); return 1; Err: packetFree(p); return 0; }
void CClientNet::buildSendMessage(ts_msg& msg) { if (!isStarted && !Start(0)) return; TS_MESSAGE_HEAD* head = (TS_MESSAGE_HEAD*) &msg; head->time = getClientTime(); head->isEnd = 0; head->UID = m_uid; head->version = VersionNumber; if (getType(msg) > PACKETCONTROL) { head->sequence = 0; m_agent->send(msg.Body, packetSize(msg)); return; } else { head->sequence = m_seq++; } // qDebug() << "sent seq, subseq:" << head->sequence << head->subSeq; TS_PEER_MESSAGE pmsg; pmsg.msg = msg; WriteOut(pmsg); }
/** * \brief 接收指令到缓冲区,不从套接口接收指令,只是把接收缓冲的数据解包 * \param pstrCmd 指令缓冲区 * \param nCmdLen 指令缓冲区的大小 * \return 实际接收的指令大小 * 返回-1,表示接收错误 * 返回0,表示接收超时 * 返回整数,表示实际接收的字节数 */ int CSocket::recvToCmd_NoPoll(void *pstrCmd,const int nCmdLen) { printf("CSocket::recvToCmd_NoPoll\n"); //够一个完整的纪录,不需要向套接口接收数据 do { if (_rcv_raw_size >= packetMinSize()/* && _rcv_queue.rd_size() >= packetMinSize()*/) { DWORD nRecordLen = packetSize(_rcv_queue.rd_buf() + PACKHEADSIZE); if (_rcv_raw_size >= nRecordLen/* && _rcv_queue.rd_size() >= nRecordLen*/) { int retval = packetUnpack(_rcv_queue.rd_buf(),nRecordLen,(BYTE*)pstrCmd); _rcv_queue.rd_flip(nRecordLen + PACKHEADLASTSIZE); InterlockedExchangeAdd((LONG*)&_rcv_raw_size, -( nRecordLen + PACKHEADLASTSIZE )); return retval; } else { printf("处理错误的包!\n"); } } } while(0); return 0; }
ConnectionError::ConnectionError(const QByteArray &data) { if (data.size() == packetSize()) { m_code = qFromLittleEndian<qint32>(reinterpret_cast<const uchar *>(data.constData())); } }
void vPreProcess::run() { Stamp zynq_stamp; Stamp local_stamp; resolution resmod = res; resmod.height -= 1; resmod.width -= 1; int nm0 = 0, nm1 = 0, nm2 = 0, nm3 = 0, nm4 = 0; AE v; SkinEvent se; SkinSample ss; bool received_half_sample = false; int32_t salvage_sample[2] = {-1, 0}; while(true) { double pyt = zynq_stamp.getTime(); std::deque<AE> qleft, qright; std::deque<int32_t> qskin; std::deque<int32_t> qskinsamples; const std::vector<int32_t> *q = inPort.read(zynq_stamp); if(!q) break; delays.push_back((Time::now() - zynq_stamp.getTime())); if(pyt) intervals.push_back(zynq_stamp.getTime() - pyt); if(precheck) { nm0 = zynq_stamp.getCount(); if(nm3 && nm0 - nm1 == 1 && nm1 - nm2 > 1 && nm1 - nm3 > 2) { yWarning() << "LOST" << nm1-nm2-1 << "PACKETS [" << nm4 << nm3 << nm2 << nm1 << nm0 << "]" << q->size() << "packet size"; } nm4 = nm3; nm3 = nm2; nm2 = nm1; nm1 = nm0; } //unsigned int events_in_packet = 0; const int32_t *qi = q->data(); while ((size_t)(qi - q->data()) < q->size()) { if(IS_SKIN(*(qi+1))) { if(IS_SAMPLE(*(qi+1))) { qskinsamples.push_back(*(qi++)); //TS qskinsamples.push_back(*(qi++)); //VALUE/TAXEL } else { qskin.push_back(*(qi++)); //TS qskin.push_back(*(qi++)); //TAXEL } } else { // IS_VISION v.decode(qi); //precheck if(precheck && (v.x < 0 || v.x > resmod.width || v.y < 0 || v.y > resmod.height)) { yWarning() << "Event Corruption:" << v.getContent().toString(); continue; } //flipx and flipy if(flipx) v.x = resmod.width - v.x; if(flipy) v.y = resmod.height - v.y; //salt and pepper filter if(pepper && !thefilter.check(v.x, v.y, v.polarity, v.channel, v.stamp)) { v_dropped++; continue; } //undistortion (including rectification) if(undistort) { cv::Vec2i mapPix; if(v.getChannel() == 0) mapPix = leftMap.at<cv::Vec2i>(v.y, v.x); else mapPix = rightMap.at<cv::Vec2i>(v.y, v.x); //truncate to sensor bounds after mapping? if(truncate && (mapPix[0] < 0 || mapPix[0] > resmod.width || mapPix[1] < 0 || mapPix[1] > resmod.height)) { continue; } v.x = mapPix[0]; v.y = mapPix[1]; //std::cout.precision(30); //std::cout<<v.channel<<mapPix<<"timestamp:"<<pyt<<std::endl; } if(split && v.channel) { qright.push_back(v); } else { qleft.push_back(v); } } } if(qskinsamples.size() > 2) { //if we have skin samples //check if we need to fix the ordering if(IS_SSV(qskinsamples[1])) { // missing address if(received_half_sample) { // but we have it from last bottle qskinsamples.push_front(salvage_sample[1]); qskinsamples.push_front(salvage_sample[0]); } else { // otherwise we are misaligned due to missing data qskinsamples.pop_front(); qskinsamples.pop_front(); } } received_half_sample = false; //either case the half sample is no longer valid //check if we now have a cut event int samples_overrun = qskinsamples.size() % packetSize(SkinSample::tag); if(samples_overrun == 2) { salvage_sample[1] = qskinsamples.back(); qskinsamples.pop_back(); salvage_sample[0] = qskinsamples.back(); qskinsamples.pop_back(); received_half_sample = true; } else if(samples_overrun) { yError() << "samples cut by " << samples_overrun; } } v_total += qleft.size() + qright.size(); if(use_local_stamp) { local_stamp.update(); zynq_stamp = local_stamp; } if(qleft.size()) { outPortCamLeft.write(qleft, zynq_stamp); } if(qright.size()) { outPortCamRight.write(qright, zynq_stamp); } if(qskin.size()) { outPortSkin.write(qskin, zynq_stamp); } if(qskinsamples.size()) { outPortSkinSamples.write(qskinsamples, zynq_stamp); } } }
qint64 Packet::write(QIODevice &device) { return device.write((const char *)&packet, packetSize()); }