Pau Espin Pedrol | ae81195 | 2021-09-23 13:34:20 +0200 | [diff] [blame] | 1 | /* |
| 2 | * MIT License |
| 3 | * |
| 4 | * Copyright (c) 2021 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> |
| 5 | * Author: Pau Espin Pedrol <pespin@sysmocom.de> |
| 6 | * |
| 7 | * SPDX-License-Identifier: MIT |
| 8 | * |
| 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
| 10 | * of this software and associated documentation files (the "Software"), to deal |
| 11 | * in the Software without restriction, including without limitation the rights |
| 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| 13 | * copies of the Software, and to permit persons to whom the Software is |
| 14 | * furnished to do so, subject to the following conditions: |
| 15 | * |
| 16 | * The above copyright notice and this permission notice (including the next |
| 17 | * paragraph) shall be included in all copies or substantial portions of the |
| 18 | * Software. |
| 19 | * |
| 20 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| 21 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| 22 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| 23 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| 24 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| 25 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE |
| 26 | * SOFTWARE. |
| 27 | */ |
| 28 | |
| 29 | /* For more info see: |
| 30 | * 3GPP TS 29.060 (GTPv1 and GTPv0) |
| 31 | * 3GPP TS 29.274 (GTPv2C) |
| 32 | */ |
| 33 | |
| 34 | #include "../config.h" |
| 35 | |
| 36 | #include <stdlib.h> |
| 37 | #include <stdbool.h> |
| 38 | #include <stdint.h> |
| 39 | #include <inttypes.h> |
| 40 | #include <unistd.h> |
| 41 | #include <limits.h> |
| 42 | #include <stdio.h> |
| 43 | #include <string.h> |
| 44 | #include <errno.h> |
| 45 | #include <getopt.h> |
| 46 | #include <netinet/in.h> |
| 47 | #include <arpa/inet.h> |
| 48 | #include <sys/select.h> |
| 49 | #include <sys/socket.h> |
| 50 | |
| 51 | #define GTP1C_PORT 2123 |
| 52 | #define GTP_MSGTYPE_ECHO_REQ 1 |
| 53 | #define GTP_MSGTYPE_ECHO_RSP 2 |
| 54 | #define GTP1C_IE_RECOVERY 14 |
| 55 | #define GTP2C_IE_RECOVERY 3 |
| 56 | #define GTP2C_IE_NODE_FEATURES 152 |
| 57 | |
| 58 | struct gtp1_hdr { |
| 59 | #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ |
| 60 | uint8_t pn:1, s:1, e:1, spare:1, pt:1, version:3; |
| 61 | #else |
| 62 | uint8_t version:3, pt:1, spare:1, e:1, s:1, pn:1; |
| 63 | #endif |
| 64 | uint8_t type; |
| 65 | uint16_t length; |
| 66 | uint32_t tei; |
| 67 | uint16_t seq; |
| 68 | uint8_t npdu; |
| 69 | uint8_t next; |
| 70 | } __attribute__((packed)); |
| 71 | |
| 72 | struct gtp2_hdr { |
| 73 | #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ |
| 74 | uint8_t reserved:3, t:1, p:1, version:3; |
| 75 | #else |
| 76 | uint8_t version:3, p:1, t:1, reserved:1; |
| 77 | #endif |
| 78 | uint8_t type; |
| 79 | uint16_t length; |
| 80 | #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ |
| 81 | uint32_t reserved2:8, seq:24; |
| 82 | #else |
| 83 | uint8_t seq:24, reserved2:1; |
| 84 | #endif |
| 85 | } __attribute__((packed)); |
| 86 | |
| 87 | struct gtp_echo_resp_state { |
| 88 | struct { |
| 89 | char laddr[INET6_ADDRSTRLEN]; |
| 90 | uint8_t recovery_ctr; |
| 91 | uint8_t node_features; |
| 92 | } cfg; |
| 93 | struct sockaddr_storage laddr_gtpc; |
| 94 | int fd_gtpc; |
| 95 | }; |
| 96 | |
| 97 | struct gtp_echo_resp_state *g_st; |
| 98 | |
| 99 | static void print_usage(void) |
| 100 | { |
| 101 | printf("Usage: gtp-echo-responder [-h] [-V] [-l listen_addr]\n"); |
| 102 | } |
| 103 | |
| 104 | static void print_help(void) |
| 105 | { |
| 106 | printf(" Some useful help...\n" |
| 107 | " -h --help This help text\n" |
| 108 | " -V --version Print the version of gtp-echo-responder\n" |
| 109 | " -l --listen-addr Listend address for GTPCv1 and GTPCv2\n" |
| 110 | " -R --recovery-counter GTP Recovery Counter to transmit in GTP Echo Response message\n" |
| 111 | " -n --node-features GTPCv2 Node Features bitmask to transmit in GTP Echo Response message\n" |
| 112 | ); |
| 113 | } |
| 114 | |
| 115 | static void print_version(void) |
| 116 | { |
| 117 | printf("gtp-echo-responder version %s\n", PACKAGE_VERSION); |
| 118 | } |
| 119 | |
| 120 | static uint8_t parse_node_features_mask(const char *arg) |
| 121 | { |
| 122 | unsigned long res; |
| 123 | char *end; |
| 124 | errno = 0; |
| 125 | |
| 126 | res = strtoul(arg, &end, 0); |
| 127 | if ((errno == ERANGE && res == ULONG_MAX) || (errno && !res) || |
Pau Espin Pedrol | 8cbdd21 | 2021-10-08 17:27:38 +0200 | [diff] [blame] | 128 | arg == end || *end != '\0') { |
Pau Espin Pedrol | ae81195 | 2021-09-23 13:34:20 +0200 | [diff] [blame] | 129 | fprintf(stderr, "Failed parsing Node Features bitmask: '%s'\n", arg); |
| 130 | exit(1); |
| 131 | } |
| 132 | if (res > 0xff) { |
| 133 | fprintf(stderr, "Failed parsing Node Features bitmask: '%s' > 0xFF\n", arg); |
| 134 | exit(1); |
| 135 | } |
| 136 | return (uint8_t)res; |
| 137 | } |
| 138 | static void handle_options(int argc, char **argv) |
| 139 | { |
| 140 | while (1) { |
| 141 | int option_index = 0, c; |
| 142 | static struct option long_options[] = { |
| 143 | { "help", 0, 0, 'h' }, |
| 144 | { "version", 0, 0, 'V' }, |
| 145 | { "listen-addr", 1, 0, 'l'}, |
| 146 | { "recovery-counter", 1, 0, 'R'}, |
| 147 | { "node-features", 1, 0, 'N'}, |
| 148 | { 0, 0, 0, 0 } |
| 149 | }; |
| 150 | |
| 151 | c = getopt_long(argc, argv, "hVl:R:N:", long_options, &option_index); |
| 152 | if (c == -1) |
| 153 | break; |
| 154 | |
| 155 | switch (c) { |
| 156 | case 'h': |
| 157 | print_usage(); |
| 158 | print_help(); |
| 159 | exit(0); |
| 160 | case 'V': |
| 161 | print_version(); |
| 162 | exit(0); |
| 163 | break; |
| 164 | case 'l': |
| 165 | strncpy(&g_st->cfg.laddr[0], optarg, sizeof(g_st->cfg.laddr)); |
| 166 | g_st->cfg.laddr[sizeof(g_st->cfg.laddr) - 1] = '\0'; |
| 167 | break; |
| 168 | case 'R': |
| 169 | g_st->cfg.recovery_ctr = (uint8_t)atoi(optarg); |
| 170 | break; |
| 171 | case 'N': |
| 172 | g_st->cfg.node_features = parse_node_features_mask(optarg); |
| 173 | break; |
| 174 | } |
| 175 | } |
| 176 | } |
| 177 | |
| 178 | static int init_socket(void) |
| 179 | { |
| 180 | struct in_addr addr; |
| 181 | struct in6_addr addr6; |
| 182 | struct sockaddr_in *saddr; |
| 183 | struct sockaddr_in6 *saddr6; |
| 184 | int family; |
| 185 | |
| 186 | if (inet_pton(AF_INET6, g_st->cfg.laddr, &addr6) == 1) { |
| 187 | family = AF_INET6; |
| 188 | saddr6 = (struct sockaddr_in6 *)&g_st->laddr_gtpc; |
| 189 | saddr6->sin6_family = family; |
| 190 | saddr6->sin6_port = htons(GTP1C_PORT); |
| 191 | memcpy(&saddr6->sin6_addr, &addr6, sizeof(addr6)); |
| 192 | } else if (inet_pton(AF_INET, g_st->cfg.laddr, &addr) == 1) { |
| 193 | family = AF_INET; |
| 194 | saddr = (struct sockaddr_in *)&g_st->laddr_gtpc; |
| 195 | saddr->sin_family = family; |
| 196 | saddr->sin_port = htons(GTP1C_PORT); |
| 197 | memcpy(&saddr->sin_addr, &addr, sizeof(addr)); |
| 198 | } else { |
| 199 | fprintf(stderr, "Failed parsing address %s\n", g_st->cfg.laddr); |
| 200 | return -1; |
| 201 | } |
| 202 | |
| 203 | if ((g_st->fd_gtpc = socket(family, SOCK_DGRAM, 0)) < 0) { |
| 204 | fprintf(stderr, "socket() failed: %s\n", strerror(errno)); |
| 205 | return -2; |
| 206 | } |
| 207 | |
| 208 | if (bind(g_st->fd_gtpc, (struct sockaddr *)&g_st->laddr_gtpc, sizeof(g_st->laddr_gtpc)) < 0) { |
| 209 | fprintf(stderr, "bind() failed: %s\n", strerror(errno)); |
| 210 | return -3; |
| 211 | } |
| 212 | |
| 213 | return 0; |
| 214 | } |
| 215 | |
| 216 | static const char *sockaddr2str(const struct sockaddr *saddr) |
| 217 | { |
| 218 | static char _rem_addr_str[INET6_ADDRSTRLEN]; |
| 219 | struct sockaddr_in *saddr4; |
| 220 | struct sockaddr_in6 *saddr6; |
| 221 | |
| 222 | switch (saddr->sa_family) { |
| 223 | case AF_INET6: |
| 224 | saddr6 = (struct sockaddr_in6 *)saddr; |
| 225 | if (!inet_ntop(saddr6->sin6_family, &saddr6->sin6_addr, _rem_addr_str, sizeof(_rem_addr_str))) |
| 226 | strcpy(_rem_addr_str, "unknown"); |
| 227 | return _rem_addr_str; |
| 228 | case AF_INET: |
| 229 | saddr4 = (struct sockaddr_in *)saddr; |
| 230 | if (!inet_ntop(saddr4->sin_family, &saddr4->sin_addr, _rem_addr_str, sizeof(_rem_addr_str))) |
| 231 | strcpy(_rem_addr_str, "unknown"); |
| 232 | return _rem_addr_str; |
| 233 | default: |
| 234 | strcpy(_rem_addr_str, "unknown-family"); |
| 235 | return _rem_addr_str; |
| 236 | } |
| 237 | } |
| 238 | |
| 239 | static int write_cb(int fd, const uint8_t *buf, size_t buf_len, const struct sockaddr *rem_saddr) |
| 240 | { |
| 241 | ssize_t rc; |
| 242 | |
| 243 | rc = sendto(fd, buf, buf_len, 0, rem_saddr, sizeof(struct sockaddr_storage)); |
| 244 | if (rc < 0) { |
| 245 | fprintf(stderr, "sendto() failed: %s\n", strerror(errno)); |
| 246 | return -1; |
| 247 | } |
| 248 | if (rc != buf_len) { |
| 249 | fprintf(stderr, "sendto() short write: %zd vs exp %zu\n", rc, buf_len); |
| 250 | return -1; |
| 251 | } |
| 252 | return 0; |
| 253 | } |
| 254 | |
| 255 | static int gen_gtpc1_echo_rsp(uint8_t *buf, struct gtp1_hdr *echo_req) |
| 256 | { |
| 257 | int offset = 0; |
| 258 | struct gtp1_hdr *echo_rsp = (struct gtp1_hdr *)buf; |
| 259 | unsigned exp_hdr_len = (echo_req->s || echo_req->pn || echo_req->e) ? 12 : 8; |
| 260 | |
| 261 | memcpy(echo_rsp, echo_req, exp_hdr_len); |
| 262 | echo_rsp->type = GTP_MSGTYPE_ECHO_RSP; |
| 263 | offset = exp_hdr_len; |
| 264 | buf[offset++] = GTP1C_IE_RECOVERY; |
| 265 | buf[offset++] = g_st->cfg.recovery_ctr; |
| 266 | |
| 267 | /* Update Length */ |
| 268 | echo_rsp->length = htons(offset - 8); |
| 269 | return offset; |
| 270 | } |
| 271 | |
| 272 | static int gen_gtpc2_echo_rsp(uint8_t *buf, struct gtp2_hdr *echo_req) |
| 273 | { |
| 274 | int offset = 0; |
| 275 | struct gtp1_hdr *echo_rsp = (struct gtp1_hdr *)buf; |
| 276 | unsigned exp_hdr_len = 8; |
| 277 | |
| 278 | memcpy(echo_rsp, echo_req, exp_hdr_len); |
| 279 | echo_rsp->type = GTP_MSGTYPE_ECHO_RSP; |
| 280 | offset = exp_hdr_len; |
| 281 | |
| 282 | /* 3GPP TS 29.274 sec 8.5 Recovery (Restart Counter) */ |
| 283 | buf[offset++] = GTP2C_IE_RECOVERY; |
| 284 | buf[offset++] = 0; /* IE Length (high) */ |
| 285 | buf[offset++] = 1; /* IE Length (low) */ |
| 286 | buf[offset++] = 0; /* Spare=0 | Instance=0 (Table 7.1.1-1) */ |
| 287 | buf[offset++] = g_st->cfg.recovery_ctr; |
| 288 | |
| 289 | /* 3GPP TS 29.274 sec 8.83 Node Features */ |
| 290 | if (g_st->cfg.node_features > 0) { |
| 291 | buf[offset++] = GTP2C_IE_NODE_FEATURES; |
| 292 | buf[offset++] = 0; /* IE Length (high) */ |
| 293 | buf[offset++] = 1; /* IE Length (low) */ |
| 294 | buf[offset++] = 0; /* Spare=0 | Instance=0 (Table 7.1.1-1) */ |
| 295 | buf[offset++] = g_st->cfg.node_features; |
| 296 | } |
| 297 | |
| 298 | /* Update Length */ |
| 299 | echo_rsp->length = htons(offset - 4); |
| 300 | return offset; |
| 301 | } |
| 302 | |
| 303 | static int rx_gtpc1_echo_req(struct gtp1_hdr *echo_req, unsigned buf_len, const struct sockaddr *rem_saddr) |
| 304 | { |
| 305 | int rc; |
| 306 | const size_t tx_buf_len = buf_len + 128; /* Leave some extra room */ |
| 307 | uint8_t *tx_buf = alloca(tx_buf_len); |
| 308 | |
| 309 | printf("Rx GTPCv1_ECHO_REQ from %s, Tx GTPCv1_ECHO_RSP\n", sockaddr2str(rem_saddr)); |
| 310 | |
| 311 | memset(tx_buf, 0, tx_buf_len); |
| 312 | rc = gen_gtpc1_echo_rsp(tx_buf, echo_req); |
| 313 | return write_cb(g_st->fd_gtpc, tx_buf, rc, rem_saddr); |
| 314 | } |
| 315 | |
| 316 | static int rx_gtpc1(struct gtp1_hdr *hdr, unsigned buf_len, const struct sockaddr *rem_saddr) |
| 317 | { |
| 318 | unsigned exp_hdr_len = (hdr->s || hdr->pn || hdr->e) ? 12 : 8; |
| 319 | unsigned pdu_len; |
| 320 | |
| 321 | if (buf_len < exp_hdr_len) { |
| 322 | fprintf(stderr, "GTPCv1 packet size smaller than header! %u < exp %u\n", buf_len, exp_hdr_len); |
| 323 | return -1; |
| 324 | } |
| 325 | |
| 326 | pdu_len = ntohs(hdr->length); |
| 327 | if (buf_len < 8 + pdu_len) { |
| 328 | fprintf(stderr, "GTPCv1 packet size smaller than announced! %u < exp %u\n", buf_len, 8 + pdu_len); |
| 329 | return -1; |
| 330 | } |
| 331 | |
| 332 | if (hdr->pt != 1) { |
| 333 | fprintf(stderr, "GTPCv1 Protocol Type GTP' not supported!\n"); |
| 334 | return -1; |
| 335 | } |
| 336 | |
| 337 | switch (hdr->type) { |
| 338 | case GTP_MSGTYPE_ECHO_REQ: |
| 339 | return rx_gtpc1_echo_req(hdr, buf_len, rem_saddr); |
| 340 | default: |
| 341 | fprintf(stderr, "Silently ignoring unexpected packet of type %u\n", hdr->type); |
| 342 | return 0; |
| 343 | } |
| 344 | } |
| 345 | |
| 346 | static int rx_gtpc2_echo_req(struct gtp2_hdr *echo_req, unsigned buf_len, const struct sockaddr *rem_saddr) |
| 347 | { |
| 348 | int rc; |
| 349 | const size_t tx_buf_len = buf_len + 128; /* Leave some extra room */ |
| 350 | uint8_t *tx_buf = alloca(tx_buf_len); |
| 351 | |
| 352 | if (echo_req->t) { |
| 353 | fprintf(stderr, "GTPCv2 ECHO message should contain T=0!\n"); |
| 354 | return -1; |
| 355 | } |
| 356 | |
| 357 | printf("Rx GTPCv2_ECHO_REQ from %s, Tx GTPCv2_ECHO_RSP\n", sockaddr2str(rem_saddr)); |
| 358 | |
| 359 | memset(tx_buf, 0, tx_buf_len); |
| 360 | rc = gen_gtpc2_echo_rsp(tx_buf, echo_req); |
| 361 | return write_cb(g_st->fd_gtpc, tx_buf, rc, rem_saddr); |
| 362 | } |
| 363 | |
| 364 | static int rx_gtpc2(struct gtp2_hdr *hdr, unsigned buf_len, const struct sockaddr *rem_saddr) |
| 365 | { |
| 366 | unsigned exp_hdr_len = hdr->t ? 12 : 8; |
| 367 | unsigned pdu_len; |
| 368 | |
| 369 | if (hdr->p) { |
| 370 | fprintf(stderr, "GTPCv2 piggybacked message not supported!\n"); |
| 371 | return -1; |
| 372 | } |
| 373 | |
| 374 | if (buf_len < exp_hdr_len) { |
| 375 | fprintf(stderr, "GTPCv2 packet size smaller than header! %u < exp %u\n", buf_len, exp_hdr_len); |
| 376 | return -1; |
| 377 | } |
| 378 | |
| 379 | pdu_len = ntohs(hdr->length); |
| 380 | /* 3GPP TS 29.274 sec 5.5.1: "Octets 3 to 4 represent the Message Length |
| 381 | * field. This field shall indicate the length of the message in octets |
| 382 | * excluding the mandatory part of the GTP-C header (the first 4 |
| 383 | * octets). The TEID (if present) and the Sequence Number shall be |
| 384 | * included in the length count" */ |
| 385 | if (buf_len < 4 + pdu_len) { |
| 386 | fprintf(stderr, "GTPCv2 packet size smaller than announced! %u < exp %u\n", buf_len, 4 + pdu_len); |
| 387 | return -1; |
| 388 | } |
| 389 | |
| 390 | switch (hdr->type) { |
| 391 | case GTP_MSGTYPE_ECHO_REQ: |
| 392 | return rx_gtpc2_echo_req(hdr, buf_len, rem_saddr); |
| 393 | default: |
| 394 | fprintf(stderr, "Silently ignoring unexpected packet of type %u\n", hdr->type); |
| 395 | return 0; |
| 396 | } |
| 397 | } |
| 398 | |
| 399 | static int read_cb(int fd) |
| 400 | { |
| 401 | ssize_t sz; |
| 402 | uint8_t buf[4096]; |
| 403 | struct sockaddr_storage rem_saddr; |
| 404 | socklen_t rem_saddr_len = sizeof(rem_saddr); |
| 405 | struct gtp1_hdr *hdr1; |
| 406 | |
| 407 | if ((sz = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr *)&rem_saddr, &rem_saddr_len)) < 0) { |
| 408 | fprintf(stderr, "recvfrom() failed: %s\n", strerror(errno)); |
| 409 | return -1; |
| 410 | } |
| 411 | if (sz == 0) { |
| 412 | fprintf(stderr, "recvfrom() read zero bytes!\n"); |
| 413 | return -1; |
| 414 | } |
| 415 | |
| 416 | hdr1 = (struct gtp1_hdr *)&buf[0]; |
| 417 | switch (hdr1->version) { |
| 418 | case 1: |
| 419 | return rx_gtpc1(hdr1, sz, (const struct sockaddr *)&rem_saddr); |
| 420 | case 2: |
| 421 | return rx_gtpc2((struct gtp2_hdr *)&buf[0], sz, (const struct sockaddr *)&rem_saddr); |
| 422 | default: |
| 423 | fprintf(stderr, "Rx GTPv%u: not supported (flags=0x%x)\n", hdr1->version, buf[0]); |
| 424 | return -1; |
| 425 | } |
| 426 | } |
| 427 | |
| 428 | static int loop(void) |
| 429 | { |
| 430 | int rc; |
| 431 | fd_set rfds; |
| 432 | int nfds; |
| 433 | |
| 434 | while (true) { |
| 435 | FD_ZERO(&rfds); |
| 436 | FD_SET(g_st->fd_gtpc, &rfds); |
| 437 | nfds = g_st->fd_gtpc + 1; |
| 438 | rc = select(nfds, &rfds, NULL, NULL, NULL); |
| 439 | if (rc == 0) |
| 440 | continue; |
| 441 | if (rc < 0) { |
| 442 | fprintf(stderr, "select() failed: %s\n", strerror(errno)); |
| 443 | return -1; |
| 444 | } |
| 445 | |
| 446 | if (FD_ISSET(g_st->fd_gtpc, &rfds)) |
| 447 | read_cb(g_st->fd_gtpc); |
| 448 | } |
| 449 | } |
| 450 | |
| 451 | int main(int argc, char **argv) |
| 452 | { |
| 453 | g_st = calloc(1, sizeof(struct gtp_echo_resp_state)); |
| 454 | |
| 455 | strcpy(g_st->cfg.laddr, "::"); |
| 456 | |
| 457 | handle_options(argc, argv); |
| 458 | |
| 459 | printf("Listening on: %s\n", g_st->cfg.laddr); |
| 460 | |
| 461 | if (init_socket() < 0) |
| 462 | exit(1); |
| 463 | |
| 464 | printf("Socket bound successfully, listening for requests...\n"); |
| 465 | |
| 466 | if (loop() < 0) |
| 467 | exit(1); |
| 468 | |
| 469 | return 0; |
| 470 | } |