[linux-2.6-block.git] / tools / testing / selftests / bpf / progs / test_seg6_loop.c

#include <stddef.h>
#include <inttypes.h>
#include <errno.h>
#include <linux/seg6_local.h>
#include <linux/bpf.h>
#include "bpf_helpers.h"
#include "bpf_endian.h"

/* Packet parsing state machine helpers. */
#define cursor_advance(_cursor, _len) \
	({ void *_tmp = _cursor; _cursor += _len; _tmp; })

#define SR6_FLAG_ALERT (1 << 4)

#define BPF_PACKET_HEADER __attribute__((packed))

struct ip6_t {
	unsigned int ver:4;
	unsigned int priority:8;
	unsigned int flow_label:20;
	unsigned short payload_len;
	unsigned char next_header;
	unsigned char hop_limit;
	unsigned long long src_hi;
	unsigned long long src_lo;
	unsigned long long dst_hi;
	unsigned long long dst_lo;
} BPF_PACKET_HEADER;

struct ip6_addr_t {
	unsigned long long hi;
	unsigned long long lo;
} BPF_PACKET_HEADER;

struct ip6_srh_t {
	unsigned char nexthdr;
	unsigned char hdrlen;
	unsigned char type;
	unsigned char segments_left;
	unsigned char first_segment;
	unsigned char flags;
	unsigned short tag;

	struct ip6_addr_t segments[0];
} BPF_PACKET_HEADER;

struct sr6_tlv_t {
	unsigned char type;
	unsigned char len;
	unsigned char value[0];
} BPF_PACKET_HEADER;

static __always_inline struct ip6_srh_t *get_srh(struct __sk_buff *skb)
{
	void *cursor, *data_end;
	struct ip6_srh_t *srh;
	struct ip6_t *ip;
	uint8_t *ipver;

	data_end = (void *)(long)skb->data_end;
	cursor = (void *)(long)skb->data;
	ipver = (uint8_t *)cursor;

	if ((void *)ipver + sizeof(*ipver) > data_end)
		return NULL;

	if ((*ipver >> 4) != 6)
		return NULL;

	ip = cursor_advance(cursor, sizeof(*ip));
	if ((void *)ip + sizeof(*ip) > data_end)
		return NULL;

	if (ip->next_header != 43)
		return NULL;

	srh = cursor_advance(cursor, sizeof(*srh));
	if ((void *)srh + sizeof(*srh) > data_end)
		return NULL;

	if (srh->type != 4)
		return NULL;

	return srh;
}

static __always_inline int update_tlv_pad(struct __sk_buff *skb,
					  uint32_t new_pad, uint32_t old_pad,
					  uint32_t pad_off)
{
	int err;

	if (new_pad != old_pad) {
		err = bpf_lwt_seg6_adjust_srh(skb, pad_off,
					  (int) new_pad - (int) old_pad);
		if (err)
			return err;
	}

	if (new_pad > 0) {
		char pad_tlv_buf[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
					0, 0, 0};
		struct sr6_tlv_t *pad_tlv = (struct sr6_tlv_t *) pad_tlv_buf;

		pad_tlv->type = SR6_TLV_PADDING;
		pad_tlv->len = new_pad - 2;

		err = bpf_lwt_seg6_store_bytes(skb, pad_off,
					       (void *)pad_tlv_buf, new_pad);
		if (err)
			return err;
	}

	return 0;
}

static __always_inline int is_valid_tlv_boundary(struct __sk_buff *skb,
						 struct ip6_srh_t *srh,
						 uint32_t *tlv_off,
						 uint32_t *pad_size,
						 uint32_t *pad_off)
{
	uint32_t srh_off, cur_off;
	int offset_valid = 0;
	int err;

	srh_off = (char *)srh - (char *)(long)skb->data;
	// cur_off = end of segments, start of possible TLVs
	cur_off = srh_off + sizeof(*srh) +
		sizeof(struct ip6_addr_t) * (srh->first_segment + 1);

	*pad_off = 0;

	// we can only go as far as ~10 TLVs due to the BPF max stack size
	// workaround: define induction variable "i" as "long" instead
	// of "int" to prevent alu32 sub-register spilling.
	#pragma clang loop unroll(disable)
	for (long i = 0; i < 100; i++) {
		struct sr6_tlv_t tlv;

		if (cur_off == *tlv_off)
			offset_valid = 1;

		if (cur_off >= srh_off + ((srh->hdrlen + 1) << 3))
			break;

		err = bpf_skb_load_bytes(skb, cur_off, &tlv, sizeof(tlv));
		if (err)
			return err;

		if (tlv.type == SR6_TLV_PADDING) {
			*pad_size = tlv.len + sizeof(tlv);
			*pad_off = cur_off;

			if (*tlv_off == srh_off) {
				*tlv_off = cur_off;
				offset_valid = 1;
			}
			break;

		} else if (tlv.type == SR6_TLV_HMAC) {
			break;
		}

		cur_off += sizeof(tlv) + tlv.len;
	} // we reached the padding or HMAC TLVs, or the end of the SRH

	if (*pad_off == 0)
		*pad_off = cur_off;

	if (*tlv_off == -1)
		*tlv_off = cur_off;
	else if (!offset_valid)
		return -EINVAL;

	return 0;
}

