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