mlxsw: Add support of latency TLV

index f2d6f8654e04..8165bf31a99a 100644
@@ -10009,6 +10009,18 @@ MLXSW_REG_DEFINE(mgir, MLXSW_REG_MGIR_ID, MLXSW_REG_MGIR_LEN);
  */
 MLXSW_ITEM32(reg, mgir, hw_info_device_hw_revision, 0x0, 16, 16);
 
+/* reg_mgir_fw_info_latency_tlv
+ * When set, latency-TLV is supported.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mgir, fw_info_latency_tlv, 0x20, 29, 1);
+
+/* reg_mgir_fw_info_string_tlv
+ * When set, string-TLV is supported.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mgir, fw_info_string_tlv, 0x20, 28, 1);
+
 #define MLXSW_REG_MGIR_FW_INFO_PSID_SIZE 16
 
 /* reg_mgir_fw_info_psid

index cb3715f1582b..36ef9ac12296 100644
@@ -476,11 +476,11 @@ static int mlxsw_emad_construct_eth_hdr(struct sk_buff *skb)
 	return 0;
 }
 
-static void mlxsw_emad_construct(struct sk_buff *skb,
+static void mlxsw_emad_construct(const struct mlxsw_core *mlxsw_core,
+				 struct sk_buff *skb,
 				 const struct mlxsw_reg_info *reg,
 				 char *payload,
-				 enum mlxsw_core_reg_access_type type,
-				 u64 tid, bool enable_string_tlv)
+				 enum mlxsw_core_reg_access_type type, u64 tid)
 {
 	char *buf;
 

@@ -490,7 +490,7 @@ static void mlxsw_emad_construct(struct sk_buff *skb,
 	buf = skb_push(skb, reg->len + sizeof(u32));
 	mlxsw_emad_pack_reg_tlv(buf, reg, payload);
 
-	if (enable_string_tlv) {
+	if (mlxsw_core->emad.enable_string_tlv) {
 		buf = skb_push(skb, MLXSW_EMAD_STRING_TLV_LEN * sizeof(u32));
 		mlxsw_emad_pack_string_tlv(buf);
 	}

@@ -876,7 +876,7 @@ static void mlxsw_emad_fini(struct mlxsw_core *mlxsw_core)
 }
 
 static struct sk_buff *mlxsw_emad_alloc(const struct mlxsw_core *mlxsw_core,
-					u16 reg_len, bool enable_string_tlv)
+					u16 reg_len)
 {
 	struct sk_buff *skb;
 	u16 emad_len;

@@ -884,7 +884,7 @@ static struct sk_buff *mlxsw_emad_alloc(const struct mlxsw_core *mlxsw_core,
 	emad_len = (reg_len + sizeof(u32) + MLXSW_EMAD_ETH_HDR_LEN +
 		    (MLXSW_EMAD_OP_TLV_LEN + MLXSW_EMAD_END_TLV_LEN) *
 		    sizeof(u32) + mlxsw_core->driver->txhdr_len);
-	if (enable_string_tlv)
+	if (mlxsw_core->emad.enable_string_tlv)
 		emad_len += MLXSW_EMAD_STRING_TLV_LEN * sizeof(u32);
 	if (emad_len > MLXSW_EMAD_MAX_FRAME_LEN)
 		return NULL;

@@ -907,7 +907,6 @@ static int mlxsw_emad_reg_access(struct mlxsw_core *mlxsw_core,
 				 mlxsw_reg_trans_cb_t *cb,
 				 unsigned long cb_priv, u64 tid)
 {
-	bool enable_string_tlv;
 	struct sk_buff *skb;
 	int err;
 

@@ -915,12 +914,7 @@ static int mlxsw_emad_reg_access(struct mlxsw_core *mlxsw_core,
 		tid, reg->id, mlxsw_reg_id_str(reg->id),
 		mlxsw_core_reg_access_type_str(type));
 
-	/* Since this can be changed during emad_reg_access, read it once and
-	 * use the value all the way.
-	 */
-	enable_string_tlv = mlxsw_core->emad.enable_string_tlv;
-
-	skb = mlxsw_emad_alloc(mlxsw_core, reg->len, enable_string_tlv);
+	skb = mlxsw_emad_alloc(mlxsw_core, reg->len);
 	if (!skb)
 		return -ENOMEM;
 

