1 /* 2 * Copyright 2018, 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 "wifi_forwarder.h" 18 19 #include "log.h" 20 21 #include <inttypes.h> 22 #include <arpa/inet.h> 23 #include <errno.h> 24 #include <linux/if_packet.h> 25 #include <linux/kernel.h> 26 // Ignore warning about unused static qemu pipe function 27 #pragma clang diagnostic push 28 #pragma clang diagnostic ignored "-Wunused-function" 29 #include <qemu_pipe.h> 30 #pragma clang diagnostic pop 31 #include <string.h> 32 #include <net/ethernet.h> 33 #include <net/if.h> 34 #include <pcap/pcap.h> 35 #include <sys/socket.h> 36 #include <sys/types.h> 37 #include <unistd.h> 38 39 static const char kQemuPipeName[] = "qemud:wififorward"; 40 41 // The largest packet size to capture with pcap on the monitor interface 42 static const int kPcapSnapLength = 65536; 43 44 static const size_t kForwardBufferIncrement = 32768; 45 static const size_t kForwardBufferMaxSize = 1 << 20; 46 47 static const uint32_t kWifiForwardMagic = 0xD5C4B3A2; 48 49 struct WifiForwardHeader { 50 WifiForwardHeader(uint32_t dataLength, uint32_t radioLength) 51 : magic(__cpu_to_le32(kWifiForwardMagic)) 52 , fullLength(__cpu_to_le32(dataLength + sizeof(WifiForwardHeader))) 53 , radioLength(__cpu_to_le32(radioLength)) { } 54 55 uint32_t magic; 56 uint32_t fullLength; 57 uint32_t radioLength; 58 } __attribute__((__packed__)); 59 60 struct RadioTapHeader { 61 uint8_t it_version; 62 uint8_t it_pad; 63 uint16_t it_len; 64 uint32_t it_present; 65 } __attribute__((__packed__)); 66 67 enum class FrameType { 68 Management, 69 Control, 70 Data, 71 Extension 72 }; 73 74 enum class ManagementType { 75 AssociationRequest, 76 AssociationResponse, 77 ReassociationRequest, 78 ReassociationResponse, 79 ProbeRequest, 80 ProbeResponse, 81 TimingAdvertisement, 82 Beacon, 83 Atim, 84 Disassociation, 85 Authentication, 86 Deauthentication, 87 Action, 88 ActionNoAck, 89 }; 90 91 enum class ControlType { 92 BeamFormingReportPoll, 93 VhtNdpAnnouncement, 94 ControlFrameExtension, 95 ControlWrapper, 96 BlockAckReq, 97 BlockAck, 98 PsPoll, 99 Rts, 100 Cts, 101 Ack, 102 CfEnd, 103 CfEndCfAck 104 }; 105 106 // Since the IEEE 802.11 header can vary in size depending on content we have 107 // to establish a minimum size that we need to be able to inspect and forward 108 // the frame. Every frame need to contain at least frame_control, duration_id, 109 // and addr1. 110 static const uint32_t kMinimumIeee80211Size = sizeof(uint16_t) + 111 sizeof(uint16_t) + 112 sizeof(MacAddress); 113 114 WifiForwarder::WifiForwarder(const char* monitorInterfaceName) 115 : mInterfaceName(monitorInterfaceName), 116 mDeadline(Pollable::Timestamp::max()), 117 mMonitorPcap(nullptr), 118 mPipeFd(-1) { 119 } 120 121 WifiForwarder::~WifiForwarder() { 122 cleanup(); 123 } 124 125 Result WifiForwarder::init() { 126 if (mMonitorPcap || mPipeFd != -1) { 127 return Result::error("WifiForwarder already initialized"); 128 } 129 130 mPipeFd = qemu_pipe_open(kQemuPipeName); 131 if (mPipeFd == -1) { 132 // It's OK if this fails, the emulator might not have been started with 133 // this feature enabled. If it's not enabled we'll try again later, in 134 // the meantime there is no point in opening the monitor socket either. 135 LOGE("WifiForwarder unable to open QEMU pipe: %s", strerror(errno)); 136 mDeadline = Pollable::Clock::now() + std::chrono::minutes(1); 137 return Result::success(); 138 } 139 140 char errorMsg[PCAP_ERRBUF_SIZE]; 141 memset(errorMsg, 0, sizeof(errorMsg)); 142 mMonitorPcap = pcap_create(mInterfaceName.c_str(), errorMsg); 143 if (mMonitorPcap == nullptr) { 144 return Result::error("WifiForwarder cannot create pcap handle: %s", 145 errorMsg); 146 } 147 int result = pcap_set_snaplen(mMonitorPcap, kPcapSnapLength); 148 if (result != 0) { 149 return Result::error("WifiForwader cannot set pcap snap length: %s", 150 pcap_statustostr(result)); 151 } 152 153 result = pcap_set_promisc(mMonitorPcap, 1); 154 if (result != 0) { 155 return Result::error("WifiForwader cannot set pcap promisc mode: %s", 156 pcap_statustostr(result)); 157 } 158 159 result = pcap_set_immediate_mode(mMonitorPcap, 1); 160 if (result != 0) { 161 return Result::error("WifiForwader cannot set pcap immediate mode: %s", 162 pcap_statustostr(result)); 163 } 164 165 result = pcap_activate(mMonitorPcap); 166 if (result > 0) { 167 // A warning, log it but keep going 168 LOGW("WifiForwader received warnings when activating pcap: %s", 169 pcap_statustostr(result)); 170 } else if (result < 0) { 171 // An error, return 172 return Result::error("WifiForwader unable to activate pcap: %s", 173 pcap_statustostr(result)); 174 } 175 176 int datalinkType = pcap_datalink(mMonitorPcap); 177 if (datalinkType != DLT_IEEE802_11_RADIO) { 178 // Unexpected data link encapsulation, we don't support this 179 return Result::error("WifiForwarder detected incompatible data link " 180 "encapsulation: %d", datalinkType); 181 } 182 // All done 183 return Result::success(); 184 } 185 186 187 void WifiForwarder::getPollData(std::vector<pollfd>* fds) const { 188 if (mPipeFd == -1) { 189 return; 190 } 191 int pcapFd = pcap_get_selectable_fd(mMonitorPcap); 192 if (pcapFd != -1) { 193 fds->push_back(pollfd{pcapFd, POLLIN, 0}); 194 } else { 195 LOGE("WifiForwarder unable to get pcap fd"); 196 } 197 if (mPipeFd != -1) { 198 fds->push_back(pollfd{mPipeFd, POLLIN, 0}); 199 } 200 } 201 202 Pollable::Timestamp WifiForwarder::getTimeout() const { 203 // If there is no pipe return the deadline, we're going to retry, otherwise 204 // use an infinite timeout. 205 return mPipeFd == -1 ? mDeadline : Pollable::Timestamp::max(); 206 } 207 208 bool WifiForwarder::onReadAvailable(int fd, int* /*status*/) { 209 if (fd == mPipeFd) { 210 injectFromPipe(); 211 } else { 212 forwardFromPcap(); 213 } 214 return true; 215 } 216 217 void WifiForwarder::forwardFromPcap() { 218 struct pcap_pkthdr* header = nullptr; 219 const u_char* data = nullptr; 220 int result = pcap_next_ex(mMonitorPcap, &header, &data); 221 if (result == 0) { 222 // Timeout, nothing to do 223 return; 224 } else if (result < 0) { 225 LOGE("WifiForwarder failed to read from pcap: %s", 226 pcap_geterr(mMonitorPcap)); 227 return; 228 } 229 if (header->caplen < header->len) { 230 LOGE("WifiForwarder received packet exceeding capture length: %u < %u", 231 header->caplen, header->len); 232 return; 233 } 234 235 if (mPipeFd == -1) { 236 LOGE("WifiForwarder unable to forward data, pipe not open"); 237 return; 238 } 239 240 if (header->caplen < sizeof(RadioTapHeader)) { 241 // This packet is too small to be a valid radiotap packet, drop it 242 LOGE("WifiForwarder captured packet that is too small: %u", 243 header->caplen); 244 return; 245 } 246 247 auto radiotap = reinterpret_cast<const RadioTapHeader*>(data); 248 uint32_t radioLen = __le16_to_cpu(radiotap->it_len); 249 if (header->caplen < radioLen + kMinimumIeee80211Size) { 250 // This packet is too small to contain a valid IEEE 802.11 frame 251 LOGE("WifiForwarder captured packet that is too small: %u < %u", 252 header->caplen, radioLen + kMinimumIeee80211Size); 253 return; 254 } 255 256 WifiForwardHeader forwardHeader(header->caplen, radioLen); 257 258 if (!WriteFully(mPipeFd, &forwardHeader, sizeof(forwardHeader))) { 259 LOGE("WifiForwarder failed to write to pipe: %s", strerror(errno)); 260 return; 261 } 262 263 if (!WriteFully(mPipeFd, data, header->caplen)) { 264 LOGE("WifiForwarder failed to write to pipe: %s", strerror(errno)); 265 return; 266 } 267 } 268 269 void WifiForwarder::injectFromPipe() { 270 size_t start = mMonitorBuffer.size(); 271 size_t newSize = start + kForwardBufferIncrement; 272 if (newSize > kForwardBufferMaxSize) { 273 // We've exceeded the maximum allowed size, drop everything we have so 274 // far and start over. This is most likely caused by some delay in 275 // injection or the injection failing in which case keeping old data 276 // around isn't going to be very useful. 277 LOGE("WifiForwarder ran out of buffer space"); 278 newSize = kForwardBufferIncrement; 279 start = 0; 280 } 281 mMonitorBuffer.resize(newSize); 282 283 while (true) { 284 int result = ::read(mPipeFd, 285 mMonitorBuffer.data() + start, 286 mMonitorBuffer.size() - start); 287 if (result < 0) { 288 if (errno == EINTR) { 289 continue; 290 } 291 LOGE("WifiForwarder failed to read to forward buffer: %s", 292 strerror(errno)); 293 // Return the buffer to its previous size 294 mMonitorBuffer.resize(start); 295 return; 296 } else if (result == 0) { 297 // Nothing received, nothing to write 298 // Return the buffer to its previous size 299 mMonitorBuffer.resize(start); 300 LOGE("WifiForwarder did not receive anything to inject"); 301 return; 302 } 303 // Adjust the buffer size to match everything we recieved 304 mMonitorBuffer.resize(start + static_cast<size_t>(result)); 305 break; 306 } 307 308 while (mMonitorBuffer.size() >= 309 sizeof(WifiForwardHeader) + sizeof(RadioTapHeader)) { 310 auto fwd = reinterpret_cast<WifiForwardHeader*>(mMonitorBuffer.data()); 311 if (__le32_to_cpu(fwd->magic) != kWifiForwardMagic) { 312 // We are not properly aligned, this can happen for the first read 313 // if the client or server happens to send something that's in the 314 // middle of a stream. Attempt to find the next packet boundary. 315 LOGE("WifiForwarder found incorrect magic, finding next magic"); 316 uint32_t le32magic = __cpu_to_le32(kWifiForwardMagic); 317 auto next = reinterpret_cast<unsigned char*>( 318 ::memmem(mMonitorBuffer.data(), mMonitorBuffer.size(), 319 &le32magic, sizeof(le32magic))); 320 if (next) { 321 // We've found a possible candidate, erase everything before 322 size_t length = next - mMonitorBuffer.data(); 323 mMonitorBuffer.erase(mMonitorBuffer.begin(), 324 mMonitorBuffer.begin() + length); 325 continue; 326 } else { 327 // There is no possible candidate, drop everything except the 328 // last three bytes. The last three bytes could possibly be the 329 // start of the next magic without actually triggering the 330 // search above. 331 if (mMonitorBuffer.size() > 3) { 332 mMonitorBuffer.erase(mMonitorBuffer.begin(), 333 mMonitorBuffer.end() - 3); 334 } 335 // In this case there is nothing left to parse so just return 336 // right away. 337 return; 338 } 339 } 340 // The length according to the wifi forward header 341 const size_t fullLength = __le32_to_cpu(fwd->fullLength); 342 const size_t payloadLength = fullLength - sizeof(WifiForwardHeader); 343 const size_t radioLength = __le32_to_cpu(fwd->radioLength); 344 // Get the radio tap header, right after the wifi forward header 345 unsigned char* radioTapLocation = mMonitorBuffer.data() + sizeof(*fwd); 346 auto hdr = reinterpret_cast<RadioTapHeader*>(radioTapLocation); 347 const size_t radioHdrLength = __le16_to_cpu(hdr->it_len); 348 349 if (radioLength != radioHdrLength) { 350 LOGE("WifiForwarder radiotap (%u), forwarder (%u) length mismatch", 351 (unsigned)(radioHdrLength), (unsigned)radioLength); 352 // The wifi forward header radio length does not match up with the 353 // radiotap header length. Either this was not an actual packet 354 // boundary or the packet is malformed. Remove a single byte from 355 // the buffer to trigger a new magic marker search. 356 mMonitorBuffer.erase(mMonitorBuffer.begin(), 357 mMonitorBuffer.begin() + 1); 358 continue; 359 } 360 // At this point we have verified that the magic marker is present and 361 // that the length in the wifi forward header matches the radiotap 362 // header length. We're now reasonably sure this is actually a valid 363 // packet that we can process. 364 365 if (fullLength > mMonitorBuffer.size()) { 366 // We have not received enough data yet, wait for more to arrive. 367 return; 368 } 369 370 if (hdr->it_version != 0) { 371 // Unknown header version, skip this packet because we don't know 372 // how to handle it. 373 LOGE("WifiForwarder encountered unknown radiotap version %u", 374 static_cast<unsigned>(hdr->it_version)); 375 mMonitorBuffer.erase(mMonitorBuffer.begin(), 376 mMonitorBuffer.begin() + fullLength); 377 continue; 378 } 379 380 if (mMonitorPcap) { 381 // A sufficient amount of data has arrived, forward it. 382 int result = pcap_inject(mMonitorPcap, hdr, payloadLength); 383 if (result < 0) { 384 LOGE("WifiForwarder failed to inject %" PRIu64 " bytes: %s", 385 static_cast<uint64_t>(payloadLength), 386 pcap_geterr(mMonitorPcap)); 387 } else if (static_cast<size_t>(result) < payloadLength) { 388 LOGE("WifiForwarder only injected %d out of %" PRIu64 " bytes", 389 result, static_cast<uint64_t>(payloadLength)); 390 } 391 } else { 392 LOGE("WifiForwarder could not forward to monitor, pcap not set up"); 393 } 394 mMonitorBuffer.erase(mMonitorBuffer.begin(), 395 mMonitorBuffer.begin() + fullLength); 396 } 397 398 } 399 400 void WifiForwarder::cleanup() { 401 if (mMonitorPcap) { 402 pcap_close(mMonitorPcap); 403 mMonitorPcap = nullptr; 404 } 405 if (mPipeFd != -1) { 406 ::close(mPipeFd); 407 mPipeFd = -1; 408 } 409 } 410 411 bool WifiForwarder::onClose(int /*fd*/, int* status) { 412 // Don't care which fd, just start all over again for simplicity 413 cleanup(); 414 Result res = init(); 415 if (!res) { 416 *status = 1; 417 return false; 418 } 419 return true; 420 } 421 422 bool WifiForwarder::onTimeout(int* status) { 423 if (mPipeFd == -1 && mMonitorPcap == nullptr) { 424 Result res = init(); 425 if (!res) { 426 *status = 1; 427 return false; 428 } 429 } 430 return true; 431 } 432 433