proto-ext-common: add extension autonegotiation (between WR and HA)

Signed-off-by: Adam Wujek <dev_public@wujek.eu>

Kconfig_ppsi

@@ -228,6 +228,11 @@ menu "Enabled profiles"
 
 endmenu
 
+config EXT_AUTONEG_SUPPORT
+	bool "Enable support for HA/WR Autonegotiation"
+	depends on ARCH_WRS && PROFILE_WR && PROFILE_HA
+	default y
+
 config PROFILE_PTP
 	bool
 	default y
@@ -268,6 +273,12 @@ config HAS_PROFILE_CUSTOM
 	default 1 if PROFILE_CUSTOM 
 	default 0 
 
+config HAS_EXT_AUTONEG_SUPPORT
+	int
+	range 0 1
+	default 1 if EXT_AUTONEG_SUPPORT
+	default 0
+
 #default to y if WRPC and VLAN support is enabled in WRPC's Kconfig
 config PPSI_VLAN
 	bool

include/ppsi/constants.h

@@ -195,6 +195,10 @@
 #define PP_MCAST_MACADDRESS		"\x01\x1B\x19\x00\x00\x00"
 #define PP_PDELAY_MACADDRESS		"\x01\x80\xC2\x00\x00\x0E"
 
+/* Define how many frames has to be received/missing to make a switch between
+ * extensions */
+#define PP_L1S_WR_AUTONEGOTIATION_FRAMES_TO_SWITCH	5
+
 #include <arch/constants.h> /* architectures may override the defaults */
 
 #endif /* __PPSI_CONSTANTS_H__ */

include/ppsi/pp-instance.h

@@ -304,6 +304,7 @@ struct pp_instance {
 	pp_exstate_t extState; /* Extension state */
 #if CONFIG_ARCH_IS_WRS
 	/* Several extensions are supported only in WRS */
+	struct ext_autoneg_data extAutonegData;
 	struct pp_ext_mem *ext_mem; /* memory needed to when using several extensions */
 #endif
 };

include/ppsi/ppsi.h

@@ -19,6 +19,7 @@
 #include <ppsi/constants.h>
 #include <ppsi/jiffies.h>
 #include <ppsi/timeout_def.h>
+#include "../proto-ext-common/ext-autoneg-api.h"
 #include <ppsi/pp-instance.h>
 #include <ppsi/diag-macros.h>
 #include <ppsi/bmc.h>

proto-ext-common/Makefile

@@ -11,3 +11,6 @@ ifdef INC_WRH_SERVO
 OBJ-y += $D/wrh-servo.o
 endif
 
+ifeq ($(CONFIG_HAS_EXT_AUTONEG_SUPPORT),1)
+OBJ-y += $D/ext-autoneg.o
+endif

proto-ext-common/ext-autoneg-api.h

+/*
+ * Copyright (C) 2023 CERN (www.cern.ch)
+ * Author: Adam Wujek for CERN
+ *
+ * Released according to the GNU LGPL, version 2.1 or any later version.
+ */
+
+#ifndef __EXT_AUTONEG_API_H__
+#define __EXT_AUTONEG_API_H__
+
+/* All data used as extension ppsi l1sync must be put here */
+struct ext_autoneg_data {
+	int counter_announce_wr;
+	int counter_announce_no_wr;
+};
+
+struct pp_instance;
+
+int ext_autoneg_l1e_handle_announce(struct pp_instance * ppi, void *buf, int len);
+int ext_autoneg_check_signaling_for_l1sync(struct pp_instance * ppi, void *buf, int len);
+
+int ext_autoneg_handle_l1sync(struct pp_instance * ppi, void *buf, int len);
+int ext_autoneg_check_signaling_for_slave_present(struct pp_instance * ppi, void *buf, int len);
+int ext_autoneg_wr_handle_announce(struct pp_instance * ppi, void *buf, int len);
+
+#endif /* __EXT_AUTONEG_API_H__ */

proto-ext-common/ext-autoneg.c