@@ -937,8 +931,7 @@ static int mlxsw_emad_reg_access(struct mlxsw_core *mlxsw_core,
 	trans->reg = reg;
 	trans->type = type;
 
-	mlxsw_emad_construct(skb, reg, payload, type, trans->tid,
-			     enable_string_tlv);
+	mlxsw_emad_construct(mlxsw_core, skb, reg, payload, type, trans->tid);
 	mlxsw_core->driver->txhdr_construct(skb, &trans->tx_info);
 
 	spin_lock_bh(&mlxsw_core->emad.trans_list_lock);

index a0a06e2eff82..cb3715f1582b 100644
@@ -794,6 +794,28 @@ static const struct mlxsw_listener mlxsw_emad_rx_listener =
 	MLXSW_RXL(mlxsw_emad_rx_listener_func, ETHEMAD, TRAP_TO_CPU, false,
 		  EMAD, DISCARD);
 
+static int mlxsw_emad_tlv_enable(struct mlxsw_core *mlxsw_core)
+{
+	char mgir_pl[MLXSW_REG_MGIR_LEN];
+	bool string_tlv;
+	int err;
+
+	mlxsw_reg_mgir_pack(mgir_pl);
+	err = mlxsw_reg_query(mlxsw_core, MLXSW_REG(mgir), mgir_pl);
+	if (err)
+		return err;
+
+	string_tlv = mlxsw_reg_mgir_fw_info_string_tlv_get(mgir_pl);
+	mlxsw_core->emad.enable_string_tlv = string_tlv;
+
+	return 0;
+}
+
+static void mlxsw_emad_tlv_disable(struct mlxsw_core *mlxsw_core)
+{
+	mlxsw_core->emad.enable_string_tlv = false;
+}
+
 static int mlxsw_emad_init(struct mlxsw_core *mlxsw_core)
 {
 	struct workqueue_struct *emad_wq;

@@ -824,10 +846,17 @@ static int mlxsw_emad_init(struct mlxsw_core *mlxsw_core)
 	if (err)
 		goto err_trap_register;
 
+	err = mlxsw_emad_tlv_enable(mlxsw_core);
+	if (err)
+		goto err_emad_tlv_enable;
+
 	mlxsw_core->emad.use_emad = true;
 
 	return 0;
 
+err_emad_tlv_enable:
+	mlxsw_core_trap_unregister(mlxsw_core, &mlxsw_emad_rx_listener,
+				   mlxsw_core);
 err_trap_register:
 	destroy_workqueue(mlxsw_core->emad_wq);
 	return err;

@@ -840,6 +869,7 @@ static void mlxsw_emad_fini(struct mlxsw_core *mlxsw_core)
 		return;
 
 	mlxsw_core->emad.use_emad = false;
+	mlxsw_emad_tlv_disable(mlxsw_core);
 	mlxsw_core_trap_unregister(mlxsw_core, &mlxsw_emad_rx_listener,
 				   mlxsw_core);
 	destroy_workqueue(mlxsw_core->emad_wq);

@@ -3377,12 +3407,6 @@ bool mlxsw_core_sdq_supports_cqe_v2(struct mlxsw_core *mlxsw_core)
 }
 EXPORT_SYMBOL(mlxsw_core_sdq_supports_cqe_v2);
 
-void mlxsw_core_emad_string_tlv_enable(struct mlxsw_core *mlxsw_core)
-{
-	mlxsw_core->emad.enable_string_tlv = true;
-}
-EXPORT_SYMBOL(mlxsw_core_emad_string_tlv_enable);
-
 static int __init mlxsw_core_module_init(void)
 {
 	int err;

index e0a6fcbbcb19..a77cb0be7108 100644
@@ -448,8 +448,6 @@ u32 mlxsw_core_read_utc_nsec(struct mlxsw_core *mlxsw_core);
 
 bool mlxsw_core_sdq_supports_cqe_v2(struct mlxsw_core *mlxsw_core);
 
-void mlxsw_core_emad_string_tlv_enable(struct mlxsw_core *mlxsw_core);
-
 bool mlxsw_core_res_valid(struct mlxsw_core *mlxsw_core,
 			  enum mlxsw_res_id res_id);
 

index f5b2d965d476..3d15d3387aa2 100644
@@ -3092,7 +3092,6 @@ static int mlxsw_sp_init(struct mlxsw_core *mlxsw_core,
 	mlxsw_sp->bus_info = mlxsw_bus_info;
 
 	mlxsw_sp_parsing_init(mlxsw_sp);
-	mlxsw_core_emad_string_tlv_enable(mlxsw_core);
 
 	err = mlxsw_sp_base_mac_get(mlxsw_sp);
 	if (err) {

index acfbbec52424..c51a61aa19b7 100644
@@ -21,6 +21,7 @@ enum {
 	MLXSW_EMAD_TLV_TYPE_OP,
 	MLXSW_EMAD_TLV_TYPE_STRING,
 	MLXSW_EMAD_TLV_TYPE_REG,
+	MLXSW_EMAD_TLV_TYPE_LATENCY,
 };
 
 /* OP TLV */

@@ -90,6 +91,9 @@ enum {
 /* STRING TLV */
 #define MLXSW_EMAD_STRING_TLV_LEN 33	/* Length in u32 */
 
+/* LATENCY TLV */
+#define MLXSW_EMAD_LATENCY_TLV_LEN 7	/* Length in u32 */
+
 /* END TLV */
 #define MLXSW_EMAD_END_TLV_LEN 1	/* Length in u32 */
 

index 36ef9ac12296..0ba38c8f7b8f 100644
@@ -378,6 +378,22 @@ MLXSW_ITEM32(emad, string_tlv, len, 0x00, 16, 11);
 MLXSW_ITEM_BUF(emad, string_tlv, string, 0x04,
 	       MLXSW_EMAD_STRING_TLV_STRING_LEN);
 
+/* emad_latency_tlv_type
+ * Type of the TLV.
+ * Must be set to 0x4 (latency TLV).
+ */
+MLXSW_ITEM32(emad, latency_tlv, type, 0x00, 27, 5);
+
+/* emad_latency_tlv_len
+ * Length of the latency TLV in u32.
+ */
+MLXSW_ITEM32(emad, latency_tlv, len, 0x00, 16, 11);
+
+/* emad_latency_tlv_latency_time
+ * EMAD latency time in units of uSec.
+ */
+MLXSW_ITEM32(emad, latency_tlv, latency_time, 0x04, 0, 32);
+
 /* emad_reg_tlv_type
  * Type of the TLV.
  * Must be set to 0x3 (register TLV).

index 0ba38c8f7b8f..de5d1e715f3c 100644
@@ -78,6 +78,7 @@ struct mlxsw_core {
 		spinlock_t trans_list_lock; /* protects trans_list writes */
 		bool use_emad;
 		bool enable_string_tlv;
+		bool enable_latency_tlv;
 	} emad;
 	struct {
 		u16 *mapping; /* lag_id+port_index to local_port mapping */

@@ -477,6 +478,12 @@ static void mlxsw_emad_pack_op_tlv(char *op_tlv,
 	mlxsw_emad_op_tlv_tid_set(op_tlv, tid);
 }
 
+static void mlxsw_emad_pack_latency_tlv(char *latency_tlv)
+{
+	mlxsw_emad_latency_tlv_type_set(latency_tlv, MLXSW_EMAD_TLV_TYPE_LATENCY);
+	mlxsw_emad_latency_tlv_len_set(latency_tlv, MLXSW_EMAD_LATENCY_TLV_LEN);
+}
+
 static int mlxsw_emad_construct_eth_hdr(struct sk_buff *skb)
 {
 	char *eth_hdr = skb_push(skb, MLXSW_EMAD_ETH_HDR_LEN);

@@ -506,6 +513,11 @@ static void mlxsw_emad_construct(const struct mlxsw_core *mlxsw_core,
 	buf = skb_push(skb, reg->len + sizeof(u32));
 	mlxsw_emad_pack_reg_tlv(buf, reg, payload);
 
+	if (mlxsw_core->emad.enable_latency_tlv) {
+		buf = skb_push(skb, MLXSW_EMAD_LATENCY_TLV_LEN * sizeof(u32));
+		mlxsw_emad_pack_latency_tlv(buf);
+	}
+
 	if (mlxsw_core->emad.enable_string_tlv) {
 		buf = skb_push(skb, MLXSW_EMAD_STRING_TLV_LEN * sizeof(u32));
 		mlxsw_emad_pack_string_tlv(buf);

@@ -520,6 +532,7 @@ static void mlxsw_emad_construct(const struct mlxsw_core *mlxsw_core,
 struct mlxsw_emad_tlv_offsets {
 	u16 op_tlv;
 	u16 string_tlv;
+	u16 latency_tlv;
 	u16 reg_tlv;
 };
 

@@ -530,6 +543,13 @@ static bool mlxsw_emad_tlv_is_string_tlv(const char *tlv)
 	return tlv_type == MLXSW_EMAD_TLV_TYPE_STRING;
 }
 
+static bool mlxsw_emad_tlv_is_latency_tlv(const char *tlv)
+{
+	u8 tlv_type = mlxsw_emad_latency_tlv_type_get(tlv);
+
+	return tlv_type == MLXSW_EMAD_TLV_TYPE_LATENCY;
+}
+
 static void mlxsw_emad_tlv_parse(struct sk_buff *skb)
 {
 	struct mlxsw_emad_tlv_offsets *offsets =

@@ -537,6 +557,8 @@ static void mlxsw_emad_tlv_parse(struct sk_buff *skb)
 
 	offsets->op_tlv = MLXSW_EMAD_ETH_HDR_LEN;
 	offsets->string_tlv = 0;
+	offsets->latency_tlv = 0;
+
 	offsets->reg_tlv = MLXSW_EMAD_ETH_HDR_LEN +
 			   MLXSW_EMAD_OP_TLV_LEN * sizeof(u32);
 

@@ -545,6 +567,11 @@ static void mlxsw_emad_tlv_parse(struct sk_buff *skb)
 		offsets->string_tlv = offsets->reg_tlv;
 		offsets->reg_tlv += MLXSW_EMAD_STRING_TLV_LEN * sizeof(u32);
 	}
+
+	if (mlxsw_emad_tlv_is_latency_tlv(skb->data + offsets->reg_tlv)) {
+		offsets->latency_tlv = offsets->reg_tlv;
+		offsets->reg_tlv += MLXSW_EMAD_LATENCY_TLV_LEN * sizeof(u32);
+	}
 }
 
 static char *mlxsw_emad_op_tlv(const struct sk_buff *skb)

@@ -813,7 +840,7 @@ static const struct mlxsw_listener mlxsw_emad_rx_listener =
 static int mlxsw_emad_tlv_enable(struct mlxsw_core *mlxsw_core)
 {
 	char mgir_pl[MLXSW_REG_MGIR_LEN];
-	bool string_tlv;
+	bool string_tlv, latency_tlv;
 	int err;
 
 	mlxsw_reg_mgir_pack(mgir_pl);

@@ -824,11 +851,15 @@ static int mlxsw_emad_tlv_enable(struct mlxsw_core *mlxsw_core)
 	string_tlv = mlxsw_reg_mgir_fw_info_string_tlv_get(mgir_pl);
 	mlxsw_core->emad.enable_string_tlv = string_tlv;
 
+	latency_tlv = mlxsw_reg_mgir_fw_info_latency_tlv_get(mgir_pl);
+	mlxsw_core->emad.enable_latency_tlv = latency_tlv;
+
 	return 0;
 }
 
 static void mlxsw_emad_tlv_disable(struct mlxsw_core *mlxsw_core)
 {
+	mlxsw_core->emad.enable_latency_tlv = false;
 	mlxsw_core->emad.enable_string_tlv = false;
 }
 

@@ -902,6 +933,8 @@ static struct sk_buff *mlxsw_emad_alloc(const struct mlxsw_core *mlxsw_core,
 		    sizeof(u32) + mlxsw_core->driver->txhdr_len);
 	if (mlxsw_core->emad.enable_string_tlv)
 		emad_len += MLXSW_EMAD_STRING_TLV_LEN * sizeof(u32);
+	if (mlxsw_core->emad.enable_latency_tlv)
+		emad_len +=  MLXSW_EMAD_LATENCY_TLV_LEN * sizeof(u32);
 	if (emad_len > MLXSW_EMAD_MAX_FRAME_LEN)
 		return NULL;