262 lines
8.4 KiB
C
262 lines
8.4 KiB
C
#pragma once
|
|
|
|
#ifdef __cplusplus
|
|
extern "C"
|
|
{
|
|
#endif
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <arpa/inet.h>
|
|
|
|
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
|
|
|
|
/******************************************************************************
|
|
* get
|
|
******************************************************************************/
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
/******************************************************************************
|
|
* set
|
|
******************************************************************************/
|
|
|
|
// TODO
|
|
|
|
/******************************************************************************
|
|
* print
|
|
******************************************************************************/
|
|
|
|
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));
|
|
}
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|