2017-03-23 11:57:55 +00:00
|
|
|
#define TRACE_MODULE _gtp_path
|
|
|
|
#include "core_debug.h"
|
|
|
|
#include "core_pkbuf.h"
|
|
|
|
#include "core_net.h"
|
|
|
|
|
2017-04-06 08:10:26 +00:00
|
|
|
#include "3gpp_common.h"
|
2017-03-23 11:57:55 +00:00
|
|
|
#include "gtp_path.h"
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
status_t gtp_listen(net_sock_t **sock,
|
|
|
|
net_sock_handler handler, c_uint32_t addr, c_uint16_t port, void *data)
|
2017-03-23 11:57:55 +00:00
|
|
|
{
|
|
|
|
char buf[INET_ADDRSTRLEN];
|
|
|
|
int rc;
|
|
|
|
|
2017-04-09 08:24:24 +00:00
|
|
|
rc = net_listen_ext(sock, SOCK_DGRAM, IPPROTO_UDP, 0, addr, port);
|
2017-03-23 11:57:55 +00:00
|
|
|
if (rc != 0)
|
|
|
|
{
|
|
|
|
d_error("Can't establish GTP[%s:%d] path(%d:%s)",
|
2017-03-27 04:22:42 +00:00
|
|
|
INET_NTOP(&addr, buf), port, errno, strerror(errno));
|
2017-03-23 11:57:55 +00:00
|
|
|
return CORE_ERROR;
|
|
|
|
}
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
rc = net_register_sock(*sock, handler, data);
|
2017-03-23 11:57:55 +00:00
|
|
|
if (rc != 0)
|
|
|
|
{
|
|
|
|
d_error("Can't establish GTP path(%d:%s)",
|
|
|
|
errno, strerror(errno));
|
2017-03-27 04:22:42 +00:00
|
|
|
net_close(*sock);
|
2017-03-23 11:57:55 +00:00
|
|
|
return CORE_ERROR;
|
|
|
|
}
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
d_trace(1, "gtp_listen() %s:%d\n", INET_NTOP(&addr, buf), port);
|
2017-03-23 11:57:55 +00:00
|
|
|
|
|
|
|
return CORE_OK;
|
|
|
|
}
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
status_t gtp_close(net_sock_t *sock)
|
2017-03-23 11:57:55 +00:00
|
|
|
{
|
2017-03-27 04:22:42 +00:00
|
|
|
d_assert(sock, return CORE_ERROR, "Null param");
|
2017-03-23 11:57:55 +00:00
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
net_unregister_sock(sock);
|
|
|
|
net_close(sock);
|
2017-03-23 11:57:55 +00:00
|
|
|
|
|
|
|
return CORE_OK;
|
|
|
|
}
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
pkbuf_t *gtp_read(net_sock_t *sock)
|
2017-03-23 11:57:55 +00:00
|
|
|
{
|
|
|
|
pkbuf_t *pkb;
|
|
|
|
int r;
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
d_assert(sock, return NULL, "Null param");
|
2017-03-23 11:57:55 +00:00
|
|
|
|
2017-03-24 12:19:24 +00:00
|
|
|
pkb = pkbuf_alloc(0, MAX_SDU_LEN);
|
2017-03-23 11:57:55 +00:00
|
|
|
d_assert(pkb, return NULL, "Can't allocate pkbuf");
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
r = net_read(sock, pkb->payload, pkb->len, 0);
|
2017-03-23 11:57:55 +00:00
|
|
|
if (r <= 0)
|
|
|
|
{
|
|
|
|
pkbuf_free(pkb);
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
if (sock->sndrcv_errno != EAGAIN)
|
2017-03-23 11:57:55 +00:00
|
|
|
{
|
|
|
|
d_warn("net_read failed(%d:%s)",
|
2017-03-27 04:22:42 +00:00
|
|
|
sock->sndrcv_errno, strerror(sock->sndrcv_errno));
|
2017-03-23 11:57:55 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
pkb->len = r;
|
|
|
|
|
|
|
|
return pkb;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
status_t gtp_send(net_sock_t *sock, gtp_node_t *gnode, pkbuf_t *pkbuf)
|
2017-03-23 11:57:55 +00:00
|
|
|
{
|
|
|
|
char buf[INET_ADDRSTRLEN];
|
|
|
|
ssize_t sent;
|
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
d_assert(sock, return CORE_ERROR, "Null param");
|
2017-03-26 15:48:33 +00:00
|
|
|
d_assert(gnode, return CORE_ERROR, "Null param");
|
2017-03-23 14:05:40 +00:00
|
|
|
d_assert(pkbuf, return CORE_ERROR, "Null param");
|
2017-03-23 11:57:55 +00:00
|
|
|
|
2017-03-27 04:22:42 +00:00
|
|
|
sent = net_sendto(sock, pkbuf->payload, pkbuf->len,
|
|
|
|
gnode->addr, gnode->port);
|
2017-03-26 15:48:33 +00:00
|
|
|
d_trace(1,"Sent %d->%d bytes to [%s:%d]\n", pkbuf->len, sent,
|
2017-03-27 04:22:42 +00:00
|
|
|
INET_NTOP(&gnode->addr, buf), gnode->port);
|
2017-03-23 14:05:40 +00:00
|
|
|
d_trace_hex(1, pkbuf->payload, pkbuf->len);
|
|
|
|
if (sent < 0 || sent != pkbuf->len)
|
2017-03-23 11:57:55 +00:00
|
|
|
{
|
|
|
|
d_error("net_send error (%d:%s)",
|
2017-03-27 04:22:42 +00:00
|
|
|
sock->sndrcv_errno, strerror(sock->sndrcv_errno));
|
2017-03-23 11:57:55 +00:00
|
|
|
return CORE_ERROR;
|
|
|
|
}
|
|
|
|
|
|
|
|
return CORE_OK;
|
|
|
|
}
|
2017-04-18 09:24:37 +00:00
|
|
|
|
|
|
|
pkbuf_t *gtp_handle_echo_req(pkbuf_t *pkb)
|
|
|
|
{
|
|
|
|
gtp_header_t *gtph = NULL;
|
|
|
|
pkbuf_t *pkb_resp;NULL;
|
|
|
|
gtp_header_t *gtph_resp;NULL;
|
|
|
|
c_uint16_t length;
|
|
|
|
int idx;
|
|
|
|
|
|
|
|
d_assert(pkb, return NULL, "pkt is NULL");
|
|
|
|
|
|
|
|
gtph = (gtp_header_t *)pkb->payload;
|
|
|
|
/* Check GTP version. Now only support GTPv1(version = 1) */
|
|
|
|
if ((gtph->flags >> 5) != 1)
|
|
|
|
{
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (gtph->type != GTPU_MSGTYPE_ECHO_REQ)
|
|
|
|
{
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
d_trace(1, "gtp_handle_without_teid(ECHO_REQ)\n");
|
|
|
|
|
|
|
|
pkb_resp = pkbuf_alloc(0, 100 /* enough for ECHO_RSP; use smaller buffer */);
|
|
|
|
d_assert(pkb_resp, return NULL, "Can't allocate pkbuf");
|
|
|
|
gtph_resp = (gtp_header_t *)pkb_resp->payload;
|
|
|
|
|
|
|
|
/* reply back immediately */
|
|
|
|
gtph_resp->flags = (1 << 5); /* set version */
|
|
|
|
gtph_resp->flags |= (1 << 4); /* set PT */
|
|
|
|
gtph_resp->type = GTPU_MSGTYPE_ECHO_RSP;
|
|
|
|
length = 0; /* length of Recovery IE */
|
|
|
|
gtph_resp->length = htons(length); /* to be overwriten */
|
|
|
|
gtph_resp->teid = 0;
|
|
|
|
idx = 8;
|
|
|
|
|
|
|
|
if (gtph->flags & (GTPU_FLAGS_PN | GTPU_FLAGS_S))
|
|
|
|
{
|
|
|
|
length += 4;
|
|
|
|
if (gtph->flags & GTPU_FLAGS_S)
|
|
|
|
{
|
|
|
|
/* sequence exists */
|
|
|
|
gtph_resp->flags |= GTPU_FLAGS_S;
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx) = *((c_uint8_t *)pkb->payload + idx);
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx + 1) = *((c_uint8_t *)pkb->payload + idx + 1);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx) = 0;
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx + 1) = 0;
|
|
|
|
}
|
|
|
|
idx += 2;
|
|
|
|
if (gtph->flags & GTPU_FLAGS_PN)
|
|
|
|
{
|
|
|
|
/* sequence exists */
|
|
|
|
gtph_resp->flags |= GTPU_FLAGS_PN;
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx) = *((c_uint8_t *)pkb->payload + idx);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx) = 0;
|
|
|
|
}
|
|
|
|
idx++;
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx) = 0; /* next-extension header */
|
|
|
|
idx++;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* fill Recovery IE */
|
|
|
|
length += 2;
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx) = 14; idx++; /* type */
|
|
|
|
*((c_uint8_t *)pkb_resp->payload + idx) = 0; idx++; /* restart counter */
|
|
|
|
|
|
|
|
gtph_resp->length = htons(length);
|
|
|
|
pkb_resp->len = idx; /* buffer length */
|
|
|
|
|
|
|
|
return pkb_resp;
|
|
|
|
}
|