Phonet: zero-copy aligned GPRS RX
[linux-2.6.git] / net / phonet / pep-gprs.c
1 /*
2  * File: pep-gprs.c
3  *
4  * GPRS over Phonet pipe end point socket
5  *
6  * Copyright (C) 2008 Nokia Corporation.
7  *
8  * Author: RĂ©mi Denis-Courmont <remi.denis-courmont@nokia.com>
9  *
10  * This program is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU General Public License
12  * version 2 as published by the Free Software Foundation.
13  *
14  * This program is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17  * General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program; if not, write to the Free Software
21  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
22  * 02110-1301 USA
23  */
24
25 #include <linux/kernel.h>
26 #include <linux/netdevice.h>
27 #include <linux/if_ether.h>
28 #include <linux/if_arp.h>
29 #include <net/sock.h>
30
31 #include <linux/if_phonet.h>
32 #include <net/tcp_states.h>
33 #include <net/phonet/gprs.h>
34
35 #define GPRS_DEFAULT_MTU 1400
36
37 struct gprs_dev {
38         struct sock             *sk;
39         void                    (*old_state_change)(struct sock *);
40         void                    (*old_data_ready)(struct sock *, int);
41         void                    (*old_write_space)(struct sock *);
42
43         struct net_device       *dev;
44 };
45
46 static __be16 gprs_type_trans(struct sk_buff *skb)
47 {
48         const u8 *pvfc;
49         u8 buf;
50
51         pvfc = skb_header_pointer(skb, 0, 1, &buf);
52         if (!pvfc)
53                 return htons(0);
54         /* Look at IP version field */
55         switch (*pvfc >> 4) {
56         case 4:
57                 return htons(ETH_P_IP);
58         case 6:
59                 return htons(ETH_P_IPV6);
60         }
61         return htons(0);
62 }
63
64 static void gprs_writeable(struct gprs_dev *gp)
65 {
66         struct net_device *dev = gp->dev;
67
68         if (pep_writeable(gp->sk))
69                 netif_wake_queue(dev);
70 }
71
72 /*
73  * Socket callbacks
74  */
75
76 static void gprs_state_change(struct sock *sk)
77 {
78         struct gprs_dev *gp = sk->sk_user_data;
79
80         if (sk->sk_state == TCP_CLOSE_WAIT) {
81                 struct net_device *dev = gp->dev;
82
83                 netif_stop_queue(dev);
84                 netif_carrier_off(dev);
85         }
86 }
87
88 static int gprs_recv(struct gprs_dev *gp, struct sk_buff *skb)
89 {
90         struct net_device *dev = gp->dev;
91         int err = 0;
92         __be16 protocol = gprs_type_trans(skb);
93
94         if (!protocol) {
95                 err = -EINVAL;
96                 goto drop;
97         }
98
99         if (skb_headroom(skb) & 3) {
100                 struct sk_buff *rskb, *fs;
101                 int flen = 0;
102
103                 /* Phonet Pipe data header may be misaligned (3 bytes),
104                  * so wrap the IP packet as a single fragment of an head-less
105                  * socket buffer. The network stack will pull what it needs,
106                  * but at least, the whole IP payload is not memcpy'd. */
107                 rskb = netdev_alloc_skb(dev, 0);
108                 if (!rskb) {
109                         err = -ENOBUFS;
110                         goto drop;
111                 }
112                 skb_shinfo(rskb)->frag_list = skb;
113                 rskb->len += skb->len;
114                 rskb->data_len += rskb->len;
115                 rskb->truesize += rskb->len;
116
117                 /* Avoid nested fragments */
118                 skb_walk_frags(skb, fs)
119                         flen += fs->len;
120                 skb->next = skb_shinfo(skb)->frag_list;
121                 skb_frag_list_init(skb);
122                 skb->len -= flen;
123                 skb->data_len -= flen;
124                 skb->truesize -= flen;
125
126                 skb = rskb;
127         }
128
129         skb->protocol = protocol;
130         skb_reset_mac_header(skb);
131         skb->dev = dev;
132
133         if (likely(dev->flags & IFF_UP)) {
134                 dev->stats.rx_packets++;
135                 dev->stats.rx_bytes += skb->len;
136                 netif_rx(skb);
137                 skb = NULL;
138         } else
139                 err = -ENODEV;
140
141 drop:
142         if (skb) {
143                 dev_kfree_skb(skb);
144                 dev->stats.rx_dropped++;
145         }
146         return err;
147 }
148
149 static void gprs_data_ready(struct sock *sk, int len)
150 {
151         struct gprs_dev *gp = sk->sk_user_data;
152         struct sk_buff *skb;
153
154         while ((skb = pep_read(sk)) != NULL) {
155                 skb_orphan(skb);
156                 gprs_recv(gp, skb);
157         }
158 }
159
160 static void gprs_write_space(struct sock *sk)
161 {
162         struct gprs_dev *gp = sk->sk_user_data;
163
164         if (netif_running(gp->dev))
165                 gprs_writeable(gp);
166 }
167
168 /*
169  * Network device callbacks
170  */
171
172 static int gprs_open(struct net_device *dev)
173 {
174         struct gprs_dev *gp = netdev_priv(dev);
175
176         gprs_writeable(gp);
177         return 0;
178 }
179
180 static int gprs_close(struct net_device *dev)
181 {
182         netif_stop_queue(dev);
183         return 0;
184 }
185
186 static netdev_tx_t gprs_xmit(struct sk_buff *skb, struct net_device *dev)
187 {
188         struct gprs_dev *gp = netdev_priv(dev);
189         struct sock *sk = gp->sk;
190         int len, err;
191
192         switch (skb->protocol) {
193         case  htons(ETH_P_IP):
194         case  htons(ETH_P_IPV6):
195                 break;
196         default:
197                 dev_kfree_skb(skb);
198                 return NETDEV_TX_OK;
199         }
200
201         skb_orphan(skb);
202         skb_set_owner_w(skb, sk);
203         len = skb->len;
204         err = pep_write(sk, skb);
205         if (err) {
206                 LIMIT_NETDEBUG(KERN_WARNING"%s: TX error (%d)\n",
207                                 dev->name, err);
208                 dev->stats.tx_aborted_errors++;
209                 dev->stats.tx_errors++;
210         } else {
211                 dev->stats.tx_packets++;
212                 dev->stats.tx_bytes += len;
213         }
214
215         netif_stop_queue(dev);
216         if (pep_writeable(sk))
217                 netif_wake_queue(dev);
218         return NETDEV_TX_OK;
219 }
220
221 static int gprs_set_mtu(struct net_device *dev, int new_mtu)
222 {
223         if ((new_mtu < 576) || (new_mtu > (PHONET_MAX_MTU - 11)))
224                 return -EINVAL;
225
226         dev->mtu = new_mtu;
227         return 0;
228 }
229
230 static const struct net_device_ops gprs_netdev_ops = {
231         .ndo_open       = gprs_open,
232         .ndo_stop       = gprs_close,
233         .ndo_start_xmit = gprs_xmit,
234         .ndo_change_mtu = gprs_set_mtu,
235 };
236
237 static void gprs_setup(struct net_device *dev)
238 {
239         dev->features           = NETIF_F_FRAGLIST;
240         dev->type               = ARPHRD_PHONET_PIPE;
241         dev->flags              = IFF_POINTOPOINT | IFF_NOARP;
242         dev->mtu                = GPRS_DEFAULT_MTU;
243         dev->hard_header_len    = 0;
244         dev->addr_len           = 0;
245         dev->tx_queue_len       = 10;
246
247         dev->netdev_ops         = &gprs_netdev_ops;
248         dev->destructor         = free_netdev;
249 }
250
251 /*
252  * External interface
253  */
254
255 /*
256  * Attach a GPRS interface to a datagram socket.
257  * Returns the interface index on success, negative error code on error.
258  */
259 int gprs_attach(struct sock *sk)
260 {
261         static const char ifname[] = "gprs%d";
262         struct gprs_dev *gp;
263         struct net_device *dev;
264         int err;
265
266         if (unlikely(sk->sk_type == SOCK_STREAM))
267                 return -EINVAL; /* need packet boundaries */
268
269         /* Create net device */
270         dev = alloc_netdev(sizeof(*gp), ifname, gprs_setup);
271         if (!dev)
272                 return -ENOMEM;
273         gp = netdev_priv(dev);
274         gp->sk = sk;
275         gp->dev = dev;
276
277         netif_stop_queue(dev);
278         err = register_netdev(dev);
279         if (err) {
280                 free_netdev(dev);
281                 return err;
282         }
283
284         lock_sock(sk);
285         if (unlikely(sk->sk_user_data)) {
286                 err = -EBUSY;
287                 goto out_rel;
288         }
289         if (unlikely((1 << sk->sk_state & (TCPF_CLOSE|TCPF_LISTEN)) ||
290                         sock_flag(sk, SOCK_DEAD))) {
291                 err = -EINVAL;
292                 goto out_rel;
293         }
294         sk->sk_user_data        = gp;
295         gp->old_state_change    = sk->sk_state_change;
296         gp->old_data_ready      = sk->sk_data_ready;
297         gp->old_write_space     = sk->sk_write_space;
298         sk->sk_state_change     = gprs_state_change;
299         sk->sk_data_ready       = gprs_data_ready;
300         sk->sk_write_space      = gprs_write_space;
301         release_sock(sk);
302         sock_hold(sk);
303
304         printk(KERN_DEBUG"%s: attached\n", dev->name);
305         return dev->ifindex;
306
307 out_rel:
308         release_sock(sk);
309         unregister_netdev(dev);
310         return err;
311 }
312
313 void gprs_detach(struct sock *sk)
314 {
315         struct gprs_dev *gp = sk->sk_user_data;
316         struct net_device *dev = gp->dev;
317
318         lock_sock(sk);
319         sk->sk_user_data        = NULL;
320         sk->sk_state_change     = gp->old_state_change;
321         sk->sk_data_ready       = gp->old_data_ready;
322         sk->sk_write_space      = gp->old_write_space;
323         release_sock(sk);
324
325         printk(KERN_DEBUG"%s: detached\n", dev->name);
326         unregister_netdev(dev);
327         sock_put(sk);
328 }