Allow gPTP over VLAN

include/net/ethernet.h

@@ -369,6 +369,15 @@ struct net_if *net_eth_get_vlan_iface(struct net_if *iface, u16_t tag);
 bool net_eth_is_vlan_enabled(struct ethernet_context *ctx,
 			     struct net_if *iface);
 
+/**
+ * @brief Get VLAN status for a given network interface (enabled or not).
+ *
+ * @param iface Network interface
+ *
+ * @return True if VLAN is enabled for this network interface, false if not.
+ */
+bool net_eth_get_vlan_status(struct net_if *iface);
+
 #define ETH_NET_DEVICE_INIT(dev_name, drv_name, init_fn,		 \
 			    data, cfg_info, prio, api, mtu)		 \
 	DEVICE_AND_API_INIT(dev_name, drv_name, init_fn, data,		 \
@@ -404,6 +413,11 @@ struct net_if *net_eth_get_vlan_iface(struct net_if *iface, u16_t tag)
 {
 	return NULL;
 }
+
+static inline bool net_eth_get_vlan_status(struct net_if *iface)
+{
+	return false;
+}
 #endif /* CONFIG_NET_VLAN */
 
 /**

include/net/ethernet_mgmt.h

@@ -80,6 +80,8 @@ struct ethernet_req_params {
 enum net_event_ethernet_cmd {
 	NET_EVENT_ETHERNET_CMD_CARRIER_ON = 1,
 	NET_EVENT_ETHERNET_CMD_CARRIER_OFF,
+	NET_EVENT_ETHERNET_CMD_VLAN_TAG_ENABLED,
+	NET_EVENT_ETHERNET_CMD_VLAN_TAG_DISABLED,
 };
 
 #define NET_EVENT_ETHERNET_CARRIER_ON					\
@@ -88,12 +90,20 @@ enum net_event_ethernet_cmd {
 #define NET_EVENT_ETHERNET_CARRIER_OFF					\
 	(_NET_ETHERNET_EVENT | NET_EVENT_ETHERNET_CMD_CARRIER_OFF)
 
+#define NET_EVENT_ETHERNET_VLAN_TAG_ENABLED				\
+	(_NET_ETHERNET_EVENT | NET_EVENT_ETHERNET_CMD_VLAN_TAG_ENABLED)
+
+#define NET_EVENT_ETHERNET_VLAN_TAG_DISABLED				\
+	(_NET_ETHERNET_EVENT | NET_EVENT_ETHERNET_CMD_VLAN_TAG_DISABLED)
+
 struct net_if;
 
 #if defined(CONFIG_NET_L2_ETHERNET_MGMT)
 void ethernet_mgmt_raise_carrier_on_event(struct net_if *iface);
 
 void ethernet_mgmt_raise_carrier_off_event(struct net_if *iface);
+void ethernet_mgmt_raise_vlan_enabled_event(struct net_if *iface, u16_t tag);
+void ethernet_mgmt_raise_vlan_disabled_event(struct net_if *iface, u16_t tag);
 #else
 static inline void ethernet_mgmt_raise_carrier_on_event(struct net_if *iface)
 {
@@ -104,6 +114,20 @@ static inline void ethernet_mgmt_raise_carrier_off_event(struct net_if *iface)
 {
 	ARG_UNUSED(iface);
 }
+
+static inline void ethernet_mgmt_raise_vlan_enabled_event(struct net_if *iface,
+							  u16_t tag)
+{
+	ARG_UNUSED(iface);
+	ARG_UNUSED(tag);
+}
+
+static inline void ethernet_mgmt_raise_vlan_disabled_event(struct net_if *iface,
+							   u16_t tag)
+{
+	ARG_UNUSED(iface);
+	ARG_UNUSED(tag);
+}
 #endif
 /**
  * @}

samples/net/gptp/prj_base.conf

@@ -76,6 +76,10 @@ CONFIG_NET_VLAN_COUNT=3
 CONFIG_NET_GPTP=y
 CONFIG_NET_GPTP_STATISTICS=y
 
+# Enable these if you want to run gPTP over a VLAN link
+#CONFIG_NET_GPTP_VLAN=y
+#CONFIG_NET_GPTP_VLAN_TAG=100
+
 # How many traffic classes to enable
 CONFIG_NET_TC_TX_COUNT=6
 CONFIG_NET_TC_RX_COUNT=4

samples/net/gptp/src/main.c

@@ -21,7 +21,7 @@
 
 static struct gptp_phase_dis_cb phase_dis;
 
-#if defined(CONFIG_NET_VLAN)
+#if defined(CONFIG_NET_GPTP_VLAN)
 /* User data for the interface callback */
 struct ud {
 	struct net_if *first;
@@ -124,7 +124,7 @@ static int init_vlan(void)
 
 	return 0;
 }
-#endif /* CONFIG_NET_VLAN */
+#endif /* CONFIG_NET_GPTP_VLAN */
 
 static void gptp_phase_dis_cb(u8_t *gm_identity,
 			      u16_t *time_base,
@@ -147,7 +147,7 @@ static void gptp_phase_dis_cb(u8_t *gm_identity,
 
 static int init_app(void)
 {
-#if defined(CONFIG_NET_VLAN)
+#if defined(CONFIG_NET_GPTP_VLAN)
 	if (init_vlan() < 0) {
 		NET_ERR("Cannot setup VLAN");
 	}

subsys/net/l2/ethernet/ethernet.c

@@ -651,6 +651,18 @@ u16_t net_eth_get_vlan_tag(struct net_if *iface)
 	return NET_VLAN_TAG_UNSPEC;
 }
 
+bool net_eth_get_vlan_status(struct net_if *iface)
+{
+	struct ethernet_context *ctx = net_if_l2_data(iface);
+
+	if (ctx->vlan_enabled &&
+	    net_eth_get_vlan_tag(iface) != NET_VLAN_TAG_UNSPEC) {
+		return true;
+	}
+
+	return false;
+}
+
 static struct ethernet_vlan *get_vlan(struct ethernet_context *ctx,
 				      struct net_if *iface,
 				      u16_t vlan_tag)
@@ -717,6 +729,8 @@ int net_eth_vlan_enable(struct net_if *iface, u16_t tag)
 			ctx->vlan_enabled = NET_VLAN_MAX_COUNT;
 		}
 
+		ethernet_mgmt_raise_vlan_enabled_event(iface, tag);
+
 		return 0;
 	}
 
@@ -753,6 +767,8 @@ int net_eth_vlan_disable(struct net_if *iface, u16_t tag)
 		eth->vlan_setup(net_if_get_device(iface), iface, tag, false);
 	}
 
