nft_byteorder.c 4.5 KB

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