hsr_framereg.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502
  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. * The HSR spec says never to forward the same frame twice on the same
  12. * interface. A frame is identified by its source MAC address and its HSR
  13. * sequence number. This code keeps track of senders and their sequence numbers
  14. * to allow filtering of duplicate frames, and to detect HSR ring errors.
  15. */
  16. #include <linux/if_ether.h>
  17. #include <linux/etherdevice.h>
  18. #include <linux/slab.h>
  19. #include <linux/rculist.h>
  20. #include "hsr_main.h"
  21. #include "hsr_framereg.h"
  22. #include "hsr_netlink.h"
  23. struct hsr_node {
  24. struct list_head mac_list;
  25. unsigned char MacAddressA[ETH_ALEN];
  26. unsigned char MacAddressB[ETH_ALEN];
  27. /* Local slave through which AddrB frames are received from this node */
  28. enum hsr_port_type AddrB_port;
  29. unsigned long time_in[HSR_PT_PORTS];
  30. bool time_in_stale[HSR_PT_PORTS];
  31. u16 seq_out[HSR_PT_PORTS];
  32. struct rcu_head rcu_head;
  33. };
  34. /* TODO: use hash lists for mac addresses (linux/jhash.h)? */
  35. /* seq_nr_after(a, b) - return true if a is after (higher in sequence than) b,
  36. * false otherwise.
  37. */
  38. static bool seq_nr_after(u16 a, u16 b)
  39. {
  40. /* Remove inconsistency where
  41. * seq_nr_after(a, b) == seq_nr_before(a, b)
  42. */
  43. if ((int) b - a == 32768)
  44. return false;
  45. return (((s16) (b - a)) < 0);
  46. }
  47. #define seq_nr_before(a, b) seq_nr_after((b), (a))
  48. #define seq_nr_after_or_eq(a, b) (!seq_nr_before((a), (b)))
  49. #define seq_nr_before_or_eq(a, b) (!seq_nr_after((a), (b)))
  50. bool hsr_addr_is_self(struct hsr_priv *hsr, unsigned char *addr)
  51. {
  52. struct hsr_node *node;
  53. node = list_first_or_null_rcu(&hsr->self_node_db, struct hsr_node,
  54. mac_list);
  55. if (!node) {
  56. WARN_ONCE(1, "HSR: No self node\n");
  57. return false;
  58. }
  59. if (ether_addr_equal(addr, node->MacAddressA))
  60. return true;
  61. if (ether_addr_equal(addr, node->MacAddressB))
  62. return true;
  63. return false;
  64. }
  65. /* Search for mac entry. Caller must hold rcu read lock.
  66. */
  67. static struct hsr_node *find_node_by_AddrA(struct list_head *node_db,
  68. const unsigned char addr[ETH_ALEN])
  69. {
  70. struct hsr_node *node;
  71. list_for_each_entry_rcu(node, node_db, mac_list) {
  72. if (ether_addr_equal(node->MacAddressA, addr))
  73. return node;
  74. }
  75. return NULL;
  76. }
  77. /* Helper for device init; the self_node_db is used in hsr_rcv() to recognize
  78. * frames from self that's been looped over the HSR ring.
  79. */
  80. int hsr_create_self_node(struct list_head *self_node_db,
  81. unsigned char addr_a[ETH_ALEN],
  82. unsigned char addr_b[ETH_ALEN])
  83. {
  84. struct hsr_node *node, *oldnode;
  85. node = kmalloc(sizeof(*node), GFP_KERNEL);
  86. if (!node)
  87. return -ENOMEM;
  88. ether_addr_copy(node->MacAddressA, addr_a);
  89. ether_addr_copy(node->MacAddressB, addr_b);
  90. rcu_read_lock();
  91. oldnode = list_first_or_null_rcu(self_node_db,
  92. struct hsr_node, mac_list);
  93. if (oldnode) {
  94. list_replace_rcu(&oldnode->mac_list, &node->mac_list);
  95. rcu_read_unlock();
  96. synchronize_rcu();
  97. kfree(oldnode);
  98. } else {
  99. rcu_read_unlock();
  100. list_add_tail_rcu(&node->mac_list, self_node_db);
  101. }
  102. return 0;
  103. }
  104. void hsr_del_node(struct list_head *self_node_db)
  105. {
  106. struct hsr_node *node;
  107. rcu_read_lock();
  108. node = list_first_or_null_rcu(self_node_db, struct hsr_node, mac_list);
  109. rcu_read_unlock();
  110. if (node) {
  111. list_del_rcu(&node->mac_list);
  112. kfree(node);
  113. }
  114. }
  115. /* Allocate an hsr_node and add it to node_db. 'addr' is the node's AddressA;
  116. * seq_out is used to initialize filtering of outgoing duplicate frames
  117. * originating from the newly added node.
  118. */
  119. struct hsr_node *hsr_add_node(struct list_head *node_db, unsigned char addr[],
  120. u16 seq_out)
  121. {
  122. struct hsr_node *node;
  123. unsigned long now;
  124. int i;
  125. node = kzalloc(sizeof(*node), GFP_ATOMIC);
  126. if (!node)
  127. return NULL;
  128. ether_addr_copy(node->MacAddressA, addr);
  129. /* We are only interested in time diffs here, so use current jiffies
  130. * as initialization. (0 could trigger an spurious ring error warning).
  131. */
  132. now = jiffies;
  133. for (i = 0; i < HSR_PT_PORTS; i++)
  134. node->time_in[i] = now;
  135. for (i = 0; i < HSR_PT_PORTS; i++)
  136. node->seq_out[i] = seq_out;
  137. list_add_tail_rcu(&node->mac_list, node_db);
  138. return node;
  139. }
  140. /* Get the hsr_node from which 'skb' was sent.
  141. */
  142. struct hsr_node *hsr_get_node(struct list_head *node_db, struct sk_buff *skb,
  143. bool is_sup)
  144. {
  145. struct hsr_node *node;
  146. struct ethhdr *ethhdr;
  147. u16 seq_out;
  148. if (!skb_mac_header_was_set(skb))
  149. return NULL;
  150. ethhdr = (struct ethhdr *) skb_mac_header(skb);
  151. list_for_each_entry_rcu(node, node_db, mac_list) {
  152. if (ether_addr_equal(node->MacAddressA, ethhdr->h_source))
  153. return node;
  154. if (ether_addr_equal(node->MacAddressB, ethhdr->h_source))
  155. return node;
  156. }
  157. if (!is_sup)
  158. return NULL; /* Only supervision frame may create node entry */
  159. if (ethhdr->h_proto == htons(ETH_P_PRP)) {
  160. /* Use the existing sequence_nr from the tag as starting point
  161. * for filtering duplicate frames.
  162. */
  163. seq_out = hsr_get_skb_sequence_nr(skb) - 1;
  164. } else {
  165. WARN_ONCE(1, "%s: Non-HSR frame\n", __func__);
  166. seq_out = 0;
  167. }
  168. return hsr_add_node(node_db, ethhdr->h_source, seq_out);
  169. }
  170. /* Use the Supervision frame's info about an eventual MacAddressB for merging
  171. * nodes that has previously had their MacAddressB registered as a separate
  172. * node.
  173. */
  174. void hsr_handle_sup_frame(struct sk_buff *skb, struct hsr_node *node_curr,
  175. struct hsr_port *port_rcv)
  176. {
  177. struct hsr_node *node_real;
  178. struct hsr_sup_payload *hsr_sp;
  179. struct list_head *node_db;
  180. int i;
  181. skb_pull(skb, sizeof(struct hsr_ethhdr_sp));
  182. hsr_sp = (struct hsr_sup_payload *) skb->data;
  183. if (ether_addr_equal(eth_hdr(skb)->h_source, hsr_sp->MacAddressA))
  184. /* Not sent from MacAddressB of a PICS_SUBS capable node */
  185. goto done;
  186. /* Merge node_curr (registered on MacAddressB) into node_real */
  187. node_db = &port_rcv->hsr->node_db;
  188. node_real = find_node_by_AddrA(node_db, hsr_sp->MacAddressA);
  189. if (!node_real)
  190. /* No frame received from AddrA of this node yet */
  191. node_real = hsr_add_node(node_db, hsr_sp->MacAddressA,
  192. HSR_SEQNR_START - 1);
  193. if (!node_real)
  194. goto done; /* No mem */
  195. if (node_real == node_curr)
  196. /* Node has already been merged */
  197. goto done;
  198. ether_addr_copy(node_real->MacAddressB, eth_hdr(skb)->h_source);
  199. for (i = 0; i < HSR_PT_PORTS; i++) {
  200. if (!node_curr->time_in_stale[i] &&
  201. time_after(node_curr->time_in[i], node_real->time_in[i])) {
  202. node_real->time_in[i] = node_curr->time_in[i];
  203. node_real->time_in_stale[i] = node_curr->time_in_stale[i];
  204. }
  205. if (seq_nr_after(node_curr->seq_out[i], node_real->seq_out[i]))
  206. node_real->seq_out[i] = node_curr->seq_out[i];
  207. }
  208. node_real->AddrB_port = port_rcv->type;
  209. list_del_rcu(&node_curr->mac_list);
  210. kfree_rcu(node_curr, rcu_head);
  211. done:
  212. skb_push(skb, sizeof(struct hsr_ethhdr_sp));
  213. }
  214. /* 'skb' is a frame meant for this host, that is to be passed to upper layers.
  215. *
  216. * If the frame was sent by a node's B interface, replace the source
  217. * address with that node's "official" address (MacAddressA) so that upper
  218. * layers recognize where it came from.
  219. */
  220. void hsr_addr_subst_source(struct hsr_node *node, struct sk_buff *skb)
  221. {
  222. if (!skb_mac_header_was_set(skb)) {
  223. WARN_ONCE(1, "%s: Mac header not set\n", __func__);
  224. return;
  225. }
  226. memcpy(&eth_hdr(skb)->h_source, node->MacAddressA, ETH_ALEN);
  227. }
  228. /* 'skb' is a frame meant for another host.
  229. * 'port' is the outgoing interface
  230. *
  231. * Substitute the target (dest) MAC address if necessary, so the it matches the
  232. * recipient interface MAC address, regardless of whether that is the
  233. * recipient's A or B interface.
  234. * This is needed to keep the packets flowing through switches that learn on
  235. * which "side" the different interfaces are.
  236. */
  237. void hsr_addr_subst_dest(struct hsr_node *node_src, struct sk_buff *skb,
  238. struct hsr_port *port)
  239. {
  240. struct hsr_node *node_dst;
  241. if (!skb_mac_header_was_set(skb)) {
  242. WARN_ONCE(1, "%s: Mac header not set\n", __func__);
  243. return;
  244. }
  245. if (!is_unicast_ether_addr(eth_hdr(skb)->h_dest))
  246. return;
  247. node_dst = find_node_by_AddrA(&port->hsr->node_db, eth_hdr(skb)->h_dest);
  248. if (!node_dst) {
  249. WARN_ONCE(1, "%s: Unknown node\n", __func__);
  250. return;
  251. }
  252. if (port->type != node_dst->AddrB_port)
  253. return;
  254. ether_addr_copy(eth_hdr(skb)->h_dest, node_dst->MacAddressB);
  255. }
  256. void hsr_register_frame_in(struct hsr_node *node, struct hsr_port *port,
  257. u16 sequence_nr)
  258. {
  259. /* Don't register incoming frames without a valid sequence number. This
  260. * ensures entries of restarted nodes gets pruned so that they can
  261. * re-register and resume communications.
  262. */
  263. if (seq_nr_before(sequence_nr, node->seq_out[port->type]))
  264. return;
  265. node->time_in[port->type] = jiffies;
  266. node->time_in_stale[port->type] = false;
  267. }
  268. /* 'skb' is a HSR Ethernet frame (with a HSR tag inserted), with a valid
  269. * ethhdr->h_source address and skb->mac_header set.
  270. *
  271. * Return:
  272. * 1 if frame can be shown to have been sent recently on this interface,
  273. * 0 otherwise, or
  274. * negative error code on error
  275. */
  276. int hsr_register_frame_out(struct hsr_port *port, struct hsr_node *node,
  277. u16 sequence_nr)
  278. {
  279. if (seq_nr_before_or_eq(sequence_nr, node->seq_out[port->type]))
  280. return 1;
  281. node->seq_out[port->type] = sequence_nr;
  282. return 0;
  283. }
  284. static struct hsr_port *get_late_port(struct hsr_priv *hsr,
  285. struct hsr_node *node)
  286. {
  287. if (node->time_in_stale[HSR_PT_SLAVE_A])
  288. return hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
  289. if (node->time_in_stale[HSR_PT_SLAVE_B])
  290. return hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
  291. if (time_after(node->time_in[HSR_PT_SLAVE_B],
  292. node->time_in[HSR_PT_SLAVE_A] +
  293. msecs_to_jiffies(MAX_SLAVE_DIFF)))
  294. return hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
  295. if (time_after(node->time_in[HSR_PT_SLAVE_A],
  296. node->time_in[HSR_PT_SLAVE_B] +
  297. msecs_to_jiffies(MAX_SLAVE_DIFF)))
  298. return hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
  299. return NULL;
  300. }
  301. /* Remove stale sequence_nr records. Called by timer every
  302. * HSR_LIFE_CHECK_INTERVAL (two seconds or so).
  303. */
  304. void hsr_prune_nodes(unsigned long data)
  305. {
  306. struct hsr_priv *hsr;
  307. struct hsr_node *node;
  308. struct hsr_port *port;
  309. unsigned long timestamp;
  310. unsigned long time_a, time_b;
  311. hsr = (struct hsr_priv *) data;
  312. rcu_read_lock();
  313. list_for_each_entry_rcu(node, &hsr->node_db, mac_list) {
  314. /* Shorthand */
  315. time_a = node->time_in[HSR_PT_SLAVE_A];
  316. time_b = node->time_in[HSR_PT_SLAVE_B];
  317. /* Check for timestamps old enough to risk wrap-around */
  318. if (time_after(jiffies, time_a + MAX_JIFFY_OFFSET/2))
  319. node->time_in_stale[HSR_PT_SLAVE_A] = true;
  320. if (time_after(jiffies, time_b + MAX_JIFFY_OFFSET/2))
  321. node->time_in_stale[HSR_PT_SLAVE_B] = true;
  322. /* Get age of newest frame from node.
  323. * At least one time_in is OK here; nodes get pruned long
  324. * before both time_ins can get stale
  325. */
  326. timestamp = time_a;
  327. if (node->time_in_stale[HSR_PT_SLAVE_A] ||
  328. (!node->time_in_stale[HSR_PT_SLAVE_B] &&
  329. time_after(time_b, time_a)))
  330. timestamp = time_b;
  331. /* Warn of ring error only as long as we get frames at all */
  332. if (time_is_after_jiffies(timestamp +
  333. msecs_to_jiffies(1.5*MAX_SLAVE_DIFF))) {
  334. rcu_read_lock();
  335. port = get_late_port(hsr, node);
  336. if (port != NULL)
  337. hsr_nl_ringerror(hsr, node->MacAddressA, port);
  338. rcu_read_unlock();
  339. }
  340. /* Prune old entries */
  341. if (time_is_before_jiffies(timestamp +
  342. msecs_to_jiffies(HSR_NODE_FORGET_TIME))) {
  343. hsr_nl_nodedown(hsr, node->MacAddressA);
  344. list_del_rcu(&node->mac_list);
  345. /* Note that we need to free this entry later: */
  346. kfree_rcu(node, rcu_head);
  347. }
  348. }
  349. rcu_read_unlock();
  350. }
  351. void *hsr_get_next_node(struct hsr_priv *hsr, void *_pos,
  352. unsigned char addr[ETH_ALEN])
  353. {
  354. struct hsr_node *node;
  355. if (!_pos) {
  356. node = list_first_or_null_rcu(&hsr->node_db,
  357. struct hsr_node, mac_list);
  358. if (node)
  359. ether_addr_copy(addr, node->MacAddressA);
  360. return node;
  361. }
  362. node = _pos;
  363. list_for_each_entry_continue_rcu(node, &hsr->node_db, mac_list) {
  364. ether_addr_copy(addr, node->MacAddressA);
  365. return node;
  366. }
  367. return NULL;
  368. }
  369. int hsr_get_node_data(struct hsr_priv *hsr,
  370. const unsigned char *addr,
  371. unsigned char addr_b[ETH_ALEN],
  372. unsigned int *addr_b_ifindex,
  373. int *if1_age,
  374. u16 *if1_seq,
  375. int *if2_age,
  376. u16 *if2_seq)
  377. {
  378. struct hsr_node *node;
  379. struct hsr_port *port;
  380. unsigned long tdiff;
  381. rcu_read_lock();
  382. node = find_node_by_AddrA(&hsr->node_db, addr);
  383. if (!node) {
  384. rcu_read_unlock();
  385. return -ENOENT; /* No such entry */
  386. }
  387. ether_addr_copy(addr_b, node->MacAddressB);
  388. tdiff = jiffies - node->time_in[HSR_PT_SLAVE_A];
  389. if (node->time_in_stale[HSR_PT_SLAVE_A])
  390. *if1_age = INT_MAX;
  391. #if HZ <= MSEC_PER_SEC
  392. else if (tdiff > msecs_to_jiffies(INT_MAX))
  393. *if1_age = INT_MAX;
  394. #endif
  395. else
  396. *if1_age = jiffies_to_msecs(tdiff);
  397. tdiff = jiffies - node->time_in[HSR_PT_SLAVE_B];
  398. if (node->time_in_stale[HSR_PT_SLAVE_B])
  399. *if2_age = INT_MAX;
  400. #if HZ <= MSEC_PER_SEC
  401. else if (tdiff > msecs_to_jiffies(INT_MAX))
  402. *if2_age = INT_MAX;
  403. #endif
  404. else
  405. *if2_age = jiffies_to_msecs(tdiff);
  406. /* Present sequence numbers as if they were incoming on interface */
  407. *if1_seq = node->seq_out[HSR_PT_SLAVE_B];
  408. *if2_seq = node->seq_out[HSR_PT_SLAVE_A];
  409. if (node->AddrB_port != HSR_PT_NONE) {
  410. port = hsr_port_get_hsr(hsr, node->AddrB_port);
  411. *addr_b_ifindex = port->dev->ifindex;
  412. } else {
  413. *addr_b_ifindex = -1;
  414. }
  415. rcu_read_unlock();
  416. return 0;
  417. }