static __always_inline int add_tlv(struct __sk_buff *skb,
				   struct ip6_srh_t *srh, uint32_t tlv_off,
				   struct sr6_tlv_t *itlv, uint8_t tlv_size)
{
	uint32_t srh_off = (char *)srh - (char *)(long)skb->data;
	uint8_t len_remaining, new_pad;
	uint32_t pad_off = 0;
	uint32_t pad_size = 0;
	uint32_t partial_srh_len;
	int err;

	if (tlv_off != -1)
		tlv_off += srh_off;

	if (itlv->type == SR6_TLV_PADDING || itlv->type == SR6_TLV_HMAC)
		return -EINVAL;

	err = is_valid_tlv_boundary(skb, srh, &tlv_off, &pad_size, &pad_off);
	if (err)
		return err;

	err = bpf_lwt_seg6_adjust_srh(skb, tlv_off, sizeof(*itlv) + itlv->len);
	if (err)
		return err;

	err = bpf_lwt_seg6_store_bytes(skb, tlv_off, (void *)itlv, tlv_size);
	if (err)
		return err;

	// the following can't be moved inside update_tlv_pad because the
	// bpf verifier has some issues with it
	pad_off += sizeof(*itlv) + itlv->len;
	partial_srh_len = pad_off - srh_off;
	len_remaining = partial_srh_len % 8;
	new_pad = 8 - len_remaining;

	if (new_pad == 1) // cannot pad for 1 byte only
		new_pad = 9;
	else if (new_pad == 8)
		new_pad = 0;

	return update_tlv_pad(skb, new_pad, pad_size, pad_off);
}

// Add an Egress TLV fc00::4, add the flag A,
// and apply End.X action to fc42::1
SEC("lwt_seg6local")
int __add_egr_x(struct __sk_buff *skb)
{
	unsigned long long hi = 0xfc42000000000000;
	unsigned long long lo = 0x1;
	struct ip6_srh_t *srh = get_srh(skb);
	uint8_t new_flags = SR6_FLAG_ALERT;
	struct ip6_addr_t addr;
	int err, offset;

	if (srh == NULL)
		return BPF_DROP;

	uint8_t tlv[20] = {2, 18, 0, 0, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
			   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4};

	err = add_tlv(skb, srh, (srh->hdrlen+1) << 3,
		      (struct sr6_tlv_t *)&tlv, 20);
	if (err)
		return BPF_DROP;

	offset = sizeof(struct ip6_t) + offsetof(struct ip6_srh_t, flags);
	err = bpf_lwt_seg6_store_bytes(skb, offset,
				       (void *)&new_flags, sizeof(new_flags));
	if (err)
		return BPF_DROP;

	addr.lo = bpf_cpu_to_be64(lo);
	addr.hi = bpf_cpu_to_be64(hi);
	err = bpf_lwt_seg6_action(skb, SEG6_LOCAL_ACTION_END_X,
				  (void *)&addr, sizeof(addr));
	if (err)
		return BPF_DROP;
	return BPF_REDIRECT;
}
char __license[] SEC("license") = "GPL";
Commit	Line	Data
b061017f AS	1	#include <stddef.h>
	2	#include <inttypes.h>
	3	#include <errno.h>
	4	#include <linux/seg6_local.h>
	5	#include <linux/bpf.h>
	6	#include "bpf_helpers.h"
	7	#include "bpf_endian.h"
	8
	9	/* Packet parsing state machine helpers. */
	10	#define cursor_advance(_cursor, _len) \
	11	({ void *_tmp = _cursor; _cursor += _len; _tmp; })
	12
	13	#define SR6_FLAG_ALERT (1 << 4)
	14
