af_phonet.c revision 4b07b3f69a8471cdc142c51461a331226fef248a 
1/*
2 * File: af_phonet.c
3 *
4 * Phonet protocols family
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/module.h>
28#include <asm/unaligned.h>
29#include <net/sock.h>
30
31#include <linux/if_phonet.h>
32#include <linux/phonet.h>
33#include <net/phonet/phonet.h>
34
35static struct net_proto_family phonet_proto_family;
36static struct phonet_protocol *phonet_proto_get(int protocol);
37static inline void phonet_proto_put(struct phonet_protocol *pp);
38
39/* protocol family functions */
40
41static int pn_socket_create(struct net *net, struct socket *sock, int protocol)
42{
43	struct phonet_protocol *pnp;
44	int err;
45
46	if (net != &init_net)
47		return -EAFNOSUPPORT;
48
49	if (!capable(CAP_SYS_ADMIN))
50		return -EPERM;
51
52	if (protocol == 0) {
53		/* Default protocol selection */
54		switch (sock->type) {
55		case SOCK_DGRAM:
56			protocol = PN_PROTO_PHONET;
57			break;
58		default:
59			return -EPROTONOSUPPORT;
60		}
61	}
62
63	pnp = phonet_proto_get(protocol);
64	if (pnp == NULL)
65		return -EPROTONOSUPPORT;
66	if (sock->type != pnp->sock_type) {
67		err = -EPROTONOSUPPORT;
68		goto out;
69	}
70
71	/* TODO: create and init the struct sock */
72	err = -EPROTONOSUPPORT;
73
74out:
75	phonet_proto_put(pnp);
76	return err;
77}
78
79static struct net_proto_family phonet_proto_family = {
80	.family = AF_PHONET,
81	.create = pn_socket_create,
82	.owner = THIS_MODULE,
83};
84
85/* packet type functions */
86
87/*
88 * Stuff received packets to associated sockets.
89 * On error, returns non-zero and releases the skb.
90 */
91static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
92			struct packet_type *pkttype,
93			struct net_device *orig_dev)
94{
95	struct phonethdr *ph;
96	struct sockaddr_pn sa;
97	u16 len;
98
99	if (dev_net(dev) != &init_net)
100		goto out;
101
102	/* check we have at least a full Phonet header */
103	if (!pskb_pull(skb, sizeof(struct phonethdr)))
104		goto out;
105
106	/* check that the advertised length is correct */
107	ph = pn_hdr(skb);
108	len = get_unaligned_be16(&ph->pn_length);
109	if (len < 2)
110		goto out;
111	len -= 2;
112	if ((len > skb->len) || pskb_trim(skb, len))
113		goto out;
114	skb_reset_transport_header(skb);
115
116	pn_skb_get_dst_sockaddr(skb, &sa);
117	if (pn_sockaddr_get_addr(&sa) == 0)
118		goto out; /* currently, we cannot be device 0 */
119
120	/* TODO: put packets to sockets backlog */
121
122out:
123	kfree_skb(skb);
124	return NET_RX_DROP;
125}
126
127static struct packet_type phonet_packet_type = {
128	.type = __constant_htons(ETH_P_PHONET),
129	.dev = NULL,
130	.func = phonet_rcv,
131};
132
133/* Transport protocol registration */
134static struct phonet_protocol *proto_tab[PHONET_NPROTO] __read_mostly;
135static DEFINE_SPINLOCK(proto_tab_lock);
136
137int __init_or_module phonet_proto_register(int protocol,
138						struct phonet_protocol *pp)
139{
140	int err = 0;
141
142	if (protocol >= PHONET_NPROTO)
143		return -EINVAL;
144
145	err = proto_register(pp->prot, 1);
146	if (err)
147		return err;
148
149	spin_lock(&proto_tab_lock);
150	if (proto_tab[protocol])
151		err = -EBUSY;
152	else
153		proto_tab[protocol] = pp;
154	spin_unlock(&proto_tab_lock);
155
156	return err;
157}
158EXPORT_SYMBOL(phonet_proto_register);
159
160void phonet_proto_unregister(int protocol, struct phonet_protocol *pp)
161{
162	spin_lock(&proto_tab_lock);
163	BUG_ON(proto_tab[protocol] != pp);
164	proto_tab[protocol] = NULL;
165	spin_unlock(&proto_tab_lock);
166	proto_unregister(pp->prot);
167}
168EXPORT_SYMBOL(phonet_proto_unregister);
169
170static struct phonet_protocol *phonet_proto_get(int protocol)
171{
172	struct phonet_protocol *pp;
173
174	if (protocol >= PHONET_NPROTO)
175		return NULL;
176
177	spin_lock(&proto_tab_lock);
178	pp = proto_tab[protocol];
179	if (pp && !try_module_get(pp->prot->owner))
180		pp = NULL;
181	spin_unlock(&proto_tab_lock);
182
183	return pp;
184}
185
186static inline void phonet_proto_put(struct phonet_protocol *pp)
187{
188	module_put(pp->prot->owner);
189}
190
191/* Module registration */
192static int __init phonet_init(void)
193{
194	int err;
195
196	err = sock_register(&phonet_proto_family);
197	if (err) {
198		printk(KERN_ALERT
199			"phonet protocol family initialization failed\n");
200		return err;
201	}
202
203	dev_add_pack(&phonet_packet_type);
204	return 0;
205}
206
207static void __exit phonet_exit(void)
208{
209	sock_unregister(AF_PHONET);
210	dev_remove_pack(&phonet_packet_type);
211}
212
213module_init(phonet_init);
214module_exit(phonet_exit);
215MODULE_DESCRIPTION("Phonet protocol stack for Linux");
216MODULE_LICENSE("GPL");
217