Holger Hans Peter Freyther | c21cfaa | 2010-12-10 03:53:28 +0100 | [diff] [blame] | 1 | /* |
| 2 | * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org> |
| 3 | * (C) 2010 by On-Waves |
| 4 | * All Rights Reserved |
| 5 | * |
Holger Hans Peter Freyther | de56c22 | 2011-01-16 17:45:14 +0100 | [diff] [blame] | 6 | * This program is free software: you can redistribute it and/or modify |
| 7 | * it under the terms of the GNU Affero General Public License as published by |
| 8 | * the Free Software Foundation, either version 3 of the License, or |
Holger Hans Peter Freyther | c21cfaa | 2010-12-10 03:53:28 +0100 | [diff] [blame] | 9 | * (at your option) any later version. |
| 10 | * |
| 11 | * This program is distributed in the hope that it will be useful, |
| 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
Holger Hans Peter Freyther | de56c22 | 2011-01-16 17:45:14 +0100 | [diff] [blame] | 14 | * GNU Affero General Public License for more details. |
Holger Hans Peter Freyther | c21cfaa | 2010-12-10 03:53:28 +0100 | [diff] [blame] | 15 | * |
Holger Hans Peter Freyther | de56c22 | 2011-01-16 17:45:14 +0100 | [diff] [blame] | 16 | * You should have received a copy of the GNU Affero General Public License |
| 17 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
Holger Hans Peter Freyther | c21cfaa | 2010-12-10 03:53:28 +0100 | [diff] [blame] | 18 | * |
| 19 | */ |
| 20 | |
| 21 | #include <isup_types.h> |
| 22 | #include <cellmgr_debug.h> |
Holger Hans Peter Freyther | 3aad776 | 2010-12-10 12:18:57 +0100 | [diff] [blame] | 23 | #include <mtp_data.h> |
Holger Hans Peter Freyther | c21cfaa | 2010-12-10 03:53:28 +0100 | [diff] [blame] | 24 | |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 25 | #include <osmocore/msgb.h> |
Holger Hans Peter Freyther | 3aad776 | 2010-12-10 12:18:57 +0100 | [diff] [blame] | 26 | #include <osmocore/tlv.h> |
| 27 | |
| 28 | static struct msgb *isup_gra_alloc(int cic, int range) |
| 29 | { |
| 30 | struct isup_msg_hdr *hdr; |
| 31 | struct msgb *msg; |
| 32 | int bits, len; |
| 33 | |
| 34 | msg = msgb_alloc_headroom(4096, 128, "ISUP GRA"); |
| 35 | if (!msg) { |
| 36 | LOGP(DISUP, LOGL_ERROR, "Allocation of GRA message failed.\n"); |
| 37 | return NULL; |
| 38 | } |
| 39 | |
| 40 | msg->l2h = msgb_put(msg, sizeof(*hdr)); |
| 41 | |
| 42 | /* write the ISUP header */ |
| 43 | hdr = (struct isup_msg_hdr *) msg->l2h; |
| 44 | hdr->cic = cic; |
| 45 | hdr->msg_type = ISUP_MSG_GRA; |
| 46 | |
| 47 | /* |
| 48 | * place the pointers here. |
| 49 | * 1.) place the variable start after us |
| 50 | * 2.) place the length |
| 51 | */ |
| 52 | msgb_v_put(msg, 1); |
| 53 | |
| 54 | bits = range + 1; |
| 55 | len = (bits / 8) + 1; |
| 56 | msgb_v_put(msg, len + 1); |
| 57 | msgb_v_put(msg, range); |
| 58 | |
| 59 | msgb_put(msg, len); |
| 60 | |
| 61 | return msg; |
| 62 | } |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 63 | |
Holger Hans Peter Freyther | c21cfaa | 2010-12-10 03:53:28 +0100 | [diff] [blame] | 64 | /* this message contains the range */ |
| 65 | int isup_parse_grs(const uint8_t *data, uint8_t in_length) |
| 66 | { |
| 67 | uint8_t ptr; |
| 68 | uint8_t length; |
| 69 | |
| 70 | if (in_length > 3) { |
| 71 | LOGP(DISUP, LOGL_ERROR, "This needs three bytes.\n"); |
| 72 | return -1; |
| 73 | } |
| 74 | |
| 75 | ptr = data[0]; |
| 76 | if (1 + ptr > in_length) { |
| 77 | LOGP(DISUP, LOGL_ERROR, "Pointing outside the packet.\n"); |
| 78 | return -1; |
| 79 | } |
| 80 | |
| 81 | length = data[0 + ptr]; |
| 82 | |
| 83 | if (1 + ptr + 1 > in_length) { |
| 84 | LOGP(DISUP, LOGL_ERROR, "No space for the data.\n"); |
| 85 | return -1; |
| 86 | } |
| 87 | |
| 88 | return data[0 + ptr + 1]; |
| 89 | } |
| 90 | |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 91 | |
| 92 | /* Handle incoming ISUP data */ |
Holger Hans Peter Freyther | 569f1e1 | 2011-01-02 18:47:49 +0100 | [diff] [blame] | 93 | static int handle_circuit_reset_grs(struct mtp_link_set *link, int sls, int cic, |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 94 | const uint8_t *data, int size) |
| 95 | { |
Holger Hans Peter Freyther | 3aad776 | 2010-12-10 12:18:57 +0100 | [diff] [blame] | 96 | struct msgb *resp; |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 97 | int range; |
| 98 | |
| 99 | range = isup_parse_grs(data, size); |
| 100 | if (range < 0) |
| 101 | return -1; |
| 102 | |
Holger Hans Peter Freyther | 3aad776 | 2010-12-10 12:18:57 +0100 | [diff] [blame] | 103 | resp = isup_gra_alloc(cic, range); |
| 104 | if (!resp) |
| 105 | return -1; |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 106 | |
Holger Hans Peter Freyther | 569f1e1 | 2011-01-02 18:47:49 +0100 | [diff] [blame] | 107 | mtp_link_set_submit_isup_data(link, sls, resp->l2h, msgb_l2len(resp)); |
Holger Hans Peter Freyther | 3aad776 | 2010-12-10 12:18:57 +0100 | [diff] [blame] | 108 | msgb_free(resp); |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 109 | return 0; |
| 110 | } |
| 111 | |
Holger Hans Peter Freyther | 569f1e1 | 2011-01-02 18:47:49 +0100 | [diff] [blame] | 112 | int mtp_link_set_forward_isup(struct mtp_link_set *link, struct msgb *msg, int sls) |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 113 | { |
| 114 | int rc = -1; |
| 115 | int payload_size; |
| 116 | struct isup_msg_hdr *hdr; |
| 117 | |
| 118 | if (msgb_l3len(msg) < sizeof(*hdr)) { |
| 119 | LOGP(DISUP, LOGL_ERROR, "ISUP header is too short.\n"); |
| 120 | return -1; |
| 121 | } |
| 122 | |
| 123 | hdr = (struct isup_msg_hdr *) msg->l3h; |
| 124 | payload_size = msgb_l3len(msg) - sizeof(*hdr); |
| 125 | |
| 126 | switch (hdr->msg_type) { |
| 127 | case ISUP_MSG_GRS: |
Holger Hans Peter Freyther | 3aad776 | 2010-12-10 12:18:57 +0100 | [diff] [blame] | 128 | rc = handle_circuit_reset_grs(link, sls, hdr->cic, hdr->data, payload_size); |
Holger Hans Peter Freyther | 433ea2f | 2010-12-10 07:34:01 +0100 | [diff] [blame] | 129 | break; |
| 130 | default: |
| 131 | LOGP(DISUP, LOGL_NOTICE, "ISUP msg not handled: 0x%x\n", hdr->msg_type); |
| 132 | break; |
| 133 | } |
| 134 | |
| 135 | return rc; |
| 136 | } |