int dsr_hw_header_create(struct dsr_pkt *dp, struct sk_buff *skb)
{
struct sockaddr broadcast =
{ AF_UNSPEC, {0xff, 0xff, 0xff, 0xff, 0xff, 0xff} };
struct neighbor_info neigh_info;
if (dp->dst.s_addr == DSR_BROADCAST)
memcpy(neigh_info.hw_addr.sa_data, broadcast.sa_data, ETH_ALEN);
else {
/* Get hardware destination address */
if (neigh_tbl_query(dp->nxt_hop, &neigh_info) < 0) {
DEBUG
("Could not get hardware address for next hop %s\n",
print_ip(dp->nxt_hop));
return -1;
}
}
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
if (skb->dev->hard_header) {
skb->dev->hard_header(skb, skb->dev, ETH_P_IP,
neigh_info.hw_addr.sa_data, 0, skb->len);
} else {
DEBUG("Missing hard_header\n");
return -1;
}
#else
dev_hard_header(skb, skb->dev, ETH_P_IP,
neigh_info.hw_addr.sa_data, 0, skb->len);
#endif
return 0;
}
/* DOWNSTACK: MASTER FUNCTION CALLED BY L3
* analagous to ip_output and ip_finish_output{2}
* they then call the equivalent of neigh_connected_output
* A LOT of arp shit happens here too!
* for reference: Figure 27.13
*/
int mhost_finish_output(struct sk_buff *skb, struct net_device *dev, const void *daddr)
{
int err = 0;
// printk(KERN_INFO "mhost_finish_output called\n");
/* set device-specific header here... */
skb->protocol = htons(ETH_P_MHOST);
skb->dev = dev;
err = dev_hard_header(skb, dev, ntohs(skb->protocol),
daddr, NULL, skb->len);
/* ...and ship off to the device driver! */
if (err >= 0) {
err = dev_queue_xmit(skb);
if (err < 0) {
printk(KERN_INFO "error: dev_queue_xmit!\n");
}
}
else {
printk(KERN_INFO "error: dev_hard_header returned %d\n", err);
err = -EINVAL;
kfree_skb(skb);
}
return err;
}
/**
* tipc_l2_send_msg - send a TIPC packet out over an L2 interface
* @buf: the packet to be sent
* @b_ptr: the bearer through which the packet is to be sent
* @dest: peer destination address
*/
int tipc_l2_send_msg(struct sk_buff *buf, struct tipc_bearer *b,
struct tipc_media_addr *dest)
{
struct sk_buff *clone;
struct net_device *dev;
int delta;
dev = (struct net_device *)rcu_dereference_rtnl(b->media_ptr);
if (!dev)
return 0;
clone = skb_clone(buf, GFP_ATOMIC);
if (!clone)
return 0;
delta = dev->hard_header_len - skb_headroom(buf);
if ((delta > 0) &&
pskb_expand_head(clone, SKB_DATA_ALIGN(delta), 0, GFP_ATOMIC)) {
kfree_skb(clone);
return 0;
}
skb_reset_network_header(clone);
clone->dev = dev;
clone->protocol = htons(ETH_P_TIPC);
dev_hard_header(clone, dev, ETH_P_TIPC, dest->value,
dev->dev_addr, clone->len);
dev_queue_xmit(clone);
return 0;
}
/**
* tipc_l2_send_msg - send a TIPC packet out over an L2 interface
* @skb: the packet to be sent
* @b: the bearer through which the packet is to be sent
* @dest: peer destination address
*/
int tipc_l2_send_msg(struct net *net, struct sk_buff *skb,
struct tipc_bearer *b, struct tipc_media_addr *dest)
{
struct net_device *dev;
int delta;
void *tipc_ptr;
dev = (struct net_device *)rcu_dereference_rtnl(b->media_ptr);
if (!dev)
return 0;
/* Send RESET message even if bearer is detached from device */
tipc_ptr = rcu_dereference_rtnl(dev->tipc_ptr);
if (unlikely(!tipc_ptr && !msg_is_reset(buf_msg(skb))))
goto drop;
delta = dev->hard_header_len - skb_headroom(skb);
if ((delta > 0) &&
pskb_expand_head(skb, SKB_DATA_ALIGN(delta), 0, GFP_ATOMIC))
goto drop;
skb_reset_network_header(skb);
skb->dev = dev;
skb->protocol = htons(ETH_P_TIPC);
dev_hard_header(skb, dev, ETH_P_TIPC, dest->value,
dev->dev_addr, skb->len);
dev_queue_xmit(skb);
return 0;
drop:
kfree_skb(skb);
return 0;
}
/**
* send_msg - send a TIPC message out over an InfiniBand interface
*/
static int send_msg(struct sk_buff *buf, struct tipc_bearer *tb_ptr,
struct tipc_media_addr *dest)
{
struct sk_buff *clone;
struct net_device *dev;
int delta;
clone = skb_clone(buf, GFP_ATOMIC);
if (!clone)
return 0;
dev = ((struct ib_bearer *)(tb_ptr->usr_handle))->dev;
delta = dev->hard_header_len - skb_headroom(buf);
if ((delta > 0) &&
pskb_expand_head(clone, SKB_DATA_ALIGN(delta), 0, GFP_ATOMIC)) {
kfree_skb(clone);
return 0;
}
skb_reset_network_header(clone);
clone->dev = dev;
clone->protocol = htons(ETH_P_TIPC);
dev_hard_header(clone, dev, ETH_P_TIPC, dest->value,
dev->dev_addr, clone->len);
dev_queue_xmit(clone);
return 0;
}
static int p8023_request(struct datalink_proto *dl,
struct sk_buff *skb, unsigned char *dest_node)
{
struct net_device *dev = skb->dev;
dev_hard_header(skb, dev, ETH_P_802_3, dest_node, NULL, skb->len);
return dev_queue_xmit(skb);
}
/*
* Prepends an ISI header and sends a datagram.
*/
static int pn_send(struct sk_buff *skb, struct net_device *dev,
u16 dst, u16 src, u8 res, u8 irq)
{
struct phonethdr *ph;
int err;
if (skb->len + 2 > 0xffff /* Phonet length field limit */ ||
skb->len + sizeof(struct phonethdr) > dev->mtu) {
err = -EMSGSIZE;
goto drop;
}
/* Broadcast sending is not implemented */
if (pn_addr(dst) == PNADDR_BROADCAST) {
err = -EOPNOTSUPP;
goto drop;
}
skb_reset_transport_header(skb);
WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */
skb_push(skb, sizeof(struct phonethdr));
skb_reset_network_header(skb);
ph = pn_hdr(skb);
ph->pn_rdev = pn_dev(dst);
ph->pn_sdev = pn_dev(src);
ph->pn_res = res;
ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph));
ph->pn_robj = pn_obj(dst);
ph->pn_sobj = pn_obj(src);
skb->protocol = htons(ETH_P_PHONET);
skb->priority = 0;
skb->dev = dev;
if (pn_addr(src) == pn_addr(dst)) {
skb_reset_mac_header(skb);
skb->pkt_type = PACKET_LOOPBACK;
skb_orphan(skb);
if (irq)
netif_rx(skb);
else
netif_rx_ni(skb);
err = 0;
} else {
err = dev_hard_header(skb, dev, ntohs(skb->protocol),
NULL, NULL, skb->len);
if (err < 0) {
err = -EHOSTUNREACH;
goto drop;
}
err = dev_queue_xmit(skb);
}
return err;
drop:
kfree_skb(skb);
return err;
}
static int pEII_request(struct datalink_proto *dl,
struct sk_buff *skb, unsigned char *dest_node)
{
struct net_device *dev = skb->dev;
skb->protocol = htons(ETH_P_IPX);
dev_hard_header(skb, dev, ETH_P_IPX, dest_node, NULL, skb->len);
return dev_queue_xmit(skb);
}
static void send_hsr_supervision_frame(struct net_device *hsr_dev, u8 type)
{
struct hsr_priv *hsr_priv;
struct sk_buff *skb;
int hlen, tlen;
struct hsr_sup_tag *hsr_stag;
struct hsr_sup_payload *hsr_sp;
unsigned long irqflags;
hlen = LL_RESERVED_SPACE(hsr_dev);
tlen = hsr_dev->needed_tailroom;
skb = alloc_skb(hsr_pad(sizeof(struct hsr_sup_payload)) + hlen + tlen,
GFP_ATOMIC);
if (skb == NULL)
return;
hsr_priv = netdev_priv(hsr_dev);
skb_reserve(skb, hlen);
skb->dev = hsr_dev;
skb->protocol = htons(ETH_P_PRP);
skb->priority = TC_PRIO_CONTROL;
if (dev_hard_header(skb, skb->dev, ETH_P_PRP,
hsr_priv->sup_multicast_addr,
skb->dev->dev_addr, skb->len) < 0)
goto out;
skb_pull(skb, sizeof(struct ethhdr));
hsr_stag = (typeof(hsr_stag)) skb->data;
set_hsr_stag_path(hsr_stag, 0xf);
set_hsr_stag_HSR_Ver(hsr_stag, 0);
spin_lock_irqsave(&hsr_priv->seqnr_lock, irqflags);
hsr_stag->sequence_nr = htons(hsr_priv->sequence_nr);
hsr_priv->sequence_nr++;
spin_unlock_irqrestore(&hsr_priv->seqnr_lock, irqflags);
hsr_stag->HSR_TLV_Type = type;
hsr_stag->HSR_TLV_Length = 12;
skb_push(skb, sizeof(struct ethhdr));
/* Payload: MacAddressA */
hsr_sp = (typeof(hsr_sp)) skb_put(skb, sizeof(*hsr_sp));
ether_addr_copy(hsr_sp->MacAddressA, hsr_dev->dev_addr);
dev_queue_xmit(skb);
return;
out:
kfree_skb(skb);
}
int virt_send_ack(struct virt_priv *virt, struct device_node *slave,
struct remote_link *link)
{
struct sk_buff *skb;
struct net_device *dev = slave->dev;
struct tunhdr *tunhdr;
struct pathinfo *path;
__be16 sport;
unsigned alloc_size = sizeof(struct tunhdr) + sizeof(struct udphdr) +
sizeof(struct iphdr) + LL_RESERVED_SPACE(dev);
path = lookup_pathinfo(&virt->network, slave, link);
if(!path)
return -EINVAL;
skb = alloc_skb(alloc_size, GFP_ATOMIC);
if(!skb) {
virt_path_put(path);
return -ENOMEM;
}
skb_reserve(skb, alloc_size);
tunhdr = virt_build_tunhdr(skb, NULL, NULL);
virt_finish_tunhdr(tunhdr, path, NULL, link->node);
/* TODO: We may want to split traffic among different ports, which
* may change how we send ACKs. For now, everything uses the same
* source port. */
sport = htons(virt_tunnel_source_port());
virt_build_udp_header(skb, sport, link->rif.data_port);
virt_build_ip_header(skb, slave->lif.ip4, link->rif.ip4);
skb->dev = dev;
skb->protocol = htons(ETH_P_IP);
dev_hard_header(skb, dev, ETH_P_IP, slave->next_hop_addr, dev->dev_addr, skb->len);
skb_reset_mac_header(skb);
/* Update link statistics -- these may not be accurate if the packet gets
* dropped after dev_queue_xmit. */
slave->stats.tx_packets++;
slave->stats.tx_bytes += skb->len;
/* Update device statistics. */
virt->stats.tx_packets++;
virt->stats.tx_bytes += skb->len;
/* Decrement refcnt. */
virt_path_put(path);
dev_queue_xmit(skb);
return 0;
}
static int vlan_passthru_hard_header(struct sk_buff *skb, struct net_device *dev,
unsigned short type,
const void *daddr, const void *saddr,
unsigned int len)
{
struct vlan_dev_priv *vlan = vlan_dev_priv(dev);
struct net_device *real_dev = vlan->real_dev;
return dev_hard_header(skb, real_dev, type, daddr, saddr, len);
}
static void netpoll_wrapper_send_arp_reply(struct netpoll_wrapper *pWrapper, struct queued_arp_reply *reply)
{
int hlen, tlen;
struct arphdr *arp;
struct sk_buff *send_skb;
unsigned char *arp_ptr;
int size = arp_hdr_len(pWrapper->pDeviceWithHandler);
hlen = LL_RESERVED_SPACE(pWrapper->pDeviceWithHandler);
tlen = pWrapper->pDeviceWithHandler->needed_tailroom;
send_skb = alloc_skb(size + hlen + tlen, GFP_ATOMIC);
if (!send_skb)
return;
skb_reserve(send_skb, hlen);
skb_reset_network_header(send_skb);
arp = (struct arphdr *) skb_put(send_skb, size);
send_skb->dev = pWrapper->pDeviceWithHandler;
send_skb->protocol = htons(ETH_P_ARP);
/* Fill the device header for the ARP frame */
if (dev_hard_header(send_skb, pWrapper->pDeviceWithHandler, ETH_P_ARP,
reply->remote_mac, pWrapper->pDeviceWithHandler->dev_addr,
send_skb->len) < 0)
{
kfree_skb(send_skb);
return;
}
/*
* Fill out the arp protocol part.
*
* we only support ethernet device type,
* which (according to RFC 1390) should
* always equal 1 (Ethernet).
*/
arp->ar_hrd = htons(pWrapper->pDeviceWithHandler->type);
arp->ar_pro = htons(ETH_P_IP);
arp->ar_hln = pWrapper->pDeviceWithHandler->addr_len;
arp->ar_pln = 4;
arp->ar_op = htons(ARPOP_REPLY);
arp_ptr = (unsigned char *)(arp + 1);
memcpy(arp_ptr, pWrapper->pDeviceWithHandler->dev_addr, pWrapper->pDeviceWithHandler->addr_len);
arp_ptr += pWrapper->pDeviceWithHandler->addr_len;
memcpy(arp_ptr, &reply->local_ip, 4);
arp_ptr += 4;
memcpy(arp_ptr, reply->remote_mac, pWrapper->pDeviceWithHandler->addr_len);
arp_ptr += pWrapper->pDeviceWithHandler->addr_len;
memcpy(arp_ptr, &reply->remote_ip, 4);
netpoll_send_skb(&pWrapper->netpoll_obj, send_skb);
}
static int vlan_passthru_hard_header(struct sk_buff *skb, struct net_device *dev,
unsigned short type,
const void *daddr, const void *saddr,
unsigned int len)
{
struct net_device *real_dev = vlan_dev_info(dev)->real_dev;
if (saddr == NULL)
saddr = dev->dev_addr;
return dev_hard_header(skb, real_dev, type, daddr, saddr, len);
}
static int ztdeth_transmit(void *pvt, unsigned char *msg, int msglen)
{
struct ztdeth *z;
struct sk_buff *skb;
struct ztdeth_header *zh;
unsigned long flags;
struct net_device *dev;
unsigned char addr[ETH_ALEN];
unsigned short subaddr; /* Network byte order */
spin_lock_irqsave(&zlock, flags);
z = pvt;
if (z->dev) {
/* Copy fields to local variables to remove spinlock ASAP */
dev = z->dev;
memcpy(addr, z->addr, sizeof(z->addr));
subaddr = z->subaddr;
spin_unlock_irqrestore(&zlock, flags);
skb = dev_alloc_skb(msglen + dev->hard_header_len + sizeof(struct ztdeth_header) + 32);
if (skb) {
/* Reserve header space */
skb_reserve(skb, dev->hard_header_len + sizeof(struct ztdeth_header));
/* Copy message body */
memcpy(skb_put(skb, msglen), msg, msglen);
/* Throw on header */
zh = (struct ztdeth_header *)skb_push(skb, sizeof(struct ztdeth_header));
zh->subaddr = subaddr;
/* Setup protocol and such */
skb->protocol = __constant_htons(ETH_P_DAHDI_DETH);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22)
skb_set_network_header(skb, 0);
#else
skb->nh.raw = skb->data;
#endif
skb->dev = dev;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
dev_hard_header(skb, dev, ETH_P_DAHDI_DETH, addr, dev->dev_addr, skb->len);
#else
if (dev->hard_header)
dev->hard_header(skb, dev, ETH_P_DAHDI_DETH, addr, dev->dev_addr, skb->len);
#endif
skb_queue_tail(&skbs, skb);
}
}
else
spin_unlock_irqrestore(&zlock, flags);
return 0;
}
int goose_trans_skb(struct net_device *dev, unsigned char *daddr,
struct sk_buff *__skb, int reliablity)
{
struct sk_buff *skb = __skb;
unsigned int skb_pull_len = NLMSG_LENGTH(0) + sizeof(struct nl_data_header);
if (unlikely((dev == NULL) || (!tran_active)))
goto goose_trans_skb_fail;
/* We use existing skb to form a new one:
*
* skb:
* old skb->data new skb->data
* | |
* -----------------------------------------------------------
* | NLMSG_LENGTH(0) | nl_data_header | goose header | APDU
* -----------------------------------------------------------
* | <== skb_pull_len ==> |
*
*/
skb_pull(skb, skb_pull_len);
skb_reset_network_header(skb);
/* But after pulling, if we have still no enough space for link-layer header,
we have to reconstruct a new skb */
if ((skb->head - skb->network_header) < LL_RESERVED_SPACE(dev)) {
skb = skb_copy_expand(__skb, LL_RESERVED_SPACE(dev), 16, GFP_ATOMIC);
kfree_skb(__skb);
}
/* Specify protocol type and frame information */
skb->dev = dev;
skb->protocol = ETH_P_GOOSE;
skb->pkt_type = PACKET_OUTGOING;
skb->csum = 0;
skb->ip_summed = 0;
/* Set the highest priority */
skb->priority = 0;
if (unlikely(dev_hard_header(skb, dev, ETH_P_GOOSE, daddr, dev->dev_addr, skb->len) < 0))
goto goose_trans_skb_fail;
/* If the message should be transmitted by GOOSE Enhanced Retransmission Mechanism,
call goose_enhan_retrans, otherwise transmit it directly.*/
return reliablity ? goose_enhan_retrans(skb):dev_queue_xmit(skb);
goose_trans_skb_fail:
kfree_skb(skb);
return -1;
}
/*
* Create the VLAN header for an arbitrary protocol layer
*
* saddr=NULL means use device source address
* daddr=NULL means leave destination address (eg unresolved arp)
*
* This is called when the SKB is moving down the stack towards the
* physical devices.
*/
static int vlan_dev_hard_header(struct sk_buff *skb, struct net_device *dev,
unsigned short type,
const void *daddr, const void *saddr,
unsigned int len)
{
struct vlan_hdr *vhdr;
unsigned int vhdrlen = 0;
u16 vlan_tci = 0;
int rc;
if (WARN_ON(skb_headroom(skb) < dev->hard_header_len))
return -ENOSPC;
if (!(vlan_dev_info(dev)->flags & VLAN_FLAG_REORDER_HDR)) {
vhdr = (struct vlan_hdr *) skb_push(skb, VLAN_HLEN);
vlan_tci = vlan_dev_info(dev)->vlan_id;
vlan_tci |= vlan_dev_get_egress_qos_mask(dev, skb);
vhdr->h_vlan_TCI = htons(vlan_tci);
/*
* Set the protocol type. For a packet of type ETH_P_802_3 we
* put the length in here instead. It is up to the 802.2
* layer to carry protocol information.
*/
if (type != ETH_P_802_3)
vhdr->h_vlan_encapsulated_proto = htons(type);
else
vhdr->h_vlan_encapsulated_proto = htons(len);
skb->protocol = htons(ETH_P_8021Q);
type = ETH_P_8021Q;
vhdrlen = VLAN_HLEN;
}
/* Before delegating work to the lower layer, enter our MAC-address */
if (saddr == NULL)
saddr = dev->dev_addr;
/* Now make the underlying real hard header */
dev = vlan_dev_info(dev)->real_dev;
rc = dev_hard_header(skb, dev, type, daddr, saddr, len + vhdrlen);
if (rc > 0)
rc += vhdrlen;
return rc;
}
static int send_msg(struct sk_buff *buf, struct tipc_bearer *tb_ptr,
struct tipc_media_addr *dest)
{
struct sk_buff *clone;
struct net_device *dev;
clone = skb_clone(buf, GFP_ATOMIC);
if (clone) {
clone->nh.raw = clone->data;
dev = ((struct eth_bearer *)(tb_ptr->usr_handle))->dev;
clone->dev = dev;
dev_hard_header(clone, dev, ETH_P_TIPC,
&dest->dev_addr.eth_addr,
dev->dev_addr, clone->len);
dev_queue_xmit(clone);
}
return TIPC_OK;
}
/**
* llc_mac_hdr_init - fills MAC header fields
* @skb: Address of the frame to initialize its MAC header
* @sa: The MAC source address
* @da: The MAC destination address
*
* Fills MAC header fields, depending on MAC type. Returns 0, If MAC type
* is a valid type and initialization completes correctly 1, otherwise.
*/
int llc_mac_hdr_init(struct sk_buff *skb,
unsigned char *sa, unsigned char *da)
{
int rc = -EINVAL;
switch (skb->dev->type) {
case ARPHRD_ETHER:
case ARPHRD_LOOPBACK:
rc = dev_hard_header(skb, skb->dev, ETH_P_802_2, da, sa,
skb->len);
if (rc > 0)
rc = 0;
break;
default:
WARN(1, "device type not supported: %d\n", skb->dev->type);
}
return rc;
}
/**
* llc_mac_hdr_init - fills MAC header fields
* @skb: Address of the frame to initialize its MAC header
* @sa: The MAC source address
* @da: The MAC destination address
*
* Fills MAC header fields, depending on MAC type. Returns 0, If MAC type
* is a valid type and initialization completes correctly 1, otherwise.
*/
int llc_mac_hdr_init(struct sk_buff *skb,
const unsigned char *sa, const unsigned char *da)
{
int rc = -EINVAL;
switch (skb->dev->type) {
case ARPHRD_IEEE802_TR:
case ARPHRD_ETHER:
case ARPHRD_LOOPBACK:
rc = dev_hard_header(skb, skb->dev, ETH_P_802_2, da, sa,
skb->len);
if (rc > 0)
rc = 0;
break;
default:
break;
}
return rc;
}
/*
* Create the VLAN header for an arbitrary protocol layer
*
* saddr=NULL means use device source address
* daddr=NULL means leave destination address (eg unresolved arp)
*
* This is called when the SKB is moving down the stack towards the
* physical devices.
*/
static int vlan_dev_hard_header(struct sk_buff *skb, struct net_device *dev,
unsigned short type,
const void *daddr, const void *saddr,
unsigned int len)
{
struct vlan_dev_priv *vlan = vlan_dev_priv(dev);
struct vlan_hdr *vhdr;
unsigned int vhdrlen = 0;
u16 vlan_tci = 0;
int rc;
if (!(vlan->flags & VLAN_FLAG_REORDER_HDR)) {
vhdr = (struct vlan_hdr *) skb_push(skb, VLAN_HLEN);
vlan_tci = vlan->vlan_id;
vlan_tci |= vlan_dev_get_egress_qos_mask(dev, skb->priority);
vhdr->h_vlan_TCI = htons(vlan_tci);
/*
* Set the protocol type. For a packet of type ETH_P_802_3/2 we
* put the length in here instead.
*/
if (type != ETH_P_802_3 && type != ETH_P_802_2)
vhdr->h_vlan_encapsulated_proto = htons(type);
else
vhdr->h_vlan_encapsulated_proto = htons(len);
skb->protocol = vlan->vlan_proto;
type = ntohs(vlan->vlan_proto);
vhdrlen = VLAN_HLEN;
}
/* Before delegating work to the lower layer, enter our MAC-address */
if (saddr == NULL)
saddr = dev->dev_addr;
/* Now make the underlying real hard header */
dev = vlan->real_dev;
rc = dev_hard_header(skb, dev, type, daddr, saddr, len + vhdrlen);
if (rc > 0)
rc += vhdrlen;
return rc;
}
/**
* tipc_l2_send_msg - send a TIPC packet out over an L2 interface
* @skb: the packet to be sent
* @b: the bearer through which the packet is to be sent
* @dest: peer destination address
*/
int tipc_l2_send_msg(struct net *net, struct sk_buff *skb,
struct tipc_bearer *b, struct tipc_media_addr *dest)
{
struct net_device *dev;
int delta;
dev = (struct net_device *)rcu_dereference_rtnl(b->media_ptr);
if (!dev)
return 0;
delta = SKB_DATA_ALIGN(dev->hard_header_len - skb_headroom(skb));
if ((delta > 0) && pskb_expand_head(skb, delta, 0, GFP_ATOMIC)) {
kfree_skb(skb);
return 0;
}
skb_reset_network_header(skb);
skb->dev = dev;
skb->protocol = htons(ETH_P_TIPC);
dev_hard_header(skb, dev, ETH_P_TIPC, dest->value,
dev->dev_addr, skb->len);
dev_queue_xmit(skb);
return 0;
}
static int
__teql_resolve(struct sk_buff *skb, struct sk_buff *skb_res,
struct net_device *dev, struct netdev_queue *txq,
struct neighbour *mn)
{
struct teql_sched_data *q = qdisc_priv(txq->qdisc);
struct neighbour *n = q->ncache;
if (mn->tbl == NULL)
return -EINVAL;
if (n && n->tbl == mn->tbl &&
memcmp(n->primary_key, mn->primary_key, mn->tbl->key_len) == 0) {
atomic_inc(&n->refcnt);
} else {
n = __neigh_lookup_errno(mn->tbl, mn->primary_key, dev);
if (IS_ERR(n))
return PTR_ERR(n);
}
if (neigh_event_send(n, skb_res) == 0) {
int err;
char haddr[MAX_ADDR_LEN];
neigh_ha_snapshot(haddr, n, dev);
err = dev_hard_header(skb, dev, ntohs(skb->protocol), haddr,
NULL, skb->len);
if (err < 0) {
neigh_release(n);
return -EINVAL;
}
teql_neigh_release(xchg(&q->ncache, n));
return 0;
}
neigh_release(n);
return (skb_res == NULL) ? -EAGAIN : 1;
}
/* Send RST reply */
void nf_send_reset(struct net *net, struct sk_buff *oldskb, int hook)
{
struct sk_buff *nskb;
struct iphdr *niph;
const struct tcphdr *oth;
struct tcphdr _oth;
oth = nf_reject_ip_tcphdr_get(oldskb, &_oth, hook);
if (!oth)
return;
if (skb_rtable(oldskb)->rt_flags & (RTCF_BROADCAST | RTCF_MULTICAST))
return;
nskb = alloc_skb(sizeof(struct iphdr) + sizeof(struct tcphdr) +
LL_MAX_HEADER, GFP_ATOMIC);
if (!nskb)
return;
/* ip_route_me_harder expects skb->dst to be set */
skb_dst_set_noref(nskb, skb_dst(oldskb));
nskb->mark = IP4_REPLY_MARK(net, oldskb->mark);
skb_reserve(nskb, LL_MAX_HEADER);
niph = nf_reject_iphdr_put(nskb, oldskb, IPPROTO_TCP,
ip4_dst_hoplimit(skb_dst(nskb)));
nf_reject_ip_tcphdr_put(nskb, oldskb, oth);
if (ip_route_me_harder(net, nskb, RTN_UNSPEC))
goto free_nskb;
niph = ip_hdr(nskb);
/* "Never happens" */
if (nskb->len > dst_mtu(skb_dst(nskb)))
goto free_nskb;
nf_ct_attach(nskb, oldskb);
#if IS_ENABLED(CONFIG_BRIDGE_NETFILTER)
/* If we use ip_local_out for bridged traffic, the MAC source on
* the RST will be ours, instead of the destination's. This confuses
* some routers/firewalls, and they drop the packet. So we need to
* build the eth header using the original destination's MAC as the
* source, and send the RST packet directly.
*/
if (oldskb->nf_bridge) {
struct ethhdr *oeth = eth_hdr(oldskb);
nskb->dev = nf_bridge_get_physindev(oldskb);
niph->tot_len = htons(nskb->len);
ip_send_check(niph);
if (dev_hard_header(nskb, nskb->dev, ntohs(nskb->protocol),
oeth->h_source, oeth->h_dest, nskb->len) < 0)
goto free_nskb;
dev_queue_xmit(nskb);
} else
#endif
ip_local_out(net, nskb->sk, nskb);
return;
free_nskb:
kfree_skb(nskb);
}
/*
* Create an arp packet. If (dest_hw == NULL), we create a broadcast
* message.
*/
struct sk_buff *arp_create(int type, int ptype, __be32 dest_ip,
struct net_device *dev, __be32 src_ip,
const unsigned char *dest_hw,
const unsigned char *src_hw,
const unsigned char *target_hw)
{
struct sk_buff *skb;
struct arphdr *arp;
unsigned char *arp_ptr;
/*
* Allocate a buffer
*/
skb = alloc_skb(arp_hdr_len(dev) + LL_ALLOCATED_SPACE(dev), GFP_ATOMIC);
if (skb == NULL)
return NULL;
skb_reserve(skb, LL_RESERVED_SPACE(dev));
skb_reset_network_header(skb);
arp = (struct arphdr *) skb_put(skb, arp_hdr_len(dev));
skb->dev = dev;
skb->protocol = htons(ETH_P_ARP);
if (src_hw == NULL)
src_hw = dev->dev_addr;
if (dest_hw == NULL)
dest_hw = dev->broadcast;
/*
* Fill the device header for the ARP frame
*/
if (dev_hard_header(skb, dev, ptype, dest_hw, src_hw, skb->len) < 0)
goto out;
/*
* Fill out the arp protocol part.
*
* The arp hardware type should match the device type, except for FDDI,
* which (according to RFC 1390) should always equal 1 (Ethernet).
*/
/*
* Exceptions everywhere. AX.25 uses the AX.25 PID value not the
* DIX code for the protocol. Make these device structure fields.
*/
switch (dev->type) {
default:
arp->ar_hrd = htons(dev->type);
arp->ar_pro = htons(ETH_P_IP);
break;
#if defined(CONFIG_AX25) || defined(CONFIG_AX25_MODULE)
case ARPHRD_AX25:
arp->ar_hrd = htons(ARPHRD_AX25);
arp->ar_pro = htons(AX25_P_IP);
break;
#if defined(CONFIG_NETROM) || defined(CONFIG_NETROM_MODULE)
case ARPHRD_NETROM:
arp->ar_hrd = htons(ARPHRD_NETROM);
arp->ar_pro = htons(AX25_P_IP);
break;
#endif
#endif
#ifdef CONFIG_FDDI
case ARPHRD_FDDI:
arp->ar_hrd = htons(ARPHRD_ETHER);
arp->ar_pro = htons(ETH_P_IP);
break;
#endif
#ifdef CONFIG_TR
case ARPHRD_IEEE802_TR:
arp->ar_hrd = htons(ARPHRD_IEEE802);
arp->ar_pro = htons(ETH_P_IP);
break;
#endif
}
arp->ar_hln = dev->addr_len;
arp->ar_pln = 4;
arp->ar_op = htons(type);
arp_ptr=(unsigned char *)(arp+1);
memcpy(arp_ptr, src_hw, dev->addr_len);
arp_ptr+=dev->addr_len;
memcpy(arp_ptr, &src_ip,4);
arp_ptr+=4;
if (target_hw != NULL)
memcpy(arp_ptr, target_hw, dev->addr_len);
else
memset(arp_ptr, 0, dev->addr_len);
arp_ptr+=dev->addr_len;
memcpy(arp_ptr, &dest_ip, 4);
return skb;
out:
kfree_skb(skb);
return NULL;
}
/*
* Stuff received packets to associated sockets.
* On error, returns non-zero and releases the skb.
*/
static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
struct packet_type *pkttype,
struct net_device *orig_dev)
{
struct net *net = dev_net(dev);
struct phonethdr *ph;
struct sockaddr_pn sa;
u16 len;
int i;
/* check we have at least a full Phonet header */
if (!pskb_pull(skb, sizeof(struct phonethdr)))
goto out;
/* check that the advertised length is correct */
ph = pn_hdr(skb);
len = get_unaligned_be16(&ph->pn_length);
if (len < 2)
goto out;
len -= 2;
if ((len > skb->len) || pskb_trim(skb, len))
goto out;
skb_reset_transport_header(skb);
pn_skb_get_dst_sockaddr(skb, &sa);
PN_PRINTK("PN rcv: hdr rdev %x sdev %x res %x robj %x sobj %x dev=%s\n",
ph->pn_rdev, ph->pn_sdev, ph->pn_res,
ph->pn_robj, ph->pn_sobj, dev->name);
PN_DATA_PRINTK("PHONET : skb data = %d\nPHONET :", skb->len);
for (i = 1; i <= skb->len; i++) {
PN_DATA_PRINTK(" %02x", skb->data[i-1]);
if ((i%8) == 0)
PN_DATA_PRINTK("\n");
}
/* check if this is multicasted */
if (pn_sockaddr_get_object(&sa) == PNOBJECT_MULTICAST) {
pn_deliver_sock_broadcast(net, skb);
goto out;
}
/* check if this is broadcasted */
if (pn_sockaddr_get_addr(&sa) == PNADDR_BROADCAST) {
pn_deliver_sock_broadcast(net, skb);
goto out;
}
/* resource routing */
if (pn_sockaddr_get_object(&sa) == 0) {
struct sock *sk = pn_find_sock_by_res(net, sa.spn_resource);
if (sk) {
printk(KERN_DEBUG "phonet new resource routing!\n");
return sk_receive_skb(sk, skb, 0);
}
}
/* check if we are the destination */
if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) {
/* Phonet packet input */
/*!*/ struct sock *sk = pn_find_sock_by_sa_and_skb(net, &sa, skb);
/*struct sock *sk = pn_find_sock_by_sa(net, &sa);*/
if (sk)
return sk_receive_skb(sk, skb, 0);
if (can_respond(skb)) {
send_obj_unreachable(skb);
send_reset_indications(skb);
}
} else if (unlikely(skb->pkt_type == PACKET_LOOPBACK))
goto out; /* Race between address deletion and loopback */
else {
/* Phonet packet routing */
struct net_device *out_dev;
out_dev = phonet_route_output(net, pn_sockaddr_get_addr(&sa));
if (!out_dev) {
LIMIT_NETDEBUG(KERN_WARNING"No Phonet route to %02X\n",
pn_sockaddr_get_addr(&sa));
goto out;
}
__skb_push(skb, sizeof(struct phonethdr));
skb->dev = out_dev;
if (out_dev == dev) {
LIMIT_NETDEBUG(KERN_ERR"Phonet loop to %02X on %s\n",
pn_sockaddr_get_addr(&sa), dev->name);
goto out_dev;
}
/* Some drivers (e.g. TUN) do not allocate HW header space */
if (skb_cow_head(skb, out_dev->hard_header_len))
goto out_dev;
if (dev_hard_header(skb, out_dev, ETH_P_PHONET, NULL, NULL,
skb->len) < 0)
goto out_dev;
dev_queue_xmit(skb);
dev_put(out_dev);
return NET_RX_SUCCESS;
out_dev:
dev_put(out_dev);
}
out:
kfree_skb(skb);
printk(KERN_DEBUG "phonet_rcv Drop message!\n");
return NET_RX_DROP;
}
struct sk_buff *arp_create(int type, int ptype, __be32 dest_ip,
struct net_device *dev, __be32 src_ip,
const unsigned char *dest_hw,
const unsigned char *src_hw,
const unsigned char *target_hw)
{
struct sk_buff *skb;
struct arphdr *arp;
unsigned char *arp_ptr;
int hlen = LL_RESERVED_SPACE(dev);
int tlen = dev->needed_tailroom;
skb = alloc_skb(arp_hdr_len(dev) + hlen + tlen, GFP_ATOMIC);
if (skb == NULL)
return NULL;
skb_reserve(skb, hlen);
skb_reset_network_header(skb);
arp = (struct arphdr *) skb_put(skb, arp_hdr_len(dev));
skb->dev = dev;
skb->protocol = htons(ETH_P_ARP);
if (src_hw == NULL)
src_hw = dev->dev_addr;
if (dest_hw == NULL)
dest_hw = dev->broadcast;
if (dev_hard_header(skb, dev, ptype, dest_hw, src_hw, skb->len) < 0)
goto out;
switch (dev->type) {
default:
arp->ar_hrd = htons(dev->type);
arp->ar_pro = htons(ETH_P_IP);
break;
#if IS_ENABLED(CONFIG_AX25)
case ARPHRD_AX25:
arp->ar_hrd = htons(ARPHRD_AX25);
arp->ar_pro = htons(AX25_P_IP);
break;
#if IS_ENABLED(CONFIG_NETROM)
case ARPHRD_NETROM:
arp->ar_hrd = htons(ARPHRD_NETROM);
arp->ar_pro = htons(AX25_P_IP);
break;
#endif
#endif
#if IS_ENABLED(CONFIG_FDDI)
case ARPHRD_FDDI:
arp->ar_hrd = htons(ARPHRD_ETHER);
arp->ar_pro = htons(ETH_P_IP);
break;
#endif
#if IS_ENABLED(CONFIG_TR)
case ARPHRD_IEEE802_TR:
arp->ar_hrd = htons(ARPHRD_IEEE802);
arp->ar_pro = htons(ETH_P_IP);
break;
#endif
}
arp->ar_hln = dev->addr_len;
arp->ar_pln = 4;
arp->ar_op = htons(type);
arp_ptr = (unsigned char *)(arp + 1);
memcpy(arp_ptr, src_hw, dev->addr_len);
arp_ptr += dev->addr_len;
memcpy(arp_ptr, &src_ip, 4);
arp_ptr += 4;
if (target_hw != NULL)
memcpy(arp_ptr, target_hw, dev->addr_len);
else
memset(arp_ptr, 0, dev->addr_len);
arp_ptr += dev->addr_len;
memcpy(arp_ptr, &dest_ip, 4);
return skb;
out:
kfree_skb(skb);
return NULL;
}
static void send_hsr_supervision_frame(struct hsr_port *master,
u8 type, u8 hsrVer)
{
struct sk_buff *skb;
int hlen, tlen;
struct hsr_tag *hsr_tag;
struct hsr_sup_tag *hsr_stag;
struct hsr_sup_payload *hsr_sp;
unsigned long irqflags;
hlen = LL_RESERVED_SPACE(master->dev);
tlen = master->dev->needed_tailroom;
skb = dev_alloc_skb(
sizeof(struct hsr_tag) +
sizeof(struct hsr_sup_tag) +
sizeof(struct hsr_sup_payload) + hlen + tlen);
if (skb == NULL)
return;
skb_reserve(skb, hlen);
skb->dev = master->dev;
skb->protocol = htons(hsrVer ? ETH_P_HSR : ETH_P_PRP);
skb->priority = TC_PRIO_CONTROL;
if (dev_hard_header(skb, skb->dev, (hsrVer ? ETH_P_HSR : ETH_P_PRP),
master->hsr->sup_multicast_addr,
skb->dev->dev_addr, skb->len) <= 0)
goto out;
skb_reset_mac_header(skb);
if (hsrVer > 0) {
hsr_tag = (typeof(hsr_tag)) skb_put(skb, sizeof(struct hsr_tag));
hsr_tag->encap_proto = htons(ETH_P_PRP);
set_hsr_tag_LSDU_size(hsr_tag, HSR_V1_SUP_LSDUSIZE);
}
hsr_stag = (typeof(hsr_stag)) skb_put(skb, sizeof(struct hsr_sup_tag));
set_hsr_stag_path(hsr_stag, (hsrVer ? 0x0 : 0xf));
set_hsr_stag_HSR_Ver(hsr_stag, hsrVer);
/* From HSRv1 on we have separate supervision sequence numbers. */
spin_lock_irqsave(&master->hsr->seqnr_lock, irqflags);
if (hsrVer > 0) {
hsr_stag->sequence_nr = htons(master->hsr->sup_sequence_nr);
hsr_tag->sequence_nr = htons(master->hsr->sequence_nr);
master->hsr->sup_sequence_nr++;
master->hsr->sequence_nr++;
} else {
hsr_stag->sequence_nr = htons(master->hsr->sequence_nr);
master->hsr->sequence_nr++;
}
spin_unlock_irqrestore(&master->hsr->seqnr_lock, irqflags);
hsr_stag->HSR_TLV_Type = type;
/* TODO: Why 12 in HSRv0? */
hsr_stag->HSR_TLV_Length = hsrVer ? sizeof(struct hsr_sup_payload) : 12;
/* Payload: MacAddressA */
hsr_sp = (typeof(hsr_sp)) skb_put(skb, sizeof(struct hsr_sup_payload));
ether_addr_copy(hsr_sp->MacAddressA, master->dev->dev_addr);
skb_put_padto(skb, ETH_ZLEN + HSR_HLEN);
hsr_forward_skb(skb, master);
return;
out:
WARN_ONCE(1, "HSR: Could not send supervision frame\n");
kfree_skb(skb);
}
/*
* Stuff received packets to associated sockets.
* On error, returns non-zero and releases the skb.
*/
static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
struct packet_type *pkttype,
struct net_device *orig_dev)
{
struct net *net = dev_net(dev);
struct phonethdr *ph;
struct sockaddr_pn sa;
u16 len;
/* check we have at least a full Phonet header */
if (!pskb_pull(skb, sizeof(struct phonethdr)))
goto out;
/* check that the advertised length is correct */
ph = pn_hdr(skb);
len = get_unaligned_be16(&ph->pn_length);
if (len < 2)
goto out;
len -= 2;
if ((len > skb->len) || pskb_trim(skb, len))
goto out;
skb_reset_transport_header(skb);
pn_skb_get_dst_sockaddr(skb, &sa);
/* check if this is broadcasted */
if (pn_sockaddr_get_addr(&sa) == PNADDR_BROADCAST) {
pn_deliver_sock_broadcast(net, skb);
goto out;
}
/* check if we are the destination */
if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) {
/* Phonet packet input */
struct sock *sk = pn_find_sock_by_sa(net, &sa);
if (sk)
return sk_receive_skb(sk, skb, 0);
if (can_respond(skb)) {
send_obj_unreachable(skb);
send_reset_indications(skb);
}
} else if (unlikely(skb->pkt_type == PACKET_LOOPBACK))
goto out; /* Race between address deletion and loopback */
else {
/* Phonet packet routing */
struct net_device *out_dev;
out_dev = phonet_route_output(net, pn_sockaddr_get_addr(&sa));
if (!out_dev) {
LIMIT_NETDEBUG(KERN_WARNING"No Phonet route to %02X\n",
pn_sockaddr_get_addr(&sa));
goto out;
}
__skb_push(skb, sizeof(struct phonethdr));
skb->dev = out_dev;
if (out_dev == dev) {
LIMIT_NETDEBUG(KERN_ERR"Phonet loop to %02X on %s\n",
pn_sockaddr_get_addr(&sa), dev->name);
goto out_dev;
}
/* Some drivers (e.g. TUN) do not allocate HW header space */
if (skb_cow_head(skb, out_dev->hard_header_len))
goto out_dev;
if (dev_hard_header(skb, out_dev, ETH_P_PHONET, NULL, NULL,
skb->len) < 0)
goto out_dev;
dev_queue_xmit(skb);
dev_put(out_dev);
return NET_RX_SUCCESS;
out_dev:
dev_put(out_dev);
}
out:
kfree_skb(skb);
return NET_RX_DROP;
}
/*
* Prepends an ISI header and sends a datagram.
*/
static int pn_send(struct sk_buff *skb, struct net_device *dev,
u16 dst, u16 src, u8 res, u8 irq)
{
struct phonethdr *ph;
int err, i;
if (skb->len + 2 > 0xffff /* Phonet length field limit */ ||
skb->len + sizeof(struct phonethdr) > dev->mtu) {
err = -EMSGSIZE;
goto drop;
}
/* Broadcast sending is not implemented */
if (pn_addr(dst) == PNADDR_BROADCAST) {
err = -EOPNOTSUPP;
goto drop;
}
skb_reset_transport_header(skb);
WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */
skb_push(skb, sizeof(struct phonethdr));
skb_reset_network_header(skb);
ph = pn_hdr(skb);
ph->pn_rdev = pn_dev(dst);
ph->pn_sdev = pn_dev(src);
ph->pn_res = res;
ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph));
ph->pn_robj = pn_obj(dst);
ph->pn_sobj = pn_obj(src);
skb->protocol = htons(ETH_P_PHONET);
skb->priority = 0;
skb->dev = dev;
PN_PRINTK("pn_send rdev %x sdev %x res %x robj %x sobj %x netdev=%s\n",
ph->pn_rdev, ph->pn_sdev, ph->pn_res,
ph->pn_robj, ph->pn_sobj, dev->name);
PN_DATA_PRINTK("PHONET : skb data = %d\nPHONET :", skb->len);
for (i = 1; i <= skb->len; i++) {
PN_DATA_PRINTK(" %02x", skb->data[i-1]);
if ((i%8) == 0)
PN_DATA_PRINTK("\n");
}
if (skb->pkt_type == PACKET_LOOPBACK) {
skb_reset_mac_header(skb);
skb_orphan(skb);
err = (irq ? netif_rx(skb) : netif_rx_ni(skb)) ? -ENOBUFS : 0;
} else {
err = dev_hard_header(skb, dev, ntohs(skb->protocol),
NULL, NULL, skb->len);
if (err < 0) {
err = -EHOSTUNREACH;
goto drop;
}
err = dev_queue_xmit(skb);
if (unlikely(err > 0))
err = net_xmit_errno(err);
}
return err;
drop:
printk(KERN_DEBUG "pn_send DROP\n");
kfree_skb(skb);
return err;
}
/* Handle linearized sk_buff post_routing */
static unsigned int post_routing_process(const struct nf_hook_ops *ops,
struct sk_buff *skb,
const struct net_device *in,
const struct net_device *out)
{
char* ip_pkt;
char* nip_pkt;
int ip_pkt_len, nip_pkt_len;
int eth_vlan_hdr_len, full_pkt_len, ip_hdr_len;
int err;
struct sk_buff *nskb;
struct ethhdr *eth_hdr;
struct flow_keys flow_key;
struct iphdr *ip_hdr;
struct iphdr *nip_hdr;
struct stack stk;
void *saddr;
void *daddr;
__be16 proto;
u32 hash;
__wsum nskb_csum = 0;
unsigned char dst[] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
/*
if(skb_is_nonlinear(skb)){
pr_debug("Non-linear skb.. linearizing...\n");
if(skb_linearize(skb)){
pr_debug("Failed to serialize!\n");
}
}
*/
if (skb_is_nonlinear(skb)){
pr_debug("Proces_pkt: Still non-linear skb.\n");
return NF_ACCEPT;
}
proto = ntohs(skb->protocol);
pr_debug("Proto: %04x\n", proto);
switch (proto) {
case ETH_P_IP:
eth_hdr = (struct ethhdr *) skb_mac_header(skb); // need to set
ip_hdr = (struct iphdr *) skb_network_header(skb);
ip_hdr_len = ip_hdrlen(skb);
ip_pkt = (char *) ip_hdr;
ip_pkt_len = ntohs(ip_hdr->tot_len);
pr_debug("IP pkt_len = %d IP hdr len = %d \n", ip_pkt_len,
ip_hdr_len);
if(skb_flow_dissect(skb, &flow_key)){
print_ip(flow_key.src);
print_ip(flow_key.dst);
pr_debug("Ports %d %d\n", ntohs(flow_key.port16[0]),
ntohs(flow_key.port16[1]));
hash = flow_keys_hash(flow_key);
pr_debug("Hash: %x\n", hash);
}
else{
pr_debug("Failed to dissect flow\n");
return NF_ACCEPT;
}
//stats_entry_inc(be32_to_cpu(flow_key.dst), ip_pkt_len);
pr_debug("Proto: IP pkt\nGetting stk from flow_table for %u\n",
be32_to_cpu(flow_key.dst));
stk = flow_table_get(flow_table, flow_key, routing_table, be32_to_cpu(flow_key.dst));
if(stk.num_tags == -1) { // no_stack
pr_debug("flow_table miss! consulting rt_table\n");
stk = get_random_stack_for_dst(be32_to_cpu(flow_key.dst), routing_table);
flow_table_set(flow_table, flow_key, stk);
}
if(stk.num_tags < 0) {
stk.num_tags=0;
}
eth_vlan_hdr_len = ETH_HLEN + stk.num_tags * sizeof(vlan_label);
full_pkt_len = eth_vlan_hdr_len + ip_pkt_len;
pr_debug("Full length: %d", full_pkt_len);
// Allocate new skb
nskb = alloc_skb(full_pkt_len, GFP_ATOMIC);
if (nskb == NULL) {
return NF_ACCEPT;
}
if (skb->sk != NULL) {
skb_set_owner_w(nskb, skb->sk);
}
else {
kfree_skb(nskb);
return NF_ACCEPT;
}
pr_debug("mod_vlan: nskb - Reserving header\n");
// Reserve space for eth and vlan headers
skb_reserve(nskb, eth_vlan_hdr_len);
// Copy IP packet
pr_debug("mod_vlan: copying IP pkt.\n");
if (!(nip_pkt = skb_put(nskb, ip_pkt_len))) {
pr_debug("skb_put failed!\n");
kfree_skb(nskb);
return NF_ACCEPT;
}
skb_reset_network_header(nskb);
memcpy(nip_pkt, ip_pkt, ip_pkt_len);
nip_hdr = (struct iphdr *) nip_pkt;
nip_pkt_len = ntohs(nip_hdr->tot_len);
nskb_csum = fix_csum(nip_hdr);
// Set VLAN stack
if (set_vlan_stack(nskb, &stk)) {
proto = ETH_P_8021Q;
}
// Get outgoing interface
nskb->dev = dev_get_by_name(&init_net, out->name);
if (!nskb->dev) {
pr_debug("mod_vlan dev_get_by_name (%s) FAILED.", out->name);
kfree_skb(nskb);
return NF_ACCEPT;
}
/*
// Reduce MTU, if needed
if (nskb->dev->mtu > 1500 - (4 * stk.num_tags)) {
pr_debug("Setting MTU: (%s) %u", out->name,
1500 - (4 * stk.num_tags));
nskb->dev->mtu = 1500 - (4 * stk.num_tags);
}
pr_debug("mod_vlan dev_get_by_name success, nskb->dev->name='%s'",
nskb->dev->name);
*/
saddr = nskb->dev->dev_addr;
daddr = dst;
// ARP Lookup
if (get_dst_haddr(daddr, flow_key.dst, nskb->dev) != 0){
pr_debug("ARP lookup - FAILED!\n");
kfree_skb(nskb);
return NF_ACCEPT;
}
// Set DL header
print_mac(saddr);
print_mac(daddr);
pr_debug("calling dev_hard_header\n");
if (!dev_hard_header(nskb, nskb->dev, proto,
daddr, saddr, nskb->dev->addr_len)) {
pr_debug("mod_vlan dev_hard_header FAILED.\n");
kfree_skb(nskb);
return NF_ACCEPT;
}
skb_reset_mac_header(nskb);
// Set skb checksum
nskb->csum = nskb_csum;
// Send out packet - dev_queue_xmit will consume nskb
pr_debug("mod_vlan: sending nskb....\n");
if ((err = dev_queue_xmit(nskb)) != NET_XMIT_SUCCESS) {
pr_debug("mod_vlan dev_queue_xmit failed. %d\n", err);
return NF_ACCEPT;
}
// Consume original skb
consume_skb(skb);
pr_debug("------ success - returning ------\n");
return NF_STOLEN;
break;
default:
pr_debug("Proto: Non-IP pkt\n");
break;
}
// default: if we didn't send a new skb, then accept the original
return NF_ACCEPT;
}
