Skip to content

Commit 452d00d

Browse files
jukkarjukkar
committed
net: gptp: Allow gPTP to run over VLAN
Allow this setup as Linux supports this too. Signed-off-by: Jukka Rissanen <[email protected]>
1 parent f3643e5 commit 452d00d

File tree

1 file changed

+184
-6
lines changed

1 file changed

+184
-6
lines changed

‎subsys/net/ip/l2/gptp/gptp_messages.c‎

Lines changed: 184 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -111,10 +111,25 @@ struct net_pkt *gptp_prepare_sync(int port)
111111
struct net_buf *frag;
112112
struct gptp_hdr *hdr;
113113

114+
#if defined(CONFIG_NET_VLAN)
115+
struct net_eth_vlan_hdr *hdr_vlan;
116+
struct ethernet_context *eth_ctx;
117+
bool vlan_enabled = false;
118+
#endif
119+
114120
NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
115121
iface = GPTP_PORT_IFACE(port);
116122
NET_ASSERT(iface);
117123

124+
#if defined(CONFIG_NET_VLAN)
125+
eth_ctx = net_if_l2_data(iface);
126+
if (eth_ctx->vlan_enabled &&
127+
net_eth_get_vlan_tag(iface) != NET_VLAN_TAG_UNSPEC) {
128+
eth_len = sizeof(struct net_eth_vlan_hdr);
129+
vlan_enabled = true;
130+
}
131+
#endif
132+
118133
pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT);
119134
if (!pkt) {
120135
goto fail;
@@ -131,6 +146,12 @@ struct net_pkt *gptp_prepare_sync(int port)
131146

132147
net_pkt_set_ll_reserve(pkt, eth_len);
133148

149+
#if defined(CONFIG_NET_VLAN)
150+
if (vlan_enabled) {
151+
hdr_vlan = (struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
152+
}
153+
#endif
154+
134155
port_ds = GPTP_PORT_DS(port);
135156
sync = GPTP_SYNC(pkt);
136157
hdr = GPTP_HDR(pkt);
@@ -159,7 +180,16 @@ struct net_pkt *gptp_prepare_sync(int port)
159180
hdr->reserved2 = 0;
160181

161182
/* Ethernet configuration. */
162-
eth->type = htons(NET_ETH_PTYPE_PTP);
183+
#if defined(CONFIG_NET_VLAN)
184+
if (vlan_enabled) {
185+
hdr_vlan->vlan.tpid = htons(NET_ETH_PTYPE_VLAN);
186+
hdr_vlan->vlan.tci = htons(net_eth_get_vlan_tag(iface));
187+
hdr_vlan->type = htons(NET_ETH_PTYPE_PTP);
188+
} else
189+
#endif
190+
{
191+
eth->type = htons(NET_ETH_PTYPE_PTP);
192+
}
163193

164194
memcpy(&eth->src.addr, net_if_get_link_addr(iface)->addr,
165195
sizeof(struct net_eth_addr));
@@ -194,11 +224,26 @@ struct net_pkt *gptp_prepare_follow_up(int port, struct net_pkt *sync)
194224
struct net_pkt *pkt;
195225
struct net_buf *frag;
196226

227+
#if defined(CONFIG_NET_VLAN)
228+
struct net_eth_vlan_hdr *hdr_vlan;
229+
struct ethernet_context *eth_ctx;
230+
bool vlan_enabled = false;
231+
#endif
232+
197233
NET_ASSERT(sync);
198234
NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
199235
iface = GPTP_PORT_IFACE(port);
200236
NET_ASSERT(iface);
201237

238+
#if defined(CONFIG_NET_VLAN)
239+
eth_ctx = net_if_l2_data(iface);
240+
if (eth_ctx->vlan_enabled &&
241+
net_eth_get_vlan_tag(iface) != NET_VLAN_TAG_UNSPEC) {
242+
eth_len = sizeof(struct net_eth_vlan_hdr);
243+
vlan_enabled = true;
244+
}
245+
#endif
246+
202247
pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT);
203248
if (!pkt) {
204249
goto fail;
@@ -214,6 +259,13 @@ struct net_pkt *gptp_prepare_follow_up(int port, struct net_pkt *sync)
214259
net_pkt_set_family(pkt, AF_UNSPEC);
215260
net_pkt_set_ll_reserve(pkt, eth_len);
216261

262+
#if defined(CONFIG_NET_VLAN)
263+
if (vlan_enabled) {
264+
hdr_vlan = (struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
265+
net_pkt_set_vlan_tag(pkt, net_pkt_vlan_tag(sync));
266+
}
267+
#endif
268+
217269
port_ds = GPTP_PORT_DS(port);
218270
hdr = GPTP_HDR(pkt);
219271
sync_hdr = GPTP_HDR(sync);
@@ -243,7 +295,16 @@ struct net_pkt *gptp_prepare_follow_up(int port, struct net_pkt *sync)
243295
hdr->reserved2 = 0;
244296

245297
/* Ethernet configuration. */
246-
eth->type = htons(NET_ETH_PTYPE_PTP);
298+
#if defined(CONFIG_NET_VLAN)
299+
if (vlan_enabled) {
300+
hdr_vlan->vlan.tpid = htons(NET_ETH_PTYPE_VLAN);
301+
hdr_vlan->vlan.tci = htons(net_pkt_vlan_tag(pkt));
302+
hdr_vlan->type = htons(NET_ETH_PTYPE_PTP);
303+
} else
304+
#endif
305+
{
306+
eth->type = htons(NET_ETH_PTYPE_PTP);
307+
}
247308

248309
memcpy(&eth->src.addr, net_if_get_link_addr(iface)->addr,
249310
sizeof(struct net_eth_addr));
@@ -276,10 +337,25 @@ struct net_pkt *gptp_prepare_pdelay_req(int port)
276337
struct net_buf *frag;
277338
struct gptp_hdr *hdr;
278339

340+
#if defined(CONFIG_NET_VLAN)
341+
struct net_eth_vlan_hdr *hdr_vlan;
342+
struct ethernet_context *eth_ctx;
343+
bool vlan_enabled = false;
344+
#endif
345+
279346
NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
280347
iface = GPTP_PORT_IFACE(port);
281348
NET_ASSERT(iface);
282349

350+
#if defined(CONFIG_NET_VLAN)
351+
eth_ctx = net_if_l2_data(iface);
352+
if (eth_ctx->vlan_enabled &&
353+
net_eth_get_vlan_tag(iface) != NET_VLAN_TAG_UNSPEC) {
354+
eth_len = sizeof(struct net_eth_vlan_hdr);
355+
vlan_enabled = true;
356+
}
357+
#endif
358+
283359
pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT);
284360
if (!pkt) {
285361
goto fail;
@@ -295,6 +371,12 @@ struct net_pkt *gptp_prepare_pdelay_req(int port)
295371
net_pkt_set_family(pkt, AF_UNSPEC);
296372
net_pkt_set_ll_reserve(pkt, eth_len);
297373

374+
#if defined(CONFIG_NET_VLAN)
375+
if (vlan_enabled) {
376+
hdr_vlan = (struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
377+
}
378+
#endif
379+
298380
port_ds = GPTP_PORT_DS(port);
299381
req = GPTP_PDELAY_REQ(pkt);
300382
hdr = GPTP_HDR(pkt);
@@ -325,7 +407,16 @@ struct net_pkt *gptp_prepare_pdelay_req(int port)
325407
&port_ds->port_id.clk_id, GPTP_CLOCK_ID_LEN);
326408

327409
/* Ethernet configuration. */
328-
eth->type = htons(NET_ETH_PTYPE_PTP);
410+
#if defined(CONFIG_NET_VLAN)
411+
if (vlan_enabled) {
412+
hdr_vlan->vlan.tpid = htons(NET_ETH_PTYPE_VLAN);
413+
hdr_vlan->vlan.tci = htons(net_eth_get_vlan_tag(iface));
414+
hdr_vlan->type = htons(NET_ETH_PTYPE_PTP);
415+
} else
416+
#endif
417+
{
418+
eth->type = htons(NET_ETH_PTYPE_PTP);
419+
}
329420

330421
memcpy(&eth->src.addr, net_if_get_link_addr(iface)->addr,
331422
sizeof(struct net_eth_addr));
@@ -365,6 +456,19 @@ struct net_pkt *gptp_prepare_pdelay_resp(int port,
365456
struct net_pkt *pkt;
366457
struct net_buf *frag;
367458

459+
#if defined(CONFIG_NET_VLAN)
460+
struct net_eth_vlan_hdr *hdr_vlan;
461+
struct ethernet_context *eth_ctx;
462+
bool vlan_enabled = false;
463+
464+
eth_ctx = net_if_l2_data(iface);
465+
if (eth_ctx->vlan_enabled &&
466+
net_eth_get_vlan_tag(iface) != NET_VLAN_TAG_UNSPEC) {
467+
eth_len = sizeof(struct net_eth_vlan_hdr);
468+
vlan_enabled = true;
469+
}
470+
#endif
471+
368472
pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT);
369473
if (!pkt) {
370474
goto fail;
@@ -380,6 +484,12 @@ struct net_pkt *gptp_prepare_pdelay_resp(int port,
380484
net_pkt_set_family(pkt, AF_INET);
381485
net_pkt_set_ll_reserve(pkt, eth_len);
382486

487+
#if defined(CONFIG_NET_VLAN)
488+
if (vlan_enabled) {
489+
hdr_vlan = (struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
490+
}
491+
#endif
492+
383493
port_ds = GPTP_PORT_DS(port);
384494

385495
pdelay_resp = GPTP_PDELAY_RESP(pkt);
@@ -416,7 +526,16 @@ struct net_pkt *gptp_prepare_pdelay_resp(int port,
416526
GPTP_CLOCK_ID_LEN);
417527

418528
/* Ethernet configuration. */
419-
eth->type = htons(NET_ETH_PTYPE_PTP);
529+
#if defined(CONFIG_NET_VLAN)
530+
if (vlan_enabled) {
531+
hdr_vlan->vlan.tpid = htons(NET_ETH_PTYPE_VLAN);
532+
hdr_vlan->vlan.tci = htons(net_pkt_vlan_tag(pkt));
533+
hdr_vlan->type = htons(NET_ETH_PTYPE_PTP);
534+
} else
535+
#endif
536+
{
537+
eth->type = htons(NET_ETH_PTYPE_PTP);
538+
}
420539

421540
memcpy(&eth->dst.addr, &gptp_multicast_eth_addr,
422541
sizeof(struct net_eth_addr));
@@ -456,6 +575,19 @@ struct net_pkt *gptp_prepare_pdelay_follow_up(int port,
456575
struct net_pkt *pkt;
457576
struct net_buf *frag;
458577

578+
#if defined(CONFIG_NET_VLAN)
579+
struct net_eth_vlan_hdr *hdr_vlan;
580+
struct ethernet_context *eth_ctx;
581+
bool vlan_enabled = false;
582+
583+
eth_ctx = net_if_l2_data(iface);
584+
if (eth_ctx->vlan_enabled &&
585+
net_eth_get_vlan_tag(iface) != NET_VLAN_TAG_UNSPEC) {
586+
eth_len = sizeof(struct net_eth_vlan_hdr);
587+
vlan_enabled = true;
588+
}
589+
#endif
590+
459591
pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT);
460592
if (!pkt) {
461593
goto fail;
@@ -471,6 +603,13 @@ struct net_pkt *gptp_prepare_pdelay_follow_up(int port,
471603
net_pkt_set_family(pkt, AF_INET);
472604
net_pkt_set_ll_reserve(pkt, eth_len);
473605

606+
#if defined(CONFIG_NET_VLAN)
607+
if (vlan_enabled) {
608+
hdr_vlan = (struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
609+
net_pkt_set_vlan_tag(pkt, net_pkt_vlan_tag(resp));
610+
}
611+
#endif
612+
474613
port_ds = GPTP_PORT_DS(port);
475614

476615
follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
@@ -507,7 +646,16 @@ struct net_pkt *gptp_prepare_pdelay_follow_up(int port,
507646
GPTP_CLOCK_ID_LEN);
508647

509648
/* Ethernet configuration. */
510-
eth->type = htons(NET_ETH_PTYPE_PTP);
649+
#if defined(CONFIG_NET_VLAN)
650+
if (vlan_enabled) {
651+
hdr_vlan->vlan.tpid = htons(NET_ETH_PTYPE_VLAN);
652+
hdr_vlan->vlan.tci = htons(net_pkt_vlan_tag(pkt));
653+
hdr_vlan->type = htons(NET_ETH_PTYPE_PTP);
654+
} else
655+
#endif
656+
{
657+
eth->type = htons(NET_ETH_PTYPE_PTP);
658+
}
511659

512660
memcpy(&eth->dst.addr, &gptp_multicast_eth_addr,
513661
sizeof(struct net_eth_addr));
@@ -546,11 +694,26 @@ struct net_pkt *gptp_prepare_announce(int port)
546694
struct net_buf *frag;
547695
struct gptp_hdr *hdr;
548696

697+
#if defined(CONFIG_NET_VLAN)
698+
struct net_eth_vlan_hdr *hdr_vlan;
699+
struct ethernet_context *eth_ctx;
700+
bool vlan_enabled = false;
701+
#endif
702+
549703
NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
550704
global_ds = GPTP_GLOBAL_DS();
551705
iface = GPTP_PORT_IFACE(port);
552706
NET_ASSERT(iface);
553707

708+
#if defined(CONFIG_NET_VLAN)
709+
eth_ctx = net_if_l2_data(iface);
710+
if (eth_ctx->vlan_enabled &&
711+
net_eth_get_vlan_tag(iface) != NET_VLAN_TAG_UNSPEC) {
712+
eth_len = sizeof(struct net_eth_vlan_hdr);
713+
vlan_enabled = true;
714+
}
715+
#endif
716+
554717
pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT);
555718
if (!pkt) {
556719
goto fail;
@@ -566,13 +729,28 @@ struct net_pkt *gptp_prepare_announce(int port)
566729
net_pkt_set_family(pkt, AF_INET);
567730
net_pkt_set_ll_reserve(pkt, eth_len);
568731

732+
#if defined(CONFIG_NET_VLAN)
733+
if (vlan_enabled) {
734+
hdr_vlan = (struct net_eth_vlan_hdr *)NET_ETH_HDR(pkt);
735+
}
736+
#endif
737+
569738
eth = NET_ETH_HDR(pkt);
570739
hdr = GPTP_HDR(pkt);
571740
ann = GPTP_ANNOUNCE(pkt);
572741
port_ds = GPTP_PORT_DS(port);
573742

574743
/* Ethernet configuration. */
575-
eth->type = htons(NET_ETH_PTYPE_PTP);
744+
#if defined(CONFIG_NET_VLAN)
745+
if (vlan_enabled) {
746+
hdr_vlan->vlan.tpid = htons(NET_ETH_PTYPE_VLAN);
747+
hdr_vlan->vlan.tci = htons(net_eth_get_vlan_tag(iface));
748+
hdr_vlan->type = htons(NET_ETH_PTYPE_PTP);
749+
} else
750+
#endif
751+
{
752+
eth->type = htons(NET_ETH_PTYPE_PTP);
753+
}
576754

577755
memcpy(&eth->src.addr, net_if_get_link_addr(iface)->addr,
578756
sizeof(struct net_eth_addr));

0 commit comments

Comments
 (0)