b061017f AS	15	#define BPF_PACKET_HEADER __attribute__((packed))
	16
	17	struct ip6_t {
	18	unsigned int ver:4;
	19	unsigned int priority:8;
	20	unsigned int flow_label:20;
	21	unsigned short payload_len;
	22	unsigned char next_header;
	23	unsigned char hop_limit;
	24	unsigned long long src_hi;
	25	unsigned long long src_lo;
	26	unsigned long long dst_hi;
	27	unsigned long long dst_lo;
	28	} BPF_PACKET_HEADER;
	29
	30	struct ip6_addr_t {
	31	unsigned long long hi;
	32	unsigned long long lo;
	33	} BPF_PACKET_HEADER;
	34
	35	struct ip6_srh_t {
	36	unsigned char nexthdr;
	37	unsigned char hdrlen;
	38	unsigned char type;
	39	unsigned char segments_left;
	40	unsigned char first_segment;
	41	unsigned char flags;
	42	unsigned short tag;
	43
	44	struct ip6_addr_t segments[0];
	45	} BPF_PACKET_HEADER;
	46
	47	struct sr6_tlv_t {
	48	unsigned char type;
	49	unsigned char len;
	50	unsigned char value[0];
	51	} BPF_PACKET_HEADER;
	52
d2f5bbbc	53	static __always_inline struct ip6_srh_t get_srh(struct __sk_buff skb)
b061017f AS	54	{
	55	void cursor, data_end;
	56	struct ip6_srh_t *srh;
	57	struct ip6_t *ip;
	58	uint8_t *ipver;
	59
	60	data_end = (void *)(long)skb->data_end;
	61	cursor = (void *)(long)skb->data;
	62	ipver = (uint8_t *)cursor;
	63
	64	if ((void )ipver + sizeof(ipver) > data_end)
	65	return NULL;
	66
	67	if ((*ipver >> 4) != 6)
	68	return NULL;
	69
	70	ip = cursor_advance(cursor, sizeof(*ip));
	71	if ((void )ip + sizeof(ip) > data_end)
	72	return NULL;
	73
	74	if (ip->next_header != 43)
	75	return NULL;
	76
	77	srh = cursor_advance(cursor, sizeof(*srh));
	78	if ((void )srh + sizeof(srh) > data_end)
	79	return NULL;
	80
	81	if (srh->type != 4)
	82	return NULL;
	83
	84	return srh;
	85	}
	86
d2f5bbbc JB	87	static __always_inline int update_tlv_pad(struct __sk_buff *skb,
	88	uint32_t new_pad, uint32_t old_pad,
	89	uint32_t pad_off)
b061017f AS	90	{
	91	int err;
	92
	93	if (new_pad != old_pad) {
	94	err = bpf_lwt_seg6_adjust_srh(skb, pad_off,
	95	(int) new_pad - (int) old_pad);
	96	if (err)
	97	return err;
	98	}
	99
	100	if (new_pad > 0) {
	101	char pad_tlv_buf[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
	102	0, 0, 0};
	103	struct sr6_tlv_t pad_tlv = (struct sr6_tlv_t ) pad_tlv_buf;
	104
	105	pad_tlv->type = SR6_TLV_PADDING;
	106	pad_tlv->len = new_pad - 2;
	107
	108	err = bpf_lwt_seg6_store_bytes(skb, pad_off,
	109	(void *)pad_tlv_buf, new_pad);
	110	if (err)
	111	return err;
	112	}
	113
	114	return 0;
	115	}
	116
d2f5bbbc JB	117	static __always_inline int is_valid_tlv_boundary(struct __sk_buff *skb,
	118	struct ip6_srh_t *srh,
	119	uint32_t *tlv_off,
	120	uint32_t *pad_size,
	121	uint32_t *pad_off)
b061017f AS	122	{
	123	uint32_t srh_off, cur_off;
	124	int offset_valid = 0;
	125	int err;
	126
	127	srh_off = (char )srh - (char )(long)skb->data;
	128	// cur_off = end of segments, start of possible TLVs
	129	cur_off = srh_off + sizeof(*srh) +
	130	sizeof(struct ip6_addr_t) * (srh->first_segment + 1);
	131
	132	*pad_off = 0;
	133
	134	// we can only go as far as ~10 TLVs due to the BPF max stack size
2ea2612b YS	135	// workaround: define induction variable "i" as "long" instead
2ea2612b YS	136	// of "int" to prevent alu32 sub-register spilling.
b061017f	137	#pragma clang loop unroll(disable)
2ea2612b	138	for (long i = 0; i < 100; i++) {
b061017f AS	139	struct sr6_tlv_t tlv;
	140
	141	if (cur_off == *tlv_off)
	142	offset_valid = 1;
	143
	144	if (cur_off >= srh_off + ((srh->hdrlen + 1) << 3))
	145	break;
	146
	147	err = bpf_skb_load_bytes(skb, cur_off, &tlv, sizeof(tlv));
	148	if (err)
	149	return err;
	150
	151	if (tlv.type == SR6_TLV_PADDING) {
	152	*pad_size = tlv.len + sizeof(tlv);
	153	*pad_off = cur_off;
	154
	155	if (*tlv_off == srh_off) {
	156	*tlv_off = cur_off;
	157	offset_valid = 1;
	158	}
	159	break;
	160
	161	} else if (tlv.type == SR6_TLV_HMAC) {
	162	break;
	163	}
	164
	165	cur_off += sizeof(tlv) + tlv.len;
	166	} // we reached the padding or HMAC TLVs, or the end of the SRH
	167
	168	if (*pad_off == 0)
	169	*pad_off = cur_off;
	170
	171	if (*tlv_off == -1)
	172	*tlv_off = cur_off;
	173	else if (!offset_valid)
	174	return -EINVAL;
	175
	176	return 0;
	177	}
	178
