XDP blog, post 1 gist 7

xdp1-7-dns_says_no_final.c

static __always_inline
void csum_remove_data(uint32_t *csum, struct cursor *c, uint16_t len)
{
    if (c->pos + len <= c->end) {
        *csum = bpf_csum_diff(c->pos, len, 0, 0, *csum);
        c->pos += len;
    }
}

int xdp_dns_says_no(struct xdp_md *ctx)
{
    struct cursor   c;
    struct ethhdr  *eth;
    uint16_t        eth_proto;
    struct iphdr   *ipv4;
    struct ipv6hdr *ipv6;
    struct udphdr  *udp;
    enum xdp_action action = XDP_PASS;
    uint16_t        to_strip = 0;
 
    cursor_init(&c, ctx);
    if (!(eth = parse_eth(&c, &eth_proto)))
        return XDP_PASS;
 
    if (eth_proto == __bpf_htons(ETH_P_IP)) {
        if (!(ipv4 = parse_iphdr(&c))
        ||    ipv4->protocol != IPPROTO_UDP
        ||   (action = udp_dns_reply(&c, &udp)) != XDP_TX)
            return action;
 
        uint32_t swap_ipv4 = ipv4->daddr;
        ipv4->daddr = ipv4->saddr;
        ipv4->saddr = swap_ipv4;
 
        to_strip = c.end - c.pos;
        if (to_strip > 0) {
            uint32_t old_ipv4_len = ipv4->tot_len;
            uint32_t new_ipv4_len =
                __bpf_htons(__bpf_ntohs(old_ipv4_len) - to_strip);
            uint32_t csum = ~((uint32_t)ipv4->check);
 
            ipv4->tot_len = new_ipv4_len;
            csum = bpf_csum_diff( &old_ipv4_len, 4
                                , &new_ipv4_len, 4, csum);
            csum = (csum & 0xFFFF) + (csum >> 16);
            csum = (csum & 0xFFFF) + (csum >> 16);
            ipv4->check = ~csum;
 
            udp->len = __bpf_htons(__bpf_ntohs(udp->len) - to_strip);
            udp->check = 0;
        }
 
    } else if (eth_proto == __bpf_htons(ETH_P_IPV6)) {
        if (!(ipv6 = parse_ipv6hdr(&c))
        ||    ipv6->nexthdr  != IPPROTO_UDP
        ||   (action = udp_dns_reply(&c, &udp)) != XDP_TX)
            return action;
 
        struct in6_addr swap_ipv6 = ipv6->daddr;
        ipv6->daddr = ipv6->saddr;
        ipv6->saddr = swap_ipv6;
 
        to_strip = c.end - c.pos;
        if (to_strip > 0 && to_strip < 0x80) {
            uint32_t old_udp_len  = udp->len;
            uint32_t new_udp_len  =
                __bpf_htons(__bpf_ntohs(udp->len) - to_strip);
            uint32_t old_ipv6_len = ipv6->payload_len;
            uint32_t new_ipv6_len = 
                __bpf_htons(__bpf_ntohs(old_ipv6_len) - to_strip);
            uint32_t csum = ~((uint32_t)udp->check);
 
 
            ipv6->payload_len = new_ipv6_len;
            udp->len = new_udp_len;
            csum = bpf_csum_diff( &old_ipv6_len, 4
                                , &new_ipv6_len, 4, csum);
            csum = bpf_csum_diff( &old_udp_len , 4
                                , &new_udp_len , 4, csum);
 
            if ((c.pos - (void *)udp) % 2 == 1)
                c.pos -= 1;
 
            csum_remove_data(&csum, &c, 0x40);
            csum_remove_data(&csum, &c, 0x20);
            csum_remove_data(&csum, &c, 0x20);
            csum_remove_data(&csum, &c, 0x08);
            csum_remove_data(&csum, &c, 0x04);
            if (c.pos + 0x02 <= c.end) {
                uint32_t old_val = *(uint8_t *)c.pos;
                csum = bpf_csum_diff(&old_val, 4, 0, 0, csum);
                c.pos += 2;
            }
            if (c.pos + 0x01 <= c.end) {
                uint32_t old_val = *(uint8_t *)c.pos;
                csum = bpf_csum_diff(&old_val, 4, 0, 0, csum);
            }
            csum = (csum & 0xFFFF) + (csum >> 16);
            csum = (csum & 0xFFFF) + (csum >> 16);
            udp->check = ~csum;
        }
    } else
        return XDP_PASS;
 
    uint8_t swap_eth[ETH_ALEN];
    memcpy(swap_eth, eth->h_dest, ETH_ALEN);
    memcpy(eth->h_dest, eth->h_source, ETH_ALEN);
    memcpy(eth->h_source, swap_eth, ETH_ALEN);
 
    if (to_strip > 0)
        bpf_xdp_adjust_tail(ctx, -(int)to_strip);
 
    return XDP_TX;
}