1341618Scy/* 2341618Scy * hostapd / IEEE 802 OUI Extended EtherType 88-B7 3341618Scy * Copyright (c) 2016, Jouni Malinen <j@w1.fi> 4341618Scy * 5341618Scy * This software may be distributed under the terms of the BSD license. 6341618Scy * See README for more details. 7341618Scy */ 8341618Scy 9341618Scy#include "utils/includes.h" 10341618Scy 11341618Scy#include "utils/common.h" 12341618Scy#include "utils/eloop.h" 13341618Scy#include "l2_packet/l2_packet.h" 14341618Scy#include "hostapd.h" 15341618Scy#include "eth_p_oui.h" 16341618Scy 17341618Scy/* 18341618Scy * See IEEE Std 802-2014, Clause 9.2.4 for the definition of the OUI Extended 19341618Scy * EtherType 88-B7. This file implements this with OUI 00:13:74 and 20341618Scy * vendor-specific subtype 0x0001. 21341618Scy */ 22341618Scystatic const u8 global_oui[] = { 0x00, 0x13, 0x74, 0x00, 0x01 }; 23341618Scy 24341618Scystruct eth_p_oui_iface { 25341618Scy struct dl_list list; 26341618Scy char ifname[IFNAMSIZ + 1]; 27341618Scy struct l2_packet_data *l2; 28341618Scy struct dl_list receiver; 29341618Scy}; 30341618Scy 31341618Scystruct eth_p_oui_ctx { 32341618Scy struct dl_list list; 33341618Scy struct eth_p_oui_iface *iface; 34341618Scy /* all data needed to deliver and unregister */ 35341618Scy u8 oui_suffix; /* last byte of OUI */ 36341618Scy void (*rx_callback)(void *ctx, const u8 *src_addr, 37341618Scy const u8 *dst_addr, u8 oui_suffix, 38341618Scy const u8 *buf, size_t len); 39341618Scy void *rx_callback_ctx; 40341618Scy}; 41341618Scy 42341618Scy 43341618Scyvoid eth_p_oui_deliver(struct eth_p_oui_ctx *ctx, const u8 *src_addr, 44341618Scy const u8 *dst_addr, const u8 *buf, size_t len) 45341618Scy{ 46341618Scy ctx->rx_callback(ctx->rx_callback_ctx, src_addr, dst_addr, 47341618Scy ctx->oui_suffix, buf, len); 48341618Scy} 49341618Scy 50341618Scy 51341618Scystatic void eth_p_rx(void *ctx, const u8 *src_addr, const u8 *buf, size_t len) 52341618Scy{ 53341618Scy struct eth_p_oui_iface *iface = ctx; 54341618Scy struct eth_p_oui_ctx *receiver; 55341618Scy const struct l2_ethhdr *ethhdr; 56341618Scy 57341618Scy if (len < sizeof(*ethhdr) + sizeof(global_oui) + 1) { 58341618Scy /* too short packet */ 59341618Scy return; 60341618Scy } 61341618Scy 62341618Scy ethhdr = (struct l2_ethhdr *) buf; 63341618Scy /* trim eth_hdr from buf and len */ 64341618Scy buf += sizeof(*ethhdr); 65341618Scy len -= sizeof(*ethhdr); 66341618Scy 67341618Scy /* verify OUI and vendor-specific subtype match */ 68341618Scy if (os_memcmp(buf, global_oui, sizeof(global_oui)) != 0) 69341618Scy return; 70341618Scy buf += sizeof(global_oui); 71341618Scy len -= sizeof(global_oui); 72341618Scy 73341618Scy dl_list_for_each(receiver, &iface->receiver, 74341618Scy struct eth_p_oui_ctx, list) { 75341618Scy if (buf[0] != receiver->oui_suffix) 76341618Scy continue; 77341618Scy 78341618Scy eth_p_oui_deliver(receiver, ethhdr->h_source, ethhdr->h_dest, 79341618Scy buf + 1, len - 1); 80341618Scy } 81341618Scy} 82341618Scy 83341618Scy 84341618Scystruct eth_p_oui_ctx * 85341618Scyeth_p_oui_register(struct hostapd_data *hapd, const char *ifname, u8 oui_suffix, 86341618Scy void (*rx_callback)(void *ctx, const u8 *src_addr, 87341618Scy const u8 *dst_addr, u8 oui_suffix, 88341618Scy const u8 *buf, size_t len), 89341618Scy void *rx_callback_ctx) 90341618Scy{ 91341618Scy struct eth_p_oui_iface *iface; 92341618Scy struct eth_p_oui_ctx *receiver; 93341618Scy int found = 0; 94341618Scy struct hapd_interfaces *interfaces; 95341618Scy 96341618Scy receiver = os_zalloc(sizeof(*receiver)); 97341618Scy if (!receiver) 98341618Scy goto err; 99341618Scy 100341618Scy receiver->oui_suffix = oui_suffix; 101341618Scy receiver->rx_callback = rx_callback; 102341618Scy receiver->rx_callback_ctx = rx_callback_ctx; 103341618Scy 104341618Scy interfaces = hapd->iface->interfaces; 105341618Scy 106341618Scy dl_list_for_each(iface, &interfaces->eth_p_oui, struct eth_p_oui_iface, 107341618Scy list) { 108341618Scy if (os_strcmp(iface->ifname, ifname) != 0) 109341618Scy continue; 110341618Scy found = 1; 111341618Scy break; 112341618Scy } 113341618Scy 114341618Scy if (!found) { 115341618Scy iface = os_zalloc(sizeof(*iface)); 116341618Scy if (!iface) 117341618Scy goto err; 118341618Scy 119341618Scy os_strlcpy(iface->ifname, ifname, sizeof(iface->ifname)); 120341618Scy iface->l2 = l2_packet_init(ifname, NULL, ETH_P_OUI, eth_p_rx, 121341618Scy iface, 1); 122341618Scy if (!iface->l2) { 123341618Scy os_free(iface); 124341618Scy goto err; 125341618Scy } 126341618Scy dl_list_init(&iface->receiver); 127341618Scy 128341618Scy dl_list_add_tail(&interfaces->eth_p_oui, &iface->list); 129341618Scy } 130341618Scy 131341618Scy dl_list_add_tail(&iface->receiver, &receiver->list); 132341618Scy receiver->iface = iface; 133341618Scy 134341618Scy return receiver; 135341618Scyerr: 136341618Scy os_free(receiver); 137341618Scy return NULL; 138341618Scy} 139341618Scy 140341618Scy 141341618Scyvoid eth_p_oui_unregister(struct eth_p_oui_ctx *ctx) 142341618Scy{ 143341618Scy struct eth_p_oui_iface *iface; 144341618Scy 145341618Scy if (!ctx) 146341618Scy return; 147341618Scy 148341618Scy iface = ctx->iface; 149341618Scy 150341618Scy dl_list_del(&ctx->list); 151341618Scy os_free(ctx); 152341618Scy 153341618Scy if (dl_list_empty(&iface->receiver)) { 154341618Scy dl_list_del(&iface->list); 155341618Scy l2_packet_deinit(iface->l2); 156341618Scy os_free(iface); 157341618Scy } 158341618Scy} 159341618Scy 160341618Scy 161341618Scyint eth_p_oui_send(struct eth_p_oui_ctx *ctx, const u8 *src_addr, 162341618Scy const u8 *dst_addr, const u8 *buf, size_t len) 163341618Scy{ 164341618Scy struct eth_p_oui_iface *iface = ctx->iface; 165341618Scy u8 *packet, *p; 166341618Scy size_t packet_len; 167341618Scy int ret; 168341618Scy struct l2_ethhdr *ethhdr; 169341618Scy 170341618Scy packet_len = sizeof(*ethhdr) + sizeof(global_oui) + 1 + len; 171341618Scy packet = os_zalloc(packet_len); 172341618Scy if (!packet) 173341618Scy return -1; 174341618Scy p = packet; 175341618Scy 176341618Scy ethhdr = (struct l2_ethhdr *) packet; 177341618Scy os_memcpy(ethhdr->h_source, src_addr, ETH_ALEN); 178341618Scy os_memcpy(ethhdr->h_dest, dst_addr, ETH_ALEN); 179341618Scy ethhdr->h_proto = host_to_be16(ETH_P_OUI); 180341618Scy p += sizeof(*ethhdr); 181341618Scy 182341618Scy os_memcpy(p, global_oui, sizeof(global_oui)); 183341618Scy p[sizeof(global_oui)] = ctx->oui_suffix; 184341618Scy p += sizeof(global_oui) + 1; 185341618Scy 186341618Scy os_memcpy(p, buf, len); 187341618Scy 188341618Scy ret = l2_packet_send(iface->l2, NULL, 0, packet, packet_len); 189341618Scy os_free(packet); 190341618Scy return ret; 191341618Scy} 192