contrib/wpa/src/drivers/driver_macsec_linux.c

   1 /*
   2  * Driver interaction with Linux MACsec kernel module
   3  * Copyright (c) 2016, Sabrina Dubroca <sd@queasysnail.net> and Red Hat, Inc.
   4  * Copyright (c) 2019, The Linux Foundation
   5  *
   6  * This software may be distributed under the terms of the BSD license.
   7  * See README for more details.
   8  */
   9
  10 #include "includes.h"
  11 #include <sys/ioctl.h>
  12 #include <net/if.h>
  13 #include <netpacket/packet.h>
  14 #include <net/if_arp.h>
  15 #include <net/if.h>
  16 #include <netlink/netlink.h>
  17 #include <netlink/genl/genl.h>
  18 #include <netlink/genl/ctrl.h>
  19 #include <netlink/route/link.h>
  20 #include <netlink/route/link/macsec.h>
  21 #include <linux/if_macsec.h>
  22 #include <inttypes.h>
  23
  24 #include "utils/common.h"
  25 #include "utils/eloop.h"
  26 #include "common/eapol_common.h"
  27 #include "pae/ieee802_1x_kay.h"
  28 #include "driver.h"
  29 #include "driver_wired_common.h"
  30
  31 #define DRV_PREFIX "macsec_linux: "
  32
  33 #define UNUSED_SCI 0xffffffffffffffff
  34
  35 struct cb_arg {
  36         struct macsec_drv_data *drv;
  37         u32 *pn;
  38         int ifindex;
  39         u8 txsa;
  40         u8 rxsa;
  41         u64 rxsci;
  42 };
  43
  44 struct macsec_genl_ctx {
  45         struct nl_sock *sk;
  46         int macsec_genl_id;
  47         struct cb_arg cb_arg;
  48 };
  49
  50 struct macsec_drv_data {
  51         struct driver_wired_common_data common;
  52         struct rtnl_link *link;
  53         struct nl_cache *link_cache;
  54         struct nl_sock *sk;
  55         struct macsec_genl_ctx ctx;
  56
  57         struct netlink_data *netlink;
  58         struct nl_handle *nl;
  59         char ifname[IFNAMSIZ + 1];
  60         int ifi;
  61         int parent_ifi;
  62         int use_pae_group_addr;
  63
  64         Boolean created_link;
  65
  66         Boolean controlled_port_enabled;
  67         Boolean controlled_port_enabled_set;
  68
  69         Boolean protect_frames;
  70         Boolean protect_frames_set;
  71
  72         Boolean encrypt;
  73         Boolean encrypt_set;
  74
  75         Boolean replay_protect;
  76         Boolean replay_protect_set;
  77
  78         u32 replay_window;
  79
  80         u8 encoding_sa;
  81         Boolean encoding_sa_set;
  82 };
  83
  84
  85 static int dump_callback(struct nl_msg *msg, void *argp);
  86
  87
  88 static struct nl_msg * msg_prepare(enum macsec_nl_commands cmd,
  89                                    const struct macsec_genl_ctx *ctx,
  90                                    unsigned int ifindex)
  91 {
  92         struct nl_msg *msg;
  93
  94         msg = nlmsg_alloc();
  95         if (!msg) {
  96                 wpa_printf(MSG_ERROR, DRV_PREFIX "failed to alloc message");
  97                 return NULL;
  98         }
  99
 100         if (!genlmsg_put(msg, 0, 0, ctx->macsec_genl_id, 0, 0, cmd, 0)) {
 101                 wpa_printf(MSG_ERROR, DRV_PREFIX "failed to put header");
 102                 goto nla_put_failure;
 103         }
 104
 105         NLA_PUT_U32(msg, MACSEC_ATTR_IFINDEX, ifindex);
 106
 107         return msg;
 108
 109 nla_put_failure:
 110         nlmsg_free(msg);
 111         return NULL;
 112 }
 113
 114
 115 static int nla_put_rxsc_config(struct nl_msg *msg, u64 sci)
 116 {
 117         struct nlattr *nest = nla_nest_start(msg, MACSEC_ATTR_RXSC_CONFIG);
 118
 119         if (!nest)
 120                 return -1;
 121
 122         NLA_PUT_U64(msg, MACSEC_RXSC_ATTR_SCI, sci);
 123
 124         nla_nest_end(msg, nest);
 125
 126         return 0;
 127
 128 nla_put_failure:
 129         return -1;
 130 }
 131
 132
 133 static int init_genl_ctx(struct macsec_drv_data *drv)
 134 {
 135         struct macsec_genl_ctx *ctx = &drv->ctx;
 136
 137         ctx->sk = nl_socket_alloc();
 138         if (!ctx->sk) {
 139                 wpa_printf(MSG_ERROR, DRV_PREFIX "failed to alloc genl socket");
 140                 return -1;
 141         }
 142
 143         if (genl_connect(ctx->sk) < 0) {
 144                 wpa_printf(MSG_ERROR,
 145                            DRV_PREFIX "connection to genl socket failed");
 146                 goto out_free;
 147         }
 148
 149         ctx->macsec_genl_id = genl_ctrl_resolve(ctx->sk, "macsec");
 150         if (ctx->macsec_genl_id < 0) {
 151                 wpa_printf(MSG_ERROR, DRV_PREFIX "genl resolve failed");
 152                 goto out_free;
 153         }
 154
 155         memset(&ctx->cb_arg, 0, sizeof(ctx->cb_arg));
 156         ctx->cb_arg.drv = drv;
 157
 158         nl_socket_modify_cb(ctx->sk, NL_CB_VALID, NL_CB_CUSTOM, dump_callback,
 159                             &ctx->cb_arg);
 160
 161         return 0;
 162
 163 out_free:
 164         nl_socket_free(ctx->sk);
 165         ctx->sk = NULL;
 166         return -1;
 167 }
 168
 169
 170 static int try_commit(struct macsec_drv_data *drv)
 171 {
 172         int err;
 173
 174         if (!drv->sk)
 175                 return 0;
 176
 177         if (!drv->link)
 178                 return 0;
 179
 180         if (drv->controlled_port_enabled_set) {
 181                 struct rtnl_link *change = rtnl_link_alloc();
 182
 183                 wpa_printf(MSG_DEBUG, DRV_PREFIX
 184                            "%s: try_commit controlled_port_enabled=%d",
 185                            drv->ifname, drv->controlled_port_enabled);
 186                 if (!change)
 187                         return -1;
 188
 189                 rtnl_link_set_name(change, drv->ifname);
 190
 191                 if (drv->controlled_port_enabled)
 192                         rtnl_link_set_flags(change, IFF_UP);
 193                 else
 194                         rtnl_link_unset_flags(change, IFF_UP);
 195
 196                 err = rtnl_link_change(drv->sk, change, change, 0);
 197                 if (err < 0)
 198                         return err;
 199
 200                 rtnl_link_put(change);
 201
 202                 drv->controlled_port_enabled_set = FALSE;
 203         }
 204
 205         if (drv->protect_frames_set) {
 206                 wpa_printf(MSG_DEBUG, DRV_PREFIX
 207                            "%s: try_commit protect_frames=%d",
 208                            drv->ifname, drv->protect_frames);
 209                 rtnl_link_macsec_set_protect(drv->link, drv->protect_frames);
 210         }
 211
 212         if (drv->encrypt_set) {
 213                 wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: try_commit encrypt=%d",
 214                            drv->ifname, drv->encrypt);
 215                 rtnl_link_macsec_set_encrypt(drv->link, drv->encrypt);
 216         }
 217
 218         if (drv->replay_protect_set) {
 219                 wpa_printf(MSG_DEBUG, DRV_PREFIX
 220                            "%s: try_commit replay_protect=%d replay_window=%d",
 221                            drv->ifname, drv->replay_protect,
 222                            drv->replay_window);
 223                 rtnl_link_macsec_set_replay_protect(drv->link,
 224                                                     drv->replay_protect);
 225                 if (drv->replay_protect)
 226                         rtnl_link_macsec_set_window(drv->link,
 227                                                     drv->replay_window);
 228         }
 229
 230         if (drv->encoding_sa_set) {
 231                 wpa_printf(MSG_DEBUG, DRV_PREFIX
 232                            "%s: try_commit encoding_sa=%d",
 233                            drv->ifname, drv->encoding_sa);
 234                 rtnl_link_macsec_set_encoding_sa(drv->link, drv->encoding_sa);
 235         }
 236
 237         err = rtnl_link_add(drv->sk, drv->link, 0);
 238         if (err < 0)
 239                 return err;
 240
 241         drv->protect_frames_set = FALSE;
 242         drv->encrypt_set = FALSE;
 243         drv->replay_protect_set = FALSE;
 244
 245         return 0;
 246 }
 247
 248
 249 static void macsec_drv_wpa_deinit(void *priv)
 250 {
 251         struct macsec_drv_data *drv = priv;
 252
 253         driver_wired_deinit_common(&drv->common);
 254         os_free(drv);
 255 }
 256
 257
 258 static int macsec_check_macsec(void)
 259 {
 260         struct nl_sock *sk;
 261         int err = -1;
 262
 263         sk = nl_socket_alloc();
 264         if (!sk) {
 265                 wpa_printf(MSG_ERROR, DRV_PREFIX "failed to alloc genl socket");
 266                 return -1;
 267         }
 268
 269         if (genl_connect(sk) < 0) {
 270                 wpa_printf(MSG_ERROR,
 271                            DRV_PREFIX "connection to genl socket failed");
 272                 goto out_free;
 273         }
 274
 275         if (genl_ctrl_resolve(sk, "macsec") < 0) {
 276                 wpa_printf(MSG_ERROR,
 277                            DRV_PREFIX "genl resolve failed - macsec kernel module not present?");
 278                 goto out_free;
 279         }
 280
 281         err = 0;
 282
 283 out_free:
 284         nl_socket_free(sk);
 285         return err;
 286 }
 287
 288
 289 static void * macsec_drv_wpa_init(void *ctx, const char *ifname)
 290 {
 291         struct macsec_drv_data *drv;
 292
 293         if (macsec_check_macsec() < 0)
 294                 return NULL;
 295
 296         drv = os_zalloc(sizeof(*drv));
 297         if (!drv)
 298                 return NULL;
 299
 300         if (driver_wired_init_common(&drv->common, ifname, ctx) < 0) {
 301                 os_free(drv);
 302                 return NULL;
 303         }
 304
 305         return drv;
 306 }
 307
 308
 309 static int macsec_drv_macsec_init(void *priv, struct macsec_init_params *params)
 310 {
 311         struct macsec_drv_data *drv = priv;
 312         int err;
 313
 314         wpa_printf(MSG_DEBUG, "%s", __func__);
 315
 316         drv->sk = nl_socket_alloc();
 317         if (!drv->sk)
 318                 return -1;
 319
 320         err = nl_connect(drv->sk, NETLINK_ROUTE);
 321         if (err < 0) {
 322                 wpa_printf(MSG_ERROR, DRV_PREFIX
 323                            "Unable to connect NETLINK_ROUTE socket: %s",
 324                            strerror(errno));
 325                 goto sock;
 326         }
 327
 328         err = rtnl_link_alloc_cache(drv->sk, AF_UNSPEC, &drv->link_cache);
 329         if (err < 0) {
 330                 wpa_printf(MSG_ERROR, DRV_PREFIX "Unable to get link cache: %s",
 331                            strerror(errno));
 332                 goto sock;
 333         }
 334
 335         drv->parent_ifi = rtnl_link_name2i(drv->link_cache, drv->common.ifname);
 336         if (drv->parent_ifi == 0) {
 337                 wpa_printf(MSG_ERROR, DRV_PREFIX
 338                            "couldn't find ifindex for interface %s",
 339                            drv->common.ifname);
 340                 goto cache;
 341         }
 342         wpa_printf(MSG_DEBUG, DRV_PREFIX "ifname=%s parent_ifi=%d",
 343                    drv->common.ifname, drv->parent_ifi);
 344
 345         err = init_genl_ctx(drv);
 346         if (err < 0)
 347                 goto cache;
 348
 349         return 0;
 350
 351 cache:
 352         nl_cache_free(drv->link_cache);
 353         drv->link_cache = NULL;
 354 sock:
 355         nl_socket_free(drv->sk);
 356         drv->sk = NULL;
 357         return -1;
 358 }
 359
 360
 361 static int macsec_drv_macsec_deinit(void *priv)
 362 {
 363         struct macsec_drv_data *drv = priv;
 364
 365         wpa_printf(MSG_DEBUG, "%s", __func__);
 366
 367         if (drv->sk)
 368                 nl_socket_free(drv->sk);
 369         drv->sk = NULL;
 370
 371         if (drv->link_cache)
 372                 nl_cache_free(drv->link_cache);
 373         drv->link_cache = NULL;
 374
 375         if (drv->ctx.sk)
 376                 nl_socket_free(drv->ctx.sk);
 377
 378         return 0;
 379 }
 380
 381
 382 static int macsec_drv_get_capability(void *priv, enum macsec_cap *cap)
 383 {
 384         wpa_printf(MSG_DEBUG, "%s", __func__);
 385
 386         *cap = MACSEC_CAP_INTEG_AND_CONF;
 387
 388         return 0;
 389 }
 390
 391
 392 /**
 393  * macsec_drv_enable_protect_frames - Set protect frames status
 394  * @priv: Private driver interface data
 395  * @enabled: TRUE = protect frames enabled
 396  *           FALSE = protect frames disabled
 397  * Returns: 0 on success, -1 on failure (or if not supported)
 398  */
 399 static int macsec_drv_enable_protect_frames(void *priv, Boolean enabled)
 400 {
 401         struct macsec_drv_data *drv = priv;
 402
 403         wpa_printf(MSG_DEBUG, "%s -> %s", __func__, enabled ? "TRUE" : "FALSE");
 404
 405         drv->protect_frames_set = TRUE;
 406         drv->protect_frames = enabled;
 407
 408         return try_commit(drv);
 409 }
 410
 411
 412 /**
 413  * macsec_drv_enable_encrypt - Set protect frames status
 414  * @priv: Private driver interface data
 415  * @enabled: TRUE = protect frames enabled
 416  *           FALSE = protect frames disabled
 417  * Returns: 0 on success, -1 on failure (or if not supported)
 418  */
 419 static int macsec_drv_enable_encrypt(void *priv, Boolean enabled)
 420 {
 421         struct macsec_drv_data *drv = priv;
 422
 423         wpa_printf(MSG_DEBUG, "%s -> %s", __func__, enabled ? "TRUE" : "FALSE");
 424
 425         drv->encrypt_set = TRUE;
 426         drv->encrypt = enabled;
 427
 428         return try_commit(drv);
 429 }
 430
 431
 432 /**
 433  * macsec_drv_set_replay_protect - Set replay protect status and window size
 434  * @priv: Private driver interface data
 435  * @enabled: TRUE = replay protect enabled
 436  *           FALSE = replay protect disabled
 437  * @window: replay window size, valid only when replay protect enabled
 438  * Returns: 0 on success, -1 on failure (or if not supported)
 439  */
 440 static int macsec_drv_set_replay_protect(void *priv, Boolean enabled,
 441                                          u32 window)
 442 {
 443         struct macsec_drv_data *drv = priv;
 444
 445         wpa_printf(MSG_DEBUG, "%s -> %s, %u", __func__,
 446                    enabled ? "TRUE" : "FALSE", window);
 447
 448         drv->replay_protect_set = TRUE;
 449         drv->replay_protect = enabled;
 450         if (enabled)
 451                 drv->replay_window = window;
 452
 453         return try_commit(drv);
 454 }
 455
 456
 457 /**
 458  * macsec_drv_set_current_cipher_suite - Set current cipher suite
 459  * @priv: Private driver interface data
 460  * @cs: EUI64 identifier
 461  * Returns: 0 on success, -1 on failure (or if not supported)
 462  */
 463 static int macsec_drv_set_current_cipher_suite(void *priv, u64 cs)
 464 {
 465         wpa_printf(MSG_DEBUG, "%s -> %016" PRIx64, __func__, cs);
 466         return 0;
 467 }
 468
 469
 470 /**
 471  * macsec_drv_enable_controlled_port - Set controlled port status
 472  * @priv: Private driver interface data
 473  * @enabled: TRUE = controlled port enabled
 474  *           FALSE = controlled port disabled
 475  * Returns: 0 on success, -1 on failure (or if not supported)
 476  */
 477 static int macsec_drv_enable_controlled_port(void *priv, Boolean enabled)
 478 {
 479         struct macsec_drv_data *drv = priv;
 480
 481         wpa_printf(MSG_DEBUG, "%s -> %s", __func__, enabled ? "TRUE" : "FALSE");
 482
 483         drv->controlled_port_enabled = enabled;
 484         drv->controlled_port_enabled_set = TRUE;
 485
 486         return try_commit(drv);
 487 }
 488
 489
 490 static struct nla_policy sa_policy[MACSEC_SA_ATTR_MAX + 1] = {
 491         [MACSEC_SA_ATTR_AN] = { .type = NLA_U8 },
 492         [MACSEC_SA_ATTR_ACTIVE] = { .type = NLA_U8 },
 493         [MACSEC_SA_ATTR_PN] = { .type = NLA_U32 },
 494         [MACSEC_SA_ATTR_KEYID] = { .type = NLA_BINARY },
 495 };
 496
 497 static struct nla_policy sc_policy[MACSEC_RXSC_ATTR_MAX + 1] = {
 498         [MACSEC_RXSC_ATTR_SCI] = { .type = NLA_U64 },
 499         [MACSEC_RXSC_ATTR_ACTIVE] = { .type = NLA_U8 },
 500         [MACSEC_RXSC_ATTR_SA_LIST] = { .type = NLA_NESTED },
 501 };
 502
 503 static struct nla_policy main_policy[MACSEC_ATTR_MAX + 1] = {
 504         [MACSEC_ATTR_IFINDEX] = { .type = NLA_U32 },
 505         [MACSEC_ATTR_SECY] = { .type = NLA_NESTED },
 506         [MACSEC_ATTR_TXSA_LIST] = { .type = NLA_NESTED },
 507         [MACSEC_ATTR_RXSC_LIST] = { .type = NLA_NESTED },
 508 };
 509
 510 static int dump_callback(struct nl_msg *msg, void *argp)
 511 {
 512         struct nlmsghdr *ret_hdr = nlmsg_hdr(msg);
 513         struct nlattr *tb_msg[MACSEC_ATTR_MAX + 1];
 514         struct cb_arg *arg = (struct cb_arg *) argp;
 515         struct genlmsghdr *gnlh = (struct genlmsghdr *) nlmsg_data(ret_hdr);
 516         int err;
 517
 518         if (ret_hdr->nlmsg_type != arg->drv->ctx.macsec_genl_id)
 519                 return 0;
 520
 521         err = nla_parse(tb_msg, MACSEC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 522                         genlmsg_attrlen(gnlh, 0), main_policy);
 523         if (err < 0)
 524                 return 0;
 525
 526         if (!tb_msg[MACSEC_ATTR_IFINDEX])
 527                 return 0;
 528
 529         if (nla_get_u32(tb_msg[MACSEC_ATTR_IFINDEX]) != (u32) arg->ifindex)
 530                 return 0;
 531
 532         if (arg->txsa < 4 && !tb_msg[MACSEC_ATTR_TXSA_LIST]) {
 533                 return 0;
 534         } else if (arg->txsa < 4) {
 535                 struct nlattr *nla;
 536                 int rem;
 537
 538                 nla_for_each_nested(nla, tb_msg[MACSEC_ATTR_TXSA_LIST], rem) {
 539                         struct nlattr *tb[MACSEC_SA_ATTR_MAX + 1];
 540
 541                         err = nla_parse_nested(tb, MACSEC_SA_ATTR_MAX, nla,
 542                                                sa_policy);
 543                         if (err < 0)
 544                                 continue;
 545                         if (!tb[MACSEC_SA_ATTR_AN])
 546                                 continue;
 547                         if (nla_get_u8(tb[MACSEC_SA_ATTR_AN]) != arg->txsa)
 548                                 continue;
 549                         if (!tb[MACSEC_SA_ATTR_PN])
 550                                 return 0;
 551                         *arg->pn = nla_get_u32(tb[MACSEC_SA_ATTR_PN]);
 552                         return 0;
 553                 }
 554
 555                 return 0;
 556         }
 557
 558         if (arg->rxsci == UNUSED_SCI)
 559                 return 0;
 560
 561         if (tb_msg[MACSEC_ATTR_RXSC_LIST]) {
 562                 struct nlattr *nla;
 563                 int rem;
 564
 565                 nla_for_each_nested(nla, tb_msg[MACSEC_ATTR_RXSC_LIST], rem) {
 566                         struct nlattr *tb[MACSEC_RXSC_ATTR_MAX + 1];
 567
 568                         err = nla_parse_nested(tb, MACSEC_RXSC_ATTR_MAX, nla,
 569                                                sc_policy);
 570                         if (err < 0)
 571                                 return 0;
 572                         if (!tb[MACSEC_RXSC_ATTR_SCI])
 573                                 continue;
 574                         if (nla_get_u64(tb[MACSEC_RXSC_ATTR_SCI]) != arg->rxsci)
 575                                 continue;
 576                         if (!tb[MACSEC_RXSC_ATTR_SA_LIST])
 577                                 return 0;
 578
 579                         nla_for_each_nested(nla, tb[MACSEC_RXSC_ATTR_SA_LIST],
 580                                             rem) {
 581                                 struct nlattr *tb_sa[MACSEC_SA_ATTR_MAX + 1];
 582
 583                                 err = nla_parse_nested(tb_sa,
 584                                                        MACSEC_SA_ATTR_MAX, nla,
 585                                                        sa_policy);
 586                                 if (err < 0)
 587                                         continue;
 588                                 if (!tb_sa[MACSEC_SA_ATTR_AN])
 589                                         continue;
 590                                 if (nla_get_u8(tb_sa[MACSEC_SA_ATTR_AN]) !=
 591                                     arg->rxsa)
 592                                         continue;
 593                                 if (!tb_sa[MACSEC_SA_ATTR_PN])
 594                                         return 0;
 595                                 *arg->pn =
 596                                         nla_get_u32(tb_sa[MACSEC_SA_ATTR_PN]);
 597
 598                                 return 0;
 599                         }
 600
 601                         return 0;
 602                 }
 603
 604                 return 0;
 605         }
 606
 607         return 0;
 608 }
 609
 610
 611 static int nl_send_recv(struct nl_sock *sk, struct nl_msg *msg)
 612 {
 613         int ret;
 614
 615         ret = nl_send_auto_complete(sk, msg);
 616         if (ret < 0) {
 617                 wpa_printf(MSG_ERROR, DRV_PREFIX "%s: failed to send: %d (%s)",
 618                            __func__, ret, nl_geterror(-ret));
 619                 return ret;
 620         }
 621
 622         ret = nl_recvmsgs_default(sk);
 623         if (ret < 0) {
 624                 wpa_printf(MSG_ERROR, DRV_PREFIX "%s: failed to recv: %d (%s)",
 625                            __func__, ret, nl_geterror(-ret));
 626         }
 627
 628         return ret;
 629 }
 630
 631
 632 static int do_dump(struct macsec_drv_data *drv, u8 txsa, u64 rxsci, u8 rxsa,
 633                    u32 *pn)
 634 {
 635         struct macsec_genl_ctx *ctx = &drv->ctx;
 636         struct nl_msg *msg;
 637         int ret = 1;
 638
 639         ctx->cb_arg.ifindex = drv->ifi;
 640         ctx->cb_arg.rxsci = rxsci;
 641         ctx->cb_arg.rxsa = rxsa;
 642         ctx->cb_arg.txsa = txsa;
 643         ctx->cb_arg.pn = pn;
 644
 645         msg = nlmsg_alloc();
 646         if (!msg) {
 647                 wpa_printf(MSG_ERROR, DRV_PREFIX "%s: failed to alloc message",
 648                            __func__);
 649                 return 1;
 650         }
 651
 652         if (!genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, ctx->macsec_genl_id, 0,
 653                          NLM_F_DUMP, MACSEC_CMD_GET_TXSC, 0)) {
 654                 wpa_printf(MSG_ERROR, DRV_PREFIX "%s: failed to put header",
 655                            __func__);
 656                 goto out_free_msg;
 657         }
 658
 659         ret = nl_send_recv(ctx->sk, msg);
 660         if (ret < 0)
 661                 wpa_printf(MSG_ERROR,
 662                            DRV_PREFIX "failed to communicate: %d (%s)",
 663                            ret, nl_geterror(-ret));
 664
 665         ctx->cb_arg.pn = NULL;
 666
 667 out_free_msg:
 668         nlmsg_free(msg);
 669         return ret;
 670 }
 671
 672
 673 /**
 674  * macsec_drv_get_receive_lowest_pn - Get receive lowest PN
 675  * @priv: Private driver interface data
 676  * @sa: secure association
 677  * Returns: 0 on success, -1 on failure (or if not supported)
 678  */
 679 static int macsec_drv_get_receive_lowest_pn(void *priv, struct receive_sa *sa)
 680 {
 681         struct macsec_drv_data *drv = priv;
 682         int err;
 683
 684         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s", __func__);
 685
 686         err = do_dump(drv, 0xff, mka_sci_u64(&sa->sc->sci), sa->an,
 687                       &sa->lowest_pn);
 688         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: result %d", __func__,
 689                    sa->lowest_pn);
 690
 691         return err;
 692 }
 693
 694
 695 /**
 696  * macsec_drv_set_receive_lowest_pn - Set receive lowest PN
 697  * @priv: Private driver interface data
 698  * @sa: secure association
 699  * Returns: 0 on success, -1 on failure (or if not supported)
 700  */
 701 static int macsec_drv_set_receive_lowest_pn(void *priv, struct receive_sa *sa)
 702 {
 703         struct macsec_drv_data *drv = priv;
 704         struct macsec_genl_ctx *ctx = &drv->ctx;
 705         struct nl_msg *msg;
 706         struct nlattr *nest;
 707         int ret = -1;
 708
 709         wpa_printf(MSG_DEBUG,
 710                    DRV_PREFIX "%s: set_receive_lowest_pn -> %d: %d",
 711                    drv->ifname, sa->an, sa->next_pn);
 712
 713         msg = msg_prepare(MACSEC_CMD_UPD_RXSA, ctx, drv->ifi);
 714         if (!msg)
 715                 return ret;
 716
 717         nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
 718         if (!nest)
 719                 goto nla_put_failure;
 720
 721         NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
 722         NLA_PUT_U32(msg, MACSEC_SA_ATTR_PN, sa->next_pn);
 723
 724         nla_nest_end(msg, nest);
 725
 726         ret = nl_send_recv(ctx->sk, msg);
 727         if (ret < 0) {
 728                 wpa_printf(MSG_ERROR,
 729                            DRV_PREFIX "failed to communicate: %d (%s)",
 730                            ret, nl_geterror(-ret));
 731         }
 732
 733 nla_put_failure:
 734         nlmsg_free(msg);
 735         return ret;
 736 }
 737
 738
 739 /**
 740  * macsec_drv_get_transmit_next_pn - Get transmit next PN
 741  * @priv: Private driver interface data
 742  * @sa: secure association
 743  * Returns: 0 on success, -1 on failure (or if not supported)
 744  */
 745 static int macsec_drv_get_transmit_next_pn(void *priv, struct transmit_sa *sa)
 746 {
 747         struct macsec_drv_data *drv = priv;
 748         int err;
 749
 750         wpa_printf(MSG_DEBUG, "%s", __func__);
 751
 752         err = do_dump(drv, sa->an, UNUSED_SCI, 0xff, &sa->next_pn);
 753         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: err %d result %d", __func__, err,
 754                    sa->next_pn);
 755         return err;
 756 }
 757
 758
 759 /**
 760  * macsec_drv_set_transmit_next_pn - Set transmit next pn
 761  * @priv: Private driver interface data
 762  * @sa: secure association
 763  * Returns: 0 on success, -1 on failure (or if not supported)
 764  */
 765 static int macsec_drv_set_transmit_next_pn(void *priv, struct transmit_sa *sa)
 766 {
 767         struct macsec_drv_data *drv = priv;
 768         struct macsec_genl_ctx *ctx = &drv->ctx;
 769         struct nl_msg *msg;
 770         struct nlattr *nest;
 771         int ret = -1;
 772
 773         wpa_printf(MSG_DEBUG, "%s -> %d: %d", __func__, sa->an, sa->next_pn);
 774
 775         msg = msg_prepare(MACSEC_CMD_UPD_TXSA, ctx, drv->ifi);
 776         if (!msg)
 777                 return ret;
 778
 779         nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
 780         if (!nest)
 781                 goto nla_put_failure;
 782
 783         NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
 784         NLA_PUT_U32(msg, MACSEC_SA_ATTR_PN, sa->next_pn);
 785
 786         nla_nest_end(msg, nest);
 787
 788         ret = nl_send_recv(ctx->sk, msg);
 789         if (ret < 0) {
 790                 wpa_printf(MSG_ERROR,
 791                            DRV_PREFIX "failed to communicate: %d (%s)",
 792                            ret, nl_geterror(-ret));
 793         }
 794
 795 nla_put_failure:
 796         nlmsg_free(msg);
 797         return ret;
 798 }
 799
 800
 801 #define SCISTR MACSTR "::%hx"
 802 #define SCI2STR(addr, port) MAC2STR(addr), htons(port)
 803
 804 /**
 805  * macsec_drv_create_receive_sc - Create secure channel for receiving
 806  * @priv: Private driver interface data
 807  * @sc: secure channel
 808  * @sci_addr: secure channel identifier - address
 809  * @sci_port: secure channel identifier - port
 810  * @conf_offset: confidentiality offset (0, 30, or 50)
 811  * @validation: frame validation policy (0 = Disabled, 1 = Checked,
 812  *      2 = Strict)
 813  * Returns: 0 on success, -1 on failure (or if not supported)
 814  */
 815 static int macsec_drv_create_receive_sc(void *priv, struct receive_sc *sc,
 816                                         unsigned int conf_offset,
 817                                         int validation)
 818 {
 819         struct macsec_drv_data *drv = priv;
 820         struct macsec_genl_ctx *ctx = &drv->ctx;
 821         struct nl_msg *msg;
 822         int ret = -1;
 823
 824         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: create_receive_sc -> " SCISTR
 825                    " (conf_offset=%u validation=%d)",
 826                    drv->ifname, SCI2STR(sc->sci.addr, sc->sci.port),
 827                    conf_offset, validation);
 828
 829         msg = msg_prepare(MACSEC_CMD_ADD_RXSC, ctx, drv->ifi);
 830         if (!msg)
 831                 return ret;
 832
 833         if (nla_put_rxsc_config(msg, mka_sci_u64(&sc->sci)))
 834                 goto nla_put_failure;
 835
 836         ret = nl_send_recv(ctx->sk, msg);
 837         if (ret < 0) {
 838                 wpa_printf(MSG_ERROR,
 839                            DRV_PREFIX "%s: failed to communicate: %d (%s)",
 840                            __func__, ret, nl_geterror(-ret));
 841         }
 842
 843 nla_put_failure:
 844         nlmsg_free(msg);
 845         return ret;
 846 }
 847
 848
 849 /**
 850  * macsec_drv_delete_receive_sc - Delete secure connection for receiving
 851  * @priv: private driver interface data from init()
 852  * @sc: secure channel
 853  * Returns: 0 on success, -1 on failure
 854  */
 855 static int macsec_drv_delete_receive_sc(void *priv, struct receive_sc *sc)
 856 {
 857         struct macsec_drv_data *drv = priv;
 858         struct macsec_genl_ctx *ctx = &drv->ctx;
 859         struct nl_msg *msg;
 860         int ret = -1;
 861
 862         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: delete_receive_sc -> " SCISTR,
 863                    drv->ifname, SCI2STR(sc->sci.addr, sc->sci.port));
 864
 865         msg = msg_prepare(MACSEC_CMD_DEL_RXSC, ctx, drv->ifi);
 866         if (!msg)
 867                 return ret;
 868
 869         if (nla_put_rxsc_config(msg, mka_sci_u64(&sc->sci)))
 870                 goto nla_put_failure;
 871
 872         ret = nl_send_recv(ctx->sk, msg);
 873         if (ret < 0) {
 874                 wpa_printf(MSG_ERROR,
 875                            DRV_PREFIX "%s: failed to communicate: %d (%s)",
 876                            __func__, ret, nl_geterror(-ret));
 877         }
 878
 879 nla_put_failure:
 880         nlmsg_free(msg);
 881         return ret;
 882 }
 883
 884
 885 /**
 886  * macsec_drv_create_receive_sa - Create secure association for receive
 887  * @priv: private driver interface data from init()
 888  * @sa: secure association
 889  * Returns: 0 on success, -1 on failure
 890  */
 891 static int macsec_drv_create_receive_sa(void *priv, struct receive_sa *sa)
 892 {
 893         struct macsec_drv_data *drv = priv;
 894         struct macsec_genl_ctx *ctx = &drv->ctx;
 895         struct nl_msg *msg;
 896         struct nlattr *nest;
 897         int ret = -1;
 898
 899         wpa_printf(MSG_DEBUG,
 900                    DRV_PREFIX "%s: create_receive_sa -> %d on " SCISTR
 901                    " (enable_receive=%d next_pn=%u)",
 902                    drv->ifname, sa->an,
 903                    SCI2STR(sa->sc->sci.addr, sa->sc->sci.port),
 904                    sa->enable_receive, sa->next_pn);
 905         wpa_hexdump(MSG_DEBUG, DRV_PREFIX "SA keyid",
 906                     &sa->pkey->key_identifier,
 907                     sizeof(sa->pkey->key_identifier));
 908         wpa_hexdump_key(MSG_DEBUG, DRV_PREFIX "SA key",
 909                         sa->pkey->key, sa->pkey->key_len);
 910
 911         msg = msg_prepare(MACSEC_CMD_ADD_RXSA, ctx, drv->ifi);
 912         if (!msg)
 913                 return ret;
 914
 915         if (nla_put_rxsc_config(msg, mka_sci_u64(&sa->sc->sci)))
 916                 goto nla_put_failure;
 917
 918         nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
 919         if (!nest)
 920                 goto nla_put_failure;
 921
 922         NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
 923         NLA_PUT_U8(msg, MACSEC_SA_ATTR_ACTIVE, sa->enable_receive);
 924         NLA_PUT_U32(msg, MACSEC_SA_ATTR_PN, sa->next_pn);
 925         NLA_PUT(msg, MACSEC_SA_ATTR_KEYID, sizeof(sa->pkey->key_identifier),
 926                 &sa->pkey->key_identifier);
 927         NLA_PUT(msg, MACSEC_SA_ATTR_KEY, sa->pkey->key_len, sa->pkey->key);
 928
 929         nla_nest_end(msg, nest);
 930
 931         ret = nl_send_recv(ctx->sk, msg);
 932         if (ret < 0) {
 933                 wpa_printf(MSG_ERROR,
 934                            DRV_PREFIX "%s: failed to communicate: %d (%s)",
 935                            __func__, ret, nl_geterror(-ret));
 936         }
 937
 938 nla_put_failure:
 939         nlmsg_free(msg);
 940         return ret;
 941 }
 942
 943
 944 /**
 945  * macsec_drv_delete_receive_sa - Delete secure association for receive
 946  * @priv: private driver interface data from init()
 947  * @sa: secure association
 948  * Returns: 0 on success, -1 on failure
 949  */
 950 static int macsec_drv_delete_receive_sa(void *priv, struct receive_sa *sa)
 951 {
 952         struct macsec_drv_data *drv = priv;
 953         struct macsec_genl_ctx *ctx = &drv->ctx;
 954         struct nl_msg *msg;
 955         struct nlattr *nest;
 956         int ret = -1;
 957
 958         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: delete_receive_sa -> %d on "
 959                    SCISTR, drv->ifname, sa->an,
 960                    SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
 961
 962         msg = msg_prepare(MACSEC_CMD_DEL_RXSA, ctx, drv->ifi);
 963         if (!msg)
 964                 return ret;
 965
 966         if (nla_put_rxsc_config(msg, mka_sci_u64(&sa->sc->sci)))
 967                 goto nla_put_failure;
 968
 969         nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
 970         if (!nest)
 971                 goto nla_put_failure;
 972
 973         NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
 974
 975         nla_nest_end(msg, nest);
 976
 977         ret = nl_send_recv(ctx->sk, msg);
 978         if (ret < 0) {
 979                 wpa_printf(MSG_ERROR,
 980                            DRV_PREFIX "%s: failed to communicate: %d (%s)",
 981                            __func__, ret, nl_geterror(-ret));
 982         }
 983
 984 nla_put_failure:
 985         nlmsg_free(msg);
 986         return ret;
 987 }
 988
 989
 990 static int set_active_rx_sa(const struct macsec_genl_ctx *ctx, int ifindex,
 991                             u64 sci, unsigned char an, Boolean state)
 992 {
 993         struct nl_msg *msg;
 994         struct nlattr *nest;
 995         int ret = -1;
 996
 997         msg = msg_prepare(MACSEC_CMD_UPD_RXSA, ctx, ifindex);
 998         if (!msg)
 999                 return ret;
1000
1001         if (nla_put_rxsc_config(msg, sci))
1002                 goto nla_put_failure;
1003
1004         nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
1005         if (!nest)
1006                 goto nla_put_failure;
1007
1008         NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, an);
1009         NLA_PUT_U8(msg, MACSEC_SA_ATTR_ACTIVE, !!state);
1010
1011         nla_nest_end(msg, nest);
1012
1013         ret = nl_send_recv(ctx->sk, msg);
1014         if (ret < 0)
1015                 wpa_printf(MSG_ERROR,
1016                            DRV_PREFIX "%s: failed to communicate: %d (%s)",
1017                            __func__, ret, nl_geterror(-ret));
1018
1019 nla_put_failure:
1020         nlmsg_free(msg);
1021         return ret;
1022 }
1023
1024
1025 /**
1026  * macsec_drv_enable_receive_sa - Enable the SA for receive
1027  * @priv: private driver interface data from init()
1028  * @sa: secure association
1029  * Returns: 0 on success, -1 on failure
1030  */
1031 static int macsec_drv_enable_receive_sa(void *priv, struct receive_sa *sa)
1032 {
1033         struct macsec_drv_data *drv = priv;
1034         struct macsec_genl_ctx *ctx = &drv->ctx;
1035
1036         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: enable_receive_sa -> %d on "
1037                    SCISTR, drv->ifname, sa->an,
1038                    SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1039
1040         return set_active_rx_sa(ctx, drv->ifi, mka_sci_u64(&sa->sc->sci),
1041                                 sa->an, TRUE);
1042 }
1043
1044
1045 /**
1046  * macsec_drv_disable_receive_sa - Disable SA for receive
1047  * @priv: private driver interface data from init()
1048  * @sa: secure association
1049  * Returns: 0 on success, -1 on failure
1050  */
1051 static int macsec_drv_disable_receive_sa(void *priv, struct receive_sa *sa)
1052 {
1053         struct macsec_drv_data *drv = priv;
1054         struct macsec_genl_ctx *ctx = &drv->ctx;
1055
1056         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: disable_receive_sa -> %d on "
1057                    SCISTR, drv->ifname, sa->an,
1058                    SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1059
1060         return set_active_rx_sa(ctx, drv->ifi, mka_sci_u64(&sa->sc->sci),
1061                                 sa->an, FALSE);
1062 }
1063
1064
1065 static struct rtnl_link * lookup_sc(struct nl_cache *cache, int parent, u64 sci)
1066 {
1067         struct rtnl_link *needle;
1068         void *match;
1069
1070         needle = rtnl_link_macsec_alloc();
1071         if (!needle)
1072                 return NULL;
1073
1074         rtnl_link_set_link(needle, parent);
1075         rtnl_link_macsec_set_sci(needle, sci);
1076
1077         match = nl_cache_find(cache, (struct nl_object *) needle);
1078         rtnl_link_put(needle);
1079
1080         return (struct rtnl_link *) match;
1081 }
1082
1083
1084 /**
1085  * macsec_drv_create_transmit_sc - Create secure connection for transmit
1086  * @priv: private driver interface data from init()
1087  * @sc: secure channel
1088  * @conf_offset: confidentiality offset
1089  * Returns: 0 on success, -1 on failure
1090  */
1091 static int macsec_drv_create_transmit_sc(
1092         void *priv, struct transmit_sc *sc,
1093         unsigned int conf_offset)
1094 {
1095         struct macsec_drv_data *drv = priv;
1096         struct rtnl_link *link;
1097         char *ifname;
1098         u64 sci;
1099         int err;
1100
1101         wpa_printf(MSG_DEBUG, DRV_PREFIX
1102                    "%s: create_transmit_sc -> " SCISTR " (conf_offset=%d)",
1103                    drv->common.ifname, SCI2STR(sc->sci.addr, sc->sci.port),
1104                    conf_offset);
1105
1106         if (!drv->sk) {
1107                 wpa_printf(MSG_ERROR, DRV_PREFIX "NULL rtnl socket");
1108                 return -1;
1109         }
1110
1111         link = rtnl_link_macsec_alloc();
1112         if (!link) {
1113                 wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't allocate link");
1114                 return -1;
1115         }
1116
1117         rtnl_link_set_link(link, drv->parent_ifi);
1118
1119         sci = mka_sci_u64(&sc->sci);
1120         rtnl_link_macsec_set_sci(link, sci);
1121
1122         drv->created_link = TRUE;
1123
1124         err = rtnl_link_add(drv->sk, link, NLM_F_CREATE);
1125         if (err == -NLE_BUSY) {
1126                 wpa_printf(MSG_INFO,
1127                            DRV_PREFIX "link already exists, using it");
1128                 drv->created_link = FALSE;
1129         } else if (err < 0) {
1130                 rtnl_link_put(link);
1131                 wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't create link: err %d",
1132                            err);
1133                 return err;
1134         }
1135
1136         rtnl_link_put(link);
1137
1138         nl_cache_refill(drv->sk, drv->link_cache);
1139         link = lookup_sc(drv->link_cache, drv->parent_ifi, sci);
1140         if (!link) {
1141                 wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't find link");
1142                 return -1;
1143         }
1144
1145         drv->ifi = rtnl_link_get_ifindex(link);
1146         ifname = rtnl_link_get_name(link);
1147         wpa_printf(MSG_DEBUG,
1148                    DRV_PREFIX "%s: create_transmit_sc: ifi=%d ifname=%s",
1149                    drv->common.ifname, drv->ifi, ifname);
1150         os_strlcpy(drv->ifname, ifname, sizeof(drv->ifname));
1151         rtnl_link_put(link);
1152
1153         drv->link = rtnl_link_macsec_alloc();
1154         if (!drv->link) {
1155                 wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't allocate link");
1156                 return -1;
1157         }
1158
1159         rtnl_link_set_name(drv->link, drv->ifname);
1160
1161         /* In case some settings have already been done but we couldn't apply
1162          * them. */
1163         return try_commit(drv);
1164 }
1165
1166
1167 /**
1168  * macsec_drv_delete_transmit_sc - Delete secure connection for transmit
1169  * @priv: private driver interface data from init()
1170  * @sc: secure channel
1171  * Returns: 0 on success, -1 on failure
1172  */
1173 static int macsec_drv_delete_transmit_sc(void *priv, struct transmit_sc *sc)
1174 {
1175         struct macsec_drv_data *drv = priv;
1176         int err;
1177
1178         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: delete_transmit_sc -> " SCISTR,
1179                    drv->ifname, SCI2STR(sc->sci.addr, sc->sci.port));
1180
1181         if (!drv->sk)
1182                 return 0;
1183
1184         if (!drv->created_link) {
1185                 rtnl_link_put(drv->link);
1186                 drv->link = NULL;
1187                 wpa_printf(MSG_DEBUG, DRV_PREFIX
1188                            "we didn't create the link, leave it alone");
1189                 return 0;
1190         }
1191
1192         err = rtnl_link_delete(drv->sk, drv->link);
1193         if (err < 0)
1194                 wpa_printf(MSG_ERROR, DRV_PREFIX "couldn't delete link");
1195         rtnl_link_put(drv->link);
1196         drv->link = NULL;
1197
1198         return err;
1199 }
1200
1201
1202 /**
1203  * macsec_drv_create_transmit_sa - Create secure association for transmit
1204  * @priv: private driver interface data from init()
1205  * @sa: secure association
1206  * Returns: 0 on success, -1 on failure
1207  */
1208 static int macsec_drv_create_transmit_sa(void *priv, struct transmit_sa *sa)
1209 {
1210         struct macsec_drv_data *drv = priv;
1211         struct macsec_genl_ctx *ctx = &drv->ctx;
1212         struct nl_msg *msg;
1213         struct nlattr *nest;
1214         int ret = -1;
1215
1216         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: create_transmit_sa -> %d on "
1217                    SCISTR " (enable_transmit=%d next_pn=%u)",
1218                    drv->ifname, sa->an,
1219                    SCI2STR(sa->sc->sci.addr, sa->sc->sci.port),
1220                    sa->enable_transmit, sa->next_pn);
1221         wpa_hexdump(MSG_DEBUG, DRV_PREFIX "SA keyid",
1222                     &sa->pkey->key_identifier,
1223                     sizeof(sa->pkey->key_identifier));
1224         wpa_hexdump_key(MSG_DEBUG, DRV_PREFIX "SA key",
1225                         sa->pkey->key, sa->pkey->key_len);
1226
1227         msg = msg_prepare(MACSEC_CMD_ADD_TXSA, ctx, drv->ifi);
1228         if (!msg)
1229                 return ret;
1230
1231         nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
1232         if (!nest)
1233                 goto nla_put_failure;
1234
1235         NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
1236         NLA_PUT_U32(msg, MACSEC_SA_ATTR_PN, sa->next_pn);
1237         NLA_PUT(msg, MACSEC_SA_ATTR_KEYID, sizeof(sa->pkey->key_identifier),
1238                 &sa->pkey->key_identifier);
1239         NLA_PUT(msg, MACSEC_SA_ATTR_KEY, sa->pkey->key_len, sa->pkey->key);
1240         NLA_PUT_U8(msg, MACSEC_SA_ATTR_ACTIVE, sa->enable_transmit);
1241
1242         nla_nest_end(msg, nest);
1243
1244         ret = nl_send_recv(ctx->sk, msg);
1245         if (ret < 0) {
1246                 wpa_printf(MSG_ERROR,
1247                            DRV_PREFIX "%s: failed to communicate: %d (%s)",
1248                            __func__, ret, nl_geterror(-ret));
1249         }
1250
1251 nla_put_failure:
1252         nlmsg_free(msg);
1253         return ret;
1254 }
1255
1256
1257 /**
1258  * macsec_drv_delete_transmit_sa - Delete secure association for transmit
1259  * @priv: private driver interface data from init()
1260  * @sa: secure association
1261  * Returns: 0 on success, -1 on failure
1262  */
1263 static int macsec_drv_delete_transmit_sa(void *priv, struct transmit_sa *sa)
1264 {
1265         struct macsec_drv_data *drv = priv;
1266         struct macsec_genl_ctx *ctx = &drv->ctx;
1267         struct nl_msg *msg;
1268         struct nlattr *nest;
1269         int ret = -1;
1270
1271         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: delete_transmit_sa -> %d on "
1272                    SCISTR, drv->ifname, sa->an,
1273                    SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1274
1275         msg = msg_prepare(MACSEC_CMD_DEL_TXSA, ctx, drv->ifi);
1276         if (!msg)
1277                 return ret;
1278
1279         nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
1280         if (!nest)
1281                 goto nla_put_failure;
1282
1283         NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, sa->an);
1284
1285         nla_nest_end(msg, nest);
1286
1287         ret = nl_send_recv(ctx->sk, msg);
1288         if (ret < 0) {
1289                 wpa_printf(MSG_ERROR,
1290                            DRV_PREFIX "%s: failed to communicate: %d (%s)",
1291                            __func__, ret, nl_geterror(-ret));
1292         }
1293
1294 nla_put_failure:
1295         nlmsg_free(msg);
1296         return ret;
1297 }
1298
1299
1300 static int set_active_tx_sa(const struct macsec_genl_ctx *ctx, int ifindex,
1301                             unsigned char an, Boolean state)
1302 {
1303         struct nl_msg *msg;
1304         struct nlattr *nest;
1305         int ret = -1;
1306
1307         msg = msg_prepare(MACSEC_CMD_UPD_TXSA, ctx, ifindex);
1308         if (!msg)
1309                 return ret;
1310
1311         nest = nla_nest_start(msg, MACSEC_ATTR_SA_CONFIG);
1312         if (!nest)
1313                 goto nla_put_failure;
1314
1315         NLA_PUT_U8(msg, MACSEC_SA_ATTR_AN, an);
1316         NLA_PUT_U8(msg, MACSEC_SA_ATTR_ACTIVE, !!state);
1317
1318         nla_nest_end(msg, nest);
1319
1320         ret = nl_send_recv(ctx->sk, msg);
1321         if (ret < 0) {
1322                 wpa_printf(MSG_ERROR,
1323                            DRV_PREFIX "%s: failed to communicate: %d (%s)",
1324                            __func__, ret, nl_geterror(-ret));
1325         }
1326
1327 nla_put_failure:
1328         nlmsg_free(msg);
1329         return ret;
1330 }
1331
1332
1333 /**
1334  * macsec_drv_enable_transmit_sa - Enable SA for transmit
1335  * @priv: private driver interface data from init()
1336  * @sa: secure association
1337  * Returns: 0 on success, -1 on failure
1338  */
1339 static int macsec_drv_enable_transmit_sa(void *priv, struct transmit_sa *sa)
1340 {
1341         struct macsec_drv_data *drv = priv;
1342         struct macsec_genl_ctx *ctx = &drv->ctx;
1343         int ret;
1344
1345         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: enable_transmit_sa -> %d on "
1346                    SCISTR, drv->ifname, sa->an,
1347                    SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1348
1349         ret = set_active_tx_sa(ctx, drv->ifi, sa->an, TRUE);
1350         if (ret < 0) {
1351                 wpa_printf(MSG_ERROR, DRV_PREFIX "failed to enable txsa");
1352                 return ret;
1353         }
1354
1355         drv->encoding_sa_set = TRUE;
1356         drv->encoding_sa = sa->an;
1357
1358         return try_commit(drv);
1359 }
1360
1361
1362 /**
1363  * macsec_drv_disable_transmit_sa - Disable SA for transmit
1364  * @priv: private driver interface data from init()
1365  * @sa: secure association
1366  * Returns: 0 on success, -1 on failure
1367  */
1368 static int macsec_drv_disable_transmit_sa(void *priv, struct transmit_sa *sa)
1369 {
1370         struct macsec_drv_data *drv = priv;
1371         struct macsec_genl_ctx *ctx = &drv->ctx;
1372
1373         wpa_printf(MSG_DEBUG, DRV_PREFIX "%s: disable_transmit_sa -> %d on "
1374                    SCISTR, drv->ifname, sa->an,
1375                    SCI2STR(sa->sc->sci.addr, sa->sc->sci.port));
1376
1377         return set_active_tx_sa(ctx, drv->ifi, sa->an, FALSE);
1378 }
1379
1380
1381 static int macsec_drv_status(void *priv, char *buf, size_t buflen)
1382 {
1383         struct macsec_drv_data *drv = priv;
1384         int res;
1385         char *pos, *end;
1386
1387         pos = buf;
1388         end = buf + buflen;
1389
1390         res = os_snprintf(pos, end - pos,
1391                           "ifname=%s\n"
1392                           "ifi=%d\n"
1393                           "parent_ifname=%s\n"
1394                           "parent_ifi=%d\n",
1395                           drv->common.ifname, drv->ifi,
1396                           drv->ifname, drv->parent_ifi);
1397         if (os_snprintf_error(end - pos, res))
1398                 return pos - buf;
1399         pos += res;
1400
1401         return pos - buf;
1402 }
1403
1404
1405 #ifdef __linux__
1406
1407 static void macsec_drv_handle_data(void *ctx, unsigned char *buf, size_t len)
1408 {
1409 #ifdef HOSTAPD
1410         struct ieee8023_hdr *hdr;
1411         u8 *pos, *sa;
1412         size_t left;
1413         union wpa_event_data event;
1414
1415         /* must contain at least ieee8023_hdr 6 byte source, 6 byte dest,
1416          * 2 byte ethertype */
1417         if (len < 14) {
1418                 wpa_printf(MSG_MSGDUMP, "%s: too short (%lu)",
1419                            __func__, (unsigned long) len);
1420                 return;
1421         }
1422
1423         hdr = (struct ieee8023_hdr *) buf;
1424
1425         switch (ntohs(hdr->ethertype)) {
1426         case ETH_P_PAE:
1427                 wpa_printf(MSG_MSGDUMP, "Received EAPOL packet");
1428                 sa = hdr->src;
1429                 os_memset(&event, 0, sizeof(event));
1430                 event.new_sta.addr = sa;
1431                 wpa_supplicant_event(ctx, EVENT_NEW_STA, &event);
1432
1433                 pos = (u8 *) (hdr + 1);
1434                 left = len - sizeof(*hdr);
1435                 drv_event_eapol_rx(ctx, sa, pos, left);
1436                 break;
1437
1438         default:
1439                 wpa_printf(MSG_DEBUG, "Unknown ethertype 0x%04x in data frame",
1440                            ntohs(hdr->ethertype));
1441                 break;
1442         }
1443 #endif /* HOSTAPD */
1444 }
1445
1446
1447 static void macsec_drv_handle_read(int sock, void *eloop_ctx, void *sock_ctx)
1448 {
1449         int len;
1450         unsigned char buf[3000];
1451
1452         len = recv(sock, buf, sizeof(buf), 0);
1453         if (len < 0) {
1454                 wpa_printf(MSG_ERROR, "macsec_linux: recv: %s",
1455                            strerror(errno));
1456                 return;
1457         }
1458
1459         macsec_drv_handle_data(eloop_ctx, buf, len);
1460 }
1461
1462 #endif /* __linux__ */
1463
1464
1465 static int macsec_drv_init_sockets(struct macsec_drv_data *drv, u8 *own_addr)
1466 {
1467 #ifdef __linux__
1468         struct ifreq ifr;
1469         struct sockaddr_ll addr;
1470
1471         drv->common.sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PAE));
1472         if (drv->common.sock < 0) {
1473                 wpa_printf(MSG_ERROR, "socket[PF_PACKET,SOCK_RAW]: %s",
1474                            strerror(errno));
1475                 return -1;
1476         }
1477
1478         if (eloop_register_read_sock(drv->common.sock, macsec_drv_handle_read,
1479                                      drv->common.ctx, NULL)) {
1480                 wpa_printf(MSG_INFO, "Could not register read socket");
1481                 return -1;
1482         }
1483
1484         os_memset(&ifr, 0, sizeof(ifr));
1485         os_strlcpy(ifr.ifr_name, drv->common.ifname, sizeof(ifr.ifr_name));
1486         if (ioctl(drv->common.sock, SIOCGIFINDEX, &ifr) != 0) {
1487                 wpa_printf(MSG_ERROR, "ioctl(SIOCGIFINDEX): %s",
1488                            strerror(errno));
1489                 return -1;
1490         }
1491
1492         os_memset(&addr, 0, sizeof(addr));
1493         addr.sll_family = AF_PACKET;
1494         addr.sll_ifindex = ifr.ifr_ifindex;
1495         wpa_printf(MSG_DEBUG, "Opening raw packet socket for ifindex %d",
1496                    addr.sll_ifindex);
1497
1498         if (bind(drv->common.sock, (struct sockaddr *) &addr, sizeof(addr)) < 0)
1499         {
1500                 wpa_printf(MSG_ERROR, "bind: %s", strerror(errno));
1501                 return -1;
1502         }
1503
1504         /* filter multicast address */
1505         if (wired_multicast_membership(drv->common.sock, ifr.ifr_ifindex,
1506                                        pae_group_addr, 1) < 0) {
1507                 wpa_printf(MSG_ERROR, "wired: Failed to add multicast group "
1508                            "membership");
1509                 return -1;
1510         }
1511
1512         os_memset(&ifr, 0, sizeof(ifr));
1513         os_strlcpy(ifr.ifr_name, drv->common.ifname, sizeof(ifr.ifr_name));
1514         if (ioctl(drv->common.sock, SIOCGIFHWADDR, &ifr) != 0) {
1515                 wpa_printf(MSG_ERROR, "ioctl(SIOCGIFHWADDR): %s",
1516                            strerror(errno));
1517                 return -1;
1518         }
1519
1520         if (ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER) {
1521                 wpa_printf(MSG_INFO, "Invalid HW-addr family 0x%04x",
1522                            ifr.ifr_hwaddr.sa_family);
1523                 return -1;
1524         }
1525         os_memcpy(own_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
1526
1527         return 0;
1528 #else /* __linux__ */
1529         return -1;
1530 #endif /* __linux__ */
1531 }
1532
1533
1534 static void * macsec_drv_hapd_init(struct hostapd_data *hapd,
1535                                    struct wpa_init_params *params)
1536 {
1537         struct macsec_drv_data *drv;
1538
1539         drv = os_zalloc(sizeof(struct macsec_drv_data));
1540         if (drv == NULL) {
1541                 wpa_printf(MSG_INFO,
1542                            "Could not allocate memory for wired driver data");
1543                 return NULL;
1544         }
1545
1546         drv->common.ctx = hapd;
1547         os_strlcpy(drv->common.ifname, params->ifname,
1548                    sizeof(drv->common.ifname));
1549         drv->use_pae_group_addr = params->use_pae_group_addr;
1550
1551         if (macsec_drv_init_sockets(drv, params->own_addr)) {
1552                 os_free(drv);
1553                 return NULL;
1554         }
1555
1556         return drv;
1557 }
1558
1559
1560 static void macsec_drv_hapd_deinit(void *priv)
1561 {
1562         struct macsec_drv_data *drv = priv;
1563
1564         if (drv->common.sock >= 0) {
1565                 eloop_unregister_read_sock(drv->common.sock);
1566                 close(drv->common.sock);
1567         }
1568
1569         os_free(drv);
1570 }
1571
1572
1573 static int macsec_drv_send_eapol(void *priv, const u8 *addr,
1574                                  const u8 *data, size_t data_len, int encrypt,
1575                                  const u8 *own_addr, u32 flags)
1576 {
1577         struct macsec_drv_data *drv = priv;
1578         struct ieee8023_hdr *hdr;
1579         size_t len;
1580         u8 *pos;
1581         int res;
1582
1583         len = sizeof(*hdr) + data_len;
1584         hdr = os_zalloc(len);
1585         if (hdr == NULL) {
1586                 wpa_printf(MSG_INFO,
1587                            "%s: malloc() failed (len=%lu)",
1588                            __func__, (unsigned long) len);
1589                 return -1;
1590         }
1591
1592         os_memcpy(hdr->dest, drv->use_pae_group_addr ? pae_group_addr : addr,
1593                   ETH_ALEN);
1594         os_memcpy(hdr->src, own_addr, ETH_ALEN);
1595         hdr->ethertype = htons(ETH_P_PAE);
1596
1597         pos = (u8 *) (hdr + 1);
1598         os_memcpy(pos, data, data_len);
1599
1600         res = send(drv->common.sock, (u8 *) hdr, len, 0);
1601         os_free(hdr);
1602
1603         if (res < 0) {
1604                 wpa_printf(MSG_ERROR,
1605                            "%s: packet len: %lu - failed: send: %s",
1606                            __func__, (unsigned long) len, strerror(errno));
1607         }
1608
1609         return res;
1610 }
1611
1612
1613 const struct wpa_driver_ops wpa_driver_macsec_linux_ops = {
1614         .name = "macsec_linux",
1615         .desc = "MACsec Ethernet driver for Linux",
1616         .get_ssid = driver_wired_get_ssid,
1617         .get_bssid = driver_wired_get_bssid,
1618         .get_capa = driver_wired_get_capa,
1619         .init = macsec_drv_wpa_init,
1620         .deinit = macsec_drv_wpa_deinit,
1621         .hapd_init = macsec_drv_hapd_init,
1622         .hapd_deinit = macsec_drv_hapd_deinit,
1623         .hapd_send_eapol = macsec_drv_send_eapol,
1624
1625         .macsec_init = macsec_drv_macsec_init,
1626         .macsec_deinit = macsec_drv_macsec_deinit,
1627         .macsec_get_capability = macsec_drv_get_capability,
1628         .enable_protect_frames = macsec_drv_enable_protect_frames,
1629         .enable_encrypt = macsec_drv_enable_encrypt,
1630         .set_replay_protect = macsec_drv_set_replay_protect,
1631         .set_current_cipher_suite = macsec_drv_set_current_cipher_suite,
1632         .enable_controlled_port = macsec_drv_enable_controlled_port,
1633         .get_receive_lowest_pn = macsec_drv_get_receive_lowest_pn,
1634         .set_receive_lowest_pn = macsec_drv_set_receive_lowest_pn,
1635         .get_transmit_next_pn = macsec_drv_get_transmit_next_pn,
1636         .set_transmit_next_pn = macsec_drv_set_transmit_next_pn,
1637         .create_receive_sc = macsec_drv_create_receive_sc,
1638         .delete_receive_sc = macsec_drv_delete_receive_sc,
1639         .create_receive_sa = macsec_drv_create_receive_sa,
1640         .delete_receive_sa = macsec_drv_delete_receive_sa,
1641         .enable_receive_sa = macsec_drv_enable_receive_sa,
1642         .disable_receive_sa = macsec_drv_disable_receive_sa,
1643         .create_transmit_sc = macsec_drv_create_transmit_sc,
1644         .delete_transmit_sc = macsec_drv_delete_transmit_sc,
1645         .create_transmit_sa = macsec_drv_create_transmit_sa,
1646         .delete_transmit_sa = macsec_drv_delete_transmit_sa,
1647         .enable_transmit_sa = macsec_drv_enable_transmit_sa,
1648         .disable_transmit_sa = macsec_drv_disable_transmit_sa,
1649
1650         .status = macsec_drv_status,
1651 };