271 lines
7.3 KiB
C
271 lines
7.3 KiB
C
#pragma once
|
|
|
|
#ifdef __cplusplus
|
|
extern "C"
|
|
{
|
|
#endif
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <arpa/inet.h>
|
|
|
|
/*
|
|
* https://en.wikipedia.org/wiki/GPRS_Tunnelling_Protocol
|
|
* https://www.etsi.org/deliver/etsi_ts/129200_129299/129274/08.04.00_60/ts_129274v080400p.pdf
|
|
*
|
|
* GTP-C: Gateway GPRS Support Nodes (GGSN) <----> Serving GPRS support Nodes (SGSN)
|
|
* GTP-U: Radio Access Network <----> Core Network
|
|
*
|
|
* GTPv2:
|
|
* -> GTPv2-C
|
|
* -> (no GTPv2-U)
|
|
*/
|
|
|
|
/*
|
|
* GTP version 2
|
|
*
|
|
* 0 1 2 3
|
|
* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* | Ver |P|T|spare| Message Type | Message Length |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* | TEID (only if T=1) |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* | Sequence Number | Spare |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
*
|
|
* Message length:
|
|
* This field shall indicate the length of the message in octets excluding the mandatory of the GTP-C header (the first 4 octets).
|
|
* The TEID (if present) and the Sequence Number shall be included in the length count.
|
|
*/
|
|
|
|
struct gtp2_hdr
|
|
{
|
|
uint8_t flags;
|
|
uint8_t msg_type;
|
|
uint16_t msg_len;
|
|
uint32_t seq_and_spare;
|
|
} __attribute__((packed));
|
|
|
|
struct gtp2_hdr_long
|
|
{
|
|
uint8_t flags;
|
|
uint8_t msg_type;
|
|
uint16_t msg_len;
|
|
uint32_t teid;
|
|
uint32_t seq_and_spare;
|
|
} __attribute__((packed));
|
|
|
|
#define GPT2_FLAG_SPARE (0x07)
|
|
#define GTP2_FLAG_TEID (0x08)
|
|
#define GTP2_FLAG_PIGGYBACK (0x10)
|
|
#define GTP2_FLAG_VERSION (0xE0)
|
|
|
|
/******************************************************************************
|
|
* get
|
|
******************************************************************************/
|
|
|
|
static inline uint8_t gtp2_hdr_get_flags(const struct gtp2_hdr *gtp)
|
|
{
|
|
return gtp->flags;
|
|
}
|
|
|
|
static inline uint8_t gtp2_hdr_get_version(const struct gtp2_hdr *gtp)
|
|
{
|
|
return (gtp->flags & GTP2_FLAG_VERSION) >> 5;
|
|
}
|
|
|
|
static inline uint8_t gtp2_hdr_get_piggyback_flag(const struct gtp2_hdr *gtp)
|
|
{
|
|
return (gtp->flags & GTP2_FLAG_PIGGYBACK) >> 4;
|
|
}
|
|
|
|
static inline uint8_t gtp2_hdr_get_teid_flag(const struct gtp2_hdr *gtp)
|
|
{
|
|
return (gtp->flags & GTP2_FLAG_TEID) >> 3;
|
|
}
|
|
|
|
static inline uint8_t gtp2_hdr_get_spare_flag(const struct gtp2_hdr *gtp)
|
|
{
|
|
return (gtp->flags & GPT2_FLAG_SPARE) >> 0;
|
|
}
|
|
|
|
static inline uint8_t gtp2_hdr_get_msg_type(const struct gtp2_hdr *gtp)
|
|
{
|
|
return gtp->msg_type;
|
|
}
|
|
|
|
static inline uint16_t gtp2_hdr_get_msg_len(const struct gtp2_hdr *gtp)
|
|
{
|
|
return ntohs(gtp->msg_len);
|
|
}
|
|
|
|
static inline uint32_t gtp2_hdr_get_teid(const struct gtp2_hdr *gtp)
|
|
{
|
|
if (gtp2_hdr_get_teid_flag(gtp))
|
|
{
|
|
const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp;
|
|
return ntohl(gtp_long->teid);
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
static inline uint32_t gtp2_hdr_get_seq(const struct gtp2_hdr *gtp)
|
|
{
|
|
if (gtp2_hdr_get_teid_flag(gtp))
|
|
{
|
|
const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp;
|
|
return ntohl(gtp_long->seq_and_spare) >> 8;
|
|
}
|
|
else
|
|
{
|
|
return ntohl(gtp->seq_and_spare) >> 8;
|
|
}
|
|
}
|
|
|
|
static inline uint8_t gtp2_hdr_get_spare(const struct gtp2_hdr *gtp)
|
|
{
|
|
if (gtp2_hdr_get_teid_flag(gtp))
|
|
{
|
|
const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp;
|
|
return ntohl(gtp_long->seq_and_spare) & 0xFF;
|
|
}
|
|
else
|
|
{
|
|
return ntohl(gtp->seq_and_spare) & 0xFF;
|
|
}
|
|
}
|
|
|
|
static inline uint16_t calc_gtp2_hdr_len(const char *data, uint16_t len)
|
|
{
|
|
if (len < sizeof(struct gtp2_hdr))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
const struct gtp2_hdr *gtp = (const struct gtp2_hdr *)data;
|
|
if (gtp2_hdr_get_version(gtp) == 2)
|
|
{
|
|
if (gtp2_hdr_get_teid_flag(gtp))
|
|
{
|
|
return sizeof(struct gtp2_hdr_long);
|
|
}
|
|
else
|
|
{
|
|
return sizeof(struct gtp2_hdr);
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/******************************************************************************
|
|
* set
|
|
******************************************************************************/
|
|
|
|
static inline void gtp2_hdr_set_flags(struct gtp2_hdr *gtp, uint8_t flags)
|
|
{
|
|
gtp->flags = flags;
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_version(struct gtp2_hdr *gtp, uint8_t version)
|
|
{
|
|
gtp->flags = (gtp->flags & ~GTP2_FLAG_VERSION) | (version << 5);
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_piggyback_flag(struct gtp2_hdr *gtp, uint8_t piggyback)
|
|
{
|
|
gtp->flags = (gtp->flags & ~GTP2_FLAG_PIGGYBACK) | (piggyback << 4);
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_teid_flag(struct gtp2_hdr *gtp, uint8_t teid_flag)
|
|
{
|
|
gtp->flags = (gtp->flags & ~GTP2_FLAG_TEID) | (teid_flag << 3);
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_spare_flag(struct gtp2_hdr *gtp, uint8_t spare_flag)
|
|
{
|
|
gtp->flags = (gtp->flags & ~GPT2_FLAG_SPARE) | (spare_flag << 0);
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_msg_type(struct gtp2_hdr *gtp, uint8_t msg_type)
|
|
{
|
|
gtp->msg_type = msg_type;
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_msg_len(struct gtp2_hdr *gtp, uint16_t msg_len)
|
|
{
|
|
gtp->msg_len = htons(msg_len);
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_teid(struct gtp2_hdr *gtp, uint32_t teid)
|
|
{
|
|
if (gtp2_hdr_get_teid_flag(gtp))
|
|
{
|
|
struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp;
|
|
gtp_long->teid = htonl(teid);
|
|
}
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_seq(struct gtp2_hdr *gtp, uint32_t seq)
|
|
{
|
|
if (gtp2_hdr_get_teid_flag(gtp))
|
|
{
|
|
struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp;
|
|
gtp_long->seq_and_spare = htonl((seq << 8) | gtp2_hdr_get_spare(gtp));
|
|
}
|
|
else
|
|
{
|
|
gtp->seq_and_spare = htonl((seq << 8) | gtp2_hdr_get_spare(gtp));
|
|
}
|
|
}
|
|
|
|
static inline void gtp2_hdr_set_spare(struct gtp2_hdr *gtp, uint8_t spare)
|
|
{
|
|
if (gtp2_hdr_get_teid_flag(gtp))
|
|
{
|
|
struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp;
|
|
gtp_long->seq_and_spare = htonl((gtp2_hdr_get_seq(gtp) << 8) | spare);
|
|
}
|
|
else
|
|
{
|
|
gtp->seq_and_spare = htonl((gtp2_hdr_get_seq(gtp) << 8) | spare);
|
|
}
|
|
}
|
|
|
|
/******************************************************************************
|
|
* print
|
|
******************************************************************************/
|
|
|
|
static inline int gtp2_hdr_to_str(const struct gtp2_hdr *gtp, char *buf, size_t len)
|
|
{
|
|
memset(buf, 0, len);
|
|
|
|
int used = 0;
|
|
used += snprintf(buf + used, len - used, "GTP: flags=0x%02x (version=%u, piggyback=%u, teid_flag=%u, spare_flag=%u), msg_type=0x%02x, msg_len=%u",
|
|
gtp2_hdr_get_flags(gtp),
|
|
gtp2_hdr_get_version(gtp),
|
|
gtp2_hdr_get_piggyback_flag(gtp),
|
|
gtp2_hdr_get_teid_flag(gtp),
|
|
gtp2_hdr_get_spare_flag(gtp),
|
|
gtp2_hdr_get_msg_type(gtp),
|
|
gtp2_hdr_get_msg_len(gtp));
|
|
|
|
if (gtp2_hdr_get_teid_flag(gtp))
|
|
{
|
|
used += snprintf(buf + used, len - used, ", teid=%u", gtp2_hdr_get_teid(gtp));
|
|
}
|
|
else
|
|
{
|
|
used += snprintf(buf + used, len - used, ", seq=%u, spare=%u", gtp2_hdr_get_seq(gtp), gtp2_hdr_get_spare(gtp));
|
|
}
|
|
return used;
|
|
}
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|