contrib/wpa/src/ap/eth_p_oui.c

   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 }