+#include <ppsi/ppsi.h>
+#include <ppsi-wrs.h>
+
+static void l1sync_detected(struct pp_instance *ppi)
+{
+	if (ppi->protocol_extension != PPSI_EXT_WR)
+		return;
+
+	pp_diag(ppi, ext, 1, "hook: %s received l1sync, switching to HA\n",
+		__func__);
+
+	set_profile_runtime(ppi, PPSI_PROFILE_HA);
+	return;
+}
+
+static void slave_present_detected(struct pp_instance *ppi)
+{
+	if (ppi->protocol_extension != PPSI_EXT_L1S)
+		return;
+
+	pp_diag(ppi, ext, 1,
+		"hook: %s received SLAVE_PRESENT, switching to WR\n",
+		__func__);
+
+	set_profile_runtime(ppi, PPSI_PROFILE_WR);
+	return;
+}
+
+int ext_autoneg_handle_l1sync(struct pp_instance * ppi, void *buf, int len)
+{
+	ppi->extAutonegData.counter_announce_wr = 0;
+
+	return 0;
+}
+
+int ext_autoneg_l1e_handle_announce(struct pp_instance * ppi, void *buf,
+				    int len)
+{
+	int len_tlv;
+
+	pp_diag(ppi, ext, 3, "hook: %s\n", __func__);
+
+	len_tlv = len - PP_ANNOUNCE_LENGTH;
+	if (!msg_check_wr_tlv(ppi, &((char *)buf)[PP_ANNOUNCE_LENGTH], &len_tlv,
+			      "Announce"))
+		return 0;
+
+	pp_diag(ppi, ext, 3, "hook: %s announce with WR TLV. Counter wr: %d\n",
+		__func__, ppi->extAutonegData.counter_announce_wr);
+
+	if (ppi->extAutonegData.counter_announce_wr
+	    < PP_L1S_WR_AUTONEGOTIATION_FRAMES_TO_SWITCH) {
+		ppi->extAutonegData.counter_announce_wr++;
+	} else {
+		/* Received no L1Sync after reception of N x Announce with
+		 * WR TLV [W1] switch to WR */
+		pp_diag(ppi, ext, 1,
+			"hook: %s no L1sync received within %d announce with "
+			"WR TLV, switching to WR!\n",
+			__func__, PP_L1S_WR_AUTONEGOTIATION_FRAMES_TO_SWITCH);
+		set_profile_runtime(ppi, PPSI_PROFILE_WR);
+		ppi->extAutonegData.counter_announce_wr = 0;
+	}
+	return 0;
+}
+
+int ext_autoneg_wr_handle_announce(struct pp_instance * ppi, void *buf, int len)
+{
+	int len_tlv;
+
+	pp_diag(ppi, ext, 3, "hook: %s\n", __func__);
+
+	len_tlv = len - PP_ANNOUNCE_LENGTH;
+	if (msg_check_wr_tlv(ppi, &((char *)buf)[PP_ANNOUNCE_LENGTH],
+			     &len_tlv, "Announce")) {
+		ppi->extAutonegData.counter_announce_no_wr = 0;
+		pp_diag(ppi, ext, 3, "hook: %s TLV ok len %d\n", __func__, len);
+		return 0;
+	}
+
+
+	pp_diag(ppi, ext, 3, "hook: %s announce without WR TLV. Counter %d\n",
+		__func__, ppi->extAutonegData.counter_announce_no_wr);
+
+	if (ppi->extAutonegData.counter_announce_no_wr
+	    < PP_L1S_WR_AUTONEGOTIATION_FRAMES_TO_SWITCH) {
+		ppi->extAutonegData.counter_announce_no_wr++;
+	} else {
+		/* Received N x Announce without WR TLV [M1] switch to HA */
+		pp_diag(ppi, ext, 1,
+			"hook: %s last %d announces were without WR TLV, "
+			"switching to HA!\n",
+			__func__, PP_L1S_WR_AUTONEGOTIATION_FRAMES_TO_SWITCH);
+		set_profile_runtime(ppi, PPSI_PROFILE_HA);
+		ppi->extAutonegData.counter_announce_no_wr = 0;
+	}
+
+	return 0;
+}
+
+int ext_autoneg_check_signaling_for_l1sync(struct pp_instance * ppi, void *buf,
+					   int len)
+{
+	if (l1e_unpack_signal(ppi, buf, len) == 0) {
+		/* Valid Sync message */
+		l1sync_detected(ppi);
+	}
+
+	return 0;
+}
+
+int ext_autoneg_check_signaling_for_slave_present(struct pp_instance * ppi,
+						  void *buf, int len)
+{
+	Enumeration16 wrMsgId;
+	MsgSignaling wrsig_msg;
+
+	if (msg_unpack_wrsig(ppi, buf, len, &wrsig_msg, &wrMsgId)) {
+		/* Valid Sync message */
+		pp_diag(ppi, ext, 3, "hook: %s Valid Sync message %d\n",
+			__func__, (int)wrMsgId);
+
+		if (wrMsgId == SLAVE_PRESENT) {
+			slave_present_detected(ppi);
+			return 1;
+		}
+	}
+
+	return 0;
+}

proto-ext-l1sync/l1e-hooks.c

@@ -74,9 +74,20 @@ static int l1e_handle_signaling(struct pp_instance * ppi, void *buf, int len)
 	pp_diag(ppi, ext, 2, "hook: %s (%i:%s) -- plen %i\n", __func__,
 		ppi->state, l1e_state_name[bds->L1SyncState], len);
 
