Merge branch 'master' of master.kernel.org:/pub/scm/linux/kernel/git/davem/net-2.6
[linux-3.10.git] / net / phonet / pep.c
1 /*
2  * File: pep.c
3  *
4  * Phonet pipe protocol 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/slab.h>
27 #include <linux/socket.h>
28 #include <net/sock.h>
29 #include <net/tcp_states.h>
30 #include <asm/ioctls.h>
31
32 #include <linux/phonet.h>
33 #include <net/phonet/phonet.h>
34 #include <net/phonet/pep.h>
35 #include <net/phonet/gprs.h>
36
37 /* sk_state values:
38  * TCP_CLOSE            sock not in use yet
39  * TCP_CLOSE_WAIT       disconnected pipe
40  * TCP_LISTEN           listening pipe endpoint
41  * TCP_SYN_RECV         connected pipe in disabled state
42  * TCP_ESTABLISHED      connected pipe in enabled state
43  *
44  * pep_sock locking:
45  *  - sk_state, ackq, hlist: sock lock needed
46  *  - listener: read only
47  *  - pipe_handle: read only
48  */
49
50 #define CREDITS_MAX     10
51 #define CREDITS_THR     7
52
53 static const struct sockaddr_pn pipe_srv = {
54         .spn_family = AF_PHONET,
55         .spn_resource = 0xD9, /* pipe service */
56 };
57
58 #define pep_sb_size(s) (((s) + 5) & ~3) /* 2-bytes head, 32-bits aligned */
59
60 /* Get the next TLV sub-block. */
61 static unsigned char *pep_get_sb(struct sk_buff *skb, u8 *ptype, u8 *plen,
62                                         void *buf)
63 {
64         void *data = NULL;
65         struct {
66                 u8 sb_type;
67                 u8 sb_len;
68         } *ph, h;
69         int buflen = *plen;
70
71         ph = skb_header_pointer(skb, 0, 2, &h);
72         if (ph == NULL || ph->sb_len < 2 || !pskb_may_pull(skb, ph->sb_len))
73                 return NULL;
74         ph->sb_len -= 2;
75         *ptype = ph->sb_type;
76         *plen = ph->sb_len;
77
78         if (buflen > ph->sb_len)
79                 buflen = ph->sb_len;
80         data = skb_header_pointer(skb, 2, buflen, buf);
81         __skb_pull(skb, 2 + ph->sb_len);
82         return data;
83 }
84
85 static int pep_reply(struct sock *sk, struct sk_buff *oskb,
86                         u8 code, const void *data, int len, gfp_t priority)
87 {
88         const struct pnpipehdr *oph = pnp_hdr(oskb);
89         struct pnpipehdr *ph;
90         struct sk_buff *skb;
91
92         skb = alloc_skb(MAX_PNPIPE_HEADER + len, priority);
93         if (!skb)
94                 return -ENOMEM;
95         skb_set_owner_w(skb, sk);
96
97         skb_reserve(skb, MAX_PNPIPE_HEADER);
98         __skb_put(skb, len);
99         skb_copy_to_linear_data(skb, data, len);
100         __skb_push(skb, sizeof(*ph));
101         skb_reset_transport_header(skb);
102         ph = pnp_hdr(skb);
103         ph->utid = oph->utid;
104         ph->message_id = oph->message_id + 1; /* REQ -> RESP */
105         ph->pipe_handle = oph->pipe_handle;
106         ph->error_code = code;
107
108         return pn_skb_send(sk, skb, &pipe_srv);
109 }
110
111 #define PAD 0x00
112 static int pep_accept_conn(struct sock *sk, struct sk_buff *skb)
113 {
114         static const u8 data[20] = {
115                 PAD, PAD, PAD, 2 /* sub-blocks */,
116                 PN_PIPE_SB_REQUIRED_FC_TX, pep_sb_size(5), 3, PAD,
117                         PN_MULTI_CREDIT_FLOW_CONTROL,
118                         PN_ONE_CREDIT_FLOW_CONTROL,
119                         PN_LEGACY_FLOW_CONTROL,
120                         PAD,
121                 PN_PIPE_SB_PREFERRED_FC_RX, pep_sb_size(5), 3, PAD,
122                         PN_MULTI_CREDIT_FLOW_CONTROL,
123                         PN_ONE_CREDIT_FLOW_CONTROL,
124                         PN_LEGACY_FLOW_CONTROL,
125                         PAD,
126         };
127
128         might_sleep();
129         return pep_reply(sk, skb, PN_PIPE_NO_ERROR, data, sizeof(data),
130                                 GFP_KERNEL);
131 }
132
133 static int pep_reject_conn(struct sock *sk, struct sk_buff *skb, u8 code)
134 {
135         static const u8 data[4] = { PAD, PAD, PAD, 0 /* sub-blocks */ };
136         WARN_ON(code == PN_PIPE_NO_ERROR);
137         return pep_reply(sk, skb, code, data, sizeof(data), GFP_ATOMIC);
138 }
139
140 /* Control requests are not sent by the pipe service and have a specific
141  * message format. */
142 static int pep_ctrlreq_error(struct sock *sk, struct sk_buff *oskb, u8 code,
143                                 gfp_t priority)
144 {
145         const struct pnpipehdr *oph = pnp_hdr(oskb);
146         struct sk_buff *skb;
147         struct pnpipehdr *ph;
148         struct sockaddr_pn dst;
149
150         skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority);
151         if (!skb)
152                 return -ENOMEM;
153         skb_set_owner_w(skb, sk);
154
155         skb_reserve(skb, MAX_PHONET_HEADER);
156         ph = (struct pnpipehdr *)skb_put(skb, sizeof(*ph) + 4);
157
158         ph->utid = oph->utid;
159         ph->message_id = PNS_PEP_CTRL_RESP;
160         ph->pipe_handle = oph->pipe_handle;
161         ph->data[0] = oph->data[1]; /* CTRL id */
162         ph->data[1] = oph->data[0]; /* PEP type */
163         ph->data[2] = code; /* error code, at an usual offset */
164         ph->data[3] = PAD;
165         ph->data[4] = PAD;
166
167         pn_skb_get_src_sockaddr(oskb, &dst);
168         return pn_skb_send(sk, skb, &dst);
169 }
170
171 static int pipe_snd_status(struct sock *sk, u8 type, u8 status, gfp_t priority)
172 {
173         struct pep_sock *pn = pep_sk(sk);
174         struct pnpipehdr *ph;
175         struct sk_buff *skb;
176
177         skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority);
178         if (!skb)
179                 return -ENOMEM;
180         skb_set_owner_w(skb, sk);
181
182         skb_reserve(skb, MAX_PNPIPE_HEADER + 4);
183         __skb_push(skb, sizeof(*ph) + 4);
184         skb_reset_transport_header(skb);
185         ph = pnp_hdr(skb);
186         ph->utid = 0;
187         ph->message_id = PNS_PEP_STATUS_IND;
188         ph->pipe_handle = pn->pipe_handle;
189         ph->pep_type = PN_PEP_TYPE_COMMON;
190         ph->data[1] = type;
191         ph->data[2] = PAD;
192         ph->data[3] = PAD;
193         ph->data[4] = status;
194
195         return pn_skb_send(sk, skb, &pipe_srv);
196 }
197
198 /* Send our RX flow control information to the sender.
199  * Socket must be locked. */
200 static void pipe_grant_credits(struct sock *sk)
201 {
202         struct pep_sock *pn = pep_sk(sk);
203
204         BUG_ON(sk->sk_state != TCP_ESTABLISHED);
205
206         switch (pn->rx_fc) {
207         case PN_LEGACY_FLOW_CONTROL: /* TODO */
208                 break;
209         case PN_ONE_CREDIT_FLOW_CONTROL:
210                 pipe_snd_status(sk, PN_PEP_IND_FLOW_CONTROL,
211                                 PEP_IND_READY, GFP_ATOMIC);
212                 pn->rx_credits = 1;
213                 break;
214         case PN_MULTI_CREDIT_FLOW_CONTROL:
215                 if ((pn->rx_credits + CREDITS_THR) > CREDITS_MAX)
216                         break;
217                 if (pipe_snd_status(sk, PN_PEP_IND_ID_MCFC_GRANT_CREDITS,
218                                         CREDITS_MAX - pn->rx_credits,
219                                         GFP_ATOMIC) == 0)
220                         pn->rx_credits = CREDITS_MAX;
221                 break;
222         }
223 }
224
225 static int pipe_rcv_status(struct sock *sk, struct sk_buff *skb)
226 {
227         struct pep_sock *pn = pep_sk(sk);
228         struct pnpipehdr *hdr = pnp_hdr(skb);
229         int wake = 0;
230
231         if (!pskb_may_pull(skb, sizeof(*hdr) + 4))
232                 return -EINVAL;
233
234         if (hdr->data[0] != PN_PEP_TYPE_COMMON) {
235                 LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP type: %u\n",
236                                 (unsigned)hdr->data[0]);
237                 return -EOPNOTSUPP;
238         }
239
240         switch (hdr->data[1]) {
241         case PN_PEP_IND_FLOW_CONTROL:
242                 switch (pn->tx_fc) {
243                 case PN_LEGACY_FLOW_CONTROL:
244                         switch (hdr->data[4]) {
245                         case PEP_IND_BUSY:
246                                 atomic_set(&pn->tx_credits, 0);
247                                 break;
248                         case PEP_IND_READY:
249                                 atomic_set(&pn->tx_credits, wake = 1);
250                                 break;
251                         }
252                         break;
253                 case PN_ONE_CREDIT_FLOW_CONTROL:
254                         if (hdr->data[4] == PEP_IND_READY)
255                                 atomic_set(&pn->tx_credits, wake = 1);
256                         break;
257                 }
258                 break;
259
260         case PN_PEP_IND_ID_MCFC_GRANT_CREDITS:
261                 if (pn->tx_fc != PN_MULTI_CREDIT_FLOW_CONTROL)
262                         break;
263                 atomic_add(wake = hdr->data[4], &pn->tx_credits);
264                 break;
265
266         default:
267                 LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP indication: %u\n",
268                                 (unsigned)hdr->data[1]);
269                 return -EOPNOTSUPP;
270         }
271         if (wake)
272                 sk->sk_write_space(sk);
273         return 0;
274 }
275
276 static int pipe_rcv_created(struct sock *sk, struct sk_buff *skb)
277 {
278         struct pep_sock *pn = pep_sk(sk);
279         struct pnpipehdr *hdr = pnp_hdr(skb);
280         u8 n_sb = hdr->data[0];
281
282         pn->rx_fc = pn->tx_fc = PN_LEGACY_FLOW_CONTROL;
283         __skb_pull(skb, sizeof(*hdr));
284         while (n_sb > 0) {
285                 u8 type, buf[2], len = sizeof(buf);
286                 u8 *data = pep_get_sb(skb, &type, &len, buf);
287
288                 if (data == NULL)
289                         return -EINVAL;
290                 switch (type) {
291                 case PN_PIPE_SB_NEGOTIATED_FC:
292                         if (len < 2 || (data[0] | data[1]) > 3)
293                                 break;
294                         pn->tx_fc = data[0] & 3;
295                         pn->rx_fc = data[1] & 3;
296                         break;
297                 }
298                 n_sb--;
299         }
300         return 0;
301 }
302
303 /* Queue an skb to a connected sock.
304  * Socket lock must be held. */
305 static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb)
306 {
307         struct pep_sock *pn = pep_sk(sk);
308         struct pnpipehdr *hdr = pnp_hdr(skb);
309         struct sk_buff_head *queue;
310         int err = 0;
311
312         BUG_ON(sk->sk_state == TCP_CLOSE_WAIT);
313
314         switch (hdr->message_id) {
315         case PNS_PEP_CONNECT_REQ:
316                 pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE);
317                 break;
318
319         case PNS_PEP_DISCONNECT_REQ:
320                 pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
321                 sk->sk_state = TCP_CLOSE_WAIT;
322                 if (!sock_flag(sk, SOCK_DEAD))
323                         sk->sk_state_change(sk);
324                 break;
325
326         case PNS_PEP_ENABLE_REQ:
327                 /* Wait for PNS_PIPE_(ENABLED|REDIRECTED)_IND */
328                 pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
329                 break;
330
331         case PNS_PEP_RESET_REQ:
332                 switch (hdr->state_after_reset) {
333                 case PN_PIPE_DISABLE:
334                         pn->init_enable = 0;
335                         break;
336                 case PN_PIPE_ENABLE:
337                         pn->init_enable = 1;
338                         break;
339                 default: /* not allowed to send an error here!? */
340                         err = -EINVAL;
341                         goto out;
342                 }
343                 /* fall through */
344         case PNS_PEP_DISABLE_REQ:
345                 atomic_set(&pn->tx_credits, 0);
346                 pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
347                 break;
348
349         case PNS_PEP_CTRL_REQ:
350                 if (skb_queue_len(&pn->ctrlreq_queue) >= PNPIPE_CTRLREQ_MAX) {
351                         atomic_inc(&sk->sk_drops);
352                         break;
353                 }
354                 __skb_pull(skb, 4);
355                 queue = &pn->ctrlreq_queue;
356                 goto queue;
357
358         case PNS_PIPE_ALIGNED_DATA:
359                 __skb_pull(skb, 1);
360                 /* fall through */
361         case PNS_PIPE_DATA:
362                 __skb_pull(skb, 3); /* Pipe data header */
363                 if (!pn_flow_safe(pn->rx_fc)) {
364                         err = sock_queue_rcv_skb(sk, skb);
365                         if (!err)
366                                 return 0;
367                         break;
368                 }
369
370                 if (pn->rx_credits == 0) {
371                         atomic_inc(&sk->sk_drops);
372                         err = -ENOBUFS;
373                         break;
374                 }
375                 pn->rx_credits--;
376                 queue = &sk->sk_receive_queue;
377                 goto queue;
378
379         case PNS_PEP_STATUS_IND:
380                 pipe_rcv_status(sk, skb);
381                 break;
382
383         case PNS_PIPE_REDIRECTED_IND:
384                 err = pipe_rcv_created(sk, skb);
385                 break;
386
387         case PNS_PIPE_CREATED_IND:
388                 err = pipe_rcv_created(sk, skb);
389                 if (err)
390                         break;
391                 /* fall through */
392         case PNS_PIPE_RESET_IND:
393                 if (!pn->init_enable)
394                         break;
395                 /* fall through */
396         case PNS_PIPE_ENABLED_IND:
397                 if (!pn_flow_safe(pn->tx_fc)) {
398                         atomic_set(&pn->tx_credits, 1);
399                         sk->sk_write_space(sk);
400                 }
401                 if (sk->sk_state == TCP_ESTABLISHED)
402                         break; /* Nothing to do */
403                 sk->sk_state = TCP_ESTABLISHED;
404                 pipe_grant_credits(sk);
405                 break;
406
407         case PNS_PIPE_DISABLED_IND:
408                 sk->sk_state = TCP_SYN_RECV;
409                 pn->rx_credits = 0;
410                 break;
411
412         default:
413                 LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP message: %u\n",
414                                 hdr->message_id);
415                 err = -EINVAL;
416         }
417 out:
418         kfree_skb(skb);
419         return err;
420
421 queue:
422         skb->dev = NULL;
423         skb_set_owner_r(skb, sk);
424         err = skb->len;
425         skb_queue_tail(queue, skb);
426         if (!sock_flag(sk, SOCK_DEAD))
427                 sk->sk_data_ready(sk, err);
428         return 0;
429 }
430
431 /* Destroy connected sock. */
432 static void pipe_destruct(struct sock *sk)
433 {
434         struct pep_sock *pn = pep_sk(sk);
435
436         skb_queue_purge(&sk->sk_receive_queue);
437         skb_queue_purge(&pn->ctrlreq_queue);
438 }
439
440 static int pep_connreq_rcv(struct sock *sk, struct sk_buff *skb)
441 {
442         struct sock *newsk;
443         struct pep_sock *newpn, *pn = pep_sk(sk);
444         struct pnpipehdr *hdr;
445         struct sockaddr_pn dst;
446         u16 peer_type;
447         u8 pipe_handle, enabled, n_sb;
448         u8 aligned = 0;
449
450         if (!pskb_pull(skb, sizeof(*hdr) + 4))
451                 return -EINVAL;
452
453         hdr = pnp_hdr(skb);
454         pipe_handle = hdr->pipe_handle;
455         switch (hdr->state_after_connect) {
456         case PN_PIPE_DISABLE:
457                 enabled = 0;
458                 break;
459         case PN_PIPE_ENABLE:
460                 enabled = 1;
461                 break;
462         default:
463                 pep_reject_conn(sk, skb, PN_PIPE_ERR_INVALID_PARAM);
464                 return -EINVAL;
465         }
466         peer_type = hdr->other_pep_type << 8;
467
468         if (unlikely(sk->sk_state != TCP_LISTEN) || sk_acceptq_is_full(sk)) {
469                 pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE);
470                 return -ENOBUFS;
471         }
472
473         /* Parse sub-blocks (options) */
474         n_sb = hdr->data[4];
475         while (n_sb > 0) {
476                 u8 type, buf[1], len = sizeof(buf);
477                 const u8 *data = pep_get_sb(skb, &type, &len, buf);
478
479                 if (data == NULL)
480                         return -EINVAL;
481                 switch (type) {
482                 case PN_PIPE_SB_CONNECT_REQ_PEP_SUB_TYPE:
483                         if (len < 1)
484                                 return -EINVAL;
485                         peer_type = (peer_type & 0xff00) | data[0];
486                         break;
487                 case PN_PIPE_SB_ALIGNED_DATA:
488                         aligned = data[0] != 0;
489                         break;
490                 }
491                 n_sb--;
492         }
493
494         skb = skb_clone(skb, GFP_ATOMIC);
495         if (!skb)
496                 return -ENOMEM;
497
498         /* Create a new to-be-accepted sock */
499         newsk = sk_alloc(sock_net(sk), PF_PHONET, GFP_ATOMIC, sk->sk_prot);
500         if (!newsk) {
501                 kfree_skb(skb);
502                 return -ENOMEM;
503         }
504         sock_init_data(NULL, newsk);
505         newsk->sk_state = TCP_SYN_RECV;
506         newsk->sk_backlog_rcv = pipe_do_rcv;
507         newsk->sk_protocol = sk->sk_protocol;
508         newsk->sk_destruct = pipe_destruct;
509
510         newpn = pep_sk(newsk);
511         pn_skb_get_dst_sockaddr(skb, &dst);
512         newpn->pn_sk.sobject = pn_sockaddr_get_object(&dst);
513         newpn->pn_sk.resource = pn->pn_sk.resource;
514         skb_queue_head_init(&newpn->ctrlreq_queue);
515         newpn->pipe_handle = pipe_handle;
516         atomic_set(&newpn->tx_credits, 0);
517         newpn->peer_type = peer_type;
518         newpn->rx_credits = 0;
519         newpn->rx_fc = newpn->tx_fc = PN_LEGACY_FLOW_CONTROL;
520         newpn->init_enable = enabled;
521         newpn->aligned = aligned;
522
523         BUG_ON(!skb_queue_empty(&newsk->sk_receive_queue));
524         skb_queue_head(&newsk->sk_receive_queue, skb);
525         if (!sock_flag(sk, SOCK_DEAD))
526                 sk->sk_data_ready(sk, 0);
527
528         sk_acceptq_added(sk);
529         sk_add_node(newsk, &pn->ackq);
530         return 0;
531 }
532
533 /* Listening sock must be locked */
534 static struct sock *pep_find_pipe(const struct hlist_head *hlist,
535                                         const struct sockaddr_pn *dst,
536                                         u8 pipe_handle)
537 {
538         struct hlist_node *node;
539         struct sock *sknode;
540         u16 dobj = pn_sockaddr_get_object(dst);
541
542         sk_for_each(sknode, node, hlist) {
543                 struct pep_sock *pnnode = pep_sk(sknode);
544
545                 /* Ports match, but addresses might not: */
546                 if (pnnode->pn_sk.sobject != dobj)
547                         continue;
548                 if (pnnode->pipe_handle != pipe_handle)
549                         continue;
550                 if (sknode->sk_state == TCP_CLOSE_WAIT)
551                         continue;
552
553                 sock_hold(sknode);
554                 return sknode;
555         }
556         return NULL;
557 }
558
559 /*
560  * Deliver an skb to a listening sock.
561  * Socket lock must be held.
562  * We then queue the skb to the right connected sock (if any).
563  */
564 static int pep_do_rcv(struct sock *sk, struct sk_buff *skb)
565 {
566         struct pep_sock *pn = pep_sk(sk);
567         struct sock *sknode;
568         struct pnpipehdr *hdr;
569         struct sockaddr_pn dst;
570         int err = NET_RX_SUCCESS;
571         u8 pipe_handle;
572
573         if (!pskb_may_pull(skb, sizeof(*hdr)))
574                 goto drop;
575
576         hdr = pnp_hdr(skb);
577         pipe_handle = hdr->pipe_handle;
578         if (pipe_handle == PN_PIPE_INVALID_HANDLE)
579                 goto drop;
580
581         pn_skb_get_dst_sockaddr(skb, &dst);
582
583         /* Look for an existing pipe handle */
584         sknode = pep_find_pipe(&pn->hlist, &dst, pipe_handle);
585         if (sknode)
586                 return sk_receive_skb(sknode, skb, 1);
587
588         /* Look for a pipe handle pending accept */
589         sknode = pep_find_pipe(&pn->ackq, &dst, pipe_handle);
590         if (sknode) {
591                 sock_put(sknode);
592                 if (net_ratelimit())
593                         printk(KERN_WARNING"Phonet unconnected PEP ignored");
594                 err = NET_RX_DROP;
595                 goto drop;
596         }
597
598         switch (hdr->message_id) {
599         case PNS_PEP_CONNECT_REQ:
600                 err = pep_connreq_rcv(sk, skb);
601                 break;
602
603         case PNS_PEP_DISCONNECT_REQ:
604                 pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
605                 break;
606
607         case PNS_PEP_CTRL_REQ:
608                 pep_ctrlreq_error(sk, skb, PN_PIPE_INVALID_HANDLE, GFP_ATOMIC);
609                 break;
610
611         case PNS_PEP_RESET_REQ:
612         case PNS_PEP_ENABLE_REQ:
613         case PNS_PEP_DISABLE_REQ:
614                 /* invalid handle is not even allowed here! */
615         default:
616                 err = NET_RX_DROP;
617         }
618 drop:
619         kfree_skb(skb);
620         return err;
621 }
622
623 /* associated socket ceases to exist */
624 static void pep_sock_close(struct sock *sk, long timeout)
625 {
626         struct pep_sock *pn = pep_sk(sk);
627         int ifindex = 0;
628
629         sock_hold(sk); /* keep a reference after sk_common_release() */
630         sk_common_release(sk);
631
632         lock_sock(sk);
633         if (sk->sk_state == TCP_LISTEN) {
634                 /* Destroy the listen queue */
635                 struct sock *sknode;
636                 struct hlist_node *p, *n;
637
638                 sk_for_each_safe(sknode, p, n, &pn->ackq)
639                         sk_del_node_init(sknode);
640                 sk->sk_state = TCP_CLOSE;
641         }
642         ifindex = pn->ifindex;
643         pn->ifindex = 0;
644         release_sock(sk);
645
646         if (ifindex)
647                 gprs_detach(sk);
648         sock_put(sk);
649 }
650
651 static int pep_wait_connreq(struct sock *sk, int noblock)
652 {
653         struct task_struct *tsk = current;
654         struct pep_sock *pn = pep_sk(sk);
655         long timeo = sock_rcvtimeo(sk, noblock);
656
657         for (;;) {
658                 DEFINE_WAIT(wait);
659
660                 if (sk->sk_state != TCP_LISTEN)
661                         return -EINVAL;
662                 if (!hlist_empty(&pn->ackq))
663                         break;
664                 if (!timeo)
665                         return -EWOULDBLOCK;
666                 if (signal_pending(tsk))
667                         return sock_intr_errno(timeo);
668
669                 prepare_to_wait_exclusive(sk_sleep(sk), &wait,
670                                                 TASK_INTERRUPTIBLE);
671                 release_sock(sk);
672                 timeo = schedule_timeout(timeo);
673                 lock_sock(sk);
674                 finish_wait(sk_sleep(sk), &wait);
675         }
676
677         return 0;
678 }
679
680 static struct sock *pep_sock_accept(struct sock *sk, int flags, int *errp)
681 {
682         struct pep_sock *pn = pep_sk(sk);
683         struct sock *newsk = NULL;
684         struct sk_buff *oskb;
685         int err;
686
687         lock_sock(sk);
688         err = pep_wait_connreq(sk, flags & O_NONBLOCK);
689         if (err)
690                 goto out;
691
692         newsk = __sk_head(&pn->ackq);
693
694         oskb = skb_dequeue(&newsk->sk_receive_queue);
695         err = pep_accept_conn(newsk, oskb);
696         if (err) {
697                 skb_queue_head(&newsk->sk_receive_queue, oskb);
698                 newsk = NULL;
699                 goto out;
700         }
701         kfree_skb(oskb);
702
703         sock_hold(sk);
704         pep_sk(newsk)->listener = sk;
705
706         sock_hold(newsk);
707         sk_del_node_init(newsk);
708         sk_acceptq_removed(sk);
709         sk_add_node(newsk, &pn->hlist);
710         __sock_put(newsk);
711
712 out:
713         release_sock(sk);
714         *errp = err;
715         return newsk;
716 }
717
718 static int pep_ioctl(struct sock *sk, int cmd, unsigned long arg)
719 {
720         struct pep_sock *pn = pep_sk(sk);
721         int answ;
722
723         switch (cmd) {
724         case SIOCINQ:
725                 if (sk->sk_state == TCP_LISTEN)
726                         return -EINVAL;
727
728                 lock_sock(sk);
729                 if (sock_flag(sk, SOCK_URGINLINE) &&
730                     !skb_queue_empty(&pn->ctrlreq_queue))
731                         answ = skb_peek(&pn->ctrlreq_queue)->len;
732                 else if (!skb_queue_empty(&sk->sk_receive_queue))
733                         answ = skb_peek(&sk->sk_receive_queue)->len;
734                 else
735                         answ = 0;
736                 release_sock(sk);
737                 return put_user(answ, (int __user *)arg);
738         }
739
740         return -ENOIOCTLCMD;
741 }
742
743 static int pep_init(struct sock *sk)
744 {
745         struct pep_sock *pn = pep_sk(sk);
746
747         INIT_HLIST_HEAD(&pn->ackq);
748         INIT_HLIST_HEAD(&pn->hlist);
749         skb_queue_head_init(&pn->ctrlreq_queue);
750         pn->pipe_handle = PN_PIPE_INVALID_HANDLE;
751         return 0;
752 }
753
754 static int pep_setsockopt(struct sock *sk, int level, int optname,
755                                 char __user *optval, unsigned int optlen)
756 {
757         struct pep_sock *pn = pep_sk(sk);
758         int val = 0, err = 0;
759
760         if (level != SOL_PNPIPE)
761                 return -ENOPROTOOPT;
762         if (optlen >= sizeof(int)) {
763                 if (get_user(val, (int __user *) optval))
764                         return -EFAULT;
765         }
766
767         lock_sock(sk);
768         switch (optname) {
769         case PNPIPE_ENCAP:
770                 if (val && val != PNPIPE_ENCAP_IP) {
771                         err = -EINVAL;
772                         break;
773                 }
774                 if (!pn->ifindex == !val)
775                         break; /* Nothing to do! */
776                 if (!capable(CAP_NET_ADMIN)) {
777                         err = -EPERM;
778                         break;
779                 }
780                 if (val) {
781                         release_sock(sk);
782                         err = gprs_attach(sk);
783                         if (err > 0) {
784                                 pn->ifindex = err;
785                                 err = 0;
786                         }
787                 } else {
788                         pn->ifindex = 0;
789                         release_sock(sk);
790                         gprs_detach(sk);
791                         err = 0;
792                 }
793                 goto out_norel;
794         default:
795                 err = -ENOPROTOOPT;
796         }
797         release_sock(sk);
798
799 out_norel:
800         return err;
801 }
802
803 static int pep_getsockopt(struct sock *sk, int level, int optname,
804                                 char __user *optval, int __user *optlen)
805 {
806         struct pep_sock *pn = pep_sk(sk);
807         int len, val;
808
809         if (level != SOL_PNPIPE)
810                 return -ENOPROTOOPT;
811         if (get_user(len, optlen))
812                 return -EFAULT;
813
814         switch (optname) {
815         case PNPIPE_ENCAP:
816                 val = pn->ifindex ? PNPIPE_ENCAP_IP : PNPIPE_ENCAP_NONE;
817                 break;
818         case PNPIPE_IFINDEX:
819                 val = pn->ifindex;
820                 break;
821         default:
822                 return -ENOPROTOOPT;
823         }
824
825         len = min_t(unsigned int, sizeof(int), len);
826         if (put_user(len, optlen))
827                 return -EFAULT;
828         if (put_user(val, (int __user *) optval))
829                 return -EFAULT;
830         return 0;
831 }
832
833 static int pipe_skb_send(struct sock *sk, struct sk_buff *skb)
834 {
835         struct pep_sock *pn = pep_sk(sk);
836         struct pnpipehdr *ph;
837
838         if (pn_flow_safe(pn->tx_fc) &&
839             !atomic_add_unless(&pn->tx_credits, -1, 0)) {
840                 kfree_skb(skb);
841                 return -ENOBUFS;
842         }
843
844         skb_push(skb, 3 + pn->aligned);
845         skb_reset_transport_header(skb);
846         ph = pnp_hdr(skb);
847         ph->utid = 0;
848         if (pn->aligned) {
849                 ph->message_id = PNS_PIPE_ALIGNED_DATA;
850                 ph->data[0] = 0; /* padding */
851         } else
852                 ph->message_id = PNS_PIPE_DATA;
853         ph->pipe_handle = pn->pipe_handle;
854
855         return pn_skb_send(sk, skb, &pipe_srv);
856 }
857
858 static int pep_sendmsg(struct kiocb *iocb, struct sock *sk,
859                         struct msghdr *msg, size_t len)
860 {
861         struct pep_sock *pn = pep_sk(sk);
862         struct sk_buff *skb;
863         long timeo;
864         int flags = msg->msg_flags;
865         int err, done;
866
867         if ((msg->msg_flags & ~(MSG_DONTWAIT|MSG_EOR|MSG_NOSIGNAL|
868                                 MSG_CMSG_COMPAT)) ||
869                         !(msg->msg_flags & MSG_EOR))
870                 return -EOPNOTSUPP;
871
872         skb = sock_alloc_send_skb(sk, MAX_PNPIPE_HEADER + len,
873                                         flags & MSG_DONTWAIT, &err);
874         if (!skb)
875                 return -ENOBUFS;
876
877         skb_reserve(skb, MAX_PHONET_HEADER + 3);
878         err = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len);
879         if (err < 0)
880                 goto outfree;
881
882         lock_sock(sk);
883         timeo = sock_sndtimeo(sk, flags & MSG_DONTWAIT);
884         if ((1 << sk->sk_state) & (TCPF_LISTEN|TCPF_CLOSE)) {
885                 err = -ENOTCONN;
886                 goto out;
887         }
888         if (sk->sk_state != TCP_ESTABLISHED) {
889                 /* Wait until the pipe gets to enabled state */
890 disabled:
891                 err = sk_stream_wait_connect(sk, &timeo);
892                 if (err)
893                         goto out;
894
895                 if (sk->sk_state == TCP_CLOSE_WAIT) {
896                         err = -ECONNRESET;
897                         goto out;
898                 }
899         }
900         BUG_ON(sk->sk_state != TCP_ESTABLISHED);
901
902         /* Wait until flow control allows TX */
903         done = atomic_read(&pn->tx_credits);
904         while (!done) {
905                 DEFINE_WAIT(wait);
906
907                 if (!timeo) {
908                         err = -EAGAIN;
909                         goto out;
910                 }
911                 if (signal_pending(current)) {
912                         err = sock_intr_errno(timeo);
913                         goto out;
914                 }
915
916                 prepare_to_wait(sk_sleep(sk), &wait,
917                                 TASK_INTERRUPTIBLE);
918                 done = sk_wait_event(sk, &timeo, atomic_read(&pn->tx_credits));
919                 finish_wait(sk_sleep(sk), &wait);
920
921                 if (sk->sk_state != TCP_ESTABLISHED)
922                         goto disabled;
923         }
924
925         err = pipe_skb_send(sk, skb);
926         if (err >= 0)
927                 err = len; /* success! */
928         skb = NULL;
929 out:
930         release_sock(sk);
931 outfree:
932         kfree_skb(skb);
933         return err;
934 }
935
936 int pep_writeable(struct sock *sk)
937 {
938         struct pep_sock *pn = pep_sk(sk);
939
940         return atomic_read(&pn->tx_credits);
941 }
942
943 int pep_write(struct sock *sk, struct sk_buff *skb)
944 {
945         struct sk_buff *rskb, *fs;
946         int flen = 0;
947
948         if (pep_sk(sk)->aligned)
949                 return pipe_skb_send(sk, skb);
950
951         rskb = alloc_skb(MAX_PNPIPE_HEADER, GFP_ATOMIC);
952         if (!rskb) {
953                 kfree_skb(skb);
954                 return -ENOMEM;
955         }
956         skb_shinfo(rskb)->frag_list = skb;
957         rskb->len += skb->len;
958         rskb->data_len += rskb->len;
959         rskb->truesize += rskb->len;
960
961         /* Avoid nested fragments */
962         skb_walk_frags(skb, fs)
963                 flen += fs->len;
964         skb->next = skb_shinfo(skb)->frag_list;
965         skb_frag_list_init(skb);
966         skb->len -= flen;
967         skb->data_len -= flen;
968         skb->truesize -= flen;
969
970         skb_reserve(rskb, MAX_PHONET_HEADER + 3);
971         return pipe_skb_send(sk, rskb);
972 }
973
974 struct sk_buff *pep_read(struct sock *sk)
975 {
976         struct sk_buff *skb = skb_dequeue(&sk->sk_receive_queue);
977
978         if (sk->sk_state == TCP_ESTABLISHED)
979                 pipe_grant_credits(sk);
980         return skb;
981 }
982
983 static int pep_recvmsg(struct kiocb *iocb, struct sock *sk,
984                         struct msghdr *msg, size_t len, int noblock,
985                         int flags, int *addr_len)
986 {
987         struct sk_buff *skb;
988         int err;
989
990         if (flags & ~(MSG_OOB|MSG_PEEK|MSG_TRUNC|MSG_DONTWAIT|MSG_WAITALL|
991                         MSG_NOSIGNAL|MSG_CMSG_COMPAT))
992                 return -EOPNOTSUPP;
993
994         if (unlikely(1 << sk->sk_state & (TCPF_LISTEN | TCPF_CLOSE)))
995                 return -ENOTCONN;
996
997         if ((flags & MSG_OOB) || sock_flag(sk, SOCK_URGINLINE)) {
998                 /* Dequeue and acknowledge control request */
999                 struct pep_sock *pn = pep_sk(sk);
1000
1001                 if (flags & MSG_PEEK)
1002                         return -EOPNOTSUPP;
1003                 skb = skb_dequeue(&pn->ctrlreq_queue);
1004                 if (skb) {
1005                         pep_ctrlreq_error(sk, skb, PN_PIPE_NO_ERROR,
1006                                                 GFP_KERNEL);
1007                         msg->msg_flags |= MSG_OOB;
1008                         goto copy;
1009                 }
1010                 if (flags & MSG_OOB)
1011                         return -EINVAL;
1012         }
1013
1014         skb = skb_recv_datagram(sk, flags, noblock, &err);
1015         lock_sock(sk);
1016         if (skb == NULL) {
1017                 if (err == -ENOTCONN && sk->sk_state == TCP_CLOSE_WAIT)
1018                         err = -ECONNRESET;
1019                 release_sock(sk);
1020                 return err;
1021         }
1022
1023         if (sk->sk_state == TCP_ESTABLISHED)
1024                 pipe_grant_credits(sk);
1025         release_sock(sk);
1026 copy:
1027         msg->msg_flags |= MSG_EOR;
1028         if (skb->len > len)
1029                 msg->msg_flags |= MSG_TRUNC;
1030         else
1031                 len = skb->len;
1032
1033         err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, len);
1034         if (!err)
1035                 err = (flags & MSG_TRUNC) ? skb->len : len;
1036
1037         skb_free_datagram(sk, skb);
1038         return err;
1039 }
1040
1041 static void pep_sock_unhash(struct sock *sk)
1042 {
1043         struct pep_sock *pn = pep_sk(sk);
1044         struct sock *skparent = NULL;
1045
1046         lock_sock(sk);
1047         if ((1 << sk->sk_state) & ~(TCPF_CLOSE|TCPF_LISTEN)) {
1048                 skparent = pn->listener;
1049                 release_sock(sk);
1050
1051                 pn = pep_sk(skparent);
1052                 lock_sock(skparent);
1053                 sk_del_node_init(sk);
1054                 sk = skparent;
1055         }
1056         /* Unhash a listening sock only when it is closed
1057          * and all of its active connected pipes are closed. */
1058         if (hlist_empty(&pn->hlist))
1059                 pn_sock_unhash(&pn->pn_sk.sk);
1060         release_sock(sk);
1061
1062         if (skparent)
1063                 sock_put(skparent);
1064 }
1065
1066 static struct proto pep_proto = {
1067         .close          = pep_sock_close,
1068         .accept         = pep_sock_accept,
1069         .ioctl          = pep_ioctl,
1070         .init           = pep_init,
1071         .setsockopt     = pep_setsockopt,
1072         .getsockopt     = pep_getsockopt,
1073         .sendmsg        = pep_sendmsg,
1074         .recvmsg        = pep_recvmsg,
1075         .backlog_rcv    = pep_do_rcv,
1076         .hash           = pn_sock_hash,
1077         .unhash         = pep_sock_unhash,
1078         .get_port       = pn_sock_get_port,
1079         .obj_size       = sizeof(struct pep_sock),
1080         .owner          = THIS_MODULE,
1081         .name           = "PNPIPE",
1082 };
1083
1084 static struct phonet_protocol pep_pn_proto = {
1085         .ops            = &phonet_stream_ops,
1086         .prot           = &pep_proto,
1087         .sock_type      = SOCK_SEQPACKET,
1088 };
1089
1090 static int __init pep_register(void)
1091 {
1092         return phonet_proto_register(PN_PROTO_PIPE, &pep_pn_proto);
1093 }
1094
1095 static void __exit pep_unregister(void)
1096 {
1097         phonet_proto_unregister(PN_PROTO_PIPE, &pep_pn_proto);
1098 }
1099
1100 module_init(pep_register);
1101 module_exit(pep_unregister);
1102 MODULE_AUTHOR("Remi Denis-Courmont, Nokia");
1103 MODULE_DESCRIPTION("Phonet pipe protocol");
1104 MODULE_LICENSE("GPL");
1105 MODULE_ALIAS_NET_PF_PROTO(PF_PHONET, PN_PROTO_PIPE);