Commit | Line | Data |
---|---|---|
d2912cb1 | 1 | // SPDX-License-Identifier: GPL-2.0-only |
febf018d DW |
2 | /* |
3 | * IEEE 802.1Q Multiple Registration Protocol (MRP) | |
4 | * | |
5 | * Copyright (c) 2012 Massachusetts Institute of Technology | |
6 | * | |
7 | * Adapted from code in net/802/garp.c | |
8 | * Copyright (c) 2008 Patrick McHardy <kaber@trash.net> | |
febf018d DW |
9 | */ |
10 | #include <linux/kernel.h> | |
11 | #include <linux/timer.h> | |
12 | #include <linux/skbuff.h> | |
13 | #include <linux/netdevice.h> | |
14 | #include <linux/etherdevice.h> | |
15 | #include <linux/rtnetlink.h> | |
16 | #include <linux/slab.h> | |
17 | #include <linux/module.h> | |
18 | #include <net/mrp.h> | |
19 | #include <asm/unaligned.h> | |
20 | ||
21 | static unsigned int mrp_join_time __read_mostly = 200; | |
22 | module_param(mrp_join_time, uint, 0644); | |
23 | MODULE_PARM_DESC(mrp_join_time, "Join time in ms (default 200ms)"); | |
9fe34f5d NBK |
24 | |
25 | static unsigned int mrp_periodic_time __read_mostly = 1000; | |
26 | module_param(mrp_periodic_time, uint, 0644); | |
27 | MODULE_PARM_DESC(mrp_periodic_time, "Periodic time in ms (default 1s)"); | |
28 | ||
febf018d DW |
29 | MODULE_LICENSE("GPL"); |
30 | ||
31 | static const u8 | |
32 | mrp_applicant_state_table[MRP_APPLICANT_MAX + 1][MRP_EVENT_MAX + 1] = { | |
33 | [MRP_APPLICANT_VO] = { | |
34 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
35 | [MRP_EVENT_JOIN] = MRP_APPLICANT_VP, | |
36 | [MRP_EVENT_LV] = MRP_APPLICANT_VO, | |
37 | [MRP_EVENT_TX] = MRP_APPLICANT_VO, | |
38 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_VO, | |
39 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AO, | |
40 | [MRP_EVENT_R_IN] = MRP_APPLICANT_VO, | |
41 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VO, | |
42 | [MRP_EVENT_R_MT] = MRP_APPLICANT_VO, | |
43 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VO, | |
44 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VO, | |
45 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO, | |
46 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VO, | |
47 | }, | |
48 | [MRP_APPLICANT_VP] = { | |
49 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
50 | [MRP_EVENT_JOIN] = MRP_APPLICANT_VP, | |
51 | [MRP_EVENT_LV] = MRP_APPLICANT_VO, | |
52 | [MRP_EVENT_TX] = MRP_APPLICANT_AA, | |
53 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_VP, | |
54 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AP, | |
55 | [MRP_EVENT_R_IN] = MRP_APPLICANT_VP, | |
56 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VP, | |
57 | [MRP_EVENT_R_MT] = MRP_APPLICANT_VP, | |
58 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VP, | |
59 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VP, | |
60 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, | |
61 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VP, | |
62 | }, | |
63 | [MRP_APPLICANT_VN] = { | |
64 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
65 | [MRP_EVENT_JOIN] = MRP_APPLICANT_VN, | |
66 | [MRP_EVENT_LV] = MRP_APPLICANT_LA, | |
67 | [MRP_EVENT_TX] = MRP_APPLICANT_AN, | |
68 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_VN, | |
69 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_VN, | |
70 | [MRP_EVENT_R_IN] = MRP_APPLICANT_VN, | |
71 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VN, | |
72 | [MRP_EVENT_R_MT] = MRP_APPLICANT_VN, | |
73 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VN, | |
74 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VN, | |
75 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VN, | |
76 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_VN, | |
77 | }, | |
78 | [MRP_APPLICANT_AN] = { | |
79 | [MRP_EVENT_NEW] = MRP_APPLICANT_AN, | |
80 | [MRP_EVENT_JOIN] = MRP_APPLICANT_AN, | |
81 | [MRP_EVENT_LV] = MRP_APPLICANT_LA, | |
82 | [MRP_EVENT_TX] = MRP_APPLICANT_QA, | |
83 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_AN, | |
84 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AN, | |
85 | [MRP_EVENT_R_IN] = MRP_APPLICANT_AN, | |
86 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AN, | |
87 | [MRP_EVENT_R_MT] = MRP_APPLICANT_AN, | |
88 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VN, | |
89 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VN, | |
90 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VN, | |
91 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AN, | |
92 | }, | |
93 | [MRP_APPLICANT_AA] = { | |
94 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
95 | [MRP_EVENT_JOIN] = MRP_APPLICANT_AA, | |
96 | [MRP_EVENT_LV] = MRP_APPLICANT_LA, | |
97 | [MRP_EVENT_TX] = MRP_APPLICANT_QA, | |
98 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_AA, | |
99 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QA, | |
100 | [MRP_EVENT_R_IN] = MRP_APPLICANT_AA, | |
101 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AA, | |
102 | [MRP_EVENT_R_MT] = MRP_APPLICANT_AA, | |
103 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VP, | |
104 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VP, | |
105 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, | |
106 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AA, | |
107 | }, | |
108 | [MRP_APPLICANT_QA] = { | |
109 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
110 | [MRP_EVENT_JOIN] = MRP_APPLICANT_QA, | |
111 | [MRP_EVENT_LV] = MRP_APPLICANT_LA, | |
112 | [MRP_EVENT_TX] = MRP_APPLICANT_QA, | |
113 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_QA, | |
114 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QA, | |
115 | [MRP_EVENT_R_IN] = MRP_APPLICANT_QA, | |
116 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AA, | |
117 | [MRP_EVENT_R_MT] = MRP_APPLICANT_AA, | |
118 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VP, | |
119 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VP, | |
120 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, | |
121 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AA, | |
122 | }, | |
123 | [MRP_APPLICANT_LA] = { | |
124 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
125 | [MRP_EVENT_JOIN] = MRP_APPLICANT_AA, | |
126 | [MRP_EVENT_LV] = MRP_APPLICANT_LA, | |
127 | [MRP_EVENT_TX] = MRP_APPLICANT_VO, | |
128 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_LA, | |
129 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_LA, | |
130 | [MRP_EVENT_R_IN] = MRP_APPLICANT_LA, | |
131 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_LA, | |
132 | [MRP_EVENT_R_MT] = MRP_APPLICANT_LA, | |
133 | [MRP_EVENT_R_LV] = MRP_APPLICANT_LA, | |
134 | [MRP_EVENT_R_LA] = MRP_APPLICANT_LA, | |
135 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_LA, | |
136 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_LA, | |
137 | }, | |
138 | [MRP_APPLICANT_AO] = { | |
139 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
140 | [MRP_EVENT_JOIN] = MRP_APPLICANT_AP, | |
141 | [MRP_EVENT_LV] = MRP_APPLICANT_AO, | |
142 | [MRP_EVENT_TX] = MRP_APPLICANT_AO, | |
143 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_AO, | |
144 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QO, | |
145 | [MRP_EVENT_R_IN] = MRP_APPLICANT_AO, | |
146 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AO, | |
147 | [MRP_EVENT_R_MT] = MRP_APPLICANT_AO, | |
148 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VO, | |
149 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VO, | |
150 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO, | |
151 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AO, | |
152 | }, | |
153 | [MRP_APPLICANT_QO] = { | |
154 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
155 | [MRP_EVENT_JOIN] = MRP_APPLICANT_QP, | |
156 | [MRP_EVENT_LV] = MRP_APPLICANT_QO, | |
157 | [MRP_EVENT_TX] = MRP_APPLICANT_QO, | |
158 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_QO, | |
159 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QO, | |
160 | [MRP_EVENT_R_IN] = MRP_APPLICANT_QO, | |
161 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AO, | |
162 | [MRP_EVENT_R_MT] = MRP_APPLICANT_AO, | |
163 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VO, | |
164 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VO, | |
165 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO, | |
166 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_QO, | |
167 | }, | |
168 | [MRP_APPLICANT_AP] = { | |
169 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
170 | [MRP_EVENT_JOIN] = MRP_APPLICANT_AP, | |
171 | [MRP_EVENT_LV] = MRP_APPLICANT_AO, | |
172 | [MRP_EVENT_TX] = MRP_APPLICANT_QA, | |
173 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_AP, | |
174 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QP, | |
175 | [MRP_EVENT_R_IN] = MRP_APPLICANT_AP, | |
176 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AP, | |
177 | [MRP_EVENT_R_MT] = MRP_APPLICANT_AP, | |
178 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VP, | |
179 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VP, | |
180 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, | |
181 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AP, | |
182 | }, | |
183 | [MRP_APPLICANT_QP] = { | |
184 | [MRP_EVENT_NEW] = MRP_APPLICANT_VN, | |
185 | [MRP_EVENT_JOIN] = MRP_APPLICANT_QP, | |
186 | [MRP_EVENT_LV] = MRP_APPLICANT_QO, | |
187 | [MRP_EVENT_TX] = MRP_APPLICANT_QP, | |
188 | [MRP_EVENT_R_NEW] = MRP_APPLICANT_QP, | |
189 | [MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QP, | |
190 | [MRP_EVENT_R_IN] = MRP_APPLICANT_QP, | |
191 | [MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AP, | |
192 | [MRP_EVENT_R_MT] = MRP_APPLICANT_AP, | |
193 | [MRP_EVENT_R_LV] = MRP_APPLICANT_VP, | |
194 | [MRP_EVENT_R_LA] = MRP_APPLICANT_VP, | |
195 | [MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, | |
196 | [MRP_EVENT_PERIODIC] = MRP_APPLICANT_AP, | |
197 | }, | |
198 | }; | |
199 | ||
200 | static const u8 | |
201 | mrp_tx_action_table[MRP_APPLICANT_MAX + 1] = { | |
202 | [MRP_APPLICANT_VO] = MRP_TX_ACTION_S_IN_OPTIONAL, | |
203 | [MRP_APPLICANT_VP] = MRP_TX_ACTION_S_JOIN_IN, | |
204 | [MRP_APPLICANT_VN] = MRP_TX_ACTION_S_NEW, | |
205 | [MRP_APPLICANT_AN] = MRP_TX_ACTION_S_NEW, | |
206 | [MRP_APPLICANT_AA] = MRP_TX_ACTION_S_JOIN_IN, | |
207 | [MRP_APPLICANT_QA] = MRP_TX_ACTION_S_JOIN_IN_OPTIONAL, | |
208 | [MRP_APPLICANT_LA] = MRP_TX_ACTION_S_LV, | |
209 | [MRP_APPLICANT_AO] = MRP_TX_ACTION_S_IN_OPTIONAL, | |
210 | [MRP_APPLICANT_QO] = MRP_TX_ACTION_S_IN_OPTIONAL, | |
211 | [MRP_APPLICANT_AP] = MRP_TX_ACTION_S_JOIN_IN, | |
212 | [MRP_APPLICANT_QP] = MRP_TX_ACTION_S_IN_OPTIONAL, | |
213 | }; | |
214 | ||
215 | static void mrp_attrvalue_inc(void *value, u8 len) | |
216 | { | |
217 | u8 *v = (u8 *)value; | |
218 | ||
219 | /* Add 1 to the last byte. If it becomes zero, | |
220 | * go to the previous byte and repeat. | |
221 | */ | |
222 | while (len > 0 && !++v[--len]) | |
223 | ; | |
224 | } | |
225 | ||
226 | static int mrp_attr_cmp(const struct mrp_attr *attr, | |
227 | const void *value, u8 len, u8 type) | |
228 | { | |
229 | if (attr->type != type) | |
230 | return attr->type - type; | |
231 | if (attr->len != len) | |
232 | return attr->len - len; | |
233 | return memcmp(attr->value, value, len); | |
234 | } | |
235 | ||
236 | static struct mrp_attr *mrp_attr_lookup(const struct mrp_applicant *app, | |
237 | const void *value, u8 len, u8 type) | |
238 | { | |
239 | struct rb_node *parent = app->mad.rb_node; | |
240 | struct mrp_attr *attr; | |
241 | int d; | |
242 | ||
243 | while (parent) { | |
244 | attr = rb_entry(parent, struct mrp_attr, node); | |
245 | d = mrp_attr_cmp(attr, value, len, type); | |
246 | if (d > 0) | |
247 | parent = parent->rb_left; | |
248 | else if (d < 0) | |
249 | parent = parent->rb_right; | |
250 | else | |
251 | return attr; | |
252 | } | |
253 | return NULL; | |
254 | } | |
255 | ||
256 | static struct mrp_attr *mrp_attr_create(struct mrp_applicant *app, | |
257 | const void *value, u8 len, u8 type) | |
258 | { | |
259 | struct rb_node *parent = NULL, **p = &app->mad.rb_node; | |
260 | struct mrp_attr *attr; | |
261 | int d; | |
262 | ||
263 | while (*p) { | |
264 | parent = *p; | |
265 | attr = rb_entry(parent, struct mrp_attr, node); | |
266 | d = mrp_attr_cmp(attr, value, len, type); | |
267 | if (d > 0) | |
268 | p = &parent->rb_left; | |
269 | else if (d < 0) | |
270 | p = &parent->rb_right; | |
271 | else { | |
272 | /* The attribute already exists; re-use it. */ | |
273 | return attr; | |
274 | } | |
275 | } | |
276 | attr = kmalloc(sizeof(*attr) + len, GFP_ATOMIC); | |
277 | if (!attr) | |
278 | return attr; | |
279 | attr->state = MRP_APPLICANT_VO; | |
280 | attr->type = type; | |
281 | attr->len = len; | |
282 | memcpy(attr->value, value, len); | |
283 | ||
284 | rb_link_node(&attr->node, parent, p); | |
285 | rb_insert_color(&attr->node, &app->mad); | |
286 | return attr; | |
287 | } | |
288 | ||
289 | static void mrp_attr_destroy(struct mrp_applicant *app, struct mrp_attr *attr) | |
290 | { | |
291 | rb_erase(&attr->node, &app->mad); | |
292 | kfree(attr); | |
293 | } | |
294 | ||
996af621 YY |
295 | static void mrp_attr_destroy_all(struct mrp_applicant *app) |
296 | { | |
297 | struct rb_node *node, *next; | |
298 | struct mrp_attr *attr; | |
299 | ||
300 | for (node = rb_first(&app->mad); | |
301 | next = node ? rb_next(node) : NULL, node != NULL; | |
302 | node = next) { | |
303 | attr = rb_entry(node, struct mrp_attr, node); | |
304 | mrp_attr_destroy(app, attr); | |
305 | } | |
306 | } | |
307 | ||
febf018d DW |
308 | static int mrp_pdu_init(struct mrp_applicant *app) |
309 | { | |
310 | struct sk_buff *skb; | |
311 | struct mrp_pdu_hdr *ph; | |
312 | ||
313 | skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev), | |
314 | GFP_ATOMIC); | |
315 | if (!skb) | |
316 | return -ENOMEM; | |
317 | ||
318 | skb->dev = app->dev; | |
319 | skb->protocol = app->app->pkttype.type; | |
320 | skb_reserve(skb, LL_RESERVED_SPACE(app->dev)); | |
321 | skb_reset_network_header(skb); | |
322 | skb_reset_transport_header(skb); | |
323 | ||
4df864c1 | 324 | ph = __skb_put(skb, sizeof(*ph)); |
febf018d DW |
325 | ph->version = app->app->version; |
326 | ||
327 | app->pdu = skb; | |
328 | return 0; | |
329 | } | |
330 | ||
331 | static int mrp_pdu_append_end_mark(struct mrp_applicant *app) | |
332 | { | |
333 | __be16 *endmark; | |
334 | ||
335 | if (skb_tailroom(app->pdu) < sizeof(*endmark)) | |
336 | return -1; | |
4df864c1 | 337 | endmark = __skb_put(app->pdu, sizeof(*endmark)); |
febf018d DW |
338 | put_unaligned(MRP_END_MARK, endmark); |
339 | return 0; | |
340 | } | |
341 | ||
342 | static void mrp_pdu_queue(struct mrp_applicant *app) | |
343 | { | |
344 | if (!app->pdu) | |
345 | return; | |
346 | ||
347 | if (mrp_cb(app->pdu)->mh) | |
348 | mrp_pdu_append_end_mark(app); | |
349 | mrp_pdu_append_end_mark(app); | |
350 | ||
351 | dev_hard_header(app->pdu, app->dev, ntohs(app->app->pkttype.type), | |
352 | app->app->group_address, app->dev->dev_addr, | |
353 | app->pdu->len); | |
354 | ||
355 | skb_queue_tail(&app->queue, app->pdu); | |
356 | app->pdu = NULL; | |
357 | } | |
358 | ||
359 | static void mrp_queue_xmit(struct mrp_applicant *app) | |
360 | { | |
361 | struct sk_buff *skb; | |
362 | ||
363 | while ((skb = skb_dequeue(&app->queue))) | |
364 | dev_queue_xmit(skb); | |
365 | } | |
366 | ||
367 | static int mrp_pdu_append_msg_hdr(struct mrp_applicant *app, | |
368 | u8 attrtype, u8 attrlen) | |
369 | { | |
370 | struct mrp_msg_hdr *mh; | |
371 | ||
372 | if (mrp_cb(app->pdu)->mh) { | |
373 | if (mrp_pdu_append_end_mark(app) < 0) | |
374 | return -1; | |
375 | mrp_cb(app->pdu)->mh = NULL; | |
376 | mrp_cb(app->pdu)->vah = NULL; | |
377 | } | |
378 | ||
379 | if (skb_tailroom(app->pdu) < sizeof(*mh)) | |
380 | return -1; | |
4df864c1 | 381 | mh = __skb_put(app->pdu, sizeof(*mh)); |
febf018d DW |
382 | mh->attrtype = attrtype; |
383 | mh->attrlen = attrlen; | |
384 | mrp_cb(app->pdu)->mh = mh; | |
385 | return 0; | |
386 | } | |
387 | ||
388 | static int mrp_pdu_append_vecattr_hdr(struct mrp_applicant *app, | |
389 | const void *firstattrvalue, u8 attrlen) | |
390 | { | |
391 | struct mrp_vecattr_hdr *vah; | |
392 | ||
393 | if (skb_tailroom(app->pdu) < sizeof(*vah) + attrlen) | |
394 | return -1; | |
4df864c1 | 395 | vah = __skb_put(app->pdu, sizeof(*vah) + attrlen); |
febf018d DW |
396 | put_unaligned(0, &vah->lenflags); |
397 | memcpy(vah->firstattrvalue, firstattrvalue, attrlen); | |
398 | mrp_cb(app->pdu)->vah = vah; | |
399 | memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen); | |
400 | return 0; | |
401 | } | |
402 | ||
403 | static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app, | |
404 | const struct mrp_attr *attr, | |
405 | enum mrp_vecattr_event vaevent) | |
406 | { | |
407 | u16 len, pos; | |
408 | u8 *vaevents; | |
409 | int err; | |
410 | again: | |
411 | if (!app->pdu) { | |
412 | err = mrp_pdu_init(app); | |
413 | if (err < 0) | |
414 | return err; | |
415 | } | |
416 | ||
417 | /* If there is no Message header in the PDU, or the Message header is | |
418 | * for a different attribute type, add an EndMark (if necessary) and a | |
419 | * new Message header to the PDU. | |
420 | */ | |
421 | if (!mrp_cb(app->pdu)->mh || | |
422 | mrp_cb(app->pdu)->mh->attrtype != attr->type || | |
423 | mrp_cb(app->pdu)->mh->attrlen != attr->len) { | |
424 | if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0) | |
425 | goto queue; | |
426 | } | |
427 | ||
428 | /* If there is no VectorAttribute header for this Message in the PDU, | |
429 | * or this attribute's value does not sequentially follow the previous | |
430 | * attribute's value, add a new VectorAttribute header to the PDU. | |
431 | */ | |
432 | if (!mrp_cb(app->pdu)->vah || | |
433 | memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) { | |
434 | if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0) | |
435 | goto queue; | |
436 | } | |
437 | ||
438 | len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags)); | |
439 | pos = len % 3; | |
440 | ||
441 | /* Events are packed into Vectors in the PDU, three to a byte. Add a | |
442 | * byte to the end of the Vector if necessary. | |
443 | */ | |
444 | if (!pos) { | |
445 | if (skb_tailroom(app->pdu) < sizeof(u8)) | |
446 | goto queue; | |
4df864c1 | 447 | vaevents = __skb_put(app->pdu, sizeof(u8)); |
febf018d DW |
448 | } else { |
449 | vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8)); | |
450 | } | |
451 | ||
452 | switch (pos) { | |
453 | case 0: | |
454 | *vaevents = vaevent * (__MRP_VECATTR_EVENT_MAX * | |
455 | __MRP_VECATTR_EVENT_MAX); | |
456 | break; | |
457 | case 1: | |
458 | *vaevents += vaevent * __MRP_VECATTR_EVENT_MAX; | |
459 | break; | |
460 | case 2: | |
461 | *vaevents += vaevent; | |
462 | break; | |
463 | default: | |
464 | WARN_ON(1); | |
465 | } | |
466 | ||
467 | /* Increment the length of the VectorAttribute in the PDU, as well as | |
468 | * the value of the next attribute that would continue its Vector. | |
469 | */ | |
470 | put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags); | |
471 | mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len); | |
472 | ||
473 | return 0; | |
474 | ||
475 | queue: | |
476 | mrp_pdu_queue(app); | |
477 | goto again; | |
478 | } | |
479 | ||
480 | static void mrp_attr_event(struct mrp_applicant *app, | |
481 | struct mrp_attr *attr, enum mrp_event event) | |
482 | { | |
483 | enum mrp_applicant_state state; | |
484 | ||
485 | state = mrp_applicant_state_table[attr->state][event]; | |
486 | if (state == MRP_APPLICANT_INVALID) { | |
487 | WARN_ON(1); | |
488 | return; | |
489 | } | |
490 | ||
491 | if (event == MRP_EVENT_TX) { | |
492 | /* When appending the attribute fails, don't update its state | |
493 | * in order to retry at the next TX event. | |
494 | */ | |
495 | ||
496 | switch (mrp_tx_action_table[attr->state]) { | |
497 | case MRP_TX_ACTION_NONE: | |
498 | case MRP_TX_ACTION_S_JOIN_IN_OPTIONAL: | |
499 | case MRP_TX_ACTION_S_IN_OPTIONAL: | |
500 | break; | |
501 | case MRP_TX_ACTION_S_NEW: | |
502 | if (mrp_pdu_append_vecattr_event( | |
503 | app, attr, MRP_VECATTR_EVENT_NEW) < 0) | |
504 | return; | |
505 | break; | |
506 | case MRP_TX_ACTION_S_JOIN_IN: | |
507 | if (mrp_pdu_append_vecattr_event( | |
508 | app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0) | |
509 | return; | |
510 | break; | |
511 | case MRP_TX_ACTION_S_LV: | |
512 | if (mrp_pdu_append_vecattr_event( | |
513 | app, attr, MRP_VECATTR_EVENT_LV) < 0) | |
514 | return; | |
515 | /* As a pure applicant, sending a leave message | |
516 | * implies that the attribute was unregistered and | |
517 | * can be destroyed. | |
518 | */ | |
519 | mrp_attr_destroy(app, attr); | |
520 | return; | |
521 | default: | |
522 | WARN_ON(1); | |
523 | } | |
524 | } | |
525 | ||
526 | attr->state = state; | |
527 | } | |
528 | ||
529 | int mrp_request_join(const struct net_device *dev, | |
530 | const struct mrp_application *appl, | |
531 | const void *value, u8 len, u8 type) | |
532 | { | |
533 | struct mrp_port *port = rtnl_dereference(dev->mrp_port); | |
534 | struct mrp_applicant *app = rtnl_dereference( | |
535 | port->applicants[appl->type]); | |
536 | struct mrp_attr *attr; | |
537 | ||
538 | if (sizeof(struct mrp_skb_cb) + len > | |
c593642c | 539 | sizeof_field(struct sk_buff, cb)) |
febf018d DW |
540 | return -ENOMEM; |
541 | ||
542 | spin_lock_bh(&app->lock); | |
543 | attr = mrp_attr_create(app, value, len, type); | |
544 | if (!attr) { | |
545 | spin_unlock_bh(&app->lock); | |
546 | return -ENOMEM; | |
547 | } | |
548 | mrp_attr_event(app, attr, MRP_EVENT_JOIN); | |
549 | spin_unlock_bh(&app->lock); | |
550 | return 0; | |
551 | } | |
552 | EXPORT_SYMBOL_GPL(mrp_request_join); | |
553 | ||
554 | void mrp_request_leave(const struct net_device *dev, | |
555 | const struct mrp_application *appl, | |
556 | const void *value, u8 len, u8 type) | |
557 | { | |
558 | struct mrp_port *port = rtnl_dereference(dev->mrp_port); | |
559 | struct mrp_applicant *app = rtnl_dereference( | |
560 | port->applicants[appl->type]); | |
561 | struct mrp_attr *attr; | |
562 | ||
563 | if (sizeof(struct mrp_skb_cb) + len > | |
c593642c | 564 | sizeof_field(struct sk_buff, cb)) |
febf018d DW |
565 | return; |
566 | ||
567 | spin_lock_bh(&app->lock); | |
568 | attr = mrp_attr_lookup(app, value, len, type); | |
569 | if (!attr) { | |
570 | spin_unlock_bh(&app->lock); | |
571 | return; | |
572 | } | |
573 | mrp_attr_event(app, attr, MRP_EVENT_LV); | |
574 | spin_unlock_bh(&app->lock); | |
575 | } | |
576 | EXPORT_SYMBOL_GPL(mrp_request_leave); | |
577 | ||
578 | static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event) | |
579 | { | |
580 | struct rb_node *node, *next; | |
581 | struct mrp_attr *attr; | |
582 | ||
583 | for (node = rb_first(&app->mad); | |
584 | next = node ? rb_next(node) : NULL, node != NULL; | |
585 | node = next) { | |
586 | attr = rb_entry(node, struct mrp_attr, node); | |
587 | mrp_attr_event(app, attr, event); | |
588 | } | |
589 | } | |
590 | ||
591 | static void mrp_join_timer_arm(struct mrp_applicant *app) | |
592 | { | |
593 | unsigned long delay; | |
594 | ||
81895a65 | 595 | delay = prandom_u32_max(msecs_to_jiffies(mrp_join_time)); |
febf018d DW |
596 | mod_timer(&app->join_timer, jiffies + delay); |
597 | } | |
598 | ||
e99e88a9 | 599 | static void mrp_join_timer(struct timer_list *t) |
febf018d | 600 | { |
e99e88a9 | 601 | struct mrp_applicant *app = from_timer(app, t, join_timer); |
febf018d DW |
602 | |
603 | spin_lock(&app->lock); | |
604 | mrp_mad_event(app, MRP_EVENT_TX); | |
605 | mrp_pdu_queue(app); | |
606 | spin_unlock(&app->lock); | |
607 | ||
608 | mrp_queue_xmit(app); | |
ab037780 SS |
609 | spin_lock(&app->lock); |
610 | if (likely(app->active)) | |
611 | mrp_join_timer_arm(app); | |
612 | spin_unlock(&app->lock); | |
febf018d DW |
613 | } |
614 | ||
9fe34f5d NBK |
615 | static void mrp_periodic_timer_arm(struct mrp_applicant *app) |
616 | { | |
617 | mod_timer(&app->periodic_timer, | |
618 | jiffies + msecs_to_jiffies(mrp_periodic_time)); | |
619 | } | |
620 | ||
e99e88a9 | 621 | static void mrp_periodic_timer(struct timer_list *t) |
9fe34f5d | 622 | { |
e99e88a9 | 623 | struct mrp_applicant *app = from_timer(app, t, periodic_timer); |
9fe34f5d NBK |
624 | |
625 | spin_lock(&app->lock); | |
ab037780 SS |
626 | if (likely(app->active)) { |
627 | mrp_mad_event(app, MRP_EVENT_PERIODIC); | |
628 | mrp_pdu_queue(app); | |
629 | mrp_periodic_timer_arm(app); | |
630 | } | |
9fe34f5d | 631 | spin_unlock(&app->lock); |
9fe34f5d NBK |
632 | } |
633 | ||
febf018d DW |
634 | static int mrp_pdu_parse_end_mark(struct sk_buff *skb, int *offset) |
635 | { | |
636 | __be16 endmark; | |
637 | ||
638 | if (skb_copy_bits(skb, *offset, &endmark, sizeof(endmark)) < 0) | |
639 | return -1; | |
640 | if (endmark == MRP_END_MARK) { | |
641 | *offset += sizeof(endmark); | |
642 | return -1; | |
643 | } | |
644 | return 0; | |
645 | } | |
646 | ||
647 | static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app, | |
648 | struct sk_buff *skb, | |
649 | enum mrp_vecattr_event vaevent) | |
650 | { | |
651 | struct mrp_attr *attr; | |
652 | enum mrp_event event; | |
653 | ||
654 | attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue, | |
655 | mrp_cb(skb)->mh->attrlen, | |
656 | mrp_cb(skb)->mh->attrtype); | |
657 | if (attr == NULL) | |
658 | return; | |
659 | ||
660 | switch (vaevent) { | |
661 | case MRP_VECATTR_EVENT_NEW: | |
662 | event = MRP_EVENT_R_NEW; | |
663 | break; | |
664 | case MRP_VECATTR_EVENT_JOIN_IN: | |
665 | event = MRP_EVENT_R_JOIN_IN; | |
666 | break; | |
667 | case MRP_VECATTR_EVENT_IN: | |
668 | event = MRP_EVENT_R_IN; | |
669 | break; | |
670 | case MRP_VECATTR_EVENT_JOIN_MT: | |
671 | event = MRP_EVENT_R_JOIN_MT; | |
672 | break; | |
673 | case MRP_VECATTR_EVENT_MT: | |
674 | event = MRP_EVENT_R_MT; | |
675 | break; | |
676 | case MRP_VECATTR_EVENT_LV: | |
677 | event = MRP_EVENT_R_LV; | |
678 | break; | |
679 | default: | |
680 | return; | |
681 | } | |
682 | ||
683 | mrp_attr_event(app, attr, event); | |
684 | } | |
685 | ||
686 | static int mrp_pdu_parse_vecattr(struct mrp_applicant *app, | |
687 | struct sk_buff *skb, int *offset) | |
688 | { | |
689 | struct mrp_vecattr_hdr _vah; | |
690 | u16 valen; | |
691 | u8 vaevents, vaevent; | |
692 | ||
693 | mrp_cb(skb)->vah = skb_header_pointer(skb, *offset, sizeof(_vah), | |
694 | &_vah); | |
695 | if (!mrp_cb(skb)->vah) | |
696 | return -1; | |
697 | *offset += sizeof(_vah); | |
698 | ||
699 | if (get_unaligned(&mrp_cb(skb)->vah->lenflags) & | |
700 | MRP_VECATTR_HDR_FLAG_LA) | |
701 | mrp_mad_event(app, MRP_EVENT_R_LA); | |
702 | valen = be16_to_cpu(get_unaligned(&mrp_cb(skb)->vah->lenflags) & | |
703 | MRP_VECATTR_HDR_LEN_MASK); | |
704 | ||
705 | /* The VectorAttribute structure in a PDU carries event information | |
706 | * about one or more attributes having consecutive values. Only the | |
707 | * value for the first attribute is contained in the structure. So | |
708 | * we make a copy of that value, and then increment it each time we | |
709 | * advance to the next event in its Vector. | |
710 | */ | |
711 | if (sizeof(struct mrp_skb_cb) + mrp_cb(skb)->mh->attrlen > | |
c593642c | 712 | sizeof_field(struct sk_buff, cb)) |
febf018d DW |
713 | return -1; |
714 | if (skb_copy_bits(skb, *offset, mrp_cb(skb)->attrvalue, | |
715 | mrp_cb(skb)->mh->attrlen) < 0) | |
716 | return -1; | |
717 | *offset += mrp_cb(skb)->mh->attrlen; | |
718 | ||
719 | /* In a VectorAttribute, the Vector contains events which are packed | |
720 | * three to a byte. We process one byte of the Vector at a time. | |
721 | */ | |
722 | while (valen > 0) { | |
723 | if (skb_copy_bits(skb, *offset, &vaevents, | |
724 | sizeof(vaevents)) < 0) | |
725 | return -1; | |
726 | *offset += sizeof(vaevents); | |
727 | ||
728 | /* Extract and process the first event. */ | |
729 | vaevent = vaevents / (__MRP_VECATTR_EVENT_MAX * | |
730 | __MRP_VECATTR_EVENT_MAX); | |
731 | if (vaevent >= __MRP_VECATTR_EVENT_MAX) { | |
732 | /* The byte is malformed; stop processing. */ | |
733 | return -1; | |
734 | } | |
735 | mrp_pdu_parse_vecattr_event(app, skb, vaevent); | |
736 | ||
737 | /* If present, extract and process the second event. */ | |
738 | if (!--valen) | |
739 | break; | |
740 | mrp_attrvalue_inc(mrp_cb(skb)->attrvalue, | |
741 | mrp_cb(skb)->mh->attrlen); | |
742 | vaevents %= (__MRP_VECATTR_EVENT_MAX * | |
743 | __MRP_VECATTR_EVENT_MAX); | |
744 | vaevent = vaevents / __MRP_VECATTR_EVENT_MAX; | |
745 | mrp_pdu_parse_vecattr_event(app, skb, vaevent); | |
746 | ||
747 | /* If present, extract and process the third event. */ | |
748 | if (!--valen) | |
749 | break; | |
750 | mrp_attrvalue_inc(mrp_cb(skb)->attrvalue, | |
751 | mrp_cb(skb)->mh->attrlen); | |
752 | vaevents %= __MRP_VECATTR_EVENT_MAX; | |
753 | vaevent = vaevents; | |
754 | mrp_pdu_parse_vecattr_event(app, skb, vaevent); | |
755 | } | |
756 | return 0; | |
757 | } | |
758 | ||
759 | static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb, | |
760 | int *offset) | |
761 | { | |
762 | struct mrp_msg_hdr _mh; | |
763 | ||
764 | mrp_cb(skb)->mh = skb_header_pointer(skb, *offset, sizeof(_mh), &_mh); | |
765 | if (!mrp_cb(skb)->mh) | |
766 | return -1; | |
767 | *offset += sizeof(_mh); | |
768 | ||
769 | if (mrp_cb(skb)->mh->attrtype == 0 || | |
770 | mrp_cb(skb)->mh->attrtype > app->app->maxattr || | |
771 | mrp_cb(skb)->mh->attrlen == 0) | |
772 | return -1; | |
773 | ||
774 | while (skb->len > *offset) { | |
775 | if (mrp_pdu_parse_end_mark(skb, offset) < 0) | |
776 | break; | |
777 | if (mrp_pdu_parse_vecattr(app, skb, offset) < 0) | |
778 | return -1; | |
779 | } | |
780 | return 0; | |
781 | } | |
782 | ||
7e307c67 SH |
783 | static int mrp_rcv(struct sk_buff *skb, struct net_device *dev, |
784 | struct packet_type *pt, struct net_device *orig_dev) | |
febf018d DW |
785 | { |
786 | struct mrp_application *appl = container_of(pt, struct mrp_application, | |
787 | pkttype); | |
788 | struct mrp_port *port; | |
789 | struct mrp_applicant *app; | |
790 | struct mrp_pdu_hdr _ph; | |
791 | const struct mrp_pdu_hdr *ph; | |
792 | int offset = skb_network_offset(skb); | |
793 | ||
794 | /* If the interface is in promiscuous mode, drop the packet if | |
795 | * it was unicast to another host. | |
796 | */ | |
797 | if (unlikely(skb->pkt_type == PACKET_OTHERHOST)) | |
798 | goto out; | |
799 | skb = skb_share_check(skb, GFP_ATOMIC); | |
800 | if (unlikely(!skb)) | |
801 | goto out; | |
802 | port = rcu_dereference(dev->mrp_port); | |
803 | if (unlikely(!port)) | |
804 | goto out; | |
805 | app = rcu_dereference(port->applicants[appl->type]); | |
806 | if (unlikely(!app)) | |
807 | goto out; | |
808 | ||
809 | ph = skb_header_pointer(skb, offset, sizeof(_ph), &_ph); | |
810 | if (!ph) | |
811 | goto out; | |
812 | offset += sizeof(_ph); | |
813 | ||
814 | if (ph->version != app->app->version) | |
815 | goto out; | |
816 | ||
817 | spin_lock(&app->lock); | |
818 | while (skb->len > offset) { | |
819 | if (mrp_pdu_parse_end_mark(skb, &offset) < 0) | |
820 | break; | |
821 | if (mrp_pdu_parse_msg(app, skb, &offset) < 0) | |
822 | break; | |
823 | } | |
824 | spin_unlock(&app->lock); | |
825 | out: | |
826 | kfree_skb(skb); | |
827 | return 0; | |
828 | } | |
829 | ||
830 | static int mrp_init_port(struct net_device *dev) | |
831 | { | |
832 | struct mrp_port *port; | |
833 | ||
834 | port = kzalloc(sizeof(*port), GFP_KERNEL); | |
835 | if (!port) | |
836 | return -ENOMEM; | |
837 | rcu_assign_pointer(dev->mrp_port, port); | |
838 | return 0; | |
839 | } | |
840 | ||
841 | static void mrp_release_port(struct net_device *dev) | |
842 | { | |
843 | struct mrp_port *port = rtnl_dereference(dev->mrp_port); | |
844 | unsigned int i; | |
845 | ||
846 | for (i = 0; i <= MRP_APPLICATION_MAX; i++) { | |
847 | if (rtnl_dereference(port->applicants[i])) | |
848 | return; | |
849 | } | |
850 | RCU_INIT_POINTER(dev->mrp_port, NULL); | |
851 | kfree_rcu(port, rcu); | |
852 | } | |
853 | ||
854 | int mrp_init_applicant(struct net_device *dev, struct mrp_application *appl) | |
855 | { | |
856 | struct mrp_applicant *app; | |
857 | int err; | |
858 | ||
859 | ASSERT_RTNL(); | |
860 | ||
861 | if (!rtnl_dereference(dev->mrp_port)) { | |
862 | err = mrp_init_port(dev); | |
863 | if (err < 0) | |
864 | goto err1; | |
865 | } | |
866 | ||
867 | err = -ENOMEM; | |
868 | app = kzalloc(sizeof(*app), GFP_KERNEL); | |
869 | if (!app) | |
870 | goto err2; | |
871 | ||
872 | err = dev_mc_add(dev, appl->group_address); | |
873 | if (err < 0) | |
874 | goto err3; | |
875 | ||
876 | app->dev = dev; | |
877 | app->app = appl; | |
878 | app->mad = RB_ROOT; | |
ab037780 | 879 | app->active = true; |
febf018d DW |
880 | spin_lock_init(&app->lock); |
881 | skb_queue_head_init(&app->queue); | |
882 | rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app); | |
e99e88a9 | 883 | timer_setup(&app->join_timer, mrp_join_timer, 0); |
febf018d | 884 | mrp_join_timer_arm(app); |
e99e88a9 | 885 | timer_setup(&app->periodic_timer, mrp_periodic_timer, 0); |
9fe34f5d | 886 | mrp_periodic_timer_arm(app); |
febf018d DW |
887 | return 0; |
888 | ||
889 | err3: | |
890 | kfree(app); | |
891 | err2: | |
892 | mrp_release_port(dev); | |
893 | err1: | |
894 | return err; | |
895 | } | |
896 | EXPORT_SYMBOL_GPL(mrp_init_applicant); | |
897 | ||
898 | void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl) | |
899 | { | |
900 | struct mrp_port *port = rtnl_dereference(dev->mrp_port); | |
901 | struct mrp_applicant *app = rtnl_dereference( | |
902 | port->applicants[appl->type]); | |
903 | ||
904 | ASSERT_RTNL(); | |
905 | ||
906 | RCU_INIT_POINTER(port->applicants[appl->type], NULL); | |
907 | ||
ab037780 SS |
908 | spin_lock_bh(&app->lock); |
909 | app->active = false; | |
910 | spin_unlock_bh(&app->lock); | |
febf018d DW |
911 | /* Delete timer and generate a final TX event to flush out |
912 | * all pending messages before the applicant is gone. | |
913 | */ | |
914 | del_timer_sync(&app->join_timer); | |
9fe34f5d | 915 | del_timer_sync(&app->periodic_timer); |
fb745e9a | 916 | |
faff57a9 | 917 | spin_lock_bh(&app->lock); |
febf018d | 918 | mrp_mad_event(app, MRP_EVENT_TX); |
996af621 | 919 | mrp_attr_destroy_all(app); |
febf018d | 920 | mrp_pdu_queue(app); |
faff57a9 | 921 | spin_unlock_bh(&app->lock); |
fb745e9a | 922 | |
febf018d DW |
923 | mrp_queue_xmit(app); |
924 | ||
925 | dev_mc_del(dev, appl->group_address); | |
926 | kfree_rcu(app, rcu); | |
927 | mrp_release_port(dev); | |
928 | } | |
929 | EXPORT_SYMBOL_GPL(mrp_uninit_applicant); | |
930 | ||
931 | int mrp_register_application(struct mrp_application *appl) | |
932 | { | |
933 | appl->pkttype.func = mrp_rcv; | |
934 | dev_add_pack(&appl->pkttype); | |
935 | return 0; | |
936 | } | |
937 | EXPORT_SYMBOL_GPL(mrp_register_application); | |
938 | ||
939 | void mrp_unregister_application(struct mrp_application *appl) | |
940 | { | |
941 | dev_remove_pack(&appl->pkttype); | |
942 | } | |
943 | EXPORT_SYMBOL_GPL(mrp_unregister_application); |