| 57 | } |
| 58 | |
| 59 | bool tp_message::add_segment(const byte_t* const _data, std::uint32_t _data_length) { |
| 60 | if (_data_length < VSOMEIP_FULL_HEADER_SIZE + VSOMEIP_TP_HEADER_SIZE) { |
| 61 | VSOMEIP_ERROR_P << "Received too short SOME/IP-TP message " << get_message_id(_data, _data_length); |
| 62 | return false; |
| 63 | } |
| 64 | bool ret = false; |
| 65 | |
| 66 | const length_t its_segment_size = _data_length - VSOMEIP_FULL_HEADER_SIZE - VSOMEIP_TP_HEADER_SIZE; |
| 67 | const tp_header_t its_tp_header = bithelper::read_uint32_be(&_data[VSOMEIP_TP_HEADER_POS_MIN]); |
| 68 | |
| 69 | if (check_lengths(_data, _data_length, its_segment_size, tp::more_segments(its_tp_header))) { |
| 70 | const length_t its_offset = tp::get_offset(its_tp_header); |
| 71 | const auto emplace_res = segments_.emplace(segment_t(its_offset, its_offset + its_segment_size - 1)); |
| 72 | if (!emplace_res.second) { |
| 73 | VSOMEIP_WARNING_P << "Received duplicate segment " << get_message_id(_data, _data_length) << " TP offset: 0x" |
| 74 | << hex8(its_offset); |
| 75 | } else { |
| 76 | const auto& seg_current = emplace_res.first; |
| 77 | const auto& seg_next = std::next(seg_current); |
| 78 | const bool current_segment_is_last = (seg_next == segments_.end()); |
| 79 | const bool current_segment_is_first = (seg_current == segments_.begin()); |
| 80 | if (current_segment_is_last) { |
| 81 | if (current_segment_is_first) { |
| 82 | // received segment of message but the first received segment was invalid |
| 83 | // resize + append |
| 84 | VSOMEIP_WARNING_P << "Received 2nd segment of message. But the first received segment already wasn't accepted. " |
| 85 | << "The message can't be completed anymore: " << get_message_id(_data, _data_length); |
| 86 | if (its_offset != 0) { |
| 87 | // segment different than the first segment was received |
| 88 | message_.resize(VSOMEIP_FULL_HEADER_SIZE + its_offset, 0x0); |
| 89 | if (!tp::more_segments(its_tp_header)) { |
| 90 | // received the last segment of the segmented message first |
| 91 | last_segment_received_ = true; |
| 92 | } |
| 93 | } |
| 94 | // append to end of message |
| 95 | message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS], &_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size); |
| 96 | current_message_size_ += its_segment_size; |
| 97 | } else { |
| 98 | const auto& seg_prev = std::prev(seg_current); |
| 99 | if (seg_prev->end_ < seg_current->start_) { |
| 100 | const bool direct_previous_segment_present = (seg_prev->end_ + 1 == seg_current->start_); |
| 101 | if (!direct_previous_segment_present) { |
| 102 | // received segment out of order behind the current end of received |
| 103 | // segments |
| 104 | // resize + append |
| 105 | message_.resize(VSOMEIP_FULL_HEADER_SIZE + its_offset, 0x0); |
| 106 | } |
| 107 | // append to end of message |
| 108 | message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS], &_data[VSOMEIP_TP_PAYLOAD_POS] + its_segment_size); |
| 109 | current_message_size_ += its_segment_size; |
| 110 | } else { |
| 111 | // this segment starts before the end of the previous and |
| 112 | // would overwrite already received data |
| 113 | VSOMEIP_WARNING_P << "Completely accepting segment would overwrite previous segment " |
| 114 | << get_message_id(_data, _data_length) << "previous segment end: " << seg_prev->end_ + 1 |
| 115 | << " this segment start: " << seg_current->start_; |
| 116 | message_.insert(message_.end(), &_data[VSOMEIP_TP_PAYLOAD_POS] + ((seg_prev->end_ + 1) - seg_current->start_), |