aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--include/net/phonet/phonet.h6
-rw-r--r--net/phonet/Makefile1
-rw-r--r--net/phonet/af_phonet.c106
-rw-r--r--net/phonet/datagram.c197
4 files changed, 310 insertions, 0 deletions
diff --git a/include/net/phonet/phonet.h b/include/net/phonet/phonet.h
index 2ae5cbb59b60..d3957d3be0fd 100644
--- a/include/net/phonet/phonet.h
+++ b/include/net/phonet/phonet.h
@@ -51,6 +51,9 @@ void pn_sock_hash(struct sock *sk);
51void pn_sock_unhash(struct sock *sk); 51void pn_sock_unhash(struct sock *sk);
52int pn_sock_get_port(struct sock *sk, unsigned short sport); 52int pn_sock_get_port(struct sock *sk, unsigned short sport);
53 53
54int pn_skb_send(struct sock *sk, struct sk_buff *skb,
55 const struct sockaddr_pn *target);
56
54static inline struct phonethdr *pn_hdr(struct sk_buff *skb) 57static inline struct phonethdr *pn_hdr(struct sk_buff *skb)
55{ 58{
56 return (struct phonethdr *)skb_network_header(skb); 59 return (struct phonethdr *)skb_network_header(skb);
@@ -95,4 +98,7 @@ int phonet_proto_register(int protocol, struct phonet_protocol *pp);
95void phonet_proto_unregister(int protocol, struct phonet_protocol *pp); 98void phonet_proto_unregister(int protocol, struct phonet_protocol *pp);
96 99
97void phonet_netlink_register(void); 100void phonet_netlink_register(void);
101int isi_register(void);
102void isi_unregister(void);
103
98#endif 104#endif
diff --git a/net/phonet/Makefile b/net/phonet/Makefile
index c1d671de7835..d218abc3f06a 100644
--- a/net/phonet/Makefile
+++ b/net/phonet/Makefile
@@ -4,4 +4,5 @@ phonet-objs := \
4 pn_dev.o \ 4 pn_dev.o \
5 pn_netlink.o \ 5 pn_netlink.o \
6 socket.o \ 6 socket.o \
7 datagram.o \
7 af_phonet.o 8 af_phonet.o
diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c
index ba54d53020ff..e6771d3961cf 100644
--- a/net/phonet/af_phonet.c
+++ b/net/phonet/af_phonet.c
@@ -99,6 +99,101 @@ static struct net_proto_family phonet_proto_family = {
99 .owner = THIS_MODULE, 99 .owner = THIS_MODULE,
100}; 100};
101 101
102/*
103 * Prepends an ISI header and sends a datagram.
104 */
105static int pn_send(struct sk_buff *skb, struct net_device *dev,
106 u16 dst, u16 src, u8 res)
107{
108 struct phonethdr *ph;
109 int err;
110
111 if (skb->len + 2 > 0xffff) {
112 /* Phonet length field would overflow */
113 err = -EMSGSIZE;
114 goto drop;
115 }
116
117 skb_reset_transport_header(skb);
118 WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */
119 skb_push(skb, sizeof(struct phonethdr));
120 skb_reset_network_header(skb);
121 ph = pn_hdr(skb);
122 ph->pn_rdev = pn_dev(dst);
123 ph->pn_sdev = pn_dev(src);
124 ph->pn_res = res;
125 ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph));
126 ph->pn_robj = pn_obj(dst);
127 ph->pn_sobj = pn_obj(src);
128
129 skb->protocol = htons(ETH_P_PHONET);
130 skb->priority = 0;
131 skb->dev = dev;
132
133 if (pn_addr(src) == pn_addr(dst)) {
134 skb_reset_mac_header(skb);
135 skb->pkt_type = PACKET_LOOPBACK;
136 skb_orphan(skb);
137 netif_rx_ni(skb);
138 err = 0;
139 } else {
140 err = dev_hard_header(skb, dev, ntohs(skb->protocol),
141 NULL, NULL, skb->len);
142 if (err < 0) {
143 err = -EHOSTUNREACH;
144 goto drop;
145 }
146 err = dev_queue_xmit(skb);
147 }
148
149 return err;
150drop:
151 kfree_skb(skb);
152 return err;
153}
154
155/*
156 * Create a Phonet header for the skb and send it out. Returns
157 * non-zero error code if failed. The skb is freed then.
158 */
159int pn_skb_send(struct sock *sk, struct sk_buff *skb,
160 const struct sockaddr_pn *target)
161{
162 struct net_device *dev;
163 struct pn_sock *pn = pn_sk(sk);
164 int err;
165 u16 src;
166 u8 daddr = pn_sockaddr_get_addr(target), saddr = PN_NO_ADDR;
167
168 err = -EHOSTUNREACH;
169 if (sk->sk_bound_dev_if)
170 dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if);
171 else
172 dev = phonet_device_get(sock_net(sk));
173 if (!dev || !(dev->flags & IFF_UP))
174 goto drop;
175
176 saddr = phonet_address_get(dev, daddr);
177 if (saddr == PN_NO_ADDR)
178 goto drop;
179
180 src = pn->sobject;
181 if (!pn_addr(src))
182 src = pn_object(saddr, pn_obj(src));
183
184 err = pn_send(skb, dev, pn_sockaddr_get_object(target),
185 src, pn_sockaddr_get_resource(target));
186 dev_put(dev);
187 return err;
188
189drop:
190 kfree_skb(skb);
191 if (dev)
192 dev_put(dev);
193 return err;
194}
195EXPORT_SYMBOL(pn_skb_send);
196
102/* packet type functions */ 197/* packet type functions */
103 198
104/* 199/*
@@ -226,11 +321,22 @@ static int __init phonet_init(void)
226 phonet_device_init(); 321 phonet_device_init();
227 dev_add_pack(&phonet_packet_type); 322 dev_add_pack(&phonet_packet_type);
228 phonet_netlink_register(); 323 phonet_netlink_register();
324
325 err = isi_register();
326 if (err)
327 goto err;
229 return 0; 328 return 0;
329
330err:
331 sock_unregister(AF_PHONET);
332 dev_remove_pack(&phonet_packet_type);
333 phonet_device_exit();
334 return err;
230} 335}
231 336
232static void __exit phonet_exit(void) 337static void __exit phonet_exit(void)
233{ 338{
339 isi_unregister();
234 sock_unregister(AF_PHONET); 340 sock_unregister(AF_PHONET);
235 dev_remove_pack(&phonet_packet_type); 341 dev_remove_pack(&phonet_packet_type);
236 phonet_device_exit(); 342 phonet_device_exit();
diff --git a/net/phonet/datagram.c b/net/phonet/datagram.c
new file mode 100644
index 000000000000..e087862ed7e4
--- /dev/null
+++ b/net/phonet/datagram.c
@@ -0,0 +1,197 @@
1/*
2 * File: datagram.c
3 *
4 * Datagram (ISI) Phonet sockets
5 *
6 * Copyright (C) 2008 Nokia Corporation.
7 *
8 * Contact: Remi Denis-Courmont <remi.denis-courmont@nokia.com>
9 * Original author: Sakari Ailus <sakari.ailus@nokia.com>
10 *
11 * This program is free software; you can redistribute it and/or
12 * modify it under the terms of the GNU General Public License
13 * version 2 as published by the Free Software Foundation.
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 * General Public License for more details.
19 *
20 * You should have received a copy of the GNU General Public License
21 * along with this program; if not, write to the Free Software
22 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
23 * 02110-1301 USA
24 */
25
26#include <linux/kernel.h>
27#include <linux/socket.h>
28#include <asm/ioctls.h>
29#include <net/sock.h>
30
31#include <linux/phonet.h>
32#include <net/phonet/phonet.h>
33
34static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb);
35
36/* associated socket ceases to exist */
37static void pn_sock_close(struct sock *sk, long timeout)
38{
39 sk_common_release(sk);
40}
41
42static int pn_ioctl(struct sock *sk, int cmd, unsigned long arg)
43{
44 struct sk_buff *skb;
45 int answ;
46
47 switch (cmd) {
48 case SIOCINQ:
49 lock_sock(sk);
50 skb = skb_peek(&sk->sk_receive_queue);
51 answ = skb ? skb->len : 0;
52 release_sock(sk);
53 return put_user(answ, (int __user *)arg);
54 }
55
56 return -ENOIOCTLCMD;
57}
58
59/* Destroy socket. All references are gone. */
60static void pn_destruct(struct sock *sk)
61{
62 skb_queue_purge(&sk->sk_receive_queue);
63}
64
65static int pn_init(struct sock *sk)
66{
67 sk->sk_destruct = pn_destruct;
68 return 0;
69}
70
71static int pn_sendmsg(struct kiocb *iocb, struct sock *sk,
72 struct msghdr *msg, size_t len)
73{
74 struct sockaddr_pn *target;
75 struct sk_buff *skb;
76 int err;
77
78 if (msg->msg_flags & MSG_OOB)
79 return -EOPNOTSUPP;
80
81 if (msg->msg_name == NULL)
82 return -EDESTADDRREQ;
83
84 if (msg->msg_namelen < sizeof(struct sockaddr_pn))
85 return -EINVAL;
86
87 target = (struct sockaddr_pn *)msg->msg_name;
88 if (target->spn_family != AF_PHONET)
89 return -EAFNOSUPPORT;
90
91 skb = sock_alloc_send_skb(sk, MAX_PHONET_HEADER + len,
92 msg->msg_flags & MSG_DONTWAIT, &err);
93 if (skb == NULL)
94 return err;
95 skb_reserve(skb, MAX_PHONET_HEADER);
96
97 err = memcpy_fromiovec((void *)skb_put(skb, len), msg->msg_iov, len);
98 if (err < 0) {
99 kfree_skb(skb);
100 return err;
101 }
102
103 /*
104 * Fill in the Phonet header and
105 * finally pass the packet forwards.
106 */
107 err = pn_skb_send(sk, skb, target);
108
109 /* If ok, return len. */
110 return (err >= 0) ? len : err;
111}
112
113static int pn_recvmsg(struct kiocb *iocb, struct sock *sk,
114 struct msghdr *msg, size_t len, int noblock,
115 int flags, int *addr_len)
116{
117 struct sk_buff *skb = NULL;
118 struct sockaddr_pn sa;
119 int rval = -EOPNOTSUPP;
120 int copylen;
121
122 if (flags & MSG_OOB)
123 goto out_nofree;
124
125 if (addr_len)
126 *addr_len = sizeof(sa);
127
128 skb = skb_recv_datagram(sk, flags, noblock, &rval);
129 if (skb == NULL)
130 goto out_nofree;
131
132 pn_skb_get_src_sockaddr(skb, &sa);
133
134 copylen = skb->len;
135 if (len < copylen) {
136 msg->msg_flags |= MSG_TRUNC;
137 copylen = len;
138 }
139
140 rval = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copylen);
141 if (rval) {
142 rval = -EFAULT;
143 goto out;
144 }
145
146 rval = (flags & MSG_TRUNC) ? skb->len : copylen;
147
148 if (msg->msg_name != NULL)
149 memcpy(msg->msg_name, &sa, sizeof(struct sockaddr_pn));
150
151out:
152 skb_free_datagram(sk, skb);
153
154out_nofree:
155 return rval;
156}
157
158/* Queue an skb for a sock. */
159static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb)
160{
161 int err = sock_queue_rcv_skb(sk, skb);
162 if (err < 0)
163 kfree_skb(skb);
164 return err ? NET_RX_DROP : NET_RX_SUCCESS;
165}
166
167/* Module registration */
168static struct proto pn_proto = {
169 .close = pn_sock_close,
170 .ioctl = pn_ioctl,
171 .init = pn_init,
172 .sendmsg = pn_sendmsg,
173 .recvmsg = pn_recvmsg,
174 .backlog_rcv = pn_backlog_rcv,
175 .hash = pn_sock_hash,
176 .unhash = pn_sock_unhash,
177 .get_port = pn_sock_get_port,
178 .obj_size = sizeof(struct pn_sock),
179 .owner = THIS_MODULE,
180 .name = "PHONET",
181};
182
183static struct phonet_protocol pn_dgram_proto = {
184 .ops = &phonet_dgram_ops,
185 .prot = &pn_proto,
186 .sock_type = SOCK_DGRAM,
187};
188
189int __init isi_register(void)
190{
191 return phonet_proto_register(PN_PROTO_PHONET, &pn_dgram_proto);
192}
193
194void __exit isi_unregister(void)
195{
196 phonet_proto_unregister(PN_PROTO_PHONET, &pn_dgram_proto);
197}