]>
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> | |
7b5f30e1 RL |
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 | ||
39 | extern struct ematch_util canid_ematch_util; | |
40 | struct 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 | ||
46 | static 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 | ||
57 | static 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 | ||
91 | static 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 | ||
f89bb021 PS |
108 | rules.rules_raw = calloc(rules.rules_capacity, |
109 | sizeof(struct can_filter)); | |
7b5f30e1 RL |
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 | |
142 | exit: | |
143 | free(rules.rules_raw); | |
144 | return ret; | |
145 | } | |
146 | ||
147 | static 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 | ||
184 | struct 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 | }; |