1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | |
3 | #include <linux/cfm_bridge.h> |
4 | #include <uapi/linux/cfm_bridge.h> |
5 | #include "br_private_cfm.h" |
6 | |
7 | static struct br_cfm_mep *br_mep_find(struct net_bridge *br, u32 instance) |
8 | { |
9 | struct br_cfm_mep *mep; |
10 | |
11 | hlist_for_each_entry(mep, &br->mep_list, head) |
12 | if (mep->instance == instance) |
13 | return mep; |
14 | |
15 | return NULL; |
16 | } |
17 | |
18 | static struct br_cfm_mep *br_mep_find_ifindex(struct net_bridge *br, |
19 | u32 ifindex) |
20 | { |
21 | struct br_cfm_mep *mep; |
22 | |
23 | hlist_for_each_entry_rcu(mep, &br->mep_list, head, |
24 | lockdep_rtnl_is_held()) |
25 | if (mep->create.ifindex == ifindex) |
26 | return mep; |
27 | |
28 | return NULL; |
29 | } |
30 | |
31 | static struct br_cfm_peer_mep *br_peer_mep_find(struct br_cfm_mep *mep, |
32 | u32 mepid) |
33 | { |
34 | struct br_cfm_peer_mep *peer_mep; |
35 | |
36 | hlist_for_each_entry_rcu(peer_mep, &mep->peer_mep_list, head, |
37 | lockdep_rtnl_is_held()) |
38 | if (peer_mep->mepid == mepid) |
39 | return peer_mep; |
40 | |
41 | return NULL; |
42 | } |
43 | |
44 | static struct net_bridge_port *br_mep_get_port(struct net_bridge *br, |
45 | u32 ifindex) |
46 | { |
47 | struct net_bridge_port *port; |
48 | |
49 | list_for_each_entry(port, &br->port_list, list) |
50 | if (port->dev->ifindex == ifindex) |
51 | return port; |
52 | |
53 | return NULL; |
54 | } |
55 | |
56 | /* Calculate the CCM interval in us. */ |
57 | static u32 interval_to_us(enum br_cfm_ccm_interval interval) |
58 | { |
59 | switch (interval) { |
60 | case BR_CFM_CCM_INTERVAL_NONE: |
61 | return 0; |
62 | case BR_CFM_CCM_INTERVAL_3_3_MS: |
63 | return 3300; |
64 | case BR_CFM_CCM_INTERVAL_10_MS: |
65 | return 10 * 1000; |
66 | case BR_CFM_CCM_INTERVAL_100_MS: |
67 | return 100 * 1000; |
68 | case BR_CFM_CCM_INTERVAL_1_SEC: |
69 | return 1000 * 1000; |
70 | case BR_CFM_CCM_INTERVAL_10_SEC: |
71 | return 10 * 1000 * 1000; |
72 | case BR_CFM_CCM_INTERVAL_1_MIN: |
73 | return 60 * 1000 * 1000; |
74 | case BR_CFM_CCM_INTERVAL_10_MIN: |
75 | return 10 * 60 * 1000 * 1000; |
76 | } |
77 | return 0; |
78 | } |
79 | |
80 | /* Convert the interface interval to CCM PDU value. */ |
81 | static u32 interval_to_pdu(enum br_cfm_ccm_interval interval) |
82 | { |
83 | switch (interval) { |
84 | case BR_CFM_CCM_INTERVAL_NONE: |
85 | return 0; |
86 | case BR_CFM_CCM_INTERVAL_3_3_MS: |
87 | return 1; |
88 | case BR_CFM_CCM_INTERVAL_10_MS: |
89 | return 2; |
90 | case BR_CFM_CCM_INTERVAL_100_MS: |
91 | return 3; |
92 | case BR_CFM_CCM_INTERVAL_1_SEC: |
93 | return 4; |
94 | case BR_CFM_CCM_INTERVAL_10_SEC: |
95 | return 5; |
96 | case BR_CFM_CCM_INTERVAL_1_MIN: |
97 | return 6; |
98 | case BR_CFM_CCM_INTERVAL_10_MIN: |
99 | return 7; |
100 | } |
101 | return 0; |
102 | } |
103 | |
104 | /* Convert the CCM PDU value to interval on interface. */ |
105 | static u32 pdu_to_interval(u32 value) |
106 | { |
107 | switch (value) { |
108 | case 0: |
109 | return BR_CFM_CCM_INTERVAL_NONE; |
110 | case 1: |
111 | return BR_CFM_CCM_INTERVAL_3_3_MS; |
112 | case 2: |
113 | return BR_CFM_CCM_INTERVAL_10_MS; |
114 | case 3: |
115 | return BR_CFM_CCM_INTERVAL_100_MS; |
116 | case 4: |
117 | return BR_CFM_CCM_INTERVAL_1_SEC; |
118 | case 5: |
119 | return BR_CFM_CCM_INTERVAL_10_SEC; |
120 | case 6: |
121 | return BR_CFM_CCM_INTERVAL_1_MIN; |
122 | case 7: |
123 | return BR_CFM_CCM_INTERVAL_10_MIN; |
124 | } |
125 | return BR_CFM_CCM_INTERVAL_NONE; |
126 | } |
127 | |
128 | static void ccm_rx_timer_start(struct br_cfm_peer_mep *peer_mep) |
129 | { |
130 | u32 interval_us; |
131 | |
132 | interval_us = interval_to_us(interval: peer_mep->mep->cc_config.exp_interval); |
133 | /* Function ccm_rx_dwork must be called with 1/4 |
134 | * of the configured CC 'expected_interval' |
135 | * in order to detect CCM defect after 3.25 interval. |
136 | */ |
137 | queue_delayed_work(wq: system_wq, dwork: &peer_mep->ccm_rx_dwork, |
138 | delay: usecs_to_jiffies(u: interval_us / 4)); |
139 | } |
140 | |
141 | static void br_cfm_notify(int event, const struct net_bridge_port *port) |
142 | { |
143 | u32 filter = RTEXT_FILTER_CFM_STATUS; |
144 | |
145 | br_info_notify(event, br: port->br, NULL, filter); |
146 | } |
147 | |
148 | static void cc_peer_enable(struct br_cfm_peer_mep *peer_mep) |
149 | { |
150 | memset(&peer_mep->cc_status, 0, sizeof(peer_mep->cc_status)); |
151 | peer_mep->ccm_rx_count_miss = 0; |
152 | |
153 | ccm_rx_timer_start(peer_mep); |
154 | } |
155 | |
156 | static void cc_peer_disable(struct br_cfm_peer_mep *peer_mep) |
157 | { |
158 | cancel_delayed_work_sync(dwork: &peer_mep->ccm_rx_dwork); |
159 | } |
160 | |
161 | static struct sk_buff *ccm_frame_build(struct br_cfm_mep *mep, |
162 | const struct br_cfm_cc_ccm_tx_info *const tx_info) |
163 | |
164 | { |
165 | struct br_cfm_common_hdr *common_hdr; |
166 | struct net_bridge_port *b_port; |
167 | struct br_cfm_maid *maid; |
168 | u8 *itu_reserved, *e_tlv; |
169 | struct ethhdr *eth_hdr; |
170 | struct sk_buff *skb; |
171 | __be32 *status_tlv; |
172 | __be32 *snumber; |
173 | __be16 *mepid; |
174 | |
175 | skb = dev_alloc_skb(CFM_CCM_MAX_FRAME_LENGTH); |
176 | if (!skb) |
177 | return NULL; |
178 | |
179 | rcu_read_lock(); |
180 | b_port = rcu_dereference(mep->b_port); |
181 | if (!b_port) { |
182 | kfree_skb(skb); |
183 | rcu_read_unlock(); |
184 | return NULL; |
185 | } |
186 | skb->dev = b_port->dev; |
187 | rcu_read_unlock(); |
188 | /* The device cannot be deleted until the work_queue functions has |
189 | * completed. This function is called from ccm_tx_work_expired() |
190 | * that is a work_queue functions. |
191 | */ |
192 | |
193 | skb->protocol = htons(ETH_P_CFM); |
194 | skb->priority = CFM_FRAME_PRIO; |
195 | |
196 | /* Ethernet header */ |
197 | eth_hdr = skb_put(skb, len: sizeof(*eth_hdr)); |
198 | ether_addr_copy(dst: eth_hdr->h_dest, src: tx_info->dmac.addr); |
199 | ether_addr_copy(dst: eth_hdr->h_source, src: mep->config.unicast_mac.addr); |
200 | eth_hdr->h_proto = htons(ETH_P_CFM); |
201 | |
202 | /* Common CFM Header */ |
203 | common_hdr = skb_put(skb, len: sizeof(*common_hdr)); |
204 | common_hdr->mdlevel_version = mep->config.mdlevel << 5; |
205 | common_hdr->opcode = BR_CFM_OPCODE_CCM; |
206 | common_hdr->flags = (mep->rdi << 7) | |
207 | interval_to_pdu(interval: mep->cc_config.exp_interval); |
208 | common_hdr->tlv_offset = CFM_CCM_TLV_OFFSET; |
209 | |
210 | /* Sequence number */ |
211 | snumber = skb_put(skb, len: sizeof(*snumber)); |
212 | if (tx_info->seq_no_update) { |
213 | *snumber = cpu_to_be32(mep->ccm_tx_snumber); |
214 | mep->ccm_tx_snumber += 1; |
215 | } else { |
216 | *snumber = 0; |
217 | } |
218 | |
219 | mepid = skb_put(skb, len: sizeof(*mepid)); |
220 | *mepid = cpu_to_be16((u16)mep->config.mepid); |
221 | |
222 | maid = skb_put(skb, len: sizeof(*maid)); |
223 | memcpy(maid->data, mep->cc_config.exp_maid.data, sizeof(maid->data)); |
224 | |
225 | /* ITU reserved (CFM_CCM_ITU_RESERVED_SIZE octets) */ |
226 | itu_reserved = skb_put(skb, CFM_CCM_ITU_RESERVED_SIZE); |
227 | memset(itu_reserved, 0, CFM_CCM_ITU_RESERVED_SIZE); |
228 | |
229 | /* Generel CFM TLV format: |
230 | * TLV type: one byte |
231 | * TLV value length: two bytes |
232 | * TLV value: 'TLV value length' bytes |
233 | */ |
234 | |
235 | /* Port status TLV. The value length is 1. Total of 4 bytes. */ |
236 | if (tx_info->port_tlv) { |
237 | status_tlv = skb_put(skb, len: sizeof(*status_tlv)); |
238 | *status_tlv = cpu_to_be32((CFM_PORT_STATUS_TLV_TYPE << 24) | |
239 | (1 << 8) | /* Value length */ |
240 | (tx_info->port_tlv_value & 0xFF)); |
241 | } |
242 | |
243 | /* Interface status TLV. The value length is 1. Total of 4 bytes. */ |
244 | if (tx_info->if_tlv) { |
245 | status_tlv = skb_put(skb, len: sizeof(*status_tlv)); |
246 | *status_tlv = cpu_to_be32((CFM_IF_STATUS_TLV_TYPE << 24) | |
247 | (1 << 8) | /* Value length */ |
248 | (tx_info->if_tlv_value & 0xFF)); |
249 | } |
250 | |
251 | /* End TLV */ |
252 | e_tlv = skb_put(skb, len: sizeof(*e_tlv)); |
253 | *e_tlv = CFM_ENDE_TLV_TYPE; |
254 | |
255 | return skb; |
256 | } |
257 | |
258 | static void ccm_frame_tx(struct sk_buff *skb) |
259 | { |
260 | skb_reset_network_header(skb); |
261 | dev_queue_xmit(skb); |
262 | } |
263 | |
264 | /* This function is called with the configured CC 'expected_interval' |
265 | * in order to drive CCM transmission when enabled. |
266 | */ |
267 | static void ccm_tx_work_expired(struct work_struct *work) |
268 | { |
269 | struct delayed_work *del_work; |
270 | struct br_cfm_mep *mep; |
271 | struct sk_buff *skb; |
272 | u32 interval_us; |
273 | |
274 | del_work = to_delayed_work(work); |
275 | mep = container_of(del_work, struct br_cfm_mep, ccm_tx_dwork); |
276 | |
277 | if (time_before_eq(mep->ccm_tx_end, jiffies)) { |
278 | /* Transmission period has ended */ |
279 | mep->cc_ccm_tx_info.period = 0; |
280 | return; |
281 | } |
282 | |
283 | skb = ccm_frame_build(mep, tx_info: &mep->cc_ccm_tx_info); |
284 | if (skb) |
285 | ccm_frame_tx(skb); |
286 | |
287 | interval_us = interval_to_us(interval: mep->cc_config.exp_interval); |
288 | queue_delayed_work(wq: system_wq, dwork: &mep->ccm_tx_dwork, |
289 | delay: usecs_to_jiffies(u: interval_us)); |
290 | } |
291 | |
292 | /* This function is called with 1/4 of the configured CC 'expected_interval' |
293 | * in order to detect CCM defect after 3.25 interval. |
294 | */ |
295 | static void ccm_rx_work_expired(struct work_struct *work) |
296 | { |
297 | struct br_cfm_peer_mep *peer_mep; |
298 | struct net_bridge_port *b_port; |
299 | struct delayed_work *del_work; |
300 | |
301 | del_work = to_delayed_work(work); |
302 | peer_mep = container_of(del_work, struct br_cfm_peer_mep, ccm_rx_dwork); |
303 | |
304 | /* After 13 counts (4 * 3,25) then 3.25 intervals are expired */ |
305 | if (peer_mep->ccm_rx_count_miss < 13) { |
306 | /* 3.25 intervals are NOT expired without CCM reception */ |
307 | peer_mep->ccm_rx_count_miss++; |
308 | |
309 | /* Start timer again */ |
310 | ccm_rx_timer_start(peer_mep); |
311 | } else { |
312 | /* 3.25 intervals are expired without CCM reception. |
313 | * CCM defect detected |
314 | */ |
315 | peer_mep->cc_status.ccm_defect = true; |
316 | |
317 | /* Change in CCM defect status - notify */ |
318 | rcu_read_lock(); |
319 | b_port = rcu_dereference(peer_mep->mep->b_port); |
320 | if (b_port) |
321 | br_cfm_notify(RTM_NEWLINK, port: b_port); |
322 | rcu_read_unlock(); |
323 | } |
324 | } |
325 | |
326 | static u32 (struct sk_buff *skb, u32 index, |
327 | struct br_cfm_peer_mep *peer_mep) |
328 | { |
329 | __be32 *s_tlv; |
330 | __be32 _s_tlv; |
331 | u32 h_s_tlv; |
332 | u8 *e_tlv; |
333 | u8 _e_tlv; |
334 | |
335 | e_tlv = skb_header_pointer(skb, offset: index, len: sizeof(_e_tlv), buffer: &_e_tlv); |
336 | if (!e_tlv) |
337 | return 0; |
338 | |
339 | /* TLV is present - get the status TLV */ |
340 | s_tlv = skb_header_pointer(skb, |
341 | offset: index, |
342 | len: sizeof(_s_tlv), buffer: &_s_tlv); |
343 | if (!s_tlv) |
344 | return 0; |
345 | |
346 | h_s_tlv = ntohl(*s_tlv); |
347 | if ((h_s_tlv >> 24) == CFM_IF_STATUS_TLV_TYPE) { |
348 | /* Interface status TLV */ |
349 | peer_mep->cc_status.tlv_seen = true; |
350 | peer_mep->cc_status.if_tlv_value = (h_s_tlv & 0xFF); |
351 | } |
352 | |
353 | if ((h_s_tlv >> 24) == CFM_PORT_STATUS_TLV_TYPE) { |
354 | /* Port status TLV */ |
355 | peer_mep->cc_status.tlv_seen = true; |
356 | peer_mep->cc_status.port_tlv_value = (h_s_tlv & 0xFF); |
357 | } |
358 | |
359 | /* The Sender ID TLV is not handled */ |
360 | /* The Organization-Specific TLV is not handled */ |
361 | |
362 | /* Return the length of this tlv. |
363 | * This is the length of the value field plus 3 bytes for size of type |
364 | * field and length field |
365 | */ |
366 | return ((h_s_tlv >> 8) & 0xFFFF) + 3; |
367 | } |
368 | |
369 | /* note: already called with rcu_read_lock */ |
370 | static int br_cfm_frame_rx(struct net_bridge_port *port, struct sk_buff *skb) |
371 | { |
372 | u32 mdlevel, interval, size, index, max; |
373 | const struct br_cfm_common_hdr *hdr; |
374 | struct br_cfm_peer_mep *peer_mep; |
375 | const struct br_cfm_maid *maid; |
376 | struct br_cfm_common_hdr _hdr; |
377 | struct br_cfm_maid _maid; |
378 | struct br_cfm_mep *mep; |
379 | struct net_bridge *br; |
380 | __be32 *snumber; |
381 | __be32 _snumber; |
382 | __be16 *mepid; |
383 | __be16 _mepid; |
384 | |
385 | if (port->state == BR_STATE_DISABLED) |
386 | return 0; |
387 | |
388 | hdr = skb_header_pointer(skb, offset: 0, len: sizeof(_hdr), buffer: &_hdr); |
389 | if (!hdr) |
390 | return 1; |
391 | |
392 | br = port->br; |
393 | mep = br_mep_find_ifindex(br, ifindex: port->dev->ifindex); |
394 | if (unlikely(!mep)) |
395 | /* No MEP on this port - must be forwarded */ |
396 | return 0; |
397 | |
398 | mdlevel = hdr->mdlevel_version >> 5; |
399 | if (mdlevel > mep->config.mdlevel) |
400 | /* The level is above this MEP level - must be forwarded */ |
401 | return 0; |
402 | |
403 | if ((hdr->mdlevel_version & 0x1F) != 0) { |
404 | /* Invalid version */ |
405 | mep->status.version_unexp_seen = true; |
406 | return 1; |
407 | } |
408 | |
409 | if (mdlevel < mep->config.mdlevel) { |
410 | /* The level is below this MEP level */ |
411 | mep->status.rx_level_low_seen = true; |
412 | return 1; |
413 | } |
414 | |
415 | if (hdr->opcode == BR_CFM_OPCODE_CCM) { |
416 | /* CCM PDU received. */ |
417 | /* MA ID is after common header + sequence number + MEP ID */ |
418 | maid = skb_header_pointer(skb, |
419 | CFM_CCM_PDU_MAID_OFFSET, |
420 | len: sizeof(_maid), buffer: &_maid); |
421 | if (!maid) |
422 | return 1; |
423 | if (memcmp(p: maid->data, q: mep->cc_config.exp_maid.data, |
424 | size: sizeof(maid->data))) |
425 | /* MA ID not as expected */ |
426 | return 1; |
427 | |
428 | /* MEP ID is after common header + sequence number */ |
429 | mepid = skb_header_pointer(skb, |
430 | CFM_CCM_PDU_MEPID_OFFSET, |
431 | len: sizeof(_mepid), buffer: &_mepid); |
432 | if (!mepid) |
433 | return 1; |
434 | peer_mep = br_peer_mep_find(mep, mepid: (u32)ntohs(*mepid)); |
435 | if (!peer_mep) |
436 | return 1; |
437 | |
438 | /* Interval is in common header flags */ |
439 | interval = hdr->flags & 0x07; |
440 | if (mep->cc_config.exp_interval != pdu_to_interval(value: interval)) |
441 | /* Interval not as expected */ |
442 | return 1; |
443 | |
444 | /* A valid CCM frame is received */ |
445 | if (peer_mep->cc_status.ccm_defect) { |
446 | peer_mep->cc_status.ccm_defect = false; |
447 | |
448 | /* Change in CCM defect status - notify */ |
449 | br_cfm_notify(RTM_NEWLINK, port); |
450 | |
451 | /* Start CCM RX timer */ |
452 | ccm_rx_timer_start(peer_mep); |
453 | } |
454 | |
455 | peer_mep->cc_status.seen = true; |
456 | peer_mep->ccm_rx_count_miss = 0; |
457 | |
458 | /* RDI is in common header flags */ |
459 | peer_mep->cc_status.rdi = (hdr->flags & 0x80) ? true : false; |
460 | |
461 | /* Sequence number is after common header */ |
462 | snumber = skb_header_pointer(skb, |
463 | CFM_CCM_PDU_SEQNR_OFFSET, |
464 | len: sizeof(_snumber), buffer: &_snumber); |
465 | if (!snumber) |
466 | return 1; |
467 | if (ntohl(*snumber) != (mep->ccm_rx_snumber + 1)) |
468 | /* Unexpected sequence number */ |
469 | peer_mep->cc_status.seq_unexp_seen = true; |
470 | |
471 | mep->ccm_rx_snumber = ntohl(*snumber); |
472 | |
473 | /* TLV end is after common header + sequence number + MEP ID + |
474 | * MA ID + ITU reserved |
475 | */ |
476 | index = CFM_CCM_PDU_TLV_OFFSET; |
477 | max = 0; |
478 | do { /* Handle all TLVs */ |
479 | size = ccm_tlv_extract(skb, index, peer_mep); |
480 | index += size; |
481 | max += 1; |
482 | } while (size != 0 && max < 4); /* Max four TLVs possible */ |
483 | |
484 | return 1; |
485 | } |
486 | |
487 | mep->status.opcode_unexp_seen = true; |
488 | |
489 | return 1; |
490 | } |
491 | |
492 | static struct br_frame_type cfm_frame_type __read_mostly = { |
493 | .type = cpu_to_be16(ETH_P_CFM), |
494 | .frame_handler = br_cfm_frame_rx, |
495 | }; |
496 | |
497 | int br_cfm_mep_create(struct net_bridge *br, |
498 | const u32 instance, |
499 | struct br_cfm_mep_create *const create, |
500 | struct netlink_ext_ack *extack) |
501 | { |
502 | struct net_bridge_port *p; |
503 | struct br_cfm_mep *mep; |
504 | |
505 | ASSERT_RTNL(); |
506 | |
507 | if (create->domain == BR_CFM_VLAN) { |
508 | NL_SET_ERR_MSG_MOD(extack, |
509 | "VLAN domain not supported" ); |
510 | return -EINVAL; |
511 | } |
512 | if (create->domain != BR_CFM_PORT) { |
513 | NL_SET_ERR_MSG_MOD(extack, |
514 | "Invalid domain value" ); |
515 | return -EINVAL; |
516 | } |
517 | if (create->direction == BR_CFM_MEP_DIRECTION_UP) { |
518 | NL_SET_ERR_MSG_MOD(extack, |
519 | "Up-MEP not supported" ); |
520 | return -EINVAL; |
521 | } |
522 | if (create->direction != BR_CFM_MEP_DIRECTION_DOWN) { |
523 | NL_SET_ERR_MSG_MOD(extack, |
524 | "Invalid direction value" ); |
525 | return -EINVAL; |
526 | } |
527 | p = br_mep_get_port(br, ifindex: create->ifindex); |
528 | if (!p) { |
529 | NL_SET_ERR_MSG_MOD(extack, |
530 | "Port is not related to bridge" ); |
531 | return -EINVAL; |
532 | } |
533 | mep = br_mep_find(br, instance); |
534 | if (mep) { |
535 | NL_SET_ERR_MSG_MOD(extack, |
536 | "MEP instance already exists" ); |
537 | return -EEXIST; |
538 | } |
539 | |
540 | /* In PORT domain only one instance can be created per port */ |
541 | if (create->domain == BR_CFM_PORT) { |
542 | mep = br_mep_find_ifindex(br, ifindex: create->ifindex); |
543 | if (mep) { |
544 | NL_SET_ERR_MSG_MOD(extack, |
545 | "Only one Port MEP on a port allowed" ); |
546 | return -EINVAL; |
547 | } |
548 | } |
549 | |
550 | mep = kzalloc(size: sizeof(*mep), GFP_KERNEL); |
551 | if (!mep) |
552 | return -ENOMEM; |
553 | |
554 | mep->create = *create; |
555 | mep->instance = instance; |
556 | rcu_assign_pointer(mep->b_port, p); |
557 | |
558 | INIT_HLIST_HEAD(&mep->peer_mep_list); |
559 | INIT_DELAYED_WORK(&mep->ccm_tx_dwork, ccm_tx_work_expired); |
560 | |
561 | if (hlist_empty(h: &br->mep_list)) |
562 | br_add_frame(br, ft: &cfm_frame_type); |
563 | |
564 | hlist_add_tail_rcu(n: &mep->head, h: &br->mep_list); |
565 | |
566 | return 0; |
567 | } |
568 | |
569 | static void mep_delete_implementation(struct net_bridge *br, |
570 | struct br_cfm_mep *mep) |
571 | { |
572 | struct br_cfm_peer_mep *peer_mep; |
573 | struct hlist_node *n_store; |
574 | |
575 | ASSERT_RTNL(); |
576 | |
577 | /* Empty and free peer MEP list */ |
578 | hlist_for_each_entry_safe(peer_mep, n_store, &mep->peer_mep_list, head) { |
579 | cancel_delayed_work_sync(dwork: &peer_mep->ccm_rx_dwork); |
580 | hlist_del_rcu(n: &peer_mep->head); |
581 | kfree_rcu(peer_mep, rcu); |
582 | } |
583 | |
584 | cancel_delayed_work_sync(dwork: &mep->ccm_tx_dwork); |
585 | |
586 | RCU_INIT_POINTER(mep->b_port, NULL); |
587 | hlist_del_rcu(n: &mep->head); |
588 | kfree_rcu(mep, rcu); |
589 | |
590 | if (hlist_empty(h: &br->mep_list)) |
591 | br_del_frame(br, ft: &cfm_frame_type); |
592 | } |
593 | |
594 | int br_cfm_mep_delete(struct net_bridge *br, |
595 | const u32 instance, |
596 | struct netlink_ext_ack *extack) |
597 | { |
598 | struct br_cfm_mep *mep; |
599 | |
600 | ASSERT_RTNL(); |
601 | |
602 | mep = br_mep_find(br, instance); |
603 | if (!mep) { |
604 | NL_SET_ERR_MSG_MOD(extack, |
605 | "MEP instance does not exists" ); |
606 | return -ENOENT; |
607 | } |
608 | |
609 | mep_delete_implementation(br, mep); |
610 | |
611 | return 0; |
612 | } |
613 | |
614 | int br_cfm_mep_config_set(struct net_bridge *br, |
615 | const u32 instance, |
616 | const struct br_cfm_mep_config *const config, |
617 | struct netlink_ext_ack *extack) |
618 | { |
619 | struct br_cfm_mep *mep; |
620 | |
621 | ASSERT_RTNL(); |
622 | |
623 | mep = br_mep_find(br, instance); |
624 | if (!mep) { |
625 | NL_SET_ERR_MSG_MOD(extack, |
626 | "MEP instance does not exists" ); |
627 | return -ENOENT; |
628 | } |
629 | |
630 | mep->config = *config; |
631 | |
632 | return 0; |
633 | } |
634 | |
635 | int br_cfm_cc_config_set(struct net_bridge *br, |
636 | const u32 instance, |
637 | const struct br_cfm_cc_config *const config, |
638 | struct netlink_ext_ack *extack) |
639 | { |
640 | struct br_cfm_peer_mep *peer_mep; |
641 | struct br_cfm_mep *mep; |
642 | |
643 | ASSERT_RTNL(); |
644 | |
645 | mep = br_mep_find(br, instance); |
646 | if (!mep) { |
647 | NL_SET_ERR_MSG_MOD(extack, |
648 | "MEP instance does not exists" ); |
649 | return -ENOENT; |
650 | } |
651 | |
652 | /* Check for no change in configuration */ |
653 | if (memcmp(p: config, q: &mep->cc_config, size: sizeof(*config)) == 0) |
654 | return 0; |
655 | |
656 | if (config->enable && !mep->cc_config.enable) |
657 | /* CC is enabled */ |
658 | hlist_for_each_entry(peer_mep, &mep->peer_mep_list, head) |
659 | cc_peer_enable(peer_mep); |
660 | |
661 | if (!config->enable && mep->cc_config.enable) |
662 | /* CC is disabled */ |
663 | hlist_for_each_entry(peer_mep, &mep->peer_mep_list, head) |
664 | cc_peer_disable(peer_mep); |
665 | |
666 | mep->cc_config = *config; |
667 | mep->ccm_rx_snumber = 0; |
668 | mep->ccm_tx_snumber = 1; |
669 | |
670 | return 0; |
671 | } |
672 | |
673 | int br_cfm_cc_peer_mep_add(struct net_bridge *br, const u32 instance, |
674 | u32 mepid, |
675 | struct netlink_ext_ack *extack) |
676 | { |
677 | struct br_cfm_peer_mep *peer_mep; |
678 | struct br_cfm_mep *mep; |
679 | |
680 | ASSERT_RTNL(); |
681 | |
682 | mep = br_mep_find(br, instance); |
683 | if (!mep) { |
684 | NL_SET_ERR_MSG_MOD(extack, |
685 | "MEP instance does not exists" ); |
686 | return -ENOENT; |
687 | } |
688 | |
689 | peer_mep = br_peer_mep_find(mep, mepid); |
690 | if (peer_mep) { |
691 | NL_SET_ERR_MSG_MOD(extack, |
692 | "Peer MEP-ID already exists" ); |
693 | return -EEXIST; |
694 | } |
695 | |
696 | peer_mep = kzalloc(size: sizeof(*peer_mep), GFP_KERNEL); |
697 | if (!peer_mep) |
698 | return -ENOMEM; |
699 | |
700 | peer_mep->mepid = mepid; |
701 | peer_mep->mep = mep; |
702 | INIT_DELAYED_WORK(&peer_mep->ccm_rx_dwork, ccm_rx_work_expired); |
703 | |
704 | if (mep->cc_config.enable) |
705 | cc_peer_enable(peer_mep); |
706 | |
707 | hlist_add_tail_rcu(n: &peer_mep->head, h: &mep->peer_mep_list); |
708 | |
709 | return 0; |
710 | } |
711 | |
712 | int br_cfm_cc_peer_mep_remove(struct net_bridge *br, const u32 instance, |
713 | u32 mepid, |
714 | struct netlink_ext_ack *extack) |
715 | { |
716 | struct br_cfm_peer_mep *peer_mep; |
717 | struct br_cfm_mep *mep; |
718 | |
719 | ASSERT_RTNL(); |
720 | |
721 | mep = br_mep_find(br, instance); |
722 | if (!mep) { |
723 | NL_SET_ERR_MSG_MOD(extack, |
724 | "MEP instance does not exists" ); |
725 | return -ENOENT; |
726 | } |
727 | |
728 | peer_mep = br_peer_mep_find(mep, mepid); |
729 | if (!peer_mep) { |
730 | NL_SET_ERR_MSG_MOD(extack, |
731 | "Peer MEP-ID does not exists" ); |
732 | return -ENOENT; |
733 | } |
734 | |
735 | cc_peer_disable(peer_mep); |
736 | |
737 | hlist_del_rcu(n: &peer_mep->head); |
738 | kfree_rcu(peer_mep, rcu); |
739 | |
740 | return 0; |
741 | } |
742 | |
743 | int br_cfm_cc_rdi_set(struct net_bridge *br, const u32 instance, |
744 | const bool rdi, struct netlink_ext_ack *extack) |
745 | { |
746 | struct br_cfm_mep *mep; |
747 | |
748 | ASSERT_RTNL(); |
749 | |
750 | mep = br_mep_find(br, instance); |
751 | if (!mep) { |
752 | NL_SET_ERR_MSG_MOD(extack, |
753 | "MEP instance does not exists" ); |
754 | return -ENOENT; |
755 | } |
756 | |
757 | mep->rdi = rdi; |
758 | |
759 | return 0; |
760 | } |
761 | |
762 | int br_cfm_cc_ccm_tx(struct net_bridge *br, const u32 instance, |
763 | const struct br_cfm_cc_ccm_tx_info *const tx_info, |
764 | struct netlink_ext_ack *extack) |
765 | { |
766 | struct br_cfm_mep *mep; |
767 | |
768 | ASSERT_RTNL(); |
769 | |
770 | mep = br_mep_find(br, instance); |
771 | if (!mep) { |
772 | NL_SET_ERR_MSG_MOD(extack, |
773 | "MEP instance does not exists" ); |
774 | return -ENOENT; |
775 | } |
776 | |
777 | if (memcmp(p: tx_info, q: &mep->cc_ccm_tx_info, size: sizeof(*tx_info)) == 0) { |
778 | /* No change in tx_info. */ |
779 | if (mep->cc_ccm_tx_info.period == 0) |
780 | /* Transmission is not enabled - just return */ |
781 | return 0; |
782 | |
783 | /* Transmission is ongoing, the end time is recalculated */ |
784 | mep->ccm_tx_end = jiffies + |
785 | usecs_to_jiffies(u: tx_info->period * 1000000); |
786 | return 0; |
787 | } |
788 | |
789 | if (tx_info->period == 0 && mep->cc_ccm_tx_info.period == 0) |
790 | /* Some change in info and transmission is not ongoing */ |
791 | goto save; |
792 | |
793 | if (tx_info->period != 0 && mep->cc_ccm_tx_info.period != 0) { |
794 | /* Some change in info and transmission is ongoing |
795 | * The end time is recalculated |
796 | */ |
797 | mep->ccm_tx_end = jiffies + |
798 | usecs_to_jiffies(u: tx_info->period * 1000000); |
799 | |
800 | goto save; |
801 | } |
802 | |
803 | if (tx_info->period == 0 && mep->cc_ccm_tx_info.period != 0) { |
804 | cancel_delayed_work_sync(dwork: &mep->ccm_tx_dwork); |
805 | goto save; |
806 | } |
807 | |
808 | /* Start delayed work to transmit CCM frames. It is done with zero delay |
809 | * to send first frame immediately |
810 | */ |
811 | mep->ccm_tx_end = jiffies + usecs_to_jiffies(u: tx_info->period * 1000000); |
812 | queue_delayed_work(wq: system_wq, dwork: &mep->ccm_tx_dwork, delay: 0); |
813 | |
814 | save: |
815 | mep->cc_ccm_tx_info = *tx_info; |
816 | |
817 | return 0; |
818 | } |
819 | |
820 | int br_cfm_mep_count(struct net_bridge *br, u32 *count) |
821 | { |
822 | struct br_cfm_mep *mep; |
823 | |
824 | *count = 0; |
825 | |
826 | rcu_read_lock(); |
827 | hlist_for_each_entry_rcu(mep, &br->mep_list, head) |
828 | *count += 1; |
829 | rcu_read_unlock(); |
830 | |
831 | return 0; |
832 | } |
833 | |
834 | int br_cfm_peer_mep_count(struct net_bridge *br, u32 *count) |
835 | { |
836 | struct br_cfm_peer_mep *peer_mep; |
837 | struct br_cfm_mep *mep; |
838 | |
839 | *count = 0; |
840 | |
841 | rcu_read_lock(); |
842 | hlist_for_each_entry_rcu(mep, &br->mep_list, head) |
843 | hlist_for_each_entry_rcu(peer_mep, &mep->peer_mep_list, head) |
844 | *count += 1; |
845 | rcu_read_unlock(); |
846 | |
847 | return 0; |
848 | } |
849 | |
850 | bool br_cfm_created(struct net_bridge *br) |
851 | { |
852 | return !hlist_empty(h: &br->mep_list); |
853 | } |
854 | |
855 | /* Deletes the CFM instances on a specific bridge port |
856 | */ |
857 | void br_cfm_port_del(struct net_bridge *br, struct net_bridge_port *port) |
858 | { |
859 | struct hlist_node *n_store; |
860 | struct br_cfm_mep *mep; |
861 | |
862 | ASSERT_RTNL(); |
863 | |
864 | hlist_for_each_entry_safe(mep, n_store, &br->mep_list, head) |
865 | if (mep->create.ifindex == port->dev->ifindex) |
866 | mep_delete_implementation(br, mep); |
867 | } |
868 | |