shaneajg | 0869f2c | 2020-07-08 10:39:14 -0400 | [diff] [blame] | 1 | // Copyright 2020 The Pigweed Authors |
| 2 | // |
| 3 | // Licensed under the Apache License, Version 2.0 (the "License"); you may not |
| 4 | // use this file except in compliance with the License. You may obtain a copy of |
| 5 | // the License at |
| 6 | // |
| 7 | // https://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, WITHOUT |
| 11 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the |
| 12 | // License for the specific language governing permissions and limitations under |
| 13 | // the License. |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 14 | |
Alexei Frolov | d3e5cb7 | 2021-01-08 13:08:45 -0800 | [diff] [blame] | 15 | #include "pw_hdlc/encoder.h" |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 16 | |
| 17 | #include <algorithm> |
| 18 | #include <array> |
| 19 | #include <cstddef> |
| 20 | #include <cstring> |
| 21 | #include <span> |
| 22 | |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 23 | #include "pw_bytes/endian.h" |
Alexei Frolov | 44cd952 | 2021-01-13 11:31:59 -0800 | [diff] [blame] | 24 | #include "pw_hdlc/internal/encoder.h" |
Alexei Frolov | b9fda58 | 2021-03-13 18:02:52 -0800 | [diff] [blame] | 25 | #include "pw_varint/varint.h" |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 26 | |
| 27 | using std::byte; |
| 28 | |
Alexei Frolov | d3e5cb7 | 2021-01-08 13:08:45 -0800 | [diff] [blame] | 29 | namespace pw::hdlc { |
Alexei Frolov | 44cd952 | 2021-01-13 11:31:59 -0800 | [diff] [blame] | 30 | namespace internal { |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 31 | |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 32 | Status EscapeAndWrite(const byte b, stream::Writer& writer) { |
Wyatt Hepler | cdb0f4e | 2020-09-14 08:49:17 -0700 | [diff] [blame] | 33 | if (b == kFlag) { |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 34 | return writer.Write(kEscapedFlag); |
| 35 | } |
Wyatt Hepler | cdb0f4e | 2020-09-14 08:49:17 -0700 | [diff] [blame] | 36 | if (b == kEscape) { |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 37 | return writer.Write(kEscapedEscape); |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 38 | } |
| 39 | return writer.Write(b); |
| 40 | } |
| 41 | |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 42 | Status Encoder::WriteData(ConstByteSpan data) { |
| 43 | auto begin = data.begin(); |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 44 | while (true) { |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 45 | auto end = std::find_if(begin, data.end(), NeedsEscaping); |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 46 | |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 47 | if (Status status = writer_.Write(std::span(begin, end)); !status.ok()) { |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 48 | return status; |
| 49 | } |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 50 | if (end == data.end()) { |
| 51 | fcs_.Update(data); |
Wyatt Hepler | 1b3da3a | 2021-01-07 13:26:57 -0800 | [diff] [blame] | 52 | return OkStatus(); |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 53 | } |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 54 | if (Status status = EscapeAndWrite(*end, writer_); !status.ok()) { |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 55 | return status; |
| 56 | } |
| 57 | begin = end + 1; |
| 58 | } |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 59 | } |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 60 | |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 61 | Status Encoder::FinishFrame() { |
Wyatt Hepler | eb8a34c | 2020-09-02 13:11:03 -0700 | [diff] [blame] | 62 | if (Status status = |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 63 | WriteData(bytes::CopyInOrder(std::endian::little, fcs_.value())); |
Wyatt Hepler | eb8a34c | 2020-09-02 13:11:03 -0700 | [diff] [blame] | 64 | !status.ok()) { |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 65 | return status; |
| 66 | } |
Wyatt Hepler | cdb0f4e | 2020-09-14 08:49:17 -0700 | [diff] [blame] | 67 | return writer_.Write(kFlag); |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 68 | } |
| 69 | |
Alexei Frolov | b9fda58 | 2021-03-13 18:02:52 -0800 | [diff] [blame] | 70 | size_t Encoder::MaxEncodedSize(uint64_t address, ConstByteSpan payload) { |
Alexei Frolov | 44cd952 | 2021-01-13 11:31:59 -0800 | [diff] [blame] | 71 | constexpr size_t kFcsMaxSize = 8; // Worst case FCS: 0x7e7e7e7e. |
Alexei Frolov | b9fda58 | 2021-03-13 18:02:52 -0800 | [diff] [blame] | 72 | size_t max_encoded_address_size = varint::EncodedSize(address) * 2; |
Alexei Frolov | 44cd952 | 2021-01-13 11:31:59 -0800 | [diff] [blame] | 73 | size_t encoded_payload_size = |
| 74 | payload.size() + |
| 75 | std::count_if(payload.begin(), payload.end(), NeedsEscaping); |
| 76 | |
Alexei Frolov | b9fda58 | 2021-03-13 18:02:52 -0800 | [diff] [blame] | 77 | return max_encoded_address_size + sizeof(kUnusedControl) + |
| 78 | encoded_payload_size + kFcsMaxSize; |
| 79 | } |
| 80 | |
| 81 | Status Encoder::StartFrame(uint64_t address, std::byte control) { |
| 82 | fcs_.clear(); |
| 83 | if (Status status = writer_.Write(kFlag); !status.ok()) { |
| 84 | return status; |
| 85 | } |
| 86 | |
| 87 | std::array<std::byte, 16> metadata_buffer; |
| 88 | size_t metadata_size = |
| 89 | varint::Encode(address, metadata_buffer, kAddressFormat); |
| 90 | if (metadata_size == 0) { |
| 91 | return Status::InvalidArgument(); |
| 92 | } |
| 93 | |
| 94 | metadata_buffer[metadata_size++] = control; |
| 95 | return WriteData(std::span(metadata_buffer).first(metadata_size)); |
Alexei Frolov | 44cd952 | 2021-01-13 11:31:59 -0800 | [diff] [blame] | 96 | } |
| 97 | |
| 98 | } // namespace internal |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 99 | |
Alexei Frolov | b9fda58 | 2021-03-13 18:02:52 -0800 | [diff] [blame] | 100 | Status WriteUIFrame(uint64_t address, |
Alexei Frolov | 6053c31 | 2020-12-09 22:43:55 -0800 | [diff] [blame] | 101 | ConstByteSpan payload, |
| 102 | stream::Writer& writer) { |
Alexei Frolov | 44cd952 | 2021-01-13 11:31:59 -0800 | [diff] [blame] | 103 | if (internal::Encoder::MaxEncodedSize(address, payload) > |
| 104 | writer.ConservativeWriteLimit()) { |
| 105 | return Status::ResourceExhausted(); |
| 106 | } |
| 107 | |
| 108 | internal::Encoder encoder(writer); |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 109 | |
Alexei Frolov | 6053c31 | 2020-12-09 22:43:55 -0800 | [diff] [blame] | 110 | if (Status status = encoder.StartUnnumberedFrame(address); !status.ok()) { |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 111 | return status; |
| 112 | } |
Wyatt Hepler | bfb669d | 2020-09-03 13:32:00 -0700 | [diff] [blame] | 113 | if (Status status = encoder.WriteData(payload); !status.ok()) { |
| 114 | return status; |
| 115 | } |
| 116 | return encoder.FinishFrame(); |
shaneajg | e6d3a61 | 2020-07-23 18:40:28 -0400 | [diff] [blame] | 117 | } |
| 118 | |
Alexei Frolov | d3e5cb7 | 2021-01-08 13:08:45 -0800 | [diff] [blame] | 119 | } // namespace pw::hdlc |