Commit | Line | Data |
---|---|---|
4b07b3f6 RDC |
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 | ||
35 | static struct net_proto_family phonet_proto_family; | |
36 | static struct phonet_protocol *phonet_proto_get(int protocol); | |
37 | static inline void phonet_proto_put(struct phonet_protocol *pp); | |
38 | ||
39 | /* protocol family functions */ | |
40 | ||
41 | static 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 | ||
74 | out: | |
75 | phonet_proto_put(pnp); | |
76 | return err; | |
77 | } | |
78 | ||
79 | static 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 | */ | |
91 | static 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 | ||
122 | out: | |
123 | kfree_skb(skb); | |
124 | return NET_RX_DROP; | |
125 | } | |
126 | ||
127 | static 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 */ | |
134 | static struct phonet_protocol *proto_tab[PHONET_NPROTO] __read_mostly; | |
135 | static DEFINE_SPINLOCK(proto_tab_lock); | |
136 | ||
137 | int __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 | } | |
158 | EXPORT_SYMBOL(phonet_proto_register); | |
159 | ||
160 | void 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 | } | |
168 | EXPORT_SYMBOL(phonet_proto_unregister); | |
169 | ||
170 | static 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 | ||
186 | static inline void phonet_proto_put(struct phonet_protocol *pp) | |
187 | { | |
188 | module_put(pp->prot->owner); | |
189 | } | |
190 | ||
191 | /* Module registration */ | |
192 | static 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 | ||
207 | static void __exit phonet_exit(void) | |
208 | { | |
209 | sock_unregister(AF_PHONET); | |
210 | dev_remove_pack(&phonet_packet_type); | |
211 | } | |
212 | ||
213 | module_init(phonet_init); | |
214 | module_exit(phonet_exit); | |
215 | MODULE_DESCRIPTION("Phonet protocol stack for Linux"); | |
216 | MODULE_LICENSE("GPL"); |