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                 if (!multi)
  35                         return NULL;
  36                 tlv_len = NLA_ALIGN(tlv_len + be16_to_cpu(multi->total_len));
  37         }
  38
  39         next = (void *) tlv + tlv_len;
  40         return mlxfw_mfa2_tlv_get(mfa2_file, next);
  41 }
  42
  43 const struct mlxfw_mfa2_tlv *
  44 mlxfw_mfa2_tlv_advance(const struct mlxfw_mfa2_file *mfa2_file,
  45                        const struct mlxfw_mfa2_tlv *from_tlv, u16 count)
  46 {
  47         const struct mlxfw_mfa2_tlv *tlv;
  48         u16 idx;
  49
  50         mlxfw_mfa2_tlv_foreach(mfa2_file, tlv, idx, from_tlv, count)
  51                 if (!tlv)
  52                         return NULL;
  53         return tlv;
  54 }
  55
  56 const struct mlxfw_mfa2_tlv *
  57 mlxfw_mfa2_tlv_multi_child_find(const struct mlxfw_mfa2_file *mfa2_file,
  58                                 const struct mlxfw_mfa2_tlv_multi *multi,
  59                                 enum mlxfw_mfa2_tlv_type type, u16 index)
  60 {
  61         const struct mlxfw_mfa2_tlv *tlv;
  62         u16 skip = 0;
  63         u16 idx;
  64
  65         mlxfw_mfa2_tlv_multi_foreach(mfa2_file, tlv, idx, multi) {
  66                 if (!tlv) {
  67                         pr_err("TLV parsing error\n");
  68                         return NULL;
  69                 }
  70                 if (tlv->type == type)
  71                         if (skip++ == index)
  72                                 return tlv;
  73         }
  74         return NULL;
  75 }
  76
  77 int mlxfw_mfa2_tlv_multi_child_count(const struct mlxfw_mfa2_file *mfa2_file,
  78                                      const struct mlxfw_mfa2_tlv_multi *multi,
  79                                      enum mlxfw_mfa2_tlv_type type,
  80                                      u16 *p_count)
  81 {
  82         const struct mlxfw_mfa2_tlv *tlv;
  83         u16 count = 0;
  84         u16 idx;
  85
  86         mlxfw_mfa2_tlv_multi_foreach(mfa2_file, tlv, idx, multi) {
  87                 if (!tlv) {
  88                         pr_err("TLV parsing error\n");
  89                         return -EINVAL;
  90                 }
  91
  92                 if (tlv->type == type)
  93                         count++;
  94         }
  95         *p_count = count;
  96         return 0;
  97 }