1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "nanopacket.h"
18
19 namespace android {
20
21 constexpr uint8_t kSyncByte(0x31);
22
23 // CRC constants.
24 constexpr uint32_t kInitialCrc(0xffffffff);
25 constexpr uint32_t kCrcTable[] = {
26 0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9,
27 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
28 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61,
29 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD
30 };
31
32 // Computes the CRC of one word.
Crc32Word(uint32_t crc,uint32_t data,int cnt)33 uint32_t Crc32Word(uint32_t crc, uint32_t data, int cnt) {
34 crc = crc ^ data;
35
36 for (int i = 0; i < cnt; i++) {
37 crc = (crc << 4) ^ kCrcTable[crc >> 28];
38 }
39
40 return crc;
41 }
42
43 // Computes the CRC32 of a buffer given a starting CRC.
Crc32(const uint8_t * buffer,int length)44 uint32_t Crc32(const uint8_t *buffer, int length) {
45 int i;
46 uint32_t crc = kInitialCrc;
47
48 // Word by word crc32
49 for (i = 0; i < (length >> 2); i++) {
50 crc = Crc32Word(crc, ((uint32_t *)buffer)[i], 8);
51 }
52
53 // Zero pad last word if required.
54 if (length & 0x3) {
55 uint32_t word = 0;
56
57 for (i*=4; i<length; i++) {
58 word |= buffer[i] << ((i & 0x3) * 8);
59 }
60
61 crc = Crc32Word(crc, word, 8);
62 }
63
64 return crc;
65 }
66
NanoPacket(uint32_t sequence_number,PacketReason reason,const std::vector<uint8_t> * data)67 NanoPacket::NanoPacket(uint32_t sequence_number, PacketReason reason,
68 const std::vector<uint8_t> *data) {
69 Reset();
70 parsing_state_ = ParsingState::Complete;
71 sequence_number_ = sequence_number;
72 reason_ = static_cast<uint32_t>(reason);
73
74 // Resize the buffer to accomodate header, footer and data content.
75 size_t data_size = data ? data->size() : 0;
76 size_t required_buffer_size = 14 + data_size;
77 if (packet_buffer_.size() < required_buffer_size) {
78 packet_buffer_.resize(required_buffer_size);
79 }
80
81 if (packet_content_.size() < required_buffer_size) {
82 packet_content_.resize(required_buffer_size);
83 }
84
85 // Format the header of the packet.
86 packet_buffer_[0] = kSyncByte;
87 packet_buffer_[1] = sequence_number;
88 packet_buffer_[2] = sequence_number >> 8;
89 packet_buffer_[3] = sequence_number >> 16;
90 packet_buffer_[4] = sequence_number >> 24;
91 packet_buffer_[5] = static_cast<uint32_t>(reason);
92 packet_buffer_[6] = static_cast<uint32_t>(reason) >> 8;
93 packet_buffer_[7] = static_cast<uint32_t>(reason) >> 16;
94 packet_buffer_[8] = static_cast<uint32_t>(reason) >> 24;
95 packet_buffer_[9] = data_size;
96
97 // Insert the data content of the packet.
98 if (data) {
99 std::copy(data->begin(), data->end(), packet_buffer_.begin() + 10);
100 std::copy(data->begin(), data->end(), packet_content_.begin());
101 }
102
103 // Format the CRC footer.
104 uint32_t crc = Crc32(packet_buffer_.data(), required_buffer_size - 4);
105 packet_buffer_[data_size + 10] = crc;
106 packet_buffer_[data_size + 11] = crc >> 8;
107 packet_buffer_[data_size + 12] = crc >> 16;
108 packet_buffer_[data_size + 13] = crc >> 24;
109 }
110
NanoPacket()111 NanoPacket::NanoPacket() {
112 Reset();
113 }
114
Reset()115 void NanoPacket::Reset() {
116 packet_buffer_.clear();
117 parsing_state_ = ParsingState::Idle;
118 parsing_progress_ = 0;
119 sequence_number_ = 0;
120 reason_ = 0;
121 packet_content_.clear();
122 crc_ = 0;
123 }
124
ParsingIsComplete() const125 bool NanoPacket::ParsingIsComplete() const {
126 return parsing_state_ == ParsingState::Complete;
127 }
128
packet_buffer() const129 const std::vector<uint8_t>& NanoPacket::packet_buffer() const {
130 return packet_buffer_;
131 }
132
reason() const133 uint32_t NanoPacket::reason() const {
134 return reason_;
135 }
136
TypedReason() const137 PacketReason NanoPacket::TypedReason() const {
138 return static_cast<PacketReason>(reason_);
139 }
140
packet_content() const141 const std::vector<uint8_t>& NanoPacket::packet_content() const {
142 return packet_content_;
143 }
144
Parse(uint8_t * buffer,size_t length,size_t * bytes_parsed)145 NanoPacket::ParseResult NanoPacket::Parse(uint8_t *buffer, size_t length,
146 size_t *bytes_parsed) {
147 for (size_t i = 0; i < length; i++) {
148 // Once the state machine is not idle, save all bytes to the current
149 // packet to allow CRC to be computed at the end.
150 if (parsing_state_ != ParsingState::Idle) {
151 packet_buffer_.push_back(buffer[i]);
152 }
153
154 // Proceed through the various states of protocol parsing.
155 if (parsing_state_ == ParsingState::Idle && buffer[i] == kSyncByte) {
156 packet_buffer_.push_back(buffer[i]);
157 parsing_state_ = ParsingState::ParsingSequenceNumber;
158 } else if (parsing_state_ == ParsingState::ParsingSequenceNumber
159 && DeserializeWord(&sequence_number_, buffer[i])) {
160 parsing_state_ = ParsingState::ParsingReason;
161 } else if (parsing_state_ == ParsingState::ParsingReason
162 && DeserializeWord(&reason_, buffer[i])) {
163 parsing_state_ = ParsingState::ParsingLength;
164 } else if (parsing_state_ == ParsingState::ParsingLength) {
165 uint8_t length = buffer[i];
166 if (length > 0) {
167 packet_content_.resize(buffer[i]);
168 parsing_state_ = ParsingState::ParsingContent;
169 } else {
170 parsing_state_ = ParsingState::ParsingCrc;
171 }
172 } else if (parsing_state_ == ParsingState::ParsingContent) {
173 packet_content_[parsing_progress_++] = buffer[i];
174
175 if (parsing_progress_ == packet_content_.size()) {
176 parsing_progress_ = 0;
177 parsing_state_ = ParsingState::ParsingCrc;
178 }
179 } else if (parsing_state_ == ParsingState::ParsingCrc
180 && DeserializeWord(&crc_, buffer[i])) {
181 *bytes_parsed = i + 1;
182 if (ValidateCrc()) {
183 parsing_state_ = ParsingState::Complete;
184 return ParseResult::Success;
185 } else {
186 return ParseResult::CrcMismatch;
187 }
188 }
189 }
190
191 *bytes_parsed = length;
192 return ParseResult::Incomplete;
193 }
194
ValidateCrc()195 bool NanoPacket::ValidateCrc() {
196 size_t crc_length = packet_buffer_.size() - 4;
197 uint32_t computed_crc = Crc32(packet_buffer_.data(), crc_length);
198
199 if (computed_crc != crc_) {
200 Reset();
201 return false;
202 }
203
204 return true;
205 }
206
207 } // namespace android
208