/* * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH * All Rights Reserved. * * Author: Neels Janosch Hofmeyr * * SPDX-License-Identifier: GPL-2.0+ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * */ #include #include #include #include int osmo_gtlv_tag_inst_cmp(const struct osmo_gtlv_tag_inst *a, const struct osmo_gtlv_tag_inst *b) { int cmp; if (a == b) return 0; if (!a) return -1; if (!b) return 1; cmp = OSMO_CMP(a->tag, b->tag); if (cmp) return cmp; cmp = OSMO_CMP(a->instance_present ? 1 : 0, b->instance_present ? 1 : 0); if (cmp) return cmp; if (a->instance_present) return OSMO_CMP(a->instance, b->instance); return 0; } int osmo_gtlv_tag_inst_to_str_buf(char *buf, size_t buflen, const struct osmo_gtlv_tag_inst *ti, const struct value_string *tag_names) { struct osmo_strbuf sb = { .buf = buf, .len = buflen }; if (!tag_names) OSMO_STRBUF_PRINTF(sb, "%u", ti->tag); else OSMO_STRBUF_PRINTF(sb, "%s", get_value_string(tag_names, ti->tag)); if (ti->instance_present) OSMO_STRBUF_PRINTF(sb, "[%u]", ti->instance); return sb.chars_needed; } char *osmo_gtlv_tag_inst_to_str_c(void *ctx, const struct osmo_gtlv_tag_inst *ti, const struct value_string *tag_names) { OSMO_NAME_C_IMPL(ctx, 64, "ERROR", osmo_gtlv_tag_inst_to_str_buf, ti, tag_names) } static int next_tl_valid(const struct osmo_gtlv_load *gtlv, const uint8_t **ie_start_p, size_t *buflen_left_p) { const uint8_t *ie_start; size_t buflen_left; /* Start of next IE, or first IE for first invocation. */ if (!gtlv->val) ie_start = gtlv->src.data; else ie_start = gtlv->val + gtlv->len; /* Sanity */ if (ie_start < gtlv->src.data || ie_start > gtlv->src.data + gtlv->src.len) return -ENOSPC; buflen_left = gtlv->src.len - (ie_start - gtlv->src.data); /* Too short for parsing an IE? Check also against integer overflow. */ if (buflen_left && ((buflen_left < gtlv->cfg->tl_min_size) || (buflen_left > gtlv->src.len))) return -EBADMSG; *ie_start_p = ie_start; *buflen_left_p = buflen_left; return 0; } /* Return a TLV IE from a message buffer. * * Return the first or next TLV data found in the data buffer, based on the state of the gtlv parameter. * When gtlv->val is NULL, return the first IE in the data buffer. * Otherwise assume that gtlv points at a valid IE in the data structure, and return the subsequent IE. * * Usage example: * * struct osmo_gtlv gtlv = { * .cfg = osmo_t16l16v_cfg, * .src = { .data = msgb_l3(msg), .len = msgb_l3len(msg) }, * }; * for (;;) { * if (osmo_gtlv_next(>lv)) { * printf("Error\n"); * break; * } * if (!gtlv.val) { * printf("End\n"); * break; * } * printf("Tag %u: %zu octets: %s\n", gtlv.tag, gtlv.len, osmo_hexdump(gtlv.val, gtlv.len)); * } * * \param[inout] gtlv Buffer to return the IE data, and state for TLV parsing position. gtlv->msg should indicate the * overall message buffer. The other gtlv members should be zero initialized before the first call, and * remain unchanged between invocations of this function. * \returns 0 on success, negative on TLV parsing error. The IE data is returned in gtlv->tag, gtlv->len and gtlv->val; * gtlv->val == NULL if no more IEs remain in the buffer. */ int osmo_gtlv_load_next(struct osmo_gtlv_load *gtlv) { const uint8_t *ie_start; const uint8_t *ie_end; size_t buflen_left; int rc; rc = next_tl_valid(gtlv, &ie_start, &buflen_left); if (rc) return rc; /* No more IEs? */ if (!buflen_left) { gtlv->val = NULL; return 0; } /* Locate next IE */ OSMO_ASSERT(gtlv->cfg->load_tl); gtlv->ti = (struct osmo_gtlv_tag_inst){}; rc = gtlv->cfg->load_tl(gtlv, ie_start, buflen_left); if (rc) return rc; /* Sanity */ ie_end = gtlv->val + gtlv->len; if (ie_end < gtlv->src.data || ie_end > gtlv->src.data + gtlv->src.len) return -EBADMSG; return 0; } /* Return the tag of the IE that osmo_gtlv_next() would yield, do not change the gtlv state. * * \param[in] gtlv state for TLV parsing position; is not modified. * \param[out] tag the tag number on success, if NULL don't return the tag. * \param[out] instance the instance number or OSMO_GTLV_NO_INSTANCE if there is no instance value, * if NULL don't return the instance value. * \returns 0 on success, negative on TLV parsing error, -ENOENT when no more tags follow. */ int osmo_gtlv_load_peek_tag(const struct osmo_gtlv_load *gtlv, struct osmo_gtlv_tag_inst *ti) { const uint8_t *ie_start; size_t buflen_left; int rc; /* Guard against modification by load_tl(). */ struct osmo_gtlv_load mtlv = *gtlv; mtlv.ti = (struct osmo_gtlv_tag_inst){}; rc = next_tl_valid(&mtlv, &ie_start, &buflen_left); if (rc) return rc; if (!buflen_left) return -ENOENT; /* Return next IE tag*/ OSMO_ASSERT(mtlv.cfg->load_tl); rc = gtlv->cfg->load_tl(&mtlv, ie_start, buflen_left); if (rc) return -EBADMSG; if (ti) *ti = mtlv.ti; return 0; } /* Same as osmo_gtlv_load_next(), but skip any IEs until the given tag is reached. Change the gtlv state only when success * is returned. * \param[out] gtlv Return the next IE's TLV info. * \param[in] tag Tag value to match. * \param[in] instance Instance value to match; For IEs that have no instance value (no TLIV), pass * OSMO_GTLV_NO_INSTANCE. * \return 0 when the tag is found. Return -ENOENT when no such tag follows and keep the gtlv unchanged. */ int osmo_gtlv_load_next_by_tag(struct osmo_gtlv_load *gtlv, unsigned int tag) { struct osmo_gtlv_tag_inst ti = { .tag = tag }; return osmo_gtlv_load_next_by_tag_inst(gtlv, &ti); } int osmo_gtlv_load_next_by_tag_inst(struct osmo_gtlv_load *gtlv, const struct osmo_gtlv_tag_inst *ti) { struct osmo_gtlv_load work = *gtlv; for (;;) { int rc = osmo_gtlv_load_next(&work); if (rc) return rc; if (!work.val) return -ENOENT; if (!osmo_gtlv_tag_inst_cmp(&work.ti, ti)) { *gtlv = work; return 0; } } } /* Put tag header and length at the end of the msgb, according to gtlv->cfg->store_tl(). * If the length is not known yet, it can be passed as 0 at first, and osmo_gtlv_put_update_tl() can determine the * resulting length after the value part was put into the msgb. * * Usage example: * * struct msgb *msg = msgb_alloc(1024, "foo"), * struct osmo_gtlv_put gtlv = { * .cfg = osmo_t16l16v_cfg, * .dst = msg, * } * * osmo_gtlv_put_tl(gtlv, 23, 0); // tag 23, length 0 = not known yet * * msgb_put(msg, 42); * ... * msgb_put(msg, 42); * ... * msgb_put(msg, 42); * * osmo_gtlv_put_update_tl(gtlv); * * Return 0 on success, -EINVAL if the tag value is invalid, -EMSGSIZE if len is too large. */ int osmo_gtlv_put_tl(struct osmo_gtlv_put *gtlv, unsigned int tag, size_t len) { struct osmo_gtlv_tag_inst ti = { .tag = tag }; return osmo_gtlv_put_tli(gtlv, &ti, len); } /* Put tag header, instance value and length at the end of the msgb, according to gtlv->cfg->store_tl(). * This is the same as osmo_gtlv_put_tl(), only osmo_gtlv_put_tl() passes instance = 0. */ int osmo_gtlv_put_tli(struct osmo_gtlv_put *gtlv, const struct osmo_gtlv_tag_inst *ti, size_t len) { int rc; uint8_t *last_tl; OSMO_ASSERT(gtlv->cfg->store_tl); last_tl = gtlv->dst->tail; rc = gtlv->cfg->store_tl(gtlv->dst->tail, msgb_tailroom(gtlv->dst), ti, len, gtlv); if (rc < 0) return rc; if (rc > 0) msgb_put(gtlv->dst, rc); gtlv->last_ti = *ti; gtlv->last_tl = last_tl; gtlv->last_val = gtlv->dst->tail; return 0; } /* Update the length of the last put IE header (last call to osmo_gtlv_put_tl()) to match with the current * gtlv->dst->tail. * Return 0 on success, -EMSGSIZE if the amount of data written since osmo_gtlv_put_tl() is too large. */ int osmo_gtlv_put_update_tl(struct osmo_gtlv_put *gtlv) { size_t len = gtlv->dst->tail - gtlv->last_val; int rc = gtlv->cfg->store_tl(gtlv->last_tl, gtlv->last_val - gtlv->last_tl, >lv->last_ti, len, gtlv); if (rc < 0) return rc; /* In case the TL has changed in size, hopefully the implementation has moved the msgb data. Make sure last_val * points at the right place now. */ gtlv->last_val = gtlv->last_tl + rc; return 0; } static int t8l8v_load_tl(struct osmo_gtlv_load *gtlv, const uint8_t *src_data, size_t src_data_len) { /* already validated in next_tl_valid(): src_data_len >= cfg->tl_min_size == 2. */ gtlv->ti.tag = src_data[0]; gtlv->len = src_data[1]; gtlv->val = src_data + 2; return 0; } static int t8l8v_store_tl(uint8_t *dst_data, size_t dst_data_avail, const struct osmo_gtlv_tag_inst *ti, size_t len, struct osmo_gtlv_put *gtlv) { if (ti->tag > UINT8_MAX) return -EINVAL; if (len > UINT8_MAX) return -EMSGSIZE; if (dst_data_avail < 2) return -ENOSPC; dst_data[0] = ti->tag; dst_data[1] = len; return 2; } const struct osmo_gtlv_cfg osmo_t8l8v_cfg = { .tl_min_size = 2, .load_tl = t8l8v_load_tl, .store_tl = t8l8v_store_tl, }; static int t16l16v_load_tl(struct osmo_gtlv_load *gtlv, const uint8_t *src_data, size_t src_data_len) { /* already validated in next_tl_valid(): src_data_len >= cfg->tl_min_size == 4. */ gtlv->ti.tag = osmo_load16be(src_data); gtlv->len = osmo_load16be(src_data + 2); gtlv->val = src_data + 4; return 0; } static int t16l16v_store_tl(uint8_t *dst_data, size_t dst_data_avail, const struct osmo_gtlv_tag_inst *ti, size_t len, struct osmo_gtlv_put *gtlv) { if (ti->tag > UINT16_MAX) return -EINVAL; if (len > UINT16_MAX) return -EMSGSIZE; if (dst_data_avail < 4) return -ENOSPC; osmo_store16be(ti->tag, dst_data); osmo_store16be(len, dst_data + 2); return 4; } const struct osmo_gtlv_cfg osmo_t16l16v_cfg = { .tl_min_size = 4, .load_tl = t16l16v_load_tl, .store_tl = t16l16v_store_tl, };