diff --git a/src/drivers/driver_macsec_qca.c b/src/drivers/driver_macsec_qca.c index 8372393f2..f4e55d5d9 100644 --- a/src/drivers/driver_macsec_qca.c +++ b/src/drivers/driver_macsec_qca.c @@ -3,6 +3,7 @@ * Copyright (c) 2005-2009, Jouni Malinen * Copyright (c) 2004, Gunter Burchardt * Copyright (c) 2013-2014, Qualcomm Atheros, Inc. + * Copyright (c) 2019, The Linux Foundation * * This software may be distributed under the terms of the BSD license. * See README for more details. @@ -29,6 +30,7 @@ #include "utils/eloop.h" #include "common/defs.h" #include "common/ieee802_1x_defs.h" +#include "common/eapol_common.h" #include "pae/ieee802_1x_kay.h" #include "driver.h" #include "driver_wired_common.h" @@ -64,6 +66,7 @@ struct channel_map { struct macsec_qca_data { struct driver_wired_common_data common; + int use_pae_group_addr; u32 secy_id; /* shadow */ @@ -126,6 +129,134 @@ static void __macsec_drv_deinit(struct macsec_qca_data *drv) } +#ifdef __linux__ + +static void macsec_qca_handle_data(void *ctx, unsigned char *buf, size_t len) +{ +#ifdef HOSTAPD + struct ieee8023_hdr *hdr; + u8 *pos, *sa; + size_t left; + union wpa_event_data event; + + /* at least 6 bytes src macaddress, 6 bytes dst macaddress + * and 2 bytes ethertype + */ + if (len < 14) { + wpa_printf(MSG_MSGDUMP, + "macsec_qca_handle_data: too short (%lu)", + (unsigned long) len); + return; + } + hdr = (struct ieee8023_hdr *) buf; + + switch (ntohs(hdr->ethertype)) { + case ETH_P_PAE: + wpa_printf(MSG_MSGDUMP, "Received EAPOL packet"); + sa = hdr->src; + os_memset(&event, 0, sizeof(event)); + event.new_sta.addr = sa; + wpa_supplicant_event(ctx, EVENT_NEW_STA, &event); + + pos = (u8 *) (hdr + 1); + left = len - sizeof(*hdr); + drv_event_eapol_rx(ctx, sa, pos, left); + break; + default: + wpa_printf(MSG_DEBUG, "Unknown ethertype 0x%04x in data frame", + ntohs(hdr->ethertype)); + break; + } +#endif /* HOSTAPD */ +} + + +static void macsec_qca_handle_read(int sock, void *eloop_ctx, void *sock_ctx) +{ + int len; + unsigned char buf[3000]; + + len = recv(sock, buf, sizeof(buf), 0); + if (len < 0) { + wpa_printf(MSG_ERROR, "macsec_qca: recv: %s", strerror(errno)); + return; + } + + macsec_qca_handle_data(eloop_ctx, buf, len); +} + +#endif /* __linux__ */ + + +static int macsec_qca_init_sockets(struct macsec_qca_data *drv, u8 *own_addr) +{ +#ifdef __linux__ + struct ifreq ifr; + struct sockaddr_ll addr; + + drv->common.sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PAE)); + if (drv->common.sock < 0) { + wpa_printf(MSG_ERROR, "socket[PF_PACKET,SOCK_RAW]: %s", + strerror(errno)); + return -1; + } + + if (eloop_register_read_sock(drv->common.sock, macsec_qca_handle_read, + drv->common.ctx, NULL)) { + wpa_printf(MSG_INFO, "Could not register read socket"); + return -1; + } + + os_memset(&ifr, 0, sizeof(ifr)); + os_strlcpy(ifr.ifr_name, drv->common.ifname, sizeof(ifr.ifr_name)); + if (ioctl(drv->common.sock, SIOCGIFINDEX, &ifr) != 0) { + wpa_printf(MSG_ERROR, "ioctl(SIOCGIFINDEX): %s", + strerror(errno)); + return -1; + } + + os_memset(&addr, 0, sizeof(addr)); + addr.sll_family = AF_PACKET; + addr.sll_ifindex = ifr.ifr_ifindex; + wpa_printf(MSG_DEBUG, "Opening raw packet socket for ifindex %d", + addr.sll_ifindex); + + if (bind(drv->common.sock, (struct sockaddr *) &addr, + sizeof(addr)) < 0) { + wpa_printf(MSG_ERROR, "macsec_qca: bind: %s", strerror(errno)); + return -1; + } + + /* filter multicast address */ + if (wired_multicast_membership(drv->common.sock, ifr.ifr_ifindex, + pae_group_addr, 1) < 0) { + wpa_printf(MSG_ERROR, + "macsec_qca_init_sockets: Failed to add multicast group membership"); + return -1; + } + + os_memset(&ifr, 0, sizeof(ifr)); + os_strlcpy(ifr.ifr_name, drv->common.ifname, sizeof(ifr.ifr_name)); + if (ioctl(drv->common.sock, SIOCGIFHWADDR, &ifr) != 0) { + wpa_printf(MSG_ERROR, "ioctl(SIOCGIFHWADDR): %s", + strerror(errno)); + return -1; + } + + if (ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER) { + wpa_printf(MSG_INFO, "Invalid HW-addr family 0x%04x", + ifr.ifr_hwaddr.sa_family); + return -1; + } + os_memcpy(own_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN); + + return 0; +#else /* __linux__ */ + return -1; +#endif /* __linux__ */ +} + + static void * macsec_qca_init(void *ctx, const char *ifname) { struct macsec_qca_data *drv; @@ -160,6 +291,97 @@ static void macsec_qca_deinit(void *priv) } +static void * macsec_qca_hapd_init(struct hostapd_data *hapd, + struct wpa_init_params *params) +{ + struct macsec_qca_data *drv; + + drv = os_zalloc(sizeof(struct macsec_qca_data)); + if (!drv) { + wpa_printf(MSG_INFO, + "Could not allocate memory for macsec_qca driver data"); + return NULL; + } + + /* Board specific settings */ + if (os_memcmp("eth2", params->ifname, 4) == 0) + drv->secy_id = 1; + else if (os_memcmp("eth3", params->ifname, 4) == 0) + drv->secy_id = 2; + else if (os_memcmp("eth4", params->ifname, 4) == 0) + drv->secy_id = 0; + else if (os_memcmp("eth5", params->ifname, 4) == 0) + drv->secy_id = 1; + else + drv->secy_id = -1; + + drv->common.ctx = hapd; + os_strlcpy(drv->common.ifname, params->ifname, + sizeof(drv->common.ifname)); + drv->use_pae_group_addr = params->use_pae_group_addr; + + if (macsec_qca_init_sockets(drv, params->own_addr)) { + os_free(drv); + return NULL; + } + + return drv; +} + + +static void macsec_qca_hapd_deinit(void *priv) +{ + struct macsec_qca_data *drv = priv; + + if (drv->common.sock >= 0) { + eloop_unregister_read_sock(drv->common.sock); + close(drv->common.sock); + } + + os_free(drv); +} + + +static int macsec_qca_send_eapol(void *priv, const u8 *addr, + const u8 *data, size_t data_len, int encrypt, + const u8 *own_addr, u32 flags) +{ + struct macsec_qca_data *drv = priv; + struct ieee8023_hdr *hdr; + size_t len; + u8 *pos; + int res; + + len = sizeof(*hdr) + data_len; + hdr = os_zalloc(len); + if (!hdr) { + wpa_printf(MSG_INFO, + "malloc() failed for macsec_qca_send_eapol(len=%lu)", + (unsigned long) len); + return -1; + } + + os_memcpy(hdr->dest, drv->use_pae_group_addr ? pae_group_addr : addr, + ETH_ALEN); + os_memcpy(hdr->src, own_addr, ETH_ALEN); + hdr->ethertype = htons(ETH_P_PAE); + + pos = (u8 *) (hdr + 1); + os_memcpy(pos, data, data_len); + + res = send(drv->common.sock, (u8 *) hdr, len, 0); + os_free(hdr); + + if (res < 0) { + wpa_printf(MSG_ERROR, + "macsec_qca_send_eapol - packet len: %lu - failed: send: %s", + (unsigned long) len, strerror(errno)); + } + + return res; +} + + static int macsec_qca_macsec_init(void *priv, struct macsec_init_params *params) { struct macsec_qca_data *drv = priv; @@ -800,6 +1022,9 @@ const struct wpa_driver_ops wpa_driver_macsec_qca_ops = { .get_capa = driver_wired_get_capa, .init = macsec_qca_init, .deinit = macsec_qca_deinit, + .hapd_init = macsec_qca_hapd_init, + .hapd_deinit = macsec_qca_hapd_deinit, + .hapd_send_eapol = macsec_qca_send_eapol, .macsec_init = macsec_qca_macsec_init, .macsec_deinit = macsec_qca_macsec_deinit,