drivers/net/macsec.c

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