/*
* Create a Phonet header for the skb and send it out. Returns
* non-zero error code if failed. The skb is freed then.
*/
int pn_skb_send(struct sock *sk, struct sk_buff *skb,
const struct sockaddr_pn *target)
{
struct net *net = sock_net(sk);
struct net_device *dev;
struct pn_sock *pn = pn_sk(sk);
int err;
u16 src, dst;
u8 daddr, saddr, res;
src = pn->sobject;
if (target != NULL) {
dst = pn_sockaddr_get_object(target);
res = pn_sockaddr_get_resource(target);
} else {
dst = pn->dobject;
res = pn->resource;
}
daddr = pn_addr(dst);
err = -EHOSTUNREACH;
if (sk->sk_bound_dev_if)
dev = dev_get_by_index(net, sk->sk_bound_dev_if);
else if (phonet_address_lookup(net, daddr) == 0) {
dev = phonet_device_get(net);
skb->pkt_type = PACKET_LOOPBACK;
} else if (dst == 0) {
/* Resource routing (small race until phonet_rcv()) */
struct sock *sk = pn_find_sock_by_res(net, res);
if (sk) {
sock_put(sk);
dev = phonet_device_get(net);
skb->pkt_type = PACKET_LOOPBACK;
} else
dev = phonet_route_output(net, daddr);
} else
dev = phonet_route_output(net, daddr);
if (!dev || !(dev->flags & IFF_UP))
goto drop;
saddr = phonet_address_get(dev, daddr);
if (saddr == PN_NO_ADDR)
goto drop;
if (!pn_addr(src))
src = pn_object(saddr, pn_obj(src));
err = pn_send(skb, dev, dst, src, res, 0);
dev_put(dev);
return err;
drop:
printk(KERN_DEBUG "pn_skb_send DROP\n");
kfree_skb(skb);
if (dev)
dev_put(dev);
return err;
}
/*
* Create a Phonet header for the skb and send it out. Returns
* non-zero error code if failed. The skb is freed then.
*/
int pn_skb_send(struct sock *sk, struct sk_buff *skb,
const struct sockaddr_pn *target)
{
struct net *net = sock_net(sk);
struct net_device *dev;
struct pn_sock *pn = pn_sk(sk);
int err;
u16 src;
u8 daddr = pn_sockaddr_get_addr(target), saddr = PN_NO_ADDR;
err = -EHOSTUNREACH;
if (sk->sk_bound_dev_if)
dev = dev_get_by_index(net, sk->sk_bound_dev_if);
else if (phonet_address_lookup(net, daddr) == 0) {
dev = phonet_device_get(net);
skb->pkt_type = PACKET_LOOPBACK;
} else
dev = phonet_route_output(net, daddr);
if (!dev || !(dev->flags & IFF_UP))
goto drop;
saddr = phonet_address_get(dev, daddr);
if (saddr == PN_NO_ADDR)
goto drop;
src = pn->sobject;
if (!pn_addr(src))
src = pn_object(saddr, pn_obj(src));
err = pn_send(skb, dev, pn_sockaddr_get_object(target),
src, pn_sockaddr_get_resource(target), 0);
dev_put(dev);
return err;
drop:
kfree_skb(skb);
if (dev)
dev_put(dev);
return err;
}
/*
* 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;
}
/*
* 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;
}
