1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
|
#include "XnLinkPacked10BitParser.h"
#include "XnLinkProtoUtils.h"
#include <XnOS.h>
namespace xn
{
LinkPacked10BitParser::LinkPacked10BitParser()
{
m_nState = 0;
}
LinkPacked10BitParser::~LinkPacked10BitParser()
{
}
XnStatus LinkPacked10BitParser::ParsePacketImpl(XnLinkFragmentation fragmentation,
const XnUInt8* pSrc,
const XnUInt8* pSrcEnd,
XnUInt8*& pDst,
const XnUInt8* pDstEnd)
{
//pDstWord always points to same address as pDst.
XnUInt16*& pDstWord = reinterpret_cast<XnUInt16*&>(pDst);
const XnUInt16* pDstWordEnd = reinterpret_cast<const XnUInt16*>(pDstEnd);
XnSizeT nPacketBits = 0;
XnSizeT nPacketDstWords = 0;
XnSizeT nPacketDataSize = pSrcEnd - pSrc;
if ((fragmentation & XN_LINK_FRAG_BEGIN) != 0)
{
//Reset state for new frame
m_nState = 0;
}
//Calculate needed space for this packet when it's unpacked
nPacketBits = (nPacketDataSize * 8);
nPacketDstWords = (nPacketBits / 10);
if (nPacketBits % 10 != 0)
{
nPacketDstWords++;
}
if ((pDstWord + nPacketDstWords) > pDstWordEnd) //Do we have enough room for this packet?
{
XN_ASSERT(FALSE);
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
while (pSrc < pSrcEnd)
{
switch (m_nState)
{
case 0:
*pDstWord = (*pSrc << 2); //8 first bits, make room for 2 more
m_nState++;
break;
case 1:
*pDstWord++ |= ((*pSrc >> 6) & 0x03); //2 more bits - got 1 whole word
*pDstWord = ((*pSrc & 0x3F) << 4); //6 first bits, make room for 4 more
m_nState++;
break;
case 2:
*pDstWord++ |= ((*pSrc >> 4) & 0x0F); //4 more bits - got 2 whole words
*pDstWord = ((*pSrc & 0x0F) << 6); //4 first bits, make room for 6 more
m_nState++;
break;
case 3:
*pDstWord++ |= ((*pSrc >> 2) & 0x3F); //6 more bits - got 3 whole words
*pDstWord = ((*pSrc & 0x03) << 8); //2 first bits, make room for 8 more
m_nState++;
break;
case 4:
*pDstWord++ |= *pSrc; //8 more bits - got 4 whole words
m_nState = 0;
break;
default:
XN_ASSERT(FALSE);
}
pSrc++;
}
return XN_STATUS_OK;
}
}
|
