diff options
Diffstat (limited to 'net/core/macsec.c')
| -rw-r--r-- | net/core/macsec.c | 445 |
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"); |