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 | |
| 172 | /* expand all the variable-length "length" fields of a NS message (Osmocom TvLV) into statlc TL16V format */ |
| 173 | OCTETSTRING f__NS__expand__len(OCTETSTRING const &in) |
| 174 | { |
| 175 | const unsigned char *in_ptr = (const unsigned char *)in; |
| 176 | int in_len = in.lengthof(); |
| 177 | uint8_t pdu_type = in_ptr[0]; |
| 178 | uint8_t static_hdr_len = 1; |
| 179 | |
| 180 | if (in_len < static_hdr_len) |
| 181 | TTCN_error("NS message is shorter (%u bytes) than minimum header length (%u bytes) for msg_type 0x%02x", |
| 182 | in_len, static_hdr_len, pdu_type); |
| 183 | |
| 184 | /* prefix = non-TLV section of header */ |
| 185 | OCTETSTRING prefix(static_hdr_len, in_ptr); |
| 186 | OCTETSTRING tlv_part_in(in_len - static_hdr_len, in_ptr + static_hdr_len); |
| 187 | |
| 188 | return prefix + expand_tlv_part(tlv_part_in); |
| 189 | } |
| 190 | |
| 191 | OCTETSTRING f__NS__compact__len(OCTETSTRING const &in) |
| 192 | { |
| 193 | const unsigned char *in_ptr = (const unsigned char *)in; |
| 194 | int in_len = in.lengthof(); |
| 195 | uint8_t pdu_type = in_ptr[0]; |
| 196 | uint8_t static_hdr_len = 1; |
| 197 | |
| 198 | if (in_len < static_hdr_len) |
| 199 | TTCN_error("NS message is shorter (%u bytes) than minimum header length (%u bytes) for msg_type 0x%02x", |
| 200 | in_len, static_hdr_len, pdu_type); |
| 201 | |
| 202 | /* prefix = non-TLV section of header */ |
| 203 | OCTETSTRING prefix(static_hdr_len, in_ptr); |
| 204 | OCTETSTRING tlv_part_in(in_len - static_hdr_len, in_ptr + static_hdr_len); |
| 205 | |
| 206 | return prefix + compact_tlv_part(tlv_part_in); |
Harald Welte | 3a19440 | 2017-07-22 17:07:51 +0200 | [diff] [blame] | 207 | } |
| 208 | |
| 209 | } |