整合INT

[JerryChenBupt]

bpf_code:

#include <uapi/linux/if_ether.h>
#include <uapi/linux/ptrace.h>
#include <uapi/linux/in.h>
#include <uapi/linux/ip.h>
#include <uapi/linux/udp.h>
#include <uapi/linux/pkt_cls.h>
#include <uapi/linux/bpf.h>
#include <linux/skbuff.h>
#include <linux/ip.h>

#define IP_CSUM_OFF (ETH_HLEN + offsetof(struct iphdr, check))
#define TOS_OFF (ETH_HLEN + offsetof(struct iphdr, tos))
#define DEST_OFF (ETH_HLEN + sizeof(struct iphdr) + offsetof(struct udphdr, dest))

#define DST_PORT 80

struct delay {
    u64 d1;
    u64 d2;
    u64 d3;
}delay_struct;

BPF_HASH(counts, u16, struct delay);

int rx(struct pt_regs *ctx, struct sk_buff *skb) {
    u64 ts;
    struct delay zero = {0, 0, 0};
    struct delay *val;
    struct iphdr *iph;
    struct udphdr *udph;
    
    iph = (struct iphdr *)(skb->head + skb->network_header);
    u8 ip_p;
    u16 ip_id;
    bpf_probe_read(&ip_p, sizeof(ip_p), &(iph->protocol));
    bpf_probe_read(&ip_id, sizeof(ip_id), &(iph->id));
    
    if(bpf_ntohs(skb->protocol)==0x0800 && ip_p==17)
    {
		udph = (struct udphdr *)(skb->head + skb->transport_header);
		u16 source;
		u16 dest;
		bpf_probe_read(&source, sizeof(source), &(udph->source));
		bpf_probe_read(&dest, sizeof(dest), &(udph->dest));

        if(ntohs(dest)==DST_PORT)
		{
            val = counts.lookup_or_init(&ip_id, &zero);
            val->d1 = bpf_ktime_get_ns();
            bpf_trace_printk("RX: \tip_id %d \td1 = %llu ns \td2 = %llu ns\n", ntohs(ip_id), val->d1, val->d2);
		}
    }
    return 0;
}

int iprcv(struct pt_regs *ctx, struct sk_buff *skb) {
    u64 ts;
    struct delay zero = {0, 0, 0};
    struct delay *val;
    struct iphdr *iph;
    struct udphdr *udph;
    
    iph = (struct iphdr *)(skb->head + skb->network_header);
    u8 ip_p;
    u16 ip_id;
    bpf_probe_read(&ip_p, sizeof(ip_p), &(iph->protocol));
    bpf_probe_read(&ip_id, sizeof(ip_id), &(iph->id));
    
    if(bpf_ntohs(skb->protocol)==0x0800 && ip_p==17)
    {
		udph = (struct udphdr *)(skb->head + skb->transport_header);
		u16 source;
		u16 dest;
		bpf_probe_read(&source, sizeof(source), &(udph->source));
		bpf_probe_read(&dest, sizeof(dest), &(udph->dest));

        if(ntohs(dest)==DST_PORT)
		{
            val = counts.lookup_or_init(&ip_id, &zero);
            val->d2 = bpf_ktime_get_ns();
            bpf_trace_printk("IP_RCV: \tip_id %d \td1 = %llu ns \td2 = %llu ns\n", ntohs(ip_id), val->d1, val->d2);
		}
    }
    return 0;
}

int ipoutput(struct pt_regs *ctx, struct net *net, struct sock *sk, struct sk_buff *skb) {
    u64 ts;
    struct delay zero = {0, 0, 0};
    struct delay *val;
    struct iphdr *iph;
    struct udphdr *udph;
    
    iph = (struct iphdr *)(skb->head + skb->network_header);
    u8 ip_p;
    u16 ip_id;
    bpf_probe_read(&ip_p, sizeof(ip_p), &(iph->protocol));
    bpf_probe_read(&ip_id, sizeof(ip_id), &(iph->id));
    
    if(bpf_ntohs(skb->protocol)==0x0800 && ip_p==17)
    {
		udph = (struct udphdr *)(skb->head + skb->transport_header);
		u16 source;
		u16 dest;
		bpf_probe_read(&source, sizeof(source), &(udph->source));
		bpf_probe_read(&dest, sizeof(dest), &(udph->dest));

        if(ntohs(dest)==DST_PORT)
		{
            val = counts.lookup_or_init(&ip_id, &zero);
            val->d3 = bpf_ktime_get_ns();
            bpf_trace_printk("IP_OUTPUT: \tip_id %d \td3 = %llu ns\n", ntohs(ip_id), val->d3);
		}
    }
    return 0;
}

int egress(struct __sk_buff *skb) {
    u16 eth_protocol = bpf_ntohl(skb->protocol)>>16;
    
    void *data = (void *)(long)skb->data;
    void *data_end = (void *)(long)skb->data_end;
    struct ethhdr *eth = data;
    struct iphdr *iph = data + sizeof(*eth);
    struct udphdr *udph = data + sizeof(*eth) + sizeof(*iph);
    
    u8 ip_p;

    if (data + sizeof(*eth) + sizeof(*iph) > data_end)
        return TC_ACT_OK;

    if (eth->h_proto == htons(ETH_P_IP) &&
        iph->protocol == IPPROTO_UDP)
    {
        u16 dest;
        bpf_skb_load_bytes(skb, DEST_OFF, &dest, sizeof(dest));
        if(bpf_ntohs(dest)==DST_PORT)
        {
            bpf_trace_printk("engress : dest = %d\n", bpf_ntohs(dest));
        }
        //bpf_skb_load_bytes(skb, TOS_OFF, &old_tos, 1);
        //bpf_l3_csum_replace(skb, IP_CSUM_OFF, bpf_htons(old_tos), bpf_htons(new_tos), 2);
        //bpf_skb_store_bytes(skb, TOS_OFF, &new_tos, sizeof(new_tos), 0);
    }
    return 1;
}

