Philipp | fcb979a | 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 | |
| 38 | /* Parse XID parameter field */ |
| 39 | static int decode_xid_field(struct gprs_llc_xid_field *xid_field, |
| 40 | const uint8_t *src, uint8_t src_len) |
| 41 | { |
| 42 | uint8_t xl; |
| 43 | uint8_t type; |
| 44 | uint8_t len; |
| 45 | int src_counter = 0; |
| 46 | |
| 47 | /* Exit immediately if it is clear that no |
| 48 | * parseable data is present */ |
| 49 | if (src_len < 1 || !src) |
| 50 | return -EINVAL; |
| 51 | |
| 52 | /* Extract header info */ |
| 53 | xl = (*src >> 7) & 1; |
| 54 | type = (*src >> 2) & 0x1F; |
| 55 | |
| 56 | /* Extract length field */ |
| 57 | len = (*src) & 0x3; |
| 58 | src++; |
| 59 | src_counter++; |
| 60 | if (xl) { |
| 61 | if (src_len < 2) |
| 62 | return -EINVAL; |
| 63 | len = (len << 6) & 0xC0; |
| 64 | len |= ((*src) >> 2) & 0x3F; |
| 65 | src++; |
| 66 | src_counter++; |
| 67 | } |
| 68 | |
| 69 | /* Fill out struct */ |
| 70 | xid_field->type = type; |
| 71 | xid_field->data_len = len; |
| 72 | if (len > 0) { |
| 73 | if (src_len < src_counter + len) |
| 74 | return -EINVAL; |
| 75 | xid_field->data = |
| 76 | talloc_memdup(xid_field,src,xid_field->data_len); |
| 77 | } else |
| 78 | xid_field->data = NULL; |
| 79 | |
| 80 | /* Return consumed length */ |
| 81 | return src_counter + len; |
| 82 | } |
| 83 | |
| 84 | /* Encode XID parameter field */ |
| 85 | static int encode_xid_field(uint8_t *dst, int dst_maxlen, |
| 86 | const struct gprs_llc_xid_field *xid_field) |
| 87 | { |
| 88 | int xl = 0; |
| 89 | |
| 90 | /* When the length does not fit into 2 bits, |
| 91 | * we need extended length fields */ |
| 92 | if (xid_field->data_len > 3) |
| 93 | xl = 1; |
| 94 | |
| 95 | /* Exit immediately if it is clear that no |
| 96 | * encoding result can be stored */ |
| 97 | if (dst_maxlen < xid_field->data_len + 1 + xl) |
| 98 | return -EINVAL; |
| 99 | |
| 100 | /* There are only 5 bits reserved for the type, exit on exceed */ |
| 101 | if (xid_field->type > 31) |
| 102 | return -EINVAL; |
| 103 | |
| 104 | /* Encode header */ |
| 105 | memset(dst, 0, dst_maxlen); |
| 106 | if (xl) |
| 107 | dst[0] |= 0x80; |
| 108 | dst[0] |= (((xid_field->type) & 0x1F) << 2); |
| 109 | |
| 110 | if (xl) { |
| 111 | dst[0] |= (((xid_field->data_len) >> 6) & 0x03); |
| 112 | dst[1] = ((xid_field->data_len) << 2) & 0xFC; |
| 113 | } else |
| 114 | dst[0] |= ((xid_field->data_len) & 0x03); |
| 115 | |
| 116 | /* Append payload data */ |
| 117 | if (xid_field->data && xid_field->data_len) |
| 118 | memcpy(dst + 1 + xl, xid_field->data, xid_field->data_len); |
| 119 | |
| 120 | /* Return generated length */ |
| 121 | return xid_field->data_len + 1 + xl; |
| 122 | } |
| 123 | |
| 124 | /* Transform a list with XID fields into a XID message (dst) */ |
| 125 | int gprs_llc_compile_xid(uint8_t *dst, int dst_maxlen, |
| 126 | const struct llist_head *xid_fields) |
| 127 | { |
| 128 | struct gprs_llc_xid_field *xid_field; |
| 129 | int rc; |
| 130 | int byte_counter = 0; |
| 131 | |
| 132 | OSMO_ASSERT(xid_fields); |
| 133 | OSMO_ASSERT(dst); |
| 134 | |
| 135 | llist_for_each_entry_reverse(xid_field, xid_fields, list) { |
| 136 | /* Encode XID-Field */ |
| 137 | rc = encode_xid_field(dst, dst_maxlen, xid_field); |
| 138 | if (rc < 0) |
| 139 | return -EINVAL; |
| 140 | |
| 141 | /* Advance pointer and lower maxlen for the |
| 142 | * next encoding round */ |
| 143 | dst += rc; |
| 144 | byte_counter += rc; |
| 145 | dst_maxlen -= rc; |
| 146 | } |
| 147 | |
| 148 | /* Return generated length */ |
| 149 | return byte_counter; |
| 150 | } |
| 151 | |
| 152 | /* Transform a XID message (dst) into a list of XID fields */ |
| 153 | struct llist_head *gprs_llc_parse_xid(const void *ctx, const uint8_t *src, |
| 154 | int src_len) |
| 155 | { |
| 156 | struct gprs_llc_xid_field *xid_field; |
| 157 | struct llist_head *xid_fields; |
| 158 | |
| 159 | int rc; |
| 160 | int max_loops = src_len; |
| 161 | |
| 162 | OSMO_ASSERT(src); |
| 163 | |
| 164 | xid_fields = talloc_zero(ctx, struct llist_head); |
| 165 | INIT_LLIST_HEAD(xid_fields); |
| 166 | |
| 167 | while (1) { |
| 168 | /* Bail in case decode_xid_field() constantly returns zero */ |
| 169 | if (max_loops <= 0) { |
| 170 | talloc_free(xid_fields); |
| 171 | return NULL; |
| 172 | } |
| 173 | |
| 174 | /* Decode XID field */ |
| 175 | xid_field = talloc_zero(xid_fields, struct gprs_llc_xid_field); |
| 176 | rc = decode_xid_field(xid_field, src, src_len); |
| 177 | |
| 178 | /* Immediately stop on error */ |
| 179 | if (rc < 0) { |
| 180 | talloc_free(xid_fields); |
| 181 | return NULL; |
| 182 | } |
| 183 | |
| 184 | /* Add parsed XID field to list */ |
| 185 | llist_add(&xid_field->list, xid_fields); |
| 186 | |
| 187 | /* Advance pointer and lower dst_len for the next |
| 188 | * decoding round */ |
| 189 | src += rc; |
| 190 | src_len -= rc; |
| 191 | |
| 192 | /* We are (scuccessfully) done when no further byes are left */ |
| 193 | if (src_len == 0) |
| 194 | return xid_fields; |
| 195 | |
| 196 | max_loops--; |
| 197 | } |
| 198 | } |
| 199 | |
| 200 | /* Create a duplicate of an XID-Field */ |
| 201 | struct gprs_llc_xid_field *gprs_llc_dup_xid_field(const void *ctx, const struct |
| 202 | gprs_llc_xid_field |
| 203 | *xid_field) |
| 204 | { |
| 205 | struct gprs_llc_xid_field *dup; |
| 206 | |
| 207 | OSMO_ASSERT(xid_field); |
| 208 | |
| 209 | /* Create a copy of the XID field in memory */ |
| 210 | dup = talloc_memdup(ctx, xid_field, sizeof(*xid_field)); |
| 211 | dup->data = talloc_memdup(ctx, xid_field->data, xid_field->data_len); |
| 212 | |
| 213 | /* Unlink duplicate from source list */ |
| 214 | INIT_LLIST_HEAD(&dup->list); |
| 215 | |
| 216 | return dup; |
| 217 | } |
| 218 | |
| 219 | /* Copy an llist with xid fields */ |
| 220 | struct llist_head *gprs_llc_copy_xid(const void *ctx, |
| 221 | const struct llist_head *xid_fields) |
| 222 | { |
| 223 | struct gprs_llc_xid_field *xid_field; |
| 224 | struct llist_head *xid_fields_copy; |
| 225 | |
| 226 | OSMO_ASSERT(xid_fields); |
| 227 | |
| 228 | xid_fields_copy = talloc_zero(ctx, struct llist_head); |
| 229 | INIT_LLIST_HEAD(xid_fields_copy); |
| 230 | |
| 231 | /* Create duplicates and add them to the target list */ |
| 232 | llist_for_each_entry(xid_field, xid_fields, list) { |
| 233 | llist_add(&gprs_llc_dup_xid_field(ctx, xid_field)->list, |
| 234 | xid_fields_copy); |
| 235 | } |
| 236 | |
| 237 | return xid_fields_copy; |
| 238 | } |
| 239 | |
| 240 | /* Dump a list with XID fields (Debug) */ |
| 241 | void gprs_llc_dump_xid_fields(const struct llist_head *xid_fields, |
| 242 | unsigned int logl) |
| 243 | { |
| 244 | struct gprs_llc_xid_field *xid_field; |
| 245 | |
| 246 | OSMO_ASSERT(xid_fields); |
| 247 | |
| 248 | llist_for_each_entry(xid_field, xid_fields, list) { |
| 249 | if (xid_field->data_len) { |
| 250 | OSMO_ASSERT(xid_field->data); |
| 251 | LOGP(DLLC, logl, |
| 252 | "XID: type=%d, data_len=%d, data=%s\n", |
| 253 | xid_field->type, xid_field->data_len, |
| 254 | osmo_hexdump_nospc(xid_field->data, |
| 255 | xid_field->data_len)); |
| 256 | } else { |
| 257 | LOGP(DLLC, logl, |
| 258 | "XID: type=%d, data_len=%d, data=NULL\n", |
| 259 | xid_field->type, xid_field->data_len); |
| 260 | } |
| 261 | } |
| 262 | } |