hsr_forward.c 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568
  1. // SPDX-License-Identifier: GPL-2.0
  2. /* Copyright 2011-2014 Autronica Fire and Security AS
  3. *
  4. * Author(s):
  5. * 2011-2014 Arvid Brodin, arvid.brodin@alten.se
  6. *
  7. * Frame router for HSR and PRP.
  8. */
  9. #include "hsr_forward.h"
  10. #include <linux/types.h>
  11. #include <linux/skbuff.h>
  12. #include <linux/etherdevice.h>
  13. #include <linux/if_vlan.h>
  14. #include "hsr_main.h"
  15. #include "hsr_framereg.h"
  16. struct hsr_node;
  17. /* The uses I can see for these HSR supervision frames are:
  18. * 1) Use the frames that are sent after node initialization ("HSR_TLV.Type =
  19. * 22") to reset any sequence_nr counters belonging to that node. Useful if
  20. * the other node's counter has been reset for some reason.
  21. * --
  22. * Or not - resetting the counter and bridging the frame would create a
  23. * loop, unfortunately.
  24. *
  25. * 2) Use the LifeCheck frames to detect ring breaks. I.e. if no LifeCheck
  26. * frame is received from a particular node, we know something is wrong.
  27. * We just register these (as with normal frames) and throw them away.
  28. *
  29. * 3) Allow different MAC addresses for the two slave interfaces, using the
  30. * MacAddressA field.
  31. */
  32. static bool is_supervision_frame(struct hsr_priv *hsr, struct sk_buff *skb)
  33. {
  34. struct ethhdr *eth_hdr;
  35. struct hsr_sup_tag *hsr_sup_tag;
  36. struct hsrv1_ethhdr_sp *hsr_V1_hdr;
  37. WARN_ON_ONCE(!skb_mac_header_was_set(skb));
  38. eth_hdr = (struct ethhdr *)skb_mac_header(skb);
  39. /* Correct addr? */
  40. if (!ether_addr_equal(eth_hdr->h_dest,
  41. hsr->sup_multicast_addr))
  42. return false;
  43. /* Correct ether type?. */
  44. if (!(eth_hdr->h_proto == htons(ETH_P_PRP) ||
  45. eth_hdr->h_proto == htons(ETH_P_HSR)))
  46. return false;
  47. /* Get the supervision header from correct location. */
  48. if (eth_hdr->h_proto == htons(ETH_P_HSR)) { /* Okay HSRv1. */
  49. hsr_V1_hdr = (struct hsrv1_ethhdr_sp *)skb_mac_header(skb);
  50. if (hsr_V1_hdr->hsr.encap_proto != htons(ETH_P_PRP))
  51. return false;
  52. hsr_sup_tag = &hsr_V1_hdr->hsr_sup;
  53. } else {
  54. hsr_sup_tag =
  55. &((struct hsrv0_ethhdr_sp *)skb_mac_header(skb))->hsr_sup;
  56. }
  57. if (hsr_sup_tag->HSR_TLV_type != HSR_TLV_ANNOUNCE &&
  58. hsr_sup_tag->HSR_TLV_type != HSR_TLV_LIFE_CHECK &&
  59. hsr_sup_tag->HSR_TLV_type != PRP_TLV_LIFE_CHECK_DD &&
  60. hsr_sup_tag->HSR_TLV_type != PRP_TLV_LIFE_CHECK_DA)
  61. return false;
  62. if (hsr_sup_tag->HSR_TLV_length != 12 &&
  63. hsr_sup_tag->HSR_TLV_length != sizeof(struct hsr_sup_payload))
  64. return false;
  65. return true;
  66. }
  67. static struct sk_buff *create_stripped_skb_hsr(struct sk_buff *skb_in,
  68. struct hsr_frame_info *frame)
  69. {
  70. struct sk_buff *skb;
  71. int copylen;
  72. unsigned char *dst, *src;
  73. skb_pull(skb_in, HSR_HLEN);
  74. skb = __pskb_copy(skb_in, skb_headroom(skb_in) - HSR_HLEN, GFP_ATOMIC);
  75. skb_push(skb_in, HSR_HLEN);
  76. if (!skb)
  77. return NULL;
  78. skb_reset_mac_header(skb);
  79. if (skb->ip_summed == CHECKSUM_PARTIAL)
  80. skb->csum_start -= HSR_HLEN;
  81. copylen = 2 * ETH_ALEN;
  82. if (frame->is_vlan)
  83. copylen += VLAN_HLEN;
  84. src = skb_mac_header(skb_in);
  85. dst = skb_mac_header(skb);
  86. memcpy(dst, src, copylen);
  87. skb->protocol = eth_hdr(skb)->h_proto;
  88. return skb;
  89. }
  90. struct sk_buff *hsr_get_untagged_frame(struct hsr_frame_info *frame,
  91. struct hsr_port *port)
  92. {
  93. if (!frame->skb_std) {
  94. if (frame->skb_hsr) {
  95. frame->skb_std =
  96. create_stripped_skb_hsr(frame->skb_hsr, frame);
  97. } else {
  98. /* Unexpected */
  99. WARN_ONCE(1, "%s:%d: Unexpected frame received (port_src %s)\n",
  100. __FILE__, __LINE__, port->dev->name);
  101. return NULL;
  102. }
  103. }
  104. return skb_clone(frame->skb_std, GFP_ATOMIC);
  105. }
  106. struct sk_buff *prp_get_untagged_frame(struct hsr_frame_info *frame,
  107. struct hsr_port *port)
  108. {
  109. if (!frame->skb_std) {
  110. if (frame->skb_prp) {
  111. /* trim the skb by len - HSR_HLEN to exclude RCT */
  112. skb_trim(frame->skb_prp,
  113. frame->skb_prp->len - HSR_HLEN);
  114. frame->skb_std =
  115. __pskb_copy(frame->skb_prp,
  116. skb_headroom(frame->skb_prp),
  117. GFP_ATOMIC);
  118. } else {
  119. /* Unexpected */
  120. WARN_ONCE(1, "%s:%d: Unexpected frame received (port_src %s)\n",
  121. __FILE__, __LINE__, port->dev->name);
  122. return NULL;
  123. }
  124. }
  125. return skb_clone(frame->skb_std, GFP_ATOMIC);
  126. }
  127. static void prp_set_lan_id(struct prp_rct *trailer,
  128. struct hsr_port *port)
  129. {
  130. int lane_id;
  131. if (port->type == HSR_PT_SLAVE_A)
  132. lane_id = 0;
  133. else
  134. lane_id = 1;
  135. /* Add net_id in the upper 3 bits of lane_id */
  136. lane_id |= port->hsr->net_id;
  137. set_prp_lan_id(trailer, lane_id);
  138. }
  139. /* Tailroom for PRP rct should have been created before calling this */
  140. static struct sk_buff *prp_fill_rct(struct sk_buff *skb,
  141. struct hsr_frame_info *frame,
  142. struct hsr_port *port)
  143. {
  144. struct prp_rct *trailer;
  145. int min_size = ETH_ZLEN;
  146. int lsdu_size;
  147. if (!skb)
  148. return skb;
  149. if (frame->is_vlan)
  150. min_size = VLAN_ETH_ZLEN;
  151. if (skb_put_padto(skb, min_size))
  152. return NULL;
  153. trailer = (struct prp_rct *)skb_put(skb, HSR_HLEN);
  154. lsdu_size = skb->len - 14;
  155. if (frame->is_vlan)
  156. lsdu_size -= 4;
  157. prp_set_lan_id(trailer, port);
  158. set_prp_LSDU_size(trailer, lsdu_size);
  159. trailer->sequence_nr = htons(frame->sequence_nr);
  160. trailer->PRP_suffix = htons(ETH_P_PRP);
  161. return skb;
  162. }
  163. static void hsr_set_path_id(struct hsr_ethhdr *hsr_ethhdr,
  164. struct hsr_port *port)
  165. {
  166. int path_id;
  167. if (port->type == HSR_PT_SLAVE_A)
  168. path_id = 0;
  169. else
  170. path_id = 1;
  171. set_hsr_tag_path(&hsr_ethhdr->hsr_tag, path_id);
  172. }
  173. static struct sk_buff *hsr_fill_tag(struct sk_buff *skb,
  174. struct hsr_frame_info *frame,
  175. struct hsr_port *port, u8 proto_version)
  176. {
  177. struct hsr_ethhdr *hsr_ethhdr;
  178. int lsdu_size;
  179. /* pad to minimum packet size which is 60 + 6 (HSR tag) */
  180. if (skb_put_padto(skb, ETH_ZLEN + HSR_HLEN))
  181. return NULL;
  182. lsdu_size = skb->len - 14;
  183. if (frame->is_vlan)
  184. lsdu_size -= 4;
  185. hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb);
  186. hsr_set_path_id(hsr_ethhdr, port);
  187. set_hsr_tag_LSDU_size(&hsr_ethhdr->hsr_tag, lsdu_size);
  188. hsr_ethhdr->hsr_tag.sequence_nr = htons(frame->sequence_nr);
  189. hsr_ethhdr->hsr_tag.encap_proto = hsr_ethhdr->ethhdr.h_proto;
  190. hsr_ethhdr->ethhdr.h_proto = htons(proto_version ?
  191. ETH_P_HSR : ETH_P_PRP);
  192. return skb;
  193. }
  194. /* If the original frame was an HSR tagged frame, just clone it to be sent
  195. * unchanged. Otherwise, create a private frame especially tagged for 'port'.
  196. */
  197. struct sk_buff *hsr_create_tagged_frame(struct hsr_frame_info *frame,
  198. struct hsr_port *port)
  199. {
  200. unsigned char *dst, *src;
  201. struct sk_buff *skb;
  202. int movelen;
  203. if (frame->skb_hsr) {
  204. struct hsr_ethhdr *hsr_ethhdr =
  205. (struct hsr_ethhdr *)skb_mac_header(frame->skb_hsr);
  206. /* set the lane id properly */
  207. hsr_set_path_id(hsr_ethhdr, port);
  208. return skb_clone(frame->skb_hsr, GFP_ATOMIC);
  209. }
  210. /* Create the new skb with enough headroom to fit the HSR tag */
  211. skb = __pskb_copy(frame->skb_std,
  212. skb_headroom(frame->skb_std) + HSR_HLEN, GFP_ATOMIC);
  213. if (!skb)
  214. return NULL;
  215. skb_reset_mac_header(skb);
  216. if (skb->ip_summed == CHECKSUM_PARTIAL)
  217. skb->csum_start += HSR_HLEN;
  218. movelen = ETH_HLEN;
  219. if (frame->is_vlan)
  220. movelen += VLAN_HLEN;
  221. src = skb_mac_header(skb);
  222. dst = skb_push(skb, HSR_HLEN);
  223. memmove(dst, src, movelen);
  224. skb_reset_mac_header(skb);
  225. /* skb_put_padto free skb on error and hsr_fill_tag returns NULL in
  226. * that case
  227. */
  228. return hsr_fill_tag(skb, frame, port, port->hsr->prot_version);
  229. }
  230. struct sk_buff *prp_create_tagged_frame(struct hsr_frame_info *frame,
  231. struct hsr_port *port)
  232. {
  233. struct sk_buff *skb;
  234. if (frame->skb_prp) {
  235. struct prp_rct *trailer = skb_get_PRP_rct(frame->skb_prp);
  236. if (trailer) {
  237. prp_set_lan_id(trailer, port);
  238. } else {
  239. WARN_ONCE(!trailer, "errored PRP skb");
  240. return NULL;
  241. }
  242. return skb_clone(frame->skb_prp, GFP_ATOMIC);
  243. }
  244. skb = skb_copy_expand(frame->skb_std, 0,
  245. skb_tailroom(frame->skb_std) + HSR_HLEN,
  246. GFP_ATOMIC);
  247. prp_fill_rct(skb, frame, port);
  248. return skb;
  249. }
  250. static void hsr_deliver_master(struct sk_buff *skb, struct net_device *dev,
  251. struct hsr_node *node_src)
  252. {
  253. bool was_multicast_frame;
  254. int res;
  255. was_multicast_frame = (skb->pkt_type == PACKET_MULTICAST);
  256. hsr_addr_subst_source(node_src, skb);
  257. skb_pull(skb, ETH_HLEN);
  258. res = netif_rx(skb);
  259. if (res == NET_RX_DROP) {
  260. dev->stats.rx_dropped++;
  261. } else {
  262. dev->stats.rx_packets++;
  263. dev->stats.rx_bytes += skb->len;
  264. if (was_multicast_frame)
  265. dev->stats.multicast++;
  266. }
  267. }
  268. static int hsr_xmit(struct sk_buff *skb, struct hsr_port *port,
  269. struct hsr_frame_info *frame)
  270. {
  271. if (frame->port_rcv->type == HSR_PT_MASTER) {
  272. hsr_addr_subst_dest(frame->node_src, skb, port);
  273. /* Address substitution (IEC62439-3 pp 26, 50): replace mac
  274. * address of outgoing frame with that of the outgoing slave's.
  275. */
  276. ether_addr_copy(eth_hdr(skb)->h_source, port->dev->dev_addr);
  277. }
  278. return dev_queue_xmit(skb);
  279. }
  280. bool prp_drop_frame(struct hsr_frame_info *frame, struct hsr_port *port)
  281. {
  282. return ((frame->port_rcv->type == HSR_PT_SLAVE_A &&
  283. port->type == HSR_PT_SLAVE_B) ||
  284. (frame->port_rcv->type == HSR_PT_SLAVE_B &&
  285. port->type == HSR_PT_SLAVE_A));
  286. }
  287. /* Forward the frame through all devices except:
  288. * - Back through the receiving device
  289. * - If it's a HSR frame: through a device where it has passed before
  290. * - if it's a PRP frame: through another PRP slave device (no bridge)
  291. * - To the local HSR master only if the frame is directly addressed to it, or
  292. * a non-supervision multicast or broadcast frame.
  293. *
  294. * HSR slave devices should insert a HSR tag into the frame, or forward the
  295. * frame unchanged if it's already tagged. Interlink devices should strip HSR
  296. * tags if they're of the non-HSR type (but only after duplicate discard). The
  297. * master device always strips HSR tags.
  298. */
  299. static void hsr_forward_do(struct hsr_frame_info *frame)
  300. {
  301. struct hsr_port *port;
  302. struct sk_buff *skb;
  303. hsr_for_each_port(frame->port_rcv->hsr, port) {
  304. struct hsr_priv *hsr = port->hsr;
  305. /* Don't send frame back the way it came */
  306. if (port == frame->port_rcv)
  307. continue;
  308. /* Don't deliver locally unless we should */
  309. if (port->type == HSR_PT_MASTER && !frame->is_local_dest)
  310. continue;
  311. /* Deliver frames directly addressed to us to master only */
  312. if (port->type != HSR_PT_MASTER && frame->is_local_exclusive)
  313. continue;
  314. /* Don't send frame over port where it has been sent before.
  315. * Also fro SAN, this shouldn't be done.
  316. */
  317. if (!frame->is_from_san &&
  318. hsr_register_frame_out(port, frame->node_src,
  319. frame->sequence_nr))
  320. continue;
  321. if (frame->is_supervision && port->type == HSR_PT_MASTER) {
  322. hsr_handle_sup_frame(frame);
  323. continue;
  324. }
  325. /* Check if frame is to be dropped. Eg. for PRP no forward
  326. * between ports.
  327. */
  328. if (hsr->proto_ops->drop_frame &&
  329. hsr->proto_ops->drop_frame(frame, port))
  330. continue;
  331. if (port->type != HSR_PT_MASTER)
  332. skb = hsr->proto_ops->create_tagged_frame(frame, port);
  333. else
  334. skb = hsr->proto_ops->get_untagged_frame(frame, port);
  335. if (!skb) {
  336. frame->port_rcv->dev->stats.rx_dropped++;
  337. continue;
  338. }
  339. skb->dev = port->dev;
  340. if (port->type == HSR_PT_MASTER)
  341. hsr_deliver_master(skb, port->dev, frame->node_src);
  342. else
  343. hsr_xmit(skb, port, frame);
  344. }
  345. }
  346. static void check_local_dest(struct hsr_priv *hsr, struct sk_buff *skb,
  347. struct hsr_frame_info *frame)
  348. {
  349. if (hsr_addr_is_self(hsr, eth_hdr(skb)->h_dest)) {
  350. frame->is_local_exclusive = true;
  351. skb->pkt_type = PACKET_HOST;
  352. } else {
  353. frame->is_local_exclusive = false;
  354. }
  355. if (skb->pkt_type == PACKET_HOST ||
  356. skb->pkt_type == PACKET_MULTICAST ||
  357. skb->pkt_type == PACKET_BROADCAST) {
  358. frame->is_local_dest = true;
  359. } else {
  360. frame->is_local_dest = false;
  361. }
  362. }
  363. static void handle_std_frame(struct sk_buff *skb,
  364. struct hsr_frame_info *frame)
  365. {
  366. struct hsr_port *port = frame->port_rcv;
  367. struct hsr_priv *hsr = port->hsr;
  368. unsigned long irqflags;
  369. frame->skb_hsr = NULL;
  370. frame->skb_prp = NULL;
  371. frame->skb_std = skb;
  372. if (port->type != HSR_PT_MASTER) {
  373. frame->is_from_san = true;
  374. } else {
  375. /* Sequence nr for the master node */
  376. spin_lock_irqsave(&hsr->seqnr_lock, irqflags);
  377. frame->sequence_nr = hsr->sequence_nr;
  378. hsr->sequence_nr++;
  379. spin_unlock_irqrestore(&hsr->seqnr_lock, irqflags);
  380. }
  381. }
  382. int hsr_fill_frame_info(__be16 proto, struct sk_buff *skb,
  383. struct hsr_frame_info *frame)
  384. {
  385. if (proto == htons(ETH_P_PRP) ||
  386. proto == htons(ETH_P_HSR)) {
  387. /* Check if skb contains hsr_ethhdr */
  388. if (skb->mac_len < sizeof(struct hsr_ethhdr))
  389. return -EINVAL;
  390. /* HSR tagged frame :- Data or Supervision */
  391. frame->skb_std = NULL;
  392. frame->skb_prp = NULL;
  393. frame->skb_hsr = skb;
  394. frame->sequence_nr = hsr_get_skb_sequence_nr(skb);
  395. return 0;
  396. }
  397. /* Standard frame or PRP from master port */
  398. handle_std_frame(skb, frame);
  399. return 0;
  400. }
  401. int prp_fill_frame_info(__be16 proto, struct sk_buff *skb,
  402. struct hsr_frame_info *frame)
  403. {
  404. /* Supervision frame */
  405. struct prp_rct *rct = skb_get_PRP_rct(skb);
  406. if (rct &&
  407. prp_check_lsdu_size(skb, rct, frame->is_supervision)) {
  408. frame->skb_hsr = NULL;
  409. frame->skb_std = NULL;
  410. frame->skb_prp = skb;
  411. frame->sequence_nr = prp_get_skb_sequence_nr(rct);
  412. return 0;
  413. }
  414. handle_std_frame(skb, frame);
  415. return 0;
  416. }
  417. static int fill_frame_info(struct hsr_frame_info *frame,
  418. struct sk_buff *skb, struct hsr_port *port)
  419. {
  420. struct hsr_priv *hsr = port->hsr;
  421. struct hsr_vlan_ethhdr *vlan_hdr;
  422. struct ethhdr *ethhdr;
  423. __be16 proto;
  424. int ret;
  425. /* Check if skb contains ethhdr */
  426. if (skb->mac_len < sizeof(struct ethhdr))
  427. return -EINVAL;
  428. memset(frame, 0, sizeof(*frame));
  429. frame->is_supervision = is_supervision_frame(port->hsr, skb);
  430. frame->node_src = hsr_get_node(port, &hsr->node_db, skb,
  431. frame->is_supervision,
  432. port->type);
  433. if (!frame->node_src)
  434. return -1; /* Unknown node and !is_supervision, or no mem */
  435. ethhdr = (struct ethhdr *)skb_mac_header(skb);
  436. frame->is_vlan = false;
  437. proto = ethhdr->h_proto;
  438. if (proto == htons(ETH_P_8021Q))
  439. frame->is_vlan = true;
  440. if (frame->is_vlan) {
  441. vlan_hdr = (struct hsr_vlan_ethhdr *)ethhdr;
  442. proto = vlan_hdr->vlanhdr.h_vlan_encapsulated_proto;
  443. /* FIXME: */
  444. netdev_warn_once(skb->dev, "VLAN not yet supported");
  445. }
  446. frame->is_from_san = false;
  447. frame->port_rcv = port;
  448. ret = hsr->proto_ops->fill_frame_info(proto, skb, frame);
  449. if (ret)
  450. return ret;
  451. check_local_dest(port->hsr, skb, frame);
  452. return 0;
  453. }
  454. /* Must be called holding rcu read lock (because of the port parameter) */
  455. void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port)
  456. {
  457. struct hsr_frame_info frame;
  458. if (fill_frame_info(&frame, skb, port) < 0)
  459. goto out_drop;
  460. hsr_register_frame_in(frame.node_src, port, frame.sequence_nr);
  461. hsr_forward_do(&frame);
  462. /* Gets called for ingress frames as well as egress from master port.
  463. * So check and increment stats for master port only here.
  464. */
  465. if (port->type == HSR_PT_MASTER) {
  466. port->dev->stats.tx_packets++;
  467. port->dev->stats.tx_bytes += skb->len;
  468. }
  469. kfree_skb(frame.skb_hsr);
  470. kfree_skb(frame.skb_prp);
  471. kfree_skb(frame.skb_std);
  472. return;
  473. out_drop:
  474. port->dev->stats.tx_dropped++;
  475. kfree_skb(skb);
  476. }