rocker_tlv.h (7331B)
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