diff --git a/src/pgw/pgw_path.c b/src/pgw/pgw_path.c index c1e965f317..be95f19d18 100644 --- a/src/pgw/pgw_path.c +++ b/src/pgw/pgw_path.c @@ -112,10 +112,9 @@ static int _gtpv2_c_recv_cb(net_sock_t *sock, void *data) static int _gtpv1_u_recv_cb(net_sock_t *sock, void *data) { pkbuf_t *pkbuf = NULL; - gtp_node_t *gnode = data; + c_uint32_t size = sizeof(gtp_header_t); d_assert(sock, return -1, "Null param"); - d_assert(gnode, return -1, "Null param"); pkbuf = gtp_read(sock); if (pkbuf == NULL) @@ -129,9 +128,22 @@ static int _gtpv1_u_recv_cb(net_sock_t *sock, void *data) d_trace(1, "S5-U PDU received from GTP\n"); d_trace_hex(1, pkbuf->payload, pkbuf->len); - /* TODO */ + /* Remove GTP header and send packets to TUN interface */ + if (pkbuf_header(pkbuf, -size) != CORE_OK) + { + d_error("pkbuf_header error"); + + pkbuf_free(pkbuf); + return -1; + } + + if (net_link_write(pgw_self()->tun_link, pkbuf->payload, pkbuf->len) <= 0) + { + d_error("Can not send packets to tuntap"); + } pkbuf_free(pkbuf); + return 0; } diff --git a/src/sgw/sgw_context.c b/src/sgw/sgw_context.c index 08452f1923..d4abbf3ff5 100644 --- a/src/sgw/sgw_context.c +++ b/src/sgw/sgw_context.c @@ -221,6 +221,11 @@ sgw_bearer_t* sgw_bearer_find_by_sgw_s5u_teid(c_uint32_t sgw_s5u_teid) return sgw_bearer_find(sgw_s5u_teid); } +sgw_bearer_t* sgw_bearer_find_by_sgw_s1u_teid(c_uint32_t sgw_s1u_teid) +{ + return sgw_bearer_find(sgw_s1u_teid); +} + sgw_bearer_t* sgw_bearer_find_by_id(sgw_sess_t *sess, c_uint8_t id) { sgw_bearer_t *bearer = NULL; diff --git a/src/sgw/sgw_context.h b/src/sgw/sgw_context.h index eb3107fb30..ddbdf2aa4d 100644 --- a/src/sgw/sgw_context.h +++ b/src/sgw/sgw_context.h @@ -110,6 +110,9 @@ CORE_DECLARE(sgw_bearer_t*) sgw_bearer_find_by_id( CORE_DECLARE(sgw_bearer_t*) sgw_bearer_first(sgw_sess_t *sess); CORE_DECLARE(sgw_bearer_t*) sgw_bearer_next(sgw_bearer_t *bearer); +CORE_DECLARE(sgw_bearer_t*) sgw_bearer_find_by_sgw_s5u_teid(c_uint32_t sgw_s5u_teid); +CORE_DECLARE(sgw_bearer_t*) sgw_bearer_find_by_sgw_s1u_teid(c_uint32_t sgw_s1u_teid); + #ifdef __cplusplus } #endif /* __cplusplus */ diff --git a/src/sgw/sgw_path.c b/src/sgw/sgw_path.c index de11c601b3..a67cf00a85 100644 --- a/src/sgw/sgw_path.c +++ b/src/sgw/sgw_path.c @@ -58,13 +58,15 @@ static int _gtpv2_c_recv_cb(net_sock_t *sock, void *data) return 0; } -static int _gtpv1_u_recv_cb(net_sock_t *sock, void *data) +static int _gtpv1_s5u_recv_cb(net_sock_t *sock, void *data) { pkbuf_t *pkbuf = NULL; - gtp_node_t *gnode = data; + gtp_node_t gnode; + gtp_header_t *gtp_h = NULL; + sgw_bearer_t *bearer = NULL; + c_uint32_t teid; d_assert(sock, return -1, "Null param"); - d_assert(gnode, return -1, "Null param"); pkbuf = gtp_read(sock); if (pkbuf == NULL) @@ -78,7 +80,61 @@ static int _gtpv1_u_recv_cb(net_sock_t *sock, void *data) d_trace(1, "S5-U PDU received from GTP\n"); d_trace_hex(1, pkbuf->payload, pkbuf->len); - /* TODO */ + gtp_h = (gtp_header_t *)pkbuf->payload; + teid = ntohl(gtp_h->teid); + + bearer = sgw_bearer_find_by_sgw_s5u_teid(ntohl(teid)); + if (bearer) + { + /* Convert Teid and send to enodeB via s1u */ + gtp_h->teid = htonl(bearer->enb_s1u_teid); + + gnode.addr = bearer->enb_s1u_addr; + gnode.port = GTPV1_U_UDP_PORT; + + gtp_send(sgw_self()->s1u_sock, &gnode, pkbuf); + } + + pkbuf_free(pkbuf); + return 0; +} + +static int _gtpv1_s1u_recv_cb(net_sock_t *sock, void *data) +{ + pkbuf_t *pkbuf = NULL; + gtp_node_t gnode; + gtp_header_t *gtp_h = NULL; + sgw_bearer_t *bearer = NULL; + c_uint32_t teid; + + d_assert(sock, return -1, "Null param"); + + pkbuf = gtp_read(sock); + if (pkbuf == NULL) + { + if (sock->sndrcv_errno == EAGAIN) + return 0; + + return -1; + } + + d_trace(1, "S1-U PDU received from GTP\n"); + d_trace_hex(1, pkbuf->payload, pkbuf->len); + + gtp_h = (gtp_header_t *)pkbuf->payload; + teid = ntohl(gtp_h->teid); + + bearer = sgw_bearer_find_by_sgw_s1u_teid(ntohl(teid)); + if (bearer) + { + /* Convert Teid and send to PGW via s5u */ + gtp_h->teid = htonl(bearer->pgw_s5u_teid); + + gnode.addr = bearer->pgw_s5u_addr; + gnode.port = GTPV1_U_UDP_PORT; + + gtp_send(sgw_self()->s5u_sock, &gnode, pkbuf); + } pkbuf_free(pkbuf); return 0; @@ -92,7 +148,7 @@ status_t sgw_path_open() sgw_self()->s11_addr, sgw_self()->s11_port, &sgw_self()->s11_node); if (rv != CORE_OK) { - d_error("Can't establish S11 Path for MME"); + d_error("Can't establish S11 Path for SGW"); return rv; } @@ -100,15 +156,23 @@ status_t sgw_path_open() sgw_self()->s5c_addr, sgw_self()->s5c_port, &sgw_self()->s5c_node); if (rv != CORE_OK) { - d_error("Can't establish S5-C Path for PGW"); + d_error("Can't establish S5-C Path for SGW"); return rv; } - rv = gtp_listen(&sgw_self()->s5u_sock, _gtpv1_u_recv_cb, + rv = gtp_listen(&sgw_self()->s5u_sock, _gtpv1_s5u_recv_cb, sgw_self()->s5u_addr, sgw_self()->s5u_port, &sgw_self()->s5u_node); if (rv != CORE_OK) { - d_error("Can't establish S5-U Path for PGW"); + d_error("Can't establish S5-U Path for SGW"); + return rv; + } + + rv = gtp_listen(&sgw_self()->s1u_sock, _gtpv1_s1u_recv_cb, + sgw_self()->s1u_addr, sgw_self()->s1u_port, &sgw_self()->s1u_node); + if (rv != CORE_OK) + { + d_error("Can't establish S1-U Path for SGW"); return rv; } @@ -178,4 +242,3 @@ status_t sgw_s5u_send_to_pgw(pkbuf_t *pkbuf) d_assert(pkbuf, return CORE_ERROR, "Null param"); return gtp_send(sgw_self()->s5u_sock, &sgw_self()->s5u_node, pkbuf); } -