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 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 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 * 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 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 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