1 // Copyright (C) 2019 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
2 // This Source Code Form is subject to the terms of the Mozilla Public
3 // License, v. 2.0. If a copy of the MPL was not distributed with this
4 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
5
6 #include <iomanip>
7 #include <sstream>
8
9 #include <vsomeip/internal/logger.hpp>
10
11 #include "../include/tp_message.hpp"
12 #include "../include/tp.hpp"
13 #include "../../utility/include/byteorder.hpp"
14
15 #ifdef ANDROID
16 #include "../../configuration/include/internal_android.hpp"
17 #else
18 #include "../../configuration/include/internal.hpp"
19 #endif // ANDROID
20
21 #ifndef _WIN32
22 #include <arpa/inet.h>
23 #else
24 #include <Winsock2.h>
25 #endif
26
27
28 namespace vsomeip_v3 {
29 namespace tp {
30
tp_message(const byte_t * const _data,std::uint32_t _data_length,std::uint32_t _max_message_size)31 tp_message::tp_message(const byte_t* const _data, std::uint32_t _data_length,
32 std::uint32_t _max_message_size) :
33 timepoint_creation_(std::chrono::steady_clock::now()),
34 max_message_size_(_max_message_size),
35 current_message_size_(0),
36 last_segment_received_(false) {
37 if (_data_length < VSOMEIP_FULL_HEADER_SIZE + VSOMEIP_TP_HEADER_SIZE) {
38 VSOMEIP_ERROR << __func__ << " received too short SOME/IP-TP message "
39 << get_message_id(_data, _data_length);
40 return;
41 }
42 // copy header
43 message_.insert(message_.end(), _data, _data + VSOMEIP_FULL_HEADER_SIZE);
44 // remove TP flag
45 message_[VSOMEIP_MESSAGE_TYPE_POS] = static_cast<byte_t>(tp::tp_flag_unset(
46 message_[VSOMEIP_MESSAGE_TYPE_POS]));
47
48 const length_t its_segment_size = _data_length - VSOMEIP_FULL_HEADER_SIZE
49 - VSOMEIP_TP_HEADER_SIZE;
50 const tp_header_t its_tp_header = VSOMEIP_BYTES_TO_LONG(
51 _data[VSOMEIP_TP_HEADER_POS_MIN],
52 _data[VSOMEIP_TP_HEADER_POS_MIN + 1],
53 _data[VSOMEIP_TP_HEADER_POS_MIN + 2],
54 _data[VSOMEIP_TP_HEADER_POS_MAX]);
55
56 if (check_lengths(_data, _data_length, its_segment_size,
57 tp::more_segments(its_tp_header))) {
58 const length_t its_offset = tp::get_offset(its_tp_header);
59 segments_.emplace(segment_t(its_offset, its_offset + its_segment_size - 1));
60 if (its_offset != 0) {
61 // segment different than the first segment was received
62 message_.resize(VSOMEIP_FULL_HEADER_SIZE + its_offset, 0x0);
63 if (!tp::more_segments(its_tp_header)) {
64 // received the last segment of the segmented message first
65 last_segment_received_ = true;
66 }
67 }
68 message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS],
69 &_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size);
70 current_message_size_ += VSOMEIP_FULL_HEADER_SIZE + its_segment_size;
71 }
72 }
73
add_segment(const byte_t * const _data,std::uint32_t _data_length)74 bool tp_message::add_segment(const byte_t* const _data,
75 std::uint32_t _data_length) {
76 if (_data_length < VSOMEIP_FULL_HEADER_SIZE + VSOMEIP_TP_HEADER_SIZE) {
77 VSOMEIP_ERROR << __func__ << " received too short SOME/IP-TP message "
78 << get_message_id(_data, _data_length);
79 return false;
80 }
81 bool ret = false;
82
83 const length_t its_segment_size = _data_length - VSOMEIP_FULL_HEADER_SIZE
84 - VSOMEIP_TP_HEADER_SIZE;
85 const tp_header_t its_tp_header = VSOMEIP_BYTES_TO_LONG(
86 _data[VSOMEIP_TP_HEADER_POS_MIN],
87 _data[VSOMEIP_TP_HEADER_POS_MIN + 1],
88 _data[VSOMEIP_TP_HEADER_POS_MIN + 2],
89 _data[VSOMEIP_TP_HEADER_POS_MAX]);
90
91 if (check_lengths(_data, _data_length, its_segment_size,
92 tp::more_segments(its_tp_header))) {
93 const length_t its_offset = tp::get_offset(its_tp_header);
94 const auto emplace_res = segments_.emplace(
95 segment_t(its_offset, its_offset + its_segment_size - 1));
96 if (!emplace_res.second) {
97 VSOMEIP_WARNING << __func__ << ":" << __LINE__
98 << " received duplicate segment " << get_message_id(_data, _data_length)
99 << "TP offset: 0x" << std::hex << its_offset;
100 } else {
101 const auto& seg_current = emplace_res.first;
102 const auto& seg_next = std::next(seg_current);
103 const bool current_segment_is_last = (seg_next == segments_.end());
104 const bool current_segment_is_first = (seg_current == segments_.begin());
105 if (current_segment_is_last) {
106 if (current_segment_is_first) {
107 // received segment of message but the first received segment was invalid
108 // resize + append
109 VSOMEIP_WARNING << __func__ << ":" << __LINE__
110 << " received 2nd segment of message. But the "
111 "first received segment already wasn't accepted. "
112 "The message can't be completed anymore: "
113 << get_message_id(_data, _data_length);
114 if (its_offset != 0) {
115 // segment different than the first segment was received
116 message_.resize(VSOMEIP_FULL_HEADER_SIZE + its_offset, 0x0);
117 if (!tp::more_segments(its_tp_header)) {
118 // received the last segment of the segmented message first
119 last_segment_received_ = true;
120 }
121 }
122 // append to end of message
123 message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS],
124 &_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size);
125 current_message_size_ += its_segment_size;
126 } else {
127 const auto& seg_prev = std::prev(seg_current);
128 if (seg_prev->end_ < seg_current->start_) {
129 const bool direct_previous_segment_present = (seg_prev->end_ + 1 == seg_current->start_);
130 if (!direct_previous_segment_present) {
131 // received segment out of order behind the current end of received segments
132 //resize + append
133 message_.resize(VSOMEIP_FULL_HEADER_SIZE + its_offset, 0x0);
134 }
135 // append to end of message
136 message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS],
137 &_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size);
138 current_message_size_ += its_segment_size;
139 } else {
140 // this segment starts before the end of the previous and
141 // would overwrite already received data
142 VSOMEIP_WARNING << __func__ << ":" << __LINE__
143 << " completely accepting segment would overwrite previous segment "
144 << get_message_id(_data, _data_length)
145 << "previous segment end: " << std::dec << seg_prev->end_ + 1
146 << " this segment start: " << std::dec << seg_current->start_;
147 message_.insert(message_.end(),
148 &_data[VSOMEIP_TP_PAYLOAD_POS] + ((seg_prev->end_ + 1) - seg_current->start_),
149 &_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size);
150 // update start of current segment
151 const std::uint32_t current_end = seg_current->end_;
152 segments_.erase(seg_current);
153 segments_.emplace(segment_t(seg_prev->end_ + 1, current_end));
154 current_message_size_ += current_end - seg_prev->end_;
155 }
156 }
157 } else {
158 // received segment in wrong order and other segments afterwards were already received
159 if ((seg_current != segments_.begin() && std::prev(seg_current)->end_ < seg_current->start_)
160 || seg_current == segments_.begin()) { // no need to check prev_segment if current segment is the first
161 if (seg_current->end_ < seg_next->start_) {
162 std::memcpy(&message_[VSOMEIP_FULL_HEADER_SIZE + its_offset], &_data[VSOMEIP_TP_PAYLOAD_POS], its_segment_size);
163 current_message_size_ += its_segment_size;
164 } else {
165 // this segment ends after the start of the next and
166 // would overwrite already received data
167 VSOMEIP_WARNING << __func__ << ":" << __LINE__
168 << " completely accepting segment would overwrite next segment "
169 << get_message_id(_data, _data_length)
170 << "next segment start: " << std::dec << seg_next->start_
171 << " this segment end: " << std::dec << seg_current->end_ + 1;
172 std::memcpy(&message_[VSOMEIP_FULL_HEADER_SIZE + its_offset], &_data[VSOMEIP_TP_PAYLOAD_POS], seg_next->start_ - its_offset);
173 // update current segment length to match size of memory
174 std::uint32_t current_start = seg_current->start_;
175 segments_.erase(seg_current);
176 segments_.emplace(segment_t(current_start, seg_next->start_ - 1));
177 current_message_size_ += seg_next->start_ - current_start;
178 }
179 } else if (seg_current->end_ < seg_next->start_) {
180 // this segment starts before the end of the previous and
181 // would overwrite already received data. But ends before the
182 // start of the next segment
183 const auto& seg_prev = std::prev(seg_current);
184 VSOMEIP_WARNING << __func__ << ":" << __LINE__
185 << " completely accepting segment would overwrite previous segment "
186 << get_message_id(_data, _data_length)
187 << "previous segment end: " << std::dec << seg_prev->end_
188 << " this segment start: " << std::dec << seg_current->start_;
189 const length_t its_corrected_offset = seg_prev->end_ + 1;
190 std::memcpy(&message_[VSOMEIP_FULL_HEADER_SIZE + its_corrected_offset],
191 &_data[VSOMEIP_TP_PAYLOAD_POS] + its_corrected_offset - its_offset,
192 seg_next->start_ - its_corrected_offset);
193 // update current segment length to match size of memory
194 std::uint32_t current_end = seg_current->end_;
195 segments_.erase(seg_current);
196 segments_.emplace(segment_t(seg_prev->end_ + 1, current_end));
197 current_message_size_ += current_end - seg_prev->end_;
198 } else {
199 // this segment starts before the end of the previous and
200 // ends after the start of the next segment and would
201 // overwrite already received data.
202 const auto& seg_prev = std::prev(seg_current);
203 VSOMEIP_WARNING << __func__ << ":" << __LINE__
204 << " completely accepting segment would overwrite "
205 << "previous and next segment "
206 << get_message_id(_data, _data_length)
207 << "previous segment end: " << std::dec << seg_prev->end_
208 << " this segment start: " << std::dec << seg_current->start_
209 << " this segment end: " << std::dec << seg_current->end_
210 << " next segment start: " << std::dec << seg_next->start_;
211 const length_t its_corrected_offset = seg_prev->end_ + 1;
212 std::memcpy(&message_[VSOMEIP_FULL_HEADER_SIZE + its_corrected_offset],
213 &_data[VSOMEIP_TP_PAYLOAD_POS] + its_corrected_offset - its_offset,
214 seg_next->start_ - its_corrected_offset);
215 segments_.erase(seg_current);
216 segments_.emplace(segment_t(seg_prev->end_ + 1, seg_next->start_ - 1));
217 current_message_size_ += seg_next->start_ - (seg_prev->end_ + 1);
218 }
219 }
220 if (!tp::more_segments(its_tp_header)) {
221 // received the last segment
222 last_segment_received_ = true;
223 }
224 if (last_segment_received_) {
225 // check if all segments are present
226 std::uint32_t last_end = (std::numeric_limits<std::uint32_t>::max)();
227 bool complete(true);
228 for (const auto& seg : segments_) {
229 if (last_end + 1 != seg.start_) {
230 complete = false;
231 break;
232 } else {
233 last_end = seg.end_;
234 }
235 }
236 if (complete) {
237 // all segments were received -> update length field of message
238 const length_t its_length = static_cast<length_t>(
239 message_.size() - VSOMEIP_SOMEIP_HEADER_SIZE);
240 *(reinterpret_cast<length_t*>(&message_[VSOMEIP_LENGTH_POS_MIN])) = htonl(its_length);
241 // all segments were received -> update return code field of message
242 message_[VSOMEIP_RETURN_CODE_POS] = _data[VSOMEIP_RETURN_CODE_POS];
243 ret = true;
244 }
245 }
246 }
247 }
248 return ret;
249 }
250
get_message()251 message_buffer_t tp_message::get_message() {
252 return std::move(message_);
253 }
254
get_creation_time() const255 std::chrono::steady_clock::time_point tp_message::get_creation_time() const {
256 return timepoint_creation_;
257 }
258
get_message_id(const byte_t * const _data,std::uint32_t _data_length)259 std::string tp_message::get_message_id(const byte_t* const _data, std::uint32_t _data_length) {
260 std::stringstream ss;
261 if (_data_length >= VSOMEIP_FULL_HEADER_SIZE) {
262 const service_t its_service = VSOMEIP_BYTES_TO_WORD(
263 _data[VSOMEIP_SERVICE_POS_MIN], _data[VSOMEIP_SERVICE_POS_MAX]);
264 const method_t its_method = VSOMEIP_BYTES_TO_WORD(
265 _data[VSOMEIP_METHOD_POS_MIN], _data[VSOMEIP_METHOD_POS_MAX]);
266 const client_t its_client = VSOMEIP_BYTES_TO_WORD(
267 _data[VSOMEIP_CLIENT_POS_MIN], _data[VSOMEIP_CLIENT_POS_MAX]);
268 const session_t its_session = VSOMEIP_BYTES_TO_WORD(
269 _data[VSOMEIP_SESSION_POS_MIN], _data[VSOMEIP_SESSION_POS_MAX]);
270 const interface_version_t its_interface_version =
271 _data[VSOMEIP_INTERFACE_VERSION_POS];
272 const message_type_e its_msg_type = tp::tp_flag_unset(
273 _data[VSOMEIP_MESSAGE_TYPE_POS]);
274
275 ss << "("
276 << std::hex << std::setw(4) << std::setfill('0') << its_client << ") ["
277 << std::hex << std::setw(4) << std::setfill('0') << its_service << "."
278 << std::hex << std::setw(4) << std::setfill('0') << its_method << "."
279 << std::hex << std::setw(2) << std::setfill('0') << std::uint32_t(its_interface_version) << "."
280 << std::hex << std::setw(2) << std::setfill('0') << std::uint32_t(its_msg_type) << "."
281 << std::hex << std::setw(4) << std::setfill('0') << its_session
282 << "] ";
283 if (_data_length > VSOMEIP_TP_HEADER_POS_MAX) {
284 const tp_header_t its_tp_header = VSOMEIP_BYTES_TO_LONG(
285 _data[VSOMEIP_TP_HEADER_POS_MIN],
286 _data[VSOMEIP_TP_HEADER_POS_MIN + 1],
287 _data[VSOMEIP_TP_HEADER_POS_MIN + 2],
288 _data[VSOMEIP_TP_HEADER_POS_MAX]);
289 const length_t its_offset = tp::get_offset(its_tp_header);
290 ss << " TP offset: 0x" << std::hex << its_offset << " ";
291 }
292 }
293 return ss.str();
294 }
295
check_lengths(const byte_t * const _data,std::uint32_t _data_length,length_t _segment_size,bool _more_fragments)296 bool tp_message::check_lengths(const byte_t* const _data,
297 std::uint32_t _data_length,
298 length_t _segment_size, bool _more_fragments) {
299 const length_t its_length = VSOMEIP_BYTES_TO_LONG(
300 _data[VSOMEIP_LENGTH_POS_MIN],
301 _data[VSOMEIP_LENGTH_POS_MIN + 1],
302 _data[VSOMEIP_LENGTH_POS_MIN + 2],
303 _data[VSOMEIP_LENGTH_POS_MAX]);
304 const tp_header_t its_tp_header = VSOMEIP_BYTES_TO_LONG(
305 _data[VSOMEIP_TP_HEADER_POS_MIN],
306 _data[VSOMEIP_TP_HEADER_POS_MIN + 1],
307 _data[VSOMEIP_TP_HEADER_POS_MIN + 2],
308 _data[VSOMEIP_TP_HEADER_POS_MAX]);
309 bool ret(true);
310 if (!tp::tp_flag_is_set(_data[VSOMEIP_MESSAGE_TYPE_POS])) {
311 VSOMEIP_ERROR << __func__ << ": TP flag not set "
312 << get_message_id(_data, _data_length);
313 ret = false;
314 } else if (_data_length != its_length + VSOMEIP_SOMEIP_HEADER_SIZE) {
315 VSOMEIP_ERROR << __func__
316 << ": data length doesn't match header length field"
317 << get_message_id(_data, _data_length)
318 << " data: " << std::dec << _data_length
319 << " header: " << std::dec << its_length;
320 ret = false;
321 } else if (_segment_size != its_length - VSOMEIP_TP_HEADER_SIZE
322 - (VSOMEIP_FULL_HEADER_SIZE - VSOMEIP_SOMEIP_HEADER_SIZE)) {
323 VSOMEIP_ERROR << __func__
324 << ": segment size doesn't align with header length field"
325 << get_message_id(_data, _data_length)
326 << "segment size: " << std::dec << _segment_size
327 << " data: " << std::dec << _data_length
328 << " header: " << std::dec << its_length;
329 ret = false;
330 } else if (_segment_size > tp::tp_max_segment_length_) {
331 VSOMEIP_ERROR << __func__ << ": Segment exceeds allowed size "
332 << get_message_id(_data, _data_length)
333 << "segment size: " << std::dec << _segment_size << " (max. "
334 << std::dec << tp::tp_max_segment_length_
335 << ") data: " << std::dec << _data_length
336 << " header: " << std::dec << its_length;
337 ret = false;
338 } else if (_more_fragments && _segment_size % 16 > 0) {
339 VSOMEIP_ERROR << __func__ << ": Segment size not multiple of 16 "
340 << get_message_id(_data, _data_length)
341 << "segment size: " << std::dec << _segment_size
342 << " data: " << std::dec << _data_length
343 << " header: " << std::dec << its_length;
344 ret = false;
345 } else if (current_message_size_ + _segment_size > max_message_size_) {
346 VSOMEIP_ERROR << __func__ << ": Message exceeds maximum configured size: "
347 << get_message_id(_data, _data_length)
348 << "segment size: " << std::dec << _segment_size
349 << " current message size: " << std::dec << current_message_size_
350 << " maximum message size: " << std::dec << max_message_size_;
351 ret = false;
352 } else if (tp::get_offset(its_tp_header) + _segment_size > max_message_size_ ) {
353 VSOMEIP_ERROR << __func__ << ": SomeIP/TP offset field exceeds maximum configured message size: "
354 << get_message_id(_data, _data_length)
355 << " TP offset [bytes]: " << std::dec << tp::get_offset(its_tp_header)
356 << " segment size: " << std::dec << _segment_size
357 << " current message size: " << std::dec << current_message_size_
358 << " maximum message size: " << std::dec << max_message_size_;
359 ret = false;
360 }
361 return ret;
362 }
363
364 } // namespace tp
365 } // namespace vsomeip_v3
366