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  */
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 
eth_p_oui_deliver(struct eth_p_oui_ctx * ctx,const u8 * src_addr,const u8 * dst_addr,const u8 * buf,size_t len)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 
eth_p_rx(void * ctx,const u8 * src_addr,const u8 * buf,size_t len)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 *
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)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 
eth_p_oui_unregister(struct eth_p_oui_ctx * ctx)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 
eth_p_oui_send(struct eth_p_oui_ctx * ctx,const u8 * src_addr,const u8 * dst_addr,const u8 * buf,size_t len)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 }
192