drivers/net/macsec.c

   1 /*
   2  * drivers/net/macsec.c - MACsec device
   3  *
   4  * Copyright (c) 2015 Sabrina Dubroca <sd@queasysnail.net>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  */
  11
  12 #include <linux/types.h>
  13 #include <linux/skbuff.h>
  14 #include <linux/socket.h>
  15 #include <linux/module.h>
  16 #include <crypto/aead.h>
  17 #include <linux/etherdevice.h>
  18 #include <linux/rtnetlink.h>
  19 #include <linux/refcount.h>
  20 #include <net/genetlink.h>
  21 #include <net/sock.h>
  22 #include <net/gro_cells.h>
  23 #include <linux/if_arp.h>
  24
  25 #include <uapi/linux/if_macsec.h>
  26
  27 typedef u64 __bitwise sci_t;
  28
  29 #define MACSEC_SCI_LEN 8
  30
  31 /* SecTAG length = macsec_eth_header without the optional SCI */
  32 #define MACSEC_TAG_LEN 6
  33
  34 struct macsec_eth_header {
  35         struct ethhdr eth;
  36         /* SecTAG */
  37         u8  tci_an;
  38 #if defined(__LITTLE_ENDIAN_BITFIELD)
  39         u8  short_length:6,
  40                   unused:2;
  41 #elif defined(__BIG_ENDIAN_BITFIELD)
  42         u8        unused:2,
  43             short_length:6;
  44 #else
  45 #error  "Please fix <asm/byteorder.h>"
  46 #endif
  47         __be32 packet_number;
  48         u8 secure_channel_id[8]; /* optional */
  49 } __packed;
  50
  51 #define MACSEC_TCI_VERSION 0x80
  52 #define MACSEC_TCI_ES      0x40 /* end station */
  53 #define MACSEC_TCI_SC      0x20 /* SCI present */
  54 #define MACSEC_TCI_SCB     0x10 /* epon */
  55 #define MACSEC_TCI_E       0x08 /* encryption */
  56 #define MACSEC_TCI_C       0x04 /* changed text */
  57 #define MACSEC_AN_MASK     0x03 /* association number */
  58 #define MACSEC_TCI_CONFID  (MACSEC_TCI_E | MACSEC_TCI_C)
  59
  60 /* minimum secure data length deemed "not short", see IEEE 802.1AE-2006 9.7 */
  61 #define MIN_NON_SHORT_LEN 48
  62
  63 #define GCM_AES_IV_LEN 12
  64 #define DEFAULT_ICV_LEN 16
  65
  66 #define MACSEC_NUM_AN 4 /* 2 bits for the association number */
  67
  68 #define for_each_rxsc(secy, sc)                 \
  69         for (sc = rcu_dereference_bh(secy->rx_sc);      \
  70              sc;                                \
  71              sc = rcu_dereference_bh(sc->next))
  72 #define for_each_rxsc_rtnl(secy, sc)                    \
  73         for (sc = rtnl_dereference(secy->rx_sc);        \
  74              sc;                                        \
  75              sc = rtnl_dereference(sc->next))
  76
  77 struct gcm_iv {
  78         union {
  79                 u8 secure_channel_id[8];
  80                 sci_t sci;
  81         };
  82         __be32 pn;
  83 };
  84
  85 /**
  86  * struct macsec_key - SA key
  87  * @id: user-provided key identifier
  88  * @tfm: crypto struct, key storage
  89  */
  90 struct macsec_key {
  91         u8 id[MACSEC_KEYID_LEN];
  92         struct crypto_aead *tfm;
  93 };
  94
  95 struct macsec_rx_sc_stats {
  96         __u64 InOctetsValidated;
  97         __u64 InOctetsDecrypted;
  98         __u64 InPktsUnchecked;
  99         __u64 InPktsDelayed;
 100         __u64 InPktsOK;
 101         __u64 InPktsInvalid;
 102         __u64 InPktsLate;
 103         __u64 InPktsNotValid;
 104         __u64 InPktsNotUsingSA;
 105         __u64 InPktsUnusedSA;
 106 };
 107
 108 struct macsec_rx_sa_stats {
 109         __u32 InPktsOK;
 110         __u32 InPktsInvalid;
 111         __u32 InPktsNotValid;
 112         __u32 InPktsNotUsingSA;
 113         __u32 InPktsUnusedSA;
 114 };
 115
 116 struct macsec_tx_sa_stats {
 117         __u32 OutPktsProtected;
 118         __u32 OutPktsEncrypted;
 119 };
 120
 121 struct macsec_tx_sc_stats {
 122         __u64 OutPktsProtected;
 123         __u64 OutPktsEncrypted;
 124         __u64 OutOctetsProtected;
 125         __u64 OutOctetsEncrypted;
 126 };
 127
 128 struct macsec_dev_stats {
 129         __u64 OutPktsUntagged;
 130         __u64 InPktsUntagged;
 131         __u64 OutPktsTooLong;
 132         __u64 InPktsNoTag;
 133         __u64 InPktsBadTag;
 134         __u64 InPktsUnknownSCI;
 135         __u64 InPktsNoSCI;
 136         __u64 InPktsOverrun;
 137 };
 138
 139 /**
 140  * struct macsec_rx_sa - receive secure association
 141  * @active:
 142  * @next_pn: packet number expected for the next packet
 143  * @lock: protects next_pn manipulations
 144  * @key: key structure
 145  * @stats: per-SA stats
 146  */
 147 struct macsec_rx_sa {
 148         struct macsec_key key;
 149         spinlock_t lock;
 150         u32 next_pn;
 151         refcount_t refcnt;
 152         bool active;
 153         struct macsec_rx_sa_stats __percpu *stats;
 154         struct macsec_rx_sc *sc;
 155         struct rcu_head rcu;
 156 };
 157
 158 struct pcpu_rx_sc_stats {
 159         struct macsec_rx_sc_stats stats;
 160         struct u64_stats_sync syncp;
 161 };
 162
 163 /**
 164  * struct macsec_rx_sc - receive secure channel
 165  * @sci: secure channel identifier for this SC
 166  * @active: channel is active
 167  * @sa: array of secure associations
 168  * @stats: per-SC stats
 169  */
 170 struct macsec_rx_sc {
 171         struct macsec_rx_sc __rcu *next;
 172         sci_t sci;
 173         bool active;
 174         struct macsec_rx_sa __rcu *sa[MACSEC_NUM_AN];
 175         struct pcpu_rx_sc_stats __percpu *stats;
 176         refcount_t refcnt;
 177         struct rcu_head rcu_head;
 178 };
 179
 180 /**
 181  * struct macsec_tx_sa - transmit secure association
 182  * @active:
 183  * @next_pn: packet number to use for the next packet
 184  * @lock: protects next_pn manipulations
 185  * @key: key structure
 186  * @stats: per-SA stats
 187  */
 188 struct macsec_tx_sa {
 189         struct macsec_key key;
 190         spinlock_t lock;
 191         u32 next_pn;
 192         refcount_t refcnt;
 193         bool active;
 194         struct macsec_tx_sa_stats __percpu *stats;
 195         struct rcu_head rcu;
 196 };
 197
 198 struct pcpu_tx_sc_stats {
 199         struct macsec_tx_sc_stats stats;
 200         struct u64_stats_sync syncp;
 201 };
 202
 203 /**
 204  * struct macsec_tx_sc - transmit secure channel
 205  * @active:
 206  * @encoding_sa: association number of the SA currently in use
 207  * @encrypt: encrypt packets on transmit, or authenticate only
 208  * @send_sci: always include the SCI in the SecTAG
 209  * @end_station:
 210  * @scb: single copy broadcast flag
 211  * @sa: array of secure associations
 212  * @stats: stats for this TXSC
 213  */
 214 struct macsec_tx_sc {
 215         bool active;
 216         u8 encoding_sa;
 217         bool encrypt;
 218         bool send_sci;
 219         bool end_station;
 220         bool scb;
 221         struct macsec_tx_sa __rcu *sa[MACSEC_NUM_AN];
 222         struct pcpu_tx_sc_stats __percpu *stats;
 223 };
 224
 225 #define MACSEC_VALIDATE_DEFAULT MACSEC_VALIDATE_STRICT
 226
 227 /**
 228  * struct macsec_secy - MACsec Security Entity
 229  * @netdev: netdevice for this SecY
 230  * @n_rx_sc: number of receive secure channels configured on this SecY
 231  * @sci: secure channel identifier used for tx
 232  * @key_len: length of keys used by the cipher suite
 233  * @icv_len: length of ICV used by the cipher suite
 234  * @validate_frames: validation mode
 235  * @operational: MAC_Operational flag
 236  * @protect_frames: enable protection for this SecY
 237  * @replay_protect: enable packet number checks on receive
 238  * @replay_window: size of the replay window
 239  * @tx_sc: transmit secure channel
 240  * @rx_sc: linked list of receive secure channels
 241  */
 242 struct macsec_secy {
 243         struct net_device *netdev;
 244         unsigned int n_rx_sc;
 245         sci_t sci;
 246         u16 key_len;
 247         u16 icv_len;
 248         enum macsec_validation_type validate_frames;
 249         bool operational;
 250         bool protect_frames;
 251         bool replay_protect;
 252         u32 replay_window;
 253         struct macsec_tx_sc tx_sc;
 254         struct macsec_rx_sc __rcu *rx_sc;
 255 };
 256
 257 struct pcpu_secy_stats {
 258         struct macsec_dev_stats stats;
 259         struct u64_stats_sync syncp;
 260 };
 261
 262 /**
 263  * struct macsec_dev - private data
 264  * @secy: SecY config
 265  * @real_dev: pointer to underlying netdevice
 266  * @stats: MACsec device stats
 267  * @secys: linked list of SecY's on the underlying device
 268  */
 269 struct macsec_dev {
 270         struct macsec_secy secy;
 271         struct net_device *real_dev;
 272         struct pcpu_secy_stats __percpu *stats;
 273         struct list_head secys;
 274         struct gro_cells gro_cells;
 275         unsigned int nest_level;
 276 };
 277
 278 /**
 279  * struct macsec_rxh_data - rx_handler private argument
 280  * @secys: linked list of SecY's on this underlying device
 281  */
 282 struct macsec_rxh_data {
 283         struct list_head secys;
 284 };
 285
 286 static struct macsec_dev *macsec_priv(const struct net_device *dev)
 287 {
 288         return (struct macsec_dev *)netdev_priv(dev);
 289 }
 290
 291 static struct macsec_rxh_data *macsec_data_rcu(const struct net_device *dev)
 292 {
 293         return rcu_dereference_bh(dev->rx_handler_data);
 294 }
 295
 296 static struct macsec_rxh_data *macsec_data_rtnl(const struct net_device *dev)
 297 {
 298         return rtnl_dereference(dev->rx_handler_data);
 299 }
 300
 301 struct macsec_cb {
 302         struct aead_request *req;
 303         union {
 304                 struct macsec_tx_sa *tx_sa;
 305                 struct macsec_rx_sa *rx_sa;
 306         };
 307         u8 assoc_num;
 308         bool valid;
 309         bool has_sci;
 310 };
 311
 312 static struct macsec_rx_sa *macsec_rxsa_get(struct macsec_rx_sa __rcu *ptr)
 313 {
 314         struct macsec_rx_sa *sa = rcu_dereference_bh(ptr);
 315
 316         if (!sa || !sa->active)
 317                 return NULL;
 318
 319         if (!refcount_inc_not_zero(&sa->refcnt))
 320                 return NULL;
 321
 322         return sa;
 323 }
 324
 325 static struct macsec_rx_sa *macsec_active_rxsa_get(struct macsec_rx_sc *rx_sc)
 326 {
 327         struct macsec_rx_sa *sa = NULL;
 328         int an;
 329
 330         for (an = 0; an < MACSEC_NUM_AN; an++)  {
 331                 sa = macsec_rxsa_get(rx_sc->sa[an]);
 332                 if (sa)
 333                         break;
 334         }
 335         return sa;
 336 }
 337
 338 static void free_rx_sc_rcu(struct rcu_head *head)
 339 {
 340         struct macsec_rx_sc *rx_sc = container_of(head, struct macsec_rx_sc, rcu_head);
 341
 342         free_percpu(rx_sc->stats);
 343         kfree(rx_sc);
 344 }
 345
 346 static struct macsec_rx_sc *macsec_rxsc_get(struct macsec_rx_sc *sc)
 347 {
 348         return refcount_inc_not_zero(&sc->refcnt) ? sc : NULL;
 349 }
 350
 351 static void macsec_rxsc_put(struct macsec_rx_sc *sc)
 352 {
 353         if (refcount_dec_and_test(&sc->refcnt))
 354                 call_rcu(&sc->rcu_head, free_rx_sc_rcu);
 355 }
 356
 357 static void free_rxsa(struct rcu_head *head)
 358 {
 359         struct macsec_rx_sa *sa = container_of(head, struct macsec_rx_sa, rcu);
 360
 361         crypto_free_aead(sa->key.tfm);
 362         free_percpu(sa->stats);
 363         kfree(sa);
 364 }
 365
 366 static void macsec_rxsa_put(struct macsec_rx_sa *sa)
 367 {
 368         if (refcount_dec_and_test(&sa->refcnt))
 369                 call_rcu(&sa->rcu, free_rxsa);
 370 }
 371
 372 static struct macsec_tx_sa *macsec_txsa_get(struct macsec_tx_sa __rcu *ptr)
 373 {
 374         struct macsec_tx_sa *sa = rcu_dereference_bh(ptr);
 375
 376         if (!sa || !sa->active)
 377                 return NULL;
 378
 379         if (!refcount_inc_not_zero(&sa->refcnt))
 380                 return NULL;
 381
 382         return sa;
 383 }
 384
 385 static void free_txsa(struct rcu_head *head)
 386 {
 387         struct macsec_tx_sa *sa = container_of(head, struct macsec_tx_sa, rcu);
 388
 389         crypto_free_aead(sa->key.tfm);
 390         free_percpu(sa->stats);
 391         kfree(sa);
 392 }
 393
 394 static void macsec_txsa_put(struct macsec_tx_sa *sa)
 395 {
 396         if (refcount_dec_and_test(&sa->refcnt))
 397                 call_rcu(&sa->rcu, free_txsa);
 398 }
 399
 400 static struct macsec_cb *macsec_skb_cb(struct sk_buff *skb)
 401 {
 402         BUILD_BUG_ON(sizeof(struct macsec_cb) > sizeof(skb->cb));
 403         return (struct macsec_cb *)skb->cb;
 404 }
 405
 406 #define MACSEC_PORT_ES (htons(0x0001))
 407 #define MACSEC_PORT_SCB (0x0000)
 408 #define MACSEC_UNDEF_SCI ((__force sci_t)0xffffffffffffffffULL)
 409
 410 #define MACSEC_GCM_AES_128_SAK_LEN 16
 411 #define MACSEC_GCM_AES_256_SAK_LEN 32
 412
 413 #define DEFAULT_SAK_LEN MACSEC_GCM_AES_128_SAK_LEN
 414 #define DEFAULT_SEND_SCI true
 415 #define DEFAULT_ENCRYPT false
 416 #define DEFAULT_ENCODING_SA 0
 417
 418 static bool send_sci(const struct macsec_secy *secy)
 419 {
 420         const struct macsec_tx_sc *tx_sc = &secy->tx_sc;
 421
 422         return tx_sc->send_sci ||
 423                 (secy->n_rx_sc > 1 && !tx_sc->end_station && !tx_sc->scb);
 424 }
 425
 426 static sci_t make_sci(u8 *addr, __be16 port)
 427 {
 428         sci_t sci;
 429
 430         memcpy(&sci, addr, ETH_ALEN);
 431         memcpy(((char *)&sci) + ETH_ALEN, &port, sizeof(port));
 432
 433         return sci;
 434 }
 435
 436 static sci_t macsec_frame_sci(struct macsec_eth_header *hdr, bool sci_present)
 437 {
 438         sci_t sci;
 439
 440         if (sci_present)
 441                 memcpy(&sci, hdr->secure_channel_id,
 442                        sizeof(hdr->secure_channel_id));
 443         else
 444                 sci = make_sci(hdr->eth.h_source, MACSEC_PORT_ES);
 445
 446         return sci;
 447 }
 448
 449 static unsigned int macsec_sectag_len(bool sci_present)
 450 {
 451         return MACSEC_TAG_LEN + (sci_present ? MACSEC_SCI_LEN : 0);
 452 }
 453
 454 static unsigned int macsec_hdr_len(bool sci_present)
 455 {
 456         return macsec_sectag_len(sci_present) + ETH_HLEN;
 457 }
 458
 459 static unsigned int macsec_extra_len(bool sci_present)
 460 {
 461         return macsec_sectag_len(sci_present) + sizeof(__be16);
 462 }
 463
 464 /* Fill SecTAG according to IEEE 802.1AE-2006 10.5.3 */
 465 static void macsec_fill_sectag(struct macsec_eth_header *h,
 466                                const struct macsec_secy *secy, u32 pn,
 467                                bool sci_present)
 468 {
 469         const struct macsec_tx_sc *tx_sc = &secy->tx_sc;
 470
 471         memset(&h->tci_an, 0, macsec_sectag_len(sci_present));
 472         h->eth.h_proto = htons(ETH_P_MACSEC);
 473
 474         if (sci_present) {
 475                 h->tci_an |= MACSEC_TCI_SC;
 476                 memcpy(&h->secure_channel_id, &secy->sci,
 477                        sizeof(h->secure_channel_id));
 478         } else {
 479                 if (tx_sc->end_station)
 480                         h->tci_an |= MACSEC_TCI_ES;
 481                 if (tx_sc->scb)
 482                         h->tci_an |= MACSEC_TCI_SCB;
 483         }
 484
 485         h->packet_number = htonl(pn);
 486
 487         /* with GCM, C/E clear for !encrypt, both set for encrypt */
 488         if (tx_sc->encrypt)
 489                 h->tci_an |= MACSEC_TCI_CONFID;
 490         else if (secy->icv_len != DEFAULT_ICV_LEN)
 491                 h->tci_an |= MACSEC_TCI_C;
 492
 493         h->tci_an |= tx_sc->encoding_sa;
 494 }
 495
 496 static void macsec_set_shortlen(struct macsec_eth_header *h, size_t data_len)
 497 {
 498         if (data_len < MIN_NON_SHORT_LEN)
 499                 h->short_length = data_len;
 500 }
 501
 502 /* validate MACsec packet according to IEEE 802.1AE-2006 9.12 */
 503 static bool macsec_validate_skb(struct sk_buff *skb, u16 icv_len)
 504 {
 505         struct macsec_eth_header *h = (struct macsec_eth_header *)skb->data;
 506         int len = skb->len - 2 * ETH_ALEN;
 507         int extra_len = macsec_extra_len(!!(h->tci_an & MACSEC_TCI_SC)) + icv_len;
 508
 509         /* a) It comprises at least 17 octets */
 510         if (skb->len <= 16)
 511                 return false;
 512
 513         /* b) MACsec EtherType: already checked */
 514
 515         /* c) V bit is clear */
 516         if (h->tci_an & MACSEC_TCI_VERSION)
 517                 return false;
 518
 519         /* d) ES or SCB => !SC */
 520         if ((h->tci_an & MACSEC_TCI_ES || h->tci_an & MACSEC_TCI_SCB) &&
 521             (h->tci_an & MACSEC_TCI_SC))
 522                 return false;
 523
 524         /* e) Bits 7 and 8 of octet 4 of the SecTAG are clear */
 525         if (h->unused)
 526                 return false;
 527
 528         /* rx.pn != 0 (figure 10-5) */
 529         if (!h->packet_number)
 530                 return false;
 531
 532         /* length check, f) g) h) i) */
 533         if (h->short_length)
 534                 return len == extra_len + h->short_length;
 535         return len >= extra_len + MIN_NON_SHORT_LEN;
 536 }
 537
 538 #define MACSEC_NEEDED_HEADROOM (macsec_extra_len(true))
 539 #define MACSEC_NEEDED_TAILROOM MACSEC_STD_ICV_LEN
 540
 541 static void macsec_fill_iv(unsigned char *iv, sci_t sci, u32 pn)
 542 {
 543         struct gcm_iv *gcm_iv = (struct gcm_iv *)iv;
 544
 545         gcm_iv->sci = sci;
 546         gcm_iv->pn = htonl(pn);
 547 }
 548
 549 static struct macsec_eth_header *macsec_ethhdr(struct sk_buff *skb)
 550 {
 551         return (struct macsec_eth_header *)skb_mac_header(skb);
 552 }
 553
 554 static u32 tx_sa_update_pn(struct macsec_tx_sa *tx_sa, struct macsec_secy *secy)
 555 {
 556         u32 pn;
 557
 558         spin_lock_bh(&tx_sa->lock);
 559         pn = tx_sa->next_pn;
 560
 561         tx_sa->next_pn++;
 562         if (tx_sa->next_pn == 0) {
 563                 pr_debug("PN wrapped, transitioning to !oper\n");
 564                 tx_sa->active = false;
 565                 if (secy->protect_frames)
 566                         secy->operational = false;
 567         }
 568         spin_unlock_bh(&tx_sa->lock);
 569
 570         return pn;
 571 }
 572
 573 static void macsec_encrypt_finish(struct sk_buff *skb, struct net_device *dev)
 574 {
 575         struct macsec_dev *macsec = netdev_priv(dev);
 576
 577         skb->dev = macsec->real_dev;
 578         skb_reset_mac_header(skb);
 579         skb->protocol = eth_hdr(skb)->h_proto;
 580 }
 581
 582 static unsigned int macsec_msdu_len(struct sk_buff *skb)
 583 {
 584         struct macsec_dev *macsec = macsec_priv(skb->dev);
 585         struct macsec_secy *secy = &macsec->secy;
 586         bool sci_present = macsec_skb_cb(skb)->has_sci;
 587
 588         return skb->len - macsec_hdr_len(sci_present) - secy->icv_len;
 589 }
 590
 591 static void macsec_count_tx(struct sk_buff *skb, struct macsec_tx_sc *tx_sc,
 592                             struct macsec_tx_sa *tx_sa)
 593 {
 594         unsigned int msdu_len = macsec_msdu_len(skb);
 595         struct pcpu_tx_sc_stats *txsc_stats = this_cpu_ptr(tx_sc->stats);
 596
 597         u64_stats_update_begin(&txsc_stats->syncp);
 598         if (tx_sc->encrypt) {
 599                 txsc_stats->stats.OutOctetsEncrypted += msdu_len;
 600                 txsc_stats->stats.OutPktsEncrypted++;
 601                 this_cpu_inc(tx_sa->stats->OutPktsEncrypted);
 602         } else {
 603                 txsc_stats->stats.OutOctetsProtected += msdu_len;
 604                 txsc_stats->stats.OutPktsProtected++;
 605                 this_cpu_inc(tx_sa->stats->OutPktsProtected);
 606         }
 607         u64_stats_update_end(&txsc_stats->syncp);
 608 }
 609
 610 static void count_tx(struct net_device *dev, int ret, int len)
 611 {
 612         if (likely(ret == NET_XMIT_SUCCESS || ret == NET_XMIT_CN)) {
 613                 struct pcpu_sw_netstats *stats = this_cpu_ptr(dev->tstats);
 614
 615                 u64_stats_update_begin(&stats->syncp);
 616                 stats->tx_packets++;
 617                 stats->tx_bytes += len;
 618                 u64_stats_update_end(&stats->syncp);
 619         }
 620 }
 621
 622 static void macsec_encrypt_done(struct crypto_async_request *base, int err)
 623 {
 624         struct sk_buff *skb = base->data;
 625         struct net_device *dev = skb->dev;
 626         struct macsec_dev *macsec = macsec_priv(dev);
 627         struct macsec_tx_sa *sa = macsec_skb_cb(skb)->tx_sa;
 628         int len, ret;
 629
 630         aead_request_free(macsec_skb_cb(skb)->req);
 631
 632         rcu_read_lock_bh();
 633         macsec_count_tx(skb, &macsec->secy.tx_sc, macsec_skb_cb(skb)->tx_sa);
 634         /* packet is encrypted/protected so tx_bytes must be calculated */
 635         len = macsec_msdu_len(skb) + 2 * ETH_ALEN;
 636         macsec_encrypt_finish(skb, dev);
 637         ret = dev_queue_xmit(skb);
 638         count_tx(dev, ret, len);
 639         rcu_read_unlock_bh();
 640
 641         macsec_txsa_put(sa);
 642         dev_put(dev);
 643 }
 644
 645 static struct aead_request *macsec_alloc_req(struct crypto_aead *tfm,
 646                                              unsigned char **iv,
 647                                              struct scatterlist **sg,
 648                                              int num_frags)
 649 {
 650         size_t size, iv_offset, sg_offset;
 651         struct aead_request *req;
 652         void *tmp;
 653
 654         size = sizeof(struct aead_request) + crypto_aead_reqsize(tfm);
 655         iv_offset = size;
 656         size += GCM_AES_IV_LEN;
 657
 658         size = ALIGN(size, __alignof__(struct scatterlist));
 659         sg_offset = size;
 660         size += sizeof(struct scatterlist) * num_frags;
 661
 662         tmp = kmalloc(size, GFP_ATOMIC);
 663         if (!tmp)
 664                 return NULL;
 665
 666         *iv = (unsigned char *)(tmp + iv_offset);
 667         *sg = (struct scatterlist *)(tmp + sg_offset);
 668         req = tmp;
 669
 670         aead_request_set_tfm(req, tfm);
 671
 672         return req;
 673 }
 674
 675 static struct sk_buff *macsec_encrypt(struct sk_buff *skb,
 676                                       struct net_device *dev)
 677 {
 678         int ret;
 679         struct scatterlist *sg;
 680         struct sk_buff *trailer;
 681         unsigned char *iv;
 682         struct ethhdr *eth;
 683         struct macsec_eth_header *hh;
 684         size_t unprotected_len;
 685         struct aead_request *req;
 686         struct macsec_secy *secy;
 687         struct macsec_tx_sc *tx_sc;
 688         struct macsec_tx_sa *tx_sa;
 689         struct macsec_dev *macsec = macsec_priv(dev);
 690         bool sci_present;
 691         u32 pn;
 692
 693         secy = &macsec->secy;
 694         tx_sc = &secy->tx_sc;
 695
 696         /* 10.5.1 TX SA assignment */
 697         tx_sa = macsec_txsa_get(tx_sc->sa[tx_sc->encoding_sa]);
 698         if (!tx_sa) {
 699                 secy->operational = false;
 700                 kfree_skb(skb);
 701                 return ERR_PTR(-EINVAL);
 702         }
 703
 704         if (unlikely(skb_headroom(skb) < MACSEC_NEEDED_HEADROOM ||
 705                      skb_tailroom(skb) < MACSEC_NEEDED_TAILROOM)) {
 706                 struct sk_buff *nskb = skb_copy_expand(skb,
 707                                                        MACSEC_NEEDED_HEADROOM,
 708                                                        MACSEC_NEEDED_TAILROOM,
 709                                                        GFP_ATOMIC);
 710                 if (likely(nskb)) {
 711                         consume_skb(skb);
 712                         skb = nskb;
 713                 } else {
 714                         macsec_txsa_put(tx_sa);
 715                         kfree_skb(skb);
 716                         return ERR_PTR(-ENOMEM);
 717                 }
 718         } else {
 719                 skb = skb_unshare(skb, GFP_ATOMIC);
 720                 if (!skb) {
 721                         macsec_txsa_put(tx_sa);
 722                         return ERR_PTR(-ENOMEM);
 723                 }
 724         }
 725
 726         unprotected_len = skb->len;
 727         eth = eth_hdr(skb);
 728         sci_present = send_sci(secy);
 729         hh = skb_push(skb, macsec_extra_len(sci_present));
 730         memmove(hh, eth, 2 * ETH_ALEN);
 731
 732         pn = tx_sa_update_pn(tx_sa, secy);
 733         if (pn == 0) {
 734                 macsec_txsa_put(tx_sa);
 735                 kfree_skb(skb);
 736                 return ERR_PTR(-ENOLINK);
 737         }
 738         macsec_fill_sectag(hh, secy, pn, sci_present);
 739         macsec_set_shortlen(hh, unprotected_len - 2 * ETH_ALEN);
 740
 741         skb_put(skb, secy->icv_len);
 742
 743         if (skb->len - ETH_HLEN > macsec_priv(dev)->real_dev->mtu) {
 744                 struct pcpu_secy_stats *secy_stats = this_cpu_ptr(macsec->stats);
 745
 746                 u64_stats_update_begin(&secy_stats->syncp);
 747                 secy_stats->stats.OutPktsTooLong++;
 748                 u64_stats_update_end(&secy_stats->syncp);
 749
 750                 macsec_txsa_put(tx_sa);
 751                 kfree_skb(skb);
 752                 return ERR_PTR(-EINVAL);
 753         }
 754
 755         ret = skb_cow_data(skb, 0, &trailer);
 756         if (unlikely(ret < 0)) {
 757                 macsec_txsa_put(tx_sa);
 758                 kfree_skb(skb);
 759                 return ERR_PTR(ret);
 760         }
 761
 762         req = macsec_alloc_req(tx_sa->key.tfm, &iv, &sg, ret);
 763         if (!req) {
 764                 macsec_txsa_put(tx_sa);
 765                 kfree_skb(skb);
 766                 return ERR_PTR(-ENOMEM);
 767         }
 768
 769         macsec_fill_iv(iv, secy->sci, pn);
 770
 771         sg_init_table(sg, ret);
 772         ret = skb_to_sgvec(skb, sg, 0, skb->len);
 773         if (unlikely(ret < 0)) {
 774                 aead_request_free(req);
 775                 macsec_txsa_put(tx_sa);
 776                 kfree_skb(skb);
 777                 return ERR_PTR(ret);
 778         }
 779
 780         if (tx_sc->encrypt) {
 781                 int len = skb->len - macsec_hdr_len(sci_present) -
 782                           secy->icv_len;
 783                 aead_request_set_crypt(req, sg, sg, len, iv);
 784                 aead_request_set_ad(req, macsec_hdr_len(sci_present));
 785         } else {
 786                 aead_request_set_crypt(req, sg, sg, 0, iv);
 787                 aead_request_set_ad(req, skb->len - secy->icv_len);
 788         }
 789
 790         macsec_skb_cb(skb)->req = req;
 791         macsec_skb_cb(skb)->tx_sa = tx_sa;
 792         macsec_skb_cb(skb)->has_sci = sci_present;
 793         aead_request_set_callback(req, 0, macsec_encrypt_done, skb);
 794
 795         dev_hold(skb->dev);
 796         ret = crypto_aead_encrypt(req);
 797         if (ret == -EINPROGRESS) {
 798                 return ERR_PTR(ret);
 799         } else if (ret != 0) {
 800                 dev_put(skb->dev);
 801                 kfree_skb(skb);
 802                 aead_request_free(req);
 803                 macsec_txsa_put(tx_sa);
 804                 return ERR_PTR(-EINVAL);
 805         }
 806
 807         dev_put(skb->dev);
 808         aead_request_free(req);
 809         macsec_txsa_put(tx_sa);
 810
 811         return skb;
 812 }
 813
 814 static bool macsec_post_decrypt(struct sk_buff *skb, struct macsec_secy *secy, u32 pn)
 815 {
 816         struct macsec_rx_sa *rx_sa = macsec_skb_cb(skb)->rx_sa;
 817         struct pcpu_rx_sc_stats *rxsc_stats = this_cpu_ptr(rx_sa->sc->stats);
 818         struct macsec_eth_header *hdr = macsec_ethhdr(skb);
 819         u32 lowest_pn = 0;
 820
 821         spin_lock(&rx_sa->lock);
 822         if (rx_sa->next_pn >= secy->replay_window)
 823                 lowest_pn = rx_sa->next_pn - secy->replay_window;
 824
 825         /* Now perform replay protection check again
 826          * (see IEEE 802.1AE-2006 figure 10-5)
 827          */
 828         if (secy->replay_protect && pn < lowest_pn) {
 829                 spin_unlock(&rx_sa->lock);
 830                 u64_stats_update_begin(&rxsc_stats->syncp);
 831                 rxsc_stats->stats.InPktsLate++;
 832                 u64_stats_update_end(&rxsc_stats->syncp);
 833                 DEV_STATS_INC(secy->netdev, rx_dropped);
 834                 return false;
 835         }
 836
 837         if (secy->validate_frames != MACSEC_VALIDATE_DISABLED) {
 838                 unsigned int msdu_len = macsec_msdu_len(skb);
 839                 u64_stats_update_begin(&rxsc_stats->syncp);
 840                 if (hdr->tci_an & MACSEC_TCI_E)
 841                         rxsc_stats->stats.InOctetsDecrypted += msdu_len;
 842                 else
 843                         rxsc_stats->stats.InOctetsValidated += msdu_len;
 844                 u64_stats_update_end(&rxsc_stats->syncp);
 845         }
 846
 847         if (!macsec_skb_cb(skb)->valid) {
 848                 spin_unlock(&rx_sa->lock);
 849
 850                 /* 10.6.5 */
 851                 if (hdr->tci_an & MACSEC_TCI_C ||
 852                     secy->validate_frames == MACSEC_VALIDATE_STRICT) {
 853                         u64_stats_update_begin(&rxsc_stats->syncp);
 854                         rxsc_stats->stats.InPktsNotValid++;
 855                         u64_stats_update_end(&rxsc_stats->syncp);
 856                         this_cpu_inc(rx_sa->stats->InPktsNotValid);
 857                         DEV_STATS_INC(secy->netdev, rx_errors);
 858                         return false;
 859                 }
 860
 861                 u64_stats_update_begin(&rxsc_stats->syncp);
 862                 if (secy->validate_frames == MACSEC_VALIDATE_CHECK) {
 863                         rxsc_stats->stats.InPktsInvalid++;
 864                         this_cpu_inc(rx_sa->stats->InPktsInvalid);
 865                 } else if (pn < lowest_pn) {
 866                         rxsc_stats->stats.InPktsDelayed++;
 867                 } else {
 868                         rxsc_stats->stats.InPktsUnchecked++;
 869                 }
 870                 u64_stats_update_end(&rxsc_stats->syncp);
 871         } else {
 872                 u64_stats_update_begin(&rxsc_stats->syncp);
 873                 if (pn < lowest_pn) {
 874                         rxsc_stats->stats.InPktsDelayed++;
 875                 } else {
 876                         rxsc_stats->stats.InPktsOK++;
 877                         this_cpu_inc(rx_sa->stats->InPktsOK);
 878                 }
 879                 u64_stats_update_end(&rxsc_stats->syncp);
 880
 881                 if (pn >= rx_sa->next_pn)
 882                         rx_sa->next_pn = pn + 1;
 883                 spin_unlock(&rx_sa->lock);
 884         }
 885
 886         return true;
 887 }
 888
 889 static void macsec_reset_skb(struct sk_buff *skb, struct net_device *dev)
 890 {
 891         skb->pkt_type = PACKET_HOST;
 892         skb->protocol = eth_type_trans(skb, dev);
 893
 894         skb_reset_network_header(skb);
 895         if (!skb_transport_header_was_set(skb))
 896                 skb_reset_transport_header(skb);
 897         skb_reset_mac_len(skb);
 898 }
 899
 900 static void macsec_finalize_skb(struct sk_buff *skb, u8 icv_len, u8 hdr_len)
 901 {
 902         skb->ip_summed = CHECKSUM_NONE;
 903         memmove(skb->data + hdr_len, skb->data, 2 * ETH_ALEN);
 904         skb_pull(skb, hdr_len);
 905         pskb_trim_unique(skb, skb->len - icv_len);
 906 }
 907
 908 static void count_rx(struct net_device *dev, int len)
 909 {
 910         struct pcpu_sw_netstats *stats = this_cpu_ptr(dev->tstats);
 911
 912         u64_stats_update_begin(&stats->syncp);
 913         stats->rx_packets++;
 914         stats->rx_bytes += len;
 915         u64_stats_update_end(&stats->syncp);
 916 }
 917
 918 static void macsec_decrypt_done(struct crypto_async_request *base, int err)
 919 {
 920         struct sk_buff *skb = base->data;
 921         struct net_device *dev = skb->dev;
 922         struct macsec_dev *macsec = macsec_priv(dev);
 923         struct macsec_rx_sa *rx_sa = macsec_skb_cb(skb)->rx_sa;
 924         struct macsec_rx_sc *rx_sc = rx_sa->sc;
 925         int len;
 926         u32 pn;
 927
 928         aead_request_free(macsec_skb_cb(skb)->req);
 929
 930         if (!err)
 931                 macsec_skb_cb(skb)->valid = true;
 932
 933         rcu_read_lock_bh();
 934         pn = ntohl(macsec_ethhdr(skb)->packet_number);
 935         if (!macsec_post_decrypt(skb, &macsec->secy, pn)) {
 936                 rcu_read_unlock_bh();
 937                 kfree_skb(skb);
 938                 goto out;
 939         }
 940
 941         macsec_finalize_skb(skb, macsec->secy.icv_len,
 942                             macsec_extra_len(macsec_skb_cb(skb)->has_sci));
 943         len = skb->len;
 944         macsec_reset_skb(skb, macsec->secy.netdev);
 945
 946         if (gro_cells_receive(&macsec->gro_cells, skb) == NET_RX_SUCCESS)
 947                 count_rx(dev, len);
 948
 949         rcu_read_unlock_bh();
 950
 951 out:
 952         macsec_rxsa_put(rx_sa);
 953         macsec_rxsc_put(rx_sc);
 954         dev_put(dev);
 955 }
 956
 957 static struct sk_buff *macsec_decrypt(struct sk_buff *skb,
 958                                       struct net_device *dev,
 959                                       struct macsec_rx_sa *rx_sa,
 960                                       sci_t sci,
 961                                       struct macsec_secy *secy)
 962 {
 963         int ret;
 964         struct scatterlist *sg;
 965         struct sk_buff *trailer;
 966         unsigned char *iv;
 967         struct aead_request *req;
 968         struct macsec_eth_header *hdr;
 969         u16 icv_len = secy->icv_len;
 970
 971         macsec_skb_cb(skb)->valid = false;
 972         skb = skb_share_check(skb, GFP_ATOMIC);
 973         if (!skb)
 974                 return ERR_PTR(-ENOMEM);
 975
 976         ret = skb_cow_data(skb, 0, &trailer);
 977         if (unlikely(ret < 0)) {
 978                 kfree_skb(skb);
 979                 return ERR_PTR(ret);
 980         }
 981         req = macsec_alloc_req(rx_sa->key.tfm, &iv, &sg, ret);
 982         if (!req) {
 983                 kfree_skb(skb);
 984                 return ERR_PTR(-ENOMEM);
 985         }
 986
 987         hdr = (struct macsec_eth_header *)skb->data;
 988         macsec_fill_iv(iv, sci, ntohl(hdr->packet_number));
 989
 990         sg_init_table(sg, ret);
 991         ret = skb_to_sgvec(skb, sg, 0, skb->len);
 992         if (unlikely(ret < 0)) {
 993                 aead_request_free(req);
 994                 kfree_skb(skb);
 995                 return ERR_PTR(ret);
 996         }
 997
 998         if (hdr->tci_an & MACSEC_TCI_E) {
 999                 /* confidentiality: ethernet + macsec header
1000                  * authenticated, encrypted payload
1001                  */
1002                 int len = skb->len - macsec_hdr_len(macsec_skb_cb(skb)->has_sci);
1003
1004                 aead_request_set_crypt(req, sg, sg, len, iv);
1005                 aead_request_set_ad(req, macsec_hdr_len(macsec_skb_cb(skb)->has_sci));
1006                 skb = skb_unshare(skb, GFP_ATOMIC);
1007                 if (!skb) {
1008                         aead_request_free(req);
1009                         return ERR_PTR(-ENOMEM);
1010                 }
1011         } else {
1012                 /* integrity only: all headers + data authenticated */
1013                 aead_request_set_crypt(req, sg, sg, icv_len, iv);
1014                 aead_request_set_ad(req, skb->len - icv_len);
1015         }
1016
1017         macsec_skb_cb(skb)->req = req;
1018         skb->dev = dev;
1019         aead_request_set_callback(req, 0, macsec_decrypt_done, skb);
1020
1021         dev_hold(dev);
1022         ret = crypto_aead_decrypt(req);
1023         if (ret == -EINPROGRESS) {
1024                 return ERR_PTR(ret);
1025         } else if (ret != 0) {
1026                 /* decryption/authentication failed
1027                  * 10.6 if validateFrames is disabled, deliver anyway
1028                  */
1029                 if (ret != -EBADMSG) {
1030                         kfree_skb(skb);
1031                         skb = ERR_PTR(ret);
1032                 }
1033         } else {
1034                 macsec_skb_cb(skb)->valid = true;
1035         }
1036         dev_put(dev);
1037
1038         aead_request_free(req);
1039
1040         return skb;
1041 }
1042
1043 static struct macsec_rx_sc *find_rx_sc(struct macsec_secy *secy, sci_t sci)
1044 {
1045         struct macsec_rx_sc *rx_sc;
1046
1047         for_each_rxsc(secy, rx_sc) {
1048                 if (rx_sc->sci == sci)
1049                         return rx_sc;
1050         }
1051
1052         return NULL;
1053 }
1054
1055 static struct macsec_rx_sc *find_rx_sc_rtnl(struct macsec_secy *secy, sci_t sci)
1056 {
1057         struct macsec_rx_sc *rx_sc;
1058
1059         for_each_rxsc_rtnl(secy, rx_sc) {
1060                 if (rx_sc->sci == sci)
1061                         return rx_sc;
1062         }
1063
1064         return NULL;
1065 }
1066
1067 static void handle_not_macsec(struct sk_buff *skb)
1068 {
1069         struct macsec_rxh_data *rxd;
1070         struct macsec_dev *macsec;
1071
1072         rcu_read_lock();
1073         rxd = macsec_data_rcu(skb->dev);
1074
1075         /* 10.6 If the management control validateFrames is not
1076          * Strict, frames without a SecTAG are received, counted, and
1077          * delivered to the Controlled Port
1078          */
1079         list_for_each_entry_rcu(macsec, &rxd->secys, secys) {
1080                 struct sk_buff *nskb;
1081                 struct pcpu_secy_stats *secy_stats = this_cpu_ptr(macsec->stats);
1082
1083                 if (macsec->secy.validate_frames == MACSEC_VALIDATE_STRICT) {
1084                         u64_stats_update_begin(&secy_stats->syncp);
1085                         secy_stats->stats.InPktsNoTag++;
1086                         u64_stats_update_end(&secy_stats->syncp);
1087                         DEV_STATS_INC(macsec->secy.netdev, rx_dropped);
1088                         continue;
1089                 }
1090
1091                 /* deliver on this port */
1092                 nskb = skb_clone(skb, GFP_ATOMIC);
1093                 if (!nskb)
1094                         break;
1095
1096                 nskb->dev = macsec->secy.netdev;
1097
1098                 if (netif_rx(nskb) == NET_RX_SUCCESS) {
1099                         u64_stats_update_begin(&secy_stats->syncp);
1100                         secy_stats->stats.InPktsUntagged++;
1101                         u64_stats_update_end(&secy_stats->syncp);
1102                 }
1103         }
1104
1105         rcu_read_unlock();
1106 }
1107
1108 static rx_handler_result_t macsec_handle_frame(struct sk_buff **pskb)
1109 {
1110         struct sk_buff *skb = *pskb;
1111         struct net_device *dev = skb->dev;
1112         struct macsec_eth_header *hdr;
1113         struct macsec_secy *secy = NULL;
1114         struct macsec_rx_sc *rx_sc;
1115         struct macsec_rx_sa *rx_sa;
1116         struct macsec_rxh_data *rxd;
1117         struct macsec_dev *macsec;
1118         unsigned int len;
1119         sci_t sci;
1120         u32 pn;
1121         bool cbit;
1122         struct pcpu_rx_sc_stats *rxsc_stats;
1123         struct pcpu_secy_stats *secy_stats;
1124         bool pulled_sci;
1125         int ret;
1126
1127         if (skb_headroom(skb) < ETH_HLEN)
1128                 goto drop_direct;
1129
1130         hdr = macsec_ethhdr(skb);
1131         if (hdr->eth.h_proto != htons(ETH_P_MACSEC)) {
1132                 handle_not_macsec(skb);
1133
1134                 /* and deliver to the uncontrolled port */
1135                 return RX_HANDLER_PASS;
1136         }
1137
1138         skb = skb_unshare(skb, GFP_ATOMIC);
1139         *pskb = skb;
1140         if (!skb)
1141                 return RX_HANDLER_CONSUMED;
1142
1143         pulled_sci = pskb_may_pull(skb, macsec_extra_len(true));
1144         if (!pulled_sci) {
1145                 if (!pskb_may_pull(skb, macsec_extra_len(false)))
1146                         goto drop_direct;
1147         }
1148
1149         hdr = macsec_ethhdr(skb);
1150
1151         /* Frames with a SecTAG that has the TCI E bit set but the C
1152          * bit clear are discarded, as this reserved encoding is used
1153          * to identify frames with a SecTAG that are not to be
1154          * delivered to the Controlled Port.
1155          */
1156         if ((hdr->tci_an & (MACSEC_TCI_C | MACSEC_TCI_E)) == MACSEC_TCI_E)
1157                 return RX_HANDLER_PASS;
1158
1159         /* now, pull the extra length */
1160         if (hdr->tci_an & MACSEC_TCI_SC) {
1161                 if (!pulled_sci)
1162                         goto drop_direct;
1163         }
1164
1165         /* ethernet header is part of crypto processing */
1166         skb_push(skb, ETH_HLEN);
1167
1168         macsec_skb_cb(skb)->has_sci = !!(hdr->tci_an & MACSEC_TCI_SC);
1169         macsec_skb_cb(skb)->assoc_num = hdr->tci_an & MACSEC_AN_MASK;
1170         sci = macsec_frame_sci(hdr, macsec_skb_cb(skb)->has_sci);
1171
1172         rcu_read_lock();
1173         rxd = macsec_data_rcu(skb->dev);
1174
1175         list_for_each_entry_rcu(macsec, &rxd->secys, secys) {
1176                 struct macsec_rx_sc *sc = find_rx_sc(&macsec->secy, sci);
1177                 sc = sc ? macsec_rxsc_get(sc) : NULL;
1178
1179                 if (sc) {
1180                         secy = &macsec->secy;
1181                         rx_sc = sc;
1182                         break;
1183                 }
1184         }
1185
1186         if (!secy)
1187                 goto nosci;
1188
1189         dev = secy->netdev;
1190         macsec = macsec_priv(dev);
1191         secy_stats = this_cpu_ptr(macsec->stats);
1192         rxsc_stats = this_cpu_ptr(rx_sc->stats);
1193
1194         if (!macsec_validate_skb(skb, secy->icv_len)) {
1195                 u64_stats_update_begin(&secy_stats->syncp);
1196                 secy_stats->stats.InPktsBadTag++;
1197                 u64_stats_update_end(&secy_stats->syncp);
1198                 DEV_STATS_INC(secy->netdev, rx_errors);
1199                 goto drop_nosa;
1200         }
1201
1202         rx_sa = macsec_rxsa_get(rx_sc->sa[macsec_skb_cb(skb)->assoc_num]);
1203         if (!rx_sa) {
1204                 /* 10.6.1 if the SA is not in use */
1205
1206                 /* If validateFrames is Strict or the C bit in the
1207                  * SecTAG is set, discard
1208                  */
1209                 struct macsec_rx_sa *active_rx_sa = macsec_active_rxsa_get(rx_sc);
1210                 if (hdr->tci_an & MACSEC_TCI_C ||
1211                     secy->validate_frames == MACSEC_VALIDATE_STRICT) {
1212                         u64_stats_update_begin(&rxsc_stats->syncp);
1213                         rxsc_stats->stats.InPktsNotUsingSA++;
1214                         u64_stats_update_end(&rxsc_stats->syncp);
1215                         DEV_STATS_INC(secy->netdev, rx_errors);
1216                         if (active_rx_sa)
1217                                 this_cpu_inc(active_rx_sa->stats->InPktsNotUsingSA);
1218                         goto drop_nosa;
1219                 }
1220
1221                 /* not Strict, the frame (with the SecTAG and ICV
1222                  * removed) is delivered to the Controlled Port.
1223                  */
1224                 u64_stats_update_begin(&rxsc_stats->syncp);
1225                 rxsc_stats->stats.InPktsUnusedSA++;
1226                 u64_stats_update_end(&rxsc_stats->syncp);
1227                 if (active_rx_sa)
1228                         this_cpu_inc(active_rx_sa->stats->InPktsUnusedSA);
1229                 goto deliver;
1230         }
1231
1232         /* First, PN check to avoid decrypting obviously wrong packets */
1233         pn = ntohl(hdr->packet_number);
1234         if (secy->replay_protect) {
1235                 bool late;
1236
1237                 spin_lock(&rx_sa->lock);
1238                 late = rx_sa->next_pn >= secy->replay_window &&
1239                        pn < (rx_sa->next_pn - secy->replay_window);
1240                 spin_unlock(&rx_sa->lock);
1241
1242                 if (late) {
1243                         u64_stats_update_begin(&rxsc_stats->syncp);
1244                         rxsc_stats->stats.InPktsLate++;
1245                         u64_stats_update_end(&rxsc_stats->syncp);
1246                         DEV_STATS_INC(macsec->secy.netdev, rx_dropped);
1247                         goto drop;
1248                 }
1249         }
1250
1251         macsec_skb_cb(skb)->rx_sa = rx_sa;
1252
1253         /* Disabled && !changed text => skip validation */
1254         if (hdr->tci_an & MACSEC_TCI_C ||
1255             secy->validate_frames != MACSEC_VALIDATE_DISABLED)
1256                 skb = macsec_decrypt(skb, dev, rx_sa, sci, secy);
1257
1258         if (IS_ERR(skb)) {
1259                 /* the decrypt callback needs the reference */
1260                 if (PTR_ERR(skb) != -EINPROGRESS) {
1261                         macsec_rxsa_put(rx_sa);
1262                         macsec_rxsc_put(rx_sc);
1263                 }
1264                 rcu_read_unlock();
1265                 *pskb = NULL;
1266                 return RX_HANDLER_CONSUMED;
1267         }
1268
1269         if (!macsec_post_decrypt(skb, secy, pn))
1270                 goto drop;
1271
1272 deliver:
1273         macsec_finalize_skb(skb, secy->icv_len,
1274                             macsec_extra_len(macsec_skb_cb(skb)->has_sci));
1275         len = skb->len;
1276         macsec_reset_skb(skb, secy->netdev);
1277
1278         if (rx_sa)
1279                 macsec_rxsa_put(rx_sa);
1280         macsec_rxsc_put(rx_sc);
1281
1282         skb_orphan(skb);
1283         ret = gro_cells_receive(&macsec->gro_cells, skb);
1284         if (ret == NET_RX_SUCCESS)
1285                 count_rx(dev, len);
1286         else
1287                 DEV_STATS_INC(macsec->secy.netdev, rx_dropped);
1288
1289         rcu_read_unlock();
1290
1291         *pskb = NULL;
1292         return RX_HANDLER_CONSUMED;
1293
1294 drop:
1295         macsec_rxsa_put(rx_sa);
1296 drop_nosa:
1297         macsec_rxsc_put(rx_sc);
1298         rcu_read_unlock();
1299 drop_direct:
1300         kfree_skb(skb);
1301         *pskb = NULL;
1302         return RX_HANDLER_CONSUMED;
1303
1304 nosci:
1305         /* 10.6.1 if the SC is not found */
1306         cbit = !!(hdr->tci_an & MACSEC_TCI_C);
1307         if (!cbit)
1308                 macsec_finalize_skb(skb, DEFAULT_ICV_LEN,
1309                                     macsec_extra_len(macsec_skb_cb(skb)->has_sci));
1310
1311         list_for_each_entry_rcu(macsec, &rxd->secys, secys) {
1312                 struct sk_buff *nskb;
1313
1314                 secy_stats = this_cpu_ptr(macsec->stats);
1315
1316                 /* If validateFrames is Strict or the C bit in the
1317                  * SecTAG is set, discard
1318                  */
1319                 if (cbit ||
1320                     macsec->secy.validate_frames == MACSEC_VALIDATE_STRICT) {
1321                         u64_stats_update_begin(&secy_stats->syncp);
1322                         secy_stats->stats.InPktsNoSCI++;
1323                         u64_stats_update_end(&secy_stats->syncp);
1324                         DEV_STATS_INC(macsec->secy.netdev, rx_errors);
1325                         continue;
1326                 }
1327
1328                 /* not strict, the frame (with the SecTAG and ICV
1329                  * removed) is delivered to the Controlled Port.
1330                  */
1331                 nskb = skb_clone(skb, GFP_ATOMIC);
1332                 if (!nskb)
1333                         break;
1334
1335                 macsec_reset_skb(nskb, macsec->secy.netdev);
1336
1337                 ret = netif_rx(nskb);
1338                 if (ret == NET_RX_SUCCESS) {
1339                         u64_stats_update_begin(&secy_stats->syncp);
1340                         secy_stats->stats.InPktsUnknownSCI++;
1341                         u64_stats_update_end(&secy_stats->syncp);
1342                 } else {
1343                         DEV_STATS_INC(macsec->secy.netdev, rx_dropped);
1344                 }
1345         }
1346
1347         rcu_read_unlock();
1348         *pskb = skb;
1349         return RX_HANDLER_PASS;
1350 }
1351
1352 static struct crypto_aead *macsec_alloc_tfm(char *key, int key_len, int icv_len)
1353 {
1354         struct crypto_aead *tfm;
1355         int ret;
1356
1357         tfm = crypto_alloc_aead("gcm(aes)", 0, 0);
1358
1359         if (IS_ERR(tfm))
1360                 return tfm;
1361
1362         ret = crypto_aead_setkey(tfm, key, key_len);
1363         if (ret < 0)
1364                 goto fail;
1365
1366         ret = crypto_aead_setauthsize(tfm, icv_len);
1367         if (ret < 0)
1368                 goto fail;
1369
1370         return tfm;
1371 fail:
1372         crypto_free_aead(tfm);
1373         return ERR_PTR(ret);
1374 }
1375
1376 static int init_rx_sa(struct macsec_rx_sa *rx_sa, char *sak, int key_len,
1377                       int icv_len)
1378 {
1379         rx_sa->stats = alloc_percpu(struct macsec_rx_sa_stats);
1380         if (!rx_sa->stats)
1381                 return -ENOMEM;
1382
1383         rx_sa->key.tfm = macsec_alloc_tfm(sak, key_len, icv_len);
1384         if (IS_ERR(rx_sa->key.tfm)) {
1385                 free_percpu(rx_sa->stats);
1386                 return PTR_ERR(rx_sa->key.tfm);
1387         }
1388
1389         rx_sa->active = false;
1390         rx_sa->next_pn = 1;
1391         refcount_set(&rx_sa->refcnt, 1);
1392         spin_lock_init(&rx_sa->lock);
1393
1394         return 0;
1395 }
1396
1397 static void clear_rx_sa(struct macsec_rx_sa *rx_sa)
1398 {
1399         rx_sa->active = false;
1400
1401         macsec_rxsa_put(rx_sa);
1402 }
1403
1404 static void free_rx_sc(struct macsec_rx_sc *rx_sc)
1405 {
1406         int i;
1407
1408         for (i = 0; i < MACSEC_NUM_AN; i++) {
1409                 struct macsec_rx_sa *sa = rtnl_dereference(rx_sc->sa[i]);
1410
1411                 RCU_INIT_POINTER(rx_sc->sa[i], NULL);
1412                 if (sa)
1413                         clear_rx_sa(sa);
1414         }
1415
1416         macsec_rxsc_put(rx_sc);
1417 }
1418
1419 static struct macsec_rx_sc *del_rx_sc(struct macsec_secy *secy, sci_t sci)
1420 {
1421         struct macsec_rx_sc *rx_sc, __rcu **rx_scp;
1422
1423         for (rx_scp = &secy->rx_sc, rx_sc = rtnl_dereference(*rx_scp);
1424              rx_sc;
1425              rx_scp = &rx_sc->next, rx_sc = rtnl_dereference(*rx_scp)) {
1426                 if (rx_sc->sci == sci) {
1427                         if (rx_sc->active)
1428                                 secy->n_rx_sc--;
1429                         rcu_assign_pointer(*rx_scp, rx_sc->next);
1430                         return rx_sc;
1431                 }
1432         }
1433
1434         return NULL;
1435 }
1436
1437 static struct macsec_rx_sc *create_rx_sc(struct net_device *dev, sci_t sci)
1438 {
1439         struct macsec_rx_sc *rx_sc;
1440         struct macsec_dev *macsec;
1441         struct net_device *real_dev = macsec_priv(dev)->real_dev;
1442         struct macsec_rxh_data *rxd = macsec_data_rtnl(real_dev);
1443         struct macsec_secy *secy;
1444
1445         list_for_each_entry(macsec, &rxd->secys, secys) {
1446                 if (find_rx_sc_rtnl(&macsec->secy, sci))
1447                         return ERR_PTR(-EEXIST);
1448         }
1449
1450         rx_sc = kzalloc(sizeof(*rx_sc), GFP_KERNEL);
1451         if (!rx_sc)
1452                 return ERR_PTR(-ENOMEM);
1453
1454         rx_sc->stats = netdev_alloc_pcpu_stats(struct pcpu_rx_sc_stats);
1455         if (!rx_sc->stats) {
1456                 kfree(rx_sc);
1457                 return ERR_PTR(-ENOMEM);
1458         }
1459
1460         rx_sc->sci = sci;
1461         rx_sc->active = true;
1462         refcount_set(&rx_sc->refcnt, 1);
1463
1464         secy = &macsec_priv(dev)->secy;
1465         rcu_assign_pointer(rx_sc->next, secy->rx_sc);
1466         rcu_assign_pointer(secy->rx_sc, rx_sc);
1467
1468         if (rx_sc->active)
1469                 secy->n_rx_sc++;
1470
1471         return rx_sc;
1472 }
1473
1474 static int init_tx_sa(struct macsec_tx_sa *tx_sa, char *sak, int key_len,
1475                       int icv_len)
1476 {
1477         tx_sa->stats = alloc_percpu(struct macsec_tx_sa_stats);
1478         if (!tx_sa->stats)
1479                 return -ENOMEM;
1480
1481         tx_sa->key.tfm = macsec_alloc_tfm(sak, key_len, icv_len);
1482         if (IS_ERR(tx_sa->key.tfm)) {
1483                 free_percpu(tx_sa->stats);
1484                 return PTR_ERR(tx_sa->key.tfm);
1485         }
1486
1487         tx_sa->active = false;
1488         refcount_set(&tx_sa->refcnt, 1);
1489         spin_lock_init(&tx_sa->lock);
1490
1491         return 0;
1492 }
1493
1494 static void clear_tx_sa(struct macsec_tx_sa *tx_sa)
1495 {
1496         tx_sa->active = false;
1497
1498         macsec_txsa_put(tx_sa);
1499 }
1500
1501 static struct genl_family macsec_fam;
1502
1503 static struct net_device *get_dev_from_nl(struct net *net,
1504                                           struct nlattr **attrs)
1505 {
1506         int ifindex = nla_get_u32(attrs[MACSEC_ATTR_IFINDEX]);
1507         struct net_device *dev;
1508
1509         dev = __dev_get_by_index(net, ifindex);
1510         if (!dev)
1511                 return ERR_PTR(-ENODEV);
1512
1513         if (!netif_is_macsec(dev))
1514                 return ERR_PTR(-ENODEV);
1515
1516         return dev;
1517 }
1518
1519 static sci_t nla_get_sci(const struct nlattr *nla)
1520 {
1521         return (__force sci_t)nla_get_u64(nla);
1522 }
1523
1524 static int nla_put_sci(struct sk_buff *skb, int attrtype, sci_t value,
1525                        int padattr)
1526 {
1527         return nla_put_u64_64bit(skb, attrtype, (__force u64)value, padattr);
1528 }
1529
1530 static struct macsec_tx_sa *get_txsa_from_nl(struct net *net,
1531                                              struct nlattr **attrs,
1532                                              struct nlattr **tb_sa,
1533                                              struct net_device **devp,
1534                                              struct macsec_secy **secyp,
1535                                              struct macsec_tx_sc **scp,
1536                                              u8 *assoc_num)
1537 {
1538         struct net_device *dev;
1539         struct macsec_secy *secy;
1540         struct macsec_tx_sc *tx_sc;
1541         struct macsec_tx_sa *tx_sa;
1542
1543         if (!tb_sa[MACSEC_SA_ATTR_AN])
1544                 return ERR_PTR(-EINVAL);
1545
1546         *assoc_num = nla_get_u8(tb_sa[MACSEC_SA_ATTR_AN]);
1547
1548         dev = get_dev_from_nl(net, attrs);
1549         if (IS_ERR(dev))
1550                 return ERR_CAST(dev);
1551
1552         if (*assoc_num >= MACSEC_NUM_AN)
1553                 return ERR_PTR(-EINVAL);
1554
1555         secy = &macsec_priv(dev)->secy;
1556         tx_sc = &secy->tx_sc;
1557
1558         tx_sa = rtnl_dereference(tx_sc->sa[*assoc_num]);
1559         if (!tx_sa)
1560                 return ERR_PTR(-ENODEV);
1561
1562         *devp = dev;
1563         *scp = tx_sc;
1564         *secyp = secy;
1565         return tx_sa;
1566 }
1567
1568 static struct macsec_rx_sc *get_rxsc_from_nl(struct net *net,
1569                                              struct nlattr **attrs,
1570                                              struct nlattr **tb_rxsc,
1571                                              struct net_device **devp,
1572                                              struct macsec_secy **secyp)
1573 {
1574         struct net_device *dev;
1575         struct macsec_secy *secy;
1576         struct macsec_rx_sc *rx_sc;
1577         sci_t sci;
1578
1579         dev = get_dev_from_nl(net, attrs);
1580         if (IS_ERR(dev))
1581                 return ERR_CAST(dev);
1582
1583         secy = &macsec_priv(dev)->secy;
1584
1585         if (!tb_rxsc[MACSEC_RXSC_ATTR_SCI])
1586                 return ERR_PTR(-EINVAL);
1587
1588         sci = nla_get_sci(tb_rxsc[MACSEC_RXSC_ATTR_SCI]);
1589         rx_sc = find_rx_sc_rtnl(secy, sci);
1590         if (!rx_sc)
1591                 return ERR_PTR(-ENODEV);
1592
1593         *secyp = secy;
1594         *devp = dev;
1595
1596         return rx_sc;
1597 }
1598
1599 static struct macsec_rx_sa *get_rxsa_from_nl(struct net *net,
1600                                              struct nlattr **attrs,
1601                                              struct nlattr **tb_rxsc,
1602                                              struct nlattr **tb_sa,
1603                                              struct net_device **devp,
1604                                              struct macsec_secy **secyp,
1605                                              struct macsec_rx_sc **scp,
1606                                              u8 *assoc_num)
1607 {
1608         struct macsec_rx_sc *rx_sc;
1609         struct macsec_rx_sa *rx_sa;
1610
1611         if (!tb_sa[MACSEC_SA_ATTR_AN])
1612                 return ERR_PTR(-EINVAL);
1613
1614         *assoc_num = nla_get_u8(tb_sa[MACSEC_SA_ATTR_AN]);
1615         if (*assoc_num >= MACSEC_NUM_AN)
1616                 return ERR_PTR(-EINVAL);
1617
1618         rx_sc = get_rxsc_from_nl(net, attrs, tb_rxsc, devp, secyp);
1619         if (IS_ERR(rx_sc))
1620                 return ERR_CAST(rx_sc);
1621
1622         rx_sa = rtnl_dereference(rx_sc->sa[*assoc_num]);
1623         if (!rx_sa)
1624                 return ERR_PTR(-ENODEV);
1625
1626         *scp = rx_sc;
1627         return rx_sa;
1628 }
1629
1630
1631 static const struct nla_policy macsec_genl_policy[NUM_MACSEC_ATTR] = {
1632         [MACSEC_ATTR_IFINDEX] = { .type = NLA_U32 },
1633         [MACSEC_ATTR_RXSC_CONFIG] = { .type = NLA_NESTED },
1634         [MACSEC_ATTR_SA_CONFIG] = { .type = NLA_NESTED },
1635 };
1636
1637 static const struct nla_policy macsec_genl_rxsc_policy[NUM_MACSEC_RXSC_ATTR] = {
1638         [MACSEC_RXSC_ATTR_SCI] = { .type = NLA_U64 },
1639         [MACSEC_RXSC_ATTR_ACTIVE] = { .type = NLA_U8 },
1640 };
1641
1642 static const struct nla_policy macsec_genl_sa_policy[NUM_MACSEC_SA_ATTR] = {
1643         [MACSEC_SA_ATTR_AN] = { .type = NLA_U8 },
1644         [MACSEC_SA_ATTR_ACTIVE] = { .type = NLA_U8 },
1645         [MACSEC_SA_ATTR_PN] = { .type = NLA_U32 },
1646         [MACSEC_SA_ATTR_KEYID] = { .type = NLA_BINARY,
1647                                    .len = MACSEC_KEYID_LEN, },
1648         [MACSEC_SA_ATTR_KEY] = { .type = NLA_BINARY,
1649                                  .len = MACSEC_MAX_KEY_LEN, },
1650 };
1651
1652 static int parse_sa_config(struct nlattr **attrs, struct nlattr **tb_sa)
1653 {
1654         if (!attrs[MACSEC_ATTR_SA_CONFIG])
1655                 return -EINVAL;
1656
1657         if (nla_parse_nested(tb_sa, MACSEC_SA_ATTR_MAX,
1658                              attrs[MACSEC_ATTR_SA_CONFIG],
1659                              macsec_genl_sa_policy, NULL))
1660                 return -EINVAL;
1661
1662         return 0;
1663 }
1664
1665 static int parse_rxsc_config(struct nlattr **attrs, struct nlattr **tb_rxsc)
1666 {
1667         if (!attrs[MACSEC_ATTR_RXSC_CONFIG])
1668                 return -EINVAL;
1669
1670         if (nla_parse_nested(tb_rxsc, MACSEC_RXSC_ATTR_MAX,
1671                              attrs[MACSEC_ATTR_RXSC_CONFIG],
1672                              macsec_genl_rxsc_policy, NULL))
1673                 return -EINVAL;
1674
1675         return 0;
1676 }
1677
1678 static bool validate_add_rxsa(struct nlattr **attrs)
1679 {
1680         if (!attrs[MACSEC_SA_ATTR_AN] ||
1681             !attrs[MACSEC_SA_ATTR_KEY] ||
1682             !attrs[MACSEC_SA_ATTR_KEYID])
1683                 return false;
1684
1685         if (nla_get_u8(attrs[MACSEC_SA_ATTR_AN]) >= MACSEC_NUM_AN)
1686                 return false;
1687
1688         if (attrs[MACSEC_SA_ATTR_PN] && nla_get_u32(attrs[MACSEC_SA_ATTR_PN]) == 0)
1689                 return false;
1690
1691         if (attrs[MACSEC_SA_ATTR_ACTIVE]) {
1692                 if (nla_get_u8(attrs[MACSEC_SA_ATTR_ACTIVE]) > 1)
1693                         return false;
1694         }
1695
1696         if (nla_len(attrs[MACSEC_SA_ATTR_KEYID]) != MACSEC_KEYID_LEN)
1697                 return false;
1698
1699         return true;
1700 }
1701
1702 static int macsec_add_rxsa(struct sk_buff *skb, struct genl_info *info)
1703 {
1704         struct net_device *dev;
1705         struct nlattr **attrs = info->attrs;
1706         struct macsec_secy *secy;
1707         struct macsec_rx_sc *rx_sc;
1708         struct macsec_rx_sa *rx_sa;
1709         unsigned char assoc_num;
1710         struct nlattr *tb_rxsc[MACSEC_RXSC_ATTR_MAX + 1];
1711         struct nlattr *tb_sa[MACSEC_SA_ATTR_MAX + 1];
1712         int err;
1713
1714         if (!attrs[MACSEC_ATTR_IFINDEX])
1715                 return -EINVAL;
1716
1717         if (parse_sa_config(attrs, tb_sa))
1718                 return -EINVAL;
1719
1720         if (parse_rxsc_config(attrs, tb_rxsc))
1721                 return -EINVAL;
1722
1723         if (!validate_add_rxsa(tb_sa))
1724                 return -EINVAL;
1725
1726         rtnl_lock();
1727         rx_sc = get_rxsc_from_nl(genl_info_net(info), attrs, tb_rxsc, &dev, &secy);
1728         if (IS_ERR(rx_sc)) {
1729                 rtnl_unlock();
1730                 return PTR_ERR(rx_sc);
1731         }
1732
1733         assoc_num = nla_get_u8(tb_sa[MACSEC_SA_ATTR_AN]);
1734
1735         if (nla_len(tb_sa[MACSEC_SA_ATTR_KEY]) != secy->key_len) {
1736                 pr_notice("macsec: nl: add_rxsa: bad key length: %d != %d\n",
1737                           nla_len(tb_sa[MACSEC_SA_ATTR_KEY]), secy->key_len);
1738                 rtnl_unlock();
1739                 return -EINVAL;
1740         }
1741
1742         rx_sa = rtnl_dereference(rx_sc->sa[assoc_num]);
1743         if (rx_sa) {
1744                 rtnl_unlock();
1745                 return -EBUSY;
1746         }
1747
1748         rx_sa = kmalloc(sizeof(*rx_sa), GFP_KERNEL);
1749         if (!rx_sa) {
1750                 rtnl_unlock();
1751                 return -ENOMEM;
1752         }
1753
1754         err = init_rx_sa(rx_sa, nla_data(tb_sa[MACSEC_SA_ATTR_KEY]),
1755                          secy->key_len, secy->icv_len);
1756         if (err < 0) {
1757                 kfree(rx_sa);
1758                 rtnl_unlock();
1759                 return err;
1760         }
1761
1762         if (tb_sa[MACSEC_SA_ATTR_PN]) {
1763                 spin_lock_bh(&rx_sa->lock);
1764                 rx_sa->next_pn = nla_get_u32(tb_sa[MACSEC_SA_ATTR_PN]);
1765                 spin_unlock_bh(&rx_sa->lock);
1766         }
1767
1768         if (tb_sa[MACSEC_SA_ATTR_ACTIVE])
1769                 rx_sa->active = !!nla_get_u8(tb_sa[MACSEC_SA_ATTR_ACTIVE]);
1770
1771         nla_memcpy(rx_sa->key.id, tb_sa[MACSEC_SA_ATTR_KEYID], MACSEC_KEYID_LEN);
1772         rx_sa->sc = rx_sc;
1773         rcu_assign_pointer(rx_sc->sa[assoc_num], rx_sa);
1774
1775         rtnl_unlock();
1776
1777         return 0;
1778 }
1779
1780 static bool validate_add_rxsc(struct nlattr **attrs)
1781 {
1782         if (!attrs[MACSEC_RXSC_ATTR_SCI])
1783                 return false;
1784
1785         if (attrs[MACSEC_RXSC_ATTR_ACTIVE]) {
1786                 if (nla_get_u8(attrs[MACSEC_RXSC_ATTR_ACTIVE]) > 1)
1787                         return false;
1788         }
1789
1790         return true;
1791 }
1792
1793 static int macsec_add_rxsc(struct sk_buff *skb, struct genl_info *info)
1794 {
1795         struct net_device *dev;
1796         sci_t sci = MACSEC_UNDEF_SCI;
1797         struct nlattr **attrs = info->attrs;
1798         struct macsec_rx_sc *rx_sc;
1799         struct nlattr *tb_rxsc[MACSEC_RXSC_ATTR_MAX + 1];
1800
1801         if (!attrs[MACSEC_ATTR_IFINDEX])
1802                 return -EINVAL;
1803
1804         if (parse_rxsc_config(attrs, tb_rxsc))
1805                 return -EINVAL;
1806
1807         if (!validate_add_rxsc(tb_rxsc))
1808                 return -EINVAL;
1809
1810         rtnl_lock();
1811         dev = get_dev_from_nl(genl_info_net(info), attrs);
1812         if (IS_ERR(dev)) {
1813                 rtnl_unlock();
1814                 return PTR_ERR(dev);
1815         }
1816
1817         sci = nla_get_sci(tb_rxsc[MACSEC_RXSC_ATTR_SCI]);
1818
1819         rx_sc = create_rx_sc(dev, sci);
1820         if (IS_ERR(rx_sc)) {
1821                 rtnl_unlock();
1822                 return PTR_ERR(rx_sc);
1823         }
1824
1825         if (tb_rxsc[MACSEC_RXSC_ATTR_ACTIVE])
1826                 rx_sc->active = !!nla_get_u8(tb_rxsc[MACSEC_RXSC_ATTR_ACTIVE]);
1827
1828         rtnl_unlock();
1829
1830         return 0;
1831 }
1832
1833 static bool validate_add_txsa(struct nlattr **attrs)
1834 {
1835         if (!attrs[MACSEC_SA_ATTR_AN] ||
1836             !attrs[MACSEC_SA_ATTR_PN] ||
1837             !attrs[MACSEC_SA_ATTR_KEY] ||
1838             !attrs[MACSEC_SA_ATTR_KEYID])
1839                 return false;
1840
1841         if (nla_get_u8(attrs[MACSEC_SA_ATTR_AN]) >= MACSEC_NUM_AN)
1842                 return false;
1843
1844         if (nla_get_u32(attrs[MACSEC_SA_ATTR_PN]) == 0)
1845                 return false;
1846
1847         if (attrs[MACSEC_SA_ATTR_ACTIVE]) {
1848                 if (nla_get_u8(attrs[MACSEC_SA_ATTR_ACTIVE]) > 1)
1849                         return false;
1850         }
1851
1852         if (nla_len(attrs[MACSEC_SA_ATTR_KEYID]) != MACSEC_KEYID_LEN)
1853                 return false;
1854
1855         return true;
1856 }
1857
1858 static int macsec_add_txsa(struct sk_buff *skb, struct genl_info *info)
1859 {
1860         struct net_device *dev;
1861         struct nlattr **attrs = info->attrs;
1862         struct macsec_secy *secy;
1863         struct macsec_tx_sc *tx_sc;
1864         struct macsec_tx_sa *tx_sa;
1865         unsigned char assoc_num;
1866         struct nlattr *tb_sa[MACSEC_SA_ATTR_MAX + 1];
1867         int err;
1868
1869         if (!attrs[MACSEC_ATTR_IFINDEX])
1870                 return -EINVAL;
1871
1872         if (parse_sa_config(attrs, tb_sa))
1873                 return -EINVAL;
1874
1875         if (!validate_add_txsa(tb_sa))
1876                 return -EINVAL;
1877
1878         rtnl_lock();
1879         dev = get_dev_from_nl(genl_info_net(info), attrs);
1880         if (IS_ERR(dev)) {
1881                 rtnl_unlock();
1882                 return PTR_ERR(dev);
1883         }
1884
1885         secy = &macsec_priv(dev)->secy;
1886         tx_sc = &secy->tx_sc;
1887
1888         assoc_num = nla_get_u8(tb_sa[MACSEC_SA_ATTR_AN]);
1889
1890         if (nla_len(tb_sa[MACSEC_SA_ATTR_KEY]) != secy->key_len) {
1891                 pr_notice("macsec: nl: add_txsa: bad key length: %d != %d\n",
1892                           nla_len(tb_sa[MACSEC_SA_ATTR_KEY]), secy->key_len);
1893                 rtnl_unlock();
1894                 return -EINVAL;
1895         }
1896
1897         tx_sa = rtnl_dereference(tx_sc->sa[assoc_num]);
1898         if (tx_sa) {
1899                 rtnl_unlock();
1900                 return -EBUSY;
1901         }
1902
1903         tx_sa = kmalloc(sizeof(*tx_sa), GFP_KERNEL);
1904         if (!tx_sa) {
1905                 rtnl_unlock();
1906                 return -ENOMEM;
1907         }
1908
1909         err = init_tx_sa(tx_sa, nla_data(tb_sa[MACSEC_SA_ATTR_KEY]),
1910                          secy->key_len, secy->icv_len);
1911         if (err < 0) {
1912                 kfree(tx_sa);
1913                 rtnl_unlock();
1914                 return err;
1915         }
1916
1917         nla_memcpy(tx_sa->key.id, tb_sa[MACSEC_SA_ATTR_KEYID], MACSEC_KEYID_LEN);
1918
1919         spin_lock_bh(&tx_sa->lock);
1920         tx_sa->next_pn = nla_get_u32(tb_sa[MACSEC_SA_ATTR_PN]);
1921         spin_unlock_bh(&tx_sa->lock);
1922
1923         if (tb_sa[MACSEC_SA_ATTR_ACTIVE])
1924                 tx_sa->active = !!nla_get_u8(tb_sa[MACSEC_SA_ATTR_ACTIVE]);
1925
1926         if (assoc_num == tx_sc->encoding_sa && tx_sa->active)
1927                 secy->operational = true;
1928
1929         rcu_assign_pointer(tx_sc->sa[assoc_num], tx_sa);
1930
1931         rtnl_unlock();
1932
1933         return 0;
1934 }
1935
1936 static int macsec_del_rxsa(struct sk_buff *skb, struct genl_info *info)
1937 {
1938         struct nlattr **attrs = info->attrs;
1939         struct net_device *dev;
1940         struct macsec_secy *secy;
1941         struct macsec_rx_sc *rx_sc;
1942         struct macsec_rx_sa *rx_sa;
1943         u8 assoc_num;
1944         struct nlattr *tb_rxsc[MACSEC_RXSC_ATTR_MAX + 1];
1945         struct nlattr *tb_sa[MACSEC_SA_ATTR_MAX + 1];
1946
1947         if (!attrs[MACSEC_ATTR_IFINDEX])
1948                 return -EINVAL;
1949
1950         if (parse_sa_config(attrs, tb_sa))
1951                 return -EINVAL;
1952
1953         if (parse_rxsc_config(attrs, tb_rxsc))
1954                 return -EINVAL;
1955
1956         rtnl_lock();
1957         rx_sa = get_rxsa_from_nl(genl_info_net(info), attrs, tb_rxsc, tb_sa,
1958                                  &dev, &secy, &rx_sc, &assoc_num);
1959         if (IS_ERR(rx_sa)) {
1960                 rtnl_unlock();
1961                 return PTR_ERR(rx_sa);
1962         }
1963
1964         if (rx_sa->active) {
1965                 rtnl_unlock();
1966                 return -EBUSY;
1967         }
1968
1969         RCU_INIT_POINTER(rx_sc->sa[assoc_num], NULL);
1970         clear_rx_sa(rx_sa);
1971
1972         rtnl_unlock();
1973
1974         return 0;
1975 }
1976
1977 static int macsec_del_rxsc(struct sk_buff *skb, struct genl_info *info)
1978 {
1979         struct nlattr **attrs = info->attrs;
1980         struct net_device *dev;
1981         struct macsec_secy *secy;
1982         struct macsec_rx_sc *rx_sc;
1983         sci_t sci;
1984         struct nlattr *tb_rxsc[MACSEC_RXSC_ATTR_MAX + 1];
1985
1986         if (!attrs[MACSEC_ATTR_IFINDEX])
1987                 return -EINVAL;
1988
1989         if (parse_rxsc_config(attrs, tb_rxsc))
1990                 return -EINVAL;
1991
1992         if (!tb_rxsc[MACSEC_RXSC_ATTR_SCI])
1993                 return -EINVAL;
1994
1995         rtnl_lock();
1996         dev = get_dev_from_nl(genl_info_net(info), info->attrs);
1997         if (IS_ERR(dev)) {
1998                 rtnl_unlock();
1999                 return PTR_ERR(dev);
2000         }
2001
2002         secy = &macsec_priv(dev)->secy;
2003         sci = nla_get_sci(tb_rxsc[MACSEC_RXSC_ATTR_SCI]);
2004
2005         rx_sc = del_rx_sc(secy, sci);
2006         if (!rx_sc) {
2007                 rtnl_unlock();
2008                 return -ENODEV;
2009         }
2010
2011         free_rx_sc(rx_sc);
2012         rtnl_unlock();
2013
2014         return 0;
2015 }
2016
2017 static int macsec_del_txsa(struct sk_buff *skb, struct genl_info *info)
2018 {
2019         struct nlattr **attrs = info->attrs;
2020         struct net_device *dev;
2021         struct macsec_secy *secy;
2022         struct macsec_tx_sc *tx_sc;
2023         struct macsec_tx_sa *tx_sa;
2024         u8 assoc_num;
2025         struct nlattr *tb_sa[MACSEC_SA_ATTR_MAX + 1];
2026
2027         if (!attrs[MACSEC_ATTR_IFINDEX])
2028                 return -EINVAL;
2029
2030         if (parse_sa_config(attrs, tb_sa))
2031                 return -EINVAL;
2032
2033         rtnl_lock();
2034         tx_sa = get_txsa_from_nl(genl_info_net(info), attrs, tb_sa,
2035                                  &dev, &secy, &tx_sc, &assoc_num);
2036         if (IS_ERR(tx_sa)) {
2037                 rtnl_unlock();
2038                 return PTR_ERR(tx_sa);
2039         }
2040
2041         if (tx_sa->active) {
2042                 rtnl_unlock();
2043                 return -EBUSY;
2044         }
2045
2046         RCU_INIT_POINTER(tx_sc->sa[assoc_num], NULL);
2047         clear_tx_sa(tx_sa);
2048
2049         rtnl_unlock();
2050
2051         return 0;
2052 }
2053
2054 static bool validate_upd_sa(struct nlattr **attrs)
2055 {
2056         if (!attrs[MACSEC_SA_ATTR_AN] ||
2057             attrs[MACSEC_SA_ATTR_KEY] ||
2058             attrs[MACSEC_SA_ATTR_KEYID])
2059                 return false;
2060
2061         if (nla_get_u8(attrs[MACSEC_SA_ATTR_AN]) >= MACSEC_NUM_AN)
2062                 return false;
2063
2064         if (attrs[MACSEC_SA_ATTR_PN] && nla_get_u32(attrs[MACSEC_SA_ATTR_PN]) == 0)
2065                 return false;
2066
2067         if (attrs[MACSEC_SA_ATTR_ACTIVE]) {
2068                 if (nla_get_u8(attrs[MACSEC_SA_ATTR_ACTIVE]) > 1)
2069                         return false;
2070         }
2071
2072         return true;
2073 }
2074
2075 static int macsec_upd_txsa(struct sk_buff *skb, struct genl_info *info)
2076 {
2077         struct nlattr **attrs = info->attrs;
2078         struct net_device *dev;
2079         struct macsec_secy *secy;
2080         struct macsec_tx_sc *tx_sc;
2081         struct macsec_tx_sa *tx_sa;
2082         u8 assoc_num;
2083         struct nlattr *tb_sa[MACSEC_SA_ATTR_MAX + 1];
2084
2085         if (!attrs[MACSEC_ATTR_IFINDEX])
2086                 return -EINVAL;
2087
2088         if (parse_sa_config(attrs, tb_sa))
2089                 return -EINVAL;
2090
2091         if (!validate_upd_sa(tb_sa))
2092                 return -EINVAL;
2093
2094         rtnl_lock();
2095         tx_sa = get_txsa_from_nl(genl_info_net(info), attrs, tb_sa,
2096                                  &dev, &secy, &tx_sc, &assoc_num);
2097         if (IS_ERR(tx_sa)) {
2098                 rtnl_unlock();
2099                 return PTR_ERR(tx_sa);
2100         }
2101
2102         if (tb_sa[MACSEC_SA_ATTR_PN]) {
2103                 spin_lock_bh(&tx_sa->lock);
2104                 tx_sa->next_pn = nla_get_u32(tb_sa[MACSEC_SA_ATTR_PN]);
2105                 spin_unlock_bh(&tx_sa->lock);
2106         }
2107
2108         if (tb_sa[MACSEC_SA_ATTR_ACTIVE])
2109                 tx_sa->active = nla_get_u8(tb_sa[MACSEC_SA_ATTR_ACTIVE]);
2110
2111         if (assoc_num == tx_sc->encoding_sa)
2112                 secy->operational = tx_sa->active;
2113
2114         rtnl_unlock();
2115
2116         return 0;
2117 }
2118
2119 static int macsec_upd_rxsa(struct sk_buff *skb, struct genl_info *info)
2120 {
2121         struct nlattr **attrs = info->attrs;
2122         struct net_device *dev;
2123         struct macsec_secy *secy;
2124         struct macsec_rx_sc *rx_sc;
2125         struct macsec_rx_sa *rx_sa;
2126         u8 assoc_num;
2127         struct nlattr *tb_rxsc[MACSEC_RXSC_ATTR_MAX + 1];
2128         struct nlattr *tb_sa[MACSEC_SA_ATTR_MAX + 1];
2129
2130         if (!attrs[MACSEC_ATTR_IFINDEX])
2131                 return -EINVAL;
2132
2133         if (parse_rxsc_config(attrs, tb_rxsc))
2134                 return -EINVAL;
2135
2136         if (parse_sa_config(attrs, tb_sa))
2137                 return -EINVAL;
2138
2139         if (!validate_upd_sa(tb_sa))
2140                 return -EINVAL;
2141
2142         rtnl_lock();
2143         rx_sa = get_rxsa_from_nl(genl_info_net(info), attrs, tb_rxsc, tb_sa,
2144                                  &dev, &secy, &rx_sc, &assoc_num);
2145         if (IS_ERR(rx_sa)) {
2146                 rtnl_unlock();
2147                 return PTR_ERR(rx_sa);
2148         }
2149
2150         if (tb_sa[MACSEC_SA_ATTR_PN]) {
2151                 spin_lock_bh(&rx_sa->lock);
2152                 rx_sa->next_pn = nla_get_u32(tb_sa[MACSEC_SA_ATTR_PN]);
2153                 spin_unlock_bh(&rx_sa->lock);
2154         }
2155
2156         if (tb_sa[MACSEC_SA_ATTR_ACTIVE])
2157                 rx_sa->active = nla_get_u8(tb_sa[MACSEC_SA_ATTR_ACTIVE]);
2158
2159         rtnl_unlock();
2160         return 0;
2161 }
2162
2163 static int macsec_upd_rxsc(struct sk_buff *skb, struct genl_info *info)
2164 {
2165         struct nlattr **attrs = info->attrs;
2166         struct net_device *dev;
2167         struct macsec_secy *secy;
2168         struct macsec_rx_sc *rx_sc;
2169         struct nlattr *tb_rxsc[MACSEC_RXSC_ATTR_MAX + 1];
2170
2171         if (!attrs[MACSEC_ATTR_IFINDEX])
2172                 return -EINVAL;
2173
2174         if (parse_rxsc_config(attrs, tb_rxsc))
2175                 return -EINVAL;
2176
2177         if (!validate_add_rxsc(tb_rxsc))
2178                 return -EINVAL;
2179
2180         rtnl_lock();
2181         rx_sc = get_rxsc_from_nl(genl_info_net(info), attrs, tb_rxsc, &dev, &secy);
2182         if (IS_ERR(rx_sc)) {
2183                 rtnl_unlock();
2184                 return PTR_ERR(rx_sc);
2185         }
2186
2187         if (tb_rxsc[MACSEC_RXSC_ATTR_ACTIVE]) {
2188                 bool new = !!nla_get_u8(tb_rxsc[MACSEC_RXSC_ATTR_ACTIVE]);
2189
2190                 if (rx_sc->active != new)
2191                         secy->n_rx_sc += new ? 1 : -1;
2192
2193                 rx_sc->active = new;
2194         }
2195
2196         rtnl_unlock();
2197
2198         return 0;
2199 }
2200
2201 static int copy_tx_sa_stats(struct sk_buff *skb,
2202                              struct macsec_tx_sa_stats __percpu *pstats)
2203 {
2204         struct macsec_tx_sa_stats sum = {0, };
2205         int cpu;
2206
2207         for_each_possible_cpu(cpu) {
2208                 const struct macsec_tx_sa_stats *stats = per_cpu_ptr(pstats, cpu);
2209
2210                 sum.OutPktsProtected += stats->OutPktsProtected;
2211                 sum.OutPktsEncrypted += stats->OutPktsEncrypted;
2212         }
2213
2214         if (nla_put_u32(skb, MACSEC_SA_STATS_ATTR_OUT_PKTS_PROTECTED, sum.OutPktsProtected) ||
2215             nla_put_u32(skb, MACSEC_SA_STATS_ATTR_OUT_PKTS_ENCRYPTED, sum.OutPktsEncrypted))
2216                 return -EMSGSIZE;
2217
2218         return 0;
2219 }
2220
2221 static int copy_rx_sa_stats(struct sk_buff *skb,
2222                              struct macsec_rx_sa_stats __percpu *pstats)
2223 {
2224         struct macsec_rx_sa_stats sum = {0, };
2225         int cpu;
2226
2227         for_each_possible_cpu(cpu) {
2228                 const struct macsec_rx_sa_stats *stats = per_cpu_ptr(pstats, cpu);
2229
2230                 sum.InPktsOK         += stats->InPktsOK;
2231                 sum.InPktsInvalid    += stats->InPktsInvalid;
2232                 sum.InPktsNotValid   += stats->InPktsNotValid;
2233                 sum.InPktsNotUsingSA += stats->InPktsNotUsingSA;
2234                 sum.InPktsUnusedSA   += stats->InPktsUnusedSA;
2235         }
2236
2237         if (nla_put_u32(skb, MACSEC_SA_STATS_ATTR_IN_PKTS_OK, sum.InPktsOK) ||
2238             nla_put_u32(skb, MACSEC_SA_STATS_ATTR_IN_PKTS_INVALID, sum.InPktsInvalid) ||
2239             nla_put_u32(skb, MACSEC_SA_STATS_ATTR_IN_PKTS_NOT_VALID, sum.InPktsNotValid) ||
2240             nla_put_u32(skb, MACSEC_SA_STATS_ATTR_IN_PKTS_NOT_USING_SA, sum.InPktsNotUsingSA) ||
2241             nla_put_u32(skb, MACSEC_SA_STATS_ATTR_IN_PKTS_UNUSED_SA, sum.InPktsUnusedSA))
2242                 return -EMSGSIZE;
2243
2244         return 0;
2245 }
2246
2247 static int copy_rx_sc_stats(struct sk_buff *skb,
2248                              struct pcpu_rx_sc_stats __percpu *pstats)
2249 {
2250         struct macsec_rx_sc_stats sum = {0, };
2251         int cpu;
2252
2253         for_each_possible_cpu(cpu) {
2254                 const struct pcpu_rx_sc_stats *stats;
2255                 struct macsec_rx_sc_stats tmp;
2256                 unsigned int start;
2257
2258                 stats = per_cpu_ptr(pstats, cpu);
2259                 do {
2260                         start = u64_stats_fetch_begin_irq(&stats->syncp);
2261                         memcpy(&tmp, &stats->stats, sizeof(tmp));
2262                 } while (u64_stats_fetch_retry_irq(&stats->syncp, start));
2263
2264                 sum.InOctetsValidated += tmp.InOctetsValidated;
2265                 sum.InOctetsDecrypted += tmp.InOctetsDecrypted;
2266                 sum.InPktsUnchecked   += tmp.InPktsUnchecked;
2267                 sum.InPktsDelayed     += tmp.InPktsDelayed;
2268                 sum.InPktsOK          += tmp.InPktsOK;
2269                 sum.InPktsInvalid     += tmp.InPktsInvalid;
2270                 sum.InPktsLate        += tmp.InPktsLate;
2271                 sum.InPktsNotValid    += tmp.InPktsNotValid;
2272                 sum.InPktsNotUsingSA  += tmp.InPktsNotUsingSA;
2273                 sum.InPktsUnusedSA    += tmp.InPktsUnusedSA;
2274         }
2275
2276         if (nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_OCTETS_VALIDATED,
2277                               sum.InOctetsValidated,
2278                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2279             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_OCTETS_DECRYPTED,
2280                               sum.InOctetsDecrypted,
2281                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2282             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_PKTS_UNCHECKED,
2283                               sum.InPktsUnchecked,
2284                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2285             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_PKTS_DELAYED,
2286                               sum.InPktsDelayed,
2287                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2288             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_PKTS_OK,
2289                               sum.InPktsOK,
2290                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2291             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_PKTS_INVALID,
2292                               sum.InPktsInvalid,
2293                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2294             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_PKTS_LATE,
2295                               sum.InPktsLate,
2296                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2297             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_PKTS_NOT_VALID,
2298                               sum.InPktsNotValid,
2299                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2300             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_PKTS_NOT_USING_SA,
2301                               sum.InPktsNotUsingSA,
2302                               MACSEC_RXSC_STATS_ATTR_PAD) ||
2303             nla_put_u64_64bit(skb, MACSEC_RXSC_STATS_ATTR_IN_PKTS_UNUSED_SA,
2304                               sum.InPktsUnusedSA,
2305                               MACSEC_RXSC_STATS_ATTR_PAD))
2306                 return -EMSGSIZE;
2307
2308         return 0;
2309 }
2310
2311 static int copy_tx_sc_stats(struct sk_buff *skb,
2312                              struct pcpu_tx_sc_stats __percpu *pstats)
2313 {
2314         struct macsec_tx_sc_stats sum = {0, };
2315         int cpu;
2316
2317         for_each_possible_cpu(cpu) {
2318                 const struct pcpu_tx_sc_stats *stats;
2319                 struct macsec_tx_sc_stats tmp;
2320                 unsigned int start;
2321
2322                 stats = per_cpu_ptr(pstats, cpu);
2323                 do {
2324                         start = u64_stats_fetch_begin_irq(&stats->syncp);
2325                         memcpy(&tmp, &stats->stats, sizeof(tmp));
2326                 } while (u64_stats_fetch_retry_irq(&stats->syncp, start));
2327
2328                 sum.OutPktsProtected   += tmp.OutPktsProtected;
2329                 sum.OutPktsEncrypted   += tmp.OutPktsEncrypted;
2330                 sum.OutOctetsProtected += tmp.OutOctetsProtected;
2331                 sum.OutOctetsEncrypted += tmp.OutOctetsEncrypted;
2332         }
2333
2334         if (nla_put_u64_64bit(skb, MACSEC_TXSC_STATS_ATTR_OUT_PKTS_PROTECTED,
2335                               sum.OutPktsProtected,
2336                               MACSEC_TXSC_STATS_ATTR_PAD) ||
2337             nla_put_u64_64bit(skb, MACSEC_TXSC_STATS_ATTR_OUT_PKTS_ENCRYPTED,
2338                               sum.OutPktsEncrypted,
2339                               MACSEC_TXSC_STATS_ATTR_PAD) ||
2340             nla_put_u64_64bit(skb, MACSEC_TXSC_STATS_ATTR_OUT_OCTETS_PROTECTED,
2341                               sum.OutOctetsProtected,
2342                               MACSEC_TXSC_STATS_ATTR_PAD) ||
2343             nla_put_u64_64bit(skb, MACSEC_TXSC_STATS_ATTR_OUT_OCTETS_ENCRYPTED,
2344                               sum.OutOctetsEncrypted,
2345                               MACSEC_TXSC_STATS_ATTR_PAD))
2346                 return -EMSGSIZE;
2347
2348         return 0;
2349 }
2350
2351 static int copy_secy_stats(struct sk_buff *skb,
2352                             struct pcpu_secy_stats __percpu *pstats)
2353 {
2354         struct macsec_dev_stats sum = {0, };
2355         int cpu;
2356
2357         for_each_possible_cpu(cpu) {
2358                 const struct pcpu_secy_stats *stats;
2359                 struct macsec_dev_stats tmp;
2360                 unsigned int start;
2361
2362                 stats = per_cpu_ptr(pstats, cpu);
2363                 do {
2364                         start = u64_stats_fetch_begin_irq(&stats->syncp);
2365                         memcpy(&tmp, &stats->stats, sizeof(tmp));
2366                 } while (u64_stats_fetch_retry_irq(&stats->syncp, start));
2367
2368                 sum.OutPktsUntagged  += tmp.OutPktsUntagged;
2369                 sum.InPktsUntagged   += tmp.InPktsUntagged;
2370                 sum.OutPktsTooLong   += tmp.OutPktsTooLong;
2371                 sum.InPktsNoTag      += tmp.InPktsNoTag;
2372                 sum.InPktsBadTag     += tmp.InPktsBadTag;
2373                 sum.InPktsUnknownSCI += tmp.InPktsUnknownSCI;
2374                 sum.InPktsNoSCI      += tmp.InPktsNoSCI;
2375                 sum.InPktsOverrun    += tmp.InPktsOverrun;
2376         }
2377
2378         if (nla_put_u64_64bit(skb, MACSEC_SECY_STATS_ATTR_OUT_PKTS_UNTAGGED,
2379                               sum.OutPktsUntagged,
2380                               MACSEC_SECY_STATS_ATTR_PAD) ||
2381             nla_put_u64_64bit(skb, MACSEC_SECY_STATS_ATTR_IN_PKTS_UNTAGGED,
2382                               sum.InPktsUntagged,
2383                               MACSEC_SECY_STATS_ATTR_PAD) ||
2384             nla_put_u64_64bit(skb, MACSEC_SECY_STATS_ATTR_OUT_PKTS_TOO_LONG,
2385                               sum.OutPktsTooLong,
2386                               MACSEC_SECY_STATS_ATTR_PAD) ||
2387             nla_put_u64_64bit(skb, MACSEC_SECY_STATS_ATTR_IN_PKTS_NO_TAG,
2388                               sum.InPktsNoTag,
2389                               MACSEC_SECY_STATS_ATTR_PAD) ||
2390             nla_put_u64_64bit(skb, MACSEC_SECY_STATS_ATTR_IN_PKTS_BAD_TAG,
2391                               sum.InPktsBadTag,
2392                               MACSEC_SECY_STATS_ATTR_PAD) ||
2393             nla_put_u64_64bit(skb, MACSEC_SECY_STATS_ATTR_IN_PKTS_UNKNOWN_SCI,
2394                               sum.InPktsUnknownSCI,
2395                               MACSEC_SECY_STATS_ATTR_PAD) ||
2396             nla_put_u64_64bit(skb, MACSEC_SECY_STATS_ATTR_IN_PKTS_NO_SCI,
2397                               sum.InPktsNoSCI,
2398                               MACSEC_SECY_STATS_ATTR_PAD) ||
2399             nla_put_u64_64bit(skb, MACSEC_SECY_STATS_ATTR_IN_PKTS_OVERRUN,
2400                               sum.InPktsOverrun,
2401                               MACSEC_SECY_STATS_ATTR_PAD))
2402                 return -EMSGSIZE;
2403
2404         return 0;
2405 }
2406
2407 static int nla_put_secy(struct macsec_secy *secy, struct sk_buff *skb)
2408 {
2409         struct macsec_tx_sc *tx_sc = &secy->tx_sc;
2410         struct nlattr *secy_nest = nla_nest_start(skb, MACSEC_ATTR_SECY);
2411         u64 csid;
2412
2413         if (!secy_nest)
2414                 return 1;
2415
2416         switch (secy->key_len) {
2417         case MACSEC_GCM_AES_128_SAK_LEN:
2418                 csid = MACSEC_DEFAULT_CIPHER_ID;
2419                 break;
2420         case MACSEC_GCM_AES_256_SAK_LEN:
2421                 csid = MACSEC_CIPHER_ID_GCM_AES_256;
2422                 break;
2423         default:
2424                 goto cancel;
2425         }
2426
2427         if (nla_put_sci(skb, MACSEC_SECY_ATTR_SCI, secy->sci,
2428                         MACSEC_SECY_ATTR_PAD) ||
2429             nla_put_u64_64bit(skb, MACSEC_SECY_ATTR_CIPHER_SUITE,
2430                               csid, MACSEC_SECY_ATTR_PAD) ||
2431             nla_put_u8(skb, MACSEC_SECY_ATTR_ICV_LEN, secy->icv_len) ||
2432             nla_put_u8(skb, MACSEC_SECY_ATTR_OPER, secy->operational) ||
2433             nla_put_u8(skb, MACSEC_SECY_ATTR_PROTECT, secy->protect_frames) ||
2434             nla_put_u8(skb, MACSEC_SECY_ATTR_REPLAY, secy->replay_protect) ||
2435             nla_put_u8(skb, MACSEC_SECY_ATTR_VALIDATE, secy->validate_frames) ||
2436             nla_put_u8(skb, MACSEC_SECY_ATTR_ENCRYPT, tx_sc->encrypt) ||
2437             nla_put_u8(skb, MACSEC_SECY_ATTR_INC_SCI, tx_sc->send_sci) ||
2438             nla_put_u8(skb, MACSEC_SECY_ATTR_ES, tx_sc->end_station) ||
2439             nla_put_u8(skb, MACSEC_SECY_ATTR_SCB, tx_sc->scb) ||
2440             nla_put_u8(skb, MACSEC_SECY_ATTR_ENCODING_SA, tx_sc->encoding_sa))
2441                 goto cancel;
2442
2443         if (secy->replay_protect) {
2444                 if (nla_put_u32(skb, MACSEC_SECY_ATTR_WINDOW, secy->replay_window))
2445                         goto cancel;
2446         }
2447
2448         nla_nest_end(skb, secy_nest);
2449         return 0;
2450
2451 cancel:
2452         nla_nest_cancel(skb, secy_nest);
2453         return 1;
2454 }
2455
2456 static int dump_secy(struct macsec_secy *secy, struct net_device *dev,
2457                      struct sk_buff *skb, struct netlink_callback *cb)
2458 {
2459         struct macsec_rx_sc *rx_sc;
2460         struct macsec_tx_sc *tx_sc = &secy->tx_sc;
2461         struct nlattr *txsa_list, *rxsc_list;
2462         int i, j;
2463         void *hdr;
2464         struct nlattr *attr;
2465
2466         hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
2467                           &macsec_fam, NLM_F_MULTI, MACSEC_CMD_GET_TXSC);
2468         if (!hdr)
2469                 return -EMSGSIZE;
2470
2471         genl_dump_check_consistent(cb, hdr);
2472
2473         if (nla_put_u32(skb, MACSEC_ATTR_IFINDEX, dev->ifindex))
2474                 goto nla_put_failure;
2475
2476         if (nla_put_secy(secy, skb))
2477                 goto nla_put_failure;
2478
2479         attr = nla_nest_start(skb, MACSEC_ATTR_TXSC_STATS);
2480         if (!attr)
2481                 goto nla_put_failure;
2482         if (copy_tx_sc_stats(skb, tx_sc->stats)) {
2483                 nla_nest_cancel(skb, attr);
2484                 goto nla_put_failure;
2485         }
2486         nla_nest_end(skb, attr);
2487
2488         attr = nla_nest_start(skb, MACSEC_ATTR_SECY_STATS);
2489         if (!attr)
2490                 goto nla_put_failure;
2491         if (copy_secy_stats(skb, macsec_priv(dev)->stats)) {
2492                 nla_nest_cancel(skb, attr);
2493                 goto nla_put_failure;
2494         }
2495         nla_nest_end(skb, attr);
2496
2497         txsa_list = nla_nest_start(skb, MACSEC_ATTR_TXSA_LIST);
2498         if (!txsa_list)
2499                 goto nla_put_failure;
2500         for (i = 0, j = 1; i < MACSEC_NUM_AN; i++) {
2501                 struct macsec_tx_sa *tx_sa = rtnl_dereference(tx_sc->sa[i]);
2502                 struct nlattr *txsa_nest;
2503
2504                 if (!tx_sa)
2505                         continue;
2506
2507                 txsa_nest = nla_nest_start(skb, j++);
2508                 if (!txsa_nest) {
2509                         nla_nest_cancel(skb, txsa_list);
2510                         goto nla_put_failure;
2511                 }
2512
2513                 if (nla_put_u8(skb, MACSEC_SA_ATTR_AN, i) ||
2514                     nla_put_u32(skb, MACSEC_SA_ATTR_PN, tx_sa->next_pn) ||
2515                     nla_put(skb, MACSEC_SA_ATTR_KEYID, MACSEC_KEYID_LEN, tx_sa->key.id) ||
2516                     nla_put_u8(skb, MACSEC_SA_ATTR_ACTIVE, tx_sa->active)) {
2517                         nla_nest_cancel(skb, txsa_nest);
2518                         nla_nest_cancel(skb, txsa_list);
2519                         goto nla_put_failure;
2520                 }
2521
2522                 attr = nla_nest_start(skb, MACSEC_SA_ATTR_STATS);
2523                 if (!attr) {
2524                         nla_nest_cancel(skb, txsa_nest);
2525                         nla_nest_cancel(skb, txsa_list);
2526                         goto nla_put_failure;
2527                 }
2528                 if (copy_tx_sa_stats(skb, tx_sa->stats)) {
2529                         nla_nest_cancel(skb, attr);
2530                         nla_nest_cancel(skb, txsa_nest);
2531                         nla_nest_cancel(skb, txsa_list);
2532                         goto nla_put_failure;
2533                 }
2534                 nla_nest_end(skb, attr);
2535
2536                 nla_nest_end(skb, txsa_nest);
2537         }
2538         nla_nest_end(skb, txsa_list);
2539
2540         rxsc_list = nla_nest_start(skb, MACSEC_ATTR_RXSC_LIST);
2541         if (!rxsc_list)
2542                 goto nla_put_failure;
2543
2544         j = 1;
2545         for_each_rxsc_rtnl(secy, rx_sc) {
2546                 int k;
2547                 struct nlattr *rxsa_list;
2548                 struct nlattr *rxsc_nest = nla_nest_start(skb, j++);
2549
2550                 if (!rxsc_nest) {
2551                         nla_nest_cancel(skb, rxsc_list);
2552                         goto nla_put_failure;
2553                 }
2554
2555                 if (nla_put_u8(skb, MACSEC_RXSC_ATTR_ACTIVE, rx_sc->active) ||
2556                     nla_put_sci(skb, MACSEC_RXSC_ATTR_SCI, rx_sc->sci,
2557                                 MACSEC_RXSC_ATTR_PAD)) {
2558                         nla_nest_cancel(skb, rxsc_nest);
2559                         nla_nest_cancel(skb, rxsc_list);
2560                         goto nla_put_failure;
2561                 }
2562
2563                 attr = nla_nest_start(skb, MACSEC_RXSC_ATTR_STATS);
2564                 if (!attr) {
2565                         nla_nest_cancel(skb, rxsc_nest);
2566                         nla_nest_cancel(skb, rxsc_list);
2567                         goto nla_put_failure;
2568                 }
2569                 if (copy_rx_sc_stats(skb, rx_sc->stats)) {
2570                         nla_nest_cancel(skb, attr);
2571                         nla_nest_cancel(skb, rxsc_nest);
2572                         nla_nest_cancel(skb, rxsc_list);
2573                         goto nla_put_failure;
2574                 }
2575                 nla_nest_end(skb, attr);
2576
2577                 rxsa_list = nla_nest_start(skb, MACSEC_RXSC_ATTR_SA_LIST);
2578                 if (!rxsa_list) {
2579                         nla_nest_cancel(skb, rxsc_nest);
2580                         nla_nest_cancel(skb, rxsc_list);
2581                         goto nla_put_failure;
2582                 }
2583
2584                 for (i = 0, k = 1; i < MACSEC_NUM_AN; i++) {
2585                         struct macsec_rx_sa *rx_sa = rtnl_dereference(rx_sc->sa[i]);
2586                         struct nlattr *rxsa_nest;
2587
2588                         if (!rx_sa)
2589                                 continue;
2590
2591                         rxsa_nest = nla_nest_start(skb, k++);
2592                         if (!rxsa_nest) {
2593                                 nla_nest_cancel(skb, rxsa_list);
2594                                 nla_nest_cancel(skb, rxsc_nest);
2595                                 nla_nest_cancel(skb, rxsc_list);
2596                                 goto nla_put_failure;
2597                         }
2598
2599                         attr = nla_nest_start(skb, MACSEC_SA_ATTR_STATS);
2600                         if (!attr) {
2601                                 nla_nest_cancel(skb, rxsa_list);
2602                                 nla_nest_cancel(skb, rxsc_nest);
2603                                 nla_nest_cancel(skb, rxsc_list);
2604                                 goto nla_put_failure;
2605                         }
2606                         if (copy_rx_sa_stats(skb, rx_sa->stats)) {
2607                                 nla_nest_cancel(skb, attr);
2608                                 nla_nest_cancel(skb, rxsa_list);
2609                                 nla_nest_cancel(skb, rxsc_nest);
2610                                 nla_nest_cancel(skb, rxsc_list);
2611                                 goto nla_put_failure;
2612                         }
2613                         nla_nest_end(skb, attr);
2614
2615                         if (nla_put_u8(skb, MACSEC_SA_ATTR_AN, i) ||
2616                             nla_put_u32(skb, MACSEC_SA_ATTR_PN, rx_sa->next_pn) ||
2617                             nla_put(skb, MACSEC_SA_ATTR_KEYID, MACSEC_KEYID_LEN, rx_sa->key.id) ||
2618                             nla_put_u8(skb, MACSEC_SA_ATTR_ACTIVE, rx_sa->active)) {
2619                                 nla_nest_cancel(skb, rxsa_nest);
2620                                 nla_nest_cancel(skb, rxsc_nest);
2621                                 nla_nest_cancel(skb, rxsc_list);
2622                                 goto nla_put_failure;
2623                         }
2624                         nla_nest_end(skb, rxsa_nest);
2625                 }
2626
2627                 nla_nest_end(skb, rxsa_list);
2628                 nla_nest_end(skb, rxsc_nest);
2629         }
2630
2631         nla_nest_end(skb, rxsc_list);
2632
2633         genlmsg_end(skb, hdr);
2634
2635         return 0;
2636
2637 nla_put_failure:
2638         genlmsg_cancel(skb, hdr);
2639         return -EMSGSIZE;
2640 }
2641
2642 static int macsec_generation = 1; /* protected by RTNL */
2643
2644 static int macsec_dump_txsc(struct sk_buff *skb, struct netlink_callback *cb)
2645 {
2646         struct net *net = sock_net(skb->sk);
2647         struct net_device *dev;
2648         int dev_idx, d;
2649
2650         dev_idx = cb->args[0];
2651
2652         d = 0;
2653         rtnl_lock();
2654
2655         cb->seq = macsec_generation;
2656
2657         for_each_netdev(net, dev) {
2658                 struct macsec_secy *secy;
2659
2660                 if (d < dev_idx)
2661                         goto next;
2662
2663                 if (!netif_is_macsec(dev))
2664                         goto next;
2665
2666                 secy = &macsec_priv(dev)->secy;
2667                 if (dump_secy(secy, dev, skb, cb) < 0)
2668                         goto done;
2669 next:
2670                 d++;
2671         }
2672
2673 done:
2674         rtnl_unlock();
2675         cb->args[0] = d;
2676         return skb->len;
2677 }
2678
2679 static const struct genl_ops macsec_genl_ops[] = {
2680         {
2681                 .cmd = MACSEC_CMD_GET_TXSC,
2682                 .dumpit = macsec_dump_txsc,
2683                 .policy = macsec_genl_policy,
2684         },
2685         {
2686                 .cmd = MACSEC_CMD_ADD_RXSC,
2687                 .doit = macsec_add_rxsc,
2688                 .policy = macsec_genl_policy,
2689                 .flags = GENL_ADMIN_PERM,
2690         },
2691         {
2692                 .cmd = MACSEC_CMD_DEL_RXSC,
2693                 .doit = macsec_del_rxsc,
2694                 .policy = macsec_genl_policy,
2695                 .flags = GENL_ADMIN_PERM,
2696         },
2697         {
2698                 .cmd = MACSEC_CMD_UPD_RXSC,
2699                 .doit = macsec_upd_rxsc,
2700                 .policy = macsec_genl_policy,
2701                 .flags = GENL_ADMIN_PERM,
2702         },
2703         {
2704                 .cmd = MACSEC_CMD_ADD_TXSA,
2705                 .doit = macsec_add_txsa,
2706                 .policy = macsec_genl_policy,
2707                 .flags = GENL_ADMIN_PERM,
2708         },
2709         {
2710                 .cmd = MACSEC_CMD_DEL_TXSA,
2711                 .doit = macsec_del_txsa,
2712                 .policy = macsec_genl_policy,
2713                 .flags = GENL_ADMIN_PERM,
2714         },
2715         {
2716                 .cmd = MACSEC_CMD_UPD_TXSA,
2717                 .doit = macsec_upd_txsa,
2718                 .policy = macsec_genl_policy,
2719                 .flags = GENL_ADMIN_PERM,
2720         },
2721         {
2722                 .cmd = MACSEC_CMD_ADD_RXSA,
2723                 .doit = macsec_add_rxsa,
2724                 .policy = macsec_genl_policy,
2725                 .flags = GENL_ADMIN_PERM,
2726         },
2727         {
2728                 .cmd = MACSEC_CMD_DEL_RXSA,
2729                 .doit = macsec_del_rxsa,
2730                 .policy = macsec_genl_policy,
2731                 .flags = GENL_ADMIN_PERM,
2732         },
2733         {
2734                 .cmd = MACSEC_CMD_UPD_RXSA,
2735                 .doit = macsec_upd_rxsa,
2736                 .policy = macsec_genl_policy,
2737                 .flags = GENL_ADMIN_PERM,
2738         },
2739 };
2740
2741 static struct genl_family macsec_fam __ro_after_init = {
2742         .name           = MACSEC_GENL_NAME,
2743         .hdrsize        = 0,
2744         .version        = MACSEC_GENL_VERSION,
2745         .maxattr        = MACSEC_ATTR_MAX,
2746         .netnsok        = true,
2747         .module         = THIS_MODULE,
2748         .ops            = macsec_genl_ops,
2749         .n_ops          = ARRAY_SIZE(macsec_genl_ops),
2750 };
2751
2752 static netdev_tx_t macsec_start_xmit(struct sk_buff *skb,
2753                                      struct net_device *dev)
2754 {
2755         struct macsec_dev *macsec = netdev_priv(dev);
2756         struct macsec_secy *secy = &macsec->secy;
2757         struct pcpu_secy_stats *secy_stats;
2758         int ret, len;
2759
2760         /* 10.5 */
2761         if (!secy->protect_frames) {
2762                 secy_stats = this_cpu_ptr(macsec->stats);
2763                 u64_stats_update_begin(&secy_stats->syncp);
2764                 secy_stats->stats.OutPktsUntagged++;
2765                 u64_stats_update_end(&secy_stats->syncp);
2766                 skb->dev = macsec->real_dev;
2767                 len = skb->len;
2768                 ret = dev_queue_xmit(skb);
2769                 count_tx(dev, ret, len);
2770                 return ret;
2771         }
2772
2773         if (!secy->operational) {
2774                 kfree_skb(skb);
2775                 DEV_STATS_INC(dev, tx_dropped);
2776                 return NETDEV_TX_OK;
2777         }
2778
2779         len = skb->len;
2780         skb = macsec_encrypt(skb, dev);
2781         if (IS_ERR(skb)) {
2782                 if (PTR_ERR(skb) != -EINPROGRESS)
2783                         DEV_STATS_INC(dev, tx_dropped);
2784                 return NETDEV_TX_OK;
2785         }
2786
2787         macsec_count_tx(skb, &macsec->secy.tx_sc, macsec_skb_cb(skb)->tx_sa);
2788
2789         macsec_encrypt_finish(skb, dev);
2790         ret = dev_queue_xmit(skb);
2791         count_tx(dev, ret, len);
2792         return ret;
2793 }
2794
2795 #define MACSEC_FEATURES \
2796         (NETIF_F_SG | NETIF_F_HIGHDMA | NETIF_F_FRAGLIST)
2797 static struct lock_class_key macsec_netdev_addr_lock_key;
2798
2799 static int macsec_dev_init(struct net_device *dev)
2800 {
2801         struct macsec_dev *macsec = macsec_priv(dev);
2802         struct net_device *real_dev = macsec->real_dev;
2803         int err;
2804
2805         dev->tstats = netdev_alloc_pcpu_stats(struct pcpu_sw_netstats);
2806         if (!dev->tstats)
2807                 return -ENOMEM;
2808
2809         err = gro_cells_init(&macsec->gro_cells, dev);
2810         if (err) {
2811                 free_percpu(dev->tstats);
2812                 return err;
2813         }
2814
2815         dev->features = real_dev->features & MACSEC_FEATURES;
2816         dev->features |= NETIF_F_LLTX | NETIF_F_GSO_SOFTWARE;
2817
2818         dev->needed_headroom = real_dev->needed_headroom +
2819                                MACSEC_NEEDED_HEADROOM;
2820         dev->needed_tailroom = real_dev->needed_tailroom +
2821                                MACSEC_NEEDED_TAILROOM;
2822
2823         if (is_zero_ether_addr(dev->dev_addr))
2824                 eth_hw_addr_inherit(dev, real_dev);
2825         if (is_zero_ether_addr(dev->broadcast))
2826                 memcpy(dev->broadcast, real_dev->broadcast, dev->addr_len);
2827
2828         return 0;
2829 }
2830
2831 static void macsec_dev_uninit(struct net_device *dev)
2832 {
2833         struct macsec_dev *macsec = macsec_priv(dev);
2834
2835         gro_cells_destroy(&macsec->gro_cells);
2836         free_percpu(dev->tstats);
2837 }
2838
2839 static netdev_features_t macsec_fix_features(struct net_device *dev,
2840                                              netdev_features_t features)
2841 {
2842         struct macsec_dev *macsec = macsec_priv(dev);
2843         struct net_device *real_dev = macsec->real_dev;
2844
2845         features &= (real_dev->features & MACSEC_FEATURES) |
2846                     NETIF_F_GSO_SOFTWARE | NETIF_F_SOFT_FEATURES;
2847         features |= NETIF_F_LLTX;
2848
2849         return features;
2850 }
2851
2852 static int macsec_dev_open(struct net_device *dev)
2853 {
2854         struct macsec_dev *macsec = macsec_priv(dev);
2855         struct net_device *real_dev = macsec->real_dev;
2856         int err;
2857
2858         err = dev_uc_add(real_dev, dev->dev_addr);
2859         if (err < 0)
2860                 return err;
2861
2862         if (dev->flags & IFF_ALLMULTI) {
2863                 err = dev_set_allmulti(real_dev, 1);
2864                 if (err < 0)
2865                         goto del_unicast;
2866         }
2867
2868         if (dev->flags & IFF_PROMISC) {
2869                 err = dev_set_promiscuity(real_dev, 1);
2870                 if (err < 0)
2871                         goto clear_allmulti;
2872         }
2873
2874         if (netif_carrier_ok(real_dev))
2875                 netif_carrier_on(dev);
2876
2877         return 0;
2878 clear_allmulti:
2879         if (dev->flags & IFF_ALLMULTI)
2880                 dev_set_allmulti(real_dev, -1);
2881 del_unicast:
2882         dev_uc_del(real_dev, dev->dev_addr);
2883         netif_carrier_off(dev);
2884         return err;
2885 }
2886
2887 static int macsec_dev_stop(struct net_device *dev)
2888 {
2889         struct macsec_dev *macsec = macsec_priv(dev);
2890         struct net_device *real_dev = macsec->real_dev;
2891
2892         netif_carrier_off(dev);
2893
2894         dev_mc_unsync(real_dev, dev);
2895         dev_uc_unsync(real_dev, dev);
2896
2897         if (dev->flags & IFF_ALLMULTI)
2898                 dev_set_allmulti(real_dev, -1);
2899
2900         if (dev->flags & IFF_PROMISC)
2901                 dev_set_promiscuity(real_dev, -1);
2902
2903         dev_uc_del(real_dev, dev->dev_addr);
2904
2905         return 0;
2906 }
2907
2908 static void macsec_dev_change_rx_flags(struct net_device *dev, int change)
2909 {
2910         struct net_device *real_dev = macsec_priv(dev)->real_dev;
2911
2912         if (!(dev->flags & IFF_UP))
2913                 return;
2914
2915         if (change & IFF_ALLMULTI)
2916                 dev_set_allmulti(real_dev, dev->flags & IFF_ALLMULTI ? 1 : -1);
2917
2918         if (change & IFF_PROMISC)
2919                 dev_set_promiscuity(real_dev,
2920                                     dev->flags & IFF_PROMISC ? 1 : -1);
2921 }
2922
2923 static void macsec_dev_set_rx_mode(struct net_device *dev)
2924 {
2925         struct net_device *real_dev = macsec_priv(dev)->real_dev;
2926
2927         dev_mc_sync(real_dev, dev);
2928         dev_uc_sync(real_dev, dev);
2929 }
2930
2931 static sci_t dev_to_sci(struct net_device *dev, __be16 port)
2932 {
2933         return make_sci(dev->dev_addr, port);
2934 }
2935
2936 static int macsec_set_mac_address(struct net_device *dev, void *p)
2937 {
2938         struct macsec_dev *macsec = macsec_priv(dev);
2939         struct net_device *real_dev = macsec->real_dev;
2940         struct sockaddr *addr = p;
2941         int err;
2942
2943         if (!is_valid_ether_addr(addr->sa_data))
2944                 return -EADDRNOTAVAIL;
2945
2946         if (!(dev->flags & IFF_UP))
2947                 goto out;
2948
2949         err = dev_uc_add(real_dev, addr->sa_data);
2950         if (err < 0)
2951                 return err;
2952
2953         dev_uc_del(real_dev, dev->dev_addr);
2954
2955 out:
2956         ether_addr_copy(dev->dev_addr, addr->sa_data);
2957         macsec->secy.sci = dev_to_sci(dev, MACSEC_PORT_ES);
2958         return 0;
2959 }
2960
2961 static int macsec_change_mtu(struct net_device *dev, int new_mtu)
2962 {
2963         struct macsec_dev *macsec = macsec_priv(dev);
2964         unsigned int extra = macsec->secy.icv_len + macsec_extra_len(true);
2965
2966         if (macsec->real_dev->mtu - extra < new_mtu)
2967                 return -ERANGE;
2968
2969         dev->mtu = new_mtu;
2970
2971         return 0;
2972 }
2973
2974 static void macsec_get_stats64(struct net_device *dev,
2975                                struct rtnl_link_stats64 *s)
2976 {
2977         int cpu;
2978
2979         if (!dev->tstats)
2980                 return;
2981
2982         for_each_possible_cpu(cpu) {
2983                 struct pcpu_sw_netstats *stats;
2984                 struct pcpu_sw_netstats tmp;
2985                 int start;
2986
2987                 stats = per_cpu_ptr(dev->tstats, cpu);
2988                 do {
2989                         start = u64_stats_fetch_begin_irq(&stats->syncp);
2990                         tmp.rx_packets = stats->rx_packets;
2991                         tmp.rx_bytes   = stats->rx_bytes;
2992                         tmp.tx_packets = stats->tx_packets;
2993                         tmp.tx_bytes   = stats->tx_bytes;
2994                 } while (u64_stats_fetch_retry_irq(&stats->syncp, start));
2995
2996                 s->rx_packets += tmp.rx_packets;
2997                 s->rx_bytes   += tmp.rx_bytes;
2998                 s->tx_packets += tmp.tx_packets;
2999                 s->tx_bytes   += tmp.tx_bytes;
3000         }
3001
3002         s->rx_dropped = DEV_STATS_READ(dev, rx_dropped);
3003         s->tx_dropped = DEV_STATS_READ(dev, tx_dropped);
3004         s->rx_errors = DEV_STATS_READ(dev, rx_errors);
3005 }
3006
3007 static int macsec_get_iflink(const struct net_device *dev)
3008 {
3009         return macsec_priv(dev)->real_dev->ifindex;
3010 }
3011
3012
3013 static int macsec_get_nest_level(struct net_device *dev)
3014 {
3015         return macsec_priv(dev)->nest_level;
3016 }
3017
3018
3019 static const struct net_device_ops macsec_netdev_ops = {
3020         .ndo_init               = macsec_dev_init,
3021         .ndo_uninit             = macsec_dev_uninit,
3022         .ndo_open               = macsec_dev_open,
3023         .ndo_stop               = macsec_dev_stop,
3024         .ndo_fix_features       = macsec_fix_features,
3025         .ndo_change_mtu         = macsec_change_mtu,
3026         .ndo_set_rx_mode        = macsec_dev_set_rx_mode,
3027         .ndo_change_rx_flags    = macsec_dev_change_rx_flags,
3028         .ndo_set_mac_address    = macsec_set_mac_address,
3029         .ndo_start_xmit         = macsec_start_xmit,
3030         .ndo_get_stats64        = macsec_get_stats64,
3031         .ndo_get_iflink         = macsec_get_iflink,
3032         .ndo_get_lock_subclass  = macsec_get_nest_level,
3033 };
3034
3035 static const struct device_type macsec_type = {
3036         .name = "macsec",
3037 };
3038
3039 static const struct nla_policy macsec_rtnl_policy[IFLA_MACSEC_MAX + 1] = {
3040         [IFLA_MACSEC_SCI] = { .type = NLA_U64 },
3041         [IFLA_MACSEC_PORT] = { .type = NLA_U16 },
3042         [IFLA_MACSEC_ICV_LEN] = { .type = NLA_U8 },
3043         [IFLA_MACSEC_CIPHER_SUITE] = { .type = NLA_U64 },
3044         [IFLA_MACSEC_WINDOW] = { .type = NLA_U32 },
3045         [IFLA_MACSEC_ENCODING_SA] = { .type = NLA_U8 },
3046         [IFLA_MACSEC_ENCRYPT] = { .type = NLA_U8 },
3047         [IFLA_MACSEC_PROTECT] = { .type = NLA_U8 },
3048         [IFLA_MACSEC_INC_SCI] = { .type = NLA_U8 },
3049         [IFLA_MACSEC_ES] = { .type = NLA_U8 },
3050         [IFLA_MACSEC_SCB] = { .type = NLA_U8 },
3051         [IFLA_MACSEC_REPLAY_PROTECT] = { .type = NLA_U8 },
3052         [IFLA_MACSEC_VALIDATION] = { .type = NLA_U8 },
3053 };
3054
3055 static void macsec_free_netdev(struct net_device *dev)
3056 {
3057         struct macsec_dev *macsec = macsec_priv(dev);
3058
3059         free_percpu(macsec->stats);
3060         free_percpu(macsec->secy.tx_sc.stats);
3061
3062 }
3063
3064 static void macsec_setup(struct net_device *dev)
3065 {
3066         ether_setup(dev);
3067         dev->min_mtu = 0;
3068         dev->max_mtu = ETH_MAX_MTU;
3069         dev->priv_flags |= IFF_NO_QUEUE;
3070         dev->netdev_ops = &macsec_netdev_ops;
3071         dev->needs_free_netdev = true;
3072         dev->priv_destructor = macsec_free_netdev;
3073         SET_NETDEV_DEVTYPE(dev, &macsec_type);
3074
3075         eth_zero_addr(dev->broadcast);
3076 }
3077
3078 static int macsec_changelink_common(struct net_device *dev,
3079                                     struct nlattr *data[])
3080 {
3081         struct macsec_secy *secy;
3082         struct macsec_tx_sc *tx_sc;
3083
3084         secy = &macsec_priv(dev)->secy;
3085         tx_sc = &secy->tx_sc;
3086
3087         if (data[IFLA_MACSEC_ENCODING_SA]) {
3088                 struct macsec_tx_sa *tx_sa;
3089
3090                 tx_sc->encoding_sa = nla_get_u8(data[IFLA_MACSEC_ENCODING_SA]);
3091                 tx_sa = rtnl_dereference(tx_sc->sa[tx_sc->encoding_sa]);
3092
3093                 secy->operational = tx_sa && tx_sa->active;
3094         }
3095
3096         if (data[IFLA_MACSEC_WINDOW])
3097                 secy->replay_window = nla_get_u32(data[IFLA_MACSEC_WINDOW]);
3098
3099         if (data[IFLA_MACSEC_ENCRYPT])
3100                 tx_sc->encrypt = !!nla_get_u8(data[IFLA_MACSEC_ENCRYPT]);
3101
3102         if (data[IFLA_MACSEC_PROTECT])
3103                 secy->protect_frames = !!nla_get_u8(data[IFLA_MACSEC_PROTECT]);
3104
3105         if (data[IFLA_MACSEC_INC_SCI])
3106                 tx_sc->send_sci = !!nla_get_u8(data[IFLA_MACSEC_INC_SCI]);
3107
3108         if (data[IFLA_MACSEC_ES])
3109                 tx_sc->end_station = !!nla_get_u8(data[IFLA_MACSEC_ES]);
3110
3111         if (data[IFLA_MACSEC_SCB])
3112                 tx_sc->scb = !!nla_get_u8(data[IFLA_MACSEC_SCB]);
3113
3114         if (data[IFLA_MACSEC_REPLAY_PROTECT])
3115                 secy->replay_protect = !!nla_get_u8(data[IFLA_MACSEC_REPLAY_PROTECT]);
3116
3117         if (data[IFLA_MACSEC_VALIDATION])
3118                 secy->validate_frames = nla_get_u8(data[IFLA_MACSEC_VALIDATION]);
3119
3120         if (data[IFLA_MACSEC_CIPHER_SUITE]) {
3121                 switch (nla_get_u64(data[IFLA_MACSEC_CIPHER_SUITE])) {
3122                 case MACSEC_CIPHER_ID_GCM_AES_128:
3123                 case MACSEC_DEFAULT_CIPHER_ID:
3124                         secy->key_len = MACSEC_GCM_AES_128_SAK_LEN;
3125                         break;
3126                 case MACSEC_CIPHER_ID_GCM_AES_256:
3127                         secy->key_len = MACSEC_GCM_AES_256_SAK_LEN;
3128                         break;
3129                 default:
3130                         return -EINVAL;
3131                 }
3132         }
3133
3134         return 0;
3135 }
3136
3137 static int macsec_changelink(struct net_device *dev, struct nlattr *tb[],
3138                              struct nlattr *data[],
3139                              struct netlink_ext_ack *extack)
3140 {
3141         if (!data)
3142                 return 0;
3143
3144         if (data[IFLA_MACSEC_CIPHER_SUITE] ||
3145             data[IFLA_MACSEC_ICV_LEN] ||
3146             data[IFLA_MACSEC_SCI] ||
3147             data[IFLA_MACSEC_PORT])
3148                 return -EINVAL;
3149
3150         return macsec_changelink_common(dev, data);
3151 }
3152
3153 static void macsec_del_dev(struct macsec_dev *macsec)
3154 {
3155         int i;
3156
3157         while (macsec->secy.rx_sc) {
3158                 struct macsec_rx_sc *rx_sc = rtnl_dereference(macsec->secy.rx_sc);
3159
3160                 rcu_assign_pointer(macsec->secy.rx_sc, rx_sc->next);
3161                 free_rx_sc(rx_sc);
3162         }
3163
3164         for (i = 0; i < MACSEC_NUM_AN; i++) {
3165                 struct macsec_tx_sa *sa = rtnl_dereference(macsec->secy.tx_sc.sa[i]);
3166
3167                 if (sa) {
3168                         RCU_INIT_POINTER(macsec->secy.tx_sc.sa[i], NULL);
3169                         clear_tx_sa(sa);
3170                 }
3171         }
3172 }
3173
3174 static void macsec_common_dellink(struct net_device *dev, struct list_head *head)
3175 {
3176         struct macsec_dev *macsec = macsec_priv(dev);
3177         struct net_device *real_dev = macsec->real_dev;
3178
3179         unregister_netdevice_queue(dev, head);
3180         list_del_rcu(&macsec->secys);
3181         macsec_del_dev(macsec);
3182         netdev_upper_dev_unlink(real_dev, dev);
3183
3184         macsec_generation++;
3185 }
3186
3187 static void macsec_dellink(struct net_device *dev, struct list_head *head)
3188 {
3189         struct macsec_dev *macsec = macsec_priv(dev);
3190         struct net_device *real_dev = macsec->real_dev;
3191         struct macsec_rxh_data *rxd = macsec_data_rtnl(real_dev);
3192
3193         macsec_common_dellink(dev, head);
3194
3195         if (list_empty(&rxd->secys)) {
3196                 netdev_rx_handler_unregister(real_dev);
3197                 kfree(rxd);
3198         }
3199 }
3200
3201 static int register_macsec_dev(struct net_device *real_dev,
3202                                struct net_device *dev)
3203 {
3204         struct macsec_dev *macsec = macsec_priv(dev);
3205         struct macsec_rxh_data *rxd = macsec_data_rtnl(real_dev);
3206
3207         if (!rxd) {
3208                 int err;
3209
3210                 rxd = kmalloc(sizeof(*rxd), GFP_KERNEL);
3211                 if (!rxd)
3212                         return -ENOMEM;
3213
3214                 INIT_LIST_HEAD(&rxd->secys);
3215
3216                 err = netdev_rx_handler_register(real_dev, macsec_handle_frame,
3217                                                  rxd);
3218                 if (err < 0) {
3219                         kfree(rxd);
3220                         return err;
3221                 }
3222         }
3223
3224         list_add_tail_rcu(&macsec->secys, &rxd->secys);
3225         return 0;
3226 }
3227
3228 static bool sci_exists(struct net_device *dev, sci_t sci)
3229 {
3230         struct macsec_rxh_data *rxd = macsec_data_rtnl(dev);
3231         struct macsec_dev *macsec;
3232
3233         list_for_each_entry(macsec, &rxd->secys, secys) {
3234                 if (macsec->secy.sci == sci)
3235                         return true;
3236         }
3237
3238         return false;
3239 }
3240
3241 static int macsec_add_dev(struct net_device *dev, sci_t sci, u8 icv_len)
3242 {
3243         struct macsec_dev *macsec = macsec_priv(dev);
3244         struct macsec_secy *secy = &macsec->secy;
3245
3246         macsec->stats = netdev_alloc_pcpu_stats(struct pcpu_secy_stats);
3247         if (!macsec->stats)
3248                 return -ENOMEM;
3249
3250         secy->tx_sc.stats = netdev_alloc_pcpu_stats(struct pcpu_tx_sc_stats);
3251         if (!secy->tx_sc.stats) {
3252                 free_percpu(macsec->stats);
3253                 return -ENOMEM;
3254         }
3255
3256         if (sci == MACSEC_UNDEF_SCI)
3257                 sci = dev_to_sci(dev, MACSEC_PORT_ES);
3258
3259         secy->netdev = dev;
3260         secy->operational = true;
3261         secy->key_len = DEFAULT_SAK_LEN;
3262         secy->icv_len = icv_len;
3263         secy->validate_frames = MACSEC_VALIDATE_DEFAULT;
3264         secy->protect_frames = true;
3265         secy->replay_protect = false;
3266
3267         secy->sci = sci;
3268         secy->tx_sc.active = true;
3269         secy->tx_sc.encoding_sa = DEFAULT_ENCODING_SA;
3270         secy->tx_sc.encrypt = DEFAULT_ENCRYPT;
3271         secy->tx_sc.send_sci = DEFAULT_SEND_SCI;
3272         secy->tx_sc.end_station = false;
3273         secy->tx_sc.scb = false;
3274
3275         return 0;
3276 }
3277
3278 static int macsec_newlink(struct net *net, struct net_device *dev,
3279                           struct nlattr *tb[], struct nlattr *data[],
3280                           struct netlink_ext_ack *extack)
3281 {
3282         struct macsec_dev *macsec = macsec_priv(dev);
3283         rx_handler_func_t *rx_handler;
3284         u8 icv_len = DEFAULT_ICV_LEN;
3285         struct net_device *real_dev;
3286         int err, mtu;
3287         sci_t sci;
3288
3289         if (!tb[IFLA_LINK])
3290                 return -EINVAL;
3291         real_dev = __dev_get_by_index(net, nla_get_u32(tb[IFLA_LINK]));
3292         if (!real_dev)
3293                 return -ENODEV;
3294         if (real_dev->type != ARPHRD_ETHER)
3295                 return -EINVAL;
3296
3297         dev->priv_flags |= IFF_MACSEC;
3298
3299         macsec->real_dev = real_dev;
3300
3301         /* send_sci must be set to true when transmit sci explicitly is set */
3302         if ((data && data[IFLA_MACSEC_SCI]) &&
3303             (data && data[IFLA_MACSEC_INC_SCI])) {
3304                 u8 send_sci = !!nla_get_u8(data[IFLA_MACSEC_INC_SCI]);
3305
3306                 if (!send_sci)
3307                         return -EINVAL;
3308         }
3309
3310         if (data && data[IFLA_MACSEC_ICV_LEN])
3311                 icv_len = nla_get_u8(data[IFLA_MACSEC_ICV_LEN]);
3312         mtu = real_dev->mtu - icv_len - macsec_extra_len(true);
3313         if (mtu < 0)
3314                 dev->mtu = 0;
3315         else
3316                 dev->mtu = mtu;
3317
3318         rx_handler = rtnl_dereference(real_dev->rx_handler);
3319         if (rx_handler && rx_handler != macsec_handle_frame)
3320                 return -EBUSY;
3321
3322         err = register_netdevice(dev);
3323         if (err < 0)
3324                 return err;
3325
3326         macsec->nest_level = dev_get_nest_level(real_dev) + 1;
3327         netdev_lockdep_set_classes(dev);
3328         lockdep_set_class_and_subclass(&dev->addr_list_lock,
3329                                        &macsec_netdev_addr_lock_key,
3330                                        macsec_get_nest_level(dev));
3331
3332         err = netdev_upper_dev_link(real_dev, dev, extack);
3333         if (err < 0)
3334                 goto unregister;
3335
3336         /* need to be already registered so that ->init has run and
3337          * the MAC addr is set
3338          */
3339         if (data && data[IFLA_MACSEC_SCI])
3340                 sci = nla_get_sci(data[IFLA_MACSEC_SCI]);
3341         else if (data && data[IFLA_MACSEC_PORT])
3342                 sci = dev_to_sci(dev, nla_get_be16(data[IFLA_MACSEC_PORT]));
3343         else
3344                 sci = dev_to_sci(dev, MACSEC_PORT_ES);
3345
3346         if (rx_handler && sci_exists(real_dev, sci)) {
3347                 err = -EBUSY;
3348                 goto unlink;
3349         }
3350
3351         err = macsec_add_dev(dev, sci, icv_len);
3352         if (err)
3353                 goto unlink;
3354
3355         if (data) {
3356                 err = macsec_changelink_common(dev, data);
3357                 if (err)
3358                         goto del_dev;
3359         }
3360
3361         err = register_macsec_dev(real_dev, dev);
3362         if (err < 0)
3363                 goto del_dev;
3364
3365         netif_stacked_transfer_operstate(real_dev, dev);
3366         linkwatch_fire_event(dev);
3367
3368         macsec_generation++;
3369
3370         return 0;
3371
3372 del_dev:
3373         macsec_del_dev(macsec);
3374 unlink:
3375         netdev_upper_dev_unlink(real_dev, dev);
3376 unregister:
3377         unregister_netdevice(dev);
3378         return err;
3379 }
3380
3381 static int macsec_validate_attr(struct nlattr *tb[], struct nlattr *data[],
3382                                 struct netlink_ext_ack *extack)
3383 {
3384         u64 csid = MACSEC_DEFAULT_CIPHER_ID;
3385         u8 icv_len = DEFAULT_ICV_LEN;
3386         int flag;
3387         bool es, scb, sci;
3388
3389         if (!data)
3390                 return 0;
3391
3392         if (data[IFLA_MACSEC_CIPHER_SUITE])
3393                 csid = nla_get_u64(data[IFLA_MACSEC_CIPHER_SUITE]);
3394
3395         if (data[IFLA_MACSEC_ICV_LEN]) {
3396                 icv_len = nla_get_u8(data[IFLA_MACSEC_ICV_LEN]);
3397                 if (icv_len != DEFAULT_ICV_LEN) {
3398                         char dummy_key[DEFAULT_SAK_LEN] = { 0 };
3399                         struct crypto_aead *dummy_tfm;
3400
3401                         dummy_tfm = macsec_alloc_tfm(dummy_key,
3402                                                      DEFAULT_SAK_LEN,
3403                                                      icv_len);
3404                         if (IS_ERR(dummy_tfm))
3405                                 return PTR_ERR(dummy_tfm);
3406                         crypto_free_aead(dummy_tfm);
3407                 }
3408         }
3409
3410         switch (csid) {
3411         case MACSEC_CIPHER_ID_GCM_AES_128:
3412         case MACSEC_CIPHER_ID_GCM_AES_256:
3413         case MACSEC_DEFAULT_CIPHER_ID:
3414                 if (icv_len < MACSEC_MIN_ICV_LEN ||
3415                     icv_len > MACSEC_STD_ICV_LEN)
3416                         return -EINVAL;
3417                 break;
3418         default:
3419                 return -EINVAL;
3420         }
3421
3422         if (data[IFLA_MACSEC_ENCODING_SA]) {
3423                 if (nla_get_u8(data[IFLA_MACSEC_ENCODING_SA]) >= MACSEC_NUM_AN)
3424                         return -EINVAL;
3425         }
3426
3427         for (flag = IFLA_MACSEC_ENCODING_SA + 1;
3428              flag < IFLA_MACSEC_VALIDATION;
3429              flag++) {
3430                 if (data[flag]) {
3431                         if (nla_get_u8(data[flag]) > 1)
3432                                 return -EINVAL;
3433                 }
3434         }
3435
3436         es  = data[IFLA_MACSEC_ES] ? nla_get_u8(data[IFLA_MACSEC_ES]) : false;
3437         sci = data[IFLA_MACSEC_INC_SCI] ? nla_get_u8(data[IFLA_MACSEC_INC_SCI]) : false;
3438         scb = data[IFLA_MACSEC_SCB] ? nla_get_u8(data[IFLA_MACSEC_SCB]) : false;
3439
3440         if ((sci && (scb || es)) || (scb && es))
3441                 return -EINVAL;
3442
3443         if (data[IFLA_MACSEC_VALIDATION] &&
3444             nla_get_u8(data[IFLA_MACSEC_VALIDATION]) > MACSEC_VALIDATE_MAX)
3445                 return -EINVAL;
3446
3447         if ((data[IFLA_MACSEC_REPLAY_PROTECT] &&
3448              nla_get_u8(data[IFLA_MACSEC_REPLAY_PROTECT])) &&
3449             !data[IFLA_MACSEC_WINDOW])
3450                 return -EINVAL;
3451
3452         return 0;
3453 }
3454
3455 static struct net *macsec_get_link_net(const struct net_device *dev)
3456 {
3457         return dev_net(macsec_priv(dev)->real_dev);
3458 }
3459
3460 static size_t macsec_get_size(const struct net_device *dev)
3461 {
3462         return  nla_total_size_64bit(8) + /* IFLA_MACSEC_SCI */
3463                 nla_total_size(1) + /* IFLA_MACSEC_ICV_LEN */
3464                 nla_total_size_64bit(8) + /* IFLA_MACSEC_CIPHER_SUITE */
3465                 nla_total_size(4) + /* IFLA_MACSEC_WINDOW */
3466                 nla_total_size(1) + /* IFLA_MACSEC_ENCODING_SA */
3467                 nla_total_size(1) + /* IFLA_MACSEC_ENCRYPT */
3468                 nla_total_size(1) + /* IFLA_MACSEC_PROTECT */
3469                 nla_total_size(1) + /* IFLA_MACSEC_INC_SCI */
3470                 nla_total_size(1) + /* IFLA_MACSEC_ES */
3471                 nla_total_size(1) + /* IFLA_MACSEC_SCB */
3472                 nla_total_size(1) + /* IFLA_MACSEC_REPLAY_PROTECT */
3473                 nla_total_size(1) + /* IFLA_MACSEC_VALIDATION */
3474                 0;
3475 }
3476
3477 static int macsec_fill_info(struct sk_buff *skb,
3478                             const struct net_device *dev)
3479 {
3480         struct macsec_secy *secy = &macsec_priv(dev)->secy;
3481         struct macsec_tx_sc *tx_sc = &secy->tx_sc;
3482         u64 csid;
3483
3484         switch (secy->key_len) {
3485         case MACSEC_GCM_AES_128_SAK_LEN:
3486                 csid = MACSEC_DEFAULT_CIPHER_ID;
3487                 break;
3488         case MACSEC_GCM_AES_256_SAK_LEN:
3489                 csid = MACSEC_CIPHER_ID_GCM_AES_256;
3490                 break;
3491         default:
3492                 goto nla_put_failure;
3493         }
3494
3495         if (nla_put_sci(skb, IFLA_MACSEC_SCI, secy->sci,
3496                         IFLA_MACSEC_PAD) ||
3497             nla_put_u8(skb, IFLA_MACSEC_ICV_LEN, secy->icv_len) ||
3498             nla_put_u64_64bit(skb, IFLA_MACSEC_CIPHER_SUITE,
3499                               csid, IFLA_MACSEC_PAD) ||
3500             nla_put_u8(skb, IFLA_MACSEC_ENCODING_SA, tx_sc->encoding_sa) ||
3501             nla_put_u8(skb, IFLA_MACSEC_ENCRYPT, tx_sc->encrypt) ||
3502             nla_put_u8(skb, IFLA_MACSEC_PROTECT, secy->protect_frames) ||
3503             nla_put_u8(skb, IFLA_MACSEC_INC_SCI, tx_sc->send_sci) ||
3504             nla_put_u8(skb, IFLA_MACSEC_ES, tx_sc->end_station) ||
3505             nla_put_u8(skb, IFLA_MACSEC_SCB, tx_sc->scb) ||
3506             nla_put_u8(skb, IFLA_MACSEC_REPLAY_PROTECT, secy->replay_protect) ||
3507             nla_put_u8(skb, IFLA_MACSEC_VALIDATION, secy->validate_frames) ||
3508             0)
3509                 goto nla_put_failure;
3510
3511         if (secy->replay_protect) {
3512                 if (nla_put_u32(skb, IFLA_MACSEC_WINDOW, secy->replay_window))
3513                         goto nla_put_failure;
3514         }
3515
3516         return 0;
3517
3518 nla_put_failure:
3519         return -EMSGSIZE;
3520 }
3521
3522 static struct rtnl_link_ops macsec_link_ops __read_mostly = {
3523         .kind           = "macsec",
3524         .priv_size      = sizeof(struct macsec_dev),
3525         .maxtype        = IFLA_MACSEC_MAX,
3526         .policy         = macsec_rtnl_policy,
3527         .setup          = macsec_setup,
3528         .validate       = macsec_validate_attr,
3529         .newlink        = macsec_newlink,
3530         .changelink     = macsec_changelink,
3531         .dellink        = macsec_dellink,
3532         .get_size       = macsec_get_size,
3533         .fill_info      = macsec_fill_info,
3534         .get_link_net   = macsec_get_link_net,
3535 };
3536
3537 static bool is_macsec_master(struct net_device *dev)
3538 {
3539         return rcu_access_pointer(dev->rx_handler) == macsec_handle_frame;
3540 }
3541
3542 static int macsec_notify(struct notifier_block *this, unsigned long event,
3543                          void *ptr)
3544 {
3545         struct net_device *real_dev = netdev_notifier_info_to_dev(ptr);
3546         LIST_HEAD(head);
3547
3548         if (!is_macsec_master(real_dev))
3549                 return NOTIFY_DONE;
3550
3551         switch (event) {
3552         case NETDEV_DOWN:
3553         case NETDEV_UP:
3554         case NETDEV_CHANGE: {
3555                 struct macsec_dev *m, *n;
3556                 struct macsec_rxh_data *rxd;
3557
3558                 rxd = macsec_data_rtnl(real_dev);
3559                 list_for_each_entry_safe(m, n, &rxd->secys, secys) {
3560                         struct net_device *dev = m->secy.netdev;
3561
3562                         netif_stacked_transfer_operstate(real_dev, dev);
3563                 }
3564                 break;
3565         }
3566         case NETDEV_UNREGISTER: {
3567                 struct macsec_dev *m, *n;
3568                 struct macsec_rxh_data *rxd;
3569
3570                 rxd = macsec_data_rtnl(real_dev);
3571                 list_for_each_entry_safe(m, n, &rxd->secys, secys) {
3572                         macsec_common_dellink(m->secy.netdev, &head);
3573                 }
3574
3575                 netdev_rx_handler_unregister(real_dev);
3576                 kfree(rxd);
3577
3578                 unregister_netdevice_many(&head);
3579                 break;
3580         }
3581         case NETDEV_CHANGEMTU: {
3582                 struct macsec_dev *m;
3583                 struct macsec_rxh_data *rxd;
3584
3585                 rxd = macsec_data_rtnl(real_dev);
3586                 list_for_each_entry(m, &rxd->secys, secys) {
3587                         struct net_device *dev = m->secy.netdev;
3588                         unsigned int mtu = real_dev->mtu - (m->secy.icv_len +
3589                                                             macsec_extra_len(true));
3590
3591                         if (dev->mtu > mtu)
3592                                 dev_set_mtu(dev, mtu);
3593                 }
3594         }
3595         }
3596
3597         return NOTIFY_OK;
3598 }
3599
3600 static struct notifier_block macsec_notifier = {
3601         .notifier_call = macsec_notify,
3602 };
3603
3604 static int __init macsec_init(void)
3605 {
3606         int err;
3607
3608         pr_info("MACsec IEEE 802.1AE\n");
3609         err = register_netdevice_notifier(&macsec_notifier);
3610         if (err)
3611                 return err;
3612
3613         err = rtnl_link_register(&macsec_link_ops);
3614         if (err)
3615                 goto notifier;
3616
3617         err = genl_register_family(&macsec_fam);
3618         if (err)
3619                 goto rtnl;
3620
3621         return 0;
3622
3623 rtnl:
3624         rtnl_link_unregister(&macsec_link_ops);
3625 notifier:
3626         unregister_netdevice_notifier(&macsec_notifier);
3627         return err;
3628 }
3629
3630 static void __exit macsec_exit(void)
3631 {
3632         genl_unregister_family(&macsec_fam);
3633         rtnl_link_unregister(&macsec_link_ops);
3634         unregister_netdevice_notifier(&macsec_notifier);
3635         rcu_barrier();
3636 }
3637
3638 module_init(macsec_init);
3639 module_exit(macsec_exit);
3640
3641 MODULE_ALIAS_RTNL_LINK("macsec");
3642 MODULE_ALIAS_GENL_FAMILY("macsec");
3643
3644 MODULE_DESCRIPTION("MACsec IEEE 802.1AE");
3645 MODULE_LICENSE("GPL v2");