nfp: dumpspec TLV traversal

- Perform dumpspec traversals for calculating size and populating the
  dump.
- Initially, wrap all spec TLVs in dump error TLVs (changed by later
  patches in the series).
Signed-off-by: Carl Heymann <carl.heymann@netronome.com>
Reviewed-by: Jakub Kicinski <jakub.kicinski@netronome.com>
Signed-off-by: Simon Horman <simon.horman@netronome.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/ethernet/netronome/nfp/nfp_net_debugdump.c

@@ -44,6 +44,7 @@
 
 enum nfp_dumpspec_type {
 	NFP_DUMPSPEC_TYPE_PROLOG = 10000,
+	NFP_DUMPSPEC_TYPE_ERROR = 10001,
 };
 
 /* The following structs must be carefully aligned so that they can be used to
@@ -63,6 +64,19 @@ struct nfp_dump_prolog {
 	__be32 dump_level;
 };
 
+struct nfp_dump_error {
+	struct nfp_dump_tl tl;
+	__be32 error;
+	char padding[4];
+	char spec[0];
+};
+
+/* to track state through debug size calculation TLV traversal */
+struct nfp_level_size {
+	u32 requested_level;	/* input */
+	u32 total_size;		/* output */
+};
+
 /* to track state during debug dump creation TLV traversal */
 struct nfp_dump_state {
 	u32 requested_level;	/* input param */
@@ -71,6 +85,43 @@ struct nfp_dump_state {
 	void *p;		/* current point in dump buffer */
 };
 
+typedef int (*nfp_tlv_visit)(struct nfp_pf *pf, struct nfp_dump_tl *tl,
+			     void *param);
+
+static int
+nfp_traverse_tlvs(struct nfp_pf *pf, void *data, u32 data_length, void *param,
+		  nfp_tlv_visit tlv_visit)
+{
+	long long remaining = data_length;
+	struct nfp_dump_tl *tl;
+	u32 total_tlv_size;
+	void *p = data;
+	int err;
+
+	while (remaining >= sizeof(*tl)) {
+		tl = p;
+		if (!tl->type && !tl->length)
+			break;
+
+		if (be32_to_cpu(tl->length) > remaining - sizeof(*tl))
+			return -EINVAL;
+
+		total_tlv_size = sizeof(*tl) + be32_to_cpu(tl->length);
+
+		/* Spec TLVs should be aligned to 4 bytes. */
+		if (total_tlv_size % 4 != 0)
+			return -EINVAL;
+
+		p += total_tlv_size;
+		remaining -= total_tlv_size;
+		err = tlv_visit(pf, tl, param);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
 struct nfp_dumpspec *
 nfp_net_dump_load_dumpspec(struct nfp_cpp *cpp, struct nfp_rtsym_table *rtbl)
 {
@@ -104,10 +155,55 @@ nfp_net_dump_load_dumpspec(struct nfp_cpp *cpp, struct nfp_rtsym_table *rtbl)
 	return dumpspec;
 }
 
+static int nfp_dump_error_tlv_size(struct nfp_dump_tl *spec)
+{
+	return ALIGN8(sizeof(struct nfp_dump_error) + sizeof(*spec) +
+		      be32_to_cpu(spec->length));
+}
+
+static int
+nfp_add_tlv_size(struct nfp_pf *pf, struct nfp_dump_tl *tl, void *param)
+{
+	u32 *size = param;
+
+	switch (be32_to_cpu(tl->type)) {
+	default:
+		*size += nfp_dump_error_tlv_size(tl);
+		break;
+	}
+
+	return 0;
+}
+
+static int
+nfp_calc_specific_level_size(struct nfp_pf *pf, struct nfp_dump_tl *dump_level,
+			     void *param)
+{
+	struct nfp_level_size *lev_sz = param;
+
+	if (be32_to_cpu(dump_level->type) != lev_sz->requested_level)
+		return 0;
+
+	return nfp_traverse_tlvs(pf, dump_level->data,
+				 be32_to_cpu(dump_level->length),
+				 &lev_sz->total_size, nfp_add_tlv_size);
+}
+
 s64 nfp_net_dump_calculate_size(struct nfp_pf *pf, struct nfp_dumpspec *spec,
 				u32 flag)
 {
-	return ALIGN8(sizeof(struct nfp_dump_prolog));
+	struct nfp_level_size lev_sz;
+	int err;
+
+	lev_sz.requested_level = flag;
+	lev_sz.total_size = ALIGN8(sizeof(struct nfp_dump_prolog));
+
+	err = nfp_traverse_tlvs(pf, spec->data, spec->size, &lev_sz,
+				nfp_calc_specific_level_size);
+	if (err)
+		return err;
+
+	return lev_sz.total_size;
 }
 
 static int nfp_add_tlv(u32 type, u32 total_tlv_sz, struct nfp_dump_state *dump)
@@ -129,6 +225,57 @@ static int nfp_add_tlv(u32 type, u32 total_tlv_sz, struct nfp_dump_state *dump)
 	return 0;
 }
 
+static int
+nfp_dump_error_tlv(struct nfp_dump_tl *spec, int error,
+		   struct nfp_dump_state *dump)
+{
+	struct nfp_dump_error *dump_header = dump->p;
+	u32 total_spec_size, total_size;
+	int err;
+
+	total_spec_size = sizeof(*spec) + be32_to_cpu(spec->length);
+	total_size = ALIGN8(sizeof(*dump_header) + total_spec_size);
+
+	err = nfp_add_tlv(NFP_DUMPSPEC_TYPE_ERROR, total_size, dump);
+	if (err)
+		return err;
+
+	dump_header->error = cpu_to_be32(error);
+	memcpy(dump_header->spec, spec, total_spec_size);
+
+	return 0;
+}
+
+static int
+nfp_dump_for_tlv(struct nfp_pf *pf, struct nfp_dump_tl *tl, void *param)
+{
+	struct nfp_dump_state *dump = param;
+	int err;
+
+	switch (be32_to_cpu(tl->type)) {
+	default:
+		err = nfp_dump_error_tlv(tl, -EOPNOTSUPP, dump);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int
+nfp_dump_specific_level(struct nfp_pf *pf, struct nfp_dump_tl *dump_level,
+			void *param)
+{
+	struct nfp_dump_state *dump = param;
+
+	if (be32_to_cpu(dump_level->type) != dump->requested_level)
+		return 0;
+
+	return nfp_traverse_tlvs(pf, dump_level->data,
+				 be32_to_cpu(dump_level->length), dump,
+				 nfp_dump_for_tlv);
+}
+
 static int nfp_dump_populate_prolog(struct nfp_dump_state *dump)
 {
 	struct nfp_dump_prolog *prolog = dump->p;
@@ -161,6 +308,11 @@ int nfp_net_dump_populate_buffer(struct nfp_pf *pf, struct nfp_dumpspec *spec,
 	if (err)
 		return err;
 
+	err = nfp_traverse_tlvs(pf, spec->data, spec->size, &dump,
+				nfp_dump_specific_level);
+	if (err)
+		return err;
+
 	/* Set size of actual dump, to trigger warning if different from
 	 * calculated size.
 	 */