1 /*
2 * Copyright (c) 2021 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15
16 #ifndef STREAM_PACKET_HEADER_H
17 #define STREAM_PACKET_HEADER_H
18
19 #include <arpa/inet.h>
20 #include <chrono>
21 #include <cstdint>
22 #include <memory>
23 #include <sys/types.h>
24 #include <utility>
25 #include <vector>
26
27 #include "securec.h"
28 #include "stream_common.h"
29 #include "i_stream.h"
30
31 using ::std::chrono::duration_cast;
32 using ::std::chrono::milliseconds;
33 using ::std::chrono::system_clock;
34
35 namespace Communication {
36 namespace SoftBus {
37 static constexpr int SHIFT = 2;
38 // Align x up to the nearest integer multiple of 2*shift
Align(int x,int shift)39 inline static int Align(int x, int shift)
40 {
41 auto tmpValue = static_cast<unsigned int>(x);
42 auto tmpShift = static_cast<unsigned int>(shift);
43 return static_cast<int>((tmpValue + ((1 << tmpShift) - 1)) & ~((1 << tmpShift) - 1));
44 }
45
AlignTo4Bytes(int x)46 inline static int AlignTo4Bytes(int x)
47 {
48 return Align(x, SHIFT);
49 }
50
51 struct CommonHeader {
52 uint8_t version : 2;
53 uint8_t subVersion : 1;
54 uint8_t extFlag : 1;
55 uint8_t streamType : 4;
56 uint8_t marker : 1;
57 uint8_t flag : 1;
58 uint8_t level : 4;
59 uint8_t pad : 2;
60 uint16_t streamId;
61 uint32_t timestamp;
62 uint32_t dataLen;
63 uint16_t seqNum;
64 uint16_t subSeqNum;
65 };
66
67 struct TypeLength {
68 uint16_t type = 0;
69 uint32_t length = 0;
70 };
71
72 class TwoLevelsTlv {
73 public:
74 static constexpr int HEADER_LEN = 2;
75 static constexpr int NUMS_LEN = 2;
76 static constexpr int CHECK_SUM_LEN = 4;
77
TwoLevelsTlv()78 TwoLevelsTlv()
79 {
80 SetTlvVersion(0);
81 }
TwoLevelsTlv(std::unique_ptr<char[]> extBuf,ssize_t extSize)82 TwoLevelsTlv(std::unique_ptr<char[]> extBuf, ssize_t extSize)
83 {
84 SetTlvVersion(0);
85 ext_ = std::move(extBuf);
86 extLen_ = extSize;
87 }
88
89 virtual ~TwoLevelsTlv() = default;
90
SetTlvVersion(uint16_t version)91 void SetTlvVersion(uint16_t version)
92 {
93 firstLevelHeader.type |= (static_cast<uint16_t>(version << TopOffset::VERSION_OFFSET) & TopMask::VERSION_MASK);
94 }
95
GetVersion()96 uint16_t GetVersion() const
97 {
98 return (firstLevelHeader.type & TopMask::VERSION_MASK) >> TopOffset::VERSION_OFFSET;
99 }
100
Packetize(char * start,ssize_t size,ssize_t offset)101 int Packetize(char *start, ssize_t size, ssize_t offset)
102 {
103 char *pos = new (start + offset) char[size];
104 if (ext_ != nullptr) {
105 auto extTlvAlignSize = HEADER_LEN + NUMS_LEN + AlignTo4Bytes(extLen_);
106 if (AddFrameExtData(pos + HEADER_LEN + NUMS_LEN) != 0) {
107 return -1;
108 }
109 checkSum_ += static_cast<uint32_t>(extTlvAlignSize);
110 } else {
111 return 0; // 目前假设只有ext，其他option后面添加
112 }
113
114 checkSum_ += HEADER_LEN + NUMS_LEN;
115
116 auto tmp = reinterpret_cast<uint16_t *>(pos);
117 *(tmp++) = htons(firstLevelHeader.type);
118 *(tmp++) = htons(firstLevelHeader.length);
119
120 // size is always bigger than CHECK_SUM_LEN
121 auto checkSumTmp = reinterpret_cast<uint32_t *>(pos + (size - CHECK_SUM_LEN));
122 *checkSumTmp = htonl(checkSum_);
123 return 0;
124 }
125
Depacketize(char * data,uint32_t size)126 void Depacketize(char *data, uint32_t size)
127 {
128 if (size < sizeof(uint16_t) + sizeof(uint16_t)) {
129 return;
130 }
131 auto tmp = reinterpret_cast<uint16_t *>(data);
132 firstLevelHeader.type = ntohs(*tmp++);
133 firstLevelHeader.length = ntohs(*tmp++);
134
135 if (firstLevelHeader.type & TopMask::EXT_BUF_MASK) {
136 constexpr uint32_t extFiledNum = 4;
137 if (size < sizeof(uint16_t) * extFiledNum) {
138 return;
139 }
140 TypeLength tl;
141 tl.type = ntohs(*tmp++);
142 tl.length = ntohs(*tmp++);
143
144 if (tl.length == 0 || sizeof(uint16_t) * extFiledNum + tl.length > size) {
145 return;
146 }
147 ext_ = std::make_unique<char[]>(tl.length);
148 int ret = memcpy_s(ext_.get(), tl.length, reinterpret_cast<void *>(tmp), tl.length);
149 if (ret == 0) {
150 extLen_ = static_cast<ssize_t>(tl.length);
151 }
152 }
153
154 checkSum_ = ntohl(*reinterpret_cast<uint32_t *>((reinterpret_cast<char *>(tmp) + AlignTo4Bytes(extLen_))));
155 }
156
GetTlvNums()157 uint16_t GetTlvNums() const
158 {
159 return firstLevelHeader.length;
160 }
161
GetExtLen()162 ssize_t GetExtLen() const
163 {
164 return extLen_;
165 }
166
GetCheckSum()167 uint32_t GetCheckSum() const
168 {
169 return checkSum_;
170 }
171
GetExtBuffer()172 std::unique_ptr<char[]> GetExtBuffer()
173 {
174 return std::move(ext_);
175 }
176
177 private:
178 enum TopMask {
179 VERSION_MASK = 0xC000,
180 EXT_BUF_MASK = 0x0001,
181 OPTION_MASK = 0x0002,
182 };
183
184 enum TopOffset {
185 VERSION_OFFSET = 14,
186 EXT_BUF_OFFSET = 0,
187 OPTION_OFFSET = 1,
188 };
189
190 enum BottomMask {
191 LEN_MASK = 0x8000,
192 EXT_MASK = 0x0001,
193 };
194
195 enum BottomOffset {
196 EXT_OFFSET = 0,
197 };
198
AddFrameExtData(char * start)199 int AddFrameExtData(char *start)
200 {
201 firstLevelHeader.type |= ((1 << TopOffset::EXT_BUF_OFFSET) & TopMask::EXT_BUF_MASK);
202 firstLevelHeader.length++;
203
204 TypeLength extTlv {};
205 extTlv.type = 0;
206 extTlv.length = static_cast<uint16_t>(extLen_);
207
208 auto pos = reinterpret_cast<uint16_t *>(start);
209 *(pos++) = htons(extTlv.type);
210 *(pos++) = htons(extTlv.length);
211
212 char *extPos = reinterpret_cast<char *>(pos);
213 int ret = memcpy_s(extPos, AlignTo4Bytes(extLen_), ext_.get(), extLen_);
214 if (ret != 0) {
215 return -1;
216 }
217 return 0;
218 }
219
220 uint32_t checkSum_ = 0;
221 TypeLength firstLevelHeader {};
222 std::vector<TypeLength> tlvList_ {};
223 std::unique_ptr<char[]> ext_ = nullptr;
224 ssize_t extLen_ = 0;
225 };
226
227 class StreamPacketHeader {
228 public:
229 static constexpr int STREAM_HEADER_SIZE = 16;
230 static constexpr int VERSION = 0;
231 static constexpr int SUB_VERSION = 1;
232
233 static constexpr uint32_t VERSION_OFFSET = 30;
234 static constexpr uint32_t SUB_VERSION_OFFSET = 29;
235 static constexpr uint32_t EXT_FLAG_OFFSET = 28;
236 static constexpr uint32_t STREAM_TYPE_OFFSET = 24;
237 static constexpr uint32_t MAKER_OFFSET = 23;
238 static constexpr uint32_t FLAG_OFFSET = 22;
239 static constexpr uint32_t LEVEL_OFFSET = 18;
240 static constexpr uint32_t SEQ_NUM_OFFSET = 0;
241
242 static constexpr uint32_t WORD_SIZE = 16;
243
StreamPacketHeader()244 StreamPacketHeader() {}
StreamPacketHeader(uint8_t streamType,bool extended,uint32_t dataLen,const Communication::SoftBus::StreamFrameInfo * streamFrameInfo)245 StreamPacketHeader(uint8_t streamType, bool extended, uint32_t dataLen,
246 const Communication::SoftBus::StreamFrameInfo* streamFrameInfo)
247 {
248 uint32_t ts = 0;
249 if (streamFrameInfo->timeStamp == 0) {
250 const auto now = system_clock::now();
251 const auto ms = duration_cast<milliseconds>(now.time_since_epoch()).count();
252 ts = static_cast<uint32_t>(ms);
253 } else {
254 ts = streamFrameInfo->timeStamp;
255 }
256
257 SetVersion(VERSION, SUB_VERSION);
258 commonHeader_.extFlag = extended ? 1 : 0;
259 commonHeader_.streamType = streamType;
260 commonHeader_.marker = 0;
261 commonHeader_.flag = 0;
262 commonHeader_.level = streamFrameInfo->level;
263 commonHeader_.pad = 0;
264 commonHeader_.streamId = streamFrameInfo->streamId;
265 commonHeader_.timestamp = ts;
266 commonHeader_.dataLen = dataLen;
267 commonHeader_.seqNum = streamFrameInfo->seqNum;
268 commonHeader_.subSeqNum = streamFrameInfo->seqSubNum;
269 }
270
271 virtual ~StreamPacketHeader() = default;
272
Packetize(char * pos,ssize_t size,ssize_t offset)273 void Packetize(char *pos, ssize_t size, ssize_t offset) const
274 {
275 auto start = reinterpret_cast<uint32_t *>(new (pos + offset) char[size]);
276
277 uint32_t common = 0;
278 common |= commonHeader_.version << VERSION_OFFSET;
279 common |= commonHeader_.subVersion << SUB_VERSION_OFFSET;
280 common |= commonHeader_.extFlag << EXT_FLAG_OFFSET;
281 common |= commonHeader_.streamType << STREAM_TYPE_OFFSET;
282 common |= commonHeader_.marker << MAKER_OFFSET;
283 common |= commonHeader_.flag << FLAG_OFFSET;
284 common |= commonHeader_.level << LEVEL_OFFSET;
285 common |= static_cast<uint16_t>(commonHeader_.streamId << SEQ_NUM_OFFSET);
286
287 *start++ = htonl(common);
288 *start++ = htonl(commonHeader_.timestamp);
289 *start++ = htonl(commonHeader_.dataLen);
290 common = 0;
291 common |= commonHeader_.seqNum << WORD_SIZE;
292 common |= commonHeader_.subSeqNum;
293 *start++ = htonl(common);
294 }
295
Depacketize(const char * header)296 void Depacketize(const char *header)
297 {
298 auto start = reinterpret_cast<const uint32_t *>(header);
299 uint32_t common = ntohl(*start++);
300 commonHeader_.timestamp = ntohl(*start++);
301 commonHeader_.dataLen = ntohl(*start++);
302 uint32_t seq = ntohl(*start);
303 commonHeader_.seqNum = seq >> WORD_SIZE;
304 commonHeader_.subSeqNum = seq & 0xff;
305
306 commonHeader_.version = common >> VERSION_OFFSET;
307 commonHeader_.subVersion = common >> SUB_VERSION_OFFSET;
308 commonHeader_.extFlag = common >> EXT_FLAG_OFFSET;
309 commonHeader_.streamType = common >> STREAM_TYPE_OFFSET;
310 commonHeader_.marker = common >> MAKER_OFFSET;
311 commonHeader_.flag = common >> FLAG_OFFSET;
312 commonHeader_.level = common >> LEVEL_OFFSET;
313 commonHeader_.streamId = common >> SEQ_NUM_OFFSET;
314 }
315
SetVersion(uint8_t version,uint8_t subVersion)316 void SetVersion(uint8_t version, uint8_t subVersion)
317 {
318 commonHeader_.version = version;
319 commonHeader_.subVersion = subVersion;
320 }
GetVersion()321 uint8_t GetVersion() const
322 {
323 return commonHeader_.version;
324 }
325
GetSubVersion()326 uint8_t GetSubVersion() const
327 {
328 return commonHeader_.subVersion;
329 }
330
GetExtFlag()331 uint8_t GetExtFlag() const
332 {
333 return commonHeader_.extFlag;
334 }
335
GetStreamType()336 uint8_t GetStreamType() const
337 {
338 return commonHeader_.streamType;
339 }
340
SetMarker(uint8_t marker)341 void SetMarker(uint8_t marker)
342 {
343 commonHeader_.marker = marker;
344 }
GetMarker()345 uint8_t GetMarker() const
346 {
347 return commonHeader_.marker;
348 }
349
GetFlag()350 uint8_t GetFlag() const
351 {
352 return commonHeader_.flag;
353 }
354
GetSeqNum()355 uint16_t GetSeqNum() const
356 {
357 return commonHeader_.seqNum;
358 }
359
GetTimestamp()360 uint32_t GetTimestamp() const
361 {
362 return commonHeader_.timestamp;
363 }
364
GetDataLen()365 uint32_t GetDataLen() const
366 {
367 return commonHeader_.dataLen;
368 }
369
GetStreamId()370 uint16_t GetStreamId() const
371 {
372 return commonHeader_.streamId;
373 }
GetSubSeqNum()374 uint16_t GetSubSeqNum() const
375 {
376 return commonHeader_.subSeqNum;
377 }
GetLevel()378 uint8_t GetLevel() const
379 {
380 return commonHeader_.level;
381 }
382
383 private:
384 CommonHeader commonHeader_ {};
385 };
386 } // namespace SoftBus
387 } // namespace Communication
388
389 #endif
390