#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <asm/barrier.h>
#include <linux/compiler.h>
#include <linux/ethtool.h>
#include <linux/filter.h>
#include <linux/if_ether.h>
#include <linux/if_link.h>
#include <linux/if_packet.h>
#include <linux/if_xdp.h>
#include <linux/kernel.h>
#include <linux/list.h>
#include <linux/netlink.h>
#include <linux/rtnetlink.h>
#include <linux/sockios.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <bpf/bpf.h>
#include <bpf/libbpf.h>
#include "xsk.h"
#include "bpf_util.h"
#ifndef SOL_XDP
#define SOL_XDP 283
#endif
#ifndef AF_XDP
#define AF_XDP 44
#endif
#ifndef PF_XDP
#define PF_XDP AF_XDP
#endif
#define pr_warn(fmt, ...) fprintf(stderr, fmt, ##__VA_ARGS__)
#define XSKMAP_SIZE 1
struct xsk_umem {
struct xsk_ring_prod *fill_save;
struct xsk_ring_cons *comp_save;
char *umem_area;
struct xsk_umem_config config;
int fd;
int refcount;
struct list_head ctx_list;
bool rx_ring_setup_done;
bool tx_ring_setup_done;
};
struct xsk_ctx {
struct xsk_ring_prod *fill;
struct xsk_ring_cons *comp;
__u32 queue_id;
struct xsk_umem *umem;
int refcount;
int ifindex;
struct list_head list;
};
struct xsk_socket {
struct xsk_ring_cons *rx;
struct xsk_ring_prod *tx;
struct xsk_ctx *ctx;
struct xsk_socket_config config;
int fd;
};
struct nl_mtu_req {
struct nlmsghdr nh;
struct ifinfomsg msg;
char buf[512];
};
int xsk_umem__fd(const struct xsk_umem *umem)
{
return umem ? umem->fd : -EINVAL;
}
int xsk_socket__fd(const struct xsk_socket *xsk)
{
return xsk ? xsk->fd : -EINVAL;
}
static bool xsk_page_aligned(void *buffer)
{
unsigned long addr = (unsigned long)buffer;
return !(addr & (getpagesize() - 1));
}
static void xsk_set_umem_config(struct xsk_umem_config *cfg,
const struct xsk_umem_config *usr_cfg)
{
if (!usr_cfg) {
cfg->fill_size = XSK_RING_PROD__DEFAULT_NUM_DESCS;
cfg->comp_size = XSK_RING_CONS__DEFAULT_NUM_DESCS;
cfg->frame_size = XSK_UMEM__DEFAULT_FRAME_SIZE;
cfg->frame_headroom = XSK_UMEM__DEFAULT_FRAME_HEADROOM;
cfg->flags = XSK_UMEM__DEFAULT_FLAGS;
cfg->tx_metadata_len = 0;
return;
}
cfg->fill_size = usr_cfg->fill_size;
cfg->comp_size = usr_cfg->comp_size;
cfg->frame_size = usr_cfg->frame_size;
cfg->frame_headroom = usr_cfg->frame_headroom;
cfg->flags = usr_cfg->flags;
cfg->tx_metadata_len = usr_cfg->tx_metadata_len;
}
static int xsk_set_xdp_socket_config(struct xsk_socket_config *cfg,
const struct xsk_socket_config *usr_cfg)
{
if (!usr_cfg) {
cfg->rx_size = XSK_RING_CONS__DEFAULT_NUM_DESCS;
cfg->tx_size = XSK_RING_PROD__DEFAULT_NUM_DESCS;
cfg->bind_flags = 0;
return 0;
}
cfg->rx_size = usr_cfg->rx_size;
cfg->tx_size = usr_cfg->tx_size;
cfg->bind_flags = usr_cfg->bind_flags;
return 0;
}
static int xsk_get_mmap_offsets(int fd, struct xdp_mmap_offsets *off)
{
socklen_t optlen;
int err;
optlen = sizeof(*off);
err = getsockopt(fd, SOL_XDP, XDP_MMAP_OFFSETS, off, &optlen);
if (err)
return err;
if (optlen == sizeof(*off))
return 0;
return -EINVAL;
}
static int xsk_create_umem_rings(struct xsk_umem *umem, int fd,
struct xsk_ring_prod *fill,
struct xsk_ring_cons *comp)
{
struct xdp_mmap_offsets off;
void *map;
int err;
err = setsockopt(fd, SOL_XDP, XDP_UMEM_FILL_RING,
&umem->config.fill_size,
sizeof(umem->config.fill_size));
if (err)
return -errno;
err = setsockopt(fd, SOL_XDP, XDP_UMEM_COMPLETION_RING,
&umem->config.comp_size,
sizeof(umem->config.comp_size));
if (err)
return -errno;
err = xsk_get_mmap_offsets(fd, &off);
if (err)
return -errno;
map = mmap(NULL, off.fr.desc + umem->config.fill_size * sizeof(__u64),
PROT_READ | PROT_WRITE, MAP_SHARED | MAP_POPULATE, fd,
XDP_UMEM_PGOFF_FILL_RING);
if (map == MAP_FAILED)
return -errno;
fill->mask = umem->config.fill_size - 1;
fill->size = umem->config.fill_size;
fill->producer = map + off.fr.producer;
fill->consumer = map + off.fr.consumer;
fill->flags = map + off.fr.flags;
fill->ring = map + off.fr.desc;
fill->cached_cons = umem->config.fill_size;
map = mmap(NULL, off.cr.desc + umem->config.comp_size * sizeof(__u64),
PROT_READ | PROT_WRITE, MAP_SHARED | MAP_POPULATE, fd,
XDP_UMEM_PGOFF_COMPLETION_RING);
if (map == MAP_FAILED) {
err = -errno;
goto out_mmap;
}
comp->mask = umem->config.comp_size - 1;
comp->size = umem->config.comp_size;
comp->producer = map + off.cr.producer;
comp->consumer = map + off.cr.consumer;
comp->flags = map + off.cr.flags;
comp->ring = map + off.cr.desc;
return 0;
out_mmap:
munmap(map, off.fr.desc + umem->config.fill_size * sizeof(__u64));
return err;
}
int xsk_umem__create(struct xsk_umem **umem_ptr, void *umem_area,
__u64 size, struct xsk_ring_prod *fill,
struct xsk_ring_cons *comp,
const struct xsk_umem_config *usr_config)
{
struct xdp_umem_reg mr;
struct xsk_umem *umem;
int err;
if (!umem_area || !umem_ptr || !fill || !comp)
return -EFAULT;
if (!size && !xsk_page_aligned(umem_area))
return -EINVAL;
umem = calloc(1, sizeof(*umem));
if (!umem)
return -ENOMEM;
umem->fd = socket(AF_XDP, SOCK_RAW | SOCK_CLOEXEC, 0);
if (umem->fd < 0) {
err = -errno;
goto out_umem_alloc;
}
umem->umem_area = umem_area;
INIT_LIST_HEAD(&umem->ctx_list);
xsk_set_umem_config(&umem->config, usr_config);
memset(&mr, 0, sizeof(mr));
mr.addr = (uintptr_t)umem_area;
mr.len = size;
mr.chunk_size = umem->config.frame_size;
mr.headroom = umem->config.frame_headroom;
mr.flags = umem->config.flags;
mr.tx_metadata_len = umem->config.tx_metadata_len;
err = setsockopt(umem->fd, SOL_XDP, XDP_UMEM_REG, &mr, sizeof(mr));
if (err) {
err = -errno;
goto out_socket;
}
err = xsk_create_umem_rings(umem, umem->fd, fill, comp);
if (err)
goto out_socket;
umem->fill_save = fill;
umem->comp_save = comp;
*umem_ptr = umem;
return 0;
out_socket:
close(umem->fd);
out_umem_alloc:
free(umem);
return err;
}
bool xsk_is_in_mode(u32 ifindex, int mode)
{
LIBBPF_OPTS(bpf_xdp_query_opts, opts);
int ret;
ret = bpf_xdp_query(ifindex, mode, &opts);
if (ret) {
printf("XDP mode query returned error %s\n", strerror(errno));
return false;
}
if (mode == XDP_FLAGS_DRV_MODE)
return opts.attach_mode == XDP_ATTACHED_DRV;
else if (mode == XDP_FLAGS_SKB_MODE)
return opts.attach_mode == XDP_ATTACHED_SKB;
return false;
}
static int netlink_recvmsg(int sock, struct msghdr *mhdr, int flags)
{
int len;
do {
len = recvmsg(sock, mhdr, flags);
} while (len < 0 && (errno == EINTR || errno == EAGAIN));
if (len < 0)
return -errno;
return len;
}
static int alloc_iov(struct iovec *iov, int len)
{
void *nbuf;
nbuf = realloc(iov->iov_base, len);
if (!nbuf)
return -ENOMEM;
iov->iov_base = nbuf;
iov->iov_len = len;
return 0;
}
static int netlink_recv(int sock)
{
struct iovec iov = {};
struct msghdr mhdr = {
.msg_iov = &iov,
.msg_iovlen = 1,
};
bool multipart = true;
struct nlmsgerr *err;
struct nlmsghdr *nh;
int len, ret;
ret = alloc_iov(&iov, 4096);
if (ret)
goto done;
while (multipart) {
multipart = false;
len = netlink_recvmsg(sock, &mhdr, MSG_PEEK | MSG_TRUNC);
if (len < 0) {
ret = len;
goto done;
}
if (len > iov.iov_len) {
ret = alloc_iov(&iov, len);
if (ret)
goto done;
}
len = netlink_recvmsg(sock, &mhdr, 0);
if (len < 0) {
ret = len;
goto done;
}
if (len == 0)
break;
for (nh = (struct nlmsghdr *)iov.iov_base; NLMSG_OK(nh, len);
nh = NLMSG_NEXT(nh, len)) {
if (nh->nlmsg_flags & NLM_F_MULTI)
multipart = true;
switch (nh->nlmsg_type) {
case NLMSG_ERROR:
err = (struct nlmsgerr *)NLMSG_DATA(nh);
if (!err->error)
continue;
ret = err->error;
goto done;
case NLMSG_DONE:
ret = 0;
goto done;
default:
break;
}
}
}
ret = 0;
done:
free(iov.iov_base);
return ret;
}
int xsk_set_mtu(int ifindex, int mtu)
{
struct nl_mtu_req req;
struct rtattr *rta;
int fd, ret;
fd = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
if (fd < 0)
return fd;
memset(&req, 0, sizeof(req));
req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
req.nh.nlmsg_type = RTM_NEWLINK;
req.msg.ifi_family = AF_UNSPEC;
req.msg.ifi_index = ifindex;
rta = (struct rtattr *)(((char *)&req) + NLMSG_ALIGN(req.nh.nlmsg_len));
rta->rta_type = IFLA_MTU;
rta->rta_len = RTA_LENGTH(sizeof(unsigned int));
req.nh.nlmsg_len = NLMSG_ALIGN(req.nh.nlmsg_len) + RTA_LENGTH(sizeof(mtu));
memcpy(RTA_DATA(rta), &mtu, sizeof(mtu));
ret = send(fd, &req, req.nh.nlmsg_len, 0);
if (ret < 0) {
close(fd);
return errno;
}
ret = netlink_recv(fd);
close(fd);
return ret;
}
int xsk_attach_xdp_program(struct bpf_program *prog, int ifindex, u32 xdp_flags)
{
int prog_fd;
prog_fd = bpf_program__fd(prog);
return bpf_xdp_attach(ifindex, prog_fd, xdp_flags, NULL);
}
void xsk_detach_xdp_program(int ifindex, u32 xdp_flags)
{
bpf_xdp_detach(ifindex, xdp_flags, NULL);
}
void xsk_clear_xskmap(struct bpf_map *map)
{
u32 index = 0;
int map_fd;
map_fd = bpf_map__fd(map);
bpf_map_delete_elem(map_fd, &index);
}
int xsk_update_xskmap(struct bpf_map *map, struct xsk_socket *xsk, u32 index)
{
int map_fd, sock_fd;
map_fd = bpf_map__fd(map);
sock_fd = xsk_socket__fd(xsk);
return bpf_map_update_elem(map_fd, &index, &sock_fd, 0);
}
static struct xsk_ctx *xsk_get_ctx(struct xsk_umem *umem, int ifindex,
__u32 queue_id)
{
struct xsk_ctx *ctx;
if (list_empty(&umem->ctx_list))
return NULL;
list_for_each_entry(ctx, &umem->ctx_list, list) {
if (ctx->ifindex == ifindex && ctx->queue_id == queue_id) {
ctx->refcount++;
return ctx;
}
}
return NULL;
}
static void xsk_put_ctx(struct xsk_ctx *ctx, bool unmap)
{
struct xsk_umem *umem = ctx->umem;
struct xdp_mmap_offsets off;
int err;
if (--ctx->refcount)
return;
if (!unmap)
goto out_free;
err = xsk_get_mmap_offsets(umem->fd, &off);
if (err)
goto out_free;
munmap(ctx->fill->ring - off.fr.desc, off.fr.desc + umem->config.fill_size *
sizeof(__u64));
munmap(ctx->comp->ring - off.cr.desc, off.cr.desc + umem->config.comp_size *
sizeof(__u64));
out_free:
list_del(&ctx->list);
free(ctx);
}
static struct xsk_ctx *xsk_create_ctx(struct xsk_socket *xsk,
struct xsk_umem *umem, int ifindex,
__u32 queue_id,
struct xsk_ring_prod *fill,
struct xsk_ring_cons *comp)
{
struct xsk_ctx *ctx;
int err;
ctx = calloc(1, sizeof(*ctx));
if (!ctx)
return NULL;
if (!umem->fill_save) {
err = xsk_create_umem_rings(umem, xsk->fd, fill, comp);
if (err) {
free(ctx);
return NULL;
}
} else if (umem->fill_save != fill || umem->comp_save != comp) {
memcpy(fill, umem->fill_save, sizeof(*fill));
memcpy(comp, umem->comp_save, sizeof(*comp));
}
ctx->ifindex = ifindex;
ctx->refcount = 1;
ctx->umem = umem;
ctx->queue_id = queue_id;
ctx->fill = fill;
ctx->comp = comp;
list_add(&ctx->list, &umem->ctx_list);
return ctx;
}
int xsk_socket__create_shared(struct xsk_socket **xsk_ptr,
int ifindex,
__u32 queue_id, struct xsk_umem *umem,
struct xsk_ring_cons *rx,
struct xsk_ring_prod *tx,
struct xsk_ring_prod *fill,
struct xsk_ring_cons *comp,
const struct xsk_socket_config *usr_config)
{
bool unmap, rx_setup_done = false, tx_setup_done = false;
void *rx_map = NULL, *tx_map = NULL;
struct sockaddr_xdp sxdp = {};
struct xdp_mmap_offsets off;
struct xsk_socket *xsk;
struct xsk_ctx *ctx;
int err;
if (!umem || !xsk_ptr || !(rx || tx))
return -EFAULT;
unmap = umem->fill_save != fill;
xsk = calloc(1, sizeof(*xsk));
if (!xsk)
return -ENOMEM;
err = xsk_set_xdp_socket_config(&xsk->config, usr_config);
if (err)
goto out_xsk_alloc;
if (umem->refcount++ > 0) {
xsk->fd = socket(AF_XDP, SOCK_RAW | SOCK_CLOEXEC, 0);
if (xsk->fd < 0) {
err = -errno;
goto out_xsk_alloc;
}
} else {
xsk->fd = umem->fd;
rx_setup_done = umem->rx_ring_setup_done;
tx_setup_done = umem->tx_ring_setup_done;
}
ctx = xsk_get_ctx(umem, ifindex, queue_id);
if (!ctx) {
if (!fill || !comp) {
err = -EFAULT;
goto out_socket;
}
ctx = xsk_create_ctx(xsk, umem, ifindex, queue_id, fill, comp);
if (!ctx) {
err = -ENOMEM;
goto out_socket;
}
}
xsk->ctx = ctx;
if (rx && !rx_setup_done) {
err = setsockopt(xsk->fd, SOL_XDP, XDP_RX_RING,
&xsk->config.rx_size,
sizeof(xsk->config.rx_size));
if (err) {
err = -errno;
goto out_put_ctx;
}
if (xsk->fd == umem->fd)
umem->rx_ring_setup_done = true;
}
if (tx && !tx_setup_done) {
err = setsockopt(xsk->fd, SOL_XDP, XDP_TX_RING,
&xsk->config.tx_size,
sizeof(xsk->config.tx_size));
if (err) {
err = -errno;
goto out_put_ctx;
}
if (xsk->fd == umem->fd)
umem->tx_ring_setup_done = true;
}
err = xsk_get_mmap_offsets(xsk->fd, &off);
if (err) {
err = -errno;
goto out_put_ctx;
}
if (rx) {
rx_map = mmap(NULL, off.rx.desc +
xsk->config.rx_size * sizeof(struct xdp_desc),
PROT_READ | PROT_WRITE, MAP_SHARED | MAP_POPULATE,
xsk->fd, XDP_PGOFF_RX_RING);
if (rx_map == MAP_FAILED) {
err = -errno;
goto out_put_ctx;
}
rx->mask = xsk->config.rx_size - 1;
rx->size = xsk->config.rx_size;
rx->producer = rx_map + off.rx.producer;
rx->consumer = rx_map + off.rx.consumer;
rx->flags = rx_map + off.rx.flags;
rx->ring = rx_map + off.rx.desc;
rx->cached_prod = *rx->producer;
rx->cached_cons = *rx->consumer;
}
xsk->rx = rx;
if (tx) {
tx_map = mmap(NULL, off.tx.desc +
xsk->config.tx_size * sizeof(struct xdp_desc),
PROT_READ | PROT_WRITE, MAP_SHARED | MAP_POPULATE,
xsk->fd, XDP_PGOFF_TX_RING);
if (tx_map == MAP_FAILED) {
err = -errno;
goto out_mmap_rx;
}
tx->mask = xsk->config.tx_size - 1;
tx->size = xsk->config.tx_size;
tx->producer = tx_map + off.tx.producer;
tx->consumer = tx_map + off.tx.consumer;
tx->flags = tx_map + off.tx.flags;
tx->ring = tx_map + off.tx.desc;
tx->cached_prod = *tx->producer;
tx->cached_cons = *tx->consumer + xsk->config.tx_size;
}
xsk->tx = tx;
sxdp.sxdp_family = PF_XDP;
sxdp.sxdp_ifindex = ctx->ifindex;
sxdp.sxdp_queue_id = ctx->queue_id;
if (umem->refcount > 1) {
sxdp.sxdp_flags |= XDP_SHARED_UMEM;
sxdp.sxdp_shared_umem_fd = umem->fd;
} else {
sxdp.sxdp_flags = xsk->config.bind_flags;
}
err = bind(xsk->fd, (struct sockaddr *)&sxdp, sizeof(sxdp));
if (err) {
err = -errno;
goto out_mmap_tx;
}
*xsk_ptr = xsk;
umem->fill_save = NULL;
umem->comp_save = NULL;
return 0;
out_mmap_tx:
if (tx)
munmap(tx_map, off.tx.desc +
xsk->config.tx_size * sizeof(struct xdp_desc));
out_mmap_rx:
if (rx)
munmap(rx_map, off.rx.desc +
xsk->config.rx_size * sizeof(struct xdp_desc));
out_put_ctx:
xsk_put_ctx(ctx, unmap);
out_socket:
if (--umem->refcount)
close(xsk->fd);
out_xsk_alloc:
free(xsk);
return err;
}
int xsk_socket__create(struct xsk_socket **xsk_ptr, int ifindex,
__u32 queue_id, struct xsk_umem *umem,
struct xsk_ring_cons *rx, struct xsk_ring_prod *tx,
const struct xsk_socket_config *usr_config)
{
if (!umem)
return -EFAULT;
return xsk_socket__create_shared(xsk_ptr, ifindex, queue_id, umem,
rx, tx, umem->fill_save,
umem->comp_save, usr_config);
}
int xsk_umem__delete(struct xsk_umem *umem)
{
struct xdp_mmap_offsets off;
int err;
if (!umem)
return 0;
if (umem->refcount)
return -EBUSY;
err = xsk_get_mmap_offsets(umem->fd, &off);
if (!err && umem->fill_save && umem->comp_save) {
munmap(umem->fill_save->ring - off.fr.desc,
off.fr.desc + umem->config.fill_size * sizeof(__u64));
munmap(umem->comp_save->ring - off.cr.desc,
off.cr.desc + umem->config.comp_size * sizeof(__u64));
}
close(umem->fd);
free(umem);
return 0;
}
void xsk_socket__delete(struct xsk_socket *xsk)
{
size_t desc_sz = sizeof(struct xdp_desc);
struct xdp_mmap_offsets off;
struct xsk_umem *umem;
struct xsk_ctx *ctx;
int err;
if (!xsk)
return;
ctx = xsk->ctx;
umem = ctx->umem;
xsk_put_ctx(ctx, true);
err = xsk_get_mmap_offsets(xsk->fd, &off);
if (!err) {
if (xsk->rx) {
munmap(xsk->rx->ring - off.rx.desc,
off.rx.desc + xsk->config.rx_size * desc_sz);
}
if (xsk->tx) {
munmap(xsk->tx->ring - off.tx.desc,
off.tx.desc + xsk->config.tx_size * desc_sz);
}
}
umem->refcount--;
if (xsk->fd != umem->fd)
close(xsk->fd);
free(xsk);
}
