aboutsummaryrefslogtreecommitdiff
path: root/net/core/macsec.c
diff options
context:
space:
mode:
Diffstat (limited to 'net/core/macsec.c')
-rw-r--r--net/core/macsec.c445
1 files changed, 445 insertions, 0 deletions
diff --git a/net/core/macsec.c b/net/core/macsec.c
new file mode 100644
index 00000000000..7c3984de39b
--- /dev/null
+++ b/net/core/macsec.c
@@ -0,0 +1,445 @@
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/scatterlist.h>
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/notifier.h>
+#include <linux/skbuff.h>
+#include <linux/proc_fs.h>
+#include <net/ip.h>
+#include <net/macsec.h>
+#include <crypto/aead.h>
+#include <crypto/authenc.h>
+
+#define MACSEC_SKB_CB(__skb) ((struct macsec_skb_cb *)&((__skb)->cb[0]))
+
+int create_cnt = 0;
+u32 delete_cnt = 0;
+u32 create_opt_cnt = 0;
+int delete_opt_cnt = 0;
+int create_force_cnt = 0;
+int macsec_ouput = 0;
+int macsec_input = 0;
+
+static int macsec_req_ctx_size(struct crypto_aead *aead, int sg_size)
+{
+ unsigned int len = 0;
+ len += sizeof(struct aead_request) + crypto_aead_reqsize(aead);
+
+ len = ALIGN(len, __alignof__(struct scatterlist));
+ len += sizeof(struct scatterlist) * (sg_size);
+
+ return len;
+}
+
+static void *macsec_alloc_req_ctx( struct macsec_skb_cb *macsec_skb,
+ struct crypto_aead *aead,
+ int nfrags)
+{
+ void *ctx_data;
+ unsigned int len;
+ struct macsec_dev_ctx *ctx = macsec_skb->ctx;
+
+#if CONFIG_INET_MACSEC_NR_REQ_CACHE > 0
+ if (nfrags <= MACSEC_NFRAGS_CACHE) {
+ macsec_skb->flags |= 0x01;
+ if (atomic_read(&ctx->req_cache_cnt)) {
+ ctx_data = ctx->req_cache[ctx->req_cache_head];
+ ctx->req_cache_head = (ctx->req_cache_head + 1) %
+ MACSEC_REQ_CACHE_MAX;
+ atomic_dec(&ctx->req_cache_cnt);
+ create_opt_cnt++;
+ return ctx_data;
+ }
+ create_force_cnt++;
+ len = ctx->req_cache_size +
+ sizeof(struct scatterlist) * MACSEC_NFRAGS_CACHE;
+ ctx_data = kmalloc(len, GFP_ATOMIC);
+ } else {
+ create_cnt++;
+ macsec_skb->flags &= ~0x01;
+ len = ctx->req_cache_size +
+ sizeof(struct scatterlist) * nfrags;
+ ctx_data = kmalloc(len, GFP_ATOMIC);
+ }
+#else
+ len = ctx->req_cache_size +
+ sizeof(struct scatterlist) * nfrags;
+ ctx_data = kmalloc(len, GFP_ATOMIC);
+#endif
+ return ctx_data;
+}
+u32 glb_free_req_ctx_out = 0;
+u32 glb_free_req_ctx_in = 0;
+static void macsec_free_req_ctx(struct macsec_skb_cb *macsec_skb)
+{
+#if CONFIG_INET_MACSEC_NR_REQ_CACHE > 0
+ struct macsec_dev_ctx *ctx = macsec_skb->ctx;
+
+ if (macsec_skb->flags & 0x01) {
+ if (atomic_read(&ctx->req_cache_cnt) < MACSEC_REQ_CACHE_MAX) {
+ ctx->req_cache[ctx->req_cache_tail] = macsec_skb->req_ctx;
+ ctx->req_cache_tail = (ctx->req_cache_tail + 1) %
+ MACSEC_REQ_CACHE_MAX;
+ atomic_inc(&ctx->req_cache_cnt);
+ delete_opt_cnt++;
+ return;
+ }
+ }
+#endif
+ delete_cnt++;
+ kfree(macsec_skb->req_ctx);
+}
+
+static inline struct scatterlist *macsec_req_sg(struct crypto_aead *aead,
+ struct aead_request *req)
+{
+ return (struct scatterlist *) ALIGN((unsigned long) (req + 1) +
+ crypto_aead_reqsize(aead), __alignof__(struct scatterlist));
+}
+
+__be16 macsec_type_trans(struct sk_buff *skb)
+{
+ struct macsec_ethhdr *eth;
+ eth = (struct macsec_ethhdr *)(skb->data - ETH_HLEN);
+ return eth->hdr.macsec_type;
+}
+
+int macsec_init_aead(struct macsec_dev_ctx *mdata)
+{
+ struct crypto_aead *aead;
+ int err;
+ char *alg_name = "macsec(gcm)";
+ char key[32] = { 0x88, 0xc5, 0x12, 0x00,
+ 0x00, 0x00, 0x00, 0x00,
+ 0x07, 0x52, 0x05, 0x20, 0x9f, 0xe8, 0x6b, 0xf8,
+ 0x8e, 0x7f, 0xa3, 0xaa, 0x77, 0x89, 0x58, 0xd2,
+ 0x50, 0x61, 0x75, 0x72, 0x81, 0x39, 0x7f, 0xcc};
+ int key_len = 32;
+
+ aead = crypto_alloc_aead(alg_name, 0, 0);
+ if (IS_ERR(aead)) {
+ printk("Failed to create aead transform for macsec(gcm)\n");
+ err = PTR_ERR(aead);
+ goto error;
+ }
+
+ mdata->aead = aead;
+
+ err = crypto_aead_setkey(aead, key, key_len);
+ if (err) {
+ printk("Failed to set key for macsec(gcm)\n");
+ goto error;
+ }
+
+ err = crypto_aead_setauthsize(aead, 24);
+ if (err) {
+ printk("Failed to set authsize for macsec(gcm)\n");
+ err = 0;
+ }
+error:
+ return err;
+}
+
+static void macsec_output_done_hw(struct crypto_async_request *base, int err)
+{
+ int ret;
+ struct sk_buff *skb = base->data;
+ struct net_device *dev = skb->dev;
+ const struct net_device_ops *ops = dev->netdev_ops;
+
+ if (err < 0) {
+ macsec_free_req_ctx(MACSEC_SKB_CB(skb));
+ return;
+ }
+ glb_free_req_ctx_out++;
+ macsec_free_req_ctx(MACSEC_SKB_CB(skb));
+ ret = ops->ndo_start_xmit(skb, dev);
+}
+
+int macsec_ouput_hw(struct sk_buff *skb, struct net_device *dev)
+{
+ int err;
+ struct macsec_dev_ctx *data;
+ struct crypto_aead *aead;
+ struct aead_request *req;
+ struct scatterlist *sg;
+ struct scatterlist *dsg;
+ struct sk_buff *trailer;
+ void *macsec_req;
+ int clen;
+ int alen;
+ int nfrags;
+
+ err = -ENOMEM;
+
+ data = netdev_macsec_priv(dev);
+ aead = data->aead;
+ alen = crypto_aead_authsize(aead);
+
+ alen = 16;
+
+ if ((err = skb_cow_data(skb, alen /* + 8 */, &trailer)) < 0)
+ goto error;
+ nfrags = err;
+
+ MACSEC_SKB_CB(skb)->ctx = data;
+ macsec_req = macsec_alloc_req_ctx(MACSEC_SKB_CB(skb), aead, nfrags * 2);
+ if (!macsec_req)
+ goto error;
+ req = (struct aead_request*) macsec_req;
+
+ aead_request_set_tfm(req, aead);
+ sg = macsec_req_sg(aead, req);
+ dsg = sg + nfrags;
+
+ /* Setup SG */
+ skb_to_sgvec(skb, sg, 0, skb->len);
+
+ clen = skb->len;
+ pskb_put(skb, trailer, alen);
+ skb_push(skb, 8);
+ skb_to_sgvec(skb, dsg, 0, skb->len);
+
+ MACSEC_SKB_CB(skb)->req_ctx = macsec_req;
+
+ aead_request_set_callback(req, 0, macsec_output_done_hw, skb);
+ aead_request_set_crypt(req, sg, dsg, clen, NULL);
+ macsec_ouput++;
+ err = crypto_aead_encrypt(req);
+
+ if (err == -EINPROGRESS)
+ goto error;
+
+ if (err == -EAGAIN || err == -EBUSY) {
+ macsec_free_req_ctx(MACSEC_SKB_CB(skb));
+ err = NET_XMIT_DROP;
+ }
+
+error:
+ return err;
+
+}
+
+void macsec_done_input_hw(struct crypto_async_request *base, int err)
+{
+ struct sk_buff *skb = base->data;
+ int hlen = 22; /* ETH Header len + Macsec Secutity Tag(TCI + PN) */
+ int ret;
+ struct macsec_ethhdr *eth;
+
+ skb_reset_mac_header(skb);
+ eth = (struct macsec_ethhdr *)skb_mac_header(skb);
+ skb->protocol = eth->h_proto;
+
+ pskb_trim(skb, skb->len - 16 /* icv */);
+ __skb_pull(skb, hlen);
+ skb_reset_network_header(skb);
+ skb->transport_header = skb->network_header;
+
+ glb_free_req_ctx_in++;
+ macsec_free_req_ctx(MACSEC_SKB_CB(skb));
+ ret = macsec_netif_receive_skb(skb, skb->protocol);
+
+}
+
+int macsec_input_hw(struct sk_buff *skb)
+{
+ struct macsec_dev_ctx *data;
+ struct crypto_aead *aead;
+ struct aead_request *req;
+ struct scatterlist *sg;
+ struct scatterlist *dsg;
+ struct sk_buff *trailer;
+ void *macsec_req;
+ int clen;
+ int nfrags;
+ int eth_len = ETH_HLEN;
+ int err = -EINVAL;
+ int src_len;
+
+ data = netdev_macsec_priv(skb->dev);
+ aead = data->aead;
+
+ if(!aead)
+ goto error;
+
+ if (!pskb_may_pull(skb, eth_len))
+ goto error;
+
+ if ((err = skb_cow_data(skb, 0, &trailer)) < 0)
+ goto error;
+ nfrags = err;
+
+ err = -ENOMEM;
+
+ MACSEC_SKB_CB(skb)->ctx = data;
+ macsec_req = macsec_alloc_req_ctx(MACSEC_SKB_CB(skb), aead, nfrags * 2);
+ if (!macsec_req)
+ goto error;
+ req = (struct aead_request*) macsec_req;
+ aead_request_set_tfm(req, aead);
+ sg = macsec_req_sg(aead, req);
+ dsg = sg + nfrags;
+
+ /* Setup SG */
+ sg_init_table(sg, nfrags);
+ skb_push(skb, eth_len);
+ clen = skb->len;
+ skb_to_sgvec(skb, sg, 0, clen);
+ src_len = clen;
+
+ sg_init_table(dsg, nfrags);
+ clen -= 16;
+ clen -= 8;
+ skb_to_sgvec(skb, dsg, 0, clen);
+ MACSEC_SKB_CB(skb)->req_ctx = macsec_req;
+
+ aead_request_set_callback(req, 0, macsec_done_input_hw, skb);
+ aead_request_set_crypt(req, sg, dsg, src_len, NULL);
+
+ //macsec_input++;
+ err = crypto_aead_decrypt(req);
+
+ if (err == -EINPROGRESS) {
+ macsec_input++;
+ goto error;
+ }
+ if (err == -EBUSY || err == -EAGAIN) {
+ macsec_free_req_ctx(MACSEC_SKB_CB(skb));
+ err = NET_XMIT_DROP;
+ }
+
+error:
+ return err;
+
+}
+
+static ssize_t macsec_console_driver_write(struct file *file, const char __user *buf,
+ size_t count, loff_t * ppos)
+{
+ if (*buf == '1') {
+ printk("Printing the delete and create stats ="
+ "create_cnt = %d, delete_cnt = %d, "
+ "create_opt_cnt = %d, delete_opt_cnt = %d,create_force_cnt = %d\n,"
+ "glb_free_req_ctx_out = %d, glb_free_req_ctx_in = %d\n,"
+ "macsec_input = %d, macsec_ouput = %d\n",
+ create_cnt, delete_cnt, create_opt_cnt,
+ delete_opt_cnt, create_force_cnt, glb_free_req_ctx_out, glb_free_req_ctx_in,
+ macsec_input, macsec_ouput);
+ }
+ return 1;
+}
+
+static int macsec_console_driver_open(struct inode *inode, struct file *file)
+{
+ return 0;
+}
+
+static int macsec_console_driver_release(struct inode *inode, struct file *file)
+{
+ return 0;
+}
+
+struct file_operations macsec_console_driver_fops = {
+ .owner = THIS_MODULE,
+ .open = macsec_console_driver_open,
+ .release = macsec_console_driver_release,
+ .write = macsec_console_driver_write,
+};
+
+#define MACSEC_CONSOLE_DRIVER_NAME "macsec"
+int macsec_console_module_init(void)
+{
+ struct proc_dir_entry *entry;
+
+ entry = create_proc_entry(MACSEC_CONSOLE_DRIVER_NAME, 0, NULL);
+ if (entry == NULL) {
+ printk(KERN_ERR "Macsec Proc entry failed !!\n");
+ return -1;
+ }
+
+ entry->proc_fops = &macsec_console_driver_fops;
+ printk("Macsec proc interface Initiliazed\n");
+ return 0;
+}
+
+void macsec_console_module_exit(void)
+{
+ remove_proc_entry(MACSEC_CONSOLE_DRIVER_NAME, NULL);
+}
+
+void macsec_destroy(struct net_device *dev)
+{
+ struct macsec_dev_ctx *macdata = dev->macsec_priv;
+
+ if (!macdata)
+ return;
+
+ crypto_free_aead(macdata->aead);
+ /* Delete request cache */
+ while ((atomic_dec_return(&macdata->req_cache_cnt)) > 0) {
+ kfree(macdata->req_cache[macdata->req_cache_head]);
+ macdata->req_cache_head = (macdata->req_cache_head + 1) %
+ MACSEC_REQ_CACHE_MAX;
+ }
+ dev->macsec_priv = NULL;
+ dev->macsec_output_hw = NULL;
+ dev->macsec_input_hw = NULL;
+
+ kfree(macdata);
+ printk("Macsec Session Destroyed\n");
+}
+
+int macsec_init_state(struct net_device *dev)
+{
+ struct macsec_dev_ctx *macdata;
+ int err;
+
+ macdata = kzalloc(sizeof(*macdata), GFP_KERNEL);
+ if (macdata == NULL)
+ return -ENOMEM;
+
+ dev->macsec_priv = macdata;
+ dev->macsec_output_hw = macsec_ouput_hw;
+ dev->macsec_input_hw = macsec_input_hw;
+
+ err = macsec_init_aead(macdata);
+ if (err)
+ goto out;
+
+#if CONFIG_INET_MACSEC_NR_REQ_CACHE > 0
+ atomic_set(&macdata->req_cache_cnt, 0);
+ macdata->req_cache_head = 0;
+ macdata->req_cache_tail = 0;
+#endif
+ macdata->req_cache_size = macsec_req_ctx_size(macdata->aead, 0);
+ printk("Macsec Session Established\n");
+out:
+ return err;
+
+}
+
+static int __init macsec_init(void)
+{
+ int ret;
+ ret = macsec_console_module_init();
+ if (ret) {
+ printk("Macsec proc driver could not initiliaze\n");
+ return ret;
+ }
+ printk("Registered Macsec Interface\n");
+ return 0;
+}
+
+static void __exit macsec_fini(void)
+{
+ macsec_console_module_exit();
+ printk("Unregistered Macsec Interface\n");
+}
+
+module_init(macsec_init);
+module_exit(macsec_fini);
+MODULE_LICENSE("GPL");
generated by cgit v1.2.3-18-g5258 (git 2.32.0) at 2024-11-21 14:27:33 +0000