/**
* \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());
}
