1311b41454dc445639924c691a949bd15fbfab0cbshemminger/* 2311b41454dc445639924c691a949bd15fbfab0cbshemminger * em_u32.c U32 Ematch 3311b41454dc445639924c691a949bd15fbfab0cbshemminger * 4311b41454dc445639924c691a949bd15fbfab0cbshemminger * This program is free software; you can distribute it and/or 5311b41454dc445639924c691a949bd15fbfab0cbshemminger * modify it under the terms of the GNU General Public License 6311b41454dc445639924c691a949bd15fbfab0cbshemminger * as published by the Free Software Foundation; either version 7311b41454dc445639924c691a949bd15fbfab0cbshemminger * 2 of the License, or (at your option) any later version. 8311b41454dc445639924c691a949bd15fbfab0cbshemminger * 9311b41454dc445639924c691a949bd15fbfab0cbshemminger * Authors: Thomas Graf <tgraf@suug.ch> 10311b41454dc445639924c691a949bd15fbfab0cbshemminger */ 11311b41454dc445639924c691a949bd15fbfab0cbshemminger 12311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <stdio.h> 13311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <stdlib.h> 14311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <unistd.h> 15311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <syslog.h> 16311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <fcntl.h> 17311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <sys/socket.h> 18311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <netinet/in.h> 19311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <arpa/inet.h> 20311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <string.h> 21311b41454dc445639924c691a949bd15fbfab0cbshemminger#include <errno.h> 22311b41454dc445639924c691a949bd15fbfab0cbshemminger 23311b41454dc445639924c691a949bd15fbfab0cbshemminger#include "m_ematch.h" 24311b41454dc445639924c691a949bd15fbfab0cbshemminger 25311b41454dc445639924c691a949bd15fbfab0cbshemmingerextern struct ematch_util u32_ematch_util; 26311b41454dc445639924c691a949bd15fbfab0cbshemminger 27311b41454dc445639924c691a949bd15fbfab0cbshemmingerstatic void u32_print_usage(FILE *fd) 28311b41454dc445639924c691a949bd15fbfab0cbshemminger{ 29311b41454dc445639924c691a949bd15fbfab0cbshemminger fprintf(fd, 30311b41454dc445639924c691a949bd15fbfab0cbshemminger "Usage: u32(ALIGN VALUE MASK at [ nexthdr+ ] OFFSET)\n" \ 31311b41454dc445639924c691a949bd15fbfab0cbshemminger "where: ALIGN := { u8 | u16 | u32 }\n" \ 32311b41454dc445639924c691a949bd15fbfab0cbshemminger "\n" \ 33311b41454dc445639924c691a949bd15fbfab0cbshemminger "Example: u32(u16 0x1122 0xffff at nexthdr+4)\n"); 34311b41454dc445639924c691a949bd15fbfab0cbshemminger} 35311b41454dc445639924c691a949bd15fbfab0cbshemminger 36311b41454dc445639924c691a949bd15fbfab0cbshemmingerstatic int u32_parse_eopt(struct nlmsghdr *n, struct tcf_ematch_hdr *hdr, 37311b41454dc445639924c691a949bd15fbfab0cbshemminger struct bstr *args) 38311b41454dc445639924c691a949bd15fbfab0cbshemminger{ 39311b41454dc445639924c691a949bd15fbfab0cbshemminger struct bstr *a; 40311b41454dc445639924c691a949bd15fbfab0cbshemminger int align, nh_len; 41311b41454dc445639924c691a949bd15fbfab0cbshemminger unsigned long key, mask, offmask = 0, offset; 42311b41454dc445639924c691a949bd15fbfab0cbshemminger struct tc_u32_key u_key; 43311b41454dc445639924c691a949bd15fbfab0cbshemminger 44311b41454dc445639924c691a949bd15fbfab0cbshemminger memset(&u_key, 0, sizeof(u_key)); 45311b41454dc445639924c691a949bd15fbfab0cbshemminger 46311b41454dc445639924c691a949bd15fbfab0cbshemminger#define PARSE_ERR(CARG, FMT, ARGS...) \ 47311b41454dc445639924c691a949bd15fbfab0cbshemminger em_parse_error(EINVAL, args, CARG, &u32_ematch_util, FMT ,##ARGS) 48311b41454dc445639924c691a949bd15fbfab0cbshemminger 49311b41454dc445639924c691a949bd15fbfab0cbshemminger if (args == NULL) 50311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(args, "u32: missing arguments"); 51311b41454dc445639924c691a949bd15fbfab0cbshemminger 52311b41454dc445639924c691a949bd15fbfab0cbshemminger if (!bstrcmp(args, "u8")) 53311b41454dc445639924c691a949bd15fbfab0cbshemminger align = 1; 54311b41454dc445639924c691a949bd15fbfab0cbshemminger else if (!bstrcmp(args, "u16")) 55311b41454dc445639924c691a949bd15fbfab0cbshemminger align = 2; 56311b41454dc445639924c691a949bd15fbfab0cbshemminger else if (!bstrcmp(args, "u32")) 57311b41454dc445639924c691a949bd15fbfab0cbshemminger align = 4; 58311b41454dc445639924c691a949bd15fbfab0cbshemminger else 59311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(args, "u32: invalid alignment"); 60311b41454dc445639924c691a949bd15fbfab0cbshemminger 61311b41454dc445639924c691a949bd15fbfab0cbshemminger a = bstr_next(args); 62311b41454dc445639924c691a949bd15fbfab0cbshemminger if (a == NULL) 63311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: missing key"); 64311b41454dc445639924c691a949bd15fbfab0cbshemminger 65311b41454dc445639924c691a949bd15fbfab0cbshemminger key = bstrtoul(a); 66311b41454dc445639924c691a949bd15fbfab0cbshemminger if (key == ULONG_MAX) 67311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: invalid key, must be numeric"); 68311b41454dc445639924c691a949bd15fbfab0cbshemminger 69311b41454dc445639924c691a949bd15fbfab0cbshemminger a = bstr_next(a); 70311b41454dc445639924c691a949bd15fbfab0cbshemminger if (a == NULL) 71311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: missing mask"); 72311b41454dc445639924c691a949bd15fbfab0cbshemminger 73311b41454dc445639924c691a949bd15fbfab0cbshemminger mask = bstrtoul(a); 74311b41454dc445639924c691a949bd15fbfab0cbshemminger if (mask == ULONG_MAX) 75311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: invalid mask, must be numeric"); 76311b41454dc445639924c691a949bd15fbfab0cbshemminger 77311b41454dc445639924c691a949bd15fbfab0cbshemminger a = bstr_next(a); 78311b41454dc445639924c691a949bd15fbfab0cbshemminger if (a == NULL || bstrcmp(a, "at") != 0) 79311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: missing \"at\""); 80311b41454dc445639924c691a949bd15fbfab0cbshemminger 81311b41454dc445639924c691a949bd15fbfab0cbshemminger a = bstr_next(a); 82311b41454dc445639924c691a949bd15fbfab0cbshemminger if (a == NULL) 83311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: missing offset"); 84311b41454dc445639924c691a949bd15fbfab0cbshemminger 85311b41454dc445639924c691a949bd15fbfab0cbshemminger nh_len = strlen("nexthdr+"); 86311b41454dc445639924c691a949bd15fbfab0cbshemminger if (a->len > nh_len && !memcmp(a->data, "nexthdr+", nh_len)) { 87311b41454dc445639924c691a949bd15fbfab0cbshemminger char buf[a->len - nh_len + 1]; 88311b41454dc445639924c691a949bd15fbfab0cbshemminger offmask = -1; 89311b41454dc445639924c691a949bd15fbfab0cbshemminger memcpy(buf, a->data + nh_len, a->len - nh_len); 90311b41454dc445639924c691a949bd15fbfab0cbshemminger offset = strtoul(buf, NULL, 0); 91311b41454dc445639924c691a949bd15fbfab0cbshemminger } else if (!bstrcmp(a, "nexthdr+")) { 92311b41454dc445639924c691a949bd15fbfab0cbshemminger a = bstr_next(a); 93311b41454dc445639924c691a949bd15fbfab0cbshemminger if (a == NULL) 94311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: missing offset"); 95311b41454dc445639924c691a949bd15fbfab0cbshemminger offset = bstrtoul(a); 96311b41454dc445639924c691a949bd15fbfab0cbshemminger } else 97311b41454dc445639924c691a949bd15fbfab0cbshemminger offset = bstrtoul(a); 98ae665a522bd46bea44c5ea84c89c8b1731954170Stephen Hemminger 99311b41454dc445639924c691a949bd15fbfab0cbshemminger if (offset == ULONG_MAX) 100311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: invalid offset"); 101311b41454dc445639924c691a949bd15fbfab0cbshemminger 102311b41454dc445639924c691a949bd15fbfab0cbshemminger if (a->next) 103311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a->next, "u32: unexpected trailer"); 104311b41454dc445639924c691a949bd15fbfab0cbshemminger 105311b41454dc445639924c691a949bd15fbfab0cbshemminger switch (align) { 106311b41454dc445639924c691a949bd15fbfab0cbshemminger case 1: 107311b41454dc445639924c691a949bd15fbfab0cbshemminger if (key > 0xFF) 108311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "Illegal key (>0xFF)"); 109311b41454dc445639924c691a949bd15fbfab0cbshemminger if (mask > 0xFF) 110311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "Illegal mask (>0xFF)"); 111311b41454dc445639924c691a949bd15fbfab0cbshemminger 112311b41454dc445639924c691a949bd15fbfab0cbshemminger key <<= 24 - ((offset & 3) * 8); 113311b41454dc445639924c691a949bd15fbfab0cbshemminger mask <<= 24 - ((offset & 3) * 8); 114311b41454dc445639924c691a949bd15fbfab0cbshemminger offset &= ~3; 115311b41454dc445639924c691a949bd15fbfab0cbshemminger break; 116311b41454dc445639924c691a949bd15fbfab0cbshemminger 117311b41454dc445639924c691a949bd15fbfab0cbshemminger case 2: 118311b41454dc445639924c691a949bd15fbfab0cbshemminger if (key > 0xFFFF) 119311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "Illegal key (>0xFFFF)"); 120311b41454dc445639924c691a949bd15fbfab0cbshemminger if (mask > 0xFFFF) 121311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "Illegal mask (>0xFFFF)"); 122311b41454dc445639924c691a949bd15fbfab0cbshemminger 123311b41454dc445639924c691a949bd15fbfab0cbshemminger if ((offset & 3) == 0) { 124311b41454dc445639924c691a949bd15fbfab0cbshemminger key <<= 16; 125311b41454dc445639924c691a949bd15fbfab0cbshemminger mask <<= 16; 126311b41454dc445639924c691a949bd15fbfab0cbshemminger } 127311b41454dc445639924c691a949bd15fbfab0cbshemminger offset &= ~3; 128311b41454dc445639924c691a949bd15fbfab0cbshemminger break; 129311b41454dc445639924c691a949bd15fbfab0cbshemminger } 130311b41454dc445639924c691a949bd15fbfab0cbshemminger 131311b41454dc445639924c691a949bd15fbfab0cbshemminger key = htonl(key); 132311b41454dc445639924c691a949bd15fbfab0cbshemminger mask = htonl(mask); 133311b41454dc445639924c691a949bd15fbfab0cbshemminger 134311b41454dc445639924c691a949bd15fbfab0cbshemminger if (offset % 4) 135311b41454dc445639924c691a949bd15fbfab0cbshemminger return PARSE_ERR(a, "u32: invalid offset alignment, " \ 136311b41454dc445639924c691a949bd15fbfab0cbshemminger "must be aligned to 4."); 137311b41454dc445639924c691a949bd15fbfab0cbshemminger 138311b41454dc445639924c691a949bd15fbfab0cbshemminger key &= mask; 139311b41454dc445639924c691a949bd15fbfab0cbshemminger 140311b41454dc445639924c691a949bd15fbfab0cbshemminger u_key.mask = mask; 141311b41454dc445639924c691a949bd15fbfab0cbshemminger u_key.val = key; 142311b41454dc445639924c691a949bd15fbfab0cbshemminger u_key.off = offset; 143311b41454dc445639924c691a949bd15fbfab0cbshemminger u_key.offmask = offmask; 144311b41454dc445639924c691a949bd15fbfab0cbshemminger 145311b41454dc445639924c691a949bd15fbfab0cbshemminger addraw_l(n, MAX_MSG, hdr, sizeof(*hdr)); 146311b41454dc445639924c691a949bd15fbfab0cbshemminger addraw_l(n, MAX_MSG, &u_key, sizeof(u_key)); 147311b41454dc445639924c691a949bd15fbfab0cbshemminger 148311b41454dc445639924c691a949bd15fbfab0cbshemminger#undef PARSE_ERR 149311b41454dc445639924c691a949bd15fbfab0cbshemminger return 0; 150311b41454dc445639924c691a949bd15fbfab0cbshemminger} 151311b41454dc445639924c691a949bd15fbfab0cbshemminger 152311b41454dc445639924c691a949bd15fbfab0cbshemmingerstatic int u32_print_eopt(FILE *fd, struct tcf_ematch_hdr *hdr, void *data, 153311b41454dc445639924c691a949bd15fbfab0cbshemminger int data_len) 154311b41454dc445639924c691a949bd15fbfab0cbshemminger{ 155311b41454dc445639924c691a949bd15fbfab0cbshemminger struct tc_u32_key *u_key = data; 156311b41454dc445639924c691a949bd15fbfab0cbshemminger 157311b41454dc445639924c691a949bd15fbfab0cbshemminger if (data_len < sizeof(*u_key)) { 158311b41454dc445639924c691a949bd15fbfab0cbshemminger fprintf(stderr, "U32 header size mismatch\n"); 159311b41454dc445639924c691a949bd15fbfab0cbshemminger return -1; 160311b41454dc445639924c691a949bd15fbfab0cbshemminger } 161311b41454dc445639924c691a949bd15fbfab0cbshemminger 162311b41454dc445639924c691a949bd15fbfab0cbshemminger fprintf(fd, "%08x/%08x at %s%d", 163311b41454dc445639924c691a949bd15fbfab0cbshemminger (unsigned int) ntohl(u_key->val), 164311b41454dc445639924c691a949bd15fbfab0cbshemminger (unsigned int) ntohl(u_key->mask), 165311b41454dc445639924c691a949bd15fbfab0cbshemminger u_key->offmask ? "nexthdr+" : "", 166311b41454dc445639924c691a949bd15fbfab0cbshemminger u_key->off); 167311b41454dc445639924c691a949bd15fbfab0cbshemminger 168311b41454dc445639924c691a949bd15fbfab0cbshemminger return 0; 169311b41454dc445639924c691a949bd15fbfab0cbshemminger} 170311b41454dc445639924c691a949bd15fbfab0cbshemminger 171311b41454dc445639924c691a949bd15fbfab0cbshemmingerstruct ematch_util u32_ematch_util = { 172311b41454dc445639924c691a949bd15fbfab0cbshemminger .kind = "u32", 173311b41454dc445639924c691a949bd15fbfab0cbshemminger .kind_num = TCF_EM_U32, 174311b41454dc445639924c691a949bd15fbfab0cbshemminger .parse_eopt = u32_parse_eopt, 175311b41454dc445639924c691a949bd15fbfab0cbshemminger .print_eopt = u32_print_eopt, 176311b41454dc445639924c691a949bd15fbfab0cbshemminger .print_usage = u32_print_usage 177311b41454dc445639924c691a949bd15fbfab0cbshemminger}; 178