Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 1 | /* Patch GSM 08.08 messages for the network and BS */ |
| 2 | /* |
| 3 | * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org> |
| 4 | * (C) 2010 by On-Waves |
| 5 | * All Rights Reserved |
| 6 | * |
Holger Hans Peter Freyther | de56c22 | 2011-01-16 17:45:14 +0100 | [diff] [blame] | 7 | * This program is free software: you can redistribute it and/or modify |
| 8 | * it under the terms of the GNU Affero General Public License as published by |
| 9 | * the Free Software Foundation, either version 3 of the License, or |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 10 | * (at your option) any later version. |
| 11 | * |
| 12 | * This program is distributed in the hope that it will be useful, |
| 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
Holger Hans Peter Freyther | de56c22 | 2011-01-16 17:45:14 +0100 | [diff] [blame] | 15 | * GNU Affero General Public License for more details. |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 16 | * |
Holger Hans Peter Freyther | de56c22 | 2011-01-16 17:45:14 +0100 | [diff] [blame] | 17 | * You should have received a copy of the GNU Affero General Public License |
| 18 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 19 | * |
| 20 | */ |
| 21 | |
| 22 | #include <bss_patch.h> |
Holger Hans Peter Freyther | cbf7d18 | 2010-07-31 05:25:35 +0800 | [diff] [blame] | 23 | #include <cellmgr_debug.h> |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 24 | |
| 25 | #include <string.h> |
| 26 | |
Harald Welte | ff397ed | 2011-05-08 10:29:23 +0200 | [diff] [blame^] | 27 | #include <osmocom/gsm/gsm0808.h> |
| 28 | #include <osmocom/gsm/protocol/gsm_08_08.h> |
| 29 | #include <osmocom/gsm/tlv.h> |
Holger Hans Peter Freyther | e66c7c1 | 2010-08-04 18:51:16 +0800 | [diff] [blame] | 30 | |
Holger Hans Peter Freyther | cf381e2 | 2010-08-04 18:39:26 +0800 | [diff] [blame] | 31 | #include <osmocom/sccp/sccp.h> |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 32 | |
| 33 | #include <arpa/inet.h> |
| 34 | |
| 35 | static void patch_ass_rqst(struct msgb *msg, int length) |
| 36 | { |
| 37 | struct tlv_parsed tp; |
Holger Hans Peter Freyther | 5aa1701 | 2010-07-31 04:37:26 +0800 | [diff] [blame] | 38 | uint8_t *data, audio; |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 39 | int len; |
| 40 | |
| 41 | tlv_parse(&tp, gsm0808_att_tlvdef(), msg->l3h + 1, length - 1, 0, 0); |
| 42 | len = TLVP_LEN(&tp, GSM0808_IE_CHANNEL_TYPE); |
| 43 | if (len < 3) |
| 44 | return; |
| 45 | |
Holger Hans Peter Freyther | 5aa1701 | 2010-07-31 04:37:26 +0800 | [diff] [blame] | 46 | data = (uint8_t *) TLVP_VAL(&tp, GSM0808_IE_CHANNEL_TYPE); |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 47 | /* no speech... ignore */ |
| 48 | if ((data[0] & 0xf) != 0x1) |
| 49 | return; |
| 50 | |
| 51 | /* blindly assign */ |
| 52 | data[1] = GSM0808_SPEECH_FULL_PREF; |
| 53 | audio = GSM0808_PERM_FR2; |
| 54 | if (len > 3) |
| 55 | audio |= 0x80; |
| 56 | data[2] = audio; |
| 57 | } |
| 58 | |
| 59 | static void patch_ass_cmpl(struct msgb *msg, int length) |
| 60 | { |
| 61 | struct tlv_parsed tp; |
Holger Hans Peter Freyther | 5aa1701 | 2010-07-31 04:37:26 +0800 | [diff] [blame] | 62 | uint8_t *data; |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 63 | |
Holger Hans Peter Freyther | d25dfce | 2010-10-27 15:59:33 +0200 | [diff] [blame] | 64 | if (length == 1) { |
| 65 | LOGP(DMSC, LOGL_ERROR, "Hacking the Assignment Complete.\n"); |
| 66 | msgb_v_put(msg, 0x21); |
| 67 | msgb_v_put(msg, 0x09); |
| 68 | msgb_v_put(msg, 0x2c); |
| 69 | msgb_v_put(msg, 0x02); |
| 70 | msgb_v_put(msg, 0x40); |
| 71 | msgb_v_put(msg, 0x25); |
| 72 | msg->l3h[-1] = 7; |
| 73 | msg->l3h[-3] = 9; |
| 74 | return; |
| 75 | } |
| 76 | |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 77 | tlv_parse(&tp, gsm0808_att_tlvdef(), msg->l3h + 1, length - 1, 0, 0); |
| 78 | if (!TLVP_PRESENT(&tp, GSM0808_IE_CHOSEN_CHANNEL)) { |
| 79 | LOGP(DMSC, LOGL_ERROR, "Chosen Channel not in the MSG.\n"); |
| 80 | return; |
| 81 | } |
| 82 | |
| 83 | if (!TLVP_PRESENT(&tp, GSM0808_IE_SPEECH_VERSION)) { |
| 84 | LOGP(DMSC, LOGL_ERROR, "Speech version not in the MSG.\n"); |
| 85 | return; |
| 86 | } |
| 87 | |
| 88 | /* claim to have a TCH/H with no mode indication */ |
Holger Hans Peter Freyther | 5aa1701 | 2010-07-31 04:37:26 +0800 | [diff] [blame] | 89 | data = (uint8_t *) TLVP_VAL(&tp, GSM0808_IE_CHOSEN_CHANNEL); |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 90 | data[0] = 0x09; |
| 91 | |
Holger Hans Peter Freyther | 5aa1701 | 2010-07-31 04:37:26 +0800 | [diff] [blame] | 92 | data = (uint8_t *) TLVP_VAL(&tp, GSM0808_IE_SPEECH_VERSION); |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 93 | data[0] = GSM0808_PERM_HR3; |
| 94 | } |
| 95 | |
| 96 | int bss_patch_filter_msg(struct msgb *msg, struct sccp_parse_result *sccp) |
| 97 | { |
| 98 | int type; |
| 99 | memset(sccp, 0, sizeof(*sccp)); |
| 100 | if (sccp_parse_header(msg, sccp) != 0) { |
| 101 | LOGP(DMSC, LOGL_ERROR, "Failed to parse SCCP header.\n"); |
| 102 | return -1; |
| 103 | } |
| 104 | |
| 105 | type = sccp_determine_msg_type(msg); |
| 106 | switch (type) { |
| 107 | case SCCP_MSG_TYPE_CR: |
| 108 | if (msg->l3h) |
| 109 | break; |
| 110 | return 0; |
| 111 | break; |
| 112 | case SCCP_MSG_TYPE_CC: |
| 113 | case SCCP_MSG_TYPE_CREF: |
| 114 | return 0; |
| 115 | break; |
| 116 | case SCCP_MSG_TYPE_RLC: |
| 117 | return BSS_FILTER_RLC; |
| 118 | break; |
| 119 | case SCCP_MSG_TYPE_RLSD: |
| 120 | return BSS_FILTER_RLSD; |
| 121 | break; |
| 122 | } |
| 123 | |
| 124 | if (msgb_l3len(msg) < sccp->data_len) { |
| 125 | LOGP(DMSC, LOGL_ERROR, "Less space than there should be.\n"); |
| 126 | return -1; |
| 127 | } |
| 128 | |
| 129 | if (!msg->l3h || msgb_l3len(msg) < 3) { |
| 130 | return -1; |
| 131 | } |
| 132 | |
| 133 | if (msg->l3h[0] != 0) { |
| 134 | return -1; |
| 135 | } |
| 136 | |
| 137 | if (msgb_l3len(msg) < 2 + msg->l3h[1]) { |
| 138 | return -1; |
| 139 | } |
| 140 | |
| 141 | switch (msg->l3h[2]) { |
| 142 | case BSS_MAP_MSG_ASSIGMENT_RQST: |
| 143 | msg->l3h = &msg->l3h[2]; |
| 144 | patch_ass_rqst(msg, sccp->data_len - 2); |
| 145 | break; |
| 146 | case BSS_MAP_MSG_ASSIGMENT_COMPLETE: |
| 147 | msg->l3h = &msg->l3h[2]; |
| 148 | patch_ass_cmpl(msg, sccp->data_len - 2); |
| 149 | break; |
| 150 | case BSS_MAP_MSG_RESET: |
| 151 | return BSS_FILTER_RESET; |
| 152 | break; |
| 153 | case BSS_MAP_MSG_RESET_ACKNOWLEDGE: |
| 154 | return BSS_FILTER_RESET_ACK; |
| 155 | break; |
| 156 | case BSS_MAP_MSG_CLEAR_COMPLETE: |
| 157 | return BSS_FILTER_CLEAR_COMPL; |
| 158 | break; |
| 159 | } |
| 160 | |
| 161 | return 0; |
| 162 | } |
| 163 | |
| 164 | static void create_cr(struct msgb *target, struct msgb *inpt, struct sccp_parse_result *sccp) |
| 165 | { |
Holger Hans Peter Freyther | 9ed3e1b | 2010-07-31 05:22:56 +0800 | [diff] [blame] | 166 | static const uint32_t optional_offset = |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 167 | offsetof(struct sccp_connection_request, optional_start); |
| 168 | |
| 169 | unsigned int optional_length, optional_start; |
| 170 | struct sccp_connection_request *cr, *in_cr; |
| 171 | |
| 172 | target->l2h = msgb_put(target, sizeof(*cr)); |
| 173 | cr = (struct sccp_connection_request *) target->l2h; |
| 174 | in_cr = (struct sccp_connection_request *) inpt->l2h; |
| 175 | |
| 176 | cr->type = in_cr->type; |
| 177 | cr->proto_class = in_cr->proto_class; |
| 178 | cr->source_local_reference = in_cr->source_local_reference; |
| 179 | cr->variable_called = 2; |
| 180 | cr->optional_start = 4; |
| 181 | |
| 182 | /* called address */ |
| 183 | target->l3h = msgb_put(target, 1 + 2); |
| 184 | target->l3h[0] = 2; |
| 185 | target->l3h[1] = 0x42; |
| 186 | target->l3h[2] = 254; |
| 187 | |
| 188 | /* |
| 189 | * We need to keep the complete optional data. The SCCP parse result |
| 190 | * is only pointing to the data payload. |
| 191 | */ |
| 192 | optional_start = in_cr->optional_start + optional_offset; |
| 193 | optional_length = msgb_l2len(inpt) - optional_start; |
| 194 | if (optional_start + optional_length <= msgb_l2len(inpt)) { |
| 195 | target->l3h = msgb_put(target, optional_length); |
| 196 | memcpy(target->l3h, inpt->l2h + optional_start, msgb_l3len(target)); |
| 197 | } else { |
| 198 | LOGP(DINP, LOGL_ERROR, "Input should at least have a byte of data.\n"); |
| 199 | } |
| 200 | } |
| 201 | |
| 202 | /* |
| 203 | * Generate a simple UDT msg. FIXME: Merge it with the SCCP code |
| 204 | */ |
| 205 | static void create_udt(struct msgb *target, struct msgb *inpt, struct sccp_parse_result *sccp) |
| 206 | { |
| 207 | struct sccp_data_unitdata *udt, *in_udt; |
| 208 | |
| 209 | target->l2h = msgb_put(target, sizeof(*udt)); |
| 210 | udt = (struct sccp_data_unitdata *) target->l2h; |
| 211 | in_udt = (struct sccp_data_unitdata *) inpt->l2h; |
| 212 | |
| 213 | udt->type = in_udt->type; |
| 214 | udt->proto_class = in_udt->proto_class; |
| 215 | udt->variable_called = 3; |
| 216 | udt->variable_calling = 5; |
| 217 | udt->variable_data = 7; |
| 218 | |
| 219 | target->l3h = msgb_put(target, 1 + 2); |
| 220 | target->l3h[0] = 2; |
| 221 | target->l3h[1] = 0x42; |
| 222 | target->l3h[2] = 254; |
| 223 | |
| 224 | target->l3h = msgb_put(target, 1 + 2); |
| 225 | target->l3h[0] = 2; |
| 226 | target->l3h[1] = 0x42; |
| 227 | target->l3h[2] = 254; |
| 228 | |
| 229 | target->l3h = msgb_put(target, sccp->data_len + 1); |
| 230 | target->l3h[0] = sccp->data_len; |
| 231 | memcpy(&target->l3h[1], inpt->l3h, msgb_l3len(target) - 1); |
| 232 | } |
| 233 | |
| 234 | void bss_rewrite_header_for_msc(int rc, struct msgb *target, struct msgb *inpt, struct sccp_parse_result *sccp) |
| 235 | { |
| 236 | |
| 237 | switch (inpt->l2h[0]) { |
| 238 | case SCCP_MSG_TYPE_CR: |
| 239 | if (rc >= 0) |
| 240 | create_cr(target, inpt, sccp); |
| 241 | else |
| 242 | target->l2h = msgb_put(target, 0); |
| 243 | break; |
| 244 | case SCCP_MSG_TYPE_UDT: |
| 245 | if (rc >= 0) |
| 246 | create_udt(target, inpt, sccp); |
| 247 | else |
| 248 | target->l2h = msgb_put(target, 0); |
| 249 | break; |
| 250 | default: |
| 251 | target->l2h = msgb_put(target, msgb_l2len(inpt)); |
| 252 | memcpy(target->l2h, inpt->l2h, msgb_l2len(target)); |
| 253 | break; |
| 254 | } |
| 255 | } |
| 256 | |
| 257 | /* it is asssumed that the SCCP stack checked the size */ |
Holger Hans Peter Freyther | 9ed3e1b | 2010-07-31 05:22:56 +0800 | [diff] [blame] | 258 | static int patch_address(uint32_t offset, int pc, struct msgb *msg) |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 259 | { |
| 260 | struct sccp_called_party_address *party; |
Holger Hans Peter Freyther | 5aa1701 | 2010-07-31 04:37:26 +0800 | [diff] [blame] | 261 | uint8_t *the_pc; |
| 262 | uint8_t pc_low, pc_high; |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 263 | |
| 264 | party = (struct sccp_called_party_address *)(msg->l2h + offset + 1); |
| 265 | the_pc = &party->data[0]; |
| 266 | |
| 267 | pc_low = pc & 0xff; |
| 268 | pc_high = (pc >> 8) & 0xff; |
| 269 | the_pc[0] = pc_low; |
| 270 | the_pc[1] = pc_high; |
| 271 | |
| 272 | return 0; |
| 273 | } |
| 274 | |
| 275 | int bss_rewrite_header_to_bsc(struct msgb *msg, int opc, int dpc) |
| 276 | { |
Holger Hans Peter Freyther | 9ed3e1b | 2010-07-31 05:22:56 +0800 | [diff] [blame] | 277 | static const uint32_t called_offset = |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 278 | offsetof(struct sccp_data_unitdata, variable_called); |
Holger Hans Peter Freyther | 9ed3e1b | 2010-07-31 05:22:56 +0800 | [diff] [blame] | 279 | static const uint32_t calling_offset = |
Holger Hans Peter Freyther | 97f66e2 | 2010-07-28 03:32:52 +0800 | [diff] [blame] | 280 | offsetof(struct sccp_data_unitdata, variable_calling); |
| 281 | |
| 282 | struct sccp_data_unitdata *udt; |
| 283 | struct sccp_parse_result sccp; |
| 284 | |
| 285 | memset(&sccp, 0, sizeof(sccp)); |
| 286 | if (sccp_parse_header(msg, &sccp) != 0) { |
| 287 | LOGP(DMSC, LOGL_ERROR, "Failed to parse SCCP header.\n"); |
| 288 | return -1; |
| 289 | } |
| 290 | |
| 291 | /* For now the MSC only sends the PC in UDT */ |
| 292 | if (msg->l2h[0] != SCCP_MSG_TYPE_UDT) |
| 293 | return 0; |
| 294 | |
| 295 | /* sanity checking */ |
| 296 | if (sccp.called.address.point_code_indicator != 1) { |
| 297 | LOGP(DMSC, LOGL_ERROR, "MSC didn't send a PC in called address\n"); |
| 298 | return -1; |
| 299 | } |
| 300 | |
| 301 | if (sccp.calling.address.point_code_indicator != 1) { |
| 302 | LOGP(DMSC, LOGL_ERROR, "MSC didn't send a PC in calling address\n"); |
| 303 | return -1; |
| 304 | } |
| 305 | |
| 306 | /* Good thing is we can avoid most of the error checking */ |
| 307 | udt = (struct sccp_data_unitdata *) msg->l2h; |
| 308 | if (patch_address(called_offset + udt->variable_called, dpc, msg) != 0) |
| 309 | return -1; |
| 310 | |
| 311 | if (patch_address(calling_offset + udt->variable_calling, opc, msg) != 0) |
| 312 | return -1; |
| 313 | return 0; |
| 314 | } |