run.py:

#!/usr/bin/python

from __future__ import print_function
from bcc import BPF
from pyroute2 import IPRoute

ipr = IPRoute()

try:
	# load BPF program
    b = BPF(src_file="BPF_attach.c", debug=0)
    
    b.attach_kprobe(event="netif_rx", fn_name="rx")
    b.attach_kprobe(event="ip_rcv", fn_name="iprcv")
    b.attach_kprobe(event="ip_output", fn_name="ipoutput")
    
    fn = b.load_func("egress", BPF.SCHED_CLS)

    idx = ipr.link_lookup(ifname="eth0")[0]
    
    ipr.tc("add", "sfq", idx, "1:")
    ipr.tc("add-filter", "bpf", idx, ":1", fd=fn.fd,
            name=fn.name, parent="1:", action="ok", classid=1)

    b.trace_print()    

except KeyboardInterrupt:
    print("Finish!")

finally:
    if "idx" in locals():
    	ipr.tc("del", "sfq", idx, "1:")

[JerryChenBupt]

测试UDP:
client:

#include <sys/types.h>
#include <sys/socket.h>
#include <string.h>
#include <netinet/in.h>
#include <stdio.h>
#include <stdlib.h>
#include <arpa/inet.h>
#include <unistd.h>

#define MAXLINE 80
#define SERV_PORT 8888

struct INThdr
{
	float t1;
	float t2;
	float t3;
}int_header;

struct sendData
{
	struct INThdr inthdr;
	char sendbuf[MAXLINE];
}send_data;

void do_cli(FILE *fp, int sockfd, struct sockaddr *pservaddr, socklen_t servlen)
{
	int n;
	char sendline[MAXLINE];
	
	/*  connect to server */
	if(connect(sockfd, (struct sockaddr *)pservaddr, servlen) == -1)
	{
		perror("connect error");
		exit(1);
	}
	
	while(fgets(sendline, MAXLINE, fp) != NULL)
	{
		// add INT header
		struct sendData senddata;
		memset(&senddata, 0, sizeof(struct sendData));
		memset(&senddata.inthdr, 0, sizeof(struct INThdr));
		memcpy(&senddata.sendbuf, sendline, sizeof(sendline));

		/*  send to server */
		write(sockfd, &senddata, sizeof(senddata));

		/*  receive data from server */
		struct sendData recvdata;
		n = read(sockfd, &recvdata, sizeof(recvdata));
		
		if(n == -1)
		{
			perror("read error");
			exit(1);
		}
		recvdata.sendbuf[n-sizeof(struct INThdr)] = 0; /*  terminate string */
		printf("INT: %f %f %f ", recvdata.inthdr.t1, recvdata.inthdr.t2, recvdata.inthdr.t3);
		fputs(recvdata.sendbuf, stdout);
	}
}

int main(int argc, char **argv)
{
	int sockfd; // socket fd
	struct sockaddr_in servaddr; // the server address

	/*  check args */
	if(argc != 2)
	{
		printf("usage: udpclient <IPaddress>/n");
		exit(1);
	}

	/*  init servaddr */
	bzero(&servaddr, sizeof(servaddr));
	servaddr.sin_family = AF_INET;
	servaddr.sin_port = htons(SERV_PORT);
	if(inet_pton(AF_INET, argv[1], &servaddr.sin_addr) <= 0)
	{
		printf("[%s] is not a valid IPaddress/n", argv[1]);
		exit(1);
	}

	// set up the socket
	sockfd = socket(AF_INET, SOCK_DGRAM, 0);

	// send the msg
	do_cli(stdin, sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr));

	return 0;
}

server:

#include <sys/types.h>
#include <sys/socket.h>
#include <string.h>
#include <netinet/in.h>
#include <stdio.h>
#include <stdlib.h>

#define MAXLINE 80
#define SERV_PORT 8888

struct INThdr
{
	float t1;
	float t2;
	float t3;
}int_header;

struct sendData
{
	struct INThdr inthdr;
	char sendbuf[MAXLINE];
}send_data;

void do_echo(int sockfd, struct sockaddr *pcliaddr, socklen_t clilen)
{
	int n;
	socklen_t len;
	char mesg[MAXLINE];
	for(;;)
	{
		len = clilen;
		/*  waiting for receive data */
		n = recvfrom(sockfd, mesg, MAXLINE, 0, pcliaddr, &len);
		/*  sent data back to client */
		sendto(sockfd, mesg, n, 0, pcliaddr, len);
	}
}

int main(void)
{
	int sockfd;
	struct sockaddr_in servaddr, cliaddr;
	sockfd = socket(AF_INET, SOCK_DGRAM, 0); /*  create a socket */

	/*  init servaddr */
	bzero(&servaddr, sizeof(servaddr));
	servaddr.sin_family = AF_INET;
	servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
	servaddr.sin_port = htons(SERV_PORT);

	/*  bind address and port to socket */
	if(bind(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) == -1)
	{
		perror("bind error");
		exit(1);
	}
	do_echo(sockfd, (struct sockaddr *)&cliaddr, sizeof(cliaddr));
	return 0;
}

compile:

#!/bin/bash
gcc -Wall -o client client.c &&
gcc -Wall -o server server.c