hw/net/rocker/rocker_tlv.h

   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