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
