1 /* 2 * Copyright (C) 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 "common/commands/wifi_relay/mac80211_hwsim.h" 18 19 #include "common/commands/wifi_relay/mac80211_hwsim_driver.h" 20 21 #include <glog/logging.h> 22 #include <netlink/genl/ctrl.h> 23 #include <netlink/genl/genl.h> 24 #include <signal.h> 25 #include <sys/uio.h> 26 #include <gflags/gflags.h> 27 28 DEFINE_string( 29 pcap, "", "Path to save a pcap file of packets"); 30 31 static constexpr char kWifiSimFamilyName[] = "MAC80211_HWSIM"; 32 static constexpr char kNl80211FamilyName[] = "nl80211"; 33 34 static constexpr uint32_t kSignalLevelDefault = -24; 35 36 #if !defined(ETH_ALEN) 37 static constexpr size_t ETH_ALEN = 6; 38 #endif 39 40 namespace { 41 42 struct pcap_hdr_t { 43 uint32_t magic_number; /* magic number */ 44 uint16_t version_major; /* major version number */ 45 uint16_t version_minor; /* minor version number */ 46 int32_t thiszone; /* GMT to local correction */ 47 uint32_t sigfigs; /* accuracy of timestamps */ 48 uint32_t snaplen; /* max length of captured packets, in octets */ 49 uint32_t network; /* data link type */ 50 }; 51 52 struct pcaprec_hdr_t { 53 uint32_t ts_sec; /* timestamp seconds */ 54 uint32_t ts_usec; /* timestamp microseconds */ 55 uint32_t incl_len; /* number of octets of packet saved in file */ 56 uint32_t orig_len; /* actual length of packet */ 57 }; 58 59 const pcap_hdr_t pcap_file_header{ 60 0xa1b2c3d4, 61 2, 62 4, 63 0, 64 0, 65 65536, 66 105 // IEEE802.11 without radiotap 67 }; 68 69 void WritePCap(const void* buffer, size_t length) { 70 if (FLAGS_pcap.empty()) { 71 return; 72 } 73 static int pcap = -1; 74 if (pcap == -1) { 75 pcap = open(FLAGS_pcap.c_str(), O_RDWR|O_CREAT|O_TRUNC, 0644); 76 if (pcap == -1) { 77 return; 78 } 79 (void)write(pcap, &pcap_file_header, sizeof(pcap_file_header)); 80 } 81 size_t write_length = length; 82 if (write_length > pcap_file_header.snaplen) { 83 write_length = pcap_file_header.snaplen; 84 } 85 pcaprec_hdr_t hdr; 86 struct timespec now; 87 clock_gettime(CLOCK_REALTIME, &now); 88 hdr.ts_sec = now.tv_sec; 89 hdr.ts_usec = now.tv_nsec / 1000; 90 hdr.incl_len = write_length; 91 hdr.orig_len = length; 92 struct iovec iov[2] { {&hdr, sizeof(hdr)}, 93 { const_cast<void*>(buffer), static_cast<size_t>(write_length)}}; 94 (void)writev(pcap, iov, 2); 95 } 96 97 } 98 99 Mac80211HwSim::Remote::Remote( 100 Mac80211HwSim *parent, 101 vsoc::wifi::WifiExchangeView *wifiExchange) 102 : mParent(parent), 103 mWifiExchange(wifiExchange) { 104 mWifiWorker = mWifiExchange->StartWorker(); 105 106 mThread = std::thread([this]{ 107 std::unique_ptr<uint8_t[]> buf( 108 new uint8_t[Mac80211HwSim::kMessageSizeMax]); 109 110 for (;;) { 111 intptr_t res = 112 mWifiExchange->Recv(buf.get(), Mac80211HwSim::kMessageSizeMax); 113 114 if (res < 0) { 115 LOG(ERROR) << "WifiExchangeView::Recv failed w/ res " << res; 116 continue; 117 } 118 WritePCap(buf.get(), static_cast<size_t>(res)); 119 120 // LOG(INFO) << "GUEST->HOST packet of size " << res; 121 mParent->injectFrame(buf.get(), res); 122 }}); 123 } 124 125 Mac80211HwSim::Remote::~Remote() { 126 mDone = true; 127 mWifiExchange->InterruptSelf(); 128 129 mThread.join(); 130 } 131 132 intptr_t Mac80211HwSim::Remote::send(const void *data, size_t size) { 133 WritePCap(data, size); 134 return mWifiExchange->Send(data, size); 135 } 136 137 Mac80211HwSim::Mac80211HwSim(const MacAddress &mac) 138 : mMAC(mac), 139 mSock(nullptr, nl_socket_free) { 140 int res; 141 142 mSock.reset(nl_socket_alloc()); 143 144 if (mSock == nullptr) { 145 goto bail; 146 } 147 148 res = nl_connect(mSock.get(), NETLINK_GENERIC); 149 if (res < 0) { 150 LOG(ERROR) << "nl_connect failed (" << nl_geterror(res) << ")"; 151 mInitCheck = res; 152 goto bail; 153 } 154 155 nl_socket_disable_seq_check(mSock.get()); 156 157 res = nl_socket_set_buffer_size( 158 mSock.get(), kMessageSizeMax, kMessageSizeMax); 159 160 if (res < 0) { 161 LOG(ERROR) 162 << "nl_socket_set_buffer_size failed (" 163 << nl_geterror(res) 164 << ")"; 165 166 mInitCheck = res; 167 goto bail; 168 } 169 170 mMac80211Family = genl_ctrl_resolve(mSock.get(), kWifiSimFamilyName); 171 if (mMac80211Family <= 0) { 172 LOG(ERROR) << "genl_ctrl_resolve failed."; 173 mInitCheck = -ENODEV; 174 goto bail; 175 } 176 177 mNl80211Family = genl_ctrl_resolve(mSock.get(), kNl80211FamilyName); 178 if (mNl80211Family <= 0) { 179 LOG(ERROR) << "genl_ctrl_resolve failed."; 180 mInitCheck = -ENODEV; 181 goto bail; 182 } 183 184 #if !defined(CUTTLEFISH_HOST) 185 res = registerOrSubscribe(mMAC); 186 187 if (res < 0) { 188 mInitCheck = res; 189 goto bail; 190 } 191 #endif 192 193 mInitCheck = 0; 194 return; 195 196 bail: 197 ; 198 } 199 200 int Mac80211HwSim::initCheck() const { 201 return mInitCheck; 202 } 203 204 int Mac80211HwSim::socketFd() const { 205 return nl_socket_get_fd(mSock.get()); 206 } 207 208 void Mac80211HwSim::ackFrame(nlmsghdr *inMsg) { 209 nlattr *attrs[__HWSIM_ATTR_MAX + 1]; 210 int res = genlmsg_parse( 211 inMsg, 212 0 /* hdrlen */, 213 attrs, 214 __HWSIM_ATTR_MAX, 215 nullptr /* policy */); 216 217 if (res < 0) { 218 LOG(ERROR) << "genlmsg_parse failed."; 219 return; 220 } 221 222 uint32_t flags = nla_get_u32(attrs[HWSIM_ATTR_FLAGS]); 223 224 if (!(flags & HWSIM_TX_CTL_REQ_TX_STATUS)) { 225 LOG(VERBOSE) << "Frame doesn't require TX_STATUS."; 226 return; 227 } 228 229 flags |= HWSIM_TX_STAT_ACK; 230 231 const uint8_t *xmitterAddr = 232 static_cast<const uint8_t *>( 233 nla_data(attrs[HWSIM_ATTR_ADDR_TRANSMITTER])); 234 235 size_t txRatesLen = nla_len(attrs[HWSIM_ATTR_TX_INFO]); 236 237 const struct hwsim_tx_rate *txRates = 238 static_cast<const struct hwsim_tx_rate *>( 239 nla_data(attrs[HWSIM_ATTR_TX_INFO])); 240 241 uint64_t cookie = nla_get_u64(attrs[HWSIM_ATTR_COOKIE]); 242 243 std::unique_ptr<nl_msg, void (*)(nl_msg *)> outMsg( 244 nlmsg_alloc(), nlmsg_free); 245 246 genlmsg_put( 247 outMsg.get(), 248 NL_AUTO_PID, 249 NL_AUTO_SEQ, 250 mMac80211Family, 251 0 /* hdrlen */, 252 NLM_F_REQUEST, 253 HWSIM_CMD_TX_INFO_FRAME, 254 0 /* version */); 255 256 nla_put(outMsg.get(), HWSIM_ATTR_ADDR_TRANSMITTER, ETH_ALEN, xmitterAddr); 257 nla_put_u32(outMsg.get(), HWSIM_ATTR_FLAGS, flags); 258 nla_put_u32(outMsg.get(), HWSIM_ATTR_SIGNAL, kSignalLevelDefault); 259 nla_put(outMsg.get(), HWSIM_ATTR_TX_INFO, txRatesLen, txRates); 260 nla_put_u64(outMsg.get(), HWSIM_ATTR_COOKIE, cookie); 261 262 res = nl_send_auto_complete(mSock.get(), outMsg.get()); 263 if (res < 0) { 264 LOG(ERROR) << "Sending TX Info failed. (" << nl_geterror(res) << ")"; 265 } else { 266 LOG(VERBOSE) << "Sending TX Info SUCCEEDED."; 267 } 268 } 269 270 void Mac80211HwSim::injectFrame(const void *data, size_t size) { 271 std::unique_ptr<nl_msg, void (*)(nl_msg *)> msg(nlmsg_alloc(), nlmsg_free); 272 273 genlmsg_put( 274 msg.get(), 275 NL_AUTO_PID, 276 NL_AUTO_SEQ, 277 mMac80211Family, 278 0 /* hdrlen */, 279 NLM_F_REQUEST, 280 HWSIM_CMD_FRAME, 281 0 /* version */); 282 283 CHECK_EQ(mMAC.size(), static_cast<size_t>(ETH_ALEN)); 284 nla_put(msg.get(), HWSIM_ATTR_ADDR_RECEIVER, ETH_ALEN, &mMAC[0]); 285 286 nla_put(msg.get(), HWSIM_ATTR_FRAME, size, data); 287 nla_put_u32(msg.get(), HWSIM_ATTR_RX_RATE, 1); 288 nla_put_u32(msg.get(), HWSIM_ATTR_SIGNAL, kSignalLevelDefault); 289 290 LOG(VERBOSE) << "INJECTING!"; 291 292 int res = nl_send_auto_complete(mSock.get(), msg.get()); 293 294 if (res < 0) { 295 LOG(ERROR) << "Injection failed. (" << nl_geterror(res) << ")"; 296 } else { 297 LOG(VERBOSE) << "Injection SUCCEEDED."; 298 } 299 } 300 301 void Mac80211HwSim::handlePacket() { 302 sockaddr_nl from; 303 uint8_t *data; 304 305 int len = nl_recv(mSock.get(), &from, &data, nullptr /* creds */); 306 if (len == 0) { 307 LOG(ERROR) << "nl_recv received EOF."; 308 return; 309 } else if (len < 0) { 310 LOG(ERROR) << "nl_recv failed (" << nl_geterror(len) << ")"; 311 return; 312 } 313 314 std::unique_ptr<nlmsghdr, void (*)(nlmsghdr *)> msg( 315 reinterpret_cast<nlmsghdr *>(data), 316 [](nlmsghdr *hdr) { free(hdr); }); 317 318 if (msg->nlmsg_type != mMac80211Family) { 319 LOG(VERBOSE) 320 << "Received msg of type other than MAC80211: " 321 << msg->nlmsg_type; 322 323 return; 324 } 325 326 #ifdef CUTTLEFISH_HOST 327 LOG(VERBOSE) << "------------------- Host -> Guest -----------------------"; 328 #else 329 LOG(VERBOSE) << "------------------- Guest -> Host -----------------------"; 330 #endif 331 332 genlmsghdr *hdr = genlmsg_hdr(msg.get()); 333 if (hdr->cmd != HWSIM_CMD_FRAME) { 334 LOG(VERBOSE) << "cmd HWSIM_CMD_FRAME."; 335 return; 336 } 337 338 nlattr *attrs[__HWSIM_ATTR_MAX + 1]; 339 int res = genlmsg_parse( 340 msg.get(), 341 0 /* hdrlen */, 342 attrs, 343 __HWSIM_ATTR_MAX, 344 nullptr /* policy */); 345 346 if (res < 0) { 347 LOG(ERROR) << "genlmsg_parse failed."; 348 return; 349 } 350 351 nlattr *attr = attrs[HWSIM_ATTR_FRAME]; 352 if (!attr) { 353 LOG(ERROR) << "no HWSIM_ATTR_FRAME."; 354 return; 355 } 356 std::lock_guard<std::mutex> autoLock(mRemotesLock); 357 for (auto &remoteEntry : mRemotes) { 358 // TODO(andih): Check which remotes to forward this packet to based 359 // on the destination address. 360 remoteEntry.second->send(nla_data(attr), nla_len(attr)); 361 } 362 363 #if !defined(CUTTLEFISH_HOST) 364 ackFrame(msg.get()); 365 #endif 366 367 } 368 369 int Mac80211HwSim::registerOrSubscribe(const MacAddress &mac) { 370 std::unique_ptr<nl_msg, void (*)(nl_msg *)> msg(nullptr, nlmsg_free); 371 372 msg.reset(nlmsg_alloc()); 373 374 genlmsg_put( 375 msg.get(), 376 NL_AUTO_PID, 377 NL_AUTO_SEQ, 378 mMac80211Family, 379 0, 380 NLM_F_REQUEST, 381 #ifdef CUTTLEFISH_HOST 382 HWSIM_CMD_SUBSCRIBE, 383 #else 384 HWSIM_CMD_REGISTER, 385 #endif 386 0); 387 388 #ifdef CUTTLEFISH_HOST 389 nla_put(msg.get(), HWSIM_ATTR_ADDR_RECEIVER, ETH_ALEN, &mac[0]); 390 #else 391 // HWSIM_CMD_REGISTER is a global command not specific to a MAC. 392 (void)mac; 393 #endif 394 395 int res = nl_send_auto_complete(mSock.get(), msg.get()); 396 397 if (res < 0) { 398 LOG(ERROR) 399 << "Registration/subscription failed. (" << nl_geterror(res) << ")"; 400 401 return res; 402 } 403 404 return 0; 405 } 406 407 int Mac80211HwSim::addRemote( 408 const MacAddress &mac, 409 vsoc::wifi::WifiExchangeView *wifiExchange) { 410 #ifdef CUTTLEFISH_HOST 411 int res = registerOrSubscribe(mac); 412 413 if (res < 0) { 414 return res; 415 } 416 #endif 417 418 std::lock_guard<std::mutex> autoLock(mRemotesLock); 419 420 std::unique_ptr<Remote> remote(new Remote(this, wifiExchange)); 421 mRemotes.insert(std::make_pair(mac, std::move(remote))); 422 423 return 0; 424 } 425 426 void Mac80211HwSim::removeRemote(const MacAddress &mac) { 427 std::lock_guard<std::mutex> autoLock(mRemotesLock); 428 auto it = mRemotes.find(mac); 429 if (it != mRemotes.end()) { 430 mRemotes.erase(it); 431 } 432 } 433