d2f5bbbc JB	179	static __always_inline int add_tlv(struct __sk_buff *skb,
	180	struct ip6_srh_t *srh, uint32_t tlv_off,
	181	struct sr6_tlv_t *itlv, uint8_t tlv_size)
b061017f AS	182	{
	183	uint32_t srh_off = (char )srh - (char )(long)skb->data;
	184	uint8_t len_remaining, new_pad;
	185	uint32_t pad_off = 0;
	186	uint32_t pad_size = 0;
	187	uint32_t partial_srh_len;
	188	int err;
	189
	190	if (tlv_off != -1)
	191	tlv_off += srh_off;
	192
	193	if (itlv->type == SR6_TLV_PADDING \|\| itlv->type == SR6_TLV_HMAC)
	194	return -EINVAL;
	195
	196	err = is_valid_tlv_boundary(skb, srh, &tlv_off, &pad_size, &pad_off);
	197	if (err)
	198	return err;
	199
	200	err = bpf_lwt_seg6_adjust_srh(skb, tlv_off, sizeof(*itlv) + itlv->len);
	201	if (err)
	202	return err;
	203
	204	err = bpf_lwt_seg6_store_bytes(skb, tlv_off, (void *)itlv, tlv_size);
	205	if (err)
	206	return err;
	207
	208	// the following can't be moved inside update_tlv_pad because the
	209	// bpf verifier has some issues with it
	210	pad_off += sizeof(*itlv) + itlv->len;
	211	partial_srh_len = pad_off - srh_off;
	212	len_remaining = partial_srh_len % 8;
	213	new_pad = 8 - len_remaining;
	214
	215	if (new_pad == 1) // cannot pad for 1 byte only
	216	new_pad = 9;
	217	else if (new_pad == 8)
	218	new_pad = 0;
	219
	220	return update_tlv_pad(skb, new_pad, pad_size, pad_off);
	221	}
	222
	223	// Add an Egress TLV fc00::4, add the flag A,
	224	// and apply End.X action to fc42::1
	225	SEC("lwt_seg6local")
	226	int __add_egr_x(struct __sk_buff *skb)
	227	{
	228	unsigned long long hi = 0xfc42000000000000;
	229	unsigned long long lo = 0x1;
	230	struct ip6_srh_t *srh = get_srh(skb);
	231	uint8_t new_flags = SR6_FLAG_ALERT;
	232	struct ip6_addr_t addr;
	233	int err, offset;
	234
	235	if (srh == NULL)
	236	return BPF_DROP;
	237
	238	uint8_t tlv[20] = {2, 18, 0, 0, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
	239	0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4};
	240
	241	err = add_tlv(skb, srh, (srh->hdrlen+1) << 3,
	242	(struct sr6_tlv_t *)&tlv, 20);
	243	if (err)
	244	return BPF_DROP;
	245
246	offset = sizeof(struct ip6_t) + offsetof(struct ip6_srh_t, flags);
247	err = bpf_lwt_seg6_store_bytes(skb, offset,
248	(void *)&new_flags, sizeof(new_flags));
249	if (err)
250	return BPF_DROP;
251
b9632679 IL	252	addr.lo = bpf_cpu_to_be64(lo);
b9632679 IL	253	addr.hi = bpf_cpu_to_be64(hi);
b061017f AS	254	err = bpf_lwt_seg6_action(skb, SEG6_LOCAL_ACTION_END_X,
	255	(void *)&addr, sizeof(addr));
	256	if (err)
	257	return BPF_DROP;
	258	return BPF_REDIRECT;
	259	}
	260	char __license[] SEC("license") = "GPL";