libosmogsm
0.11.0-dirty
Osmocom GSM library
|
00001 #pragma once 00002 00003 #include <stdint.h> 00004 #include <string.h> 00005 00006 #include <osmocom/core/msgb.h> 00007 #include <osmocom/core/bit16gen.h> 00008 #include <osmocom/core/bit32gen.h> 00009 00014 /* Terminology / wording 00015 tag length value (in bits) 00016 00017 V - - 8 00018 LV - 8 N * 8 00019 TLV 8 8 N * 8 00020 TL16V 8 16 N * 8 00021 TLV16 8 8 N * 16 00022 TvLV 8 8/16 N * 8 00023 vTvLV 8/16 8/16 N * 8 00024 T16LV 16 8 N * 8 00025 */ 00026 00028 #define LV_GROSS_LEN(x) (x+1) 00029 00030 #define TLV_GROSS_LEN(x) (x+2) 00031 00032 #define TLV16_GROSS_LEN(x) ((2*x)+2) 00033 00034 #define TL16V_GROSS_LEN(x) (x+3) 00035 00036 #define L16TV_GROSS_LEN(x) (x+3) 00037 00038 #define T16LV_GROSS_LEN(x) (x+3) 00039 00041 #define TVLV_MAX_ONEBYTE 0x7f 00042 00044 static inline uint16_t TVLV_GROSS_LEN(uint16_t len) 00045 { 00046 if (len <= TVLV_MAX_ONEBYTE) 00047 return TLV_GROSS_LEN(len); 00048 else 00049 return TL16V_GROSS_LEN(len); 00050 } 00051 00053 static inline uint16_t VTVL_GAN_GROSS_LEN(uint16_t tag, uint16_t len) 00054 { 00055 uint16_t ret = 2; 00056 00057 if (tag > TVLV_MAX_ONEBYTE) 00058 ret++; 00059 00060 if (len > TVLV_MAX_ONEBYTE) 00061 ret++; 00062 00063 return ret; 00064 } 00065 00067 static inline uint16_t VTVLV_GAN_GROSS_LEN(uint16_t tag, uint16_t len) 00068 { 00069 uint16_t ret; 00070 00071 if (len <= TVLV_MAX_ONEBYTE) 00072 ret = TLV_GROSS_LEN(len); 00073 else 00074 ret = TL16V_GROSS_LEN(len); 00075 00076 if (tag > TVLV_MAX_ONEBYTE) 00077 ret += 1; 00078 00079 return ret; 00080 } 00081 00082 /* TLV generation */ 00083 00085 static inline uint8_t *lv_put(uint8_t *buf, uint8_t len, 00086 const uint8_t *val) 00087 { 00088 *buf++ = len; 00089 memcpy(buf, val, len); 00090 return buf + len; 00091 } 00092 00100 static inline uint8_t *tlv_put(uint8_t *buf, uint8_t tag, uint8_t len, 00101 const uint8_t *val) 00102 { 00103 *buf++ = tag; 00104 *buf++ = len; 00105 if (len) { 00106 if (val) 00107 memcpy(buf, val, len); 00108 else 00109 memset(buf, 0, len); 00110 } 00111 return buf + len; 00112 } 00113 00115 static inline uint8_t *tlv16_put(uint8_t *buf, uint8_t tag, uint8_t len, 00116 const uint16_t *val) 00117 { 00118 *buf++ = tag; 00119 *buf++ = len; 00120 memcpy(buf, val, len*2); 00121 return buf + len*2; 00122 } 00123 00125 static inline uint8_t *tl16v_put(uint8_t *buf, uint8_t tag, uint16_t len, 00126 const uint8_t *val) 00127 { 00128 *buf++ = tag; 00129 *buf++ = len >> 8; 00130 *buf++ = len & 0xff; 00131 memcpy(buf, val, len); 00132 return buf + len*2; 00133 } 00134 00136 static inline uint8_t *t16lv_put(uint8_t *buf, uint16_t tag, uint8_t len, 00137 const uint8_t *val) 00138 { 00139 *buf++ = tag >> 8; 00140 *buf++ = tag & 0xff; 00141 *buf++ = len; 00142 memcpy(buf, val, len); 00143 return buf + len + 2; 00144 } 00145 00147 static inline uint8_t *tvlv_put(uint8_t *buf, uint8_t tag, uint16_t len, 00148 const uint8_t *val) 00149 { 00150 uint8_t *ret; 00151 00152 if (len <= TVLV_MAX_ONEBYTE) { 00153 ret = tlv_put(buf, tag, len, val); 00154 buf[1] |= 0x80; 00155 } else 00156 ret = tl16v_put(buf, tag, len, val); 00157 00158 return ret; 00159 } 00160 00162 static inline uint8_t *vt_gan_put(uint8_t *buf, uint16_t tag) 00163 { 00164 if (tag > TVLV_MAX_ONEBYTE) { 00165 /* two-byte TAG */ 00166 *buf++ = 0x80 | (tag >> 8); 00167 *buf++ = (tag & 0xff); 00168 } else 00169 *buf++ = tag; 00170 00171 return buf; 00172 } 00173 00174 /* put (append) vTvL (GAN) field (tag + length)*/ 00175 static inline uint8_t *vtvl_gan_put(uint8_t *buf, uint16_t tag, uint16_t len) 00176 { 00177 uint8_t *ret; 00178 00179 ret = vt_gan_put(buf, tag); 00180 return vt_gan_put(ret, len); 00181 } 00182 00183 /* put (append) vTvLV (GAN) field (tag + length + val) */ 00184 static inline uint8_t *vtvlv_gan_put(uint8_t *buf, uint16_t tag, uint16_t len, 00185 const uint8_t *val) 00186 { 00187 uint8_t *ret; 00188 00189 ret = vtvl_gan_put(buf, tag, len ); 00190 00191 memcpy(ret, val, len); 00192 ret = buf + len; 00193 00194 return ret; 00195 } 00196 00198 static inline uint8_t *msgb_tlv16_put(struct msgb *msg, uint8_t tag, uint8_t len, const uint16_t *val) 00199 { 00200 uint8_t *buf = msgb_put(msg, TLV16_GROSS_LEN(len)); 00201 return tlv16_put(buf, tag, len, val); 00202 } 00203 00205 static inline uint8_t *msgb_tl16v_put(struct msgb *msg, uint8_t tag, uint16_t len, 00206 const uint8_t *val) 00207 { 00208 uint8_t *buf = msgb_put(msg, TL16V_GROSS_LEN(len)); 00209 return tl16v_put(buf, tag, len, val); 00210 } 00211 00212 static inline uint8_t *msgb_t16lv_put(struct msgb *msg, uint16_t tag, uint8_t len, const uint8_t *val) 00213 { 00214 uint8_t *buf = msgb_put(msg, T16LV_GROSS_LEN(len)); 00215 return t16lv_put(buf, tag, len, val); 00216 } 00217 00219 static inline uint8_t *msgb_tvlv_put(struct msgb *msg, uint8_t tag, uint16_t len, 00220 const uint8_t *val) 00221 { 00222 uint8_t *buf = msgb_put(msg, TVLV_GROSS_LEN(len)); 00223 return tvlv_put(buf, tag, len, val); 00224 } 00225 00227 static inline uint8_t *msgb_vtvlv_gan_put(struct msgb *msg, uint16_t tag, 00228 uint16_t len, const uint8_t *val) 00229 { 00230 uint8_t *buf = msgb_put(msg, VTVLV_GAN_GROSS_LEN(tag, len)); 00231 return vtvlv_gan_put(buf, tag, len, val); 00232 } 00233 00235 static inline uint8_t *msgb_l16tv_put(struct msgb *msg, uint16_t len, uint8_t tag, 00236 const uint8_t *val) 00237 { 00238 uint8_t *buf = msgb_put(msg, L16TV_GROSS_LEN(len)); 00239 00240 *buf++ = len >> 8; 00241 *buf++ = len & 0xff; 00242 *buf++ = tag; 00243 memcpy(buf, val, len); 00244 return buf + len; 00245 } 00246 00248 static inline uint8_t *v_put(uint8_t *buf, uint8_t val) 00249 { 00250 *buf++ = val; 00251 return buf; 00252 } 00253 00255 static inline uint8_t *tv_put(uint8_t *buf, uint8_t tag, 00256 uint8_t val) 00257 { 00258 *buf++ = tag; 00259 *buf++ = val; 00260 return buf; 00261 } 00262 00264 static inline uint8_t *tv_fixed_put(uint8_t *buf, uint8_t tag, 00265 unsigned int len, const uint8_t *val) 00266 { 00267 *buf++ = tag; 00268 memcpy(buf, val, len); 00269 return buf + len; 00270 } 00271 00277 static inline uint8_t *tv16_put(uint8_t *buf, uint8_t tag, 00278 uint16_t val) 00279 { 00280 *buf++ = tag; 00281 *buf++ = val >> 8; 00282 *buf++ = val & 0xff; 00283 return buf; 00284 } 00285 00288 static inline uint8_t *msgb_lv_put(struct msgb *msg, uint8_t len, const uint8_t *val) 00289 { 00290 uint8_t *buf = msgb_put(msg, LV_GROSS_LEN(len)); 00291 return lv_put(buf, len, val); 00292 } 00293 00296 static inline uint8_t *msgb_tlv_put(struct msgb *msg, uint8_t tag, uint8_t len, const uint8_t *val) 00297 { 00298 uint8_t *buf = msgb_put(msg, TLV_GROSS_LEN(len)); 00299 return tlv_put(buf, tag, len, val); 00300 } 00301 00304 static inline uint8_t *msgb_tv_put(struct msgb *msg, uint8_t tag, uint8_t val) 00305 { 00306 uint8_t *buf = msgb_put(msg, 2); 00307 return tv_put(buf, tag, val); 00308 } 00309 00312 static inline uint8_t *msgb_tv_fixed_put(struct msgb *msg, uint8_t tag, 00313 unsigned int len, const uint8_t *val) 00314 { 00315 uint8_t *buf = msgb_put(msg, 1+len); 00316 return tv_fixed_put(buf, tag, len, val); 00317 } 00318 00321 static inline uint8_t *msgb_v_put(struct msgb *msg, uint8_t val) 00322 { 00323 uint8_t *buf = msgb_put(msg, 1); 00324 return v_put(buf, val); 00325 } 00326 00329 static inline uint8_t *msgb_tv16_put(struct msgb *msg, uint8_t tag, uint16_t val) 00330 { 00331 uint8_t *buf = msgb_put(msg, 3); 00332 return tv16_put(buf, tag, val); 00333 } 00334 00337 static inline uint8_t *msgb_tlv_push(struct msgb *msg, uint8_t tag, uint8_t len, const uint8_t *val) 00338 { 00339 uint8_t *buf = msgb_push(msg, TLV_GROSS_LEN(len)); 00340 tlv_put(buf, tag, len, val); 00341 return buf; 00342 } 00343 00346 static inline uint8_t *msgb_tv_push(struct msgb *msg, uint8_t tag, uint8_t val) 00347 { 00348 uint8_t *buf = msgb_push(msg, 2); 00349 tv_put(buf, tag, val); 00350 return buf; 00351 } 00352 00355 static inline uint8_t *msgb_tv16_push(struct msgb *msg, uint8_t tag, uint16_t val) 00356 { 00357 uint8_t *buf = msgb_push(msg, 3); 00358 tv16_put(buf, tag, val); 00359 return buf; 00360 } 00361 00364 static inline uint8_t *msgb_tvlv_push(struct msgb *msg, uint8_t tag, uint16_t len, 00365 const uint8_t *val) 00366 { 00367 uint8_t *buf = msgb_push(msg, TVLV_GROSS_LEN(len)); 00368 tvlv_put(buf, tag, len, val); 00369 return buf; 00370 } 00371 00372 /* push (prepend) a vTvL header to a \ref msgb 00373 */ 00374 static inline uint8_t *msgb_vtvl_gan_push(struct msgb *msg, uint16_t tag, 00375 uint16_t len) 00376 { 00377 uint8_t *buf = msgb_push(msg, VTVL_GAN_GROSS_LEN(tag, len)); 00378 vtvl_gan_put(buf, tag, len); 00379 return buf; 00380 } 00381 00382 00383 static inline uint8_t *msgb_vtvlv_gan_push(struct msgb *msg, uint16_t tag, 00384 uint16_t len, const uint8_t *val) 00385 { 00386 uint8_t *buf = msgb_push(msg, VTVLV_GAN_GROSS_LEN(tag, len)); 00387 vtvlv_gan_put(buf, tag, len, val); 00388 return buf; 00389 } 00390 00391 /* TLV parsing */ 00392 00394 struct tlv_p_entry { 00395 uint16_t len; 00396 const uint8_t *val; 00397 }; 00398 00400 enum tlv_type { 00401 TLV_TYPE_NONE, 00402 TLV_TYPE_FIXED, 00403 TLV_TYPE_T, 00404 TLV_TYPE_TV, 00405 TLV_TYPE_TLV, 00406 TLV_TYPE_TL16V, 00407 TLV_TYPE_TvLV, 00408 TLV_TYPE_SINGLE_TV, 00409 TLV_TYPE_vTvLV_GAN, 00410 }; 00411 00413 struct tlv_def { 00414 enum tlv_type type; 00415 uint8_t fixed_len; 00416 }; 00417 00419 struct tlv_definition { 00420 struct tlv_def def[256]; 00421 }; 00422 00424 struct tlv_parsed { 00425 struct tlv_p_entry lv[256]; 00426 }; 00427 00428 extern struct tlv_definition tvlv_att_def; 00429 extern struct tlv_definition vtvlv_gan_att_def; 00430 00431 int tlv_parse_one(uint8_t *o_tag, uint16_t *o_len, const uint8_t **o_val, 00432 const struct tlv_definition *def, 00433 const uint8_t *buf, int buf_len); 00434 int tlv_parse(struct tlv_parsed *dec, const struct tlv_definition *def, 00435 const uint8_t *buf, int buf_len, uint8_t lv_tag, uint8_t lv_tag2); 00436 int tlv_parse2(struct tlv_parsed *dec, int dec_multiples, 00437 const struct tlv_definition *def, const uint8_t *buf, int buf_len, 00438 uint8_t lv_tag, uint8_t lv_tag2); 00439 /* take a master (src) tlv def and fill up all empty slots in 'dst' */ 00440 void tlv_def_patch(struct tlv_definition *dst, const struct tlv_definition *src); 00441 00442 #define TLVP_PRESENT(x, y) ((x)->lv[y].val) 00443 #define TLVP_LEN(x, y) (x)->lv[y].len 00444 #define TLVP_VAL(x, y) (x)->lv[y].val 00445 00446 #define TLVP_PRES_LEN(tp, tag, min_len) \ 00447 (TLVP_PRESENT(tp, tag) && TLVP_LEN(tp, tag) >= min_len) 00448 00459 #define TLVP_GET(_tp, tag) (TLVP_PRESENT(_tp, tag)? &(_tp)->lv[tag] : NULL) 00460 00467 #define TLVP_GET_MINLEN(_tp, tag, min_len) \ 00468 (TLVP_PRES_LEN(_tp, tag, min_len)? &(_tp)->lv[tag] : NULL) 00469 00475 static inline uint16_t tlvp_val16_unal(const struct tlv_parsed *tp, int pos) 00476 { 00477 uint16_t res; 00478 memcpy(&res, TLVP_VAL(tp, pos), sizeof(res)); 00479 return res; 00480 } 00481 00487 static inline uint32_t tlvp_val32_unal(const struct tlv_parsed *tp, int pos) 00488 { 00489 uint32_t res; 00490 memcpy(&res, TLVP_VAL(tp, pos), sizeof(res)); 00491 return res; 00492 } 00493 00499 static inline uint16_t tlvp_val16be(const struct tlv_parsed *tp, int pos) 00500 { 00501 return osmo_load16be(TLVP_VAL(tp, pos)); 00502 } 00503 00509 static inline uint32_t tlvp_val32be(const struct tlv_parsed *tp, int pos) 00510 { 00511 return osmo_load32be(TLVP_VAL(tp, pos)); 00512 } 00513 00514 00515 struct tlv_parsed *osmo_tlvp_copy(const struct tlv_parsed *tp_orig, void *ctx); 00516 int osmo_tlvp_merge(struct tlv_parsed *dst, const struct tlv_parsed *src); 00517 int osmo_shift_v_fixed(uint8_t **data, size_t *data_len, 00518 size_t len, uint8_t **value); 00519 int osmo_match_shift_tv_fixed(uint8_t **data, size_t *data_len, 00520 uint8_t tag, size_t len, uint8_t **value); 00521 int osmo_shift_tlv(uint8_t **data, size_t *data_len, 00522 uint8_t *tag, uint8_t **value, size_t *value_len); 00523 int osmo_match_shift_tlv(uint8_t **data, size_t *data_len, 00524 uint8_t tag, uint8_t **value, size_t *value_len); 00525 int osmo_shift_lv(uint8_t **data, size_t *data_len, 00526 uint8_t **value, size_t *value_len); 00527