release/src/router/iproute2/tc/em_u32.c

   1 /*
   2  * em_u32.c             U32 Ematch
   3  *
   4  *              This program is free software; you can distribute it and/or
   5  *              modify it under the terms of the GNU General Public License
   6  *              as published by the Free Software Foundation; either version
   7  *              2 of the License, or (at your option) any later version.
   8  *
   9  * Authors:     Thomas Graf <tgraf@suug.ch>
  10  */
  11
  12 #include <stdio.h>
  13 #include <stdlib.h>
  14 #include <unistd.h>
  15 #include <syslog.h>
  16 #include <fcntl.h>
  17 #include <sys/socket.h>
  18 #include <netinet/in.h>
  19 #include <arpa/inet.h>
  20 #include <string.h>
  21 #include <dlfcn.h>
  22 #include <errno.h>
  23
  24 #include "m_ematch.h"
  25
  26 extern struct ematch_util u32_ematch_util;
  27
  28 static void u32_print_usage(FILE *fd)
  29 {
  30         fprintf(fd,
  31             "Usage: u32(ALIGN VALUE MASK at [ nexthdr+ ] OFFSET)\n" \
  32             "where: ALIGN  := { u8 | u16 | u32 }\n" \
  33             "\n" \
  34             "Example: u32(u16 0x1122 0xffff at nexthdr+4)\n");
  35 }
  36
  37 static int u32_parse_eopt(struct nlmsghdr *n, struct tcf_ematch_hdr *hdr,
  38                           struct bstr *args)
  39 {
  40         struct bstr *a;
  41         int align, nh_len;
  42         unsigned long key, mask, offmask = 0, offset;
  43         struct tc_u32_key u_key;
  44
  45         memset(&u_key, 0, sizeof(u_key));
  46
  47 #define PARSE_ERR(CARG, FMT, ARGS...) \
  48         em_parse_error(EINVAL, args, CARG, &u32_ematch_util, FMT ,##ARGS)
  49
  50         if (args == NULL)
  51                 return PARSE_ERR(args, "u32: missing arguments");
  52
  53         if (!bstrcmp(args, "u8"))
  54                 align = 1;
  55         else if (!bstrcmp(args, "u16"))
  56                 align = 2;
  57         else if (!bstrcmp(args, "u32"))
  58                 align = 4;
  59         else
  60                 return PARSE_ERR(args, "u32: invalid alignment");
  61
  62         a = bstr_next(args);
  63         if (a == NULL)
  64                 return PARSE_ERR(a, "u32: missing key");
  65
  66         key = bstrtoul(a);
  67         if (key == ULONG_MAX)
  68                 return PARSE_ERR(a, "u32: invalid key, must be numeric");
  69
  70         a = bstr_next(a);
  71         if (a == NULL)
  72                 return PARSE_ERR(a, "u32: missing mask");
  73
  74         mask = bstrtoul(a);
  75         if (mask == ULONG_MAX)
  76                 return PARSE_ERR(a, "u32: invalid mask, must be numeric");
  77
  78         a = bstr_next(a);
  79         if (a == NULL || bstrcmp(a, "at") != 0)
  80                 return PARSE_ERR(a, "u32: missing \"at\"");
  81
  82         a = bstr_next(a);
  83         if (a == NULL)
  84                 return PARSE_ERR(a, "u32: missing offset");
  85
  86         nh_len = strlen("nexthdr+");
  87         if (a->len > nh_len && !memcmp(a->data, "nexthdr+", nh_len)) {
  88                 char buf[a->len - nh_len + 1];
  89                 offmask = -1;
  90                 memcpy(buf, a->data + nh_len, a->len - nh_len);
  91                 offset = strtoul(buf, NULL, 0);
  92         } else if (!bstrcmp(a, "nexthdr+")) {
  93                 a = bstr_next(a);
  94                 if (a == NULL)
  95                         return PARSE_ERR(a, "u32: missing offset");
  96                 offset = bstrtoul(a);
  97         } else
  98                 offset = bstrtoul(a);
  99
 100         if (offset == ULONG_MAX)
 101                 return PARSE_ERR(a, "u32: invalid offset");
 102
 103         if (a->next)
 104                 return PARSE_ERR(a->next, "u32: unexpected trailer");
 105
 106         switch (align) {
 107                 case 1:
 108                         if (key > 0xFF)
 109                                 return PARSE_ERR(a, "Illegal key (>0xFF)");
 110                         if (mask > 0xFF)
 111                                 return PARSE_ERR(a, "Illegal mask (>0xFF)");
 112
 113                         key <<= 24 - ((offset & 3) * 8);
 114                         mask <<= 24 - ((offset & 3) * 8);
 115                         offset &= ~3;
 116                         break;
 117
 118                 case 2:
 119                         if (key > 0xFFFF)
 120                                 return PARSE_ERR(a, "Illegal key (>0xFFFF)");
 121                         if (mask > 0xFFFF)
 122                                 return PARSE_ERR(a, "Illegal mask (>0xFFFF)");
 123
 124                         if ((offset & 3) == 0) {
 125                                 key <<= 16;
 126                                 mask <<= 16;
 127                         }
 128                         offset &= ~3;
 129                         break;
 130         }
 131
 132         key = htonl(key);
 133         mask = htonl(mask);
 134
 135         if (offset % 4)
 136                 return PARSE_ERR(a, "u32: invalid offset alignment, " \
 137                     "must be aligned to 4.");
 138
 139         key &= mask;
 140
 141         u_key.mask = mask;
 142         u_key.val = key;
 143         u_key.off = offset;
 144         u_key.offmask = offmask;
 145
 146         addraw_l(n, MAX_MSG, hdr, sizeof(*hdr));
 147         addraw_l(n, MAX_MSG, &u_key, sizeof(u_key));
 148
 149 #undef PARSE_ERR
 150         return 0;
 151 }
 152
 153 static int u32_print_eopt(FILE *fd, struct tcf_ematch_hdr *hdr, void *data,
 154                           int data_len)
 155 {
 156         struct tc_u32_key *u_key = data;
 157
 158         if (data_len < sizeof(*u_key)) {
 159                 fprintf(stderr, "U32 header size mismatch\n");
 160                 return -1;
 161         }
 162
 163         fprintf(fd, "%08x/%08x at %s%d",
 164             (unsigned int) ntohl(u_key->val),
 165             (unsigned int) ntohl(u_key->mask),
 166             u_key->offmask ? "nexthdr+" : "",
 167             u_key->off);
 168
 169         return 0;
 170 }
 171
 172 struct ematch_util u32_ematch_util = {
 173         .kind = "u32",
 174         .kind_num = TCF_EM_U32,
 175         .parse_eopt = u32_parse_eopt,
 176         .print_eopt = u32_print_eopt,
 177         .print_usage = u32_print_usage
 178 };