diff options
-rw-r--r-- | include/net/phonet/phonet.h | 6 | ||||
-rw-r--r-- | net/phonet/Makefile | 1 | ||||
-rw-r--r-- | net/phonet/af_phonet.c | 106 | ||||
-rw-r--r-- | net/phonet/datagram.c | 197 |
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); | |||
51 | void pn_sock_unhash(struct sock *sk); | 51 | void pn_sock_unhash(struct sock *sk); |
52 | int pn_sock_get_port(struct sock *sk, unsigned short sport); | 52 | int pn_sock_get_port(struct sock *sk, unsigned short sport); |
53 | 53 | ||
54 | int pn_skb_send(struct sock *sk, struct sk_buff *skb, | ||
55 | const struct sockaddr_pn *target); | ||
56 | |||
54 | static inline struct phonethdr *pn_hdr(struct sk_buff *skb) | 57 | static 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); | |||
95 | void phonet_proto_unregister(int protocol, struct phonet_protocol *pp); | 98 | void phonet_proto_unregister(int protocol, struct phonet_protocol *pp); |
96 | 99 | ||
97 | void phonet_netlink_register(void); | 100 | void phonet_netlink_register(void); |
101 | int isi_register(void); | ||
102 | void 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 | */ | ||
105 | static 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; | ||
150 | drop: | ||
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 | */ | ||
159 | int 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 | |||
189 | drop: | ||
190 | kfree_skb(skb); | ||
191 | if (dev) | ||
192 | dev_put(dev); | ||
193 | return err; | ||
194 | } | ||
195 | EXPORT_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 | |||
330 | err: | ||
331 | sock_unregister(AF_PHONET); | ||
332 | dev_remove_pack(&phonet_packet_type); | ||
333 | phonet_device_exit(); | ||
334 | return err; | ||
230 | } | 335 | } |
231 | 336 | ||
232 | static void __exit phonet_exit(void) | 337 | static 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 | |||
34 | static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb); | ||
35 | |||
36 | /* associated socket ceases to exist */ | ||
37 | static void pn_sock_close(struct sock *sk, long timeout) | ||
38 | { | ||
39 | sk_common_release(sk); | ||
40 | } | ||
41 | |||
42 | static 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. */ | ||
60 | static void pn_destruct(struct sock *sk) | ||
61 | { | ||
62 | skb_queue_purge(&sk->sk_receive_queue); | ||
63 | } | ||
64 | |||
65 | static int pn_init(struct sock *sk) | ||
66 | { | ||
67 | sk->sk_destruct = pn_destruct; | ||
68 | return 0; | ||
69 | } | ||
70 | |||
71 | static 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 | |||
113 | static 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 | |||
151 | out: | ||
152 | skb_free_datagram(sk, skb); | ||
153 | |||
154 | out_nofree: | ||
155 | return rval; | ||
156 | } | ||
157 | |||
158 | /* Queue an skb for a sock. */ | ||
159 | static 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 */ | ||
168 | static 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 | |||
183 | static struct phonet_protocol pn_dgram_proto = { | ||
184 | .ops = &phonet_dgram_ops, | ||
185 | .prot = &pn_proto, | ||
186 | .sock_type = SOCK_DGRAM, | ||
187 | }; | ||
188 | |||
189 | int __init isi_register(void) | ||
190 | { | ||
191 | return phonet_proto_register(PN_PROTO_PHONET, &pn_dgram_proto); | ||
192 | } | ||
193 | |||
194 | void __exit isi_unregister(void) | ||
195 | { | ||
196 | phonet_proto_unregister(PN_PROTO_PHONET, &pn_dgram_proto); | ||
197 | } | ||