Harald Welte | 3a19440 | 2017-07-22 17:07:51 +0200 | [diff] [blame] | 1 | |
| 2 | #include "Octetstring.hh" |
| 3 | #include "Error.hh" |
| 4 | #include "Logger.hh" |
| 5 | |
| 6 | #include <stdint.h> |
| 7 | |
| 8 | namespace BSSGP__Helper__Functions { |
| 9 | |
| 10 | /* convert a buffer filled with TLVs that have variable-length "length" fields (Osmocom TvLV) into a |
| 11 | * buffer filled with TLVs that have fixed 16-bit length values (TL16V format) */ |
Harald Welte | f1fd016 | 2017-07-22 20:34:05 +0200 | [diff] [blame] | 12 | static OCTETSTRING expand_tlv_part(OCTETSTRING const &in) |
Harald Welte | 3a19440 | 2017-07-22 17:07:51 +0200 | [diff] [blame] | 13 | { |
| 14 | const unsigned char *in_ptr = (const unsigned char *)in; |
| 15 | int in_len = in.lengthof(); |
| 16 | int ofs = 0; |
| 17 | uint16_t data_len; |
| 18 | OCTETSTRING out(0, (const unsigned char *)""); |
| 19 | |
| 20 | while (ofs < in_len) { |
| 21 | int remain_len = in_len - ofs; |
| 22 | int tl_length; |
| 23 | |
| 24 | if (remain_len < 2) { |
| 25 | TTCN_error("Remaining input length (%d) insufficient for Tag+Length", remain_len); |
| 26 | break; |
| 27 | } |
| 28 | |
| 29 | /* copy over tag */ |
| 30 | if (in_ptr[ofs+1] & 0x80) { |
| 31 | /* E bit is set, 7-bit length field */ |
| 32 | data_len = in_ptr[ofs+1] & 0x7F; |
| 33 | tl_length = 2; |
| 34 | } else { |
| 35 | /* E bit is not set, 15 bit length field */ |
Harald Welte | f1fd016 | 2017-07-22 20:34:05 +0200 | [diff] [blame] | 36 | if (remain_len < 3) { |
Harald Welte | 3a19440 | 2017-07-22 17:07:51 +0200 | [diff] [blame] | 37 | TTCN_error("Remaining input length insufficient for 2-octet length"); |
| 38 | break; |
| 39 | } |
| 40 | data_len = in_ptr[ofs+1] << 8 | in_ptr[ofs+2]; |
| 41 | tl_length = 3; |
| 42 | } |
| 43 | if (in_len < tl_length + data_len) { |
| 44 | TTCN_error("Remaining input length insufficient for TLV value length"); |
| 45 | break; |
| 46 | } |
| 47 | |
| 48 | /* Tag + 16bit length */ |
| 49 | uint8_t hdr_buf[3]; |
| 50 | hdr_buf[0] = in_ptr[ofs+0]; |
| 51 | hdr_buf[1] = data_len >> 8; |
| 52 | hdr_buf[2] = data_len & 0xff; |
| 53 | |
| 54 | OCTETSTRING tlv_hdr(3, hdr_buf); |
| 55 | out += tlv_hdr; |
| 56 | |
| 57 | if (data_len) { |
| 58 | /* append octet string of current TLV to output octetstring */ |
| 59 | OCTETSTRING tlv_val(data_len, in_ptr + ofs + tl_length); |
| 60 | out += tlv_val; |
| 61 | } |
| 62 | |
| 63 | /* advance input offset*/ |
| 64 | ofs += data_len + tl_length; |
| 65 | } |
| 66 | |
| 67 | return out; |
| 68 | } |
| 69 | |
Harald Welte | f1fd016 | 2017-07-22 20:34:05 +0200 | [diff] [blame] | 70 | /* convert a buffer filled with TLVs that have fixed-length "length" fields (Osmocom TL16V) into a |
| 71 | * buffer filled with TLVs that have variable-length values (TvLV format) */ |
| 72 | static OCTETSTRING compact_tlv_part(OCTETSTRING const &in) |
| 73 | { |
| 74 | const unsigned char *in_ptr = (const unsigned char *)in; |
| 75 | int in_len = in.lengthof(); |
| 76 | int ofs = 0; |
| 77 | uint16_t data_len; |
| 78 | OCTETSTRING out(0, (const unsigned char *)""); |
| 79 | |
| 80 | while (ofs < in_len) { |
| 81 | int remain_len = in_len - ofs; |
| 82 | int tl_length; |
| 83 | |
| 84 | if (remain_len < 3) { |
| 85 | TTCN_error("Remaining input length (%d) insufficient for Tag+Length", remain_len); |
| 86 | break; |
| 87 | } |
| 88 | |
| 89 | data_len = (in_ptr[ofs+1] << 8) | in_ptr[ofs+2]; |
| 90 | |
| 91 | /* Tag + 16bit length */ |
| 92 | uint8_t hdr_buf[3]; |
| 93 | hdr_buf[0] = in_ptr[ofs+0]; |
| 94 | |
| 95 | if (data_len <= 0x7f) { |
| 96 | /* E bit is set, 7-bit length field */ |
| 97 | hdr_buf[1] = 0x80 | data_len; |
| 98 | tl_length = 2; |
| 99 | } else { |
| 100 | /* E bit is not set, 15 bit length field */ |
| 101 | hdr_buf[1] = data_len >> 8; |
| 102 | hdr_buf[2] = data_len & 0xff; |
| 103 | tl_length = 3; |
| 104 | } |
| 105 | if (in_len < 3 + data_len) { |
| 106 | TTCN_error("Remaining input length insufficient for TLV value length"); |
| 107 | break; |
| 108 | } |
| 109 | |
| 110 | OCTETSTRING tlv_hdr(tl_length, hdr_buf); |
| 111 | out += tlv_hdr; |
| 112 | |
| 113 | if (data_len) { |
| 114 | /* append octet string of current TLV to output octetstring */ |
| 115 | OCTETSTRING tlv_val(data_len, in_ptr + ofs + 3); |
| 116 | out += tlv_val; |
| 117 | } |
| 118 | |
| 119 | /* advance input offset*/ |
| 120 | ofs += data_len + 3; |
| 121 | } |
| 122 | |
| 123 | return out; |
| 124 | } |
| 125 | |
Harald Welte | 3a19440 | 2017-07-22 17:07:51 +0200 | [diff] [blame] | 126 | #define BSSGP_PDUT_DL_UNITDATA 0x00 |
| 127 | #define BSSGP_PDUT_UL_UNITDATA 0x01 |
| 128 | |
Harald Welte | f1fd016 | 2017-07-22 20:34:05 +0200 | [diff] [blame] | 129 | /* expand all the variable-length "length" fields of a BSSGP message (Osmocom TvLV) into statlc TL16V format */ |
| 130 | OCTETSTRING f__BSSGP__expand__len(OCTETSTRING const &in) |
Harald Welte | 3a19440 | 2017-07-22 17:07:51 +0200 | [diff] [blame] | 131 | { |
| 132 | const unsigned char *in_ptr = (const unsigned char *)in; |
| 133 | int in_len = in.lengthof(); |
| 134 | uint8_t pdu_type = in_ptr[0]; |
| 135 | uint8_t static_hdr_len = 1; |
| 136 | |
| 137 | if (pdu_type == BSSGP_PDUT_DL_UNITDATA || pdu_type == BSSGP_PDUT_UL_UNITDATA) |
| 138 | static_hdr_len = 8; |
| 139 | |
| 140 | if (in_len < static_hdr_len) |
| 141 | TTCN_error("BSSGP message is shorter (%u bytes) than minimum header length (%u bytes) for msg_type 0x%02x", |
| 142 | in_len, static_hdr_len, pdu_type); |
| 143 | |
| 144 | /* prefix = non-TLV section of header */ |
| 145 | OCTETSTRING prefix(static_hdr_len, in_ptr); |
| 146 | OCTETSTRING tlv_part_in(in_len - static_hdr_len, in_ptr + static_hdr_len); |
| 147 | |
Harald Welte | f1fd016 | 2017-07-22 20:34:05 +0200 | [diff] [blame] | 148 | return prefix + expand_tlv_part(tlv_part_in); |
| 149 | } |
| 150 | |
| 151 | OCTETSTRING f__BSSGP__compact__len(OCTETSTRING const &in) |
| 152 | { |
| 153 | const unsigned char *in_ptr = (const unsigned char *)in; |
| 154 | int in_len = in.lengthof(); |
| 155 | uint8_t pdu_type = in_ptr[0]; |
| 156 | uint8_t static_hdr_len = 1; |
| 157 | |
| 158 | if (pdu_type == BSSGP_PDUT_DL_UNITDATA || pdu_type == BSSGP_PDUT_UL_UNITDATA) |
| 159 | static_hdr_len = 8; |
| 160 | |
| 161 | if (in_len < static_hdr_len) |
| 162 | TTCN_error("BSSGP message is shorter (%u bytes) than minimum header length (%u bytes) for msg_type 0x%02x", |
| 163 | in_len, static_hdr_len, pdu_type); |
| 164 | |
| 165 | /* prefix = non-TLV section of header */ |
| 166 | OCTETSTRING prefix(static_hdr_len, in_ptr); |
| 167 | OCTETSTRING tlv_part_in(in_len - static_hdr_len, in_ptr + static_hdr_len); |
| 168 | |
| 169 | return prefix + compact_tlv_part(tlv_part_in); |
| 170 | } |
| 171 | |
Harald Welte | 6fff364 | 2017-07-22 21:36:13 +0200 | [diff] [blame] | 172 | #define NS_PDUT_NS_UNITDATA 0x00 |
| 173 | |
Harald Welte | f1fd016 | 2017-07-22 20:34:05 +0200 | [diff] [blame] | 174 | /* expand all the variable-length "length" fields of a NS message (Osmocom TvLV) into statlc TL16V format */ |
| 175 | OCTETSTRING f__NS__expand__len(OCTETSTRING const &in) |
| 176 | { |
| 177 | const unsigned char *in_ptr = (const unsigned char *)in; |
| 178 | int in_len = in.lengthof(); |
| 179 | uint8_t pdu_type = in_ptr[0]; |
| 180 | uint8_t static_hdr_len = 1; |
| 181 | |
Harald Welte | 6fff364 | 2017-07-22 21:36:13 +0200 | [diff] [blame] | 182 | if (pdu_type == NS_PDUT_NS_UNITDATA) |
| 183 | return in; |
| 184 | |
Harald Welte | f1fd016 | 2017-07-22 20:34:05 +0200 | [diff] [blame] | 185 | if (in_len < static_hdr_len) |
| 186 | TTCN_error("NS message is shorter (%u bytes) than minimum header length (%u bytes) for msg_type 0x%02x", |
| 187 | in_len, static_hdr_len, pdu_type); |
| 188 | |
| 189 | /* prefix = non-TLV section of header */ |
| 190 | OCTETSTRING prefix(static_hdr_len, in_ptr); |
| 191 | OCTETSTRING tlv_part_in(in_len - static_hdr_len, in_ptr + static_hdr_len); |
| 192 | |
| 193 | return prefix + expand_tlv_part(tlv_part_in); |
| 194 | } |
| 195 | |
| 196 | OCTETSTRING f__NS__compact__len(OCTETSTRING const &in) |
| 197 | { |
| 198 | const unsigned char *in_ptr = (const unsigned char *)in; |
| 199 | int in_len = in.lengthof(); |
| 200 | uint8_t pdu_type = in_ptr[0]; |
| 201 | uint8_t static_hdr_len = 1; |
| 202 | |
Harald Welte | 6fff364 | 2017-07-22 21:36:13 +0200 | [diff] [blame] | 203 | if (pdu_type == NS_PDUT_NS_UNITDATA) |
| 204 | return in; |
| 205 | |
Harald Welte | f1fd016 | 2017-07-22 20:34:05 +0200 | [diff] [blame] | 206 | if (in_len < static_hdr_len) |
| 207 | TTCN_error("NS message is shorter (%u bytes) than minimum header length (%u bytes) for msg_type 0x%02x", |
| 208 | in_len, static_hdr_len, pdu_type); |
| 209 | |
| 210 | /* prefix = non-TLV section of header */ |
| 211 | OCTETSTRING prefix(static_hdr_len, in_ptr); |
| 212 | OCTETSTRING tlv_part_in(in_len - static_hdr_len, in_ptr + static_hdr_len); |
| 213 | |
| 214 | return prefix + compact_tlv_part(tlv_part_in); |
Harald Welte | 3a19440 | 2017-07-22 17:07:51 +0200 | [diff] [blame] | 215 | } |
| 216 | |
Harald Welte | 336959f | 2017-07-23 21:33:35 +0200 | [diff] [blame] | 217 | |
| 218 | |
| 219 | /* GPRS LLC CRC-24 Implementation */ |
| 220 | |
| 221 | /* (C) 2008-2009 by Harald Welte <laforge@gnumonks.org> |
| 222 | * |
| 223 | * All Rights Reserved |
| 224 | * |
| 225 | * This program is free software; you can redistribute it and/or modify |
| 226 | * it under the terms of the GNU General Public License as published by |
| 227 | * the Free Software Foundation; either version 2 of the License, or |
| 228 | * (at your option) any later version. |
| 229 | * |
| 230 | * This program is distributed in the hope that it will be useful, |
| 231 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 232 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
Harald Welte | 4895d7a | 2020-03-01 20:26:26 +0100 | [diff] [blame] | 233 | * GNU General Public License for more details. |
Harald Welte | 336959f | 2017-07-23 21:33:35 +0200 | [diff] [blame] | 234 | * |
Harald Welte | 4895d7a | 2020-03-01 20:26:26 +0100 | [diff] [blame] | 235 | * You should have received a copy of the GNU General Public License |
Harald Welte | 336959f | 2017-07-23 21:33:35 +0200 | [diff] [blame] | 236 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 237 | * |
| 238 | */ |
| 239 | |
| 240 | /* CRC24 table - FCS */ |
| 241 | static const uint32_t tbl_crc24[256] = { |
| 242 | 0x00000000, 0x00d6a776, 0x00f64557, 0x0020e221, 0x00b78115, 0x00612663, 0x0041c442, 0x00976334, |
| 243 | 0x00340991, 0x00e2aee7, 0x00c24cc6, 0x0014ebb0, 0x00838884, 0x00552ff2, 0x0075cdd3, 0x00a36aa5, |
| 244 | 0x00681322, 0x00beb454, 0x009e5675, 0x0048f103, 0x00df9237, 0x00093541, 0x0029d760, 0x00ff7016, |
| 245 | 0x005c1ab3, 0x008abdc5, 0x00aa5fe4, 0x007cf892, 0x00eb9ba6, 0x003d3cd0, 0x001ddef1, 0x00cb7987, |
| 246 | 0x00d02644, 0x00068132, 0x00266313, 0x00f0c465, 0x0067a751, 0x00b10027, 0x0091e206, 0x00474570, |
| 247 | 0x00e42fd5, 0x003288a3, 0x00126a82, 0x00c4cdf4, 0x0053aec0, 0x008509b6, 0x00a5eb97, 0x00734ce1, |
| 248 | 0x00b83566, 0x006e9210, 0x004e7031, 0x0098d747, 0x000fb473, 0x00d91305, 0x00f9f124, 0x002f5652, |
| 249 | 0x008c3cf7, 0x005a9b81, 0x007a79a0, 0x00acded6, 0x003bbde2, 0x00ed1a94, 0x00cdf8b5, 0x001b5fc3, |
| 250 | 0x00fb4733, 0x002de045, 0x000d0264, 0x00dba512, 0x004cc626, 0x009a6150, 0x00ba8371, 0x006c2407, |
| 251 | 0x00cf4ea2, 0x0019e9d4, 0x00390bf5, 0x00efac83, 0x0078cfb7, 0x00ae68c1, 0x008e8ae0, 0x00582d96, |
| 252 | 0x00935411, 0x0045f367, 0x00651146, 0x00b3b630, 0x0024d504, 0x00f27272, 0x00d29053, 0x00043725, |
| 253 | 0x00a75d80, 0x0071faf6, 0x005118d7, 0x0087bfa1, 0x0010dc95, 0x00c67be3, 0x00e699c2, 0x00303eb4, |
| 254 | 0x002b6177, 0x00fdc601, 0x00dd2420, 0x000b8356, 0x009ce062, 0x004a4714, 0x006aa535, 0x00bc0243, |
| 255 | 0x001f68e6, 0x00c9cf90, 0x00e92db1, 0x003f8ac7, 0x00a8e9f3, 0x007e4e85, 0x005eaca4, 0x00880bd2, |
| 256 | 0x00437255, 0x0095d523, 0x00b53702, 0x00639074, 0x00f4f340, 0x00225436, 0x0002b617, 0x00d41161, |
| 257 | 0x00777bc4, 0x00a1dcb2, 0x00813e93, 0x005799e5, 0x00c0fad1, 0x00165da7, 0x0036bf86, 0x00e018f0, |
| 258 | 0x00ad85dd, 0x007b22ab, 0x005bc08a, 0x008d67fc, 0x001a04c8, 0x00cca3be, 0x00ec419f, 0x003ae6e9, |
| 259 | 0x00998c4c, 0x004f2b3a, 0x006fc91b, 0x00b96e6d, 0x002e0d59, 0x00f8aa2f, 0x00d8480e, 0x000eef78, |
| 260 | 0x00c596ff, 0x00133189, 0x0033d3a8, 0x00e574de, 0x007217ea, 0x00a4b09c, 0x008452bd, 0x0052f5cb, |
| 261 | 0x00f19f6e, 0x00273818, 0x0007da39, 0x00d17d4f, 0x00461e7b, 0x0090b90d, 0x00b05b2c, 0x0066fc5a, |
| 262 | 0x007da399, 0x00ab04ef, 0x008be6ce, 0x005d41b8, 0x00ca228c, 0x001c85fa, 0x003c67db, 0x00eac0ad, |
| 263 | 0x0049aa08, 0x009f0d7e, 0x00bfef5f, 0x00694829, 0x00fe2b1d, 0x00288c6b, 0x00086e4a, 0x00dec93c, |
| 264 | 0x0015b0bb, 0x00c317cd, 0x00e3f5ec, 0x0035529a, 0x00a231ae, 0x007496d8, 0x005474f9, 0x0082d38f, |
| 265 | 0x0021b92a, 0x00f71e5c, 0x00d7fc7d, 0x00015b0b, 0x0096383f, 0x00409f49, 0x00607d68, 0x00b6da1e, |
| 266 | 0x0056c2ee, 0x00806598, 0x00a087b9, 0x007620cf, 0x00e143fb, 0x0037e48d, 0x001706ac, 0x00c1a1da, |
| 267 | 0x0062cb7f, 0x00b46c09, 0x00948e28, 0x0042295e, 0x00d54a6a, 0x0003ed1c, 0x00230f3d, 0x00f5a84b, |
| 268 | 0x003ed1cc, 0x00e876ba, 0x00c8949b, 0x001e33ed, 0x008950d9, 0x005ff7af, 0x007f158e, 0x00a9b2f8, |
| 269 | 0x000ad85d, 0x00dc7f2b, 0x00fc9d0a, 0x002a3a7c, 0x00bd5948, 0x006bfe3e, 0x004b1c1f, 0x009dbb69, |
| 270 | 0x0086e4aa, 0x005043dc, 0x0070a1fd, 0x00a6068b, 0x003165bf, 0x00e7c2c9, 0x00c720e8, 0x0011879e, |
| 271 | 0x00b2ed3b, 0x00644a4d, 0x0044a86c, 0x00920f1a, 0x00056c2e, 0x00d3cb58, 0x00f32979, 0x00258e0f, |
| 272 | 0x00eef788, 0x003850fe, 0x0018b2df, 0x00ce15a9, 0x0059769d, 0x008fd1eb, 0x00af33ca, 0x007994bc, |
| 273 | 0x00dafe19, 0x000c596f, 0x002cbb4e, 0x00fa1c38, 0x006d7f0c, 0x00bbd87a, 0x009b3a5b, 0x004d9d2d |
| 274 | }; |
| 275 | |
| 276 | #define INIT_CRC24 0xffffff |
| 277 | |
| 278 | static uint32_t crc24_calc(uint32_t fcs, const unsigned char *cp, int len) |
| 279 | { |
| 280 | while (len--) |
| 281 | fcs = (fcs >> 8) ^ tbl_crc24[(fcs ^ *cp++) & 0xff]; |
| 282 | return fcs; |
| 283 | } |
| 284 | |
| 285 | OCTETSTRING f__LLC__compute__fcs(OCTETSTRING const &in) |
| 286 | { |
| 287 | uint32_t fcs_calc; |
| 288 | unsigned char fcs_buf[3]; |
| 289 | const unsigned char *data = (const unsigned char *)in; |
| 290 | int len = in.lengthof(); |
| 291 | |
| 292 | fcs_calc = crc24_calc(INIT_CRC24, data, len); |
| 293 | fcs_calc = ~fcs_calc; |
| 294 | fcs_calc &= 0xffffff; |
| 295 | |
| 296 | fcs_buf[0] = fcs_calc & 0xff; |
| 297 | fcs_buf[1] = (fcs_calc >> 8) & 0xff; |
| 298 | fcs_buf[2] = (fcs_calc >> 16) & 0xff; |
| 299 | |
| 300 | return OCTETSTRING(3, fcs_buf); |
| 301 | } |
| 302 | |
Harald Welte | 3a19440 | 2017-07-22 17:07:51 +0200 | [diff] [blame] | 303 | } |