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, ptr: (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, ptr: 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 | } |
98 | |