+	ethernet_mgmt_raise_vlan_disabled_event(iface, tag);
+
 	ctx->vlan_enabled--;
 	if (ctx->vlan_enabled < 0) {
 		ctx->vlan_enabled = 0;

subsys/net/l2/ethernet/ethernet_mgmt.c

@@ -121,3 +121,24 @@ void ethernet_mgmt_raise_carrier_off_event(struct net_if *iface)
 {
 	net_mgmt_event_notify(NET_EVENT_ETHERNET_CARRIER_OFF, iface);
 }
+
+void ethernet_mgmt_raise_vlan_enabled_event(struct net_if *iface, u16_t tag)
+{
+#if defined(CONFIG_NET_MGMT_EVENT_INFO)
+	net_mgmt_event_notify_with_info(NET_EVENT_ETHERNET_VLAN_TAG_ENABLED,
+					iface, &tag, sizeof(tag));
+#else
+	net_mgmt_event_notify(NET_EVENT_ETHERNET_VLAN_TAG_ENABLED,
+			      iface);
+#endif
+}
+
+void ethernet_mgmt_raise_vlan_disabled_event(struct net_if *iface, u16_t tag)
+{
+#if defined(CONFIG_NET_MGMT_EVENT_INFO)
+	net_mgmt_event_notify_with_info(NET_EVENT_ETHERNET_VLAN_TAG_DISABLED,
+					iface, &tag, sizeof(tag));
+#else
+	net_mgmt_event_notify(NET_EVENT_ETHERNET_VLAN_TAG_DISABLED, iface);
+#endif
+}

subsys/net/l2/ethernet/gptp/Kconfig

@@ -30,6 +30,29 @@ config NET_GPTP_NUM_PORTS
 	  Configures the gPTP stack to work with the given number of ports.
 	  The port concept is the same thing as network interface.
 
+config NET_GPTP_VLAN
+	bool "Run gPTP over VLAN link"
+	default n
+	depends on NET_VLAN
+	select NET_L2_ETHERNET_MGMT
+	select NET_MGMT_EVENT
+	select NET_MGMT_EVENT_INFO
+	help
+	  This setting allows gPTP to run over VLAN link. Currently only
+	  one port can have VLAN tag set. Note that CONFIG_NET_GPTP_VLAN_TAG
+	  setting must have a proper tag value set, otherwise the gPTP over
+	  VLAN will not work.
+
+config NET_GPTP_VLAN_TAG
+	int "VLAN tag to use"
+	default 4095
+	depends on NET_GPTP_VLAN
+	help
+	  The VLAN tag to use when sending and receiving gPTP messages.
+	  The default value 4095 (0x0fff) means unspecified tag which is
+	  not a valid value. This means that you need to set the tag to
+	  a valid value.
+
 config NET_GPTP_NEIGHBOR_PROP_DELAY_THR
 	int "Set neighbor propagation delay threshold (ns)"
 	default 100000

subsys/net/l2/ethernet/gptp/gptp.c

@@ -11,6 +11,7 @@
 
 #include <net/net_pkt.h>
 #include <ptp_clock.h>
+#include <net/ethernet_mgmt.h>
 
 #include <net/gptp.h>
 
@@ -37,6 +38,7 @@ NET_STACK_DEFINE(GPTP, gptp_stack, CONFIG_NET_GPTP_STACK_SIZE,
 		CONFIG_NET_GPTP_STACK_SIZE);
 K_FIFO_DEFINE(gptp_rx_queue);
 
+static k_tid_t tid;
 static struct k_thread gptp_thread_data;
 struct gptp_domain gptp_domain;
 
@@ -561,6 +563,19 @@ static void gptp_add_port(struct net_if *iface, void *user_data)
 		return;
 	}
 
