| /* |
| * osmo-pcap-client code |
| * |
| * (C) 2011 by Holger Hans Peter Freyther <zecke@selfish.org> |
| * (C) 2011 by On-Waves |
| * All Rights Reserved |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU Affero General Public License as published by |
| * the Free Software Foundation; either version 3 of the License, or |
| * (at your option) any later version. |
| * |
| * This program is distributed in the hope that it will be useful, |
| * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| * GNU Affero General Public License for more details. |
| * |
| * You should have received a copy of the GNU Affero General Public License |
| * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| * |
| */ |
| |
| #define _BSD_SOURCE |
| #include <osmo-pcap/osmo_pcap_client.h> |
| #include <osmo-pcap/common.h> |
| |
| #include <osmocom/gprs/gprs_bssgp.h> |
| #include <osmocom/gprs/protocol/gsm_08_16.h> |
| #include <osmocom/gprs/protocol/gsm_08_18.h> |
| |
| #include <osmocom/core/talloc.h> |
| |
| #include <netinet/in.h> |
| #include <netinet/ip.h> |
| #include <netinet/udp.h> |
| |
| #include <limits.h> |
| |
| #ifndef PCAP_NETMASK_UNKNOWN |
| #define PCAP_NETMASK_UNKNOWN 0xffffffff |
| #endif |
| |
| #define IP_LEN sizeof(struct ip) |
| #define UDP_LEN sizeof(struct udphdr) |
| #define NS_LEN 1 |
| |
| static int check_gprs(const u_char *data, bpf_u_int32 len) |
| { |
| struct tlv_parsed tp; |
| struct gprs_ns_hdr *hdr = (struct gprs_ns_hdr *) data; |
| struct bssgp_ud_hdr *bssgp_hdr; |
| uint8_t llc_sapi; |
| |
| switch (hdr->pdu_type) { |
| case NS_PDUT_UNITDATA: |
| break; |
| default: |
| return 1; |
| } |
| |
| len -= sizeof(*hdr); |
| |
| /* NS_PDUT_UNITDATA from here.. */ |
| /* skip NS SDU control bits and BVCI */ |
| if (len < 3) |
| return 1; |
| len -= 3; |
| |
| /* Check if the BSSGP UD hdr fits */ |
| if (len < sizeof(*bssgp_hdr)) |
| return 1; |
| bssgp_hdr = (struct bssgp_ud_hdr *) &hdr->data[3]; |
| |
| /* BVC flow control is creating too much noise. Drop it */ |
| if (bssgp_hdr->pdu_type == BSSGP_PDUT_FLOW_CONTROL_BVC |
| || bssgp_hdr->pdu_type == BSSGP_PDUT_FLOW_CONTROL_BVC_ACK) |
| return 0; |
| |
| /* We only need to check UL/DL messages for the sapi */ |
| if (bssgp_hdr->pdu_type != BSSGP_PDUT_DL_UNITDATA |
| && bssgp_hdr->pdu_type != BSSGP_PDUT_UL_UNITDATA) |
| return 1; |
| len -= sizeof(*bssgp_hdr); |
| |
| /* now parse the rest of the IEs */ |
| memset(&tp, 0, sizeof(tp)); |
| if (bssgp_tlv_parse(&tp, &bssgp_hdr->data[0], len) < 0) |
| return 1; |
| |
| if (!TLVP_PRESENT(&tp, BSSGP_IE_LLC_PDU)) |
| return 1; |
| if (TLVP_LEN(&tp, BSSGP_IE_LLC_PDU) < 1) |
| return 1; |
| |
| llc_sapi = TLVP_VAL(&tp, BSSGP_IE_LLC_PDU)[0] & 0x0f; |
| /* Skip user data 3, 5, 9, 11 */ |
| if (llc_sapi == 3 || llc_sapi == 5 || llc_sapi == 9 || llc_sapi == 11) |
| return 0; |
| return 1; |
| } |
| |
| static int forward_packet( |
| struct osmo_pcap_client *client, |
| struct pcap_pkthdr *hdr, |
| const u_char *data) |
| { |
| int ll_type; |
| int offset; |
| struct ip *ip_hdr; |
| const u_char *ip_data; |
| const u_char *udp_data; |
| const u_char *payload_data; |
| bpf_u_int32 payload_len; |
| |
| if (!client->gprs_filtering) |
| return 1; |
| |
| ll_type = pcap_datalink(client->handle); |
| switch (ll_type) { |
| case DLT_EN10MB: |
| offset = 14; |
| break; |
| case DLT_LINUX_SLL: |
| offset = 16; |
| break; |
| default: |
| LOGP(DCLIENT, LOGL_ERROR, "LL type %d/%s not handled.\n", |
| ll_type, pcap_datalink_val_to_name(ll_type)); |
| return 1; |
| } |
| |
| /* Check if this can be a full UDP frame with NS */ |
| if (offset + IP_LEN + UDP_LEN + NS_LEN > hdr->caplen) |
| return 1; |
| |
| ip_data = data + offset; |
| ip_hdr = (struct ip *) ip_data; |
| |
| /* Only handle IPv4 */ |
| if (ip_hdr->ip_v != 4) |
| return 1; |
| /* Only handle UDP */ |
| if (ip_hdr->ip_p != 17) |
| return 1; |
| |
| udp_data = ip_data + IP_LEN; |
| payload_data = udp_data + UDP_LEN; |
| payload_len = hdr->caplen - offset - IP_LEN - UDP_LEN; |
| |
| return check_gprs(payload_data, payload_len); |
| } |
| |
| |
| static int pcap_read_cb(struct osmo_fd *fd, unsigned int what) |
| { |
| struct osmo_pcap_client *client = fd->data; |
| struct pcap_pkthdr hdr; |
| const u_char *data; |
| |
| data = pcap_next(client->handle, &hdr); |
| if (!data) |
| return -1; |
| |
| if (!forward_packet(client, &hdr, data)) |
| return 0; |
| |
| osmo_client_send_data(client, &hdr, data); |
| return 0; |
| } |
| |
| static int osmo_install_filter(struct osmo_pcap_client *client) |
| { |
| int rc; |
| pcap_freecode(&client->bpf); |
| |
| if (!client->handle) { |
| LOGP(DCLIENT, LOGL_NOTICE, |
| "Filter will only be applied later.\n"); |
| return 1; |
| } |
| |
| rc = pcap_compile(client->handle, &client->bpf, |
| client->filter_string, 1, PCAP_NETMASK_UNKNOWN); |
| if (rc != 0) { |
| LOGP(DCLIENT, LOGL_ERROR, |
| "Failed to compile the filter: %s\n", |
| pcap_geterr(client->handle)); |
| return rc; |
| } |
| |
| rc = pcap_setfilter(client->handle, &client->bpf); |
| if (rc != 0) { |
| LOGP(DCLIENT, LOGL_ERROR, |
| "Failed to set the filter on the interface: %s\n", |
| pcap_geterr(client->handle)); |
| pcap_freecode(&client->bpf); |
| return rc; |
| } |
| |
| return rc; |
| } |
| |
| static void free_all(struct osmo_pcap_client *client) |
| { |
| if (!client->handle) |
| return; |
| |
| pcap_freecode(&client->bpf); |
| |
| if (client->fd.fd >= 0) { |
| osmo_fd_unregister(&client->fd); |
| client->fd.fd = -1; |
| } |
| |
| pcap_close(client->handle); |
| client->handle = NULL; |
| } |
| |
| int osmo_client_capture(struct osmo_pcap_client *client, const char *device) |
| { |
| int fd; |
| |
| talloc_free(client->device); |
| free_all(client); |
| |
| client->device = talloc_strdup(client, device); |
| if (!client->device) { |
| LOGP(DCLIENT, LOGL_ERROR, "Failed to copy string.\n"); |
| return 1; |
| } |
| |
| client->handle = pcap_open_live(client->device, 9000, 0, |
| 1000, client->errbuf); |
| if (!client->handle) { |
| LOGP(DCLIENT, LOGL_ERROR, |
| "Failed to open the device: %s\n", client->errbuf); |
| return 2; |
| } |
| |
| fd = pcap_fileno(client->handle); |
| if (fd == -1) { |
| LOGP(DCLIENT, LOGL_ERROR, |
| "No file descriptor provided.\n"); |
| free_all(client); |
| return 3; |
| } |
| |
| client->fd.fd = fd; |
| client->fd.when = BSC_FD_READ; |
| client->fd.cb = pcap_read_cb; |
| client->fd.data = client; |
| if (osmo_fd_register(&client->fd) != 0) { |
| LOGP(DCLIENT, LOGL_ERROR, |
| "Failed to register the fd.\n"); |
| client->fd.fd = -1; |
| free_all(client); |
| return 4; |
| } |
| |
| osmo_client_send_link(client); |
| |
| if (client->filter_string) { |
| osmo_install_filter(client); |
| } |
| |
| return 0; |
| } |
| |
| int osmo_client_filter(struct osmo_pcap_client *client, const char *filter) |
| { |
| talloc_free(client->filter_string); |
| client->filter_string = talloc_strdup(client, filter); |
| return osmo_install_filter(client); |
| } |