| 1 |
/* |
| 2 |
* hostapd / IEEE 802 OUI Extended EtherType 88-B7 |
| 3 |
* Copyright (c) 2016, Jouni Malinen <[email protected]> |
| 4 |
* |
| 5 |
* This software may be distributed under the terms of the BSD license. |
| 6 |
* See README for more details. |
| 7 |
*/ |
| 8 |
|
| 9 |
#include "utils/includes.h" |
| 10 |
|
| 11 |
#include "utils/common.h" |
| 12 |
#include "utils/eloop.h" |
| 13 |
#include "l2_packet/l2_packet.h" |
| 14 |
#include "hostapd.h" |
| 15 |
#include "eth_p_oui.h" |
| 16 |
|
| 17 |
/* |
| 18 |
* See IEEE Std 802-2014, Clause 9.2.4 for the definition of the OUI Extended |
| 19 |
* EtherType 88-B7. This file implements this with OUI 00:13:74 and |
| 20 |
* vendor-specific subtype 0x0001. |
| 21 |
*/ |
| 22 |
static const u8 global_oui[] = { 0x00, 0x13, 0x74, 0x00, 0x01 }; |
| 23 |
|
| 24 |
struct eth_p_oui_iface { |
| 25 |
struct dl_list list; |
| 26 |
char ifname[IFNAMSIZ + 1]; |
| 27 |
struct l2_packet_data *l2; |
| 28 |
struct dl_list receiver; |
| 29 |
}; |
| 30 |
|
| 31 |
struct eth_p_oui_ctx { |
| 32 |
struct dl_list list; |
| 33 |
struct eth_p_oui_iface *iface; |
| 34 |
/* all data needed to deliver and unregister */ |
| 35 |
u8 oui_suffix; /* last byte of OUI */ |
| 36 |
void (*rx_callback)(void *ctx, const u8 *src_addr, |
| 37 |
const u8 *dst_addr, u8 oui_suffix, |
| 38 |
const u8 *buf, size_t len); |
| 39 |
void *rx_callback_ctx; |
| 40 |
}; |
| 41 |
|
| 42 |
|
| 43 |
void eth_p_oui_deliver(struct eth_p_oui_ctx *ctx, const u8 *src_addr, |
| 44 |
const u8 *dst_addr, const u8 *buf, size_t len) |
| 45 |
{ |
| 46 |
ctx->rx_callback(ctx->rx_callback_ctx, src_addr, dst_addr, |
| 47 |
ctx->oui_suffix, buf, len); |
| 48 |
} |
| 49 |
|
| 50 |
|
| 51 |
static void eth_p_rx(void *ctx, const u8 *src_addr, const u8 *buf, size_t len) |
| 52 |
{ |
| 53 |
struct eth_p_oui_iface *iface = ctx; |
| 54 |
struct eth_p_oui_ctx *receiver; |
| 55 |
const struct l2_ethhdr *ethhdr; |
| 56 |
|
| 57 |
if (len < sizeof(*ethhdr) + sizeof(global_oui) + 1) { |
| 58 |
/* too short packet */ |
| 59 |
return; |
| 60 |
} |
| 61 |
|
| 62 |
ethhdr = (struct l2_ethhdr *) buf; |
| 63 |
/* trim eth_hdr from buf and len */ |
| 64 |
buf += sizeof(*ethhdr); |
| 65 |
len -= sizeof(*ethhdr); |
| 66 |
|
| 67 |
/* verify OUI and vendor-specific subtype match */ |
| 68 |
if (os_memcmp(buf, global_oui, sizeof(global_oui)) != 0) |
| 69 |
return; |
| 70 |
buf += sizeof(global_oui); |
| 71 |
len -= sizeof(global_oui); |
| 72 |
|
| 73 |
dl_list_for_each(receiver, &iface->receiver, |
| 74 |
struct eth_p_oui_ctx, list) { |
| 75 |
if (buf[0] != receiver->oui_suffix) |
| 76 |
continue; |
| 77 |
|
| 78 |
eth_p_oui_deliver(receiver, ethhdr->h_source, ethhdr->h_dest, |
| 79 |
buf + 1, len - 1); |
| 80 |
} |
| 81 |
} |
| 82 |
|
| 83 |
|
| 84 |
struct eth_p_oui_ctx * |
| 85 |
eth_p_oui_register(struct hostapd_data *hapd, const char *ifname, u8 oui_suffix, |
| 86 |
void (*rx_callback)(void *ctx, const u8 *src_addr, |
| 87 |
const u8 *dst_addr, u8 oui_suffix, |
| 88 |
const u8 *buf, size_t len), |
| 89 |
void *rx_callback_ctx) |
| 90 |
{ |
| 91 |
struct eth_p_oui_iface *iface; |
| 92 |
struct eth_p_oui_ctx *receiver; |
| 93 |
int found = 0; |
| 94 |
struct hapd_interfaces *interfaces; |
| 95 |
|
| 96 |
receiver = os_zalloc(sizeof(*receiver)); |
| 97 |
if (!receiver) |
| 98 |
goto err; |
| 99 |
|
| 100 |
receiver->oui_suffix = oui_suffix; |
| 101 |
receiver->rx_callback = rx_callback; |
| 102 |
receiver->rx_callback_ctx = rx_callback_ctx; |
| 103 |
|
| 104 |
interfaces = hapd->iface->interfaces; |
| 105 |
|
| 106 |
dl_list_for_each(iface, &interfaces->eth_p_oui, struct eth_p_oui_iface, |
| 107 |
list) { |
| 108 |
if (os_strcmp(iface->ifname, ifname) != 0) |
| 109 |
continue; |
| 110 |
found = 1; |
| 111 |
break; |
| 112 |
} |
| 113 |
|
| 114 |
if (!found) { |
| 115 |
iface = os_zalloc(sizeof(*iface)); |
| 116 |
if (!iface) |
| 117 |
goto err; |
| 118 |
|
| 119 |
os_strlcpy(iface->ifname, ifname, sizeof(iface->ifname)); |
| 120 |
iface->l2 = l2_packet_init(ifname, NULL, ETH_P_OUI, eth_p_rx, |
| 121 |
iface, 1); |
| 122 |
if (!iface->l2) { |
| 123 |
os_free(iface); |
| 124 |
goto err; |
| 125 |
} |
| 126 |
dl_list_init(&iface->receiver); |
| 127 |
|
| 128 |
dl_list_add_tail(&interfaces->eth_p_oui, &iface->list); |
| 129 |
} |
| 130 |
|
| 131 |
dl_list_add_tail(&iface->receiver, &receiver->list); |
| 132 |
receiver->iface = iface; |
| 133 |
|
| 134 |
return receiver; |
| 135 |
err: |
| 136 |
os_free(receiver); |
| 137 |
return NULL; |
| 138 |
} |
| 139 |
|
| 140 |
|
| 141 |
void eth_p_oui_unregister(struct eth_p_oui_ctx *ctx) |
| 142 |
{ |
| 143 |
struct eth_p_oui_iface *iface; |
| 144 |
|
| 145 |
if (!ctx) |
| 146 |
return; |
| 147 |
|
| 148 |
iface = ctx->iface; |
| 149 |
|
| 150 |
dl_list_del(&ctx->list); |
| 151 |
os_free(ctx); |
| 152 |
|
| 153 |
if (dl_list_empty(&iface->receiver)) { |
| 154 |
dl_list_del(&iface->list); |
| 155 |
l2_packet_deinit(iface->l2); |
| 156 |
os_free(iface); |
| 157 |
} |
| 158 |
} |
| 159 |
|
| 160 |
|
| 161 |
int eth_p_oui_send(struct eth_p_oui_ctx *ctx, const u8 *src_addr, |
| 162 |
const u8 *dst_addr, const u8 *buf, size_t len) |
| 163 |
{ |
| 164 |
struct eth_p_oui_iface *iface = ctx->iface; |
| 165 |
u8 *packet, *p; |
| 166 |
size_t packet_len; |
| 167 |
int ret; |
| 168 |
struct l2_ethhdr *ethhdr; |
| 169 |
|
| 170 |
packet_len = sizeof(*ethhdr) + sizeof(global_oui) + 1 + len; |
| 171 |
packet = os_zalloc(packet_len); |
| 172 |
if (!packet) |
| 173 |
return -1; |
| 174 |
p = packet; |
| 175 |
|
| 176 |
ethhdr = (struct l2_ethhdr *) packet; |
| 177 |
os_memcpy(ethhdr->h_source, src_addr, ETH_ALEN); |
| 178 |
os_memcpy(ethhdr->h_dest, dst_addr, ETH_ALEN); |
| 179 |
ethhdr->h_proto = host_to_be16(ETH_P_OUI); |
| 180 |
p += sizeof(*ethhdr); |
| 181 |
|
| 182 |
os_memcpy(p, global_oui, sizeof(global_oui)); |
| 183 |
p[sizeof(global_oui)] = ctx->oui_suffix; |
| 184 |
p += sizeof(global_oui) + 1; |
| 185 |
|
| 186 |
os_memcpy(p, buf, len); |
| 187 |
|
| 188 |
ret = l2_packet_send(iface->l2, NULL, 0, packet, packet_len); |
| 189 |
os_free(packet); |
| 190 |
return ret; |
| 191 |
} |