Phonet: Phonet datagram transport protocol
This provides the basic SOCK_DGRAM transport protocol for Phonet.
Signed-off-by: Remi Denis-Courmont <remi.denis-courmont@nokia.com>
Signed-off-by: David S. Miller <davem@davemloft.net>
diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c
index ba54d53..e6771d3 100644
--- a/net/phonet/af_phonet.c
+++ b/net/phonet/af_phonet.c
@@ -99,6 +99,101 @@
.owner = THIS_MODULE,
};
+/*
+ * 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)
+{
+ struct phonethdr *ph;
+ int err;
+
+ if (skb->len + 2 > 0xffff) {
+ /* Phonet length field would overflow */
+ err = -EMSGSIZE;
+ 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);
+ 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;
+}
+
+/*
+ * 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_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(sock_net(sk), sk->sk_bound_dev_if);
+ else
+ dev = phonet_device_get(sock_net(sk));
+ 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));
+ dev_put(dev);
+ return err;
+
+drop:
+ kfree_skb(skb);
+ if (dev)
+ dev_put(dev);
+ return err;
+}
+EXPORT_SYMBOL(pn_skb_send);
+
/* packet type functions */
/*
@@ -226,11 +321,22 @@
phonet_device_init();
dev_add_pack(&phonet_packet_type);
phonet_netlink_register();
+
+ err = isi_register();
+ if (err)
+ goto err;
return 0;
+
+err:
+ sock_unregister(AF_PHONET);
+ dev_remove_pack(&phonet_packet_type);
+ phonet_device_exit();
+ return err;
}
static void __exit phonet_exit(void)
{
+ isi_unregister();
sock_unregister(AF_PHONET);
dev_remove_pack(&phonet_packet_type);
phonet_device_exit();