drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.c

   1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
   2 /* Copyright (c) 2017-2019 Mellanox Technologies. All rights reserved */
   3
   4 #define pr_fmt(fmt) "MFA2: " fmt
   5
   6 #include "mlxfw_mfa2_tlv_multi.h"
   7 #include <uapi/linux/netlink.h>
   8
   9 #define MLXFW_MFA2_TLV_TOTAL_SIZE(tlv) \
  10         NLA_ALIGN(sizeof(*(tlv)) + be16_to_cpu((tlv)->len))
  11
  12 const struct mlxfw_mfa2_tlv *
  13 mlxfw_mfa2_tlv_multi_child(const struct mlxfw_mfa2_file *mfa2_file,
  14                            const struct mlxfw_mfa2_tlv_multi *multi)
  15 {
  16         size_t multi_len;
  17
  18         multi_len = NLA_ALIGN(sizeof(struct mlxfw_mfa2_tlv_multi));
  19         return mlxfw_mfa2_tlv_get(mfa2_file, (void *) multi + multi_len);
  20 }
  21
  22 const struct mlxfw_mfa2_tlv *
  23 mlxfw_mfa2_tlv_next(const struct mlxfw_mfa2_file *mfa2_file,
  24                     const struct mlxfw_mfa2_tlv *tlv)
  25 {
  26         const struct mlxfw_mfa2_tlv_multi *multi;
  27         u16 tlv_len;
  28         void *next;
  29
  30         tlv_len = MLXFW_MFA2_TLV_TOTAL_SIZE(tlv);
  31
  32         if (tlv->type == MLXFW_MFA2_TLV_MULTI_PART) {
  33                 multi = mlxfw_mfa2_tlv_multi_get(mfa2_file, tlv);
  34                 tlv_len = NLA_ALIGN(tlv_len + be16_to_cpu(multi->total_len));
  35         }
  36
  37         next = (void *) tlv + tlv_len;
  38         return mlxfw_mfa2_tlv_get(mfa2_file, next);
  39 }
  40
  41 const struct mlxfw_mfa2_tlv *
  42 mlxfw_mfa2_tlv_advance(const struct mlxfw_mfa2_file *mfa2_file,
  43                        const struct mlxfw_mfa2_tlv *from_tlv, u16 count)
  44 {
  45         const struct mlxfw_mfa2_tlv *tlv;
  46         u16 idx;
  47
  48         mlxfw_mfa2_tlv_foreach(mfa2_file, tlv, idx, from_tlv, count)
  49                 if (!tlv)
  50                         return NULL;
  51         return tlv;
  52 }
  53
  54 const struct mlxfw_mfa2_tlv *
  55 mlxfw_mfa2_tlv_multi_child_find(const struct mlxfw_mfa2_file *mfa2_file,
  56                                 const struct mlxfw_mfa2_tlv_multi *multi,
  57                                 enum mlxfw_mfa2_tlv_type type, u16 index)
  58 {
  59         const struct mlxfw_mfa2_tlv *tlv;
  60         u16 skip = 0;
  61         u16 idx;
  62
  63         mlxfw_mfa2_tlv_multi_foreach(mfa2_file, tlv, idx, multi) {
  64                 if (!tlv) {
  65                         pr_err("TLV parsing error\n");
  66                         return NULL;
  67                 }
  68                 if (tlv->type == type)
  69                         if (skip++ == index)
  70                                 return tlv;
  71         }
  72         return NULL;
  73 }
  74
  75 int mlxfw_mfa2_tlv_multi_child_count(const struct mlxfw_mfa2_file *mfa2_file,
  76                                      const struct mlxfw_mfa2_tlv_multi *multi,
  77                                      enum mlxfw_mfa2_tlv_type type,
  78                                      u16 *p_count)
  79 {
  80         const struct mlxfw_mfa2_tlv *tlv;
  81         u16 count = 0;
  82         u16 idx;
  83
  84         mlxfw_mfa2_tlv_multi_foreach(mfa2_file, tlv, idx, multi) {
  85                 if (!tlv) {
  86                         pr_err("TLV parsing error\n");
  87                         return -EINVAL;
  88                 }
  89
  90                 if (tlv->type == type)
  91                         count++;
  92         }
  93         *p_count = count;
  94         return 0;
  95 }