skb_util.c

来自「xen虚拟机源代码安装包」· C语言 代码 · 共 557 行 · 第 1/2 页

/*
 * Copyright (C) 2004, 2005 Mike Wray <[e-mail]>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by the 
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free software Foundation, Inc.,
 * 59 Temple Place, suite 330, Boston, MA 02111-1307 USA
 *
 */
#ifdef __KERNEL__
#include <linux/config.h>
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/version.h>

#include <asm/scatterlist.h>
#include <linux/crypto.h>
#include <linux/pfkeyv2.h>
#include <linux/random.h>

#include <linux/net.h>
#include <linux/in.h>
#include <linux/inet.h>
#include <linux/netdevice.h>
#include <linux/tcp.h>
#include <linux/udp.h>

#include <net/ip.h>
#include <net/protocol.h>
#include <net/route.h>
#include <linux/skbuff.h>

#else

#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <unistd.h>
#include <stdio.h>
#include <errno.h>

#include <netinet/in.h>
#include <arpa/inet.h>

#include <sys/types.h>
#include <sys/socket.h>

#include <linux/if_ether.h>
#include <linux/if_arp.h>
#include <linux/ip.h>
#include <linux/tcp.h>
#include <linux/udp.h>

#include "sys_kernel.h"
#include "skbuff.h"

#if defined(__LITTLE_ENDIAN)
#define HIPQUAD(addr) \
	((unsigned char *)&addr)[3], \
	((unsigned char *)&addr)[2], \
	((unsigned char *)&addr)[1], \
	((unsigned char *)&addr)[0]
#elif defined(__BIG_ENDIAN)
#define HIPQUAD	NIPQUAD
#else
#error "Please fix asm/byteorder.h"
#endif /* __LITTLE_ENDIAN */

#endif

#include <varp.h>
#include <skb_util.h>

#define MODULE_NAME "VNET"
#define DEBUG 1
#undef DEBUG
#include "debug.h"

//============================================================================
/** Make enough room in an skb for extra header and trailer.
 *
 * @param pskb return parameter for expanded skb
 * @param skb skb
 * @param head_n required headroom
 * @param tail_n required tailroom
 * @return 0 on success, error code otherwise
 */
int skb_make_room(struct sk_buff **pskb, struct sk_buff *skb, int head_n, int tail_n){
    int err = 0;
    int has_headroom = (head_n <= skb_headroom(skb));
    int has_tailroom = (tail_n <= skb_tailroom(skb));
    int writeable = !skb_cloned(skb) && !skb_shared(skb);

    dprintf("> skb=%p headroom=%d head_n=%d tailroom=%d tail_n=%d\n",
            skb,
            skb_headroom(skb), head_n,
            skb_tailroom(skb), tail_n);
    if(writeable && has_headroom && has_tailroom){
        // There's room! Reuse it.
        *pskb = skb;
    } else if(writeable && has_tailroom){
        // Tailroom, no headroom. Expand header the way GRE does.
        struct sk_buff *new_skb = skb_realloc_headroom(skb, head_n + 16);
        if(!new_skb){
            err = -ENOMEM;
            goto exit;
        }
        kfree_skb(skb);
        *pskb = new_skb;
    } else {
        // No room. Expand. There may be more efficient ways to do
        // this, but this is simple and correct.
        struct sk_buff *new_skb = skb_copy_expand(skb, head_n + 16, tail_n, GFP_ATOMIC);
        if(!new_skb){
            err = -ENOMEM;
            goto exit;
        }
        kfree_skb(skb);
        *pskb = new_skb;
    }
    dprintf("> skb=%p headroom=%d head_n=%d tailroom=%d tail_n=%d\n",
            *pskb,
            skb_headroom(*pskb), head_n,
            skb_tailroom(*pskb), tail_n);
  exit:
    dprintf("< err=%d\n", err);
    return err;
}

/** Copy some data bits from a kernel buffer to an skb.
 * Derived in the obvious way from skb_copy_bits().
 */
int skb_put_bits(const struct sk_buff *skb, int offset, void *src, int len)
{
    int i, copy;
    int start = skb->len - skb->data_len;

    if (offset > (int)skb->len-len)
        goto fault;

    /* Copy header. */
    if ((copy = start-offset) > 0) {
        if (copy > len)
            copy = len;
        memcpy(skb->data + offset, src, copy);
        if ((len -= copy) == 0)
            return 0;
        offset += copy;
        src += copy;
    }

#ifdef __KERNEL__
    for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
        int end;

        BUG_TRAP(start <= offset+len);

        end = start + skb_shinfo(skb)->frags[i].size;
        if ((copy = end-offset) > 0) {
            u8 *vaddr;

            if (copy > len)
                copy = len;

            vaddr = kmap_skb_frag(&skb_shinfo(skb)->frags[i]);
            memcpy(vaddr + skb_shinfo(skb)->frags[i].page_offset + offset - start,
                   src,
                   copy);
            kunmap_skb_frag(vaddr);

            if ((len -= copy) == 0)
                return 0;
            offset += copy;
            src += copy;
        }
        start = end;
    }

    if (skb_shinfo(skb)->frag_list) {
        struct sk_buff *list;
        
        for (list = skb_shinfo(skb)->frag_list; list; list=list->next) {
            int end;
            
            BUG_TRAP(start <= offset+len);
            
            end = start + list->len;
            if ((copy = end-offset) > 0) {
                if (copy > len)
                    copy = len;
                if (skb_put_bits(list, offset-start, src, copy))
                    goto fault;
                if ((len -= copy) == 0)
                    return 0;
                offset += copy;
                src += copy;
            }
            start = end;
        }
    }
#else
    i=0;
#endif

    if (len == 0)
        return 0;

 fault:
    return -EFAULT;
}

int skboffset(struct sk_buff *skb, unsigned char *ptr){
    if(!ptr || ptr < skb->head || ptr > skb->tail){
        return -1;
    }
    return (ptr - skb->head);
}

/** Print some bits of an skb.
 *
 * @param skb to print
 * @param offset byte offset to start printing at
 * @param n number of bytes to print
 */
void skb_print_bits(const char *msg, struct sk_buff *skb, int offset, int n){
    int chunk = 16;
    int i, k;
    u8 buff[chunk];
    if(!skb) return;
    printk("%s> tot=%d len=%d data=%d mac=%d nh=%d h=%d\n",
           msg,
           skb->tail - skb->head,
           skb->len,
           skboffset(skb, skb->data),
           skboffset(skb, skb->mac.raw),
           skboffset(skb, skb->nh.raw),
           skboffset(skb, skb->h.raw));
    printk("%s> head=%p data=%p mac=%p nh=%p h=%p tail=%p\n",
           msg, skb->head, skb->data,
           skb->mac.raw, skb->nh.raw, skb->h.raw,
           skb->tail);
    while(n){
        k = (n > chunk ? chunk : n);
        skb_copy_bits(skb, offset, buff, k);
        printk("%03d ", offset);
        for(i=0; i<k; i++){
            if(i == 8)printk(" "); 
            printk(":%02x", buff[i] & 0xff);
        }
        printk(" \n");
        n -= k;
        offset += k;
    }
}

/** Print a buffer.
 *
 * @param buf to print
 * @param n number of bytes to print
 */
void buf_print(char *buf, int n){
    int i;
    for(i=0; i<n; i++){
        if( i % 16 == 0) printk("\n%04d ", i);
        else if(i % 8 == 0) printk(" ");
        printk(":%02x", buf[i] & 0xff);
    }
    printk(" %04d\n", n);
}