libosmogsm
0.9.3
Osmocom GSM library
|
00001 #pragma once 00002 00003 #include <stdint.h> 00004 #include <string.h> 00005 00006 #include <osmocom/core/msgb.h> 00007 00013 /* Terminology / wording 00014 tag length value (in bits) 00015 00016 V - - 8 00017 LV - 8 N * 8 00018 TLV 8 8 N * 8 00019 TL16V 8 16 N * 8 00020 TLV16 8 8 N * 16 00021 TvLV 8 8/16 N * 8 00022 vTvLV 8/16 8/16 N * 8 00023 00024 */ 00025 00027 #define LV_GROSS_LEN(x) (x+1) 00028 00029 #define TLV_GROSS_LEN(x) (x+2) 00030 00031 #define TLV16_GROSS_LEN(x) ((2*x)+2) 00032 00033 #define TL16V_GROSS_LEN(x) (x+3) 00034 00035 #define L16TV_GROSS_LEN(x) (x+3) 00036 00038 #define TVLV_MAX_ONEBYTE 0x7f 00039 00041 static inline uint16_t TVLV_GROSS_LEN(uint16_t len) 00042 { 00043 if (len <= TVLV_MAX_ONEBYTE) 00044 return TLV_GROSS_LEN(len); 00045 else 00046 return TL16V_GROSS_LEN(len); 00047 } 00048 00050 static inline uint16_t VTVL_GAN_GROSS_LEN(uint16_t tag, uint16_t len) 00051 { 00052 uint16_t ret = 2; 00053 00054 if (tag > TVLV_MAX_ONEBYTE) 00055 ret++; 00056 00057 if (len > TVLV_MAX_ONEBYTE) 00058 ret++; 00059 00060 return ret; 00061 } 00062 00064 static inline uint16_t VTVLV_GAN_GROSS_LEN(uint16_t tag, uint16_t len) 00065 { 00066 uint16_t ret; 00067 00068 if (len <= TVLV_MAX_ONEBYTE) 00069 return TLV_GROSS_LEN(len); 00070 else 00071 return TL16V_GROSS_LEN(len); 00072 00073 if (tag > TVLV_MAX_ONEBYTE) 00074 ret += 1; 00075 00076 return ret; 00077 } 00078 00079 /* TLV generation */ 00080 00082 static inline uint8_t *lv_put(uint8_t *buf, uint8_t len, 00083 const uint8_t *val) 00084 { 00085 *buf++ = len; 00086 memcpy(buf, val, len); 00087 return buf + len; 00088 } 00089 00091 static inline uint8_t *tlv_put(uint8_t *buf, uint8_t tag, uint8_t len, 00092 const uint8_t *val) 00093 { 00094 *buf++ = tag; 00095 *buf++ = len; 00096 memcpy(buf, val, len); 00097 return buf + len; 00098 } 00099 00101 static inline uint8_t *tlv16_put(uint8_t *buf, uint8_t tag, uint8_t len, 00102 const uint16_t *val) 00103 { 00104 *buf++ = tag; 00105 *buf++ = len; 00106 memcpy(buf, val, len*2); 00107 return buf + len*2; 00108 } 00109 00111 static inline uint8_t *tl16v_put(uint8_t *buf, uint8_t tag, uint16_t len, 00112 const uint8_t *val) 00113 { 00114 *buf++ = tag; 00115 *buf++ = len >> 8; 00116 *buf++ = len & 0xff; 00117 memcpy(buf, val, len); 00118 return buf + len*2; 00119 } 00120 00122 static inline uint8_t *tvlv_put(uint8_t *buf, uint8_t tag, uint16_t len, 00123 const uint8_t *val) 00124 { 00125 uint8_t *ret; 00126 00127 if (len <= TVLV_MAX_ONEBYTE) { 00128 ret = tlv_put(buf, tag, len, val); 00129 buf[1] |= 0x80; 00130 } else 00131 ret = tl16v_put(buf, tag, len, val); 00132 00133 return ret; 00134 } 00135 00137 static inline uint8_t *vt_gan_put(uint8_t *buf, uint16_t tag) 00138 { 00139 if (tag > TVLV_MAX_ONEBYTE) { 00140 /* two-byte TAG */ 00141 *buf++ = 0x80 | (tag >> 8); 00142 *buf++ = (tag & 0xff); 00143 } else 00144 *buf++ = tag; 00145 00146 return buf; 00147 } 00148 00149 /* \brief put (append) vTvL (GAN) field (tag + length)*/ 00150 static inline uint8_t *vtvl_gan_put(uint8_t *buf, uint16_t tag, uint16_t len) 00151 { 00152 uint8_t *ret; 00153 00154 ret = vt_gan_put(buf, tag); 00155 return vt_gan_put(ret, len); 00156 } 00157 00158 /* \brief put (append) vTvLV (GAN) field (tag + length + val) */ 00159 static inline uint8_t *vtvlv_gan_put(uint8_t *buf, uint16_t tag, uint16_t len, 00160 const uint8_t *val) 00161 { 00162 uint8_t *ret; 00163 00164 ret = vtvl_gan_put(buf, tag, len ); 00165 00166 memcpy(ret, val, len); 00167 ret = buf + len; 00168 00169 return ret; 00170 } 00171 00173 static inline uint8_t *msgb_tlv16_put(struct msgb *msg, uint8_t tag, uint8_t len, const uint16_t *val) 00174 { 00175 uint8_t *buf = msgb_put(msg, TLV16_GROSS_LEN(len)); 00176 return tlv16_put(buf, tag, len, val); 00177 } 00178 00180 static inline uint8_t *msgb_tl16v_put(struct msgb *msg, uint8_t tag, uint16_t len, 00181 const uint8_t *val) 00182 { 00183 uint8_t *buf = msgb_put(msg, TL16V_GROSS_LEN(len)); 00184 return tl16v_put(buf, tag, len, val); 00185 } 00186 00188 static inline uint8_t *msgb_tvlv_put(struct msgb *msg, uint8_t tag, uint16_t len, 00189 const uint8_t *val) 00190 { 00191 uint8_t *buf = msgb_put(msg, TVLV_GROSS_LEN(len)); 00192 return tvlv_put(buf, tag, len, val); 00193 } 00194 00196 static inline uint8_t *msgb_vtvlv_gan_put(struct msgb *msg, uint16_t tag, 00197 uint16_t len, const uint8_t *val) 00198 { 00199 uint8_t *buf = msgb_put(msg, VTVLV_GAN_GROSS_LEN(tag, len)); 00200 return vtvlv_gan_put(buf, tag, len, val); 00201 } 00202 00204 static inline uint8_t *msgb_l16tv_put(struct msgb *msg, uint16_t len, uint8_t tag, 00205 const uint8_t *val) 00206 { 00207 uint8_t *buf = msgb_put(msg, L16TV_GROSS_LEN(len)); 00208 00209 *buf++ = len >> 8; 00210 *buf++ = len & 0xff; 00211 *buf++ = tag; 00212 memcpy(buf, val, len); 00213 return buf + len; 00214 } 00215 00217 static inline uint8_t *v_put(uint8_t *buf, uint8_t val) 00218 { 00219 *buf++ = val; 00220 return buf; 00221 } 00222 00224 static inline uint8_t *tv_put(uint8_t *buf, uint8_t tag, 00225 uint8_t val) 00226 { 00227 *buf++ = tag; 00228 *buf++ = val; 00229 return buf; 00230 } 00231 00233 static inline uint8_t *tv_fixed_put(uint8_t *buf, uint8_t tag, 00234 unsigned int len, const uint8_t *val) 00235 { 00236 *buf++ = tag; 00237 memcpy(buf, val, len); 00238 return buf + len; 00239 } 00240 00246 static inline uint8_t *tv16_put(uint8_t *buf, uint8_t tag, 00247 uint16_t val) 00248 { 00249 *buf++ = tag; 00250 *buf++ = val >> 8; 00251 *buf++ = val & 0xff; 00252 return buf; 00253 } 00254 00257 static inline uint8_t *msgb_lv_put(struct msgb *msg, uint8_t len, const uint8_t *val) 00258 { 00259 uint8_t *buf = msgb_put(msg, LV_GROSS_LEN(len)); 00260 return lv_put(buf, len, val); 00261 } 00262 00265 static inline uint8_t *msgb_tlv_put(struct msgb *msg, uint8_t tag, uint8_t len, const uint8_t *val) 00266 { 00267 uint8_t *buf = msgb_put(msg, TLV_GROSS_LEN(len)); 00268 return tlv_put(buf, tag, len, val); 00269 } 00270 00273 static inline uint8_t *msgb_tv_put(struct msgb *msg, uint8_t tag, uint8_t val) 00274 { 00275 uint8_t *buf = msgb_put(msg, 2); 00276 return tv_put(buf, tag, val); 00277 } 00278 00281 static inline uint8_t *msgb_tv_fixed_put(struct msgb *msg, uint8_t tag, 00282 unsigned int len, const uint8_t *val) 00283 { 00284 uint8_t *buf = msgb_put(msg, 1+len); 00285 return tv_fixed_put(buf, tag, len, val); 00286 } 00287 00290 static inline uint8_t *msgb_v_put(struct msgb *msg, uint8_t val) 00291 { 00292 uint8_t *buf = msgb_put(msg, 1); 00293 return v_put(buf, val); 00294 } 00295 00298 static inline uint8_t *msgb_tv16_put(struct msgb *msg, uint8_t tag, uint16_t val) 00299 { 00300 uint8_t *buf = msgb_put(msg, 3); 00301 return tv16_put(buf, tag, val); 00302 } 00303 00306 static inline uint8_t *msgb_tlv_push(struct msgb *msg, uint8_t tag, uint8_t len, const uint8_t *val) 00307 { 00308 uint8_t *buf = msgb_push(msg, TLV_GROSS_LEN(len)); 00309 tlv_put(buf, tag, len, val); 00310 return buf; 00311 } 00312 00315 static inline uint8_t *msgb_tv_push(struct msgb *msg, uint8_t tag, uint8_t val) 00316 { 00317 uint8_t *buf = msgb_push(msg, 2); 00318 tv_put(buf, tag, val); 00319 return buf; 00320 } 00321 00324 static inline uint8_t *msgb_tv16_push(struct msgb *msg, uint8_t tag, uint16_t val) 00325 { 00326 uint8_t *buf = msgb_push(msg, 3); 00327 tv16_put(buf, tag, val); 00328 return buf; 00329 } 00330 00333 static inline uint8_t *msgb_tvlv_push(struct msgb *msg, uint8_t tag, uint16_t len, 00334 const uint8_t *val) 00335 { 00336 uint8_t *buf = msgb_push(msg, TVLV_GROSS_LEN(len)); 00337 tvlv_put(buf, tag, len, val); 00338 return buf; 00339 } 00340 00341 /* \brief push (prepend) a vTvL header to a \ref msgb 00342 */ 00343 static inline uint8_t *msgb_vtvl_gan_push(struct msgb *msg, uint16_t tag, 00344 uint16_t len) 00345 { 00346 uint8_t *buf = msgb_push(msg, VTVL_GAN_GROSS_LEN(tag, len)); 00347 vtvl_gan_put(buf, tag, len); 00348 return buf; 00349 } 00350 00351 00352 static inline uint8_t *msgb_vtvlv_gan_push(struct msgb *msg, uint16_t tag, 00353 uint16_t len, const uint8_t *val) 00354 { 00355 uint8_t *buf = msgb_push(msg, VTVLV_GAN_GROSS_LEN(tag, len)); 00356 vtvlv_gan_put(buf, tag, len, val); 00357 return buf; 00358 } 00359 00360 /* TLV parsing */ 00361 00363 struct tlv_p_entry { 00364 uint16_t len; 00365 const uint8_t *val; 00366 }; 00367 00369 enum tlv_type { 00370 TLV_TYPE_NONE, 00371 TLV_TYPE_FIXED, 00372 TLV_TYPE_T, 00373 TLV_TYPE_TV, 00374 TLV_TYPE_TLV, 00375 TLV_TYPE_TL16V, 00376 TLV_TYPE_TvLV, 00377 TLV_TYPE_SINGLE_TV, 00378 TLV_TYPE_vTvLV_GAN, 00379 }; 00380 00382 struct tlv_def { 00383 enum tlv_type type; 00384 uint8_t fixed_len; 00385 }; 00386 00388 struct tlv_definition { 00389 struct tlv_def def[256]; 00390 }; 00391 00393 struct tlv_parsed { 00394 struct tlv_p_entry lv[256]; 00395 }; 00396 00397 extern struct tlv_definition tvlv_att_def; 00398 extern struct tlv_definition vtvlv_gan_att_def; 00399 00400 int tlv_parse_one(uint8_t *o_tag, uint16_t *o_len, const uint8_t **o_val, 00401 const struct tlv_definition *def, 00402 const uint8_t *buf, int buf_len); 00403 int tlv_parse(struct tlv_parsed *dec, const struct tlv_definition *def, 00404 const uint8_t *buf, int buf_len, uint8_t lv_tag, uint8_t lv_tag2); 00405 /* take a master (src) tlvdev and fill up all empty slots in 'dst' */ 00406 void tlv_def_patch(struct tlv_definition *dst, const struct tlv_definition *src); 00407 00408 #define TLVP_PRESENT(x, y) ((x)->lv[y].val) 00409 #define TLVP_LEN(x, y) (x)->lv[y].len 00410 #define TLVP_VAL(x, y) (x)->lv[y].val 00411 00412 #define TLVP_PRES_LEN(tp, tag, min_len) \ 00413 (TLVP_PRESENT(tp, tag) && TLVP_LEN(tp, tag) >= min_len) 00414 00420 static inline uint16_t tlvp_val16_unal(const struct tlv_parsed *tp, int pos) 00421 { 00422 uint16_t res; 00423 memcpy(&res, TLVP_VAL(tp, pos), sizeof(res)); 00424 return res; 00425 } 00426 00432 static inline uint32_t tlvp_val32_unal(const struct tlv_parsed *tp, int pos) 00433 { 00434 uint32_t res; 00435 memcpy(&res, TLVP_VAL(tp, pos), sizeof(res)); 00436 return res; 00437 } 00438