tag_sja1105.c 10 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373
  1. // SPDX-License-Identifier: GPL-2.0
  2. /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
  3. */
  4. #include <linux/if_vlan.h>
  5. #include <linux/dsa/sja1105.h>
  6. #include <linux/dsa/8021q.h>
  7. #include <linux/packing.h>
  8. #include "dsa_priv.h"
  9. /* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */
  10. static inline bool sja1105_is_link_local(const struct sk_buff *skb)
  11. {
  12. const struct ethhdr *hdr = eth_hdr(skb);
  13. u64 dmac = ether_addr_to_u64(hdr->h_dest);
  14. if (ntohs(hdr->h_proto) == ETH_P_SJA1105_META)
  15. return false;
  16. if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) ==
  17. SJA1105_LINKLOCAL_FILTER_A)
  18. return true;
  19. if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) ==
  20. SJA1105_LINKLOCAL_FILTER_B)
  21. return true;
  22. return false;
  23. }
  24. struct sja1105_meta {
  25. u64 tstamp;
  26. u64 dmac_byte_4;
  27. u64 dmac_byte_3;
  28. u64 source_port;
  29. u64 switch_id;
  30. };
  31. static void sja1105_meta_unpack(const struct sk_buff *skb,
  32. struct sja1105_meta *meta)
  33. {
  34. u8 *buf = skb_mac_header(skb) + ETH_HLEN;
  35. /* UM10944.pdf section 4.2.17 AVB Parameters:
  36. * Structure of the meta-data follow-up frame.
  37. * It is in network byte order, so there are no quirks
  38. * while unpacking the meta frame.
  39. *
  40. * Also SJA1105 E/T only populates bits 23:0 of the timestamp
  41. * whereas P/Q/R/S does 32 bits. Since the structure is the
  42. * same and the E/T puts zeroes in the high-order byte, use
  43. * a unified unpacking command for both device series.
  44. */
  45. packing(buf, &meta->tstamp, 31, 0, 4, UNPACK, 0);
  46. packing(buf + 4, &meta->dmac_byte_4, 7, 0, 1, UNPACK, 0);
  47. packing(buf + 5, &meta->dmac_byte_3, 7, 0, 1, UNPACK, 0);
  48. packing(buf + 6, &meta->source_port, 7, 0, 1, UNPACK, 0);
  49. packing(buf + 7, &meta->switch_id, 7, 0, 1, UNPACK, 0);
  50. }
  51. static inline bool sja1105_is_meta_frame(const struct sk_buff *skb)
  52. {
  53. const struct ethhdr *hdr = eth_hdr(skb);
  54. u64 smac = ether_addr_to_u64(hdr->h_source);
  55. u64 dmac = ether_addr_to_u64(hdr->h_dest);
  56. if (smac != SJA1105_META_SMAC)
  57. return false;
  58. if (dmac != SJA1105_META_DMAC)
  59. return false;
  60. if (ntohs(hdr->h_proto) != ETH_P_SJA1105_META)
  61. return false;
  62. return true;
  63. }
  64. static bool sja1105_can_use_vlan_as_tags(const struct sk_buff *skb)
  65. {
  66. struct vlan_ethhdr *hdr = vlan_eth_hdr(skb);
  67. u16 vlan_tci;
  68. if (hdr->h_vlan_proto == htons(ETH_P_SJA1105))
  69. return true;
  70. if (hdr->h_vlan_proto != htons(ETH_P_8021Q) &&
  71. !skb_vlan_tag_present(skb))
  72. return false;
  73. if (skb_vlan_tag_present(skb))
  74. vlan_tci = skb_vlan_tag_get(skb);
  75. else
  76. vlan_tci = ntohs(hdr->h_vlan_TCI);
  77. return vid_is_dsa_8021q(vlan_tci & VLAN_VID_MASK);
  78. }
  79. /* This is the first time the tagger sees the frame on RX.
  80. * Figure out if we can decode it.
  81. */
  82. static bool sja1105_filter(const struct sk_buff *skb, struct net_device *dev)
  83. {
  84. if (sja1105_can_use_vlan_as_tags(skb))
  85. return true;
  86. if (sja1105_is_link_local(skb))
  87. return true;
  88. if (sja1105_is_meta_frame(skb))
  89. return true;
  90. return false;
  91. }
  92. /* Calls sja1105_port_deferred_xmit in sja1105_main.c */
  93. static struct sk_buff *sja1105_defer_xmit(struct sja1105_port *sp,
  94. struct sk_buff *skb)
  95. {
  96. /* Increase refcount so the kfree_skb in dsa_slave_xmit
  97. * won't really free the packet.
  98. */
  99. skb_queue_tail(&sp->xmit_queue, skb_get(skb));
  100. kthread_queue_work(sp->xmit_worker, &sp->xmit_work);
  101. return NULL;
  102. }
  103. static u16 sja1105_xmit_tpid(struct sja1105_port *sp)
  104. {
  105. return sp->xmit_tpid;
  106. }
  107. static struct sk_buff *sja1105_xmit(struct sk_buff *skb,
  108. struct net_device *netdev)
  109. {
  110. struct dsa_port *dp = dsa_slave_to_port(netdev);
  111. u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index);
  112. u16 queue_mapping = skb_get_queue_mapping(skb);
  113. u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
  114. /* Transmitting management traffic does not rely upon switch tagging,
  115. * but instead SPI-installed management routes. Part 2 of this
  116. * is the .port_deferred_xmit driver callback.
  117. */
  118. if (unlikely(sja1105_is_link_local(skb)))
  119. return sja1105_defer_xmit(dp->priv, skb);
  120. return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp->priv),
  121. ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
  122. }
  123. static void sja1105_transfer_meta(struct sk_buff *skb,
  124. const struct sja1105_meta *meta)
  125. {
  126. struct ethhdr *hdr = eth_hdr(skb);
  127. hdr->h_dest[3] = meta->dmac_byte_3;
  128. hdr->h_dest[4] = meta->dmac_byte_4;
  129. SJA1105_SKB_CB(skb)->meta_tstamp = meta->tstamp;
  130. }
  131. /* This is a simple state machine which follows the hardware mechanism of
  132. * generating RX timestamps:
  133. *
  134. * After each timestampable skb (all traffic for which send_meta1 and
  135. * send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame
  136. * containing a partial timestamp is immediately generated by the switch and
  137. * sent as a follow-up to the link-local frame on the CPU port.
  138. *
  139. * The meta frames have no unique identifier (such as sequence number) by which
  140. * one may pair them to the correct timestampable frame.
  141. * Instead, the switch has internal logic that ensures no frames are sent on
  142. * the CPU port between a link-local timestampable frame and its corresponding
  143. * meta follow-up. It also ensures strict ordering between ports (lower ports
  144. * have higher priority towards the CPU port). For this reason, a per-port
  145. * data structure is not needed/desirable.
  146. *
  147. * This function pairs the link-local frame with its partial timestamp from the
  148. * meta follow-up frame. The full timestamp will be reconstructed later in a
  149. * work queue.
  150. */
  151. static struct sk_buff
  152. *sja1105_rcv_meta_state_machine(struct sk_buff *skb,
  153. struct sja1105_meta *meta,
  154. bool is_link_local,
  155. bool is_meta)
  156. {
  157. struct sja1105_port *sp;
  158. struct dsa_port *dp;
  159. dp = dsa_slave_to_port(skb->dev);
  160. sp = dp->priv;
  161. /* Step 1: A timestampable frame was received.
  162. * Buffer it until we get its meta frame.
  163. */
  164. if (is_link_local) {
  165. if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state))
  166. /* Do normal processing. */
  167. return skb;
  168. spin_lock(&sp->data->meta_lock);
  169. /* Was this a link-local frame instead of the meta
  170. * that we were expecting?
  171. */
  172. if (sp->data->stampable_skb) {
  173. dev_err_ratelimited(dp->ds->dev,
  174. "Expected meta frame, is %12llx "
  175. "in the DSA master multicast filter?\n",
  176. SJA1105_META_DMAC);
  177. kfree_skb(sp->data->stampable_skb);
  178. }
  179. /* Hold a reference to avoid dsa_switch_rcv
  180. * from freeing the skb.
  181. */
  182. sp->data->stampable_skb = skb_get(skb);
  183. spin_unlock(&sp->data->meta_lock);
  184. /* Tell DSA we got nothing */
  185. return NULL;
  186. /* Step 2: The meta frame arrived.
  187. * Time to take the stampable skb out of the closet, annotate it
  188. * with the partial timestamp, and pretend that we received it
  189. * just now (basically masquerade the buffered frame as the meta
  190. * frame, which serves no further purpose).
  191. */
  192. } else if (is_meta) {
  193. struct sk_buff *stampable_skb;
  194. /* Drop the meta frame if we're not in the right state
  195. * to process it.
  196. */
  197. if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state))
  198. return NULL;
  199. spin_lock(&sp->data->meta_lock);
  200. stampable_skb = sp->data->stampable_skb;
  201. sp->data->stampable_skb = NULL;
  202. /* Was this a meta frame instead of the link-local
  203. * that we were expecting?
  204. */
  205. if (!stampable_skb) {
  206. dev_err_ratelimited(dp->ds->dev,
  207. "Unexpected meta frame\n");
  208. spin_unlock(&sp->data->meta_lock);
  209. return NULL;
  210. }
  211. if (stampable_skb->dev != skb->dev) {
  212. dev_err_ratelimited(dp->ds->dev,
  213. "Meta frame on wrong port\n");
  214. spin_unlock(&sp->data->meta_lock);
  215. return NULL;
  216. }
  217. /* Free the meta frame and give DSA the buffered stampable_skb
  218. * for further processing up the network stack.
  219. */
  220. kfree_skb(skb);
  221. skb = stampable_skb;
  222. sja1105_transfer_meta(skb, meta);
  223. spin_unlock(&sp->data->meta_lock);
  224. }
  225. return skb;
  226. }
  227. static void sja1105_decode_subvlan(struct sk_buff *skb, u16 subvlan)
  228. {
  229. struct dsa_port *dp = dsa_slave_to_port(skb->dev);
  230. struct sja1105_port *sp = dp->priv;
  231. u16 vid = sp->subvlan_map[subvlan];
  232. u16 vlan_tci;
  233. if (vid == VLAN_N_VID)
  234. return;
  235. vlan_tci = (skb->priority << VLAN_PRIO_SHIFT) | vid;
  236. __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci);
  237. }
  238. static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
  239. struct net_device *netdev,
  240. struct packet_type *pt)
  241. {
  242. struct sja1105_meta meta = {0};
  243. int source_port, switch_id;
  244. struct ethhdr *hdr;
  245. u16 tpid, vid, tci;
  246. bool is_link_local;
  247. u16 subvlan = 0;
  248. bool is_tagged;
  249. bool is_meta;
  250. hdr = eth_hdr(skb);
  251. tpid = ntohs(hdr->h_proto);
  252. is_tagged = (tpid == ETH_P_SJA1105 || tpid == ETH_P_8021Q ||
  253. skb_vlan_tag_present(skb));
  254. is_link_local = sja1105_is_link_local(skb);
  255. is_meta = sja1105_is_meta_frame(skb);
  256. skb->offload_fwd_mark = 1;
  257. if (is_tagged) {
  258. /* Normal traffic path. */
  259. skb_push_rcsum(skb, ETH_HLEN);
  260. if (skb_vlan_tag_present(skb)) {
  261. tci = skb_vlan_tag_get(skb);
  262. __vlan_hwaccel_clear_tag(skb);
  263. } else {
  264. __skb_vlan_pop(skb, &tci);
  265. }
  266. skb_pull_rcsum(skb, ETH_HLEN);
  267. skb_reset_network_header(skb);
  268. skb_reset_transport_header(skb);
  269. vid = tci & VLAN_VID_MASK;
  270. source_port = dsa_8021q_rx_source_port(vid);
  271. switch_id = dsa_8021q_rx_switch_id(vid);
  272. skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
  273. subvlan = dsa_8021q_rx_subvlan(vid);
  274. } else if (is_link_local) {
  275. /* Management traffic path. Switch embeds the switch ID and
  276. * port ID into bytes of the destination MAC, courtesy of
  277. * the incl_srcpt options.
  278. */
  279. source_port = hdr->h_dest[3];
  280. switch_id = hdr->h_dest[4];
  281. /* Clear the DMAC bytes that were mangled by the switch */
  282. hdr->h_dest[3] = 0;
  283. hdr->h_dest[4] = 0;
  284. } else if (is_meta) {
  285. sja1105_meta_unpack(skb, &meta);
  286. source_port = meta.source_port;
  287. switch_id = meta.switch_id;
  288. } else {
  289. return NULL;
  290. }
  291. skb->dev = dsa_master_find_slave(netdev, switch_id, source_port);
  292. if (!skb->dev) {
  293. netdev_warn(netdev, "Couldn't decode source port\n");
  294. return NULL;
  295. }
  296. if (subvlan)
  297. sja1105_decode_subvlan(skb, subvlan);
  298. return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local,
  299. is_meta);
  300. }
  301. static void sja1105_flow_dissect(const struct sk_buff *skb, __be16 *proto,
  302. int *offset)
  303. {
  304. /* No tag added for management frames, all ok */
  305. if (unlikely(sja1105_is_link_local(skb)))
  306. return;
  307. dsa_tag_generic_flow_dissect(skb, proto, offset);
  308. }
  309. static const struct dsa_device_ops sja1105_netdev_ops = {
  310. .name = "sja1105",
  311. .proto = DSA_TAG_PROTO_SJA1105,
  312. .xmit = sja1105_xmit,
  313. .rcv = sja1105_rcv,
  314. .filter = sja1105_filter,
  315. .overhead = VLAN_HLEN,
  316. .flow_dissect = sja1105_flow_dissect,
  317. .promisc_on_master = true,
  318. };
  319. MODULE_LICENSE("GPL v2");
  320. MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105);
  321. module_dsa_tag_driver(sja1105_netdev_ops);