net/netfilter/nft_byteorder.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net>
   4  *
   5  * Development of this code funded by Astaro AG (http://www.astaro.com/)
   6  */
   7
   8 #include <asm/unaligned.h>
   9 #include <linux/kernel.h>
  10 #include <linux/init.h>
  11 #include <linux/module.h>
  12 #include <linux/netlink.h>
  13 #include <linux/netfilter.h>
  14 #include <linux/netfilter/nf_tables.h>
  15 #include <net/netfilter/nf_tables_core.h>
  16 #include <net/netfilter/nf_tables.h>
  17
  18 struct nft_byteorder {
  19         enum nft_registers      sreg:8;
  20         enum nft_registers      dreg:8;
  21         enum nft_byteorder_ops  op:8;
  22         u8                      len;
  23         u8                      size;
  24 };
  25
  26 void nft_byteorder_eval(const struct nft_expr *expr,
  27                         struct nft_regs *regs,
  28                         const struct nft_pktinfo *pkt)
  29 {
  30         const struct nft_byteorder *priv = nft_expr_priv(expr);
  31         u32 *src = &regs->data[priv->sreg];
  32         u32 *dst = &regs->data[priv->dreg];
  33         union { u32 u32; u16 u16; } *s, *d;
  34         unsigned int i;
  35
  36         s = (void *)src;
  37         d = (void *)dst;
  38
  39         switch (priv->size) {
  40         case 8: {
  41                 u64 src64;
  42
  43                 switch (priv->op) {
  44                 case NFT_BYTEORDER_NTOH:
  45                         for (i = 0; i < priv->len / 8; i++) {
  46                                 src64 = get_unaligned((u64 *)&src[i]);
  47                                 put_unaligned_be64(src64, &dst[i]);
  48                         }
  49                         break;
  50                 case NFT_BYTEORDER_HTON:
  51                         for (i = 0; i < priv->len / 8; i++) {
  52                                 src64 = get_unaligned_be64(&src[i]);
  53                                 put_unaligned(src64, (u64 *)&dst[i]);
  54                         }
  55                         break;
  56                 }
  57                 break;
  58         }
  59         case 4:
  60                 switch (priv->op) {
  61                 case NFT_BYTEORDER_NTOH:
  62                         for (i = 0; i < priv->len / 4; i++)
  63                                 d[i].u32 = ntohl((__force __be32)s[i].u32);
  64                         break;
  65                 case NFT_BYTEORDER_HTON:
  66                         for (i = 0; i < priv->len / 4; i++)
  67                                 d[i].u32 = (__force __u32)htonl(s[i].u32);
  68                         break;
  69                 }
  70                 break;
  71         case 2:
  72                 switch (priv->op) {
  73                 case NFT_BYTEORDER_NTOH:
  74                         for (i = 0; i < priv->len / 2; i++)
  75                                 d[i].u16 = ntohs((__force __be16)s[i].u16);
  76                         break;
  77                 case NFT_BYTEORDER_HTON:
  78                         for (i = 0; i < priv->len / 2; i++)
  79                                 d[i].u16 = (__force __u16)htons(s[i].u16);
  80                         break;
  81                 }
  82                 break;
  83         }
  84 }
  85
  86 static const struct nla_policy nft_byteorder_policy[NFTA_BYTEORDER_MAX + 1] = {
  87         [NFTA_BYTEORDER_SREG]   = { .type = NLA_U32 },
  88         [NFTA_BYTEORDER_DREG]   = { .type = NLA_U32 },
  89         [NFTA_BYTEORDER_OP]     = { .type = NLA_U32 },
  90         [NFTA_BYTEORDER_LEN]    = { .type = NLA_U32 },
  91         [NFTA_BYTEORDER_SIZE]   = { .type = NLA_U32 },
  92 };
  93
  94 static int nft_byteorder_init(const struct nft_ctx *ctx,
  95                               const struct nft_expr *expr,
  96                               const struct nlattr * const tb[])
  97 {
  98         struct nft_byteorder *priv = nft_expr_priv(expr);
  99         u32 size, len;
 100         int err;
 101
 102         if (tb[NFTA_BYTEORDER_SREG] == NULL ||
 103             tb[NFTA_BYTEORDER_DREG] == NULL ||
 104             tb[NFTA_BYTEORDER_LEN] == NULL ||
 105             tb[NFTA_BYTEORDER_SIZE] == NULL ||
 106             tb[NFTA_BYTEORDER_OP] == NULL)
 107                 return -EINVAL;
 108
 109         priv->op = ntohl(nla_get_be32(tb[NFTA_BYTEORDER_OP]));
 110         switch (priv->op) {
 111         case NFT_BYTEORDER_NTOH:
 112         case NFT_BYTEORDER_HTON:
 113                 break;
 114         default:
 115                 return -EINVAL;
 116         }
 117
 118         err = nft_parse_u32_check(tb[NFTA_BYTEORDER_SIZE], U8_MAX, &size);
 119         if (err < 0)
 120                 return err;
 121
 122         priv->size = size;
 123
 124         switch (priv->size) {
 125         case 2:
 126         case 4:
 127         case 8:
 128                 break;
 129         default:
 130                 return -EINVAL;
 131         }
 132
 133         priv->sreg = nft_parse_register(tb[NFTA_BYTEORDER_SREG]);
 134         err = nft_parse_u32_check(tb[NFTA_BYTEORDER_LEN], U8_MAX, &len);
 135         if (err < 0)
 136                 return err;
 137
 138         priv->len = len;
 139
 140         err = nft_validate_register_load(priv->sreg, priv->len);
 141         if (err < 0)
 142                 return err;
 143
 144         priv->dreg = nft_parse_register(tb[NFTA_BYTEORDER_DREG]);
 145         return nft_validate_register_store(ctx, priv->dreg, NULL,
 146                                            NFT_DATA_VALUE, priv->len);
 147 }
 148
 149 static int nft_byteorder_dump(struct sk_buff *skb, const struct nft_expr *expr)
 150 {
 151         const struct nft_byteorder *priv = nft_expr_priv(expr);
 152
 153         if (nft_dump_register(skb, NFTA_BYTEORDER_SREG, priv->sreg))
 154                 goto nla_put_failure;
 155         if (nft_dump_register(skb, NFTA_BYTEORDER_DREG, priv->dreg))
 156                 goto nla_put_failure;
 157         if (nla_put_be32(skb, NFTA_BYTEORDER_OP, htonl(priv->op)))
 158                 goto nla_put_failure;
 159         if (nla_put_be32(skb, NFTA_BYTEORDER_LEN, htonl(priv->len)))
 160                 goto nla_put_failure;
 161         if (nla_put_be32(skb, NFTA_BYTEORDER_SIZE, htonl(priv->size)))
 162                 goto nla_put_failure;
 163         return 0;
 164
 165 nla_put_failure:
 166         return -1;
 167 }
 168
 169 static const struct nft_expr_ops nft_byteorder_ops = {
 170         .type           = &nft_byteorder_type,
 171         .size           = NFT_EXPR_SIZE(sizeof(struct nft_byteorder)),
 172         .eval           = nft_byteorder_eval,
 173         .init           = nft_byteorder_init,
 174         .dump           = nft_byteorder_dump,
 175 };
 176
 177 struct nft_expr_type nft_byteorder_type __read_mostly = {
 178         .name           = "byteorder",
 179         .ops            = &nft_byteorder_ops,
 180         .policy         = nft_byteorder_policy,
 181         .maxattr        = NFTA_BYTEORDER_MAX,
 182         .owner          = THIS_MODULE,
 183 };