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>
This commit is contained in:
Remi Denis-Courmont
David S. Miller
parent
ba113a94b7
commit
107d0d9b8d
|
|
@ -51,6 +51,9 @@ void pn_sock_hash(struct sock *sk);
|
|||
void pn_sock_unhash(struct sock *sk);
|
||||
int pn_sock_get_port(struct sock *sk, unsigned short sport);
|
||||
|
||||
int pn_skb_send(struct sock *sk, struct sk_buff *skb,
|
||||
const struct sockaddr_pn *target);
|
||||
|
||||
static inline struct phonethdr *pn_hdr(struct sk_buff *skb)
|
||||
{
|
||||
return (struct phonethdr *)skb_network_header(skb);
|
||||
|
|
@ -95,4 +98,7 @@ int phonet_proto_register(int protocol, struct phonet_protocol *pp);
|
|||
void phonet_proto_unregister(int protocol, struct phonet_protocol *pp);
|
||||
|
||||
void phonet_netlink_register(void);
|
||||
int isi_register(void);
|
||||
void isi_unregister(void);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -4,4 +4,5 @@ phonet-objs := \
|
|||
pn_dev.o \
|
||||
pn_netlink.o \
|
||||
socket.o \
|
||||
datagram.o \
|
||||
af_phonet.o
|
||||
|
|
|
|||
|
|
@ -99,6 +99,101 @@ static struct net_proto_family phonet_proto_family = {
|
|||
.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 @@ static int __init phonet_init(void)
|
|||
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();
|
||||
|
|
|
|||
|
|
@ -0,0 +1,197 @@
|
|||
/*
|
||||
* File: datagram.c
|
||||
*
|
||||
* Datagram (ISI) Phonet sockets
|
||||
*
|
||||
* Copyright (C) 2008 Nokia Corporation.
|
||||
*
|
||||
* Contact: Remi Denis-Courmont <remi.denis-courmont@nokia.com>
|
||||
* Original author: Sakari Ailus <sakari.ailus@nokia.com>
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU General Public License
|
||||
* version 2 as published by the Free Software Foundation.
|
||||
*
|
||||
* 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., 51 Franklin St, Fifth Floor, Boston, MA
|
||||
* 02110-1301 USA
|
||||
*/
|
||||
|
||||
#include <linux/kernel.h>
|
||||
#include <linux/socket.h>
|
||||
#include <asm/ioctls.h>
|
||||
#include <net/sock.h>
|
||||
|
||||
#include <linux/phonet.h>
|
||||
#include <net/phonet/phonet.h>
|
||||
|
||||
static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb);
|
||||
|
||||
/* associated socket ceases to exist */
|
||||
static void pn_sock_close(struct sock *sk, long timeout)
|
||||
{
|
||||
sk_common_release(sk);
|
||||
}
|
||||
|
||||
static int pn_ioctl(struct sock *sk, int cmd, unsigned long arg)
|
||||
{
|
||||
struct sk_buff *skb;
|
||||
int answ;
|
||||
|
||||
switch (cmd) {
|
||||
case SIOCINQ:
|
||||
lock_sock(sk);
|
||||
skb = skb_peek(&sk->sk_receive_queue);
|
||||
answ = skb ? skb->len : 0;
|
||||
release_sock(sk);
|
||||
return put_user(answ, (int __user *)arg);
|
||||
}
|
||||
|
||||
return -ENOIOCTLCMD;
|
||||
}
|
||||
|
||||
/* Destroy socket. All references are gone. */
|
||||
static void pn_destruct(struct sock *sk)
|
||||
{
|
||||
skb_queue_purge(&sk->sk_receive_queue);
|
||||
}
|
||||
|
||||
static int pn_init(struct sock *sk)
|
||||
{
|
||||
sk->sk_destruct = pn_destruct;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int pn_sendmsg(struct kiocb *iocb, struct sock *sk,
|
||||
struct msghdr *msg, size_t len)
|
||||
{
|
||||
struct sockaddr_pn *target;
|
||||
struct sk_buff *skb;
|
||||
int err;
|
||||
|
||||
if (msg->msg_flags & MSG_OOB)
|
||||
return -EOPNOTSUPP;
|
||||
|
||||
if (msg->msg_name == NULL)
|
||||
return -EDESTADDRREQ;
|
||||
|
||||
if (msg->msg_namelen < sizeof(struct sockaddr_pn))
|
||||
return -EINVAL;
|
||||
|
||||
target = (struct sockaddr_pn *)msg->msg_name;
|
||||
if (target->spn_family != AF_PHONET)
|
||||
return -EAFNOSUPPORT;
|
||||
|
||||
skb = sock_alloc_send_skb(sk, MAX_PHONET_HEADER + len,
|
||||
msg->msg_flags & MSG_DONTWAIT, &err);
|
||||
if (skb == NULL)
|
||||
return err;
|
||||
skb_reserve(skb, MAX_PHONET_HEADER);
|
||||
|
||||
err = memcpy_fromiovec((void *)skb_put(skb, len), msg->msg_iov, len);
|
||||
if (err < 0) {
|
||||
kfree_skb(skb);
|
||||
return err;
|
||||
}
|
||||
|
||||
/*
|
||||
* Fill in the Phonet header and
|
||||
* finally pass the packet forwards.
|
||||
*/
|
||||
err = pn_skb_send(sk, skb, target);
|
||||
|
||||
/* If ok, return len. */
|
||||
return (err >= 0) ? len : err;
|
||||
}
|
||||
|
||||
static int pn_recvmsg(struct kiocb *iocb, struct sock *sk,
|
||||
struct msghdr *msg, size_t len, int noblock,
|
||||
int flags, int *addr_len)
|
||||
{
|
||||
struct sk_buff *skb = NULL;
|
||||
struct sockaddr_pn sa;
|
||||
int rval = -EOPNOTSUPP;
|
||||
int copylen;
|
||||
|
||||
if (flags & MSG_OOB)
|
||||
goto out_nofree;
|
||||
|
||||
if (addr_len)
|
||||
*addr_len = sizeof(sa);
|
||||
|
||||
skb = skb_recv_datagram(sk, flags, noblock, &rval);
|
||||
if (skb == NULL)
|
||||
goto out_nofree;
|
||||
|
||||
pn_skb_get_src_sockaddr(skb, &sa);
|
||||
|
||||
copylen = skb->len;
|
||||
if (len < copylen) {
|
||||
msg->msg_flags |= MSG_TRUNC;
|
||||
copylen = len;
|
||||
}
|
||||
|
||||
rval = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copylen);
|
||||
if (rval) {
|
||||
rval = -EFAULT;
|
||||
goto out;
|
||||
}
|
||||
|
||||
rval = (flags & MSG_TRUNC) ? skb->len : copylen;
|
||||
|
||||
if (msg->msg_name != NULL)
|
||||
memcpy(msg->msg_name, &sa, sizeof(struct sockaddr_pn));
|
||||
|
||||
out:
|
||||
skb_free_datagram(sk, skb);
|
||||
|
||||
out_nofree:
|
||||
return rval;
|
||||
}
|
||||
|
||||
/* Queue an skb for a sock. */
|
||||
static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb)
|
||||
{
|
||||
int err = sock_queue_rcv_skb(sk, skb);
|
||||
if (err < 0)
|
||||
kfree_skb(skb);
|
||||
return err ? NET_RX_DROP : NET_RX_SUCCESS;
|
||||
}
|
||||
|
||||
/* Module registration */
|
||||
static struct proto pn_proto = {
|
||||
.close = pn_sock_close,
|
||||
.ioctl = pn_ioctl,
|
||||
.init = pn_init,
|
||||
.sendmsg = pn_sendmsg,
|
||||
.recvmsg = pn_recvmsg,
|
||||
.backlog_rcv = pn_backlog_rcv,
|
||||
.hash = pn_sock_hash,
|
||||
.unhash = pn_sock_unhash,
|
||||
.get_port = pn_sock_get_port,
|
||||
.obj_size = sizeof(struct pn_sock),
|
||||
.owner = THIS_MODULE,
|
||||
.name = "PHONET",
|
||||
};
|
||||
|
||||
static struct phonet_protocol pn_dgram_proto = {
|
||||
.ops = &phonet_dgram_ops,
|
||||
.prot = &pn_proto,
|
||||
.sock_type = SOCK_DGRAM,
|
||||
};
|
||||
|
||||
int __init isi_register(void)
|
||||
{
|
||||
return phonet_proto_register(PN_PROTO_PHONET, &pn_dgram_proto);
|
||||
}
|
||||
|
||||
void __exit isi_unregister(void)
|
||||
{
|
||||
phonet_proto_unregister(PN_PROTO_PHONET, &pn_dgram_proto);
|
||||
}
|
||||
Reference in New Issue