mirror of https://gerrit.osmocom.org/osmo-pcap
275 lines
6.2 KiB
C
275 lines
6.2 KiB
C
/*
|
|
* 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);
|
|
}
|