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
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
|
// Copyright (C) 2019-2021 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
#include <iomanip>
#include <sstream>
#include <vsomeip/internal/logger.hpp>
#include "../include/tp_message.hpp"
#include "../include/tp.hpp"
#include "../../utility/include/byteorder.hpp"
#ifdef ANDROID
#include "../../configuration/include/internal_android.hpp"
#else
#include "../../configuration/include/internal.hpp"
#endif // ANDROID
#if defined(__linux__) || defined(ANDROID)
#include <arpa/inet.h>
#else
#include <Winsock2.h>
#endif
namespace vsomeip_v3 {
namespace tp {
tp_message::tp_message(const byte_t* const _data, std::uint32_t _data_length,
std::uint32_t _max_message_size) :
timepoint_creation_(std::chrono::steady_clock::now()),
max_message_size_(_max_message_size),
current_message_size_(0),
last_segment_received_(false) {
if (_data_length < VSOMEIP_FULL_HEADER_SIZE + VSOMEIP_TP_HEADER_SIZE) {
VSOMEIP_ERROR << __func__ << " received too short SOME/IP-TP message "
<< get_message_id(_data, _data_length);
return;
}
// copy header
message_.insert(message_.end(), _data, _data + VSOMEIP_FULL_HEADER_SIZE);
// remove TP flag
message_[VSOMEIP_MESSAGE_TYPE_POS] = static_cast<byte_t>(tp::tp_flag_unset(
message_[VSOMEIP_MESSAGE_TYPE_POS]));
const length_t its_segment_size = _data_length - VSOMEIP_FULL_HEADER_SIZE
- VSOMEIP_TP_HEADER_SIZE;
const tp_header_t its_tp_header = VSOMEIP_BYTES_TO_LONG(
_data[VSOMEIP_TP_HEADER_POS_MIN],
_data[VSOMEIP_TP_HEADER_POS_MIN + 1],
_data[VSOMEIP_TP_HEADER_POS_MIN + 2],
_data[VSOMEIP_TP_HEADER_POS_MAX]);
if (check_lengths(_data, _data_length, its_segment_size,
tp::more_segments(its_tp_header))) {
const length_t its_offset = tp::get_offset(its_tp_header);
segments_.emplace(segment_t(its_offset, its_offset + its_segment_size - 1));
if (its_offset != 0) {
// segment different than the first segment was received
message_.resize(VSOMEIP_FULL_HEADER_SIZE + its_offset, 0x0);
if (!tp::more_segments(its_tp_header)) {
// received the last segment of the segmented message first
last_segment_received_ = true;
}
}
message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS],
&_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size);
current_message_size_ += VSOMEIP_FULL_HEADER_SIZE + its_segment_size;
}
}
bool tp_message::add_segment(const byte_t* const _data,
std::uint32_t _data_length) {
if (_data_length < VSOMEIP_FULL_HEADER_SIZE + VSOMEIP_TP_HEADER_SIZE) {
VSOMEIP_ERROR << __func__ << " received too short SOME/IP-TP message "
<< get_message_id(_data, _data_length);
return false;
}
bool ret = false;
const length_t its_segment_size = _data_length - VSOMEIP_FULL_HEADER_SIZE
- VSOMEIP_TP_HEADER_SIZE;
const tp_header_t its_tp_header = VSOMEIP_BYTES_TO_LONG(
_data[VSOMEIP_TP_HEADER_POS_MIN],
_data[VSOMEIP_TP_HEADER_POS_MIN + 1],
_data[VSOMEIP_TP_HEADER_POS_MIN + 2],
_data[VSOMEIP_TP_HEADER_POS_MAX]);
if (check_lengths(_data, _data_length, its_segment_size,
tp::more_segments(its_tp_header))) {
const length_t its_offset = tp::get_offset(its_tp_header);
const auto emplace_res = segments_.emplace(
segment_t(its_offset, its_offset + its_segment_size - 1));
if (!emplace_res.second) {
VSOMEIP_WARNING << __func__ << ":" << __LINE__
<< " received duplicate segment " << get_message_id(_data, _data_length)
<< "TP offset: 0x" << std::hex << its_offset;
} else {
const auto& seg_current = emplace_res.first;
const auto& seg_next = std::next(seg_current);
const bool current_segment_is_last = (seg_next == segments_.end());
const bool current_segment_is_first = (seg_current == segments_.begin());
if (current_segment_is_last) {
if (current_segment_is_first) {
// received segment of message but the first received segment was invalid
// resize + append
VSOMEIP_WARNING << __func__ << ":" << __LINE__
<< " received 2nd segment of message. But the "
"first received segment already wasn't accepted. "
"The message can't be completed anymore: "
<< get_message_id(_data, _data_length);
if (its_offset != 0) {
// segment different than the first segment was received
message_.resize(VSOMEIP_FULL_HEADER_SIZE + its_offset, 0x0);
if (!tp::more_segments(its_tp_header)) {
// received the last segment of the segmented message first
last_segment_received_ = true;
}
}
// append to end of message
message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS],
&_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size);
current_message_size_ += its_segment_size;
} else {
const auto& seg_prev = std::prev(seg_current);
if (seg_prev->end_ < seg_current->start_) {
const bool direct_previous_segment_present = (seg_prev->end_ + 1 == seg_current->start_);
if (!direct_previous_segment_present) {
// received segment out of order behind the current end of received segments
//resize + append
message_.resize(VSOMEIP_FULL_HEADER_SIZE + its_offset, 0x0);
}
// append to end of message
message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS],
&_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size);
current_message_size_ += its_segment_size;
} else {
// this segment starts before the end of the previous and
// would overwrite already received data
VSOMEIP_WARNING << __func__ << ":" << __LINE__
<< " completely accepting segment would overwrite previous segment "
<< get_message_id(_data, _data_length)
<< "previous segment end: " << std::dec << seg_prev->end_ + 1
<< " this segment start: " << std::dec << seg_current->start_;
message_.insert(message_.end(),
&_data[VSOMEIP_TP_PAYLOAD_POS] + ((seg_prev->end_ + 1) - seg_current->start_),
&_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size);
// update start of current segment
const std::uint32_t current_end = seg_current->end_;
segments_.erase(seg_current);
segments_.emplace(segment_t(seg_prev->end_ + 1, current_end));
current_message_size_ += current_end - seg_prev->end_;
}
}
} else {
// received segment in wrong order and other segments afterwards were already received
if ((seg_current != segments_.begin() && std::prev(seg_current)->end_ < seg_current->start_)
|| seg_current == segments_.begin()) { // no need to check prev_segment if current segment is the first
if (seg_current->end_ < seg_next->start_) {
std::memcpy(&message_[VSOMEIP_FULL_HEADER_SIZE + its_offset], &_data[VSOMEIP_TP_PAYLOAD_POS], its_segment_size);
current_message_size_ += its_segment_size;
} else {
// this segment ends after the start of the next and
// would overwrite already received data
VSOMEIP_WARNING << __func__ << ":" << __LINE__
<< " completely accepting segment would overwrite next segment "
<< get_message_id(_data, _data_length)
<< "next segment start: " << std::dec << seg_next->start_
<< " this segment end: " << std::dec << seg_current->end_ + 1;
std::memcpy(&message_[VSOMEIP_FULL_HEADER_SIZE + its_offset], &_data[VSOMEIP_TP_PAYLOAD_POS], seg_next->start_ - its_offset);
// update current segment length to match size of memory
std::uint32_t current_start = seg_current->start_;
segments_.erase(seg_current);
segments_.emplace(segment_t(current_start, seg_next->start_ - 1));
current_message_size_ += seg_next->start_ - current_start;
}
} else if (seg_current->end_ < seg_next->start_) {
// this segment starts before the end of the previous and
// would overwrite already received data. But ends before the
// start of the next segment
const auto& seg_prev = std::prev(seg_current);
VSOMEIP_WARNING << __func__ << ":" << __LINE__
<< " completely accepting segment would overwrite previous segment "
<< get_message_id(_data, _data_length)
<< "previous segment end: " << std::dec << seg_prev->end_
<< " this segment start: " << std::dec << seg_current->start_;
const length_t its_corrected_offset = seg_prev->end_ + 1;
std::memcpy(&message_[VSOMEIP_FULL_HEADER_SIZE + its_corrected_offset],
&_data[VSOMEIP_TP_PAYLOAD_POS] + its_corrected_offset - its_offset,
seg_next->start_ - its_corrected_offset);
// update current segment length to match size of memory
std::uint32_t current_end = seg_current->end_;
segments_.erase(seg_current);
segments_.emplace(segment_t(seg_prev->end_ + 1, current_end));
current_message_size_ += current_end - seg_prev->end_;
} else {
// this segment starts before the end of the previous and
// ends after the start of the next segment and would
// overwrite already received data.
const auto& seg_prev = std::prev(seg_current);
VSOMEIP_WARNING << __func__ << ":" << __LINE__
<< " completely accepting segment would overwrite "
<< "previous and next segment "
<< get_message_id(_data, _data_length)
<< "previous segment end: " << std::dec << seg_prev->end_
<< " this segment start: " << std::dec << seg_current->start_
<< " this segment end: " << std::dec << seg_current->end_
<< " next segment start: " << std::dec << seg_next->start_;
const length_t its_corrected_offset = seg_prev->end_ + 1;
std::memcpy(&message_[VSOMEIP_FULL_HEADER_SIZE + its_corrected_offset],
&_data[VSOMEIP_TP_PAYLOAD_POS] + its_corrected_offset - its_offset,
seg_next->start_ - its_corrected_offset);
segments_.erase(seg_current);
segments_.emplace(segment_t(seg_prev->end_ + 1, seg_next->start_ - 1));
current_message_size_ += seg_next->start_ - (seg_prev->end_ + 1);
}
}
if (!tp::more_segments(its_tp_header)) {
// received the last segment
last_segment_received_ = true;
}
if (last_segment_received_) {
// check if all segments are present
std::uint32_t last_end = (std::numeric_limits<std::uint32_t>::max)();
bool complete(true);
for (const auto& seg : segments_) {
if (last_end + 1 != seg.start_) {
complete = false;
break;
} else {
last_end = seg.end_;
}
}
if (complete) {
// all segments were received -> update length field of message
const length_t its_length = static_cast<length_t>(
message_.size() - VSOMEIP_SOMEIP_HEADER_SIZE);
*(reinterpret_cast<length_t*>(&message_[VSOMEIP_LENGTH_POS_MIN])) = htonl(its_length);
// all segments were received -> update return code field of message
message_[VSOMEIP_RETURN_CODE_POS] = _data[VSOMEIP_RETURN_CODE_POS];
ret = true;
}
}
}
}
return ret;
}
message_buffer_t tp_message::get_message() {
return std::move(message_);
}
std::chrono::steady_clock::time_point tp_message::get_creation_time() const {
return timepoint_creation_;
}
std::string tp_message::get_message_id(const byte_t* const _data, std::uint32_t _data_length) {
std::stringstream ss;
if (_data_length >= VSOMEIP_FULL_HEADER_SIZE) {
const service_t its_service = VSOMEIP_BYTES_TO_WORD(
_data[VSOMEIP_SERVICE_POS_MIN], _data[VSOMEIP_SERVICE_POS_MAX]);
const method_t its_method = VSOMEIP_BYTES_TO_WORD(
_data[VSOMEIP_METHOD_POS_MIN], _data[VSOMEIP_METHOD_POS_MAX]);
const client_t its_client = VSOMEIP_BYTES_TO_WORD(
_data[VSOMEIP_CLIENT_POS_MIN], _data[VSOMEIP_CLIENT_POS_MAX]);
const session_t its_session = VSOMEIP_BYTES_TO_WORD(
_data[VSOMEIP_SESSION_POS_MIN], _data[VSOMEIP_SESSION_POS_MAX]);
const interface_version_t its_interface_version =
_data[VSOMEIP_INTERFACE_VERSION_POS];
const message_type_e its_msg_type = tp::tp_flag_unset(
_data[VSOMEIP_MESSAGE_TYPE_POS]);
ss << "("
<< std::hex << std::setw(4) << std::setfill('0') << its_client << ") ["
<< std::hex << std::setw(4) << std::setfill('0') << its_service << "."
<< std::hex << std::setw(4) << std::setfill('0') << its_method << "."
<< std::hex << std::setw(2) << std::setfill('0') << std::uint32_t(its_interface_version) << "."
<< std::hex << std::setw(2) << std::setfill('0') << std::uint32_t(its_msg_type) << "."
<< std::hex << std::setw(4) << std::setfill('0') << its_session
<< "] ";
if (_data_length > VSOMEIP_TP_HEADER_POS_MAX) {
const tp_header_t its_tp_header = VSOMEIP_BYTES_TO_LONG(
_data[VSOMEIP_TP_HEADER_POS_MIN],
_data[VSOMEIP_TP_HEADER_POS_MIN + 1],
_data[VSOMEIP_TP_HEADER_POS_MIN + 2],
_data[VSOMEIP_TP_HEADER_POS_MAX]);
const length_t its_offset = tp::get_offset(its_tp_header);
ss << " TP offset: 0x" << std::hex << its_offset << " ";
}
}
return ss.str();
}
bool tp_message::check_lengths(const byte_t* const _data,
std::uint32_t _data_length,
length_t _segment_size, bool _more_fragments) {
const length_t its_length = VSOMEIP_BYTES_TO_LONG(
_data[VSOMEIP_LENGTH_POS_MIN],
_data[VSOMEIP_LENGTH_POS_MIN + 1],
_data[VSOMEIP_LENGTH_POS_MIN + 2],
_data[VSOMEIP_LENGTH_POS_MAX]);
const tp_header_t its_tp_header = VSOMEIP_BYTES_TO_LONG(
_data[VSOMEIP_TP_HEADER_POS_MIN],
_data[VSOMEIP_TP_HEADER_POS_MIN + 1],
_data[VSOMEIP_TP_HEADER_POS_MIN + 2],
_data[VSOMEIP_TP_HEADER_POS_MAX]);
bool ret(true);
if (!tp::tp_flag_is_set(_data[VSOMEIP_MESSAGE_TYPE_POS])) {
VSOMEIP_ERROR << __func__ << ": TP flag not set "
<< get_message_id(_data, _data_length);
ret = false;
} else if (_data_length != its_length + VSOMEIP_SOMEIP_HEADER_SIZE) {
VSOMEIP_ERROR << __func__
<< ": data length doesn't match header length field"
<< get_message_id(_data, _data_length)
<< " data: " << std::dec << _data_length
<< " header: " << std::dec << its_length;
ret = false;
} else if (_segment_size != its_length - VSOMEIP_TP_HEADER_SIZE
- (VSOMEIP_FULL_HEADER_SIZE - VSOMEIP_SOMEIP_HEADER_SIZE)) {
VSOMEIP_ERROR << __func__
<< ": segment size doesn't align with header length field"
<< get_message_id(_data, _data_length)
<< "segment size: " << std::dec << _segment_size
<< " data: " << std::dec << _data_length
<< " header: " << std::dec << its_length;
ret = false;
} else if (_segment_size > tp::tp_max_segment_length_) {
VSOMEIP_ERROR << __func__ << ": Segment exceeds allowed size "
<< get_message_id(_data, _data_length)
<< "segment size: " << std::dec << _segment_size << " (max. "
<< std::dec << tp::tp_max_segment_length_
<< ") data: " << std::dec << _data_length
<< " header: " << std::dec << its_length;
ret = false;
} else if (_more_fragments && _segment_size % 16 > 0) {
VSOMEIP_ERROR << __func__ << ": Segment size not multiple of 16 "
<< get_message_id(_data, _data_length)
<< "segment size: " << std::dec << _segment_size
<< " data: " << std::dec << _data_length
<< " header: " << std::dec << its_length;
ret = false;
} else if (current_message_size_ + _segment_size > max_message_size_) {
VSOMEIP_ERROR << __func__ << ": Message exceeds maximum configured size: "
<< get_message_id(_data, _data_length)
<< "segment size: " << std::dec << _segment_size
<< " current message size: " << std::dec << current_message_size_
<< " maximum message size: " << std::dec << max_message_size_;
ret = false;
} else if (tp::get_offset(its_tp_header) + _segment_size > max_message_size_ ) {
VSOMEIP_ERROR << __func__ << ": SomeIP/TP offset field exceeds maximum configured message size: "
<< get_message_id(_data, _data_length)
<< " TP offset [bytes]: " << std::dec << tp::get_offset(its_tp_header)
<< " segment size: " << std::dec << _segment_size
<< " current message size: " << std::dec << current_message_size_
<< " maximum message size: " << std::dec << max_message_size_;
ret = false;
}
return ret;
}
} // namespace tp
} // namespace vsomeip_v3
|
