2874 lines
80 KiB
C
2874 lines
80 KiB
C
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C"
|
||
{
|
||
#endif
|
||
|
||
#include "utils_internal.h"
|
||
#include <stdio.h>
|
||
#include <string.h>
|
||
#include <stdbool.h>
|
||
#include <arpa/inet.h>
|
||
#include <linux/if_ether.h>
|
||
#include <netinet/ip.h>
|
||
#include <netinet/ip6.h>
|
||
#include <linux/mpls.h>
|
||
#define __FAVOR_BSD 1
|
||
#include <netinet/tcp.h>
|
||
#include <netinet/udp.h>
|
||
|
||
/******************************************************************************
|
||
* ETH
|
||
******************************************************************************/
|
||
|
||
static inline int eth_hdr_get_dest(const struct ethhdr *hdr, char *buff, int size)
|
||
{
|
||
return snprintf(buff, size, "%02x:%02x:%02x:%02x:%02x:%02x",
|
||
hdr->h_dest[0], hdr->h_dest[1], hdr->h_dest[2],
|
||
hdr->h_dest[3], hdr->h_dest[4], hdr->h_dest[5]);
|
||
}
|
||
|
||
static inline int eth_hdr_get_source(const struct ethhdr *hdr, char *buff, int size)
|
||
{
|
||
return snprintf(buff, size, "%02x:%02x:%02x:%02x:%02x:%02x",
|
||
hdr->h_source[0], hdr->h_source[1], hdr->h_source[2],
|
||
hdr->h_source[3], hdr->h_source[4], hdr->h_source[5]);
|
||
}
|
||
|
||
static inline uint16_t eth_hdr_get_proto(const struct ethhdr *hdr)
|
||
{
|
||
return ntohs(hdr->h_proto);
|
||
}
|
||
|
||
static inline void eth_hdr_set_dest(struct ethhdr *hdr, const char *dest)
|
||
{
|
||
sscanf(dest, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
|
||
&hdr->h_dest[0], &hdr->h_dest[1], &hdr->h_dest[2],
|
||
&hdr->h_dest[3], &hdr->h_dest[4], &hdr->h_dest[5]);
|
||
}
|
||
|
||
static inline void eth_hdr_set_source(struct ethhdr *hdr, const char *source)
|
||
{
|
||
sscanf(source, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
|
||
&hdr->h_source[0], &hdr->h_source[1], &hdr->h_source[2],
|
||
&hdr->h_source[3], &hdr->h_source[4], &hdr->h_source[5]);
|
||
}
|
||
|
||
static inline void eth_hdr_set_proto(struct ethhdr *hdr, uint16_t proto)
|
||
{
|
||
hdr->h_proto = htons(proto);
|
||
}
|
||
|
||
// /usr/include/linux/if_ether.h
|
||
static inline int is_eth_proto(uint16_t proto)
|
||
{
|
||
switch (proto)
|
||
{
|
||
case ETH_P_LOOP:
|
||
case ETH_P_PUP:
|
||
case ETH_P_PUPAT:
|
||
case ETH_P_IP:
|
||
case ETH_P_X25:
|
||
case ETH_P_ARP:
|
||
case ETH_P_BPQ:
|
||
case ETH_P_IEEEPUP:
|
||
case ETH_P_IEEEPUPAT:
|
||
case ETH_P_DEC:
|
||
case ETH_P_DNA_DL:
|
||
case ETH_P_DNA_RC:
|
||
case ETH_P_DNA_RT:
|
||
case ETH_P_LAT:
|
||
case ETH_P_DIAG:
|
||
case ETH_P_CUST:
|
||
case ETH_P_SCA:
|
||
case ETH_P_TEB:
|
||
case ETH_P_RARP:
|
||
case ETH_P_ATALK:
|
||
case ETH_P_AARP:
|
||
case ETH_P_8021Q:
|
||
case ETH_P_8021AD:
|
||
case ETH_P_IPX:
|
||
case ETH_P_IPV6:
|
||
case ETH_P_PAUSE:
|
||
case ETH_P_SLOW:
|
||
case ETH_P_WCCP:
|
||
case ETH_P_PPP_DISC:
|
||
case ETH_P_PPP_SES:
|
||
case ETH_P_MPLS_UC:
|
||
case ETH_P_MPLS_MC:
|
||
case ETH_P_ATMMPOA:
|
||
case ETH_P_ATMFATE:
|
||
case ETH_P_PAE:
|
||
case ETH_P_AOE:
|
||
case ETH_P_TIPC:
|
||
case ETH_P_1588:
|
||
case ETH_P_FCOE:
|
||
case ETH_P_FIP:
|
||
case ETH_P_EDSA:
|
||
case ETH_P_802_3:
|
||
case ETH_P_AX25:
|
||
case ETH_P_ALL:
|
||
case ETH_P_802_2:
|
||
case ETH_P_SNAP:
|
||
case ETH_P_DDCMP:
|
||
case ETH_P_WAN_PPP:
|
||
case ETH_P_PPP_MP:
|
||
case ETH_P_LOCALTALK:
|
||
case ETH_P_CAN:
|
||
case ETH_P_PPPTALK:
|
||
case ETH_P_TR_802_2:
|
||
case ETH_P_MOBITEX:
|
||
case ETH_P_CONTROL:
|
||
case ETH_P_IRDA:
|
||
case ETH_P_ECONET:
|
||
case ETH_P_HDLC:
|
||
case ETH_P_ARCNET:
|
||
case ETH_P_DSA:
|
||
case ETH_P_TRAILER:
|
||
case ETH_P_PHONET:
|
||
case ETH_P_IEEE802154:
|
||
case 0x880B: // PPP：Point-to-Point Protoco
|
||
return 1;
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline const char *eth_proto_to_str(uint16_t proto)
|
||
{
|
||
switch (proto)
|
||
{
|
||
case ETH_P_LOOP:
|
||
return "ETH_P_LOOP";
|
||
case ETH_P_PUP:
|
||
return "ETH_P_PUP";
|
||
case ETH_P_PUPAT:
|
||
return "ETH_P_PUPAT";
|
||
case ETH_P_IP:
|
||
return "ETH_P_IP";
|
||
case ETH_P_X25:
|
||
return "ETH_P_X25";
|
||
case ETH_P_ARP:
|
||
return "ETH_P_ARP";
|
||
case ETH_P_BPQ:
|
||
return "ETH_P_BPQ";
|
||
case ETH_P_IEEEPUP:
|
||
return "ETH_P_IEEEPUP";
|
||
case ETH_P_IEEEPUPAT:
|
||
return "ETH_P_IEEEPUPAT";
|
||
case ETH_P_DEC:
|
||
return "ETH_P_DEC";
|
||
case ETH_P_DNA_DL:
|
||
return "ETH_P_DNA_DL";
|
||
case ETH_P_DNA_RC:
|
||
return "ETH_P_DNA_RC";
|
||
case ETH_P_DNA_RT:
|
||
return "ETH_P_DNA_RT";
|
||
case ETH_P_LAT:
|
||
return "ETH_P_LAT";
|
||
case ETH_P_DIAG:
|
||
return "ETH_P_DIAG";
|
||
case ETH_P_CUST:
|
||
return "ETH_P_CUST";
|
||
case ETH_P_SCA:
|
||
return "ETH_P_SCA";
|
||
case ETH_P_TEB:
|
||
return "ETH_P_TEB";
|
||
case ETH_P_RARP:
|
||
return "ETH_P_RARP";
|
||
case ETH_P_ATALK:
|
||
return "ETH_P_ATALK";
|
||
case ETH_P_AARP:
|
||
return "ETH_P_AARP";
|
||
case ETH_P_8021Q:
|
||
return "ETH_P_8021Q";
|
||
case ETH_P_8021AD:
|
||
return "ETH_P_8021AD";
|
||
case ETH_P_IPX:
|
||
return "ETH_P_IPX";
|
||
case ETH_P_IPV6:
|
||
return "ETH_P_IPV6";
|
||
case ETH_P_PAUSE:
|
||
return "ETH_P_PAUSE";
|
||
case ETH_P_SLOW:
|
||
return "ETH_P_SLOW";
|
||
case ETH_P_WCCP:
|
||
return "ETH_P_WCCP";
|
||
case ETH_P_PPP_DISC:
|
||
return "ETH_P_PPP_DISC";
|
||
case ETH_P_PPP_SES:
|
||
return "ETH_P_PPP_SES";
|
||
case ETH_P_MPLS_UC:
|
||
return "ETH_P_MPLS_UC";
|
||
case ETH_P_MPLS_MC:
|
||
return "ETH_P_MPLS_MC";
|
||
case ETH_P_ATMMPOA:
|
||
return "ETH_P_ATMMPOA";
|
||
case ETH_P_ATMFATE:
|
||
return "ETH_P_ATMFATE";
|
||
case ETH_P_PAE:
|
||
return "ETH_P_PAE";
|
||
case ETH_P_AOE:
|
||
return "ETH_P_AOE";
|
||
case ETH_P_TIPC:
|
||
return "ETH_P_TIPC";
|
||
case ETH_P_1588:
|
||
return "ETH_P_1588";
|
||
case ETH_P_FCOE:
|
||
return "ETH_P_FCOE";
|
||
case ETH_P_FIP:
|
||
return "ETH_P_FIP";
|
||
case ETH_P_EDSA:
|
||
return "ETH_P_EDSA";
|
||
case ETH_P_802_3:
|
||
return "ETH_P_802_3";
|
||
case ETH_P_AX25:
|
||
return "ETH_P_AX25";
|
||
case ETH_P_ALL:
|
||
return "ETH_P_ALL";
|
||
case ETH_P_802_2:
|
||
return "ETH_P_802_2";
|
||
case ETH_P_SNAP:
|
||
return "ETH_P_SNAP";
|
||
case ETH_P_DDCMP:
|
||
return "ETH_P_DDCMP";
|
||
case ETH_P_WAN_PPP:
|
||
return "ETH_P_WAN_PPP";
|
||
case ETH_P_PPP_MP:
|
||
return "ETH_P_PPP_MP";
|
||
case ETH_P_LOCALTALK:
|
||
return "ETH_P_LOCALTALK";
|
||
case ETH_P_CAN:
|
||
return "ETH_P_CAN";
|
||
case ETH_P_PPPTALK:
|
||
return "ETH_P_PPPTALK";
|
||
case ETH_P_TR_802_2:
|
||
return "ETH_P_TR_802_2";
|
||
case ETH_P_MOBITEX:
|
||
return "ETH_P_MOBITEX";
|
||
case ETH_P_CONTROL:
|
||
return "ETH_P_CONTROL";
|
||
case ETH_P_IRDA:
|
||
return "ETH_P_IRDA";
|
||
case ETH_P_ECONET:
|
||
return "ETH_P_ECONET";
|
||
case ETH_P_HDLC:
|
||
return "ETH_P_HDLC";
|
||
case ETH_P_ARCNET:
|
||
return "ETH_P_ARCNET";
|
||
case ETH_P_DSA:
|
||
return "ETH_P_DSA";
|
||
case ETH_P_TRAILER:
|
||
return "ETH_P_TRAILER";
|
||
case ETH_P_PHONET:
|
||
return "ETH_P_PHONET";
|
||
case ETH_P_IEEE802154:
|
||
return "ETH_P_IEEE802154";
|
||
case 0x880B:
|
||
return "ETH_P_PPP";
|
||
default:
|
||
return "ETH_P_UNKNOWN";
|
||
}
|
||
}
|
||
|
||
static inline int eth_hdr_to_str(const struct ethhdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
char dest[18] = {0};
|
||
char source[18] = {0};
|
||
uint16_t proto = eth_hdr_get_proto(hdr);
|
||
eth_hdr_get_dest(hdr, dest, sizeof(dest));
|
||
eth_hdr_get_source(hdr, source, sizeof(source));
|
||
return snprintf(buf, size, "ETH: source=%s dest=%s proto=%s",
|
||
source, dest, eth_proto_to_str(proto));
|
||
}
|
||
|
||
/******************************************************************************
|
||
* GREv0
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* GRE Header Format (Version 0)
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |C|R|K|S|s|Recur| Flags | Ver | Protocol Type |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Checksum (optional) | Offset (optional) |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Key (optional) |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Sequence Number (optional) |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Routing (optional)
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Address Family | SRE Offset | SRE Length |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Routing Information ...
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* https://datatracker.ietf.org/doc/html/rfc1701
|
||
* https://datatracker.ietf.org/doc/html/rfc2890
|
||
*/
|
||
|
||
struct gre0_hdr
|
||
{
|
||
uint16_t flags;
|
||
uint16_t protocol;
|
||
};
|
||
|
||
struct sre
|
||
{
|
||
uint16_t family;
|
||
uint8_t offset;
|
||
uint8_t length;
|
||
};
|
||
|
||
#define GRE0_FLAG_CHECKSUM 0x8000
|
||
#define GRE0_FLAG_ROUTING 0x4000
|
||
#define GRE0_FLAG_KEY 0x2000
|
||
#define GRE0_FLAG_SEQUENCE 0x1000
|
||
#define GRE0_FLAG_STRICT 0x0800
|
||
#define GRE0_FLAG_RECURSION 0x0700
|
||
#define GRE0_FLAG_VERSION 0x0007
|
||
|
||
static inline uint16_t gre0_hdr_get_flags(const struct gre0_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->flags);
|
||
}
|
||
|
||
static inline uint8_t gre0_hdr_get_checksum_flag(const struct gre0_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->flags) & GRE0_FLAG_CHECKSUM) >> 15;
|
||
}
|
||
|
||
static inline uint8_t gre0_hdr_get_routing_flag(const struct gre0_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->flags) & GRE0_FLAG_ROUTING) >> 14;
|
||
}
|
||
|
||
static inline uint8_t gre0_hdr_get_key_flag(const struct gre0_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->flags) & GRE0_FLAG_KEY) >> 13;
|
||
}
|
||
|
||
static inline uint8_t gre0_hdr_get_seq_flag(const struct gre0_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->flags) & GRE0_FLAG_SEQUENCE) >> 12;
|
||
}
|
||
|
||
static inline uint8_t gre0_hdr_get_strict_flag(const struct gre0_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->flags) & GRE0_FLAG_STRICT) >> 11;
|
||
}
|
||
|
||
static inline uint8_t gre0_hdr_get_version(const struct gre0_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->flags) & GRE0_FLAG_VERSION;
|
||
}
|
||
|
||
// ethernet protocol
|
||
static inline uint16_t gre0_hdr_get_proto(const struct gre0_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->protocol);
|
||
}
|
||
|
||
static inline uint16_t gre0_hdr_get_checksum(const struct gre0_hdr *hdr)
|
||
{
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
const char *ptr = ((const char *)hdr) + 4;
|
||
return ntohs(*(uint16_t *)ptr);
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline uint16_t gre0_hdr_get_offset(const struct gre0_hdr *hdr)
|
||
{
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
const char *ptr = ((const char *)hdr) + 6;
|
||
return ntohs(*(uint16_t *)ptr);
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
#pragma GCC diagnostic warning "-Warray-bounds"
|
||
|
||
static inline uint32_t gre0_hdr_get_key(const struct gre0_hdr *hdr)
|
||
{
|
||
if (gre0_hdr_get_key_flag(hdr))
|
||
{
|
||
const char *ptr = ((const char *)hdr) + 4;
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
return ntohl(*(uint32_t *)ptr);
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline uint32_t gre0_hdr_get_seq(const struct gre0_hdr *hdr)
|
||
{
|
||
if (gre0_hdr_get_seq_flag(hdr))
|
||
{
|
||
const char *ptr = ((const char *)hdr) + 4;
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
if (gre0_hdr_get_key_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
return ntohl(*(uint32_t *)ptr);
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline const char *gre0_hdr_get_routing_data(const struct gre0_hdr *hdr)
|
||
{
|
||
if (gre0_hdr_get_routing_flag(hdr))
|
||
{
|
||
const char *ptr = ((const char *)hdr) + 4;
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
if (gre0_hdr_get_key_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
if (gre0_hdr_get_seq_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
return ptr;
|
||
}
|
||
else
|
||
{
|
||
return NULL;
|
||
}
|
||
}
|
||
|
||
static inline uint16_t gre0_hdr_get_routing_len(const struct gre0_hdr *hdr)
|
||
{
|
||
if (gre0_hdr_get_routing_flag(hdr))
|
||
{
|
||
const char *ptr = gre0_hdr_get_routing_data(hdr);
|
||
|
||
uint16_t hdr_len = 0;
|
||
while (1)
|
||
{
|
||
struct sre *sre = (struct sre *)(ptr + hdr_len);
|
||
if (sre->length == 0)
|
||
{
|
||
hdr_len += sizeof(struct sre);
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
hdr_len += sizeof(struct sre) + sre->length;
|
||
}
|
||
}
|
||
return hdr_len;
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline uint16_t calc_gre0_hdr_len(const char *data, uint32_t len)
|
||
{
|
||
if (data == NULL || len < sizeof(struct gre0_hdr))
|
||
{
|
||
return 0;
|
||
}
|
||
const struct gre0_hdr *hdr = (const struct gre0_hdr *)data;
|
||
uint16_t hdr_len = 4;
|
||
uint16_t flags = ntohs(hdr->flags);
|
||
if ((flags & GRE0_FLAG_CHECKSUM) || (flags & GRE0_FLAG_ROUTING))
|
||
{
|
||
hdr_len += 4; // skip checksum and offset fields
|
||
}
|
||
if (flags & GRE0_FLAG_KEY)
|
||
{
|
||
hdr_len += 4; // skip key field
|
||
}
|
||
if (flags & GRE0_FLAG_SEQUENCE)
|
||
{
|
||
hdr_len += 4; // skip sequence number field
|
||
}
|
||
if (flags & GRE0_FLAG_ROUTING)
|
||
{
|
||
while (hdr_len + sizeof(struct sre) <= len)
|
||
{
|
||
struct sre *sre = (struct sre *)((char *)data + hdr_len);
|
||
if (sre->length == 0)
|
||
{
|
||
hdr_len += sizeof(struct sre);
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
hdr_len += sizeof(struct sre) + sre->length;
|
||
}
|
||
}
|
||
}
|
||
return hdr_len;
|
||
}
|
||
|
||
static inline void gre0_hdr_set_flags(struct gre0_hdr *hdr, uint16_t flags)
|
||
{
|
||
hdr->flags = htons(flags);
|
||
}
|
||
|
||
static inline void gre0_hdr_set_checksum_flag(struct gre0_hdr *hdr, uint8_t flag)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_CHECKSUM) | flag << 15);
|
||
}
|
||
|
||
static inline void gre0_hdr_set_routing_flag(struct gre0_hdr *hdr, uint8_t flag)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_ROUTING) | flag << 14);
|
||
}
|
||
|
||
static inline void gre0_hdr_set_key_flag(struct gre0_hdr *hdr, uint8_t flag)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_KEY) | flag << 13);
|
||
}
|
||
|
||
static inline void gre0_hdr_set_seq_flag(struct gre0_hdr *hdr, uint8_t flag)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_SEQUENCE) | flag << 12);
|
||
}
|
||
|
||
static inline void gre0_hdr_set_strict_flag(struct gre0_hdr *hdr, uint8_t flag)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_STRICT) | flag << 11);
|
||
}
|
||
|
||
static inline void gre0_hdr_set_version(struct gre0_hdr *hdr, uint8_t version)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_VERSION) | version);
|
||
}
|
||
|
||
static inline void gre0_hdr_set_proto(struct gre0_hdr *hdr, uint16_t proto)
|
||
{
|
||
hdr->protocol = htons(proto);
|
||
}
|
||
|
||
static inline void gre0_hdr_set_checksum(struct gre0_hdr *hdr, uint16_t checksum)
|
||
{
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
char *ptr = ((char *)hdr) + 4;
|
||
*(uint16_t *)ptr = htons(checksum);
|
||
}
|
||
}
|
||
|
||
static inline void gre0_hdr_set_offset(struct gre0_hdr *hdr, uint16_t offset)
|
||
{
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
char *ptr = ((char *)hdr) + 6;
|
||
*(uint16_t *)ptr = htons(offset);
|
||
}
|
||
}
|
||
|
||
static inline void gre0_hdr_set_key(struct gre0_hdr *hdr, uint32_t key)
|
||
{
|
||
if (gre0_hdr_get_key_flag(hdr))
|
||
{
|
||
char *ptr = ((char *)hdr) + 4;
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
*(uint32_t *)ptr = htonl(key);
|
||
}
|
||
}
|
||
|
||
static inline void gre0_hdr_set_seq(struct gre0_hdr *hdr, uint32_t sequence)
|
||
{
|
||
if (gre0_hdr_get_seq_flag(hdr))
|
||
{
|
||
char *ptr = ((char *)hdr) + 4;
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
if (gre0_hdr_get_key_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
*(uint32_t *)ptr = htonl(sequence);
|
||
}
|
||
}
|
||
|
||
static inline void gre0_hdr_set_routing_data(struct gre0_hdr *hdr, const char *data, uint16_t len)
|
||
{
|
||
if (gre0_hdr_get_routing_flag(hdr))
|
||
{
|
||
char *ptr = ((char *)hdr) + 4;
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
if (gre0_hdr_get_key_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
if (gre0_hdr_get_seq_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
|
||
memcpy(ptr, data, len);
|
||
}
|
||
}
|
||
|
||
static inline int gre0_hdr_to_str(const struct gre0_hdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
|
||
int used = 0;
|
||
uint16_t proto = gre0_hdr_get_proto(hdr);
|
||
used += snprintf(buf + used, size - used, "GRE: flags=0x%04x (checksum_flag=%u, routing_flag=%u, key_flag=%u, seq_flag=%u, strict_flag=%u, version=%u), proto=%s",
|
||
gre0_hdr_get_flags(hdr), gre0_hdr_get_checksum_flag(hdr), gre0_hdr_get_routing_flag(hdr), gre0_hdr_get_key_flag(hdr),
|
||
gre0_hdr_get_seq_flag(hdr), gre0_hdr_get_strict_flag(hdr), gre0_hdr_get_version(hdr), eth_proto_to_str(proto));
|
||
if (gre0_hdr_get_checksum_flag(hdr))
|
||
{
|
||
used += snprintf(buf + used, size - used, ", checksum=0x%x, offset=%u", gre0_hdr_get_checksum(hdr), gre0_hdr_get_offset(hdr));
|
||
}
|
||
if (gre0_hdr_get_key_flag(hdr))
|
||
{
|
||
used += snprintf(buf + used, size - used, ", key=%u", gre0_hdr_get_key(hdr));
|
||
}
|
||
if (gre0_hdr_get_seq_flag(hdr))
|
||
{
|
||
used += snprintf(buf + used, size - used, ", seq=%u", gre0_hdr_get_seq(hdr));
|
||
}
|
||
if (gre0_hdr_get_routing_flag(hdr))
|
||
{
|
||
used += snprintf(buf + used, size - used, ", routing_len=%u", gre0_hdr_get_routing_len(hdr));
|
||
}
|
||
|
||
return used;
|
||
}
|
||
|
||
/******************************************************************************
|
||
* GREv1
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* Enhanced GRE header (Version 1)
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |C|R|K|S|s|Recur|A| Flags | Ver | Protocol Type |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Key (HW) Payload Length | Key (LW) Call ID |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Sequence Number (Optional) |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Acknowledgment Number (Optional) |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* https://datatracker.ietf.org/doc/html/rfc2637
|
||
*/
|
||
|
||
struct gre1_hdr
|
||
{
|
||
uint16_t flags;
|
||
uint16_t protocol;
|
||
uint16_t payload_length;
|
||
uint16_t call_id;
|
||
};
|
||
|
||
#define GRE1_FLAG_CHECKSUM 0x8000
|
||
#define GRE1_FLAG_ROUTING 0x4000
|
||
#define GRE1_FLAG_KEY 0x2000
|
||
#define GRE1_FLAG_SEQUENCE 0x1000
|
||
#define GRE1_FLAG_STRICT 0x0800
|
||
#define GRE1_FLAG_RECURSION 0x0700
|
||
#define GRE1_FLAG_ACK 0x0080 /* only in special PPTPized GRE header */
|
||
#define GRE1_FLAG_VERSION 0x0007
|
||
|
||
static inline uint16_t gre1_hdr_get_flags(const struct gre1_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->flags);
|
||
}
|
||
|
||
static inline uint8_t gre1_hdr_get_seq_flag(const struct gre1_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->flags) & GRE1_FLAG_SEQUENCE) >> 12;
|
||
}
|
||
|
||
static inline uint8_t gre1_hdr_get_ack_flag(const struct gre1_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->flags) & GRE1_FLAG_ACK) >> 7;
|
||
}
|
||
|
||
static inline uint8_t gre1_hdr_get_version(const struct gre1_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->flags) & GRE1_FLAG_VERSION;
|
||
}
|
||
|
||
// ethernet protocol type
|
||
static inline uint16_t gre1_hdr_get_proto(const struct gre1_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->protocol);
|
||
}
|
||
|
||
static inline uint16_t gre1_hdr_get_payload_length(const struct gre1_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->payload_length);
|
||
}
|
||
|
||
static inline uint16_t gre1_hdr_get_call_id(const struct gre1_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->call_id);
|
||
}
|
||
|
||
static inline uint32_t gre1_hdr_get_seq(const struct gre1_hdr *hdr)
|
||
{
|
||
if (gre1_hdr_get_seq_flag(hdr))
|
||
{
|
||
const char *ptr = ((const char *)hdr) + 8;
|
||
return ntohl(*((uint32_t *)ptr));
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline uint32_t gre1_hdr_get_ack(const struct gre1_hdr *hdr)
|
||
{
|
||
if (gre1_hdr_get_ack_flag(hdr))
|
||
{
|
||
const char *ptr = ((const char *)hdr) + 8;
|
||
if (gre1_hdr_get_seq_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
return ntohl(*((uint32_t *)ptr));
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline uint16_t calc_gre1_hdr_len(const char *data, uint32_t len)
|
||
{
|
||
if (data == NULL || len < sizeof(struct gre1_hdr))
|
||
{
|
||
return 0;
|
||
}
|
||
const struct gre1_hdr *hdr = (const struct gre1_hdr *)data;
|
||
uint16_t hdr_len = 8;
|
||
uint16_t flags = gre1_hdr_get_flags(hdr);
|
||
if (flags & GRE1_FLAG_SEQUENCE)
|
||
{
|
||
hdr_len += 4;
|
||
}
|
||
if (flags & GRE1_FLAG_ACK)
|
||
{
|
||
hdr_len += 4;
|
||
}
|
||
return hdr_len;
|
||
}
|
||
|
||
static inline void gre1_hdr_set_flags(struct gre1_hdr *hdr, uint16_t flags)
|
||
{
|
||
hdr->flags = htons(flags);
|
||
}
|
||
|
||
static inline void gre1_hdr_set_seq_flag(struct gre1_hdr *hdr, uint8_t seq_flag)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_SEQUENCE) | seq_flag << 12);
|
||
}
|
||
|
||
static inline void gre1_hdr_set_ack_flag(struct gre1_hdr *hdr, uint8_t ack_flag)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_ACK) | ack_flag << 7);
|
||
}
|
||
|
||
static inline void gre1_hdr_set_version(struct gre1_hdr *hdr, uint8_t version)
|
||
{
|
||
hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_VERSION) | version);
|
||
}
|
||
|
||
static inline void gre1_hdr_set_proto(struct gre1_hdr *hdr, uint16_t proto)
|
||
{
|
||
hdr->protocol = htons(proto);
|
||
}
|
||
|
||
static inline void gre1_hdr_set_payload_length(struct gre1_hdr *hdr, uint16_t payload_length)
|
||
{
|
||
hdr->payload_length = htons(payload_length);
|
||
}
|
||
|
||
static inline void gre1_hdr_set_call_id(struct gre1_hdr *hdr, uint16_t call_id)
|
||
{
|
||
hdr->call_id = htons(call_id);
|
||
}
|
||
|
||
static inline void gre1_hdr_set_seq(struct gre1_hdr *hdr, uint32_t seq)
|
||
{
|
||
if (gre1_hdr_get_seq_flag(hdr))
|
||
{
|
||
char *ptr = ((char *)hdr) + 8;
|
||
*((uint32_t *)ptr) = htonl(seq);
|
||
}
|
||
}
|
||
|
||
static inline void gre1_hdr_set_ack(struct gre1_hdr *hdr, uint32_t ack)
|
||
{
|
||
if (gre1_hdr_get_ack_flag(hdr))
|
||
{
|
||
char *ptr = ((char *)hdr) + 8;
|
||
if (gre1_hdr_get_seq_flag(hdr))
|
||
{
|
||
ptr += 4;
|
||
}
|
||
*((uint32_t *)ptr) = htonl(ack);
|
||
}
|
||
}
|
||
|
||
static inline int gre1_hdr_to_str(const struct gre1_hdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
|
||
int used = 0;
|
||
uint16_t proto = gre1_hdr_get_proto(hdr);
|
||
used += snprintf(buf + used, size - used, "GRE: flags=0x%04x (seq_flag=%u ack_flag=%u version=%u), proto=%s, payload_length=%u, call_id=%u",
|
||
gre1_hdr_get_flags(hdr), gre1_hdr_get_seq_flag(hdr), gre1_hdr_get_ack_flag(hdr), gre1_hdr_get_version(hdr),
|
||
eth_proto_to_str(proto), gre1_hdr_get_payload_length(hdr), gre1_hdr_get_call_id(hdr));
|
||
if (gre1_hdr_get_seq_flag(hdr))
|
||
{
|
||
used += snprintf(buf + used, size - used, ", seq=%u", gre1_hdr_get_seq(hdr));
|
||
}
|
||
if (gre1_hdr_get_ack_flag(hdr))
|
||
{
|
||
used += snprintf(buf + used, size - used, ", ack=%u", gre1_hdr_get_ack(hdr));
|
||
}
|
||
|
||
return used;
|
||
}
|
||
|
||
/******************************************************************************
|
||
* GRE
|
||
******************************************************************************/
|
||
|
||
// return GRE version 0 or 1
|
||
// return 255 if not GRE
|
||
static inline uint8_t peek_gre_version(const char *data, uint32_t len)
|
||
{
|
||
if (len < MIN(sizeof(struct gre0_hdr), sizeof(struct gre1_hdr)))
|
||
{
|
||
return 255;
|
||
}
|
||
|
||
uint16_t flag = *(uint16_t *)data;
|
||
return ntohs(flag) & 0x0007;
|
||
}
|
||
|
||
static inline uint16_t peek_gre_proto(const char *data, uint32_t len)
|
||
{
|
||
switch (peek_gre_version(data, len))
|
||
{
|
||
case 0:
|
||
return gre0_hdr_get_proto((const struct gre0_hdr *)data);
|
||
case 1:
|
||
return gre1_hdr_get_proto((const struct gre1_hdr *)data);
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline uint16_t calc_gre_hdr_len(const char *data, uint32_t len)
|
||
{
|
||
switch (peek_gre_version(data, len))
|
||
{
|
||
case 0:
|
||
return calc_gre0_hdr_len(data, len);
|
||
case 1:
|
||
return calc_gre1_hdr_len(data, len);
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline int gre_hdr_to_str(const char *hdr, uint16_t len, char *buf, size_t size)
|
||
{
|
||
switch (peek_gre_version(hdr, len))
|
||
{
|
||
case 0:
|
||
return gre0_hdr_to_str((const struct gre0_hdr *)hdr, buf, size);
|
||
case 1:
|
||
return gre1_hdr_to_str((const struct gre1_hdr *)hdr, buf, size);
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
/******************************************************************************
|
||
* GTPv1
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* https://en.wikipedia.org/wiki/GPRS_Tunnelling_Protocol
|
||
*
|
||
* GTP-C: Gateway GPRS Support Nodes (GGSN) <----> Serving GPRS support Nodes (SGSN)
|
||
* GTP-U: Radio Access Network <----> Core Network
|
||
*
|
||
* GTPv1:
|
||
* -> GTPv1-C
|
||
* -> GTPv1-U
|
||
*/
|
||
|
||
/*
|
||
* GTP version 1
|
||
*
|
||
* 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 |T|R|E|S|N| Message Type | Message Length |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | TEID |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Sequence Number(O) |N-PDU Number(O)|Next Ext Hdr(O)| (optional headers)
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* Message Length:
|
||
* a 16-bit field that indicates the length of the payload in bytes
|
||
* (rest of the packet following the mandatory 8-byte GTP header).
|
||
* Includes the optional fields.
|
||
*/
|
||
|
||
/*
|
||
* Next Extension Headers
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Length | Contents |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | ... |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Contents | Next Ext Hdr |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* Extension length
|
||
* an 8-bit field. This field states the length of this extension header,
|
||
* including the length, the contents, and the next extension header field,
|
||
* in 4-octet units, so the length of the extension must always be a multiple of 4.
|
||
*/
|
||
|
||
struct gtp1_hdr
|
||
{
|
||
uint8_t flags;
|
||
uint8_t msg_type;
|
||
uint16_t msg_len;
|
||
uint32_t teid;
|
||
} __attribute__((packed));
|
||
|
||
struct gtp1_hdr_long
|
||
{
|
||
uint8_t flags;
|
||
uint8_t msg_type;
|
||
uint16_t msg_len;
|
||
uint32_t teid;
|
||
|
||
uint16_t seq;
|
||
uint8_t npdu;
|
||
uint8_t next_ext_hdr;
|
||
} __attribute__((packed));
|
||
|
||
#define GTP1_FLAG_N_PDU (0x01)
|
||
#define GTP1_FLAG_SEQ_NUM (0x02)
|
||
#define GTP1_FLAG_EXT_HDR (0x04)
|
||
#define GTP1_FLAG_RESERVED (0x08)
|
||
#define GTP1_FLAG_PROTOCOL (0x10)
|
||
#define GTP1_FLAG_VERSION (0xE0)
|
||
|
||
static inline uint8_t gtp1_hdr_get_flags(const struct gtp1_hdr *gtp)
|
||
{
|
||
return gtp->flags;
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_version(const struct gtp1_hdr *gtp)
|
||
{
|
||
return (gtp->flags & GTP1_FLAG_VERSION) >> 5;
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_proto(const struct gtp1_hdr *gtp)
|
||
{
|
||
return (gtp->flags & GTP1_FLAG_PROTOCOL) >> 4;
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_reserved(const struct gtp1_hdr *gtp)
|
||
{
|
||
return (gtp->flags & GTP1_FLAG_RESERVED) >> 3;
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_ext_flag(const struct gtp1_hdr *gtp)
|
||
{
|
||
return (gtp->flags & GTP1_FLAG_EXT_HDR) >> 2;
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_seq_flag(const struct gtp1_hdr *gtp)
|
||
{
|
||
return (gtp->flags & GTP1_FLAG_SEQ_NUM) >> 1;
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_npdu_flag(const struct gtp1_hdr *gtp)
|
||
{
|
||
return (gtp->flags & GTP1_FLAG_N_PDU) >> 0;
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_msg_type(const struct gtp1_hdr *gtp)
|
||
{
|
||
return gtp->msg_type;
|
||
}
|
||
|
||
static inline uint16_t gtp1_hdr_get_msg_len(const struct gtp1_hdr *gtp)
|
||
{
|
||
return ntohs(gtp->msg_len);
|
||
}
|
||
|
||
static inline uint32_t gtp1_hdr_get_teid(const struct gtp1_hdr *gtp)
|
||
{
|
||
return ntohl(gtp->teid);
|
||
}
|
||
|
||
static inline uint16_t gtp1_hdr_get_seq(const struct gtp1_hdr *gtp)
|
||
{
|
||
if (gtp1_hdr_get_seq_flag(gtp))
|
||
{
|
||
const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp;
|
||
return ntohs(gtp_long->seq);
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_npdu(const struct gtp1_hdr *gtp)
|
||
{
|
||
if (gtp1_hdr_get_npdu_flag(gtp))
|
||
{
|
||
const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp;
|
||
return gtp_long->npdu;
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline uint8_t gtp1_hdr_get_next_ext_type(const struct gtp1_hdr *gtp)
|
||
{
|
||
if (gtp1_hdr_get_ext_flag(gtp))
|
||
{
|
||
const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp;
|
||
return gtp_long->next_ext_hdr;
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
// include gtp fixed header and optional headers and extension headers
|
||
static inline uint16_t calc_gtp1_hdr_len(const char *data, uint16_t len)
|
||
{
|
||
if (data == NULL || len < sizeof(struct gtp1_hdr))
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
const struct gtp1_hdr *gtp = (const struct gtp1_hdr *)data;
|
||
if (gtp1_hdr_get_version(gtp) != 1)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
if (gtp1_hdr_get_flags(gtp) & (GTP1_FLAG_SEQ_NUM | GTP1_FLAG_N_PDU | GTP1_FLAG_EXT_HDR))
|
||
{
|
||
if (sizeof(struct gtp1_hdr_long) > len)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)data;
|
||
uint8_t next_ext_hdr = gtp_long->next_ext_hdr;
|
||
uint16_t offset = sizeof(struct gtp1_hdr_long);
|
||
|
||
while (next_ext_hdr)
|
||
{
|
||
if (offset + 1 > len)
|
||
{
|
||
return 0;
|
||
}
|
||
uint16_t ext_hdr_len = *((char *)data + offset) * 4;
|
||
if (ext_hdr_len == 0 || offset + ext_hdr_len > len)
|
||
{
|
||
return 0;
|
||
}
|
||
offset += ext_hdr_len; // skip extension header
|
||
next_ext_hdr = *((char *)data + offset - 1);
|
||
}
|
||
|
||
return offset;
|
||
}
|
||
else
|
||
{
|
||
return sizeof(struct gtp1_hdr);
|
||
}
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_flags(struct gtp1_hdr *gtp, uint8_t flags)
|
||
{
|
||
gtp->flags = flags;
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_version(struct gtp1_hdr *gtp, uint8_t version)
|
||
{
|
||
gtp->flags = (gtp->flags & ~GTP1_FLAG_VERSION) | (version << 5);
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_proto(struct gtp1_hdr *gtp, uint8_t protocol_type)
|
||
{
|
||
gtp->flags = (gtp->flags & ~GTP1_FLAG_PROTOCOL) | (protocol_type << 4);
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_reserved(struct gtp1_hdr *gtp, uint8_t reserved)
|
||
{
|
||
gtp->flags = (gtp->flags & ~GTP1_FLAG_RESERVED) | (reserved << 3);
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_ext_flag(struct gtp1_hdr *gtp, uint8_t ext_flag)
|
||
{
|
||
gtp->flags = (gtp->flags & ~GTP1_FLAG_EXT_HDR) | (ext_flag << 2);
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_seq_flag(struct gtp1_hdr *gtp, uint8_t seq_flag)
|
||
{
|
||
gtp->flags = (gtp->flags & ~GTP1_FLAG_SEQ_NUM) | (seq_flag << 1);
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_npdu_flag(struct gtp1_hdr *gtp, uint8_t npdu_flag)
|
||
{
|
||
gtp->flags = (gtp->flags & ~GTP1_FLAG_N_PDU) | (npdu_flag << 0);
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_msg_type(struct gtp1_hdr *gtp, uint8_t msg_type)
|
||
{
|
||
gtp->msg_type = msg_type;
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_msg_len(struct gtp1_hdr *gtp, uint16_t msg_len)
|
||
{
|
||
gtp->msg_len = htons(msg_len);
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_teid(struct gtp1_hdr *gtp, uint32_t teid)
|
||
{
|
||
gtp->teid = htonl(teid);
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_seq(struct gtp1_hdr *gtp, uint16_t seq)
|
||
{
|
||
if (gtp1_hdr_get_seq_flag(gtp))
|
||
{
|
||
struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp;
|
||
gtp_long->seq = htons(seq);
|
||
}
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_npdu(struct gtp1_hdr *gtp, uint8_t npdu)
|
||
{
|
||
if (gtp1_hdr_get_npdu_flag(gtp))
|
||
{
|
||
struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp;
|
||
gtp_long->npdu = npdu;
|
||
}
|
||
}
|
||
|
||
static inline void gtp1_hdr_set_next_ext_type(struct gtp1_hdr *gtp, uint8_t next_ext_hdr)
|
||
{
|
||
if (gtp1_hdr_get_ext_flag(gtp))
|
||
{
|
||
struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp;
|
||
gtp_long->next_ext_hdr = next_ext_hdr;
|
||
}
|
||
}
|
||
|
||
static inline int gtp1_hdr_to_str(const struct gtp1_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, protocol=%u, reserved=%u, ext_flag=%u, seq_flag=%u, npdu_flag=%u), msg_type=0x%02x, msg_len=%u, teid=%u",
|
||
gtp1_hdr_get_flags(gtp),
|
||
gtp1_hdr_get_version(gtp),
|
||
gtp1_hdr_get_proto(gtp),
|
||
gtp1_hdr_get_reserved(gtp),
|
||
gtp1_hdr_get_ext_flag(gtp),
|
||
gtp1_hdr_get_seq_flag(gtp),
|
||
gtp1_hdr_get_npdu_flag(gtp),
|
||
gtp1_hdr_get_msg_type(gtp),
|
||
gtp1_hdr_get_msg_len(gtp),
|
||
gtp1_hdr_get_teid(gtp));
|
||
|
||
if (gtp1_hdr_get_seq_flag(gtp))
|
||
{
|
||
used += snprintf(buf + used, len - used, ", seq=%u", gtp1_hdr_get_seq(gtp));
|
||
}
|
||
if (gtp1_hdr_get_npdu_flag(gtp))
|
||
{
|
||
used += snprintf(buf + used, len - used, ", npdu=%u", gtp1_hdr_get_npdu(gtp));
|
||
}
|
||
if (gtp1_hdr_get_ext_flag(gtp))
|
||
{
|
||
used += snprintf(buf + used, len - used, ", next_ext_hdr=%u", gtp1_hdr_get_next_ext_type(gtp));
|
||
}
|
||
return used;
|
||
}
|
||
|
||
/******************************************************************************
|
||
* GTPv2
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* 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)
|
||
|
||
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;
|
||
}
|
||
|
||
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);
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
/******************************************************************************
|
||
* GTP
|
||
******************************************************************************/
|
||
|
||
// return GTP version 0 or 1
|
||
// return 255 if not GTP
|
||
static inline uint8_t peek_gtp_version(const char *data, uint16_t len)
|
||
{
|
||
if (data == NULL || len == 0)
|
||
{
|
||
return 255;
|
||
}
|
||
|
||
return ((*(uint8_t *)data) >> 5) & 0x07;
|
||
}
|
||
|
||
static inline uint16_t calc_gtp_hdr_len(const char *data, uint32_t len)
|
||
{
|
||
switch (peek_gtp_version(data, len))
|
||
{
|
||
case 1:
|
||
return calc_gtp1_hdr_len(data, len);
|
||
case 2:
|
||
return calc_gtp2_hdr_len(data, len);
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline int gtp_hdr_to_str(const char *hdr, uint16_t len, char *buf, size_t size)
|
||
{
|
||
switch (peek_gtp_version(hdr, len))
|
||
{
|
||
case 1:
|
||
return gtp1_hdr_to_str((const struct gtp1_hdr *)hdr, buf, size);
|
||
case 2:
|
||
return gtp2_hdr_to_str((const struct gtp2_hdr *)hdr, buf, size);
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
/******************************************************************************
|
||
* IPv4
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* Internet Header Format
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |Version| IHL |Type of Service| Total Length |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Identification |Flags| Fragment Offset |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Time to Live | Protocol | Header Checksum |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Source Address |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Destination Address |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Options | Padding |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*/
|
||
|
||
static inline uint8_t ip4_hdr_get_version(const struct ip *hdr)
|
||
{
|
||
return hdr->ip_v;
|
||
}
|
||
|
||
// IP Options are included in the hdr_len field
|
||
static inline uint8_t ip4_hdr_get_hdr_len(const struct ip *hdr)
|
||
{
|
||
return hdr->ip_hl << 2;
|
||
}
|
||
|
||
static inline uint8_t ip4_hdr_get_tos(const struct ip *hdr)
|
||
{
|
||
return hdr->ip_tos;
|
||
}
|
||
|
||
static inline uint16_t ip4_hdr_get_total_len(const struct ip *hdr)
|
||
{
|
||
return ntohs(hdr->ip_len);
|
||
}
|
||
|
||
static inline uint16_t ip4_hdr_get_ipid(const struct ip *hdr)
|
||
{
|
||
return ntohs(hdr->ip_id);
|
||
}
|
||
|
||
static inline uint8_t ip4_hdr_get_flags(const struct ip *hdr)
|
||
{
|
||
return (ntohs(hdr->ip_off) & (~IP_OFFMASK)) >> 13;
|
||
}
|
||
|
||
static inline bool ip4_hdr_get_rf_flag(const struct ip *hdr)
|
||
{
|
||
return (ntohs(hdr->ip_off) & IP_RF) != 0;
|
||
}
|
||
|
||
static inline bool ip4_hdr_get_df_flag(const struct ip *hdr)
|
||
{
|
||
return (ntohs(hdr->ip_off) & IP_DF) != 0;
|
||
}
|
||
|
||
static inline bool ip4_hdr_get_mf_flag(const struct ip *hdr)
|
||
{
|
||
return (ntohs(hdr->ip_off) & IP_MF) != 0;
|
||
}
|
||
|
||
static inline uint16_t ip4_hdr_get_frag_offset(const struct ip *hdr)
|
||
{
|
||
return (ntohs(hdr->ip_off) & IP_OFFMASK) << 3;
|
||
}
|
||
|
||
static inline uint8_t ip4_hdr_get_ttl(const struct ip *hdr)
|
||
{
|
||
return hdr->ip_ttl;
|
||
}
|
||
|
||
static inline uint8_t ip4_hdr_get_proto(const struct ip *hdr)
|
||
{
|
||
return hdr->ip_p;
|
||
}
|
||
|
||
static inline uint16_t ip4_hdr_get_checksum(const struct ip *hdr)
|
||
{
|
||
return ntohs(hdr->ip_sum);
|
||
}
|
||
|
||
static inline uint32_t ip4_hdr_get_src_addr(const struct ip *hdr)
|
||
{
|
||
return ntohl(hdr->ip_src.s_addr);
|
||
}
|
||
|
||
static inline uint32_t ip4_hdr_get_dst_addr(const struct ip *hdr)
|
||
{
|
||
return ntohl(hdr->ip_dst.s_addr);
|
||
}
|
||
|
||
static inline struct in_addr ip4_hdr_get_src_in_addr(const struct ip *hdr)
|
||
{
|
||
return hdr->ip_src;
|
||
}
|
||
|
||
static inline struct in_addr ip4_hdr_get_dst_in_addr(const struct ip *hdr)
|
||
{
|
||
return hdr->ip_dst;
|
||
}
|
||
|
||
static inline uint8_t ip4_hdr_get_opt_len(const struct ip *hdr)
|
||
{
|
||
return ip4_hdr_get_hdr_len(hdr) - sizeof(struct ip);
|
||
}
|
||
|
||
static inline const char *ip4_hdr_get_opt_data(const struct ip *hdr)
|
||
{
|
||
if (ip4_hdr_get_opt_len(hdr) == 0)
|
||
{
|
||
return NULL;
|
||
}
|
||
|
||
return (const char *)hdr + sizeof(struct ip);
|
||
}
|
||
|
||
static inline void ip4_hdr_set_version(struct ip *hdr, uint8_t version)
|
||
{
|
||
hdr->ip_v = version;
|
||
}
|
||
|
||
static inline void ip4_hdr_set_hdr_len(struct ip *hdr, uint8_t hdr_len)
|
||
{
|
||
hdr->ip_hl = hdr_len >> 2;
|
||
}
|
||
|
||
static inline void ip4_hdr_set_tos(struct ip *hdr, uint8_t tos)
|
||
{
|
||
hdr->ip_tos = tos;
|
||
}
|
||
|
||
static inline void ip4_hdr_set_total_len(struct ip *hdr, uint16_t total_len)
|
||
{
|
||
hdr->ip_len = htons(total_len);
|
||
}
|
||
|
||
static inline void ip4_hdr_set_ipid(struct ip *hdr, uint16_t ipid)
|
||
{
|
||
hdr->ip_id = htons(ipid);
|
||
}
|
||
|
||
static inline void ip4_hdr_set_flags(struct ip *hdr, uint8_t flags)
|
||
{
|
||
hdr->ip_off = htons((flags << 13) | (ntohs(hdr->ip_off) & IP_OFFMASK));
|
||
}
|
||
|
||
static inline void ip4_hdr_set_rf_flag(struct ip *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_RF);
|
||
}
|
||
else
|
||
{
|
||
hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_RF);
|
||
}
|
||
}
|
||
|
||
static inline void ip4_hdr_set_df_flag(struct ip *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_DF);
|
||
}
|
||
else
|
||
{
|
||
hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_DF);
|
||
}
|
||
}
|
||
|
||
static inline void ip4_hdr_set_mf_flag(struct ip *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_MF);
|
||
}
|
||
else
|
||
{
|
||
hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_MF);
|
||
}
|
||
}
|
||
|
||
static inline void ip4_hdr_set_frag_offset(struct ip *hdr, uint16_t frag_offset)
|
||
{
|
||
hdr->ip_off = htons((frag_offset >> 3) | (ntohs(hdr->ip_off) & ~IP_OFFMASK));
|
||
}
|
||
|
||
static inline void ip4_hdr_set_ttl(struct ip *hdr, uint8_t ttl)
|
||
{
|
||
hdr->ip_ttl = ttl;
|
||
}
|
||
|
||
static inline void ip4_hdr_set_protocol(struct ip *hdr, uint8_t protocol)
|
||
{
|
||
hdr->ip_p = protocol;
|
||
}
|
||
|
||
static inline void ip4_hdr_set_checksum(struct ip *hdr, uint16_t checksum)
|
||
{
|
||
hdr->ip_sum = htons(checksum);
|
||
}
|
||
|
||
static inline void ip4_hdr_set_src_addr(struct ip *hdr, uint32_t saddr)
|
||
{
|
||
hdr->ip_src.s_addr = htonl(saddr);
|
||
}
|
||
|
||
static inline void ip4_hdr_set_dst_addr(struct ip *hdr, uint32_t daddr)
|
||
{
|
||
hdr->ip_dst.s_addr = htonl(daddr);
|
||
}
|
||
|
||
static inline void ip4_hdr_set_src_in_addr(struct ip *hdr, struct in_addr saddr)
|
||
{
|
||
hdr->ip_src = saddr;
|
||
}
|
||
|
||
static inline void ip4_hdr_set_dst_in_addr(struct ip *hdr, struct in_addr daddr)
|
||
{
|
||
hdr->ip_dst = daddr;
|
||
}
|
||
|
||
static inline void ip4_hdr_set_opt_len(struct ip *hdr, uint8_t opt_len)
|
||
{
|
||
ip4_hdr_set_hdr_len(hdr, opt_len + sizeof(struct ip));
|
||
}
|
||
|
||
// must be called after ip4_hdr_set_opt_len
|
||
static inline void ip4_hdr_set_opt_data(struct ip *hdr, const char *opt_data)
|
||
{
|
||
if (opt_data)
|
||
{
|
||
memcpy((char *)hdr + sizeof(struct ip), opt_data, ip4_hdr_get_opt_len(hdr));
|
||
}
|
||
}
|
||
|
||
// /usr/include/netinet/in.h
|
||
static inline int is_ip_proto(uint16_t proto)
|
||
{
|
||
switch (proto)
|
||
{
|
||
case IPPROTO_IP:
|
||
case IPPROTO_ICMP:
|
||
case IPPROTO_IGMP:
|
||
case IPPROTO_IPIP:
|
||
case IPPROTO_TCP:
|
||
case IPPROTO_EGP:
|
||
case IPPROTO_PUP:
|
||
case IPPROTO_UDP:
|
||
case IPPROTO_IDP:
|
||
case IPPROTO_TP:
|
||
case IPPROTO_DCCP:
|
||
case IPPROTO_IPV6:
|
||
case IPPROTO_RSVP:
|
||
case IPPROTO_GRE:
|
||
case IPPROTO_ESP:
|
||
case IPPROTO_AH:
|
||
case IPPROTO_MTP:
|
||
case IPPROTO_BEETPH:
|
||
case IPPROTO_ENCAP:
|
||
case IPPROTO_PIM:
|
||
case IPPROTO_COMP:
|
||
case IPPROTO_SCTP:
|
||
case IPPROTO_UDPLITE:
|
||
case IPPROTO_MPLS:
|
||
case IPPROTO_ETHERNET:
|
||
case IPPROTO_RAW:
|
||
case IPPROTO_MPTCP:
|
||
return 1;
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
static inline const char *ip_proto_to_str(uint16_t proto)
|
||
{
|
||
switch (proto)
|
||
{
|
||
case IPPROTO_IP:
|
||
return "IPPROTO_IP";
|
||
case IPPROTO_ICMP:
|
||
return "IPPROTO_ICMP";
|
||
case IPPROTO_IGMP:
|
||
return "IPPROTO_IGMP";
|
||
case IPPROTO_IPIP:
|
||
return "IPPROTO_IPIP";
|
||
case IPPROTO_TCP:
|
||
return "IPPROTO_TCP";
|
||
case IPPROTO_EGP:
|
||
return "IPPROTO_EGP";
|
||
case IPPROTO_PUP:
|
||
return "IPPROTO_PUP";
|
||
case IPPROTO_UDP:
|
||
return "IPPROTO_UDP";
|
||
case IPPROTO_IDP:
|
||
return "IPPROTO_IDP";
|
||
case IPPROTO_TP:
|
||
return "IPPROTO_TP";
|
||
case IPPROTO_DCCP:
|
||
return "IPPROTO_DCCP";
|
||
case IPPROTO_IPV6:
|
||
return "IPPROTO_IPV6";
|
||
case IPPROTO_RSVP:
|
||
return "IPPROTO_RSVP";
|
||
case IPPROTO_GRE:
|
||
return "IPPROTO_GRE";
|
||
case IPPROTO_ESP:
|
||
return "IPPROTO_ESP";
|
||
case IPPROTO_AH:
|
||
return "IPPROTO_AH";
|
||
case IPPROTO_MTP:
|
||
return "IPPROTO_MTP";
|
||
case IPPROTO_BEETPH:
|
||
return "IPPROTO_BEETPH";
|
||
case IPPROTO_ENCAP:
|
||
return "IPPROTO_ENCAP";
|
||
case IPPROTO_PIM:
|
||
return "IPPROTO_PIM";
|
||
case IPPROTO_COMP:
|
||
return "IPPROTO_COMP";
|
||
case IPPROTO_SCTP:
|
||
return "IPPROTO_SCTP";
|
||
case IPPROTO_UDPLITE:
|
||
return "IPPROTO_UDPLITE";
|
||
case IPPROTO_MPLS:
|
||
return "IPPROTO_MPLS";
|
||
case IPPROTO_ETHERNET:
|
||
return "IPPROTO_ETHERNET";
|
||
case IPPROTO_RAW:
|
||
return "IPPROTO_RAW";
|
||
case IPPROTO_MPTCP:
|
||
return "IPPROTO_MPTCP";
|
||
default:
|
||
return "IPPROTO_UNKNOWN";
|
||
}
|
||
}
|
||
|
||
static inline int ip4_hdr_to_str(const struct ip *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
char src_addr_str[INET6_ADDRSTRLEN] = {0};
|
||
char dst_addr_str[INET6_ADDRSTRLEN] = {0};
|
||
|
||
uint16_t proto = ip4_hdr_get_proto(hdr);
|
||
struct in_addr src_addr = ip4_hdr_get_src_in_addr(hdr);
|
||
struct in_addr dst_addr = ip4_hdr_get_dst_in_addr(hdr);
|
||
inet_ntop(AF_INET, &src_addr, src_addr_str, sizeof(src_addr_str));
|
||
inet_ntop(AF_INET, &dst_addr, dst_addr_str, sizeof(dst_addr_str));
|
||
|
||
return snprintf(buf, size, "IPv4: version=%u hdr_len=%u tos=%u total_len=%u ipid=%u flags=%u(rf=%u df=%u mf=%u) frag_offset=%u ttl=%u proto=%s checksum=0x%x src_addr=%s dst_addr=%s opt_len=%u",
|
||
ip4_hdr_get_version(hdr), ip4_hdr_get_hdr_len(hdr), ip4_hdr_get_tos(hdr),
|
||
ip4_hdr_get_total_len(hdr), ip4_hdr_get_ipid(hdr), ip4_hdr_get_flags(hdr),
|
||
ip4_hdr_get_rf_flag(hdr), ip4_hdr_get_df_flag(hdr), ip4_hdr_get_mf_flag(hdr),
|
||
ip4_hdr_get_frag_offset(hdr), ip4_hdr_get_ttl(hdr), ip_proto_to_str(proto),
|
||
ip4_hdr_get_checksum(hdr), src_addr_str, dst_addr_str, ip4_hdr_get_opt_len(hdr));
|
||
}
|
||
|
||
/******************************************************************************
|
||
* IPv6
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* IPv6 Header Format
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |Version| Traffic Class | Flow Label |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Payload Length | Next Header | Hop Limit |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | |
|
||
* + +
|
||
* | |
|
||
* + Source Address +
|
||
* | |
|
||
* + +
|
||
* | |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | |
|
||
* + +
|
||
* | |
|
||
* + Destination Address +
|
||
* | |
|
||
* + +
|
||
* | |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* Fragment Header
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Next Header | Reserved | Fragment Offset |Res|M|
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Identification |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*/
|
||
|
||
static inline uint8_t ip6_hdr_get_version(const struct ip6_hdr *hdr)
|
||
{
|
||
return (ntohl(hdr->ip6_flow) & 0xf0000000) >> 28;
|
||
}
|
||
|
||
static inline uint8_t ip6_hdr_get_traffic_class(const struct ip6_hdr *hdr)
|
||
{
|
||
return (ntohl(hdr->ip6_flow) & 0x0ff00000) >> 20;
|
||
}
|
||
|
||
static inline uint32_t ip6_hdr_get_flow_label(const struct ip6_hdr *hdr)
|
||
{
|
||
return ntohl(hdr->ip6_flow) & 0x000fffff;
|
||
}
|
||
|
||
static inline uint16_t ip6_hdr_get_payload_len(const struct ip6_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->ip6_plen);
|
||
}
|
||
|
||
static inline uint8_t ip6_hdr_get_next_header(const struct ip6_hdr *hdr)
|
||
{
|
||
return hdr->ip6_nxt;
|
||
}
|
||
|
||
static inline uint8_t ip6_hdr_get_hop_limit(const struct ip6_hdr *hdr)
|
||
{
|
||
return hdr->ip6_hlim;
|
||
}
|
||
|
||
static inline struct in6_addr ip6_hdr_get_src_in6_addr(const struct ip6_hdr *hdr)
|
||
{
|
||
return hdr->ip6_src;
|
||
}
|
||
|
||
static inline struct in6_addr ip6_hdr_get_dst_in6_addr(const struct ip6_hdr *hdr)
|
||
{
|
||
return hdr->ip6_dst;
|
||
}
|
||
|
||
static inline struct ip6_frag *ip6_hdr_get_frag_ext(const struct ip6_hdr *hdr)
|
||
{
|
||
if (hdr->ip6_nxt != IPPROTO_FRAGMENT)
|
||
{
|
||
return NULL;
|
||
}
|
||
return (struct ip6_frag *)((char *)hdr + sizeof(struct ip6_hdr));
|
||
}
|
||
|
||
static inline void ip6_hdr_set_version(struct ip6_hdr *hdr, uint8_t version)
|
||
{
|
||
hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0x0fffffff) | (version << 28));
|
||
}
|
||
|
||
static inline void ip6_hdr_set_traffic_class(struct ip6_hdr *hdr, uint8_t traffic_class)
|
||
{
|
||
hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0xf00fffff) | (traffic_class << 20));
|
||
}
|
||
|
||
static inline void ip6_hdr_set_flow_label(struct ip6_hdr *hdr, uint32_t flow_label)
|
||
{
|
||
hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0xfff00000) | flow_label);
|
||
}
|
||
|
||
static inline void ip6_hdr_set_payload_len(struct ip6_hdr *hdr, uint16_t payload_len)
|
||
{
|
||
hdr->ip6_plen = htons(payload_len);
|
||
}
|
||
|
||
static inline void ip6_hdr_set_next_header(struct ip6_hdr *hdr, uint8_t next_header)
|
||
{
|
||
hdr->ip6_nxt = next_header;
|
||
}
|
||
|
||
static inline void ip6_hdr_set_hop_limit(struct ip6_hdr *hdr, uint8_t hop_limit)
|
||
{
|
||
hdr->ip6_hlim = hop_limit;
|
||
}
|
||
|
||
static inline void ip6_hdr_set_src_in6_addr(struct ip6_hdr *hdr, struct in6_addr src_addr)
|
||
{
|
||
hdr->ip6_src = src_addr;
|
||
}
|
||
|
||
static inline void ip6_hdr_set_dst_in6_addr(struct ip6_hdr *hdr, struct in6_addr dst_addr)
|
||
{
|
||
hdr->ip6_dst = dst_addr;
|
||
}
|
||
|
||
static inline uint8_t ipv6_frag_get_next_header(const struct ip6_frag *frag)
|
||
{
|
||
return frag->ip6f_nxt;
|
||
}
|
||
|
||
static inline uint16_t ipv6_frag_get_offset(const struct ip6_frag *frag)
|
||
{
|
||
return ntohs(frag->ip6f_offlg & IP6F_OFF_MASK);
|
||
}
|
||
|
||
static inline uint32_t ipv6_frag_get_ident(const struct ip6_frag *frag)
|
||
{
|
||
return ntohl(frag->ip6f_ident);
|
||
}
|
||
|
||
static inline bool ipv6_frag_get_more(const struct ip6_frag *frag)
|
||
{
|
||
return (frag->ip6f_offlg & IP6F_MORE_FRAG);
|
||
}
|
||
|
||
static inline void ipv6_frag_set_next_header(struct ip6_frag *frag, uint8_t next_header)
|
||
{
|
||
frag->ip6f_nxt = next_header;
|
||
}
|
||
|
||
static inline void ipv6_frag_set_offset(struct ip6_frag *frag, uint16_t offset)
|
||
{
|
||
frag->ip6f_offlg = (frag->ip6f_offlg & ~IP6F_OFF_MASK) | htons(offset);
|
||
}
|
||
|
||
static inline void ipv6_frag_set_ident(struct ip6_frag *frag, uint32_t ident)
|
||
{
|
||
frag->ip6f_ident = htonl(ident);
|
||
}
|
||
|
||
static inline void ipv6_frag_set_more(struct ip6_frag *frag, bool more)
|
||
{
|
||
if (more)
|
||
{
|
||
frag->ip6f_offlg |= IP6F_MORE_FRAG;
|
||
}
|
||
else
|
||
{
|
||
frag->ip6f_offlg &= ~IP6F_MORE_FRAG;
|
||
}
|
||
}
|
||
|
||
static inline int ip6_hdr_to_str(const struct ip6_hdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
char src_addr_str[INET6_ADDRSTRLEN] = {0};
|
||
char dst_addr_str[INET6_ADDRSTRLEN] = {0};
|
||
|
||
struct in6_addr src_addr = ip6_hdr_get_src_in6_addr(hdr);
|
||
struct in6_addr dst_addr = ip6_hdr_get_dst_in6_addr(hdr);
|
||
inet_ntop(AF_INET6, &src_addr, src_addr_str, INET6_ADDRSTRLEN);
|
||
inet_ntop(AF_INET6, &dst_addr, dst_addr_str, INET6_ADDRSTRLEN);
|
||
|
||
return snprintf(buf, size, "IPv6: version=%u traffic_class=%u flow_label=%u payload_len=%u next_header=%u hop_limit=%u src_addr=%s dst_addr=%s",
|
||
ip6_hdr_get_version(hdr), ip6_hdr_get_traffic_class(hdr), ip6_hdr_get_flow_label(hdr), ip6_hdr_get_payload_len(hdr),
|
||
ip6_hdr_get_next_header(hdr), ip6_hdr_get_hop_limit(hdr), src_addr_str, dst_addr_str);
|
||
}
|
||
|
||
/******************************************************************************
|
||
* L2TP
|
||
******************************************************************************/
|
||
|
||
struct l2tp_hdr
|
||
{
|
||
uint16_t flags;
|
||
// other option fields
|
||
};
|
||
|
||
#define L2TP_CONTROL_BIT 0x8000
|
||
#define L2TP_LENGTH_BIT 0x4000
|
||
#define L2TP_SEQUENCE_BIT 0x0800
|
||
#define L2TP_OFFSET_BIT 0x0200
|
||
#define L2TP_PRIORITY_BIT 0x0100
|
||
#define L2TP_VERSION 0x000f
|
||
|
||
static inline uint8_t l2tp_hdr_get_ver(const struct l2tp_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->flags) & L2TP_VERSION;
|
||
}
|
||
|
||
// 1: control message
|
||
// 0: data message
|
||
static inline uint8_t l2tp_hdr_get_type(const struct l2tp_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->flags) & L2TP_CONTROL_BIT) >> 15;
|
||
}
|
||
|
||
/*
|
||
* Layer Two Tunneling Protocol "L2TP" (V2)
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |T|L|x|x|S|x|O|P|x|x|x|x| Ver | Length (opt) |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Tunnel ID | Session ID |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Ns (opt) | Nr (opt) |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Offset Size (opt) | Offset pad... (opt)
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* https://datatracker.ietf.org/doc/html/rfc2661
|
||
*/
|
||
|
||
static inline uint16_t calc_udp_l2tpv2_hdr_len(const char *data, uint16_t len)
|
||
{
|
||
const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data;
|
||
uint16_t flags = ntohs(hdr->flags);
|
||
|
||
// ctrl message
|
||
if (flags & L2TP_CONTROL_BIT)
|
||
{
|
||
if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0 ||
|
||
(flags & L2TP_OFFSET_BIT) != 0 || (flags & L2TP_PRIORITY_BIT) != 0)
|
||
{
|
||
return 0;
|
||
}
|
||
else
|
||
{
|
||
return ntohs(*((uint16_t *)(data + 2)));
|
||
}
|
||
}
|
||
// data message
|
||
else
|
||
{
|
||
uint16_t skip_len = 2; // skip flags field
|
||
if (flags & L2TP_LENGTH_BIT)
|
||
{
|
||
skip_len += 2; // skip length field
|
||
}
|
||
skip_len += 2; // skip tunnel id field
|
||
skip_len += 2; // skip session id field
|
||
if (flags & L2TP_SEQUENCE_BIT)
|
||
{
|
||
skip_len += 2; // skip ns field
|
||
skip_len += 2; // skip nr field
|
||
}
|
||
if (flags & L2TP_OFFSET_BIT)
|
||
{
|
||
if (skip_len + 2 > len)
|
||
{
|
||
return 0;
|
||
}
|
||
uint16_t offset = ntohs(*((uint16_t *)(data + skip_len)));
|
||
if (offset == 0)
|
||
{
|
||
skip_len += 2; // skip offset field
|
||
}
|
||
else
|
||
{
|
||
skip_len = offset;
|
||
}
|
||
}
|
||
return skip_len;
|
||
}
|
||
}
|
||
|
||
/*
|
||
* Figure 4.1.1.1: L2TPv3 Session Header Over IP
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Session ID |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Cookie (optional, maximum 64 bits)...
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* Figure 4.1.1.2: L2TPv3 Control Message Header Over IP
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | (32 bits of zeros) |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |T|L|x|x|S|x|x|x|x|x|x|x| Ver | Length |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Control Connection ID |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Ns | Nr |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* Note: Unlike L2TP over UDP, which uses the T bit to distinguish between
|
||
* L2TP control and data packets, L2TP over IP uses the reserved Session
|
||
* ID of zero (0) when sending control messages.
|
||
*
|
||
* https://www.rfc-editor.org/rfc/rfc3931.html
|
||
*/
|
||
|
||
static inline uint16_t calc_ip_l2tpv3_hdr_len(const char *data, uint16_t len)
|
||
{
|
||
if (len < 4)
|
||
{
|
||
return 0;
|
||
}
|
||
uint32_t session_id = ntohl(*((uint32_t *)data));
|
||
// data message
|
||
if (session_id)
|
||
{
|
||
// TODO The optional Cookie field contains a variable-length value (maximum 64 bits)
|
||
// TODO L2-Specific Sublayer 4 bytes
|
||
return 4 + 4;
|
||
}
|
||
// control message
|
||
else
|
||
{
|
||
if (len < 16)
|
||
{
|
||
return 0;
|
||
}
|
||
uint16_t flags = ntohs(*((uint16_t *)(data + 4)));
|
||
if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0)
|
||
{
|
||
return 0;
|
||
}
|
||
else
|
||
{
|
||
return ntohs(*((uint16_t *)(data + 4 + 2)));
|
||
}
|
||
}
|
||
}
|
||
|
||
/*
|
||
* Layer Two Tunneling Protocol - Version 3 (L2TPv3)
|
||
*
|
||
* Figure 3.2.1: L2TP Control Message Header
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |T|L|x|x|S|x|x|x|x|x|x|x| Ver | Length |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Control Connection ID |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Ns | Nr |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* Figure 4.1.2.1: L2TPv3 Session Header over UDP
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |T|x|x|x|x|x|x|x|x|x|x|x| Ver | Reserved |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Session ID |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Cookie (optional, maximum 64 bits)...
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*
|
||
* https://www.rfc-editor.org/rfc/rfc3931.html
|
||
*/
|
||
|
||
static inline uint16_t calc_udp_l2tpv3_hdr_len(const char *data, uint16_t len)
|
||
{
|
||
if (len < 8)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data;
|
||
uint16_t flags = ntohs(hdr->flags);
|
||
|
||
// ctrl message
|
||
if (flags & L2TP_CONTROL_BIT)
|
||
{
|
||
if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0)
|
||
{
|
||
return 0;
|
||
}
|
||
else
|
||
{
|
||
return ntohs(*((uint16_t *)(data + 2)));
|
||
}
|
||
}
|
||
// data message
|
||
else
|
||
{
|
||
// TODO The optional Cookie field contains a variable-length value (maximum 64 bits)
|
||
// TODO L2-Specific Sublayer 4 bytes
|
||
return 8 + 4;
|
||
}
|
||
}
|
||
|
||
static inline int l2tp_hdr_to_str(const struct l2tp_hdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
return snprintf(buf, size, "L2TP: type=%s version=%u",
|
||
l2tp_hdr_get_type(hdr) ? "control" : "data", l2tp_hdr_get_ver(hdr));
|
||
}
|
||
|
||
/******************************************************************************
|
||
* MPLS
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* Reference: RFC 5462, RFC 3032
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Label | TC |S| TTL |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*/
|
||
|
||
static inline uint32_t mpls_label_get_label(const struct mpls_label *hdr)
|
||
{
|
||
return ((ntohl(hdr->entry) & MPLS_LS_LABEL_MASK) >> MPLS_LS_LABEL_SHIFT);
|
||
}
|
||
|
||
static inline uint8_t mpls_label_get_tc(const struct mpls_label *hdr)
|
||
{
|
||
return ((ntohl(hdr->entry) & MPLS_LS_TC_MASK) >> MPLS_LS_TC_SHIFT);
|
||
}
|
||
|
||
static inline uint8_t mpls_label_get_bos(const struct mpls_label *hdr)
|
||
{
|
||
return ((ntohl(hdr->entry) & MPLS_LS_S_MASK) >> MPLS_LS_S_SHIFT);
|
||
}
|
||
|
||
static inline uint8_t mpls_label_get_ttl(const struct mpls_label *hdr)
|
||
{
|
||
return ((ntohl(hdr->entry) & MPLS_LS_TTL_MASK) >> MPLS_LS_TTL_SHIFT);
|
||
}
|
||
|
||
static inline void mpls_label_set_label(struct mpls_label *hdr, uint32_t label)
|
||
{
|
||
hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_LABEL_MASK) | (label << MPLS_LS_LABEL_SHIFT));
|
||
}
|
||
|
||
static inline void mpls_label_set_tc(struct mpls_label *hdr, uint8_t tc)
|
||
{
|
||
hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_TC_MASK) | (tc << MPLS_LS_TC_SHIFT));
|
||
}
|
||
|
||
static inline void mpls_label_set_bos(struct mpls_label *hdr, uint8_t bos)
|
||
{
|
||
hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_S_MASK) | (bos << MPLS_LS_S_SHIFT));
|
||
}
|
||
|
||
static inline void mpls_label_set_ttl(struct mpls_label *hdr, uint8_t ttl)
|
||
{
|
||
hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_TTL_MASK) | (ttl << MPLS_LS_TTL_SHIFT));
|
||
}
|
||
|
||
static inline int mpls_label_to_str(const struct mpls_label *hdr, char *buf, size_t size)
|
||
{
|
||
return snprintf(buf, size, "MPLS: label=%u tc=%u bos=%u ttl=%u",
|
||
mpls_label_get_label(hdr), mpls_label_get_tc(hdr),
|
||
mpls_label_get_bos(hdr), mpls_label_get_ttl(hdr));
|
||
}
|
||
|
||
/******************************************************************************
|
||
* TCP
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* TCP Header Format
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Source Port | Destination Port |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Sequence Number |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Acknowledgment Number |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Data | |U|A|P|R|S|F| |
|
||
* | Offset| Reserved |R|C|S|S|Y|I| Window |
|
||
* | | |G|K|H|T|N|N| |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Checksum | Urgent Pointer |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Options | Padding |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | data |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*/
|
||
|
||
static inline uint16_t tcp_hdr_get_src_port(const struct tcphdr *hdr)
|
||
{
|
||
return ntohs(hdr->th_sport);
|
||
}
|
||
|
||
static inline uint16_t tcp_hdr_get_dst_port(const struct tcphdr *hdr)
|
||
{
|
||
return ntohs(hdr->th_dport);
|
||
}
|
||
|
||
static inline uint32_t tcp_hdr_get_seq(const struct tcphdr *hdr)
|
||
{
|
||
return ntohl(hdr->th_seq);
|
||
}
|
||
|
||
static inline uint32_t tcp_hdr_get_ack(const struct tcphdr *hdr)
|
||
{
|
||
return ntohl(hdr->th_ack);
|
||
}
|
||
|
||
static inline uint8_t tcp_hdr_get_hdr_len(const struct tcphdr *hdr)
|
||
{
|
||
return hdr->th_off << 2;
|
||
}
|
||
|
||
static inline uint8_t tcp_hdr_get_flags(const struct tcphdr *hdr)
|
||
{
|
||
return hdr->th_flags;
|
||
}
|
||
|
||
static inline bool tcp_hdr_get_urg_flag(const struct tcphdr *hdr)
|
||
{
|
||
return hdr->th_flags & TH_URG;
|
||
}
|
||
|
||
static inline bool tcp_hdr_get_ack_flag(const struct tcphdr *hdr)
|
||
{
|
||
return hdr->th_flags & TH_ACK;
|
||
}
|
||
|
||
static inline bool tcp_hdr_get_push_flag(const struct tcphdr *hdr)
|
||
{
|
||
return hdr->th_flags & TH_PUSH;
|
||
}
|
||
|
||
static inline bool tcp_hdr_get_rst_flag(const struct tcphdr *hdr)
|
||
{
|
||
return hdr->th_flags & TH_RST;
|
||
}
|
||
|
||
static inline bool tcp_hdr_get_syn_flag(const struct tcphdr *hdr)
|
||
{
|
||
return hdr->th_flags & TH_SYN;
|
||
}
|
||
|
||
static inline bool tcp_hdr_get_fin_flag(const struct tcphdr *hdr)
|
||
{
|
||
return hdr->th_flags & TH_FIN;
|
||
}
|
||
|
||
static inline uint16_t tcp_hdr_get_window(const struct tcphdr *hdr)
|
||
{
|
||
return ntohs(hdr->th_win);
|
||
}
|
||
|
||
static inline uint16_t tcp_hdr_get_checksum(const struct tcphdr *hdr)
|
||
{
|
||
return ntohs(hdr->th_sum);
|
||
}
|
||
|
||
static inline uint16_t tcp_hdr_get_urg_ptr(const struct tcphdr *hdr)
|
||
{
|
||
return ntohs(hdr->th_urp);
|
||
}
|
||
|
||
static inline uint16_t tcp_hdr_get_opt_len(const struct tcphdr *hdr)
|
||
{
|
||
return tcp_hdr_get_hdr_len(hdr) - sizeof(struct tcphdr);
|
||
}
|
||
|
||
static inline const char *tcp_hdr_get_opt_data(const struct tcphdr *hdr)
|
||
{
|
||
if (tcp_hdr_get_opt_len(hdr) == 0)
|
||
{
|
||
return NULL;
|
||
}
|
||
|
||
return ((const char *)hdr) + sizeof(struct tcphdr);
|
||
}
|
||
|
||
static inline void tcp_hdr_set_src_port(struct tcphdr *hdr, uint16_t port)
|
||
{
|
||
hdr->th_sport = htons(port);
|
||
}
|
||
|
||
static inline void tcp_hdr_set_dst_port(struct tcphdr *hdr, uint16_t port)
|
||
{
|
||
hdr->th_dport = htons(port);
|
||
}
|
||
|
||
static inline void tcp_hdr_set_seq(struct tcphdr *hdr, uint32_t seq)
|
||
{
|
||
hdr->th_seq = htonl(seq);
|
||
}
|
||
|
||
static inline void tcp_hdr_set_ack(struct tcphdr *hdr, uint32_t ack)
|
||
{
|
||
hdr->th_ack = htonl(ack);
|
||
}
|
||
|
||
static inline void tcp_hdr_set_hdr_len(struct tcphdr *hdr, uint8_t offset)
|
||
{
|
||
hdr->th_off = offset >> 2;
|
||
}
|
||
|
||
static inline void tcp_hdr_set_flags(struct tcphdr *hdr, uint8_t flags)
|
||
{
|
||
hdr->th_flags = flags;
|
||
}
|
||
|
||
static inline void tcp_hdr_set_urg_flag(struct tcphdr *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->th_flags |= TH_URG;
|
||
}
|
||
else
|
||
{
|
||
hdr->th_flags &= ~TH_URG;
|
||
}
|
||
}
|
||
|
||
static inline void tcp_hdr_set_ack_flag(struct tcphdr *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->th_flags |= TH_ACK;
|
||
}
|
||
else
|
||
{
|
||
hdr->th_flags &= ~TH_ACK;
|
||
}
|
||
}
|
||
|
||
static inline void tcp_hdr_set_push_flag(struct tcphdr *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->th_flags |= TH_PUSH;
|
||
}
|
||
else
|
||
{
|
||
hdr->th_flags &= ~TH_PUSH;
|
||
}
|
||
}
|
||
|
||
static inline void tcp_hdr_set_rst_flag(struct tcphdr *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->th_flags |= TH_RST;
|
||
}
|
||
else
|
||
{
|
||
hdr->th_flags &= ~TH_RST;
|
||
}
|
||
}
|
||
|
||
static inline void tcp_hdr_set_syn_flag(struct tcphdr *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->th_flags |= TH_SYN;
|
||
}
|
||
else
|
||
{
|
||
hdr->th_flags &= ~TH_SYN;
|
||
}
|
||
}
|
||
|
||
static inline void tcp_hdr_set_fin_flag(struct tcphdr *hdr, bool flag)
|
||
{
|
||
if (flag)
|
||
{
|
||
hdr->th_flags |= TH_FIN;
|
||
}
|
||
else
|
||
{
|
||
hdr->th_flags &= ~TH_FIN;
|
||
}
|
||
}
|
||
|
||
static inline void tcp_hdr_set_window(struct tcphdr *hdr, uint16_t window)
|
||
{
|
||
hdr->th_win = htons(window);
|
||
}
|
||
|
||
static inline void tcp_hdr_set_checksum(struct tcphdr *hdr, uint16_t checksum)
|
||
{
|
||
hdr->th_sum = htons(checksum);
|
||
}
|
||
|
||
static inline void tcp_hdr_set_urg_ptr(struct tcphdr *hdr, uint16_t ptr)
|
||
{
|
||
hdr->th_urp = htons(ptr);
|
||
}
|
||
|
||
static inline void tcp_hdr_set_opt_len(struct tcphdr *hdr, uint16_t len)
|
||
{
|
||
hdr->th_off = (sizeof(struct tcphdr) + len) >> 2;
|
||
}
|
||
|
||
// must be called after tcp_hdr_set_opt_len
|
||
static inline void tcp_hdr_set_opt_data(struct tcphdr *hdr, const char *ptr)
|
||
{
|
||
memcpy((char *)hdr + sizeof(struct tcphdr), ptr, tcp_hdr_get_opt_len(hdr));
|
||
}
|
||
|
||
static inline int tcp_hdr_to_str(const struct tcphdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
return snprintf(buf, size, "TCP: src_port=%u dst_port=%u seq=%u ack=%u hdr_len=%u flags=0x%02x(%s%s%s%s%s%s) window=%u checksum=0x%x urg_ptr=%u opt_len=%u",
|
||
tcp_hdr_get_src_port(hdr), tcp_hdr_get_dst_port(hdr),
|
||
tcp_hdr_get_seq(hdr), tcp_hdr_get_ack(hdr),
|
||
tcp_hdr_get_hdr_len(hdr), tcp_hdr_get_flags(hdr),
|
||
tcp_hdr_get_urg_flag(hdr) ? "URG " : "",
|
||
tcp_hdr_get_ack_flag(hdr) ? "ACK " : "",
|
||
tcp_hdr_get_push_flag(hdr) ? "PSH " : "",
|
||
tcp_hdr_get_rst_flag(hdr) ? "RST " : "",
|
||
tcp_hdr_get_syn_flag(hdr) ? "SYN " : "",
|
||
tcp_hdr_get_fin_flag(hdr) ? "FIN " : "",
|
||
tcp_hdr_get_window(hdr), tcp_hdr_get_checksum(hdr),
|
||
tcp_hdr_get_urg_ptr(hdr), tcp_hdr_get_opt_len(hdr));
|
||
}
|
||
|
||
/******************************************************************************
|
||
* UDP
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* User Datagram Header Format
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Source Port | Destination Port |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Length | Checksum |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Data |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*/
|
||
|
||
static inline uint16_t udp_hdr_get_src_port(const struct udphdr *hdr)
|
||
{
|
||
return ntohs(hdr->uh_sport);
|
||
}
|
||
|
||
static inline uint16_t udp_hdr_get_dst_port(const struct udphdr *hdr)
|
||
{
|
||
return ntohs(hdr->uh_dport);
|
||
}
|
||
|
||
static inline uint16_t udp_hdr_get_total_len(const struct udphdr *hdr)
|
||
{
|
||
return ntohs(hdr->uh_ulen);
|
||
}
|
||
|
||
static inline uint16_t udp_hdr_get_checksum(const struct udphdr *hdr)
|
||
{
|
||
return ntohs(hdr->uh_sum);
|
||
}
|
||
|
||
static inline void udp_hdr_set_src_port(struct udphdr *hdr, uint16_t port)
|
||
{
|
||
hdr->uh_sport = htons(port);
|
||
}
|
||
|
||
static inline void udp_hdr_set_dst_port(struct udphdr *hdr, uint16_t port)
|
||
{
|
||
hdr->uh_dport = htons(port);
|
||
}
|
||
|
||
static inline void udp_hdr_set_total_len(struct udphdr *hdr, uint16_t len)
|
||
{
|
||
hdr->uh_ulen = htons(len);
|
||
}
|
||
|
||
static inline void udp_hdr_set_checksum(struct udphdr *hdr, uint16_t sum)
|
||
{
|
||
hdr->uh_sum = htons(sum);
|
||
}
|
||
|
||
static inline int udp_hdr_to_str(const struct udphdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
return snprintf(buf, size, "UDP: src_port=%u dst_port=%u total_len=%u checksum=0x%x",
|
||
udp_hdr_get_src_port(hdr), udp_hdr_get_dst_port(hdr),
|
||
udp_hdr_get_total_len(hdr), udp_hdr_get_checksum(hdr));
|
||
}
|
||
|
||
/******************************************************************************
|
||
* VLAN
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* VLAN Header Format
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | Pri |I| VLAN ID | Ethertype |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*/
|
||
|
||
struct vlan_hdr
|
||
{
|
||
uint16_t vlan_tci;
|
||
uint16_t vlan_ethertype;
|
||
};
|
||
|
||
static inline uint8_t vlan_hdr_get_priority(const struct vlan_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->vlan_tci) & 0xe000) >> 13;
|
||
}
|
||
|
||
static inline uint8_t vlan_hdr_get_dei(const struct vlan_hdr *hdr)
|
||
{
|
||
return (ntohs(hdr->vlan_tci) & 0x1000) >> 12;
|
||
}
|
||
|
||
static inline uint16_t vlan_hdr_get_vid(const struct vlan_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->vlan_tci) & 0x0fff;
|
||
}
|
||
|
||
static inline uint16_t vlan_hdr_get_ethertype(const struct vlan_hdr *hdr)
|
||
{
|
||
return ntohs(hdr->vlan_ethertype);
|
||
}
|
||
|
||
static inline void vlan_hdr_set_priority(struct vlan_hdr *hdr, uint8_t priority)
|
||
{
|
||
hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0xe000) | (priority << 13));
|
||
}
|
||
|
||
static inline void vlan_hdr_set_dei(struct vlan_hdr *hdr, uint8_t dei)
|
||
{
|
||
hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x1000) | (dei << 12));
|
||
}
|
||
|
||
static inline void vlan_hdr_set_vid(struct vlan_hdr *hdr, uint16_t vid)
|
||
{
|
||
hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x0fff) | vid);
|
||
}
|
||
|
||
static inline void vlan_hdr_set_ethertype(struct vlan_hdr *hdr, uint16_t ethertype)
|
||
{
|
||
hdr->vlan_ethertype = htons(ethertype);
|
||
}
|
||
|
||
static inline int vlan_hdr_to_str(const struct vlan_hdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
uint16_t proto = vlan_hdr_get_ethertype(hdr);
|
||
return snprintf(buf, size, "VLAN: priority=%u dei=%u id=%u ethertype=%s",
|
||
vlan_hdr_get_priority(hdr), vlan_hdr_get_dei(hdr),
|
||
vlan_hdr_get_vid(hdr), eth_proto_to_str(proto));
|
||
}
|
||
|
||
/******************************************************************************
|
||
* VXLAN
|
||
******************************************************************************/
|
||
|
||
/*
|
||
* VXLAN Header
|
||
*
|
||
* 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
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* |R|R|R|R|I|R|R|R| Reserved |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
* | VXLAN Network Identifier (VNI) | Reserved |
|
||
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||
*/
|
||
|
||
struct vxlan_hdr
|
||
{
|
||
uint32_t vxlan_flags;
|
||
uint32_t vxlan_vni;
|
||
};
|
||
|
||
static inline uint8_t vxlan_hdr_get_flags(const struct vxlan_hdr *hdr)
|
||
{
|
||
return (ntohl(hdr->vxlan_flags) & 0xff000000) >> 24;
|
||
}
|
||
|
||
static inline uint32_t vxlan_hdr_get_vni(const struct vxlan_hdr *hdr)
|
||
{
|
||
return (ntohl(hdr->vxlan_vni) & 0xffffff00) >> 8;
|
||
}
|
||
|
||
static inline void vxlan_hdr_set_flags(struct vxlan_hdr *hdr, uint8_t flags)
|
||
{
|
||
hdr->vxlan_flags = htonl((ntohl(hdr->vxlan_flags) & ~0xff000000) | (flags << 24));
|
||
}
|
||
|
||
static inline void vxlan_hdr_set_vni(struct vxlan_hdr *hdr, uint32_t vni)
|
||
{
|
||
hdr->vxlan_vni = htonl((ntohl(hdr->vxlan_vni) & ~0xffffff00) | (vni << 8));
|
||
}
|
||
|
||
static inline int vxlan_hdr_to_str(const struct vxlan_hdr *hdr, char *buf, size_t size)
|
||
{
|
||
memset(buf, 0, size);
|
||
return snprintf(buf, size, "VXLAN: flags=%u vni=%u",
|
||
vxlan_hdr_get_flags(hdr), vxlan_hdr_get_vni(hdr));
|
||
}
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|