hwsim: Add wmedium implementation
Implement a hwsim wireless medium inside hwsim.c. This doesn't do anything to the frames it moves around yet, only tries to implement the same logic that the kernel medium contains.
This commit is contained in:
Andrew Zaborowski
Denis Kenzior
parent
6280a6c72d
commit
7460b6e1a7
370
tools/hwsim.c
370
tools/hwsim.c
|
|
@ -35,6 +35,7 @@
|
|||
|
||||
#include "src/util.h"
|
||||
#include "src/storage.h"
|
||||
#include "src/mpdu.h"
|
||||
|
||||
#define HWSIM_SERVICE "net.connman.iwd.hwsim"
|
||||
|
||||
|
|
@ -79,6 +80,14 @@ enum {
|
|||
};
|
||||
#define HWSIM_ATTR_MAX (__HWSIM_ATTR_MAX - 1)
|
||||
|
||||
enum hwsim_tx_control_flags {
|
||||
HWSIM_TX_CTL_REQ_TX_STATUS = 1 << 0,
|
||||
HWSIM_TX_CTL_NO_ACK = 1 << 1,
|
||||
HWSIM_TX_STAT_ACK = 1 << 2,
|
||||
};
|
||||
|
||||
#define IEEE80211_TX_RATE_TABLE_SIZE 4
|
||||
|
||||
static struct l_genl *genl;
|
||||
static struct l_genl_family *hwsim;
|
||||
static struct l_genl_family *nl80211;
|
||||
|
|
@ -312,6 +321,14 @@ static bool radio_info_match_wiphy_id(const void *a, const void *b)
|
|||
return rec->wiphy_id == id;
|
||||
}
|
||||
|
||||
static bool radio_info_match_addr1(const void *a, const void *b)
|
||||
{
|
||||
const struct radio_info_rec *rec = a;
|
||||
const uint8_t *addr1 = b;
|
||||
|
||||
return !memcmp(rec->addrs[1], addr1, ETH_ALEN);
|
||||
}
|
||||
|
||||
static bool interface_info_match_id(const void *a, const void *b)
|
||||
{
|
||||
const struct interface_info_rec *rec = a;
|
||||
|
|
@ -899,6 +916,327 @@ static void nl80211_config_notify(struct l_genl_msg *msg, void *user_data)
|
|||
}
|
||||
}
|
||||
|
||||
static bool is_multicast_addr(const uint8_t *addr)
|
||||
{
|
||||
return util_is_bit_set(addr[0], 7);
|
||||
}
|
||||
|
||||
struct hwsim_tx_info {
|
||||
int8_t idx;
|
||||
uint8_t count;
|
||||
};
|
||||
|
||||
struct hwsim_frame {
|
||||
int refcount;
|
||||
uint8_t src_ether_addr[ETH_ALEN];
|
||||
uint8_t dst_ether_addr[ETH_ALEN];
|
||||
struct radio_info_rec *src_radio;
|
||||
struct radio_info_rec *ack_radio;
|
||||
uint32_t flags;
|
||||
const uint64_t *cookie;
|
||||
int32_t signal;
|
||||
uint32_t frequency;
|
||||
uint16_t tx_info_len;
|
||||
const struct hwsim_tx_info *tx_info;
|
||||
uint16_t payload_len;
|
||||
const uint8_t *payload;
|
||||
bool acked;
|
||||
struct l_genl_msg *msg;
|
||||
int pending_callback_count;
|
||||
};
|
||||
|
||||
struct send_frame_info {
|
||||
struct hwsim_frame *frame;
|
||||
struct radio_info_rec *radio;
|
||||
};
|
||||
|
||||
static bool send_frame_tx_info(struct hwsim_frame *frame)
|
||||
{
|
||||
struct l_genl_msg *msg;
|
||||
|
||||
msg = l_genl_msg_new_sized(HWSIM_CMD_TX_INFO_FRAME,
|
||||
128 + frame->tx_info_len);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_ADDR_TRANSMITTER, ETH_ALEN,
|
||||
frame->src_radio->addrs[1]);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_FLAGS, 4, &frame->flags);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_SIGNAL, 4, &frame->signal);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_COOKIE, 8, frame->cookie);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_TX_INFO, frame->tx_info_len,
|
||||
frame->tx_info);
|
||||
|
||||
if (!l_genl_family_send(hwsim, msg, NULL, NULL, NULL)) {
|
||||
l_error("Sending HWSIM_CMD_TX_INFO_FRAME failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool send_frame(struct send_frame_info *info,
|
||||
l_genl_msg_func_t callback,
|
||||
l_genl_destroy_func_t destroy)
|
||||
{
|
||||
struct l_genl_msg *msg;
|
||||
uint32_t rx_rate = 2;
|
||||
unsigned int id;
|
||||
|
||||
msg = l_genl_msg_new_sized(HWSIM_CMD_FRAME,
|
||||
128 + info->frame->payload_len);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_ADDR_RECEIVER, ETH_ALEN,
|
||||
info->radio->addrs[1]);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_FRAME, info->frame->payload_len,
|
||||
info->frame->payload);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_RX_RATE, 4,
|
||||
&rx_rate);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_SIGNAL, 4,
|
||||
&info->frame->signal);
|
||||
l_genl_msg_append_attr(msg, HWSIM_ATTR_FREQ, 4,
|
||||
&info->frame->frequency);
|
||||
|
||||
id = l_genl_family_send(hwsim, msg, callback, info, destroy);
|
||||
if (!id) {
|
||||
l_error("Sending HWSIM_CMD_FRAME failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static struct hwsim_frame *hwsim_frame_ref(struct hwsim_frame *frame)
|
||||
{
|
||||
__sync_fetch_and_add(&frame->refcount, 1);
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
static void hwsim_frame_unref(struct hwsim_frame *frame)
|
||||
{
|
||||
if (__sync_sub_and_fetch(&frame->refcount, 1))
|
||||
return;
|
||||
|
||||
if (!frame->pending_callback_count) {
|
||||
/*
|
||||
* Apparently done with this frame, send tx info and signal
|
||||
* the returning of an ACK frame in the opposite direction.
|
||||
*/
|
||||
|
||||
if (!(frame->flags & HWSIM_TX_CTL_NO_ACK) && frame->acked)
|
||||
frame->flags |= HWSIM_TX_STAT_ACK;
|
||||
|
||||
send_frame_tx_info(frame);
|
||||
}
|
||||
|
||||
l_genl_msg_unref(frame->msg);
|
||||
l_free(frame);
|
||||
}
|
||||
|
||||
static void send_frame_callback(struct l_genl_msg *msg, void *user_data)
|
||||
{
|
||||
struct send_frame_info *info = user_data;
|
||||
|
||||
if (l_genl_msg_get_error(msg) < 0)
|
||||
/* Radio address or frequency didn't match */
|
||||
l_debug("HWSIM_CMD_FRAME failed for destination %s",
|
||||
util_address_to_string(info->radio->addrs[0]));
|
||||
else {
|
||||
info->frame->acked = true;
|
||||
info->frame->ack_radio = info->radio;
|
||||
}
|
||||
|
||||
info->frame->pending_callback_count--;
|
||||
}
|
||||
|
||||
static void send_frame_destroy(void *user_data)
|
||||
{
|
||||
struct send_frame_info *info = user_data;
|
||||
|
||||
hwsim_frame_unref(info->frame);
|
||||
l_free(info);
|
||||
}
|
||||
|
||||
struct interface_match_data {
|
||||
struct radio_info_rec *radio;
|
||||
const uint8_t *addr;
|
||||
};
|
||||
|
||||
static bool interface_info_match_dst(const void *a, const void *b)
|
||||
{
|
||||
const struct interface_info_rec *rec = a;
|
||||
const struct interface_match_data *dst = b;
|
||||
|
||||
return rec->radio_rec == dst->radio &&
|
||||
!memcmp(rec->addr, dst->addr, ETH_ALEN);
|
||||
}
|
||||
|
||||
/*
|
||||
* Process frames in a similar way to how the kernel built-in hwsim medium
|
||||
* does this, with an additional optimization for unicast frames and
|
||||
* additonal modifications to frames decided by user-configurable rules.
|
||||
*/
|
||||
static void process_frame(struct hwsim_frame *frame)
|
||||
{
|
||||
const struct l_queue_entry *entry;
|
||||
|
||||
for (entry = l_queue_get_entries(radio_info); entry;
|
||||
entry = entry->next) {
|
||||
struct radio_info_rec *radio = entry->data;
|
||||
struct send_frame_info *send_info;
|
||||
|
||||
if (radio == frame->src_radio)
|
||||
continue;
|
||||
|
||||
/*
|
||||
* The kernel hwsim medium passes multicast frames to all
|
||||
* radios that are on the same frequency as this frame but
|
||||
* the netlink medium API only lets userspace pass frames to
|
||||
* radios by known hardware address. It does check that the
|
||||
* receiving radio is on the same frequency though so we can
|
||||
* send to all known addresses.
|
||||
*
|
||||
* If the frame's Receiver Address (RA) is a multicast
|
||||
* address, then send the frame to every radio that is
|
||||
* registered. If it's a unicast address then optimize
|
||||
* by only forwarding the frame to the radios that have
|
||||
* at least one interface with this specific address.
|
||||
*/
|
||||
if (!is_multicast_addr(frame->dst_ether_addr)) {
|
||||
struct interface_match_data match_data = {
|
||||
radio,
|
||||
frame->dst_ether_addr,
|
||||
};
|
||||
struct interface_info_rec *interface =
|
||||
l_queue_find(interface_info,
|
||||
interface_info_match_dst,
|
||||
&match_data);
|
||||
|
||||
if (!interface)
|
||||
continue;
|
||||
}
|
||||
|
||||
send_info = l_new(struct send_frame_info, 1);
|
||||
send_info->radio = radio;
|
||||
send_info->frame = hwsim_frame_ref(frame);
|
||||
|
||||
if (send_frame(send_info, send_frame_callback,
|
||||
send_frame_destroy))
|
||||
frame->pending_callback_count++;
|
||||
else
|
||||
send_frame_destroy(send_info);
|
||||
}
|
||||
|
||||
hwsim_frame_unref(frame);
|
||||
}
|
||||
|
||||
static void unicast_handler(struct l_genl_msg *msg, void *user_data)
|
||||
{
|
||||
struct hwsim_frame *frame;
|
||||
const struct mpdu *mpdu;
|
||||
struct l_genl_attr attr;
|
||||
uint16_t type, len;
|
||||
const void *data;
|
||||
const uint8_t *transmitter = NULL, *freq = NULL, *flags = NULL;
|
||||
|
||||
if (l_genl_msg_get_command(msg) != HWSIM_CMD_FRAME)
|
||||
return;
|
||||
|
||||
if (!l_genl_attr_init(&attr, msg))
|
||||
return;
|
||||
|
||||
frame = l_new(struct hwsim_frame, 1);
|
||||
|
||||
while (l_genl_attr_next(&attr, &type, &len, &data)) {
|
||||
switch (type) {
|
||||
case HWSIM_ATTR_ADDR_TRANSMITTER:
|
||||
if (len != ETH_ALEN)
|
||||
break;
|
||||
|
||||
transmitter = data;
|
||||
break;
|
||||
|
||||
case HWSIM_ATTR_FREQ:
|
||||
if (len != 4)
|
||||
break;
|
||||
|
||||
freq = data;
|
||||
break;
|
||||
|
||||
case HWSIM_ATTR_FLAGS:
|
||||
if (len != 4)
|
||||
break;
|
||||
|
||||
flags = data;
|
||||
break;
|
||||
|
||||
case HWSIM_ATTR_COOKIE:
|
||||
if (len != 8)
|
||||
break;
|
||||
|
||||
frame->cookie = (const uint64_t *) data;
|
||||
break;
|
||||
|
||||
case HWSIM_ATTR_FRAME:
|
||||
if (len > IEEE80211_MAX_DATA_LEN)
|
||||
break;
|
||||
|
||||
if (len < sizeof(struct mpdu_fc) +
|
||||
sizeof(struct mpdu_mgmt_header)) {
|
||||
l_error("Frame payload too short for header");
|
||||
break;
|
||||
}
|
||||
|
||||
frame->payload_len = len;
|
||||
frame->payload = data;
|
||||
|
||||
break;
|
||||
|
||||
case HWSIM_ATTR_TX_INFO:
|
||||
if (len > sizeof(struct hwsim_tx_info) *
|
||||
IEEE80211_TX_RATE_TABLE_SIZE)
|
||||
break;
|
||||
|
||||
frame->tx_info_len = len;
|
||||
frame->tx_info = data;
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
l_warn("Unhandled attribute type: %u", type);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!frame->payload || !frame->tx_info || !frame->cookie ||
|
||||
!flags || !freq || !transmitter) {
|
||||
l_error("Incomplete HWSIM_CMD_FRAME");
|
||||
l_free(frame);
|
||||
return;
|
||||
}
|
||||
|
||||
frame->signal = -30;
|
||||
frame->msg = l_genl_msg_ref(msg);
|
||||
frame->refcount = 1;
|
||||
|
||||
frame->src_radio = l_queue_find(radio_info, radio_info_match_addr1,
|
||||
transmitter);
|
||||
if (!frame->src_radio) {
|
||||
l_error("Unknown transmitter address %s, probably need to "
|
||||
"update radio dump code for this kernel",
|
||||
util_address_to_string(transmitter));
|
||||
hwsim_frame_unref(frame);
|
||||
return;
|
||||
}
|
||||
|
||||
frame->frequency = *(uint32_t *) freq;
|
||||
frame->flags = *(uint32_t *) flags;
|
||||
|
||||
mpdu = (struct mpdu *) frame->payload;
|
||||
|
||||
memcpy(frame->src_ether_addr, mpdu->mgmt_hdr.address_2, ETH_ALEN);
|
||||
memcpy(frame->dst_ether_addr, mpdu->mgmt_hdr.address_1, ETH_ALEN);
|
||||
|
||||
process_frame(frame);
|
||||
}
|
||||
|
||||
static void radio_manager_create_callback(struct l_genl_msg *msg,
|
||||
void *user_data)
|
||||
{
|
||||
|
|
@ -1242,6 +1580,30 @@ static bool setup_dbus_hwsim(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void register_callback(struct l_genl_msg *msg, void *user_data)
|
||||
{
|
||||
int err = l_genl_msg_get_error(msg);
|
||||
|
||||
if (err < 0) {
|
||||
l_error("HWSIM_CMD_REGISTER failed: %s (%d)",
|
||||
strerror(-err), -err);
|
||||
|
||||
exit_status = EXIT_FAILURE;
|
||||
l_main_quit();
|
||||
return;
|
||||
}
|
||||
|
||||
l_info("Registered as a transmission medium");
|
||||
}
|
||||
|
||||
static void get_interface_done_initial(void *user_data)
|
||||
{
|
||||
struct l_genl_msg *msg;
|
||||
|
||||
msg = l_genl_msg_new_sized(HWSIM_CMD_REGISTER, 4);
|
||||
l_genl_family_send(hwsim, msg, register_callback, NULL, NULL);
|
||||
}
|
||||
|
||||
static void get_radio_done_initial(void *user_data)
|
||||
{
|
||||
struct l_genl_msg *msg;
|
||||
|
|
@ -1253,7 +1615,7 @@ static void get_radio_done_initial(void *user_data)
|
|||
*/
|
||||
msg = l_genl_msg_new(NL80211_CMD_GET_INTERFACE);
|
||||
if (!l_genl_family_dump(nl80211, msg, get_interface_callback,
|
||||
NULL, NULL)) {
|
||||
NULL, get_interface_done_initial)) {
|
||||
l_error("Getting nl80211 interface information failed");
|
||||
goto error;
|
||||
}
|
||||
|
|
@ -1380,6 +1742,12 @@ static void hwsim_ready(void *user_data)
|
|||
l_genl_family_set_watches(nl80211, nl80211_ready, NULL,
|
||||
NULL, NULL);
|
||||
|
||||
if (!l_genl_set_unicast_handler(genl, unicast_handler,
|
||||
NULL, NULL)) {
|
||||
l_error("Failed to set unicast handler");
|
||||
goto error;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue