]>
Commit | Line | Data |
---|---|---|
7b5f30e1 RL |
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 <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 | ||
40 | extern struct ematch_util canid_ematch_util; | |
41 | struct 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 | ||
47 | static 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 | ||
58 | static 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 | ||
92 | static 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 | ||
f89bb021 PS |
109 | rules.rules_raw = calloc(rules.rules_capacity, |
110 | sizeof(struct can_filter)); | |
7b5f30e1 RL |
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 | |
143 | exit: | |
144 | free(rules.rules_raw); | |
145 | return ret; | |
146 | } | |
147 | ||
148 | static 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 | ||
185 | struct 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 | }; |