1 /* Copyright 2011-2014 Autronica Fire and Security AS 2 * 3 * This program is free software; you can redistribute it and/or modify it 4 * under the terms of the GNU General Public License as published by the Free 5 * Software Foundation; either version 2 of the License, or (at your option) 6 * any later version. 7 * 8 * Author(s): 9 * 2011-2014 Arvid Brodin, arvid.brodin@alten.se 10 */ 11 12 #include "hsr_forward.h" 13 #include <linux/types.h> 14 #include <linux/skbuff.h> 15 #include <linux/etherdevice.h> 16 #include <linux/if_vlan.h> 17 #include "hsr_main.h" 18 #include "hsr_framereg.h" 19 20 21 struct hsr_node; 22 23 struct hsr_frame_info { 24 struct sk_buff *skb_std; 25 struct sk_buff *skb_hsr; 26 struct hsr_port *port_rcv; 27 struct hsr_node *node_src; 28 u16 sequence_nr; 29 bool is_supervision; 30 bool is_vlan; 31 bool is_local_dest; 32 bool is_local_exclusive; 33 }; 34 35 36 /* The uses I can see for these HSR supervision frames are: 37 * 1) Use the frames that are sent after node initialization ("HSR_TLV.Type = 38 * 22") to reset any sequence_nr counters belonging to that node. Useful if 39 * the other node's counter has been reset for some reason. 40 * -- 41 * Or not - resetting the counter and bridging the frame would create a 42 * loop, unfortunately. 43 * 44 * 2) Use the LifeCheck frames to detect ring breaks. I.e. if no LifeCheck 45 * frame is received from a particular node, we know something is wrong. 46 * We just register these (as with normal frames) and throw them away. 47 * 48 * 3) Allow different MAC addresses for the two slave interfaces, using the 49 * MacAddressA field. 50 */ 51 static bool is_supervision_frame(struct hsr_priv *hsr, struct sk_buff *skb) 52 { 53 struct hsr_ethhdr_sp *hdr; 54 55 WARN_ON_ONCE(!skb_mac_header_was_set(skb)); 56 hdr = (struct hsr_ethhdr_sp *) skb_mac_header(skb); 57 58 if (!ether_addr_equal(hdr->ethhdr.h_dest, 59 hsr->sup_multicast_addr)) 60 return false; 61 62 if (get_hsr_stag_path(&hdr->hsr_sup) != 0x0f) 63 return false; 64 if ((hdr->hsr_sup.HSR_TLV_Type != HSR_TLV_ANNOUNCE) && 65 (hdr->hsr_sup.HSR_TLV_Type != HSR_TLV_LIFE_CHECK)) 66 return false; 67 if (hdr->hsr_sup.HSR_TLV_Length != 12) 68 return false; 69 70 return true; 71 } 72 73 74 static struct sk_buff *create_stripped_skb(struct sk_buff *skb_in, 75 struct hsr_frame_info *frame) 76 { 77 struct sk_buff *skb; 78 int copylen; 79 unsigned char *dst, *src; 80 81 skb_pull(skb_in, HSR_HLEN); 82 skb = __pskb_copy(skb_in, skb_headroom(skb_in) - HSR_HLEN, GFP_ATOMIC); 83 skb_push(skb_in, HSR_HLEN); 84 if (skb == NULL) 85 return NULL; 86 87 skb_reset_mac_header(skb); 88 89 if (skb->ip_summed == CHECKSUM_PARTIAL) 90 skb->csum_start -= HSR_HLEN; 91 92 copylen = 2*ETH_ALEN; 93 if (frame->is_vlan) 94 copylen += VLAN_HLEN; 95 src = skb_mac_header(skb_in); 96 dst = skb_mac_header(skb); 97 memcpy(dst, src, copylen); 98 99 skb->protocol = eth_hdr(skb)->h_proto; 100 return skb; 101 } 102 103 static struct sk_buff *frame_get_stripped_skb(struct hsr_frame_info *frame, 104 struct hsr_port *port) 105 { 106 if (!frame->skb_std) 107 frame->skb_std = create_stripped_skb(frame->skb_hsr, frame); 108 return skb_clone(frame->skb_std, GFP_ATOMIC); 109 } 110 111 112 static void hsr_fill_tag(struct sk_buff *skb, struct hsr_frame_info *frame, 113 struct hsr_port *port) 114 { 115 struct hsr_ethhdr *hsr_ethhdr; 116 int lane_id; 117 int lsdu_size; 118 119 if (port->type == HSR_PT_SLAVE_A) 120 lane_id = 0; 121 else 122 lane_id = 1; 123 124 lsdu_size = skb->len - 14; 125 if (frame->is_vlan) 126 lsdu_size -= 4; 127 128 hsr_ethhdr = (struct hsr_ethhdr *) skb_mac_header(skb); 129 130 set_hsr_tag_path(&hsr_ethhdr->hsr_tag, lane_id); 131 set_hsr_tag_LSDU_size(&hsr_ethhdr->hsr_tag, lsdu_size); 132 hsr_ethhdr->hsr_tag.sequence_nr = htons(frame->sequence_nr); 133 hsr_ethhdr->hsr_tag.encap_proto = hsr_ethhdr->ethhdr.h_proto; 134 hsr_ethhdr->ethhdr.h_proto = htons(ETH_P_PRP); 135 } 136 137 static struct sk_buff *create_tagged_skb(struct sk_buff *skb_o, 138 struct hsr_frame_info *frame, 139 struct hsr_port *port) 140 { 141 int movelen; 142 unsigned char *dst, *src; 143 struct sk_buff *skb; 144 145 /* Create the new skb with enough headroom to fit the HSR tag */ 146 skb = __pskb_copy(skb_o, skb_headroom(skb_o) + HSR_HLEN, GFP_ATOMIC); 147 if (skb == NULL) 148 return NULL; 149 skb_reset_mac_header(skb); 150 151 if (skb->ip_summed == CHECKSUM_PARTIAL) 152 skb->csum_start += HSR_HLEN; 153 154 movelen = ETH_HLEN; 155 if (frame->is_vlan) 156 movelen += VLAN_HLEN; 157 158 src = skb_mac_header(skb); 159 dst = skb_push(skb, HSR_HLEN); 160 memmove(dst, src, movelen); 161 skb_reset_mac_header(skb); 162 163 hsr_fill_tag(skb, frame, port); 164 165 return skb; 166 } 167 168 /* If the original frame was an HSR tagged frame, just clone it to be sent 169 * unchanged. Otherwise, create a private frame especially tagged for 'port'. 170 */ 171 static struct sk_buff *frame_get_tagged_skb(struct hsr_frame_info *frame, 172 struct hsr_port *port) 173 { 174 if (frame->skb_hsr) 175 return skb_clone(frame->skb_hsr, GFP_ATOMIC); 176 177 if ((port->type != HSR_PT_SLAVE_A) && (port->type != HSR_PT_SLAVE_B)) { 178 WARN_ONCE(1, "HSR: Bug: trying to create a tagged frame for a non-ring port"); 179 return NULL; 180 } 181 182 return create_tagged_skb(frame->skb_std, frame, port); 183 } 184 185 186 static void hsr_deliver_master(struct sk_buff *skb, struct net_device *dev, 187 struct hsr_node *node_src) 188 { 189 bool was_multicast_frame; 190 int res; 191 192 was_multicast_frame = (skb->pkt_type == PACKET_MULTICAST); 193 hsr_addr_subst_source(node_src, skb); 194 skb_pull(skb, ETH_HLEN); 195 res = netif_rx(skb); 196 if (res == NET_RX_DROP) { 197 dev->stats.rx_dropped++; 198 } else { 199 dev->stats.rx_packets++; 200 dev->stats.rx_bytes += skb->len; 201 if (was_multicast_frame) 202 dev->stats.multicast++; 203 } 204 } 205 206 static int hsr_xmit(struct sk_buff *skb, struct hsr_port *port, 207 struct hsr_frame_info *frame) 208 { 209 if (frame->port_rcv->type == HSR_PT_MASTER) { 210 hsr_addr_subst_dest(frame->node_src, skb, port); 211 212 /* Address substitution (IEC62439-3 pp 26, 50): replace mac 213 * address of outgoing frame with that of the outgoing slave's. 214 */ 215 ether_addr_copy(eth_hdr(skb)->h_source, port->dev->dev_addr); 216 } 217 return dev_queue_xmit(skb); 218 } 219 220 221 /* Forward the frame through all devices except: 222 * - Back through the receiving device 223 * - If it's a HSR frame: through a device where it has passed before 224 * - To the local HSR master only if the frame is directly addressed to it, or 225 * a non-supervision multicast or broadcast frame. 226 * 227 * HSR slave devices should insert a HSR tag into the frame, or forward the 228 * frame unchanged if it's already tagged. Interlink devices should strip HSR 229 * tags if they're of the non-HSR type (but only after duplicate discard). The 230 * master device always strips HSR tags. 231 */ 232 static void hsr_forward_do(struct hsr_frame_info *frame) 233 { 234 struct hsr_port *port; 235 struct sk_buff *skb; 236 237 hsr_for_each_port(frame->port_rcv->hsr, port) { 238 /* Don't send frame back the way it came */ 239 if (port == frame->port_rcv) 240 continue; 241 242 /* Don't deliver locally unless we should */ 243 if ((port->type == HSR_PT_MASTER) && !frame->is_local_dest) 244 continue; 245 246 /* Deliver frames directly addressed to us to master only */ 247 if ((port->type != HSR_PT_MASTER) && frame->is_local_exclusive) 248 continue; 249 250 /* Don't send frame over port where it has been sent before */ 251 if (hsr_register_frame_out(port, frame->node_src, 252 frame->sequence_nr)) 253 continue; 254 255 if (frame->is_supervision && (port->type == HSR_PT_MASTER)) { 256 hsr_handle_sup_frame(frame->skb_hsr, 257 frame->node_src, 258 frame->port_rcv); 259 continue; 260 } 261 262 if (port->type != HSR_PT_MASTER) 263 skb = frame_get_tagged_skb(frame, port); 264 else 265 skb = frame_get_stripped_skb(frame, port); 266 if (skb == NULL) { 267 /* FIXME: Record the dropped frame? */ 268 continue; 269 } 270 271 skb->dev = port->dev; 272 if (port->type == HSR_PT_MASTER) 273 hsr_deliver_master(skb, port->dev, frame->node_src); 274 else 275 hsr_xmit(skb, port, frame); 276 } 277 } 278 279 280 static void check_local_dest(struct hsr_priv *hsr, struct sk_buff *skb, 281 struct hsr_frame_info *frame) 282 { 283 struct net_device *master_dev; 284 285 master_dev = hsr_port_get_hsr(hsr, HSR_PT_MASTER)->dev; 286 287 if (hsr_addr_is_self(hsr, eth_hdr(skb)->h_dest)) { 288 frame->is_local_exclusive = true; 289 skb->pkt_type = PACKET_HOST; 290 } else { 291 frame->is_local_exclusive = false; 292 } 293 294 if ((skb->pkt_type == PACKET_HOST) || 295 (skb->pkt_type == PACKET_MULTICAST) || 296 (skb->pkt_type == PACKET_BROADCAST)) { 297 frame->is_local_dest = true; 298 } else { 299 frame->is_local_dest = false; 300 } 301 } 302 303 304 static int hsr_fill_frame_info(struct hsr_frame_info *frame, 305 struct sk_buff *skb, struct hsr_port *port) 306 { 307 struct ethhdr *ethhdr; 308 unsigned long irqflags; 309 310 frame->is_supervision = is_supervision_frame(port->hsr, skb); 311 frame->node_src = hsr_get_node(&port->hsr->node_db, skb, 312 frame->is_supervision); 313 if (frame->node_src == NULL) 314 return -1; /* Unknown node and !is_supervision, or no mem */ 315 316 ethhdr = (struct ethhdr *) skb_mac_header(skb); 317 frame->is_vlan = false; 318 if (ethhdr->h_proto == htons(ETH_P_8021Q)) { 319 frame->is_vlan = true; 320 /* FIXME: */ 321 WARN_ONCE(1, "HSR: VLAN not yet supported"); 322 } 323 if (ethhdr->h_proto == htons(ETH_P_PRP)) { 324 frame->skb_std = NULL; 325 frame->skb_hsr = skb; 326 frame->sequence_nr = hsr_get_skb_sequence_nr(skb); 327 } else { 328 frame->skb_std = skb; 329 frame->skb_hsr = NULL; 330 /* Sequence nr for the master node */ 331 spin_lock_irqsave(&port->hsr->seqnr_lock, irqflags); 332 frame->sequence_nr = port->hsr->sequence_nr; 333 port->hsr->sequence_nr++; 334 spin_unlock_irqrestore(&port->hsr->seqnr_lock, irqflags); 335 } 336 337 frame->port_rcv = port; 338 check_local_dest(port->hsr, skb, frame); 339 340 return 0; 341 } 342 343 /* Must be called holding rcu read lock (because of the port parameter) */ 344 void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port) 345 { 346 struct hsr_frame_info frame; 347 348 if (skb_mac_header(skb) != skb->data) { 349 WARN_ONCE(1, "%s:%d: Malformed frame (port_src %s)\n", 350 __FILE__, __LINE__, port->dev->name); 351 goto out_drop; 352 } 353 354 if (hsr_fill_frame_info(&frame, skb, port) < 0) 355 goto out_drop; 356 hsr_register_frame_in(frame.node_src, port, frame.sequence_nr); 357 hsr_forward_do(&frame); 358 359 if (frame.skb_hsr != NULL) 360 kfree_skb(frame.skb_hsr); 361 if (frame.skb_std != NULL) 362 kfree_skb(frame.skb_std); 363 return; 364 365 out_drop: 366 port->dev->stats.tx_dropped++; 367 kfree_skb(skb); 368 } 369