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. 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. 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 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 111 NanoPacket::NanoPacket() { 112 Reset(); 113 } 114 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 125 bool NanoPacket::ParsingIsComplete() const { 126 return parsing_state_ == ParsingState::Complete; 127 } 128 129 const std::vector<uint8_t>& NanoPacket::packet_buffer() const { 130 return packet_buffer_; 131 } 132 133 uint32_t NanoPacket::reason() const { 134 return reason_; 135 } 136 137 PacketReason NanoPacket::TypedReason() const { 138 return static_cast<PacketReason>(reason_); 139 } 140 141 const std::vector<uint8_t>& NanoPacket::packet_content() const { 142 return packet_content_; 143 } 144 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 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
