+/*
+ * hostapd / IEEE 802 OUI Extended EtherType 88-B7
+ * Copyright (c) 2016, Jouni Malinen <j@w1.fi>
+ *
+ * This software may be distributed under the terms of the BSD license.
+ * See README for more details.
+ */
+
+#include "utils/includes.h"
+
+#include "utils/common.h"
+#include "utils/eloop.h"
+#include "l2_packet/l2_packet.h"
+#include "hostapd.h"
+#include "eth_p_oui.h"
+
+/*
+ * See IEEE Std 802-2014, Clause 9.2.4 for the definition of the OUI Extended
+ * EtherType 88-B7. This file implements this with OUI 00:13:74 and
+ * vendor-specific subtype 0x0001.
+ */
+static const u8 global_oui[] = { 0x00, 0x13, 0x74, 0x00, 0x01 };
+
+struct eth_p_oui_iface {
+ struct dl_list list;
+ char ifname[IFNAMSIZ + 1];
+ struct l2_packet_data *l2;
+ struct dl_list receiver;
+};
+
+struct eth_p_oui_ctx {
+ struct dl_list list;
+ struct eth_p_oui_iface *iface;
+ /* all data needed to deliver and unregister */
+ u8 oui_suffix; /* last byte of OUI */
+ void (*rx_callback)(void *ctx, const u8 *src_addr,
+ const u8 *dst_addr, u8 oui_suffix,
+ const u8 *buf, size_t len);
+ void *rx_callback_ctx;
+};
+
+
+void eth_p_oui_deliver(struct eth_p_oui_ctx *ctx, const u8 *src_addr,
+ const u8 *dst_addr, const u8 *buf, size_t len)
+{
+ ctx->rx_callback(ctx->rx_callback_ctx, src_addr, dst_addr,
+ ctx->oui_suffix, buf, len);
+}
+
+
+static void eth_p_rx(void *ctx, const u8 *src_addr, const u8 *buf, size_t len)
+{
+ struct eth_p_oui_iface *iface = ctx;
+ struct eth_p_oui_ctx *receiver;
+ const struct l2_ethhdr *ethhdr;
+
+ if (len < sizeof(*ethhdr) + sizeof(global_oui) + 1) {
+ /* too short packet */
+ return;
+ }
+
+ ethhdr = (struct l2_ethhdr *) buf;
+ /* trim eth_hdr from buf and len */
+ buf += sizeof(*ethhdr);
+ len -= sizeof(*ethhdr);
+
+ /* verify OUI and vendor-specific subtype match */
+ if (os_memcmp(buf, global_oui, sizeof(global_oui)) != 0)
+ return;
+ buf += sizeof(global_oui);
+ len -= sizeof(global_oui);
+
+ dl_list_for_each(receiver, &iface->receiver,
+ struct eth_p_oui_ctx, list) {
+ if (buf[0] != receiver->oui_suffix)
+ continue;
+
+ eth_p_oui_deliver(receiver, ethhdr->h_source, ethhdr->h_dest,
+ buf + 1, len - 1);
+ }
+}
+
+
+struct eth_p_oui_ctx *
+eth_p_oui_register(struct hostapd_data *hapd, const char *ifname, u8 oui_suffix,
+ void (*rx_callback)(void *ctx, const u8 *src_addr,
+ const u8 *dst_addr, u8 oui_suffix,
+ const u8 *buf, size_t len),
+ void *rx_callback_ctx)
+{
+ struct eth_p_oui_iface *iface;
+ struct eth_p_oui_ctx *receiver;
+ int found = 0;
+ struct hapd_interfaces *interfaces;
+
+ receiver = os_zalloc(sizeof(*receiver));
+ if (!receiver)
+ goto err;
+
+ receiver->oui_suffix = oui_suffix;
+ receiver->rx_callback = rx_callback;
+ receiver->rx_callback_ctx = rx_callback_ctx;
+
+ interfaces = hapd->iface->interfaces;
+
+ dl_list_for_each(iface, &interfaces->eth_p_oui, struct eth_p_oui_iface,
+ list) {
+ if (os_strcmp(iface->ifname, ifname) != 0)
+ continue;
+ found = 1;
+ break;
+ }
+
+ if (!found) {
+ iface = os_zalloc(sizeof(*iface));
+ if (!iface)
+ goto err;
+
+ os_strlcpy(iface->ifname, ifname, sizeof(iface->ifname));
+ iface->l2 = l2_packet_init(ifname, NULL, ETH_P_OUI, eth_p_rx,
+ iface, 1);
+ if (!iface->l2) {
+ os_free(iface);
+ goto err;
+ }
+ dl_list_init(&iface->receiver);
+
+ dl_list_add_tail(&interfaces->eth_p_oui, &iface->list);
+ }
+
+ dl_list_add_tail(&iface->receiver, &receiver->list);
+ receiver->iface = iface;
+
+ return receiver;
+err:
+ os_free(receiver);
+ return NULL;
+}
+
+
+void eth_p_oui_unregister(struct eth_p_oui_ctx *ctx)
+{
+ struct eth_p_oui_iface *iface;
+
+ if (!ctx)
+ return;
+
+ iface = ctx->iface;
+
+ dl_list_del(&ctx->list);
+ os_free(ctx);
+
+ if (dl_list_empty(&iface->receiver)) {
+ dl_list_del(&iface->list);
+ l2_packet_deinit(iface->l2);
+ os_free(iface);
+ }
+}
+
+
+int eth_p_oui_send(struct eth_p_oui_ctx *ctx, const u8 *src_addr,
+ const u8 *dst_addr, const u8 *buf, size_t len)
+{
+ struct eth_p_oui_iface *iface = ctx->iface;
+ u8 *packet, *p;
+ size_t packet_len;
+ int ret;
+ struct l2_ethhdr *ethhdr;
+
+ packet_len = sizeof(*ethhdr) + sizeof(global_oui) + 1 + len;
+ packet = os_zalloc(packet_len);
+ if (!packet)
+ return -1;
+ p = packet;
+
+ ethhdr = (struct l2_ethhdr *) packet;
+ os_memcpy(ethhdr->h_source, src_addr, ETH_ALEN);
+ os_memcpy(ethhdr->h_dest, dst_addr, ETH_ALEN);
+ ethhdr->h_proto = host_to_be16(ETH_P_OUI);
+ p += sizeof(*ethhdr);
+
+ os_memcpy(p, global_oui, sizeof(global_oui));
+ p[sizeof(global_oui)] = ctx->oui_suffix;
+ p += sizeof(global_oui) + 1;
+
+ os_memcpy(p, buf, len);
+
+ ret = l2_packet_send(iface->l2, NULL, 0, packet, packet_len);
+ os_free(packet);
+ return ret;
+}