2 * em_canid.c Ematch rule to match CAN frames according to their CAN identifiers
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.
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
17 * Documentation: http://rtime.felk.cvut.cz/can/socketcan-qdisc-final.pdf
24 #include <sys/socket.h>
25 #include <netinet/in.h>
26 #include <arpa/inet.h>
29 #include <linux/can.h>
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 */
39 extern struct ematch_util canid_ematch_util
;
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 */
46 static void canid_print_usage(FILE *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");
57 static int canid_parse_rule(struct rules
*rules
, struct bstr
*a
, int iseff
)
59 unsigned int can_id
= 0;
60 unsigned int can_mask
= 0;
62 if (sscanf(a
->data
, "%"SCNx32
":" "%"SCNx32
, &can_id
, &can_mask
) != 2) {
63 if (sscanf(a
->data
, "%"SCNx32
, &can_id
) != 1) {
66 can_mask
= (iseff
) ? CAN_EFF_MASK
: CAN_SFF_MASK
;
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
);
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
;
91 static int canid_parse_eopt(struct nlmsghdr
*n
, struct tcf_ematch_hdr
*hdr
,
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() */
102 #define PARSE_ERR(CARG, FMT, ARGS...) \
103 em_parse_error(EINVAL, args, CARG, &canid_ematch_util, FMT, ##ARGS)
106 return PARSE_ERR(args
, "canid: missing arguments");
108 rules
.rules_raw
= calloc(rules
.rules_capacity
,
109 sizeof(struct can_filter
));
112 if (!bstrcmp(args
, "sff")) {
114 } else if (!bstrcmp(args
, "eff")) {
117 ret
= PARSE_ERR(args
, "canid: invalid key");
121 args
= bstr_next(args
);
123 ret
= PARSE_ERR(args
, "canid: missing argument");
127 ret
= canid_parse_rule(&rules
, args
, iseff
);
129 ret
= PARSE_ERR(args
, "canid: Improperly formed CAN ID & mask\n");
131 } else if (ret
== -2) {
132 ret
= PARSE_ERR(args
, "canid: Too many arguments on input\n");
135 } while ((args
= bstr_next(args
)) != NULL
);
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
);
143 free(rules
.rules_raw
);
147 static int canid_print_eopt(FILE *fd
, struct tcf_ematch_hdr
*hdr
, void *data
,
150 struct can_filter
*conf
= data
; /* Array with rules */
154 rules_count
= data_len
/ sizeof(struct can_filter
);
156 for (i
= 0; i
< rules_count
; i
++) {
157 struct can_filter
*pcfltr
= &conf
[i
];
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
);
164 fprintf(fd
, "eff 0x%"PRIX32
":0x%"PRIX32
,
165 pcfltr
->can_id
& CAN_EFF_MASK
,
166 pcfltr
->can_mask
& CAN_EFF_MASK
);
168 if (pcfltr
->can_mask
== (CAN_EFF_FLAG
| CAN_SFF_MASK
))
169 fprintf(fd
, "sff 0x%"PRIX32
,
170 pcfltr
->can_id
& CAN_SFF_MASK
);
172 fprintf(fd
, "sff 0x%"PRIX32
":0x%"PRIX32
,
173 pcfltr
->can_id
& CAN_SFF_MASK
,
174 pcfltr
->can_mask
& CAN_SFF_MASK
);
177 if ((i
+ 1) < rules_count
)
184 struct ematch_util canid_ematch_util
= {
186 .kind_num
= TCF_EM_CANID
,
187 .parse_eopt
= canid_parse_eopt
,
188 .print_eopt
= canid_print_eopt
,
189 .print_usage
= canid_print_usage