Skip to content

Commit 107d0d9

Browse files
Remi Denis-Courmontdavem330
Remi Denis-Courmont
authored and
davem330
committed
Phonet: Phonet datagram transport protocol
This provides the basic SOCK_DGRAM transport protocol for Phonet. Signed-off-by: Remi Denis-Courmont <[email protected]> Signed-off-by: David S. Miller <[email protected]>
1 parent ba113a9 commit 107d0d9

File tree

4 files changed

+310
-0
lines changed

4 files changed

+310
-0
lines changed

‎include/net/phonet/phonet.h

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,9 @@ void pn_sock_hash(struct sock *sk);
5151
void pn_sock_unhash(struct sock *sk);
5252
int pn_sock_get_port(struct sock *sk, unsigned short sport);
5353

54+
int pn_skb_send(struct sock *sk, struct sk_buff *skb,
55+
const struct sockaddr_pn *target);
56+
5457
static inline struct phonethdr *pn_hdr(struct sk_buff *skb)
5558
{
5659
return (struct phonethdr *)skb_network_header(skb);
@@ -95,4 +98,7 @@ int phonet_proto_register(int protocol, struct phonet_protocol *pp);
9598
void phonet_proto_unregister(int protocol, struct phonet_protocol *pp);
9699

97100
void phonet_netlink_register(void);
101+
int isi_register(void);
102+
void isi_unregister(void);
103+
98104
#endif

‎net/phonet/Makefile

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,5 @@ phonet-objs := \
44
pn_dev.o \
55
pn_netlink.o \
66
socket.o \
7+
datagram.o \
78
af_phonet.o

‎net/phonet/af_phonet.c

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,101 @@ static struct net_proto_family phonet_proto_family = {
9999
.owner = THIS_MODULE,
100100
};
101101

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+
102197
/* packet type functions */
103198

104199
/*
@@ -226,11 +321,22 @@ static int __init phonet_init(void)
226321
phonet_device_init();
227322
dev_add_pack(&phonet_packet_type);
228323
phonet_netlink_register();
324+
325+
err = isi_register();
326+
if (err)
327+
goto err;
229328
return 0;
329+
330+
err:
331+
sock_unregister(AF_PHONET);
332+
dev_remove_pack(&phonet_packet_type);
333+
phonet_device_exit();
334+
return err;
230335
}
231336

232337
static void __exit phonet_exit(void)
233338
{
339+
isi_unregister();
234340
sock_unregister(AF_PHONET);
235341
dev_remove_pack(&phonet_packet_type);
236342
phonet_device_exit();

‎net/phonet/datagram.c

Lines changed: 197 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -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 <[email protected]>
9+
* Original author: Sakari Ailus <[email protected]>
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+
}

0 commit comments

Comments
 (0)