/// <summary>
/// Add captured audio data to the circular queue of buffers ready for client reading.
/// </summary>
/// <param name="pData">Pointer to audio data to be added to queue.</param>
/// <param name="cbData">Number of bytes to be added to queue.</param>
void KinectAudioStream::QueueCapturedData(BYTE *pData, UINT cbData)
{
BYTE *pWriteData = NULL;
DWORD cbWriteData = 0;
DWORD cbMaxLength = 0;
if (cbData <= 0)
{
return;
}
if (NULL == m_CurrentWriteBuffer)
{
m_CurrentWriteBuffer = GetWriteBuffer();
}
m_CurrentWriteBuffer->GetBufferAndLength(&pWriteData, &cbWriteData);
m_CurrentWriteBuffer->GetMaxLength(&cbMaxLength);
if (cbWriteData + cbData < cbMaxLength)
{
memcpy(pWriteData + cbWriteData, pData, cbData);
m_CurrentWriteBuffer->SetLength(cbWriteData + cbData);
}
else
{
QueueCapturedBuffer(m_CurrentWriteBuffer);
m_CurrentWriteBuffer = GetWriteBuffer();
m_CurrentWriteBuffer->GetBufferAndLength(&pWriteData, &cbWriteData);
memcpy(pWriteData, pData, cbData);
m_CurrentWriteBuffer->SetLength(cbData);
}
}
bool KOSocket::Send(Packet * pkt)
{
if (!IsConnected() || pkt->size() + 1 > GetWriteBuffer().GetAllocatedSize())
return false;
bool r;
uint8 opcode = pkt->GetOpcode();
uint8 * out_stream = nullptr;
uint16 len = (uint16)(pkt->size() + 1);
if (isCryptoEnabled())
{
len += 5;
out_stream = new uint8[len];
*(uint16 *)&out_stream[0] = 0x1efc;
*(uint16 *)&out_stream[2] = (uint16)(m_sequence); // this isn't actually incremented here
out_stream[4] = 0;
out_stream[5] = pkt->GetOpcode();
if (pkt->size() > 0)
memcpy(&out_stream[6], pkt->contents(), pkt->size());
m_crypto.JvEncryptionFast(len, out_stream, out_stream);
}
else
{
out_stream = new uint8[len];
out_stream[0] = pkt->GetOpcode();
if (pkt->size() > 0)
memcpy(&out_stream[1], pkt->contents(), pkt->size());
}
BurstBegin();
if (GetWriteBuffer().GetSpace() < size_t(len + 6))
{
BurstEnd();
Disconnect();
return false;
}
r = BurstSend((const uint8*)"\xaa\x55", 2);
if (r) r = BurstSend((const uint8*)&len, 2);
if (r) r = BurstSend((const uint8*)out_stream, len);
if (r) r = BurstSend((const uint8*)"\x55\xaa", 2);
if (r) BurstPush();
BurstEnd();
delete [] out_stream;
return r;
}
void XnUncompressedDepthProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnUncompressedDepthProcessor::ProcessFramePacketChunk")
// when depth is uncompressed, we can just copy it directly to write buffer
XnBuffer* pWriteBuffer = GetWriteBuffer();
// make sure we have enough room
if (CheckWriteBufferForOverflow(nDataSize))
{
// sometimes, when packets are lost, we get uneven number of bytes, so we need to complete
// one byte, in order to keep UINT16 alignment
if (nDataSize % 2 != 0)
{
nDataSize--;
pData++;
}
// copy values. Make sure we do not get corrupted shifts
XnUInt16* pRaw = (XnUInt16*)(pData);
XnUInt16* pRawEnd = (XnUInt16*)(pData + nDataSize);
XnDepthPixel* pWriteBuf = (XnDepthPixel*)pWriteBuffer->GetUnsafeWritePointer();
while (pRaw < pRawEnd)
{
*pWriteBuf = GetOutput(XN_MIN(*pRaw, XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1));
++pRaw;
++pWriteBuf;
}
pWriteBuffer->UnsafeUpdateSize(nDataSize);
}
XN_PROFILING_END_SECTION
}
void AuthSocket::HandleReconnectProof()
{
/*
printf("Len: %u\n", this->GetReadBufferSize());
ByteBuffer buf(58);
buf.resize(58);
Read(58, const_cast<uint8*>(buf.contents()));
buf.hexlike();*/
if (!m_account)
return;
// Don't update when IP banned, but update anyway if it's an account ban
sLogonSQL->Execute("UPDATE accounts SET lastlogin=NOW(), lastip='%s' WHERE acct=%u;", GetRemoteIP().c_str(), m_account->AccountId);
//RemoveReadBufferBytes(GetReadBufferSize(), true);
GetReadBuffer().Remove(GetWriteBuffer().GetSize());
if(!m_account->SessionKey)
{
uint8 buffer[4];
buffer[0] = 3;
buffer[1] = 0;
buffer[2] = 1;
buffer[3] = 0;
Send(buffer, 4);
}
else
{
uint32 x = 3;
Send((const uint8*)&x, 4);
}
}
void VoiceChatClientSocket::SendPacket(WorldPacket* data)
{
//if((m_writeByteCount + len + 4) >= m_writeBufferSize)
//if( GetWriteBuffer().GetSpace() < (len+4) )
//if( m_writeByteCount + 4 + data->size() > m_writeBufferSize )
if( GetWriteBuffer().GetSpace() < (data->size()+4) )
{
Log.Error("VoiceChatHandler","!!! VOICE CHAT CLIENT SOCKET OVERLOAD !!!");
return;
}
uint16 opcode = data->GetOpcode();
uint32 sz = (uint32)data->size();
bool rv;
BurstBegin();
rv = BurstSend((const uint8*)&opcode, 2);
if(rv) BurstSend((const uint8*)&sz, 2);
if( sz > 0 && rv )
{
rv = BurstSend((const uint8*)data->contents(), (uint32)data->size());
}
Log.Debug("VoiceChatHandler","sent packet of %u bytes with op %u, buffer len is now %u", data->size(), data->GetOpcode(), GetWriteBuffer().GetSize());
if( rv )
BurstPush();
BurstEnd();
}
void MessageConnection::Write(Buffer& msg)
{
GetWriteBuffer().Appendf("%#B", &msg);
if (autoFlush)
Flush();
}
void MessageConnection::Write(Buffer& prefix, Buffer& msg)
{
unsigned length;
length = prefix.GetLength() + 1 + msg.GetLength();
GetWriteBuffer().Appendf("%u:%B:%B", length, &prefix, &msg);
if (autoFlush)
Flush();
}
void XnImageProcessor::OnEndOfFrame(const XnSensorProtocolResponseHeader* pHeader)
{
XnUInt32 nExpectedSize = CalculateExpectedSize();
if (GetWriteBuffer()->GetSize() != nExpectedSize)
{
xnLogWarning(XN_MASK_SENSOR_READ, "Read: Image buffer is corrupt. Size is %u (!= %u)", GetWriteBuffer()->GetSize(), nExpectedSize);
FrameIsCorrupted();
}
// call base
XnFrameStreamProcessor::OnEndOfFrame(pHeader);
}
void MessageConnection::Write(Message& msg)
{
// TODO: optimize
Buffer tmpBuffer;
msg.Write(tmpBuffer);
GetWriteBuffer().Appendf("%#B", &tmpBuffer);
if (autoFlush)
Flush();
}
void XnUncompressedYUVtoRGBImageProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnUncompressedYUVtoRGBImageProcessor::ProcessFramePacketChunk")
XnBuffer* pWriteBuffer = GetWriteBuffer();
if (m_ContinuousBuffer.GetSize() != 0)
{
// fill in to a whole element
XnUInt32 nReadBytes = XN_MIN(nDataSize, XN_YUV_TO_RGB_INPUT_ELEMENT_SIZE - m_ContinuousBuffer.GetSize());
m_ContinuousBuffer.UnsafeWrite(pData, nReadBytes);
pData += nReadBytes;
nDataSize -= nReadBytes;
if (m_ContinuousBuffer.GetSize() == XN_YUV_TO_RGB_INPUT_ELEMENT_SIZE)
{
if (CheckWriteBufferForOverflow(XN_YUV_TO_RGB_OUTPUT_ELEMENT_SIZE))
{
// process it
XnUInt32 nActualRead = 0;
XnUInt32 nOutputSize = pWriteBuffer->GetFreeSpaceInBuffer();
YUV422ToRGB888(m_ContinuousBuffer.GetData(), pWriteBuffer->GetUnsafeWritePointer(), XN_YUV_TO_RGB_INPUT_ELEMENT_SIZE, &nActualRead, &nOutputSize);
pWriteBuffer->UnsafeUpdateSize(XN_YUV_TO_RGB_OUTPUT_ELEMENT_SIZE);
}
m_ContinuousBuffer.Reset();
}
}
if (CheckWriteBufferForOverflow(nDataSize / XN_YUV_TO_RGB_INPUT_ELEMENT_SIZE * XN_YUV_TO_RGB_OUTPUT_ELEMENT_SIZE))
{
XnUInt32 nActualRead = 0;
XnUInt32 nOutputSize = pWriteBuffer->GetFreeSpaceInBuffer();
YUV422ToRGB888(pData, pWriteBuffer->GetUnsafeWritePointer(), nDataSize, &nActualRead, &nOutputSize);
pWriteBuffer->UnsafeUpdateSize(nOutputSize);
pData += nActualRead;
nDataSize -= nActualRead;
// if we have any bytes left, store them for next packet.
if (nDataSize > 0)
{
// no need to check for overflow. there can not be a case in which more than XN_INPUT_ELEMENT_SIZE
// are left.
m_ContinuousBuffer.UnsafeWrite(pData, nDataSize);
}
}
XN_PROFILING_END_SECTION
}
void XnPassThroughImageProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* /*pHeader*/, const XnUChar* pData, XnUInt32 /*nDataOffset*/, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnUncompressedYUVImageProcessor::ProcessFramePacketChunk")
// when image is uncompressed, we can just copy it directly to write buffer
XnBuffer* pWriteBuffer = GetWriteBuffer();
// make sure we have enough room
if (CheckWriteBufferForOverflow(nDataSize))
{
pWriteBuffer->UnsafeWrite(pData, nDataSize);
}
XN_PROFILING_END_SECTION
}
void MessageConnection::Write(Buffer& prefix, Message& msg)
{
// TODO: optimize
unsigned length;
Buffer tmpBuffer;
msg.Write(tmpBuffer);
length = prefix.GetLength() + 1 + tmpBuffer.GetLength();
GetWriteBuffer().Appendf("%u:%B:%B", length, &prefix, &tmpBuffer);
if (autoFlush)
Flush();
}
XnStatus XnPacked11DepthProcessor::Unpack11to16(const XnUInt8* pcInput, const XnUInt32 nInputSize, XnUInt32* pnActualRead)
{
const XnUInt8* pOrigInput = pcInput;
XnUInt32 nElements = nInputSize / XN_INPUT_ELEMENT_SIZE; // floored
XnUInt32 nNeededOutput = nElements * XN_OUTPUT_ELEMENT_SIZE;
*pnActualRead = 0;
XnBuffer* pWriteBuffer = GetWriteBuffer();
if (!CheckWriteBufferForOverflow(nNeededOutput))
{
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
XnUInt16* pnOutput = (XnUInt16*)pWriteBuffer->GetUnsafeWritePointer();
// Convert the 11bit packed data into 16bit shorts
for (XnUInt32 nElem = 0; nElem < nElements; ++nElem)
{
// input: 0, 1, 2,3, 4, 5, 6,7, 8, 9,10
// -,---,---,-,---,---,---,-,---,---,-
// bits: 8,3,5,6,2,8,1,7,4,4,7,1,8,2,6,5,3,8
// ---,---,-----,---,---,-----,---,---
// output: 0, 1, 2, 3, 4, 5, 6, 7
pnOutput[0] = GetOutput((XN_TAKE_BITS(pcInput[0],8,0) << 3) | XN_TAKE_BITS(pcInput[1],3,5));
pnOutput[1] = GetOutput((XN_TAKE_BITS(pcInput[1],5,0) << 6) | XN_TAKE_BITS(pcInput[2],6,2));
pnOutput[2] = GetOutput((XN_TAKE_BITS(pcInput[2],2,0) << 9) | (XN_TAKE_BITS(pcInput[3],8,0) << 1) | XN_TAKE_BITS(pcInput[4],1,7));
pnOutput[3] = GetOutput((XN_TAKE_BITS(pcInput[4],7,0) << 4) | XN_TAKE_BITS(pcInput[5],4,4));
pnOutput[4] = GetOutput((XN_TAKE_BITS(pcInput[5],4,0) << 7) | XN_TAKE_BITS(pcInput[6],7,1));
pnOutput[5] = GetOutput((XN_TAKE_BITS(pcInput[6],1,0) << 10) | (XN_TAKE_BITS(pcInput[7],8,0) << 2) | XN_TAKE_BITS(pcInput[8],2,6));
pnOutput[6] = GetOutput((XN_TAKE_BITS(pcInput[8],6,0) << 5) | XN_TAKE_BITS(pcInput[9],5,3));
pnOutput[7] = GetOutput((XN_TAKE_BITS(pcInput[9],3,0) << 8) | XN_TAKE_BITS(pcInput[10],8,0));
pcInput += XN_INPUT_ELEMENT_SIZE;
pnOutput += 8;
}
*pnActualRead = (XnUInt32)(pcInput - pOrigInput);
pWriteBuffer->UnsafeUpdateSize(nNeededOutput);
return XN_STATUS_OK;
}
void XnUncompressedDepthProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* /*pHeader*/, const XnUChar* pData, XnUInt32 /*nDataOffset*/, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnUncompressedDepthProcessor::ProcessFramePacketChunk")
// when depth is uncompressed, we can just copy it directly to write buffer
XnBuffer* pWriteBuffer = GetWriteBuffer();
// Check there is enough room for the depth pixels
if (CheckDepthBufferForOverflow(nDataSize))
{
// sometimes, when packets are lost, we get uneven number of bytes, so we need to complete
// one byte, in order to keep UINT16 alignment
if (nDataSize % 2 != 0)
{
nDataSize--;
pData++;
}
// copy values. Make sure we do not get corrupted shifts
XnUInt16* pRaw = (XnUInt16*)(pData);
XnUInt16* pRawEnd = (XnUInt16*)(pData + nDataSize);
OniDepthPixel* pDepthBuf = GetDepthOutputBuffer();
OniDepthPixel* pShiftBuf = GetShiftsOutputBuffer();
XnUInt16 shift;
while (pRaw < pRawEnd)
{
shift = (((*pRaw) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (*pRaw) : 0);
*pShiftBuf = shift;
*pDepthBuf = GetOutput(shift);
++pRaw;
++pDepthBuf;
++pShiftBuf;
}
pWriteBuffer->UnsafeUpdateSize(nDataSize);
}
XN_PROFILING_END_SECTION
}
OUTPACKET_RESULT WorldSocket::_OutPacket(uint16 opcode, size_t len, const void* data, bool InWorld)
{
bool rv;
if(!IsConnected())
return OUTPACKET_RESULT_NOT_CONNECTED;
if( GetWriteBuffer()->GetSpace() < (len+4) )
return OUTPACKET_RESULT_NO_ROOM_IN_BUFFER;
/* if(InWorld)
{
if(Uniques.find(opcode) == Uniques.end())
{
Log.Notice("", "Sent packet %s (0x%03X)", LookupOpcodeName(opcode), uint(opcode), uint(opcode));
Uniques.insert(opcode);
}
}
else if(opcode != SMSG_UPDATE_OBJECT && opcode != SMSG_PONG && opcode != SMSG_WORLD_STATE_UI_TIMER_UPDATE && opcode != SMSG_WEATHER)
printf("Sent packet %s (0x%03X)\n", LookupOpcodeName(opcode), uint(opcode), uint(opcode));*/
LockWriteBuffer();
// Encrypt the packet
// First, create the header.
ServerPktHeader Header;
Header.cmd = opcode;
Header.size = ntohs((uint16)len + 2);
_crypt.EncryptSend((uint8*)&Header, sizeof (ServerPktHeader));
// Pass the header to our send buffer
rv = WriteButHold((const uint8*)&Header, 4);
// Pass the rest of the packet to our send buffer (if there is any)
if(len > 0 && rv)
rv = Write((const uint8*)data, (uint32)len);
else if(rv)
rv = ForceSend();
UnlockWriteBuffer();
if(len > 0 && rv && !bServerShutdown)
sWorld.NetworkStressOut += float(float(len+4)/1024);
return rv ? OUTPACKET_RESULT_SUCCESS : OUTPACKET_RESULT_SOCKET_ERROR;
}
OUTPACKET_RESULT WorldSocket::_OutPacket(uint16 opcode, size_t len, const void* data)
{
bool rv;
if(!IsConnected())
return OUTPACKET_RESULT_NOT_CONNECTED;
BurstBegin();
//if((m_writeByteCount + len + 4) >= m_writeBufferSize)
if( GetWriteBuffer().GetSpace() < (len+4) )
{
BurstEnd();
return OUTPACKET_RESULT_NO_ROOM_IN_BUFFER;
}
// Packet logger :)
sWorldLog.LogPacket((uint32)len, opcode, (const uint8*)data, 1);
// Encrypt the packet
// First, create the header.
ServerPktHeader Header;
#ifdef USING_BIG_ENDIAN
Header.size = len + 2;
Header.cmd = swap16(opcode);
#else
Header.cmd = opcode;
Header.size = ntohs((uint16)len + 2);
#endif
_crypt.EncryptFourSend((uint8*)&Header);
// Pass the header to our send buffer
rv = BurstSend((const uint8*)&Header, 4);
// Pass the rest of the packet to our send buffer (if there is any)
if(len > 0 && rv)
{
rv = BurstSend((const uint8*)data, (uint32)len);
}
if(rv) BurstPush();
BurstEnd();
return rv ? OUTPACKET_RESULT_SUCCESS : OUTPACKET_RESULT_SOCKET_ERROR;
}
/*----------------------------------------------------------------------------*/
uint8_t *
EthernetOutput(struct mtcp_manager *mtcp, struct pkt_ctx *pctx,
uint16_t h_proto, int nif, unsigned char* dst_haddr, uint16_t iplen,
uint32_t cur_ts)
{
uint8_t *buf;
struct ethhdr *ethh;
int i;
#if E_PSIO || USE_CHUNK_BUF
if (!mtcp->iom->get_wptr) {
TRACE_INFO("get_wptr() in io_module is undefined.");
return NULL;
}
buf = mtcp->iom->get_wptr(mtcp->ctx, nif, iplen + ETHERNET_HEADER_LEN);
#else
buf = GetWriteBuffer(mtcp->ctx,
BUF_RET_MAYBE, nif, iplen + ETHERNET_HEADER_LEN);
#endif
if (!buf) {
TRACE_DBG("Failed to get available write buffer\n");
return NULL;
}
TRACE_DBG("dst_hwaddr: %02X:%02X:%02X:%02X:%02X:%02X\n",
stream->sndvar->d_haddr[0], stream->sndvar->d_haddr[1],
stream->sndvar->d_haddr[2], stream->sndvar->d_haddr[3],
stream->sndvar->d_haddr[4], stream->sndvar->d_haddr[5]);
ethh = (struct ethhdr *)buf;
for (i = 0; i < ETH_ALEN; i++) {
ethh->h_source[i] = g_config.mos->netdev_table->ent[nif]->haddr[i];
ethh->h_dest[i] = dst_haddr[i];
}
ethh->h_proto = htons(h_proto);
if (pctx)
FillOutPacketEthContext(pctx, cur_ts, nif,
ethh, iplen + ETHERNET_HEADER_LEN);
return (uint8_t *)(ethh + 1);
}
void XnUncompressedBayerProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnUncompressedBayerProcessor::ProcessFramePacketChunk")
// if output format is Gray8, we can write directly to output buffer. otherwise, we need
// to write to a temp buffer.
XnBuffer* pWriteBuffer = (GetStream()->GetOutputFormat() == XN_OUTPUT_FORMAT_GRAYSCALE8) ? GetWriteBuffer() : &m_UncompressedBayerBuffer;
// make sure we have enough room
if (CheckWriteBufferForOverflow(nDataSize))
{
pWriteBuffer->UnsafeWrite(pData, nDataSize);
}
XN_PROFILING_END_SECTION
}
void XnBayerImageProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnBayerImageProcessor::ProcessFramePacketChunk")
// if output format is Gray8, we can write directly to output buffer. otherwise, we need
// to write to a temp buffer.
XnBuffer* pWriteBuffer = (GetStream()->GetOutputFormat() == XN_OUTPUT_FORMAT_GRAYSCALE8) ? GetWriteBuffer() : &m_UncompressedBayerBuffer;
const XnUChar* pBuf = NULL;
XnUInt32 nBufSize = 0;
// check if we have bytes stored from previous calls
if (m_ContinuousBuffer.GetSize() > 0)
{
// we have no choice. We need to append current buffer to previous bytes
if (m_ContinuousBuffer.GetFreeSpaceInBuffer() < nDataSize)
{
xnLogWarning(XN_MASK_SENSOR_PROTOCOL_DEPTH, "Bad overflow image! %d", m_ContinuousBuffer.GetSize());
FrameIsCorrupted();
}
else
{
m_ContinuousBuffer.UnsafeWrite(pData, nDataSize);
}
pBuf = m_ContinuousBuffer.GetData();
nBufSize = m_ContinuousBuffer.GetSize();
}
else
{
// we can process the data directly
pBuf = pData;
nBufSize = nDataSize;
}
XnUInt32 nOutputSize = pWriteBuffer->GetFreeSpaceInBuffer();
XnUInt32 nWrittenOutput = nOutputSize;
XnUInt32 nActualRead = 0;
XnBool bLastPart = pHeader->nType == XN_SENSOR_PROTOCOL_RESPONSE_IMAGE_END && (nDataOffset + nDataSize) == pHeader->nBufSize;
XnStatus nRetVal = XnStreamUncompressImageNew(pBuf, nBufSize, pWriteBuffer->GetUnsafeWritePointer(),
&nWrittenOutput, (XnUInt16)GetActualXRes(), &nActualRead, bLastPart);
if (nRetVal != XN_STATUS_OK)
{
xnLogWarning(XN_MASK_SENSOR_PROTOCOL_IMAGE, "Image decompression failed: %s (%d of %d, requested %d, last %d)", xnGetStatusString(nRetVal), nWrittenOutput, nBufSize, nOutputSize, bLastPart);
FrameIsCorrupted();
}
pWriteBuffer->UnsafeUpdateSize(nWrittenOutput);
nBufSize -= nActualRead;
m_ContinuousBuffer.Reset();
// if we have any bytes left, keep them for next time
if (nBufSize > 0)
{
pBuf += nActualRead;
m_ContinuousBuffer.UnsafeWrite(pBuf, nBufSize);
}
XN_PROFILING_END_SECTION
}
XnStatus XnPacked11DepthProcessor::Unpack11to16(const XnUInt8* pcInput, const XnUInt32 nInputSize, XnUInt32* pnActualRead)
{
const XnUInt8* pOrigInput = pcInput;
XnUInt32 nElements = nInputSize / XN_INPUT_ELEMENT_SIZE; // floored
XnUInt32 nNeededOutput = nElements * XN_OUTPUT_ELEMENT_SIZE;
*pnActualRead = 0;
XnBuffer* pWriteBuffer = GetWriteBuffer();
// Check there is enough room for the depth pixels
if (!CheckWriteBufferForOverflow(nNeededOutput))
{
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
XnUInt16* pnOutput = (XnUInt16*)pWriteBuffer->GetUnsafeWritePointer();
XnUInt16 a0,a1,a2,a3,a4,a5,a6,a7;
#ifdef XN_NEON
XnUInt16 depth[8];
uint16x8_t Q0;
#endif
// Convert the 11bit packed data into 16bit shorts
for (XnUInt32 nElem = 0; nElem < nElements; ++nElem)
{
if(m_nScaleFactor > 1)
{
XnUInt32 px = m_nOffsetInFrame%m_CurrentVideoMode.resolutionX;
XnUInt32 py = (m_nOffsetInFrame)/m_CurrentVideoMode.resolutionX;
if(py%m_nScaleFactor != 0)
{
// Skip as many pixels as possible
XnUInt32 nEltsToSkip =
XN_MIN(nElements - nElem,
(m_CurrentVideoMode.resolutionX - px)/8
+ (m_nScaleFactor-(py%m_nScaleFactor) - 1)*m_CurrentVideoMode.resolutionX/8);
// ::memset(pnOutput, 0, nEltsToSkip*8*sizeof(XnUInt16));
pcInput += nEltsToSkip*XN_INPUT_ELEMENT_SIZE;
pnOutput += nEltsToSkip*8;
m_nOffsetInFrame += nEltsToSkip*8;
nElem += (nEltsToSkip-1);
continue;
}
}
// input: 0, 1, 2,3, 4, 5, 6,7, 8, 9,10
// -,---,---,-,---,---,---,-,---,---,-
// bits: 8,3,5,6,2,8,1,7,4,4,7,1,8,2,6,5,3,8
// ---,---,-----,---,---,-----,---,---
// output: 0, 1, 2, 3, 4, 5, 6, 7
if(m_nScaleFactor == 2)
{
a0 = (XN_TAKE_BITS(pcInput[0],8,0) << 3) | XN_TAKE_BITS(pcInput[1],3,5);
a2 = (XN_TAKE_BITS(pcInput[2],2,0) << 9) | (XN_TAKE_BITS(pcInput[3],8,0) << 1) | XN_TAKE_BITS(pcInput[4],1,7);
a4 = (XN_TAKE_BITS(pcInput[5],4,0) << 7) | XN_TAKE_BITS(pcInput[6],7,1);
a6 = (XN_TAKE_BITS(pcInput[8],6,0) << 5) | XN_TAKE_BITS(pcInput[9],5,3);
}
else if(m_nScaleFactor == 4)
{
a0 = (XN_TAKE_BITS(pcInput[0],8,0) << 3) | XN_TAKE_BITS(pcInput[1],3,5);
a4 = (XN_TAKE_BITS(pcInput[5],4,0) << 7) | XN_TAKE_BITS(pcInput[6],7,1);
}
else
{
a0 = (XN_TAKE_BITS(pcInput[0],8,0) << 3) | XN_TAKE_BITS(pcInput[1],3,5);
a1 = (XN_TAKE_BITS(pcInput[1],5,0) << 6) | XN_TAKE_BITS(pcInput[2],6,2);
a2 = (XN_TAKE_BITS(pcInput[2],2,0) << 9) | (XN_TAKE_BITS(pcInput[3],8,0) << 1) | XN_TAKE_BITS(pcInput[4],1,7);
a3 = (XN_TAKE_BITS(pcInput[4],7,0) << 4) | XN_TAKE_BITS(pcInput[5],4,4);
a4 = (XN_TAKE_BITS(pcInput[5],4,0) << 7) | XN_TAKE_BITS(pcInput[6],7,1);
a5 = (XN_TAKE_BITS(pcInput[6],1,0) << 10) | (XN_TAKE_BITS(pcInput[7],8,0) << 2) | XN_TAKE_BITS(pcInput[8],2,6);
a6 = (XN_TAKE_BITS(pcInput[8],6,0) << 5) | XN_TAKE_BITS(pcInput[9],5,3);
a7 = (XN_TAKE_BITS(pcInput[9],3,0) << 8) | XN_TAKE_BITS(pcInput[10],8,0);
}
#ifdef XN_NEON
depth[0] = GetOutput(a0);
depth[1] = GetOutput(a1);
depth[2] = GetOutput(a2);
depth[3] = GetOutput(a3);
depth[4] = GetOutput(a4);
depth[5] = GetOutput(a5);
depth[6] = GetOutput(a6);
depth[7] = GetOutput(a7);
// Load
Q0 = vld1q_u16(depth);
// Store
vst1q_u16(pnOutput, Q0);
#else
if(m_nScaleFactor == 2)
{
*pnOutput++ = GetOutput(a0);
*pnOutput++ = 0;
*pnOutput++ = GetOutput(a2);
*pnOutput++ = 0;
*pnOutput++ = GetOutput(a4);
*pnOutput++ = 0;
*pnOutput++ = GetOutput(a6);
*pnOutput++ = 0;
}
else if(m_nScaleFactor == 4)
{
*pnOutput++ = GetOutput(a0);
*pnOutput++ = 0;
*pnOutput++ = 0;
*pnOutput++ = 0;
*pnOutput++ = GetOutput(a4);
*pnOutput++ = 0;
*pnOutput++ = 0;
*pnOutput++ = 0;
}
else
{
*pnOutput++ = GetOutput(a0);
*pnOutput++ = GetOutput(a1);
*pnOutput++ = GetOutput(a2);
*pnOutput++ = GetOutput(a3);
*pnOutput++ = GetOutput(a4);
*pnOutput++ = GetOutput(a5);
*pnOutput++ = GetOutput(a6);
*pnOutput++ = GetOutput(a7);
}
#endif
pcInput += XN_INPUT_ELEMENT_SIZE;
m_nOffsetInFrame+=8;
}
*pnActualRead = (XnUInt32)(pcInput - pOrigInput);
pWriteBuffer->UnsafeUpdateSize(nNeededOutput);
return XN_STATUS_OK;
}
XnStatus XnPacked12DepthProcessor::Unpack12to16(const XnUInt8* pcInput, const XnUInt32 nInputSize, XnUInt32* pnActualRead)
{
const XnUInt8* pOrigInput = pcInput;
XnUInt32 nElements = nInputSize / XN_INPUT_ELEMENT_SIZE; // floored
XnUInt32 nNeededOutput = nElements * XN_OUTPUT_ELEMENT_SIZE;
*pnActualRead = 0;
XnBuffer* pWriteBuffer = GetWriteBuffer();
if (!CheckDepthBufferForOverflow(nNeededOutput))
{
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
XnUInt16* pnOutput = GetDepthOutputBuffer();
XnUInt16* pShiftOut = GetShiftsOutputBuffer();
XnUInt16 shift[16];
#ifdef XN_NEON
XnUInt16 depth[16];
uint8x8x3_t inD3;
uint8x8_t rshft4D, lshft4D;
uint16x8_t rshft4Q, lshft4Q;
uint16x8_t depthQ;
uint16x8x2_t shiftQ2;
#endif
// Convert the 11bit packed data into 16bit shorts
for (XnUInt32 nElem = 0; nElem < nElements; ++nElem)
{
#ifndef XN_NEON
// input: 0, 1,2,3, 4,5,6, 7,8,9, 10,11,12, 13,14,15, 16,17,18, 19,20,21, 22,23
// -,---,-,-,---,-,-,---,-,-,---,--,--,---,--,--,---,--,--,---,--,--,---,--
// bits: 8,4,4,8,8,4,4,8,8,4,4,8,8,4,4, 8, 8,4,4, 8, 8,4,4, 8, 8,4,4, 8, 8,4,4, 8
// ---,---,---,---,---,---,---,----,----,----,----,----,----,----,----,----
// output: 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15
shift[0] = (XN_TAKE_BITS(pcInput[0],8,0) << 4) | XN_TAKE_BITS(pcInput[1],4,4);
shift[1] = (XN_TAKE_BITS(pcInput[1],4,0) << 8) | XN_TAKE_BITS(pcInput[2],8,0);
shift[2] = (XN_TAKE_BITS(pcInput[3],8,0) << 4) | XN_TAKE_BITS(pcInput[4],4,4);
shift[3] = (XN_TAKE_BITS(pcInput[4],4,0) << 8) | XN_TAKE_BITS(pcInput[5],8,0);
shift[4] = (XN_TAKE_BITS(pcInput[6],8,0) << 4) | XN_TAKE_BITS(pcInput[7],4,4);
shift[5] = (XN_TAKE_BITS(pcInput[7],4,0) << 8) | XN_TAKE_BITS(pcInput[8],8,0);
shift[6] = (XN_TAKE_BITS(pcInput[9],8,0) << 4) | XN_TAKE_BITS(pcInput[10],4,4);
shift[7] = (XN_TAKE_BITS(pcInput[10],4,0) << 8) | XN_TAKE_BITS(pcInput[11],8,0);
shift[8] = (XN_TAKE_BITS(pcInput[12],8,0) << 4) | XN_TAKE_BITS(pcInput[13],4,4);
shift[9] = (XN_TAKE_BITS(pcInput[13],4,0) << 8) | XN_TAKE_BITS(pcInput[14],8,0);
shift[10] = (XN_TAKE_BITS(pcInput[15],8,0) << 4) | XN_TAKE_BITS(pcInput[16],4,4);
shift[11] = (XN_TAKE_BITS(pcInput[16],4,0) << 8) | XN_TAKE_BITS(pcInput[17],8,0);
shift[12] = (XN_TAKE_BITS(pcInput[18],8,0) << 4) | XN_TAKE_BITS(pcInput[19],4,4);
shift[13] = (XN_TAKE_BITS(pcInput[19],4,0) << 8) | XN_TAKE_BITS(pcInput[20],8,0);
shift[14] = (XN_TAKE_BITS(pcInput[21],8,0) << 4) | XN_TAKE_BITS(pcInput[22],4,4);
shift[15] = (XN_TAKE_BITS(pcInput[22],4,0) << 8) | XN_TAKE_BITS(pcInput[23],8,0);
pShiftOut[0] = (((shift[0]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[0]) : 0);
pShiftOut[1] = (((shift[1]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[1]) : 0);
pShiftOut[2] = (((shift[2]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[2]) : 0);
pShiftOut[3] = (((shift[3]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[3]) : 0);
pShiftOut[4] = (((shift[4]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[4]) : 0);
pShiftOut[5] = (((shift[5]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[5]) : 0);
pShiftOut[6] = (((shift[6]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[6]) : 0);
pShiftOut[7] = (((shift[7]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[7]) : 0);
pShiftOut[8] = (((shift[0]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[8]) : 0);
pShiftOut[9] = (((shift[1]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[9]) : 0);
pShiftOut[10] = (((shift[2]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[10]) : 0);
pShiftOut[11] = (((shift[3]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[11]) : 0);
pShiftOut[12] = (((shift[4]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[12]) : 0);
pShiftOut[13] = (((shift[5]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[13]) : 0);
pShiftOut[14] = (((shift[6]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[14]) : 0);
pShiftOut[15] = (((shift[7]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[15]) : 0);
pnOutput[0] = GetOutput(shift[0]);
pnOutput[1] = GetOutput(shift[1]);
pnOutput[2] = GetOutput(shift[2]);
pnOutput[3] = GetOutput(shift[3]);
pnOutput[4] = GetOutput(shift[4]);
pnOutput[5] = GetOutput(shift[5]);
pnOutput[6] = GetOutput(shift[6]);
pnOutput[7] = GetOutput(shift[7]);
pnOutput[8] = GetOutput(shift[8]);
pnOutput[9] = GetOutput(shift[9]);
pnOutput[10] = GetOutput(shift[10]);
pnOutput[11] = GetOutput(shift[11]);
pnOutput[12] = GetOutput(shift[12]);
pnOutput[13] = GetOutput(shift[13]);
pnOutput[14] = GetOutput(shift[14]);
pnOutput[15] = GetOutput(shift[15]);
#else
// input: 0, 1,2 (X8)
// -,---,-
// bits: 8,4,4,8 (X8)
// ---,---
// output: 0, 1 (X8)
// Split 24 bytes into 3 vectors (64 bit each)
inD3 = vld3_u8(pcInput);
// rshft4D0 contains 4 MSB of second vector (placed at offset 0)
rshft4D = vshr_n_u8(inD3.val[1], 4);
// lshft4D0 contains 4 LSB of second vector (placed at offset 4)
lshft4D = vshl_n_u8(inD3.val[1], 4);
// Expand 64 bit vectors to 128 bit (8 values of 16 bits)
shiftQ2.val[0] = vmovl_u8(inD3.val[0]);
shiftQ2.val[1] = vmovl_u8(inD3.val[2]);
rshft4Q = vmovl_u8(rshft4D);
lshft4Q = vmovl_u8(lshft4D);
// Even indexed shift = 8 bits from first vector + 4 MSB bits of second vector
shiftQ2.val[0] = vshlq_n_u16(shiftQ2.val[0], 4);
shiftQ2.val[0] = vorrq_u16(shiftQ2.val[0], rshft4Q);
// Odd indexed shift = 4 LSB bits of second vector + 8 bits from third vector
lshft4Q = vshlq_n_u16(lshft4Q, 4);
shiftQ2.val[1] = vorrq_u16(shiftQ2.val[1], lshft4Q);
// Interleave shift values to a single vector
vst2q_u16(shift, shiftQ2);
shift[0] = (((shift[0]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[0]) : 0);
shift[1] = (((shift[1]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[1]) : 0);
shift[2] = (((shift[2]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[2]) : 0);
shift[3] = (((shift[3]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[3]) : 0);
shift[4] = (((shift[4]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[4]) : 0);
shift[5] = (((shift[5]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[5]) : 0);
shift[6] = (((shift[6]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[6]) : 0);
shift[7] = (((shift[7]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[7]) : 0);
shift[8] = (((shift[0]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[8]) : 0);
shift[9] = (((shift[1]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[9]) : 0);
shift[10] = (((shift[2]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[10]) : 0);
shift[11] = (((shift[3]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[11]) : 0);
shift[12] = (((shift[4]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[12]) : 0);
shift[13] = (((shift[5]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[13]) : 0);
shift[14] = (((shift[6]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[14]) : 0);
shift[15] = (((shift[7]) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (shift[15]) : 0);
depth[0] = GetOutput(shift[0]);
depth[1] = GetOutput(shift[1]);
depth[2] = GetOutput(shift[2]);
depth[3] = GetOutput(shift[3]);
depth[4] = GetOutput(shift[4]);
depth[5] = GetOutput(shift[5]);
depth[6] = GetOutput(shift[6]);
depth[7] = GetOutput(shift[7]);
// Load
depthQ = vld1q_u16(depth);
//Store
vst1q_u16(pnOutput, depthQ);
// Load
depthQ = vld1q_u16(shift);
// Store
vst1q_u16(pShiftOut, depthQ);
depth[8] = GetOutput(shift[8]);
depth[9] = GetOutput(shift[9]);
depth[10] = GetOutput(shift[10]);
depth[11] = GetOutput(shift[11]);
depth[12] = GetOutput(shift[12]);
depth[13] = GetOutput(shift[13]);
depth[14] = GetOutput(shift[14]);
depth[15] = GetOutput(shift[15]);
// Load
depthQ = vld1q_u16(depth + 8);
// Store
vst1q_u16(pnOutput + 8, depthQ);
// Load
depthQ = vld1q_u16(shift + 8);
// Store
vst1q_u16(pShiftOut + 8, depthQ);
#endif
pcInput += XN_INPUT_ELEMENT_SIZE;
pnOutput += 16;
pShiftOut += 16;
}
*pnActualRead = (XnUInt32)(pcInput - pOrigInput);
pWriteBuffer->UnsafeUpdateSize(nNeededOutput);
return XN_STATUS_OK;
}
void XnFrameStreamProcessor::WriteBufferOverflowed()
{
XnBuffer* pBuffer = GetWriteBuffer();
xnLogWarning(XN_MASK_SENSOR_PROTOCOL, "%s Frame Buffer overflow! current size: %d", m_csName, pBuffer->GetSize());
FrameIsCorrupted();
}
void AuthSocket::HandleReconnectProof()
{
if( m_account == NULL )
return;
// Load sessionkey from account database.
QueryResult * result = sLogonSQL->Query ("SELECT SessionKey FROM accounts WHERE acct = %u", m_account->AccountId);
if(result)
{
Field * field = result->Fetch();
K.SetHexStr(field[0].GetString ());
delete result;
}
else
{
// Disconnect if the sessionkey invalid or not found
DEBUG_LOG("AuthReConnectProof","No matching SessionKey found while user %s tried to login.", AccountName.c_str());
Disconnect();
return;
}
if(GetBuild() <= 6005)
{
GetReadBuffer()->Remove(GetWriteBuffer()->GetSize());
if(!m_account->SessionKey)
{
uint8 buffer[4];
buffer[0] = 3;
buffer[1] = 0;
buffer[2] = 1;
buffer[3] = 0;
Send(buffer, 4);
}
else
{
uint32 x = 3;
Send((const uint8*)&x, 4);
}
return;
}
if(GetReadBuffer()->GetSize() < sizeof(sAuthLogonProofKey_C))
return;
sAuthLogonProofKey_C lp;
GetReadBuffer()->Read(&lp, sizeof(sAuthLogonProofKey_C));
BigNumber A;
A.SetBinary(lp.R1, 16);
Sha1Hash sha;
sha.Initialize();
sha.UpdateData(AccountName);
sha.UpdateBigNumbers(&A, &rs, &K, 0);
sha.Finalize();
if (!memcmp(sha.GetDigest(), lp.R2, SHA_DIGEST_LENGTH))
{
///- Sending response
ByteBuffer pkt;
pkt << (uint8) 0x03; //ReconnectProof
pkt << (uint8) 0x00;
pkt << (uint16) 0x00; // 2 bytes zeros
Send(pkt.contents(), uint32(pkt.size()));
// we're authenticated now :)
m_authenticated = true;
DEBUG_LOG("AuthReConnectProof","Authentication Success.");
}
else
DEBUG_LOG("AuthReConnectProof","Authentication Failed.");
}
XnStatus XnPacked11DepthProcessor::Unpack11to16(const XnUInt8* pcInput, const XnUInt32 nInputSize, XnUInt32* pnActualRead)
{
const XnUInt8* pOrigInput = pcInput;
XnUInt32 nElements = nInputSize / XN_INPUT_ELEMENT_SIZE; // floored
XnUInt32 nNeededOutput = nElements * XN_OUTPUT_ELEMENT_SIZE;
*pnActualRead = 0;
XnBuffer* pWriteBuffer = GetWriteBuffer();
// Check there is enough room for the depth pixels
if (!CheckDepthBufferForOverflow(nNeededOutput))
{
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
XnUInt16* pShiftOut = GetShiftsOutputBuffer();
XnUInt16* pnOutput = GetDepthOutputBuffer();
XnUInt16 a0,a1,a2,a3,a4,a5,a6,a7;
#ifdef XN_NEON
XnUInt16 shift[8];
XnUInt16 depth[8];
uint16x8_t Q0;
#endif
// Convert the 11bit packed data into 16bit shorts
for (XnUInt32 nElem = 0; nElem < nElements; ++nElem)
{
// input: 0, 1, 2,3, 4, 5, 6,7, 8, 9,10
// -,---,---,-,---,---,---,-,---,---,-
// bits: 8,3,5,6,2,8,1,7,4,4,7,1,8,2,6,5,3,8
// ---,---,-----,---,---,-----,---,---
// output: 0, 1, 2, 3, 4, 5, 6, 7
a0 = (XN_TAKE_BITS(pcInput[0],8,0) << 3) | XN_TAKE_BITS(pcInput[1],3,5);
a1 = (XN_TAKE_BITS(pcInput[1],5,0) << 6) | XN_TAKE_BITS(pcInput[2],6,2);
a2 = (XN_TAKE_BITS(pcInput[2],2,0) << 9) | (XN_TAKE_BITS(pcInput[3],8,0) << 1) | XN_TAKE_BITS(pcInput[4],1,7);
a3 = (XN_TAKE_BITS(pcInput[4],7,0) << 4) | XN_TAKE_BITS(pcInput[5],4,4);
a4 = (XN_TAKE_BITS(pcInput[5],4,0) << 7) | XN_TAKE_BITS(pcInput[6],7,1);
a5 = (XN_TAKE_BITS(pcInput[6],1,0) << 10) | (XN_TAKE_BITS(pcInput[7],8,0) << 2) | XN_TAKE_BITS(pcInput[8],2,6);
a6 = (XN_TAKE_BITS(pcInput[8],6,0) << 5) | XN_TAKE_BITS(pcInput[9],5,3);
a7 = (XN_TAKE_BITS(pcInput[9],3,0) << 8) | XN_TAKE_BITS(pcInput[10],8,0);
#ifdef XN_NEON
shift[0] = (((a0) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a0) : 0);
shift[1] = (((a1) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a1) : 0);
shift[2] = (((a2) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a2) : 0);
shift[3] = (((a3) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a3) : 0);
shift[4] = (((a4) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a4) : 0);
shift[5] = (((a5) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a5) : 0);
shift[6] = (((a6) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a6) : 0);
shift[7] = (((a7) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a7) : 0);
depth[0] = GetOutput(a0);
depth[1] = GetOutput(a1);
depth[2] = GetOutput(a2);
depth[3] = GetOutput(a3);
depth[4] = GetOutput(a4);
depth[5] = GetOutput(a5);
depth[6] = GetOutput(a6);
depth[7] = GetOutput(a7);
// Load
Q0 = vld1q_u16(depth);
// Store
vst1q_u16(pnOutput, Q0);
// Load
Q0 = vld1q_u16(shift);
// Store
vst1q_u16(pShiftOut, Q0);
#else
pShiftOut[0] = (((a0) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a0) : 0);
pShiftOut[1] = (((a1) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a1) : 0);
pShiftOut[2] = (((a2) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a2) : 0);
pShiftOut[3] = (((a3) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a3) : 0);
pShiftOut[4] = (((a4) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a4) : 0);
pShiftOut[5] = (((a5) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a5) : 0);
pShiftOut[6] = (((a6) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a6) : 0);
pShiftOut[7] = (((a7) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (a7) : 0);
pnOutput[0] = GetOutput(a0);
pnOutput[1] = GetOutput(a1);
pnOutput[2] = GetOutput(a2);
pnOutput[3] = GetOutput(a3);
pnOutput[4] = GetOutput(a4);
pnOutput[5] = GetOutput(a5);
pnOutput[6] = GetOutput(a6);
pnOutput[7] = GetOutput(a7);
#endif
pcInput += XN_INPUT_ELEMENT_SIZE;
pnOutput += 8;
pShiftOut += 8;
}
*pnActualRead = (XnUInt32)(pcInput - pOrigInput);
pWriteBuffer->UnsafeUpdateSize(nNeededOutput);
return XN_STATUS_OK;
}
void XnPSCompressedDepthProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnPSCompressedDepthProcessor::ProcessFramePacketChunk")
XnBuffer* pWriteBuffer = GetWriteBuffer();
const XnUChar* pBuf = NULL;
XnUInt32 nBufSize = 0;
// check if we have bytes stored from previous calls
if (m_RawData.GetSize() > 0)
{
// we have no choice. We need to append current buffer to previous bytes
if (m_RawData.GetFreeSpaceInBuffer() < nDataSize)
{
xnLogWarning(XN_MASK_SENSOR_PROTOCOL_DEPTH, "Bad overflow depth! %d", m_RawData.GetSize());
FrameIsCorrupted();
}
else
{
m_RawData.UnsafeWrite(pData, nDataSize);
}
pBuf = m_RawData.GetData();
nBufSize = m_RawData.GetSize();
}
else
{
// we can process the data directly
pBuf = pData;
nBufSize = nDataSize;
}
XnUInt32 nOutputSize = pWriteBuffer->GetFreeSpaceInBuffer();
XnUInt32 nWrittenOutput = nOutputSize;
XnUInt32 nActualRead = 0;
XnBool bLastPart = pHeader->nType == XN_SENSOR_PROTOCOL_RESPONSE_DEPTH_END && (nDataOffset + nDataSize) == pHeader->nBufSize;
XnStatus nRetVal = UncompressDepthPS(pBuf, nBufSize, (XnUInt16*)pWriteBuffer->GetUnsafeWritePointer(),
&nWrittenOutput, &nActualRead, bLastPart);
if (nRetVal != XN_STATUS_OK)
{
FrameIsCorrupted();
static XnUInt64 nLastPrinted = 0;
XnUInt64 nCurrTime;
xnOSGetTimeStamp(&nCurrTime);
if (nOutputSize != 0 || (nCurrTime - nLastPrinted) > 1000)
{
xnLogWarning(XN_MASK_SENSOR_PROTOCOL_DEPTH, "Uncompress depth failed: %s. Input Size: %u, Output Space: %u, Last Part: %d.", xnGetStatusString(nRetVal), nBufSize, nOutputSize, bLastPart);
xnOSGetTimeStamp(&nLastPrinted);
}
}
pWriteBuffer->UnsafeUpdateSize(nWrittenOutput);
nBufSize -= nActualRead;
m_RawData.Reset();
// if we have any bytes left, keep them for next time
if (nBufSize > 0)
{
pBuf += nActualRead;
m_RawData.UnsafeWrite(pBuf, nBufSize);
}
XN_PROFILING_END_SECTION
}
