1/*
2 * hostapd / IEEE 802 OUI Extended EtherType 88-B7
3 * Copyright (c) 2016, Jouni Malinen <j@w1.fi>
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 */
22static const u8 global_oui[] = { 0x00, 0x13, 0x74, 0x00, 0x01 };
23
24struct 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
31struct 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
43void 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
51static 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
84struct eth_p_oui_ctx *
85eth_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;
135err:
136	os_free(receiver);
137	return NULL;
138}
139
140
141void 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
161int 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}
192