@samitouri / QOSamiQemu
Code Commits Issues Pulls Sessions CI/CD
master
master
h 244 lines 7.16 KB
Raw
1 /*
2 * QEMU rocker switch emulation - TLV parsing and composing
3 *
4 * Copyright (c) 2014 Jiri Pirko <jiri@resnulli.us>
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 */
16
17 #ifndef ROCKER_TLV_H
18 #define ROCKER_TLV_H
19
20 #define ROCKER_TLV_ALIGNTO 8U
21 #define ROCKER_TLV_ALIGN(len) \
22 (((len) + ROCKER_TLV_ALIGNTO - 1) & ~(ROCKER_TLV_ALIGNTO - 1))
23 #define ROCKER_TLV_HDRLEN ROCKER_TLV_ALIGN(sizeof(RockerTlv))
24
25 /*
26 * <------- ROCKER_TLV_HDRLEN -------> <--- ROCKER_TLV_ALIGN(payload) --->
27 * +-----------------------------+- - -+- - - - - - - - - - - - - - -+- - -+
28 * | Header | Pad | Payload | Pad |
29 * | (RockerTlv) | ing | | ing |
30 * +-----------------------------+- - -+- - - - - - - - - - - - - - -+- - -+
31 * <--------------------------- tlv->len -------------------------->
32 */
33
34 static inline RockerTlv *rocker_tlv_next(const RockerTlv *tlv, int *remaining)
35 {
36 int totlen = ROCKER_TLV_ALIGN(le16_to_cpu(tlv->len));
37
38 *remaining -= totlen;
39 return (RockerTlv *) ((char *) tlv + totlen);
40 }
41
42 static inline int rocker_tlv_ok(const RockerTlv *tlv, int remaining)
43 {
44 return remaining >= (int) ROCKER_TLV_HDRLEN &&
45 le16_to_cpu(tlv->len) >= ROCKER_TLV_HDRLEN &&
46 le16_to_cpu(tlv->len) <= remaining;
47 }
48
49 #define rocker_tlv_for_each(pos, head, len, rem) \
50 for (pos = head, rem = len; \
51 rocker_tlv_ok(pos, rem); \
52 pos = rocker_tlv_next(pos, &(rem)))
53
54 #define rocker_tlv_for_each_nested(pos, tlv, rem) \
55 rocker_tlv_for_each(pos, rocker_tlv_data(tlv), rocker_tlv_len(tlv), rem)
56
57 static inline int rocker_tlv_size(int payload)
58 {
59 return ROCKER_TLV_HDRLEN + payload;
60 }
61
62 static inline int rocker_tlv_total_size(int payload)
63 {
64 return ROCKER_TLV_ALIGN(rocker_tlv_size(payload));
65 }
66
67 static inline int rocker_tlv_padlen(int payload)
68 {
69 return rocker_tlv_total_size(payload) - rocker_tlv_size(payload);
70 }
71
72 static inline int rocker_tlv_type(const RockerTlv *tlv)
73 {
74 return le32_to_cpu(tlv->type);
75 }
76
77 static inline void *rocker_tlv_data(const RockerTlv *tlv)
78 {
79 return (char *) tlv + ROCKER_TLV_HDRLEN;
80 }
81
82 static inline int rocker_tlv_len(const RockerTlv *tlv)
83 {
84 return le16_to_cpu(tlv->len) - ROCKER_TLV_HDRLEN;
85 }
86
87 static inline uint8_t rocker_tlv_get_u8(const RockerTlv *tlv)
88 {
89 return *(uint8_t *) rocker_tlv_data(tlv);
90 }
91
92 static inline uint16_t rocker_tlv_get_u16(const RockerTlv *tlv)
93 {
94 return *(uint16_t *) rocker_tlv_data(tlv);
95 }
96
97 static inline uint32_t rocker_tlv_get_u32(const RockerTlv *tlv)
98 {
99 return *(uint32_t *) rocker_tlv_data(tlv);
100 }
101
102 static inline uint64_t rocker_tlv_get_u64(const RockerTlv *tlv)
103 {
104 return *(uint64_t *) rocker_tlv_data(tlv);
105 }
106
107 static inline uint16_t rocker_tlv_get_le16(const RockerTlv *tlv)
108 {
109 return lduw_le_p(rocker_tlv_data(tlv));
110 }
111
112 static inline uint32_t rocker_tlv_get_le32(const RockerTlv *tlv)
113 {
114 return ldl_le_p(rocker_tlv_data(tlv));
115 }
116
117 static inline uint64_t rocker_tlv_get_le64(const RockerTlv *tlv)
118 {
119 return ldq_le_p(rocker_tlv_data(tlv));
120 }
121
122 static inline void rocker_tlv_parse(RockerTlv **tb, int maxtype,
123 const char *buf, int buf_len)
124 {
125 const RockerTlv *tlv;
126 const RockerTlv *head = (const RockerTlv *) buf;
127 int rem;
128
129 memset(tb, 0, sizeof(RockerTlv *) * (maxtype + 1));
130
131 rocker_tlv_for_each(tlv, head, buf_len, rem) {
132 uint32_t type = rocker_tlv_type(tlv);
133
134 if (type > 0 && type <= maxtype) {
135 tb[type] = (RockerTlv *) tlv;
136 }
137 }
138 }
139
140 static inline void rocker_tlv_parse_nested(RockerTlv **tb, int maxtype,
141 const RockerTlv *tlv)
142 {
143 rocker_tlv_parse(tb, maxtype, rocker_tlv_data(tlv), rocker_tlv_len(tlv));
144 }
145
146 static inline RockerTlv *rocker_tlv_start(char *buf, int buf_pos)
147 {
148 return (RockerTlv *) (buf + buf_pos);
149 }
150
151 static inline void rocker_tlv_put_iov(char *buf, int *buf_pos,
152 int type, const struct iovec *iov,
153 const unsigned int iovcnt)
154 {
155 size_t len = iov_size(iov, iovcnt);
156 int total_size = rocker_tlv_total_size(len);
157 RockerTlv *tlv;
158
159 tlv = rocker_tlv_start(buf, *buf_pos);
160 *buf_pos += total_size;
161 tlv->type = cpu_to_le32(type);
162 tlv->len = cpu_to_le16(rocker_tlv_size(len));
163 iov_to_buf(iov, iovcnt, 0, rocker_tlv_data(tlv), len);
164 memset((char *) tlv + le16_to_cpu(tlv->len), 0, rocker_tlv_padlen(len));
165 }
166
167 static inline void rocker_tlv_put(char *buf, int *buf_pos,
168 int type, int len, void *data)
169 {
170 struct iovec iov = {
171 .iov_base = data,
172 .iov_len = len,
173 };
174
175 rocker_tlv_put_iov(buf, buf_pos, type, &iov, 1);
176 }
177
178 static inline void rocker_tlv_put_u8(char *buf, int *buf_pos,
179 int type, uint8_t value)
180 {
181 rocker_tlv_put(buf, buf_pos, type, sizeof(uint8_t), &value);
182 }
183
184 static inline void rocker_tlv_put_u16(char *buf, int *buf_pos,
185 int type, uint16_t value)
186 {
187 rocker_tlv_put(buf, buf_pos, type, sizeof(uint16_t), &value);
188 }
189
190 static inline void rocker_tlv_put_u32(char *buf, int *buf_pos,
191 int type, uint32_t value)
192 {
193 rocker_tlv_put(buf, buf_pos, type, sizeof(uint32_t), &value);
194 }
195
196 static inline void rocker_tlv_put_u64(char *buf, int *buf_pos,
197 int type, uint64_t value)
198 {
199 rocker_tlv_put(buf, buf_pos, type, sizeof(uint64_t), &value);
200 }
201
202 static inline void rocker_tlv_put_le16(char *buf, int *buf_pos,
203 int type, uint16_t value)
204 {
205 value = cpu_to_le16(value);
206 rocker_tlv_put(buf, buf_pos, type, sizeof(uint16_t), &value);
207 }
208
209 static inline void rocker_tlv_put_le32(char *buf, int *buf_pos,
210 int type, uint32_t value)
211 {
212 value = cpu_to_le32(value);
213 rocker_tlv_put(buf, buf_pos, type, sizeof(uint32_t), &value);
214 }
215
216 static inline void rocker_tlv_put_le64(char *buf, int *buf_pos,
217 int type, uint64_t value)
218 {
219 value = cpu_to_le64(value);
220 rocker_tlv_put(buf, buf_pos, type, sizeof(uint64_t), &value);
221 }
222
223 static inline RockerTlv *rocker_tlv_nest_start(char *buf, int *buf_pos,
224 int type)
225 {
226 RockerTlv *start = rocker_tlv_start(buf, *buf_pos);
227
228 rocker_tlv_put(buf, buf_pos, type, 0, NULL);
229 return start;
230 }
231
232 static inline void rocker_tlv_nest_end(char *buf, int *buf_pos,
233 RockerTlv *start)
234 {
235 start->len = (char *) rocker_tlv_start(buf, *buf_pos) - (char *) start;
236 }
237
238 static inline void rocker_tlv_nest_cancel(char *buf, int *buf_pos,
239 RockerTlv *start)
240 {
241 *buf_pos = (char *) start - buf;
242 }
243
244 #endif