LXR iproute2/tc/em

   1/*
   2 * em_canid.c  Ematch rule to match CAN frames according to their CAN identifiers
   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 * Idea:       Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
  10 * Copyright:  (c) 2011 Czech Technical University in Prague
  11 *             (c) 2011 Volkswagen Group Research
  12 * Authors:    Michal Sojka <sojkam1@fel.cvut.cz>
  13 *             Pavel Pisa <pisa@cmp.felk.cvut.cz>
  14 *             Rostislav Lisovy <lisovy@gmail.cz>
  15 * Funded by:  Volkswagen Group Research
  16 *
  17 * Documentation: http://rtime.felk.cvut.cz/can/socketcan-qdisc-final.pdf
  18 */
  19
  20#include <stdio.h>
  21#include <stdlib.h>
  22#include <unistd.h>
  23#include <fcntl.h>
  24#include <sys/socket.h>
  25#include <netinet/in.h>
  26#include <arpa/inet.h>
  27#include <string.h>
  28#include <errno.h>
  29#include <linux/can.h>
  30#include <inttypes.h>
  31#include "m_ematch.h"
  32
  33#define EM_CANID_RULES_MAX 400 /* Main reason for this number is Nelink
  34        message size limit equal to Single memory page size. When dump()
  35        is invoked, there are even some ematch related headers sent from
  36        kernel to userspace together with em_canid configuration --
  37        400*sizeof(struct can_filter) should fit without any problems */
  38
  39extern struct ematch_util canid_ematch_util;
  40struct rules {
  41        struct can_filter *rules_raw;
  42        int rules_capacity;     /* Size of array allocated for rules_raw */
  43        int rules_cnt;          /* Actual number of rules stored in rules_raw */
  44};
  45
  46static void canid_print_usage(FILE *fd)
  47{
  48        fprintf(fd,
  49                "Usage: canid(IDLIST)\n" \
  50                "where: IDLIST := IDSPEC [ IDLIST ]\n" \
  51                "       IDSPEC := { ’sff’ CANID | ’eff’ CANID }\n" \
  52                "       CANID := ID[:MASK]\n" \
  53                "       ID, MASK := hexadecimal number (i.e. 0x123)\n" \
  54                "Example: canid(sff 0x123 sff 0x124 sff 0x125:0xf)\n");
  55}
  56
  57static int canid_parse_rule(struct rules *rules, struct bstr *a, int iseff)
  58{
  59        unsigned int can_id = 0;
  60        unsigned int can_mask = 0;
  61
  62        if (sscanf(a->data, "%"SCNx32 ":" "%"SCNx32, &can_id, &can_mask) != 2) {
  63                if (sscanf(a->data, "%"SCNx32, &can_id) != 1) {
  64                        return -1;
  65                } else {
  66                        can_mask = (iseff) ? CAN_EFF_MASK : CAN_SFF_MASK;
  67                }
  68        }
  69
  70        /* Stretch rules array up to EM_CANID_RULES_MAX if necessary */
  71        if (rules->rules_cnt == rules->rules_capacity) {
  72                if (rules->rules_capacity <= EM_CANID_RULES_MAX/2) {
  73                        rules->rules_capacity *= 2;
  74                        rules->rules_raw = realloc(rules->rules_raw,
  75                                sizeof(struct can_filter) * rules->rules_capacity);
  76                } else {
  77                        return -2;
  78                }
  79        }
  80
  81        rules->rules_raw[rules->rules_cnt].can_id =
  82                can_id | ((iseff) ? CAN_EFF_FLAG : 0);
  83        rules->rules_raw[rules->rules_cnt].can_mask =
  84                can_mask | CAN_EFF_FLAG;
  85
  86        rules->rules_cnt++;
  87
  88        return 0;
  89}
  90
  91static int canid_parse_eopt(struct nlmsghdr *n, struct tcf_ematch_hdr *hdr,
  92                          struct bstr *args)
  93{
  94        int iseff = 0;
  95        int ret = 0;
  96        struct rules rules = {
  97                .rules_capacity = 25, /* Denominator of EM_CANID_RULES_MAX
  98                        Will be multiplied by 2 to calculate the size for realloc() */
  99                .rules_cnt = 0
 100        };
 101
 102#define PARSE_ERR(CARG, FMT, ARGS...) \
 103        em_parse_error(EINVAL, args, CARG, &canid_ematch_util, FMT, ##ARGS)
 104
 105        if (args == NULL)
 106                return PARSE_ERR(args, "canid: missing arguments");
 107
 108        rules.rules_raw = calloc(rules.rules_capacity,
 109                                 sizeof(struct can_filter));
 110
 111        do {
 112                if (!bstrcmp(args, "sff")) {
 113                        iseff = 0;
 114                } else if (!bstrcmp(args, "eff")) {
 115                        iseff = 1;
 116                } else {
 117                        ret = PARSE_ERR(args, "canid: invalid key");
 118                        goto exit;
 119                }
 120
 121                args = bstr_next(args);
 122                if (args == NULL) {
 123                        ret = PARSE_ERR(args, "canid: missing argument");
 124                        goto exit;
 125                }
 126
 127                ret = canid_parse_rule(&rules, args, iseff);
 128                if (ret == -1) {
 129                        ret = PARSE_ERR(args, "canid: Improperly formed CAN ID & mask\n");
 130                        goto exit;
 131                } else if (ret == -2) {
 132                        ret = PARSE_ERR(args, "canid: Too many arguments on input\n");
 133                        goto exit;
 134                }
 135        } while ((args = bstr_next(args)) != NULL);
 136
 137        addraw_l(n, MAX_MSG, hdr, sizeof(*hdr));
 138        addraw_l(n, MAX_MSG, rules.rules_raw,
 139                sizeof(struct can_filter) * rules.rules_cnt);
 140
 141#undef PARSE_ERR
 142exit:
 143        free(rules.rules_raw);
 144        return ret;
 145}
 146
 147static int canid_print_eopt(FILE *fd, struct tcf_ematch_hdr *hdr, void *data,
 148                          int data_len)
 149{
 150        struct can_filter *conf = data; /* Array with rules */
 151        int rules_count;
 152        int i;
 153
 154        rules_count = data_len / sizeof(struct can_filter);
 155
 156        for (i = 0; i < rules_count; i++) {
 157                struct can_filter *pcfltr = &conf[i];
 158
 159                if (pcfltr->can_id & CAN_EFF_FLAG) {
 160                        if (pcfltr->can_mask == (CAN_EFF_FLAG | CAN_EFF_MASK))
 161                                fprintf(fd, "eff 0x%"PRIX32,
 162                                                pcfltr->can_id & CAN_EFF_MASK);
 163                        else
 164                                fprintf(fd, "eff 0x%"PRIX32":0x%"PRIX32,
 165                                                pcfltr->can_id & CAN_EFF_MASK,
 166                                                pcfltr->can_mask & CAN_EFF_MASK);
 167                } else {
 168                        if (pcfltr->can_mask == (CAN_EFF_FLAG | CAN_SFF_MASK))
 169                                fprintf(fd, "sff 0x%"PRIX32,
 170                                                pcfltr->can_id & CAN_SFF_MASK);
 171                        else
 172                                fprintf(fd, "sff 0x%"PRIX32":0x%"PRIX32,
 173                                                pcfltr->can_id & CAN_SFF_MASK,
 174                                                pcfltr->can_mask & CAN_SFF_MASK);
 175                }
 176
 177                if ((i + 1) < rules_count)
 178                        fprintf(fd, " ");
 179        }
 180
 181        return 0;
 182}
 183
 184struct ematch_util canid_ematch_util = {
 185        .kind = "canid",
 186        .kind_num = TCF_EM_CANID,
 187        .parse_eopt = canid_parse_eopt,
 188        .print_eopt = canid_print_eopt,
 189        .print_usage = canid_print_usage
 190};
 191