| /* GPRS LLC XID field encoding/decoding as per 3GPP TS 44.064 */ |
| |
| /* (C) 2016 by sysmocom s.f.m.c. GmbH <info@sysmocom.de> |
| * All Rights Reserved |
| * |
| * Author: Philipp Maier |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU Affero General Public License as published by |
| * the Free Software Foundation; either version 3 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 Affero General Public License for more details. |
| * |
| * You should have received a copy of the GNU Affero General Public License |
| * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| */ |
| |
| #include <stdio.h> |
| #include <string.h> |
| #include <stdint.h> |
| #include <errno.h> |
| |
| #include <osmocom/core/utils.h> |
| #include <osmocom/core/linuxlist.h> |
| #include <osmocom/core/talloc.h> |
| #include <osmocom/core/msgb.h> |
| #include <osmocom/core/talloc.h> |
| |
| #include <openbsc/debug.h> |
| #include <openbsc/gprs_llc.h> |
| #include <openbsc/sgsn.h> |
| #include <openbsc/gprs_llc_xid.h> |
| |
| /* Parse XID parameter field */ |
| static int decode_xid_field(struct gprs_llc_xid_field *xid_field, |
| const uint8_t *src, uint8_t src_len) |
| { |
| uint8_t xl; |
| uint8_t type; |
| uint8_t len; |
| int src_counter = 0; |
| |
| /* Exit immediately if it is clear that no |
| * parseable data is present */ |
| if (src_len < 1 || !src) |
| return -EINVAL; |
| |
| /* Extract header info */ |
| xl = (*src >> 7) & 1; |
| type = (*src >> 2) & 0x1F; |
| |
| /* Extract length field */ |
| len = (*src) & 0x3; |
| src++; |
| src_counter++; |
| if (xl) { |
| if (src_len < 2) |
| return -EINVAL; |
| len = (len << 6) & 0xC0; |
| len |= ((*src) >> 2) & 0x3F; |
| src++; |
| src_counter++; |
| } |
| |
| /* Fill out struct */ |
| xid_field->type = type; |
| xid_field->data_len = len; |
| if (len > 0) { |
| if (src_len < src_counter + len) |
| return -EINVAL; |
| xid_field->data = |
| talloc_memdup(xid_field,src,xid_field->data_len); |
| } else |
| xid_field->data = NULL; |
| |
| /* Return consumed length */ |
| return src_counter + len; |
| } |
| |
| /* Encode XID parameter field */ |
| static int encode_xid_field(uint8_t *dst, int dst_maxlen, |
| const struct gprs_llc_xid_field *xid_field) |
| { |
| int xl = 0; |
| |
| /* When the length does not fit into 2 bits, |
| * we need extended length fields */ |
| if (xid_field->data_len > 3) |
| xl = 1; |
| |
| /* Exit immediately if it is clear that no |
| * encoding result can be stored */ |
| if (dst_maxlen < xid_field->data_len + 1 + xl) |
| return -EINVAL; |
| |
| /* There are only 5 bits reserved for the type, exit on exceed */ |
| if (xid_field->type > 31) |
| return -EINVAL; |
| |
| /* Encode header */ |
| memset(dst, 0, dst_maxlen); |
| if (xl) |
| dst[0] |= 0x80; |
| dst[0] |= (((xid_field->type) & 0x1F) << 2); |
| |
| if (xl) { |
| dst[0] |= (((xid_field->data_len) >> 6) & 0x03); |
| dst[1] = ((xid_field->data_len) << 2) & 0xFC; |
| } else |
| dst[0] |= ((xid_field->data_len) & 0x03); |
| |
| /* Append payload data */ |
| if (xid_field->data && xid_field->data_len) |
| memcpy(dst + 1 + xl, xid_field->data, xid_field->data_len); |
| |
| /* Return generated length */ |
| return xid_field->data_len + 1 + xl; |
| } |
| |
| /* Transform a list with XID fields into a XID message (dst) */ |
| int gprs_llc_compile_xid(uint8_t *dst, int dst_maxlen, |
| const struct llist_head *xid_fields) |
| { |
| struct gprs_llc_xid_field *xid_field; |
| int rc; |
| int byte_counter = 0; |
| |
| OSMO_ASSERT(xid_fields); |
| OSMO_ASSERT(dst); |
| |
| llist_for_each_entry_reverse(xid_field, xid_fields, list) { |
| /* Encode XID-Field */ |
| rc = encode_xid_field(dst, dst_maxlen, xid_field); |
| if (rc < 0) |
| return -EINVAL; |
| |
| /* Advance pointer and lower maxlen for the |
| * next encoding round */ |
| dst += rc; |
| byte_counter += rc; |
| dst_maxlen -= rc; |
| } |
| |
| /* Return generated length */ |
| return byte_counter; |
| } |
| |
| /* Transform a XID message (dst) into a list of XID fields */ |
| struct llist_head *gprs_llc_parse_xid(const void *ctx, const uint8_t *src, |
| int src_len) |
| { |
| struct gprs_llc_xid_field *xid_field; |
| struct llist_head *xid_fields; |
| |
| int rc; |
| int max_loops = src_len; |
| |
| OSMO_ASSERT(src); |
| |
| xid_fields = talloc_zero(ctx, struct llist_head); |
| INIT_LLIST_HEAD(xid_fields); |
| |
| while (1) { |
| /* Bail in case decode_xid_field() constantly returns zero */ |
| if (max_loops <= 0) { |
| talloc_free(xid_fields); |
| return NULL; |
| } |
| |
| /* Decode XID field */ |
| xid_field = talloc_zero(xid_fields, struct gprs_llc_xid_field); |
| rc = decode_xid_field(xid_field, src, src_len); |
| |
| /* Immediately stop on error */ |
| if (rc < 0) { |
| talloc_free(xid_fields); |
| return NULL; |
| } |
| |
| /* Add parsed XID field to list */ |
| llist_add(&xid_field->list, xid_fields); |
| |
| /* Advance pointer and lower dst_len for the next |
| * decoding round */ |
| src += rc; |
| src_len -= rc; |
| |
| /* We are (scuccessfully) done when no further byes are left */ |
| if (src_len == 0) |
| return xid_fields; |
| |
| max_loops--; |
| } |
| } |
| |
| /* Create a duplicate of an XID-Field */ |
| struct gprs_llc_xid_field *gprs_llc_dup_xid_field(const void *ctx, const struct |
| gprs_llc_xid_field |
| *xid_field) |
| { |
| struct gprs_llc_xid_field *dup; |
| |
| OSMO_ASSERT(xid_field); |
| |
| /* Create a copy of the XID field in memory */ |
| dup = talloc_memdup(ctx, xid_field, sizeof(*xid_field)); |
| dup->data = talloc_memdup(ctx, xid_field->data, xid_field->data_len); |
| |
| /* Unlink duplicate from source list */ |
| INIT_LLIST_HEAD(&dup->list); |
| |
| return dup; |
| } |
| |
| /* Copy an llist with xid fields */ |
| struct llist_head *gprs_llc_copy_xid(const void *ctx, |
| const struct llist_head *xid_fields) |
| { |
| struct gprs_llc_xid_field *xid_field; |
| struct llist_head *xid_fields_copy; |
| |
| OSMO_ASSERT(xid_fields); |
| |
| xid_fields_copy = talloc_zero(ctx, struct llist_head); |
| INIT_LLIST_HEAD(xid_fields_copy); |
| |
| /* Create duplicates and add them to the target list */ |
| llist_for_each_entry(xid_field, xid_fields, list) { |
| llist_add(&gprs_llc_dup_xid_field(ctx, xid_field)->list, |
| xid_fields_copy); |
| } |
| |
| return xid_fields_copy; |
| } |
| |
| /* Dump a list with XID fields (Debug) */ |
| void gprs_llc_dump_xid_fields(const struct llist_head *xid_fields, |
| unsigned int logl) |
| { |
| struct gprs_llc_xid_field *xid_field; |
| |
| OSMO_ASSERT(xid_fields); |
| |
| llist_for_each_entry(xid_field, xid_fields, list) { |
| if (xid_field->data_len) { |
| OSMO_ASSERT(xid_field->data); |
| LOGP(DLLC, logl, |
| "XID: type=%d, data_len=%d, data=%s\n", |
| xid_field->type, xid_field->data_len, |
| osmo_hexdump_nospc(xid_field->data, |
| xid_field->data_len)); |
| } else { |
| LOGP(DLLC, logl, |
| "XID: type=%d, data_len=%d, data=NULL\n", |
| xid_field->type, xid_field->data_len); |
| } |
| } |
| } |