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