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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
|
#include "XnLinkMsgEncoder.h"
#include "XnLinkProtoUtils.h"
#include <XnOS.h>
#include <XnLog.h>
namespace xn
{
LinkMsgEncoder::LinkMsgEncoder()
{
m_nMaxMsgSize = 0;
m_nMaxPacketSize = 0;
m_nMaxNumPackets = 0;
m_nBufferSize = 0;
m_pCurrPacket = NULL;
m_nEncodedSize = 0;
m_pOutputBuffer = NULL;
xnOSMemSet(&m_packetHeader, 0, sizeof(m_packetHeader));
//Set constant packet header values - for all messages
m_packetHeader.SetMagic();
m_packetHeader.SetSize(sizeof(m_packetHeader)); //this is the minimum size, we'll add to this.
}
LinkMsgEncoder::~LinkMsgEncoder()
{
Shutdown();
}
XnStatus LinkMsgEncoder::Init(XnUInt32 nMaxMsgSize, XnUInt16 nMaxPacketSize)
{
if (nMaxPacketSize == 0)
{
xnLogError(XN_MASK_LINK, "Got max packet size of 0 in link msg encoder init :(");
XN_ASSERT(FALSE);
return XN_STATUS_ERROR;
}
m_nMaxMsgSize = nMaxMsgSize;
m_nMaxPacketSize = nMaxPacketSize;
XnUInt16 nMaxPacketDataSize = m_nMaxPacketSize - sizeof(LinkPacketHeader);
m_nMaxNumPackets = m_nMaxMsgSize / nMaxPacketDataSize;
if (m_nMaxPacketSize % nMaxPacketDataSize != 0)
{
//We need one more packet for remainder of data
m_nMaxNumPackets++;
}
m_nBufferSize = m_nMaxNumPackets * m_nMaxPacketSize;
m_pOutputBuffer = reinterpret_cast<XnUInt8*>(xnOSMallocAligned(m_nBufferSize, XN_DEFAULT_MEM_ALIGN));
XN_VALIDATE_ALLOC_PTR(m_pOutputBuffer);
return XN_STATUS_OK;
}
void LinkMsgEncoder::Shutdown()
{
xnOSFreeAligned(m_pOutputBuffer);
m_pOutputBuffer = NULL;
m_nMaxMsgSize = 0;
}
void LinkMsgEncoder::BeginEncoding(XnUInt16 nMsgType,
XnUInt16 nBasePacketID,
XnUInt16 nStreamID,
XnLinkFragmentation firstPacketFrag /*= XN_LINK_FRAG_BEGIN*/,
XnUInt16 nCID /*= 0*/)
{
//Initialize m_packetHeader values for all packets in this message.
m_packetHeader.SetMsgType(nMsgType);
m_packetHeader.SetPacketID(nBasePacketID); //We'll increment this every time we advance m_pCurrPacket
m_packetHeader.SetStreamID(nStreamID); //This is the same for all packets
m_packetHeader.SetFragmentationFlags(XN_LINK_FRAG_MIDDLE); //(for MOST packets)
m_packetHeader.SetCID(nCID);
//Point to first packet - this also sets m_pCurrPacket.
m_pCurrPacketBuffer = m_pOutputBuffer;
//Copy prepared packet header into first destination packet
xnOSMemCopy(m_pCurrPacket, &m_packetHeader, sizeof(m_packetHeader));
//Set values specific to first packet
m_pCurrPacket->SetFragmentationFlags(firstPacketFrag);
//Minimum size is always one packet header
m_nEncodedSize = sizeof(LinkPacketHeader);
}
void LinkMsgEncoder::EncodeData(const void* pSourceData, XnUInt32 nSize)
{
XnUInt16 nPacketRemainingSpace = 0; //Remaining space in current packet in each iteration
XnUInt16 nPacketBytesToCopy = 0; //Number of bytes to copy to current packet in each iteration
XnUInt32 nBytesLeftToCopy = nSize; //Total number of bytes left to copy
const XnUInt8* pCurrData = reinterpret_cast<const XnUInt8*>(pSourceData); //Current source data pointer
while (nBytesLeftToCopy > 0)
{
if (m_pCurrPacket->GetSize() == m_nMaxPacketSize)
{
//Current packet is full. Move to next packet (this also advances m_pCurrPacket).
m_pCurrPacketBuffer += m_nMaxPacketSize;
if (m_pCurrPacketBuffer >= m_pOutputBuffer + m_nBufferSize)
{
xnLogError(XN_MASK_LINK, "Msg encoder buffer overrun :( Was about to write to position %u, but buffer size is only %u",
(m_pCurrPacketBuffer - m_pOutputBuffer), m_nBufferSize);
XN_ASSERT(FALSE);
return;
}
//Advance packet ID
m_packetHeader.SetPacketID(m_packetHeader.GetPacketID() + 1);
/*Copy prepared packet header into destination packet. This also sets m_pCurrPacket->m_nSize to minimum
and m_pCurrPacket->m_nFragmentation to XN_LINK_FRAG_MIDDLE.*/
xnOSMemCopy(m_pCurrPacket, &m_packetHeader, sizeof(m_packetHeader));
//Increase encoded size for packet header
m_nEncodedSize += sizeof(m_packetHeader);
}
//Calculate remaining space in current packet
nPacketRemainingSpace = m_nMaxPacketSize - m_pCurrPacket->GetSize();
//Calculate how many bytes we're copying to the current packet
nPacketBytesToCopy = static_cast<XnUInt16>(XN_MIN(nPacketRemainingSpace, nBytesLeftToCopy));
/************ Copy data to current packet ********************/
xnOSMemCopy(m_pCurrPacketBuffer + m_pCurrPacket->GetSize(),
pCurrData,
nPacketBytesToCopy);
/*************************************************************/
//Advance current source data pointer
pCurrData += nPacketBytesToCopy;
//Increase encoded size for packet data
m_nEncodedSize += nPacketBytesToCopy;
//Increase size of current packet
m_pCurrPacket->SetSize(m_pCurrPacket->GetSize() + nPacketBytesToCopy);
//Decrease number of bytes we have left to copy
nBytesLeftToCopy -= nPacketBytesToCopy;
}
}
void LinkMsgEncoder::EndEncoding(XnLinkFragmentation lastPacketFrag /*= XN_LINK_FRAG_END*/)
{
//Add END flag to fragmentation flags if it's set
m_pCurrPacket->SetFragmentationFlags(
XnLinkFragmentation(m_pCurrPacket->GetFragmentationFlags() | (lastPacketFrag & XN_LINK_FRAG_END)));
}
const void* LinkMsgEncoder::GetEncodedData() const
{
return m_pOutputBuffer;
}
XnUInt32 LinkMsgEncoder::GetEncodedSize() const
{
return m_nEncodedSize;
}
XnUInt16 LinkMsgEncoder::GetMaxPacketSize() const
{
return m_nMaxPacketSize;
}
XnUInt16 LinkMsgEncoder::GetPacketID() const
{
return m_pCurrPacket->GetPacketID();
}
XnUInt32 LinkMsgEncoder::GetMaxMsgSize() const
{
return m_nMaxMsgSize;
}
void LinkMsgEncoder::SetPacketID(XnUInt16 nPacketID)
{
m_pCurrPacket->SetPacketID(nPacketID);
}
}
|