+	if (CONFIG_HAS_EXT_AUTONEG_SUPPORT
+	    && ppi->cfg.extAutonegEnable == PPSI_EXT_AUTONEG_ENABLE
+	    && ext_autoneg_check_signaling_for_slave_present(ppi, buf, len)
+	    ) {
+		return 0;
+	    }
+
 	if ( l1e_unpack_signal(ppi, buf, len)==0 ) {
 		/* Valid Sync message */
 
+		if (CONFIG_HAS_EXT_AUTONEG_SUPPORT
+		    && ppi->cfg.extAutonegEnable == PPSI_EXT_AUTONEG_ENABLE)
+			ext_autoneg_handle_l1sync(ppi, buf, len);
+
 		/* Reset reception timeout */
 		pp_timeout_set(ppi, PP_TO_L1E_RX_SYNC, l1e_get_rx_tmo_ms(bds));
 
@@ -247,17 +258,38 @@ static int l1e_extension_state_changed( struct pp_instance * ppi)
 	return 0;
 }
 
+/* Send WR announce by master */
+static int l1e_pack_announce(struct pp_instance *ppi)
+{
+	if (CONFIG_HAS_EXT_AUTONEG_SUPPORT
+	    && ppi->cfg.extAutonegEnable == PPSI_EXT_AUTONEG_ENABLE)
+		return wr_ext_hooks.pack_announce(ppi);
+
+	return 0;
+}
+
+static int l1e_handle_announce(struct pp_instance *ppi, void *buf, int len)
+{
+	if (CONFIG_HAS_EXT_AUTONEG_SUPPORT
+	    && ppi->cfg.extAutonegEnable == PPSI_EXT_AUTONEG_ENABLE)
+		return ext_autoneg_l1e_handle_announce(ppi, buf, len);
+
+	return 0;
+}
+
 /* The global structure used by ppsi */
 const struct pp_ext_hooks l1e_ext_hooks = {
 	.open = l1e_open,
 	.init = l1e_init,
 	.handle_signaling = l1e_handle_signaling,
 	.run_ext_state_machine = l1e_run_state_machine,
+	.pack_announce = l1e_pack_announce,
 	.ready_for_slave = l1e_ready_for_slave,
 	.handle_resp = l1e_handle_resp,
 	.handle_dreq = l1e_handle_dreq,
 	.handle_sync = l1e_handle_sync,
 	.handle_followup = l1e_handle_followup,
+	.handle_announce = l1e_handle_announce,
 	.new_slave = l1e_new_slave,
 #if CONFIG_HAS_P2P
 	.handle_presp = l1e_handle_presp,

proto-ext-whiterabbit/hooks.c

@@ -345,6 +345,24 @@ static int wr_new_slave (struct pp_instance *ppi, void *buf, int len) {
 	return 0;
 }
 
+/* This hook is called whenever a signaling message is received */
+static int wr_handle_signaling(struct pp_instance * ppi, void *buf, int len)
+{
+	if (CONFIG_HAS_EXT_AUTONEG_SUPPORT
+	    && ppi->cfg.extAutonegEnable == PPSI_EXT_AUTONEG_ENABLE)
+		return ext_autoneg_check_signaling_for_l1sync(ppi, buf, len);
+
+	return 0;
+}
+
+static int wr_handle_announce(struct pp_instance *ppi, void *buf, int len)
+{
+	if (CONFIG_HAS_EXT_AUTONEG_SUPPORT
+	    && ppi->cfg.extAutonegEnable == PPSI_EXT_AUTONEG_ENABLE)
+		return ext_autoneg_wr_handle_announce(ppi, buf, len);
+
+	return 0;
+}
 
 const struct pp_ext_hooks wr_ext_hooks = {
 	.init = wr_init,
@@ -353,6 +371,7 @@ const struct pp_ext_hooks wr_ext_hooks = {
 	.handle_dreq = wr_handle_dreq,
 	.handle_sync = wr_handle_sync,
 	.handle_followup = wr_handle_followup,
+	.handle_announce = wr_handle_announce,
 	.ready_for_slave = wr_ready_for_slave,
 	.run_ext_state_machine = wr_run_state_machine,
 	.new_slave = wr_new_slave,
@@ -369,5 +388,6 @@ const struct pp_ext_hooks wr_ext_hooks = {
 	.get_egress_latency=wr_get_latency,
 	.is_correction_field_compliant=wr_is_correction_field_compliant,
 	.extension_state_changed= wr_extension_state_changed,
-	.bmca_s1=wr_bmca_s1
+	.handle_signaling = wr_handle_signaling,
+	.bmca_s1=wr_bmca_s1,
 };

tools/wrs_dump_shmem_ppsi.c

@@ -308,6 +308,8 @@ struct dump_info ppi_info [] = {
 
 	DUMP_FIELD_SIZE(pointer, iface_name,16),
 	DUMP_FIELD_SIZE(pointer, port_name,16),
+	DUMP_FIELD(int, extAutonegData.counter_announce_wr),
+	DUMP_FIELD(int, extAutonegData.counter_announce_no_wr),
 	DUMP_FIELD(int, port_idx),
 	DUMP_FIELD(int, vlans_array_len),
 	/* pass the size of a vlans array in the nvlans field */
@@ -756,7 +758,6 @@ void dump_one_field_ppsi_wrs(int type, int size, void *p, int i)
 		print_str(char_p);
 		printf("\n");
 		break;
-
 	}
 }