Clone of wifirouter from partner repo
Change-Id: Ic41fde03d9acc4a03d0273a01019001d15ece7ae
Merged-In: Ic41fde03d9acc4a03d0273a01019001d15ece7ae
(cherry picked from commit a881de5c05830abfeaec7499329286b6027bf259)
diff --git a/common/libs/wifi/router.h b/common/libs/wifi/router.h
index 364de94..237417d 100644
--- a/common/libs/wifi/router.h
+++ b/common/libs/wifi/router.h
@@ -21,15 +21,22 @@
// WIFIROUTER_CMD_REGISTER is used by client to request notifications for
// packets sent from an interface with specific MAC address. Recognized
// attributes:
- // - WIFIROUTER_ATTR_MAC - MAC address (byte array) of interface to receive
- // notifications for.
+ // - WIFIROUTER_ATTR_HWSIM_ID - ID of HWSIM card to receive notifications for,
+ // - WIFIROUTER_ATTR_HWSIM_ADDR - MAC address (byte array) of interface to
+ // receive notifications for.
WIFIROUTER_CMD_REGISTER,
// WIFIROUTER_CMD_NOTIFY is issued by the server to notify clients for every
// new WIFIROUTER packet the client is registered for. Comes with attributes:
- // - WIFIROUTER_ATTR_MAC - MAC address of interface that received packet,
+ // - WIFIROUTER_ATTR_HWSIM_ID - MAC address of interface that received packet,
// - WIFIROUTER_ATTR_PACKET - content of the MAC80211_HWSIM packet.
WIFIROUTER_CMD_NOTIFY,
+
+ // WIFIROUTER_RMD_SEND is issued by the client to request injection of a
+ // packet to all interfaces the client is registered for. Comes with
+ // attributes:
+ // - WIFIROUTER_ATTR_PACKET - content of the MAC80211_HWSIM packet.
+ WIFIROUTER_CMD_SEND,
};
// Attributes recognized by WIFIRouter netlink family.
@@ -40,6 +47,9 @@
// MAC address representing interface from which the packet originated.
WIFIROUTER_ATTR_HWSIM_ID,
+ // Physical address of wireless interface.
+ WIFIROUTER_ATTR_HWSIM_ADDR,
+
// MAC80211_HWSIM packet content.
WIFIROUTER_ATTR_PACKET,
diff --git a/guest/commands/wifirouter/router.cc b/guest/commands/wifirouter/router.cc
index a143ffb..47727f3 100644
--- a/guest/commands/wifirouter/router.cc
+++ b/guest/commands/wifirouter/router.cc
@@ -17,12 +17,15 @@
#include <cerrno>
#include <cstddef>
+#include <iomanip>
#include <map>
#include <memory>
#include <set>
#include <gflags/gflags.h>
#include <glog/logging.h>
+#include <netinet/in.h>
+#include <linux/netdevice.h>
#include <netlink/genl/ctrl.h>
#include <netlink/genl/family.h>
#include <netlink/genl/genl.h>
@@ -34,23 +37,87 @@
DEFINE_string(socket_name, "cvd-wifirouter",
"Name of the unix-domain socket providing access for routing. "
"Socket will be created in abstract namespace.");
+DEFINE_bool(use_fixed_addresses, false,
+ "Specify to use hard-coded WIFI addresses issued by MAC80211 HWSIM."
+ " This is relevant for systems, where mac address update is not"
+ " reflected in mac80211_hwsim module.");
+DEFINE_bool(log_broadcast_frames, false, "Specify to log broadcast frames.");
namespace cvd {
-namespace {
// Copied out of mac80211_hwsim.h header.
constexpr int HWSIM_CMD_REGISTER = 1;
+constexpr int HWSIM_CMD_FRAME = 2;
+constexpr int HWSIM_CMD_TX_INFO_FRAME = 3;
+
+constexpr int HWSIM_TX_CTL_REQ_TX_STATUS = 1;
+constexpr int HWSIM_TX_CTL_NO_ACK = 2;
+constexpr int HWSIM_TX_STAT_ACK = 4;
+
+constexpr int HWSIM_ATTR_ADDR_RECEIVER = 1;
constexpr int HWSIM_ATTR_ADDR_TRANSMITTER = 2;
+constexpr int HWSIM_ATTR_FRAME = 3;
+constexpr int HWSIM_ATTR_FLAGS = 4;
+constexpr int HWSIM_ATTR_RX_RATE = 5;
+constexpr int HWSIM_ATTR_SIGNAL = 6;
+constexpr int HWSIM_ATTR_TX_INFO = 7;
+constexpr int HWSIM_ATTR_COOKIE = 8;
constexpr int HWSIM_ATTR_MAX = 19;
// Name of the WIFI SIM Netlink Family.
constexpr char kWifiSimFamilyName[] = "MAC80211_HWSIM";
const int kMaxSupportedPacketSize = getpagesize();
+constexpr int kDefaultSignalLevel = -24;
+
+using MACAddress = uint8_t[6];
+
+struct IEEE80211Hdr {
+ uint16_t frame_control;
+ uint16_t duration_id;
+ MACAddress destination;
+ MACAddress source;
+ MACAddress bssid;
+ uint16_t seq;
+
+ bool IsBroadcast() const;
+} __attribute__((packed));
+
+std::ostream& operator<<(std::ostream& out, const MACAddress& addr) {
+ out << std::hex
+ << std::setfill('0') << std::setw(2) << int(addr[0]) << ':'
+ << std::setfill('0') << std::setw(2) << int(addr[1]) << ':'
+ << std::setfill('0') << std::setw(2) << int(addr[2]) << ':'
+ << std::setfill('0') << std::setw(2) << int(addr[3]) << ':'
+ << std::setfill('0') << std::setw(2) << int(addr[4]) << ':'
+ << std::setfill('0') << std::setw(2) << int(addr[5]) << std::dec;
+
+ return out;
+}
+
+std::ostream& operator<<(std::ostream& out, const IEEE80211Hdr& frm) {
+ out << "IEEE80211Hdr{ Type=" << std::hex << std::setw(4) << std::setfill('0')
+ << frm.frame_control << std::dec
+ << " From=" << frm.source
+ << " To=" << frm.destination << " Via=" << frm.bssid << " }";
+ return out;
+}
+
+bool IEEE80211Hdr::IsBroadcast() const {
+ return (destination[0] & destination[1] & destination[2] & destination[3] &
+ destination[4] & destination[5]) == 0xff;
+}
+
class WifiRouter {
public:
- using MacHash = uint16_t;
- using MacToClientsTable = std::multimap<MacHash, int>;
- using ClientsTable = std::set<int>;
+ using RadioID = int32_t;
+ using Radio = struct {
+ RadioID id;
+ uint8_t mac[ETH_ALEN];
+ };
+ using RadioToClientsTable = std::multimap<RadioID, int>;
+ using ClientToRadiosTable = std::multimap<int, Radio>;
+ using MacAddrToRadioIDTable = std::map<uint64_t, RadioID>;
+ const RadioID RadioID_Invalid = -1;
WifiRouter() : sock_(nullptr, nl_socket_free) {}
~WifiRouter() = default;
@@ -59,7 +126,8 @@
void ServerLoop();
private:
- MacHash GetMacHash(const void* macaddr);
+ void AddRadioID(int client, RadioID radio_id, const void* macaddr);
+ RadioID GetRadioID(const void* macaddr);
void CreateWifiRouterServerSocket();
void RegisterForHWSimNotifications();
@@ -72,17 +140,56 @@
std::unique_ptr<nl_sock, void (*)(nl_sock*)> sock_;
int server_fd_ = 0;
int mac80211_family_ = 0;
- ClientsTable registered_clients_;
- MacToClientsTable registered_addresses_;
+ ClientToRadiosTable registered_clients_;
+ RadioToClientsTable registered_addresses_;
+ MacAddrToRadioIDTable known_addresses_;
};
-WifiRouter::MacHash WifiRouter::GetMacHash(const void* macaddr) {
- const uint8_t* t = reinterpret_cast<const uint8_t*>(macaddr);
+void WifiRouter::AddRadioID(int client, RadioID radio_id, const void* macaddr) {
+ const uint8_t* addr_bytes = reinterpret_cast<const uint8_t*>(macaddr);
+ uint64_t mac;
+ Radio r{radio_id, {}};
- // This is guaranteed to be unique. Address here is assigned at creation time
- // and is (well) non-mutable. This is a unique ID of the MAC80211 HWSIM
- // interface.
- return t[3] << 8 | t[4];
+ mac = (addr_bytes[0] << 24) | (addr_bytes[1] << 16) | (addr_bytes[2] >> 8) |
+ addr_bytes[3];
+ mac <<= 16;
+ mac |= (addr_bytes[4] << 8) | addr_bytes[5];
+
+ known_addresses_[mac] = radio_id;
+ // Add two MAC addresses registered internally by MAC80211_HWSIM.
+ mac = 0x020000000000ull;
+ mac |= (radio_id << 8);
+ known_addresses_[mac] = radio_id;
+
+ mac |= 0x400000000000ull;
+ known_addresses_[mac] = radio_id;
+
+ if (FLAGS_use_fixed_addresses) {
+ r.mac[0] = mac >> 40;
+ r.mac[1] = mac >> 32;
+ r.mac[2] = mac >> 24;
+ r.mac[3] = mac >> 16;
+ r.mac[4] = mac >> 8;
+ r.mac[5] = mac;
+ } else {
+ memcpy(r.mac, macaddr, ETH_ALEN);
+ }
+ registered_addresses_.emplace(radio_id, client);
+ registered_clients_.emplace(client, r);
+}
+
+WifiRouter::RadioID WifiRouter::GetRadioID(const void* macaddr) {
+ const uint8_t* addr_bytes = reinterpret_cast<const uint8_t*>(macaddr);
+ uint64_t mac;
+
+ mac = (addr_bytes[0] << 24) | (addr_bytes[1] << 16) | (addr_bytes[2] >> 8) |
+ addr_bytes[3];
+ mac <<= 16;
+ mac |= (addr_bytes[4] << 8) | addr_bytes[5];
+
+ auto iter = known_addresses_.find(mac);
+ if (iter == known_addresses_.end()) return RadioID_Invalid;
+ return iter->second;
}
void WifiRouter::Init() {
@@ -98,6 +205,11 @@
void WifiRouter::RegisterForHWSimNotifications() {
sock_.reset(nl_socket_alloc());
+ // Disable sequence number checks. Occasional "Message sequence number
+ // mismatch" errors were observed, despite netlink allocating sequence numbers
+ // itself.
+ nl_socket_disable_seq_check(sock_.get());
+
auto res = nl_connect(sock_.get(), NETLINK_GENERIC);
if (res < 0) {
LOG(ERROR) << "Could not connect to netlink generic: " << nl_geterror(res);
@@ -158,7 +270,7 @@
return;
}
- registered_clients_.insert(client);
+ registered_clients_.insert({client, {RadioID_Invalid, {}}});
LOG(INFO) << "Client " << client << " added.";
}
@@ -166,6 +278,7 @@
// of WLAN traffic.
void WifiRouter::RemoveClient(int client) {
close(client);
+
registered_clients_.erase(client);
for (auto iter = registered_addresses_.begin();
@@ -197,6 +310,12 @@
// Discard messages that originate from anything else than MAC80211_HWSIM.
if (msg->nlmsg_type != mac80211_family_) return;
+ genlmsghdr* gmsg = reinterpret_cast<genlmsghdr*>(nlmsg_data(msg.get()));
+ if (gmsg->cmd != HWSIM_CMD_FRAME) {
+ LOG(INFO) << "Discarding non-FRAME message.";
+ return;
+ }
+
std::unique_ptr<nl_msg, void (*)(nl_msg*)> rep(
nlmsg_alloc(), [](nl_msg* m) { nlmsg_free(m); });
genlmsg_put(rep.get(), 0, 0, 0, 0, 0, WIFIROUTER_CMD_NOTIFY, 0);
@@ -206,15 +325,20 @@
nlattr* attrs[HWSIM_ATTR_MAX + 1];
if (genlmsg_parse(msg.get(), 0, attrs, HWSIM_ATTR_MAX, nullptr)) return;
+ auto ieee80211hdr = reinterpret_cast<IEEE80211Hdr*>(nla_data(attrs[HWSIM_ATTR_FRAME]));
+ if (!ieee80211hdr->IsBroadcast() || FLAGS_log_broadcast_frames) {
+ LOG(INFO) << "SND " << *ieee80211hdr;
+ }
+
std::set<int> pending_removals;
auto addr = attrs[HWSIM_ATTR_ADDR_TRANSMITTER];
if (addr != nullptr) {
- nla_put_u32(rep.get(), WIFIROUTER_ATTR_HWSIM_ID, GetMacHash(nla_data(addr)));
+ nla_put_u32(rep.get(), WIFIROUTER_ATTR_HWSIM_ID,
+ GetRadioID(nla_data(addr)));
nla_put(rep.get(), WIFIROUTER_ATTR_PACKET, len, buf);
auto hdr = nlmsg_hdr(rep.get());
- auto key = GetMacHash(nla_data(attrs[HWSIM_ATTR_ADDR_TRANSMITTER]));
- LOG(INFO) << "Received netlink packet from " << std::hex << key;
+ auto key = GetRadioID(nla_data(attrs[HWSIM_ATTR_ADDR_TRANSMITTER]));
for (auto it = registered_addresses_.find(key);
it != registered_addresses_.end() && it->first == key; ++it) {
auto num_written = send(it->second, hdr, hdr->nlmsg_len, MSG_NOSIGNAL);
@@ -241,23 +365,72 @@
int result = -EINVAL;
genlmsghdr* ghdr = reinterpret_cast<genlmsghdr*>(nlmsg_data(msg.get()));
+ nlattr* attrs[WIFIROUTER_ATTR_MAX];
+ if (nlmsg_parse(msg.get(), sizeof(genlmsghdr), attrs, WIFIROUTER_ATTR_MAX - 1,
+ nullptr))
+ return false;
+
switch (ghdr->cmd) {
case WIFIROUTER_CMD_REGISTER:
- // Register client to receive notifications for specified MAC address.
- nlattr* attrs[WIFIROUTER_ATTR_MAX];
- if (!nlmsg_parse(msg.get(), sizeof(genlmsghdr), attrs,
- WIFIROUTER_ATTR_MAX - 1, nullptr)) {
- if (attrs[WIFIROUTER_ATTR_HWSIM_ID] != nullptr) {
- LOG(INFO) << "Registering new client to receive data for "
- << nla_get_u32(attrs[WIFIROUTER_ATTR_HWSIM_ID]);
- registered_addresses_.emplace(
- nla_get_u32(attrs[WIFIROUTER_ATTR_HWSIM_ID]), client);
- // This is unfortunate, but it is a bug in mac80211_hwsim stack.
- // Apparently, the imperfect medium will not receive notifications for
- // newly created wifi interfaces. How about that...
- RegisterForHWSimNotifications();
- result = 0;
+ if (attrs[WIFIROUTER_ATTR_HWSIM_ID] != nullptr) {
+ int simid = nla_get_u32(attrs[WIFIROUTER_ATTR_HWSIM_ID]);
+ uint8_t* simaddr = reinterpret_cast<uint8_t*>(
+ nla_data(attrs[WIFIROUTER_ATTR_HWSIM_ADDR]));
+
+ AddRadioID(client, simid, simaddr);
+ // This is unfortunate, but it is a bug in mac80211_hwsim stack.
+ // Apparently, the imperfect medium will not receive notifications for
+ // newly created wifi interfaces. How about that...
+ RegisterForHWSimNotifications();
+ result = 0;
+ }
+ break;
+
+ case WIFIROUTER_CMD_SEND:
+ if (attrs[WIFIROUTER_ATTR_PACKET] != nullptr) {
+ std::unique_ptr<nl_msg, void (*)(nl_msg*)> frame(
+ nlmsg_convert(reinterpret_cast<nlmsghdr*>(
+ nla_data(attrs[WIFIROUTER_ATTR_PACKET]))),
+ nlmsg_free);
+
+ // Netlink is not smart enough to re-alloc.
+ nlmsg_expand(frame.get(), nlmsg_get_max_size(frame.get()) + 64);
+
+ auto hdr = nlmsg_hdr(frame.get());
+ hdr->nlmsg_type = mac80211_family_;
+ hdr->nlmsg_flags = NLM_F_REQUEST;
+
+ auto pktdata = nlmsg_find_attr(nlmsg_hdr(frame.get()),
+ sizeof(genlmsghdr),
+ HWSIM_ATTR_FRAME);
+ auto ieee80211hdr = reinterpret_cast<IEEE80211Hdr*>(nla_data(pktdata));
+ if (!ieee80211hdr->IsBroadcast() || FLAGS_log_broadcast_frames) {
+ LOG(INFO) << "RCV " << *ieee80211hdr;
}
+
+ auto receiver =
+ nla_reserve(frame.get(), HWSIM_ATTR_ADDR_RECEIVER, ETH_ALEN);
+ if (nla_put_u32(frame.get(), HWSIM_ATTR_RX_RATE, 1) ||
+ nla_put_u32(frame.get(), HWSIM_ATTR_SIGNAL, kDefaultSignalLevel) ||
+ !receiver) {
+ LOG(ERROR) << "Could not add netlink attribute: buffer too short.";
+ } else {
+ uint8_t* macaddr = reinterpret_cast<uint8_t*>(nla_data(receiver));
+ for (auto iter = registered_clients_.find(client);
+ iter->first == client; ++iter) {
+ if (iter->second.id == RadioID_Invalid) continue;
+ memcpy(macaddr, iter->second.mac, ETH_ALEN);
+ hdr->nlmsg_seq = NL_AUTO_SEQ;
+ hdr->nlmsg_pid = NL_AUTO_PID;
+ nl_send_auto(sock_.get(), frame.get());
+ auto res = nl_wait_for_ack(sock_.get());
+ if (res) {
+ LOG(INFO) << "Packet send from " << client << " to "
+ << iter->second.id << " result: " << nl_geterror(-res);
+ }
+ }
+ }
+ result = 0;
}
break;
@@ -290,27 +463,29 @@
fdset(server_fd_);
fdset(nl_socket_get_fd(sock_.get()));
- for (int client : registered_clients_) fdset(client);
+ for (const auto& client : registered_clients_) fdset(client.first);
if (select(max_fd + 1, &reads, nullptr, nullptr, nullptr) <= 0) continue;
if (FD_ISSET(server_fd_, &reads)) AcceptNewClient();
if (FD_ISSET(nl_socket_get_fd(sock_.get()), &reads)) RouteWIFIPacket();
+ std::set<int> rogue_clients;
// Process any client messages left. Drop any client that is no longer
// talking with us.
- for (auto client = registered_clients_.begin();
- client != registered_clients_.end();) {
- auto cfd = *client++;
+ for (auto cfd : registered_clients_) {
// Is our client sending us data?
- if (FD_ISSET(cfd, &reads)) {
- if (!HandleClientMessage(cfd)) RemoveClient(cfd);
+ if (FD_ISSET(cfd.first, &reads)) {
+ if (!HandleClientMessage(cfd.first)) rogue_clients.insert(cfd.first);
+ // Note: we iterate over multimap.
+ FD_CLR(cfd.first, &reads);
}
}
+
+ for (auto client : rogue_clients) RemoveClient(client);
}
}
-} // namespace
} // namespace cvd
int main(int argc, char* argv[]) {
