Philipp | a536fc6 | 2016-08-10 12:14:57 +0200 | [diff] [blame] | 1 | /* GPRS LLC XID field encoding/decoding as per 3GPP TS 44.064 */ |
| 2 | |
| 3 | /* (C) 2016 by sysmocom s.f.m.c. GmbH <info@sysmocom.de> |
| 4 | * All Rights Reserved |
| 5 | * |
| 6 | * Author: Philipp Maier |
| 7 | * |
| 8 | * This program is free software; you can redistribute it and/or modify |
| 9 | * it under the terms of the GNU Affero General Public License as published by |
| 10 | * the Free Software Foundation; either version 3 of the License, or |
| 11 | * (at your option) any later version. |
| 12 | * |
| 13 | * This program is distributed in the hope that it will be useful, |
| 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 16 | * GNU Affero General Public License for more details. |
| 17 | * |
| 18 | * You should have received a copy of the GNU Affero General Public License |
| 19 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 20 | */ |
| 21 | |
| 22 | #include <stdio.h> |
| 23 | #include <string.h> |
| 24 | #include <stdint.h> |
| 25 | #include <errno.h> |
| 26 | |
| 27 | #include <osmocom/core/utils.h> |
| 28 | #include <osmocom/core/linuxlist.h> |
| 29 | #include <osmocom/core/talloc.h> |
| 30 | #include <osmocom/core/msgb.h> |
| 31 | #include <osmocom/core/talloc.h> |
| 32 | |
| 33 | #include <openbsc/debug.h> |
| 34 | #include <openbsc/gprs_llc.h> |
| 35 | #include <openbsc/sgsn.h> |
| 36 | #include <openbsc/gprs_llc_xid.h> |
| 37 | |
Max | 549ebc7 | 2016-11-18 14:07:04 +0100 | [diff] [blame] | 38 | const struct value_string gprs_llc_xid_type_names[] = { |
| 39 | { GPRS_LLC_XID_T_VERSION, "VERSION"}, |
| 40 | { GPRS_LLC_XID_T_IOV_UI, "IOV_UI"}, |
| 41 | { GPRS_LLC_XID_T_IOV_I, "IOV_I"}, |
| 42 | { GPRS_LLC_XID_T_T200, "T200"}, |
| 43 | { GPRS_LLC_XID_T_N200, "N200"}, |
| 44 | { GPRS_LLC_XID_T_N201_U, "N201_"}, |
| 45 | { GPRS_LLC_XID_T_N201_I, "N201_I"}, |
| 46 | { GPRS_LLC_XID_T_mD, "mD"}, |
| 47 | { GPRS_LLC_XID_T_mU, "mU"}, |
| 48 | { GPRS_LLC_XID_T_kD, "kD"}, |
| 49 | { GPRS_LLC_XID_T_kU, "kU"}, |
| 50 | { GPRS_LLC_XID_T_L3_PAR, "L3_PAR"}, |
| 51 | { GPRS_LLC_XID_T_RESET, "RESET"}, |
| 52 | { 0, NULL }, |
| 53 | }; |
| 54 | |
Philipp | a536fc6 | 2016-08-10 12:14:57 +0200 | [diff] [blame] | 55 | /* Parse XID parameter field */ |
| 56 | static int decode_xid_field(struct gprs_llc_xid_field *xid_field, |
| 57 | const uint8_t *src, uint8_t src_len) |
| 58 | { |
| 59 | uint8_t xl; |
| 60 | uint8_t type; |
| 61 | uint8_t len; |
| 62 | int src_counter = 0; |
| 63 | |
| 64 | /* Exit immediately if it is clear that no |
| 65 | * parseable data is present */ |
| 66 | if (src_len < 1 || !src) |
| 67 | return -EINVAL; |
| 68 | |
| 69 | /* Extract header info */ |
| 70 | xl = (*src >> 7) & 1; |
| 71 | type = (*src >> 2) & 0x1F; |
| 72 | |
| 73 | /* Extract length field */ |
| 74 | len = (*src) & 0x3; |
| 75 | src++; |
| 76 | src_counter++; |
| 77 | if (xl) { |
| 78 | if (src_len < 2) |
| 79 | return -EINVAL; |
| 80 | len = (len << 6) & 0xC0; |
| 81 | len |= ((*src) >> 2) & 0x3F; |
| 82 | src++; |
| 83 | src_counter++; |
| 84 | } |
| 85 | |
| 86 | /* Fill out struct */ |
| 87 | xid_field->type = type; |
| 88 | xid_field->data_len = len; |
| 89 | if (len > 0) { |
| 90 | if (src_len < src_counter + len) |
| 91 | return -EINVAL; |
| 92 | xid_field->data = |
| 93 | talloc_memdup(xid_field,src,xid_field->data_len); |
| 94 | } else |
| 95 | xid_field->data = NULL; |
| 96 | |
| 97 | /* Return consumed length */ |
| 98 | return src_counter + len; |
| 99 | } |
| 100 | |
| 101 | /* Encode XID parameter field */ |
| 102 | static int encode_xid_field(uint8_t *dst, int dst_maxlen, |
| 103 | const struct gprs_llc_xid_field *xid_field) |
| 104 | { |
| 105 | int xl = 0; |
| 106 | |
| 107 | /* When the length does not fit into 2 bits, |
| 108 | * we need extended length fields */ |
| 109 | if (xid_field->data_len > 3) |
| 110 | xl = 1; |
| 111 | |
| 112 | /* Exit immediately if it is clear that no |
| 113 | * encoding result can be stored */ |
| 114 | if (dst_maxlen < xid_field->data_len + 1 + xl) |
| 115 | return -EINVAL; |
| 116 | |
| 117 | /* There are only 5 bits reserved for the type, exit on exceed */ |
| 118 | if (xid_field->type > 31) |
| 119 | return -EINVAL; |
| 120 | |
| 121 | /* Encode header */ |
| 122 | memset(dst, 0, dst_maxlen); |
| 123 | if (xl) |
| 124 | dst[0] |= 0x80; |
| 125 | dst[0] |= (((xid_field->type) & 0x1F) << 2); |
| 126 | |
| 127 | if (xl) { |
| 128 | dst[0] |= (((xid_field->data_len) >> 6) & 0x03); |
| 129 | dst[1] = ((xid_field->data_len) << 2) & 0xFC; |
| 130 | } else |
| 131 | dst[0] |= ((xid_field->data_len) & 0x03); |
| 132 | |
| 133 | /* Append payload data */ |
| 134 | if (xid_field->data && xid_field->data_len) |
| 135 | memcpy(dst + 1 + xl, xid_field->data, xid_field->data_len); |
| 136 | |
| 137 | /* Return generated length */ |
| 138 | return xid_field->data_len + 1 + xl; |
| 139 | } |
| 140 | |
| 141 | /* Transform a list with XID fields into a XID message (dst) */ |
| 142 | int gprs_llc_compile_xid(uint8_t *dst, int dst_maxlen, |
| 143 | const struct llist_head *xid_fields) |
| 144 | { |
| 145 | struct gprs_llc_xid_field *xid_field; |
| 146 | int rc; |
| 147 | int byte_counter = 0; |
| 148 | |
| 149 | OSMO_ASSERT(xid_fields); |
| 150 | OSMO_ASSERT(dst); |
| 151 | |
| 152 | llist_for_each_entry_reverse(xid_field, xid_fields, list) { |
| 153 | /* Encode XID-Field */ |
| 154 | rc = encode_xid_field(dst, dst_maxlen, xid_field); |
| 155 | if (rc < 0) |
| 156 | return -EINVAL; |
| 157 | |
| 158 | /* Advance pointer and lower maxlen for the |
| 159 | * next encoding round */ |
| 160 | dst += rc; |
| 161 | byte_counter += rc; |
| 162 | dst_maxlen -= rc; |
| 163 | } |
| 164 | |
| 165 | /* Return generated length */ |
| 166 | return byte_counter; |
| 167 | } |
| 168 | |
| 169 | /* Transform a XID message (dst) into a list of XID fields */ |
| 170 | struct llist_head *gprs_llc_parse_xid(const void *ctx, const uint8_t *src, |
| 171 | int src_len) |
| 172 | { |
| 173 | struct gprs_llc_xid_field *xid_field; |
| 174 | struct llist_head *xid_fields; |
| 175 | |
| 176 | int rc; |
| 177 | int max_loops = src_len; |
| 178 | |
| 179 | OSMO_ASSERT(src); |
| 180 | |
| 181 | xid_fields = talloc_zero(ctx, struct llist_head); |
| 182 | INIT_LLIST_HEAD(xid_fields); |
| 183 | |
| 184 | while (1) { |
| 185 | /* Bail in case decode_xid_field() constantly returns zero */ |
| 186 | if (max_loops <= 0) { |
| 187 | talloc_free(xid_fields); |
| 188 | return NULL; |
| 189 | } |
| 190 | |
| 191 | /* Decode XID field */ |
| 192 | xid_field = talloc_zero(xid_fields, struct gprs_llc_xid_field); |
| 193 | rc = decode_xid_field(xid_field, src, src_len); |
| 194 | |
| 195 | /* Immediately stop on error */ |
| 196 | if (rc < 0) { |
| 197 | talloc_free(xid_fields); |
| 198 | return NULL; |
| 199 | } |
| 200 | |
| 201 | /* Add parsed XID field to list */ |
| 202 | llist_add(&xid_field->list, xid_fields); |
| 203 | |
| 204 | /* Advance pointer and lower dst_len for the next |
| 205 | * decoding round */ |
| 206 | src += rc; |
| 207 | src_len -= rc; |
| 208 | |
| 209 | /* We are (scuccessfully) done when no further byes are left */ |
| 210 | if (src_len == 0) |
| 211 | return xid_fields; |
| 212 | |
| 213 | max_loops--; |
| 214 | } |
| 215 | } |
| 216 | |
| 217 | /* Create a duplicate of an XID-Field */ |
| 218 | struct gprs_llc_xid_field *gprs_llc_dup_xid_field(const void *ctx, const struct |
| 219 | gprs_llc_xid_field |
| 220 | *xid_field) |
| 221 | { |
| 222 | struct gprs_llc_xid_field *dup; |
| 223 | |
| 224 | OSMO_ASSERT(xid_field); |
| 225 | |
| 226 | /* Create a copy of the XID field in memory */ |
| 227 | dup = talloc_memdup(ctx, xid_field, sizeof(*xid_field)); |
| 228 | dup->data = talloc_memdup(ctx, xid_field->data, xid_field->data_len); |
| 229 | |
| 230 | /* Unlink duplicate from source list */ |
| 231 | INIT_LLIST_HEAD(&dup->list); |
| 232 | |
| 233 | return dup; |
| 234 | } |
| 235 | |
| 236 | /* Copy an llist with xid fields */ |
| 237 | struct llist_head *gprs_llc_copy_xid(const void *ctx, |
| 238 | const struct llist_head *xid_fields) |
| 239 | { |
| 240 | struct gprs_llc_xid_field *xid_field; |
| 241 | struct llist_head *xid_fields_copy; |
| 242 | |
| 243 | OSMO_ASSERT(xid_fields); |
| 244 | |
| 245 | xid_fields_copy = talloc_zero(ctx, struct llist_head); |
| 246 | INIT_LLIST_HEAD(xid_fields_copy); |
| 247 | |
| 248 | /* Create duplicates and add them to the target list */ |
| 249 | llist_for_each_entry(xid_field, xid_fields, list) { |
| 250 | llist_add(&gprs_llc_dup_xid_field(ctx, xid_field)->list, |
| 251 | xid_fields_copy); |
| 252 | } |
| 253 | |
| 254 | return xid_fields_copy; |
| 255 | } |
| 256 | |
| 257 | /* Dump a list with XID fields (Debug) */ |
| 258 | void gprs_llc_dump_xid_fields(const struct llist_head *xid_fields, |
| 259 | unsigned int logl) |
| 260 | { |
| 261 | struct gprs_llc_xid_field *xid_field; |
| 262 | |
| 263 | OSMO_ASSERT(xid_fields); |
| 264 | |
| 265 | llist_for_each_entry(xid_field, xid_fields, list) { |
| 266 | if (xid_field->data_len) { |
| 267 | OSMO_ASSERT(xid_field->data); |
| 268 | LOGP(DLLC, logl, |
Max | 549ebc7 | 2016-11-18 14:07:04 +0100 | [diff] [blame] | 269 | "XID: type %s, data_len=%d, data=%s\n", |
| 270 | get_value_string(gprs_llc_xid_type_names, |
| 271 | xid_field->type), |
| 272 | xid_field->data_len, |
Philipp | a536fc6 | 2016-08-10 12:14:57 +0200 | [diff] [blame] | 273 | osmo_hexdump_nospc(xid_field->data, |
| 274 | xid_field->data_len)); |
| 275 | } else { |
| 276 | LOGP(DLLC, logl, |
| 277 | "XID: type=%d, data_len=%d, data=NULL\n", |
| 278 | xid_field->type, xid_field->data_len); |
| 279 | } |
| 280 | } |
| 281 | } |