]>
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 | ||
295 | static int mrp_pdu_init(struct mrp_applicant *app) | |
296 | { | |
297 | struct sk_buff *skb; | |
298 | struct mrp_pdu_hdr *ph; | |
299 | ||
300 | skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev), | |
301 | GFP_ATOMIC); | |
302 | if (!skb) | |
303 | return -ENOMEM; | |
304 | ||
305 | skb->dev = app->dev; | |
306 | skb->protocol = app->app->pkttype.type; | |
307 | skb_reserve(skb, LL_RESERVED_SPACE(app->dev)); | |
308 | skb_reset_network_header(skb); | |
309 | skb_reset_transport_header(skb); | |
310 | ||
4df864c1 | 311 | ph = __skb_put(skb, sizeof(*ph)); |
febf018d DW |
312 | ph->version = app->app->version; |
313 | ||
314 | app->pdu = skb; | |
315 | return 0; | |
316 | } | |
317 | ||
318 | static int mrp_pdu_append_end_mark(struct mrp_applicant *app) | |
319 | { | |
320 | __be16 *endmark; | |
321 | ||
322 | if (skb_tailroom(app->pdu) < sizeof(*endmark)) | |
323 | return -1; | |
4df864c1 | 324 | endmark = __skb_put(app->pdu, sizeof(*endmark)); |
febf018d DW |
325 | put_unaligned(MRP_END_MARK, endmark); |
326 | return 0; | |
327 | } | |
328 | ||
329 | static void mrp_pdu_queue(struct mrp_applicant *app) | |
330 | { | |
331 | if (!app->pdu) | |
332 | return; | |
333 | ||
334 | if (mrp_cb(app->pdu)->mh) | |
335 | mrp_pdu_append_end_mark(app); | |
336 | mrp_pdu_append_end_mark(app); | |
337 | ||
338 | dev_hard_header(app->pdu, app->dev, ntohs(app->app->pkttype.type), | |
339 | app->app->group_address, app->dev->dev_addr, | |
340 | app->pdu->len); | |
341 | ||
342 | skb_queue_tail(&app->queue, app->pdu); | |
343 | app->pdu = NULL; | |
344 | } | |
345 | ||
346 | static void mrp_queue_xmit(struct mrp_applicant *app) | |
347 | { | |
348 | struct sk_buff *skb; | |
349 | ||
350 | while ((skb = skb_dequeue(&app->queue))) | |
351 | dev_queue_xmit(skb); | |
352 | } | |
353 | ||
354 | static int mrp_pdu_append_msg_hdr(struct mrp_applicant *app, | |
355 | u8 attrtype, u8 attrlen) | |
356 | { | |
357 | struct mrp_msg_hdr *mh; | |
358 | ||
359 | if (mrp_cb(app->pdu)->mh) { | |
360 | if (mrp_pdu_append_end_mark(app) < 0) | |
361 | return -1; | |
362 | mrp_cb(app->pdu)->mh = NULL; | |
363 | mrp_cb(app->pdu)->vah = NULL; | |
364 | } | |
365 | ||
366 | if (skb_tailroom(app->pdu) < sizeof(*mh)) | |
367 | return -1; | |
4df864c1 | 368 | mh = __skb_put(app->pdu, sizeof(*mh)); |
febf018d DW |
369 | mh->attrtype = attrtype; |
370 | mh->attrlen = attrlen; | |
371 | mrp_cb(app->pdu)->mh = mh; | |
372 | return 0; | |
373 | } | |
374 | ||
375 | static int mrp_pdu_append_vecattr_hdr(struct mrp_applicant *app, | |
376 | const void *firstattrvalue, u8 attrlen) | |
377 | { | |
378 | struct mrp_vecattr_hdr *vah; | |
379 | ||
380 | if (skb_tailroom(app->pdu) < sizeof(*vah) + attrlen) | |
381 | return -1; | |
4df864c1 | 382 | vah = __skb_put(app->pdu, sizeof(*vah) + attrlen); |
febf018d DW |
383 | put_unaligned(0, &vah->lenflags); |
384 | memcpy(vah->firstattrvalue, firstattrvalue, attrlen); | |
385 | mrp_cb(app->pdu)->vah = vah; | |
386 | memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen); | |
387 | return 0; | |
388 | } | |
389 | ||
390 | static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app, | |
391 | const struct mrp_attr *attr, | |
392 | enum mrp_vecattr_event vaevent) | |
393 | { | |
394 | u16 len, pos; | |
395 | u8 *vaevents; | |
396 | int err; | |
397 | again: | |
398 | if (!app->pdu) { | |
399 | err = mrp_pdu_init(app); | |
400 | if (err < 0) | |
401 | return err; | |
402 | } | |
403 | ||
404 | /* If there is no Message header in the PDU, or the Message header is | |
405 | * for a different attribute type, add an EndMark (if necessary) and a | |
406 | * new Message header to the PDU. | |
407 | */ | |
408 | if (!mrp_cb(app->pdu)->mh || | |
409 | mrp_cb(app->pdu)->mh->attrtype != attr->type || | |
410 | mrp_cb(app->pdu)->mh->attrlen != attr->len) { | |
411 | if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0) | |
412 | goto queue; | |
413 | } | |
414 | ||
415 | /* If there is no VectorAttribute header for this Message in the PDU, | |
416 | * or this attribute's value does not sequentially follow the previous | |
417 | * attribute's value, add a new VectorAttribute header to the PDU. | |
418 | */ | |
419 | if (!mrp_cb(app->pdu)->vah || | |
420 | memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) { | |
421 | if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0) | |
422 | goto queue; | |
423 | } | |
424 | ||
425 | len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags)); | |
426 | pos = len % 3; | |
427 | ||
428 | /* Events are packed into Vectors in the PDU, three to a byte. Add a | |
429 | * byte to the end of the Vector if necessary. | |
430 | */ | |
431 | if (!pos) { | |
432 | if (skb_tailroom(app->pdu) < sizeof(u8)) | |
433 | goto queue; | |
4df864c1 | 434 | vaevents = __skb_put(app->pdu, sizeof(u8)); |
febf018d DW |
435 | } else { |
436 | vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8)); | |
437 | } | |
438 | ||
439 | switch (pos) { | |
440 | case 0: | |
441 | *vaevents = vaevent * (__MRP_VECATTR_EVENT_MAX * | |
442 | __MRP_VECATTR_EVENT_MAX); | |
443 | break; | |
444 | case 1: | |
445 | *vaevents += vaevent * __MRP_VECATTR_EVENT_MAX; | |
446 | break; | |
447 | case 2: | |
448 | *vaevents += vaevent; | |
449 | break; | |
450 | default: | |
451 | WARN_ON(1); | |
452 | } | |
453 | ||
454 | /* Increment the length of the VectorAttribute in the PDU, as well as | |
455 | * the value of the next attribute that would continue its Vector. | |
456 | */ | |
457 | put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags); | |
458 | mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len); | |
459 | ||
460 | return 0; | |
461 | ||
462 | queue: | |
463 | mrp_pdu_queue(app); | |
464 | goto again; | |
465 | } | |
466 | ||
467 | static void mrp_attr_event(struct mrp_applicant *app, | |
468 | struct mrp_attr *attr, enum mrp_event event) | |
469 | { | |
470 | enum mrp_applicant_state state; | |
471 | ||
472 | state = mrp_applicant_state_table[attr->state][event]; | |
473 | if (state == MRP_APPLICANT_INVALID) { | |
474 | WARN_ON(1); | |
475 | return; | |
476 | } | |
477 | ||
478 | if (event == MRP_EVENT_TX) { | |
479 | /* When appending the attribute fails, don't update its state | |
480 | * in order to retry at the next TX event. | |
481 | */ | |
482 | ||
483 | switch (mrp_tx_action_table[attr->state]) { | |
484 | case MRP_TX_ACTION_NONE: | |
485 | case MRP_TX_ACTION_S_JOIN_IN_OPTIONAL: | |
486 | case MRP_TX_ACTION_S_IN_OPTIONAL: | |
487 | break; | |
488 | case MRP_TX_ACTION_S_NEW: | |
489 | if (mrp_pdu_append_vecattr_event( | |
490 | app, attr, MRP_VECATTR_EVENT_NEW) < 0) | |
491 | return; | |
492 | break; | |
493 | case MRP_TX_ACTION_S_JOIN_IN: | |
494 | if (mrp_pdu_append_vecattr_event( | |
495 | app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0) | |
496 | return; | |
497 | break; | |
498 | case MRP_TX_ACTION_S_LV: | |
499 | if (mrp_pdu_append_vecattr_event( | |
500 | app, attr, MRP_VECATTR_EVENT_LV) < 0) | |
501 | return; | |
502 | /* As a pure applicant, sending a leave message | |
503 | * implies that the attribute was unregistered and | |
504 | * can be destroyed. | |
505 | */ | |
506 | mrp_attr_destroy(app, attr); | |
507 | return; | |
508 | default: | |
509 | WARN_ON(1); | |
510 | } | |
511 | } | |
512 | ||
513 | attr->state = state; | |
514 | } | |
515 | ||
516 | int mrp_request_join(const struct net_device *dev, | |
517 | const struct mrp_application *appl, | |
518 | const void *value, u8 len, u8 type) | |
519 | { | |
520 | struct mrp_port *port = rtnl_dereference(dev->mrp_port); | |
521 | struct mrp_applicant *app = rtnl_dereference( | |
522 | port->applicants[appl->type]); | |
523 | struct mrp_attr *attr; | |
524 | ||
525 | if (sizeof(struct mrp_skb_cb) + len > | |
526 | FIELD_SIZEOF(struct sk_buff, cb)) | |
527 | return -ENOMEM; | |
528 | ||
529 | spin_lock_bh(&app->lock); | |
530 | attr = mrp_attr_create(app, value, len, type); | |
531 | if (!attr) { | |
532 | spin_unlock_bh(&app->lock); | |
533 | return -ENOMEM; | |
534 | } | |
535 | mrp_attr_event(app, attr, MRP_EVENT_JOIN); | |
536 | spin_unlock_bh(&app->lock); | |
537 | return 0; | |
538 | } | |
539 | EXPORT_SYMBOL_GPL(mrp_request_join); | |
540 | ||
541 | void mrp_request_leave(const struct net_device *dev, | |
542 | const struct mrp_application *appl, | |
543 | const void *value, u8 len, u8 type) | |
544 | { | |
545 | struct mrp_port *port = rtnl_dereference(dev->mrp_port); | |
546 | struct mrp_applicant *app = rtnl_dereference( | |
547 | port->applicants[appl->type]); | |
548 | struct mrp_attr *attr; | |
549 | ||
550 | if (sizeof(struct mrp_skb_cb) + len > | |
551 | FIELD_SIZEOF(struct sk_buff, cb)) | |
552 | return; | |
553 | ||
554 | spin_lock_bh(&app->lock); | |
555 | attr = mrp_attr_lookup(app, value, len, type); | |
556 | if (!attr) { | |
557 | spin_unlock_bh(&app->lock); | |
558 | return; | |
559 | } | |
560 | mrp_attr_event(app, attr, MRP_EVENT_LV); | |
561 | spin_unlock_bh(&app->lock); | |
562 | } | |
563 | EXPORT_SYMBOL_GPL(mrp_request_leave); | |
564 | ||
565 | static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event) | |
566 | { | |
567 | struct rb_node *node, *next; | |
568 | struct mrp_attr *attr; | |
569 | ||
570 | for (node = rb_first(&app->mad); | |
571 | next = node ? rb_next(node) : NULL, node != NULL; | |
572 | node = next) { | |
573 | attr = rb_entry(node, struct mrp_attr, node); | |
574 | mrp_attr_event(app, attr, event); | |
575 | } | |
576 | } | |
577 | ||
578 | static void mrp_join_timer_arm(struct mrp_applicant *app) | |
579 | { | |
580 | unsigned long delay; | |
581 | ||
63862b5b | 582 | delay = (u64)msecs_to_jiffies(mrp_join_time) * prandom_u32() >> 32; |
febf018d DW |
583 | mod_timer(&app->join_timer, jiffies + delay); |
584 | } | |
585 | ||
e99e88a9 | 586 | static void mrp_join_timer(struct timer_list *t) |
febf018d | 587 | { |
e99e88a9 | 588 | struct mrp_applicant *app = from_timer(app, t, join_timer); |
febf018d DW |
589 | |
590 | spin_lock(&app->lock); | |
591 | mrp_mad_event(app, MRP_EVENT_TX); | |
592 | mrp_pdu_queue(app); | |
593 | spin_unlock(&app->lock); | |
594 | ||
595 | mrp_queue_xmit(app); | |
596 | mrp_join_timer_arm(app); | |
597 | } | |
598 | ||
9fe34f5d NBK |
599 | static void mrp_periodic_timer_arm(struct mrp_applicant *app) |
600 | { | |
601 | mod_timer(&app->periodic_timer, | |
602 | jiffies + msecs_to_jiffies(mrp_periodic_time)); | |
603 | } | |
604 | ||
e99e88a9 | 605 | static void mrp_periodic_timer(struct timer_list *t) |
9fe34f5d | 606 | { |
e99e88a9 | 607 | struct mrp_applicant *app = from_timer(app, t, periodic_timer); |
9fe34f5d NBK |
608 | |
609 | spin_lock(&app->lock); | |
610 | mrp_mad_event(app, MRP_EVENT_PERIODIC); | |
611 | mrp_pdu_queue(app); | |
612 | spin_unlock(&app->lock); | |
613 | ||
614 | mrp_periodic_timer_arm(app); | |
615 | } | |
616 | ||
febf018d DW |
617 | static int mrp_pdu_parse_end_mark(struct sk_buff *skb, int *offset) |
618 | { | |
619 | __be16 endmark; | |
620 | ||
621 | if (skb_copy_bits(skb, *offset, &endmark, sizeof(endmark)) < 0) | |
622 | return -1; | |
623 | if (endmark == MRP_END_MARK) { | |
624 | *offset += sizeof(endmark); | |
625 | return -1; | |
626 | } | |
627 | return 0; | |
628 | } | |
629 | ||
630 | static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app, | |
631 | struct sk_buff *skb, | |
632 | enum mrp_vecattr_event vaevent) | |
633 | { | |
634 | struct mrp_attr *attr; | |
635 | enum mrp_event event; | |
636 | ||
637 | attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue, | |
638 | mrp_cb(skb)->mh->attrlen, | |
639 | mrp_cb(skb)->mh->attrtype); | |
640 | if (attr == NULL) | |
641 | return; | |
642 | ||
643 | switch (vaevent) { | |
644 | case MRP_VECATTR_EVENT_NEW: | |
645 | event = MRP_EVENT_R_NEW; | |
646 | break; | |
647 | case MRP_VECATTR_EVENT_JOIN_IN: | |
648 | event = MRP_EVENT_R_JOIN_IN; | |
649 | break; | |
650 | case MRP_VECATTR_EVENT_IN: | |
651 | event = MRP_EVENT_R_IN; | |
652 | break; | |
653 | case MRP_VECATTR_EVENT_JOIN_MT: | |
654 | event = MRP_EVENT_R_JOIN_MT; | |
655 | break; | |
656 | case MRP_VECATTR_EVENT_MT: | |
657 | event = MRP_EVENT_R_MT; | |
658 | break; | |
659 | case MRP_VECATTR_EVENT_LV: | |
660 | event = MRP_EVENT_R_LV; | |
661 | break; | |
662 | default: | |
663 | return; | |
664 | } | |
665 | ||
666 | mrp_attr_event(app, attr, event); | |
667 | } | |
668 | ||
669 | static int mrp_pdu_parse_vecattr(struct mrp_applicant *app, | |
670 | struct sk_buff *skb, int *offset) | |
671 | { | |
672 | struct mrp_vecattr_hdr _vah; | |
673 | u16 valen; | |
674 | u8 vaevents, vaevent; | |
675 | ||
676 | mrp_cb(skb)->vah = skb_header_pointer(skb, *offset, sizeof(_vah), | |
677 | &_vah); | |
678 | if (!mrp_cb(skb)->vah) | |
679 | return -1; | |
680 | *offset += sizeof(_vah); | |
681 | ||
682 | if (get_unaligned(&mrp_cb(skb)->vah->lenflags) & | |
683 | MRP_VECATTR_HDR_FLAG_LA) | |
684 | mrp_mad_event(app, MRP_EVENT_R_LA); | |
685 | valen = be16_to_cpu(get_unaligned(&mrp_cb(skb)->vah->lenflags) & | |
686 | MRP_VECATTR_HDR_LEN_MASK); | |
687 | ||
688 | /* The VectorAttribute structure in a PDU carries event information | |
689 | * about one or more attributes having consecutive values. Only the | |
690 | * value for the first attribute is contained in the structure. So | |
691 | * we make a copy of that value, and then increment it each time we | |
692 | * advance to the next event in its Vector. | |
693 | */ | |
694 | if (sizeof(struct mrp_skb_cb) + mrp_cb(skb)->mh->attrlen > | |
695 | FIELD_SIZEOF(struct sk_buff, cb)) | |
696 | return -1; | |
697 | if (skb_copy_bits(skb, *offset, mrp_cb(skb)->attrvalue, | |
698 | mrp_cb(skb)->mh->attrlen) < 0) | |
699 | return -1; | |
700 | *offset += mrp_cb(skb)->mh->attrlen; | |
701 | ||
702 | /* In a VectorAttribute, the Vector contains events which are packed | |
703 | * three to a byte. We process one byte of the Vector at a time. | |
704 | */ | |
705 | while (valen > 0) { | |
706 | if (skb_copy_bits(skb, *offset, &vaevents, | |
707 | sizeof(vaevents)) < 0) | |
708 | return -1; | |
709 | *offset += sizeof(vaevents); | |
710 | ||
711 | /* Extract and process the first event. */ | |
712 | vaevent = vaevents / (__MRP_VECATTR_EVENT_MAX * | |
713 | __MRP_VECATTR_EVENT_MAX); | |
714 | if (vaevent >= __MRP_VECATTR_EVENT_MAX) { | |
715 | /* The byte is malformed; stop processing. */ | |
716 | return -1; | |
717 | } | |
718 | mrp_pdu_parse_vecattr_event(app, skb, vaevent); | |
719 | ||
720 | /* If present, extract and process the second event. */ | |
721 | if (!--valen) | |
722 | break; | |
723 | mrp_attrvalue_inc(mrp_cb(skb)->attrvalue, | |
724 | mrp_cb(skb)->mh->attrlen); | |
725 | vaevents %= (__MRP_VECATTR_EVENT_MAX * | |
726 | __MRP_VECATTR_EVENT_MAX); | |
727 | vaevent = vaevents / __MRP_VECATTR_EVENT_MAX; | |
728 | mrp_pdu_parse_vecattr_event(app, skb, vaevent); | |
729 | ||
730 | /* If present, extract and process the third event. */ | |
731 | if (!--valen) | |
732 | break; | |
733 | mrp_attrvalue_inc(mrp_cb(skb)->attrvalue, | |
734 | mrp_cb(skb)->mh->attrlen); | |
735 | vaevents %= __MRP_VECATTR_EVENT_MAX; | |
736 | vaevent = vaevents; | |
737 | mrp_pdu_parse_vecattr_event(app, skb, vaevent); | |
738 | } | |
739 | return 0; | |
740 | } | |
741 | ||
742 | static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb, | |
743 | int *offset) | |
744 | { | |
745 | struct mrp_msg_hdr _mh; | |
746 | ||
747 | mrp_cb(skb)->mh = skb_header_pointer(skb, *offset, sizeof(_mh), &_mh); | |
748 | if (!mrp_cb(skb)->mh) | |
749 | return -1; | |
750 | *offset += sizeof(_mh); | |
751 | ||
752 | if (mrp_cb(skb)->mh->attrtype == 0 || | |
753 | mrp_cb(skb)->mh->attrtype > app->app->maxattr || | |
754 | mrp_cb(skb)->mh->attrlen == 0) | |
755 | return -1; | |
756 | ||
757 | while (skb->len > *offset) { | |
758 | if (mrp_pdu_parse_end_mark(skb, offset) < 0) | |
759 | break; | |
760 | if (mrp_pdu_parse_vecattr(app, skb, offset) < 0) | |
761 | return -1; | |
762 | } | |
763 | return 0; | |
764 | } | |
765 | ||
7e307c67 SH |
766 | static int mrp_rcv(struct sk_buff *skb, struct net_device *dev, |
767 | struct packet_type *pt, struct net_device *orig_dev) | |
febf018d DW |
768 | { |
769 | struct mrp_application *appl = container_of(pt, struct mrp_application, | |
770 | pkttype); | |
771 | struct mrp_port *port; | |
772 | struct mrp_applicant *app; | |
773 | struct mrp_pdu_hdr _ph; | |
774 | const struct mrp_pdu_hdr *ph; | |
775 | int offset = skb_network_offset(skb); | |
776 | ||
777 | /* If the interface is in promiscuous mode, drop the packet if | |
778 | * it was unicast to another host. | |
779 | */ | |
780 | if (unlikely(skb->pkt_type == PACKET_OTHERHOST)) | |
781 | goto out; | |
782 | skb = skb_share_check(skb, GFP_ATOMIC); | |
783 | if (unlikely(!skb)) | |
784 | goto out; | |
785 | port = rcu_dereference(dev->mrp_port); | |
786 | if (unlikely(!port)) | |
787 | goto out; | |
788 | app = rcu_dereference(port->applicants[appl->type]); | |
789 | if (unlikely(!app)) | |
790 | goto out; | |
791 | ||
792 | ph = skb_header_pointer(skb, offset, sizeof(_ph), &_ph); | |
793 | if (!ph) | |
794 | goto out; | |
795 | offset += sizeof(_ph); | |
796 | ||
797 | if (ph->version != app->app->version) | |
798 | goto out; | |
799 | ||
800 | spin_lock(&app->lock); | |
801 | while (skb->len > offset) { | |
802 | if (mrp_pdu_parse_end_mark(skb, &offset) < 0) | |
803 | break; | |
804 | if (mrp_pdu_parse_msg(app, skb, &offset) < 0) | |
805 | break; | |
806 | } | |
807 | spin_unlock(&app->lock); | |
808 | out: | |
809 | kfree_skb(skb); | |
810 | return 0; | |
811 | } | |
812 | ||
813 | static int mrp_init_port(struct net_device *dev) | |
814 | { | |
815 | struct mrp_port *port; | |
816 | ||
817 | port = kzalloc(sizeof(*port), GFP_KERNEL); | |
818 | if (!port) | |
819 | return -ENOMEM; | |
820 | rcu_assign_pointer(dev->mrp_port, port); | |
821 | return 0; | |
822 | } | |
823 | ||
824 | static void mrp_release_port(struct net_device *dev) | |
825 | { | |
826 | struct mrp_port *port = rtnl_dereference(dev->mrp_port); | |
827 | unsigned int i; | |
828 | ||
829 | for (i = 0; i <= MRP_APPLICATION_MAX; i++) { | |
830 | if (rtnl_dereference(port->applicants[i])) | |
831 | return; | |
832 | } | |
833 | RCU_INIT_POINTER(dev->mrp_port, NULL); | |
834 | kfree_rcu(port, rcu); | |
835 | } | |
836 | ||
837 | int mrp_init_applicant(struct net_device *dev, struct mrp_application *appl) | |
838 | { | |
839 | struct mrp_applicant *app; | |
840 | int err; | |
841 | ||
842 | ASSERT_RTNL(); | |
843 | ||
844 | if (!rtnl_dereference(dev->mrp_port)) { | |
845 | err = mrp_init_port(dev); | |
846 | if (err < 0) | |
847 | goto err1; | |
848 | } | |
849 | ||
850 | err = -ENOMEM; | |
851 | app = kzalloc(sizeof(*app), GFP_KERNEL); | |
852 | if (!app) | |
853 | goto err2; | |
854 | ||
855 | err = dev_mc_add(dev, appl->group_address); | |
856 | if (err < 0) | |
857 | goto err3; | |
858 | ||
859 | app->dev = dev; | |
860 | app->app = appl; | |
861 | app->mad = RB_ROOT; | |
862 | spin_lock_init(&app->lock); | |
863 | skb_queue_head_init(&app->queue); | |
864 | rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app); | |
e99e88a9 | 865 | timer_setup(&app->join_timer, mrp_join_timer, 0); |
febf018d | 866 | mrp_join_timer_arm(app); |
e99e88a9 | 867 | timer_setup(&app->periodic_timer, mrp_periodic_timer, 0); |
9fe34f5d | 868 | mrp_periodic_timer_arm(app); |
febf018d DW |
869 | return 0; |
870 | ||
871 | err3: | |
872 | kfree(app); | |
873 | err2: | |
874 | mrp_release_port(dev); | |
875 | err1: | |
876 | return err; | |
877 | } | |
878 | EXPORT_SYMBOL_GPL(mrp_init_applicant); | |
879 | ||
880 | void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl) | |
881 | { | |
882 | struct mrp_port *port = rtnl_dereference(dev->mrp_port); | |
883 | struct mrp_applicant *app = rtnl_dereference( | |
884 | port->applicants[appl->type]); | |
885 | ||
886 | ASSERT_RTNL(); | |
887 | ||
888 | RCU_INIT_POINTER(port->applicants[appl->type], NULL); | |
889 | ||
890 | /* Delete timer and generate a final TX event to flush out | |
891 | * all pending messages before the applicant is gone. | |
892 | */ | |
893 | del_timer_sync(&app->join_timer); | |
9fe34f5d | 894 | del_timer_sync(&app->periodic_timer); |
fb745e9a | 895 | |
faff57a9 | 896 | spin_lock_bh(&app->lock); |
febf018d DW |
897 | mrp_mad_event(app, MRP_EVENT_TX); |
898 | mrp_pdu_queue(app); | |
faff57a9 | 899 | spin_unlock_bh(&app->lock); |
fb745e9a | 900 | |
febf018d DW |
901 | mrp_queue_xmit(app); |
902 | ||
903 | dev_mc_del(dev, appl->group_address); | |
904 | kfree_rcu(app, rcu); | |
905 | mrp_release_port(dev); | |
906 | } | |
907 | EXPORT_SYMBOL_GPL(mrp_uninit_applicant); | |
908 | ||
909 | int mrp_register_application(struct mrp_application *appl) | |
910 | { | |
911 | appl->pkttype.func = mrp_rcv; | |
912 | dev_add_pack(&appl->pkttype); | |
913 | return 0; | |
914 | } | |
915 | EXPORT_SYMBOL_GPL(mrp_register_application); | |
916 | ||
917 | void mrp_unregister_application(struct mrp_application *appl) | |
918 | { | |
919 | dev_remove_pack(&appl->pkttype); | |
920 | } | |
921 | EXPORT_SYMBOL_GPL(mrp_unregister_application); |