hostapd/src/ap/eth_p_oui.c

/*
 * 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;
}