+#if defined(CONFIG_NET_GPTP_VLAN)
+	if (CONFIG_NET_GPTP_VLAN_TAG >= 0 &&
+	    CONFIG_NET_GPTP_VLAN_TAG < NET_VLAN_TAG_UNSPEC) {
+		struct net_if *vlan_iface;
+
+		vlan_iface = net_eth_get_vlan_iface(iface,
+						    CONFIG_NET_GPTP_VLAN_TAG);
+		if (vlan_iface != iface) {
+			return;
+		}
+	}
+#endif /* CONFIG_NET_GPTP_VLAN */
+
 	/* Check if interface has a PTP clock. */
 	clk = net_eth_get_ptp_clock(iface);
 	if (clk) {
@@ -869,15 +884,141 @@ int gptp_get_port_data(struct gptp_domain *domain,
 	return 0;
 }
 
-void net_gptp_init(void)
+static void init_ports(void)
 {
-	gptp_domain.default_ds.nb_ports = 0;
 	net_if_foreach(gptp_add_port, &gptp_domain.default_ds.nb_ports);
 
 	/* Only initialize the state machine once the ports are known. */
 	gptp_init_state_machine();
 
-	k_thread_create(&gptp_thread_data, gptp_stack, sizeof(gptp_stack),
-			(k_thread_entry_t)gptp_thread,
-			NULL, NULL, NULL, K_PRIO_COOP(5), 0, 0);
+	tid = k_thread_create(&gptp_thread_data, gptp_stack,
+			      sizeof(gptp_stack),
+			      (k_thread_entry_t)gptp_thread,
+			      NULL, NULL, NULL, K_PRIO_COOP(5), 0, 0);
+}
+
+#if defined(CONFIG_NET_GPTP_VLAN)
+static struct net_mgmt_event_callback vlan_cb;
+
+struct vlan_work {
+	struct k_work work;
+	struct net_if *iface;
+} vlan;
+
+static void disable_port(int port)
+{
+	GPTP_GLOBAL_DS()->selected_role[port] = GPTP_PORT_DISABLED;
+
+	gptp_state_machine();
+}
+
+static void vlan_enabled(struct k_work *work)
+{
+	struct vlan_work *vlan = CONTAINER_OF(work,
+					      struct vlan_work,
+					      work);
+	if (tid) {
+		int port;
+
+		port = gptp_get_port_number(vlan->iface);
+		if (port < 0) {
+			NET_DBG("No port found for iface %p", vlan->iface);
+			return;
+		}
+
+		GPTP_GLOBAL_DS()->selected_role[port] = GPTP_PORT_SLAVE;
+
+		gptp_state_machine();
+	} else {
+		init_ports();
+	}
+}
+
+static void vlan_disabled(struct k_work *work)
+{
+	struct vlan_work *vlan = CONTAINER_OF(work,
+					      struct vlan_work,
+					      work);
+	int port;
+
+	port = gptp_get_port_number(vlan->iface);
+	if (port < 0) {
+		NET_DBG("No port found for iface %p", vlan->iface);
+		return;
+	}
+
+	disable_port(port);
+}
+
+static void vlan_event_handler(struct net_mgmt_event_callback *cb,
+			       u32_t mgmt_event,
+			       struct net_if *iface)
+{
+	u16_t tag;
+
+	if (mgmt_event != NET_EVENT_ETHERNET_VLAN_TAG_ENABLED &&
+	    mgmt_event != NET_EVENT_ETHERNET_VLAN_TAG_DISABLED) {
+		return;
+	}
+
+#if defined(CONFIG_NET_MGMT_EVENT_INFO)
+	if (!cb->info) {
+		return;
+	}
+
+	tag = *((u16_t *)cb->info);
+	if (tag != CONFIG_NET_GPTP_VLAN_TAG) {
+		return;
+	}
+
+	vlan.iface = iface;
+
+	if (mgmt_event == NET_EVENT_ETHERNET_VLAN_TAG_ENABLED) {
+		/* We found the right tag, now start gPTP for this interface */
+		k_work_init(&vlan.work, vlan_enabled);
+
+		NET_DBG("VLAN tag %d %s for iface %p", tag, "enabled", iface);
+	} else {
+		k_work_init(&vlan.work, vlan_disabled);
+
+		NET_DBG("VLAN tag %d %s for iface %p", tag, "disabled", iface);
+	}
+
+	k_work_submit(&vlan.work);
+#else
+	NET_WARN("VLAN event but tag info missing!");
+
+	ARG_UNUSED(tag);
+#endif
+}
+
+static void setup_vlan_events_listener(void)
+{
+	net_mgmt_init_event_callback(&vlan_cb, vlan_event_handler,
+				     NET_EVENT_ETHERNET_VLAN_TAG_ENABLED |
+				     NET_EVENT_ETHERNET_VLAN_TAG_DISABLED);
+	net_mgmt_add_event_callback(&vlan_cb);
+}
+#endif /* CONFIG_NET_GPTP_VLAN */
+
+void net_gptp_init(void)
+{
+	gptp_domain.default_ds.nb_ports = 0;
+
+#if defined(CONFIG_NET_GPTP_VLAN)
+	/* If user has enabled gPTP over VLAN support, then we start gPTP
+	 * support after we have received correct "VLAN tag enabled" event.
+	 */
+	if (CONFIG_NET_GPTP_VLAN_TAG >= 0 &&
+	    CONFIG_NET_GPTP_VLAN_TAG < NET_VLAN_TAG_UNSPEC) {
+		setup_vlan_events_listener();
+	} else {
+		NET_WARN("VLAN tag %d set but the value is not valid.",
+			 CONFIG_NET_GPTP_VLAN_TAG);
+
+		init_ports();
+	}
+#else
+	init_ports();
+#endif
 }