d0e7eb24c8b9da87a842d9337561663ecc5d5a40
[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 static int pipe_do_remove(struct sock *sk)
624 {
625         struct pep_sock *pn = pep_sk(sk);
626         struct pnpipehdr *ph;
627         struct sk_buff *skb;
628
629         skb = alloc_skb(MAX_PNPIPE_HEADER, GFP_KERNEL);
630         if (!skb)
631                 return -ENOMEM;
632
633         skb_reserve(skb, MAX_PNPIPE_HEADER);
634         __skb_push(skb, sizeof(*ph));
635         skb_reset_transport_header(skb);
636         ph = pnp_hdr(skb);
637         ph->utid = 0;
638         ph->message_id = PNS_PIPE_REMOVE_REQ;
639         ph->pipe_handle = pn->pipe_handle;
640         ph->data[0] = PAD;
641
642         return pn_skb_send(sk, skb, &pipe_srv);
643 }
644
645 /* associated socket ceases to exist */
646 static void pep_sock_close(struct sock *sk, long timeout)
647 {
648         struct pep_sock *pn = pep_sk(sk);
649         int ifindex = 0;
650
651         sock_hold(sk); /* keep a reference after sk_common_release() */
652         sk_common_release(sk);
653
654         lock_sock(sk);
655         if (sk->sk_state == TCP_LISTEN) {
656                 /* Destroy the listen queue */
657                 struct sock *sknode;
658                 struct hlist_node *p, *n;
659
660                 sk_for_each_safe(sknode, p, n, &pn->ackq)
661                         sk_del_node_init(sknode);
662                 sk->sk_state = TCP_CLOSE;
663         } else if ((1 << sk->sk_state) & (TCPF_SYN_RECV|TCPF_ESTABLISHED))
664                 /* Forcefully remove dangling Phonet pipe */
665                 pipe_do_remove(sk);
666
667         ifindex = pn->ifindex;
668         pn->ifindex = 0;
669         release_sock(sk);
670
671         if (ifindex)
672                 gprs_detach(sk);
673         sock_put(sk);
674 }
675
676 static int pep_wait_connreq(struct sock *sk, int noblock)
677 {
678         struct task_struct *tsk = current;
679         struct pep_sock *pn = pep_sk(sk);
680         long timeo = sock_rcvtimeo(sk, noblock);
681
682         for (;;) {
683                 DEFINE_WAIT(wait);
684
685                 if (sk->sk_state != TCP_LISTEN)
686                         return -EINVAL;
687                 if (!hlist_empty(&pn->ackq))
688                         break;
689                 if (!timeo)
690                         return -EWOULDBLOCK;
691                 if (signal_pending(tsk))
692                         return sock_intr_errno(timeo);
693
694                 prepare_to_wait_exclusive(sk_sleep(sk), &wait,
695                                                 TASK_INTERRUPTIBLE);
696                 release_sock(sk);
697                 timeo = schedule_timeout(timeo);
698                 lock_sock(sk);
699                 finish_wait(sk_sleep(sk), &wait);
700         }
701
702         return 0;
703 }
704
705 static struct sock *pep_sock_accept(struct sock *sk, int flags, int *errp)
706 {
707         struct pep_sock *pn = pep_sk(sk);
708         struct sock *newsk = NULL;
709         struct sk_buff *oskb;
710         int err;
711
712         lock_sock(sk);
713         err = pep_wait_connreq(sk, flags & O_NONBLOCK);
714         if (err)
715                 goto out;
716
717         newsk = __sk_head(&pn->ackq);
718
719         oskb = skb_dequeue(&newsk->sk_receive_queue);
720         err = pep_accept_conn(newsk, oskb);
721         if (err) {
722                 skb_queue_head(&newsk->sk_receive_queue, oskb);
723                 newsk = NULL;
724                 goto out;
725         }
726         kfree_skb(oskb);
727
728         sock_hold(sk);
729         pep_sk(newsk)->listener = sk;
730
731         sock_hold(newsk);
732         sk_del_node_init(newsk);
733         sk_acceptq_removed(sk);
734         sk_add_node(newsk, &pn->hlist);
735         __sock_put(newsk);
736
737 out:
738         release_sock(sk);
739         *errp = err;
740         return newsk;
741 }
742
743 static int pep_ioctl(struct sock *sk, int cmd, unsigned long arg)
744 {
745         struct pep_sock *pn = pep_sk(sk);
746         int answ;
747
748         switch (cmd) {
749         case SIOCINQ:
750                 if (sk->sk_state == TCP_LISTEN)
751                         return -EINVAL;
752
753                 lock_sock(sk);
754                 if (sock_flag(sk, SOCK_URGINLINE) &&
755                     !skb_queue_empty(&pn->ctrlreq_queue))
756                         answ = skb_peek(&pn->ctrlreq_queue)->len;
757                 else if (!skb_queue_empty(&sk->sk_receive_queue))
758                         answ = skb_peek(&sk->sk_receive_queue)->len;
759                 else
760                         answ = 0;
761                 release_sock(sk);
762                 return put_user(answ, (int __user *)arg);
763         }
764
765         return -ENOIOCTLCMD;
766 }
767
768 static int pep_init(struct sock *sk)
769 {
770         struct pep_sock *pn = pep_sk(sk);
771
772         INIT_HLIST_HEAD(&pn->ackq);
773         INIT_HLIST_HEAD(&pn->hlist);
774         skb_queue_head_init(&pn->ctrlreq_queue);
775         pn->pipe_handle = PN_PIPE_INVALID_HANDLE;
776         return 0;
777 }
778
779 static int pep_setsockopt(struct sock *sk, int level, int optname,
780                                 char __user *optval, unsigned int optlen)
781 {
782         struct pep_sock *pn = pep_sk(sk);
783         int val = 0, err = 0;
784
785         if (level != SOL_PNPIPE)
786                 return -ENOPROTOOPT;
787         if (optlen >= sizeof(int)) {
788                 if (get_user(val, (int __user *) optval))
789                         return -EFAULT;
790         }
791
792         lock_sock(sk);
793         switch (optname) {
794         case PNPIPE_ENCAP:
795                 if (val && val != PNPIPE_ENCAP_IP) {
796                         err = -EINVAL;
797                         break;
798                 }
799                 if (!pn->ifindex == !val)
800                         break; /* Nothing to do! */
801                 if (!capable(CAP_NET_ADMIN)) {
802                         err = -EPERM;
803                         break;
804                 }
805                 if (val) {
806                         release_sock(sk);
807                         err = gprs_attach(sk);
808                         if (err > 0) {
809                                 pn->ifindex = err;
810                                 err = 0;
811                         }
812                 } else {
813                         pn->ifindex = 0;
814                         release_sock(sk);
815                         gprs_detach(sk);
816                         err = 0;
817                 }
818                 goto out_norel;
819         default:
820                 err = -ENOPROTOOPT;
821         }
822         release_sock(sk);
823
824 out_norel:
825         return err;
826 }
827
828 static int pep_getsockopt(struct sock *sk, int level, int optname,
829                                 char __user *optval, int __user *optlen)
830 {
831         struct pep_sock *pn = pep_sk(sk);
832         int len, val;
833
834         if (level != SOL_PNPIPE)
835                 return -ENOPROTOOPT;
836         if (get_user(len, optlen))
837                 return -EFAULT;
838
839         switch (optname) {
840         case PNPIPE_ENCAP:
841                 val = pn->ifindex ? PNPIPE_ENCAP_IP : PNPIPE_ENCAP_NONE;
842                 break;
843         case PNPIPE_IFINDEX:
844                 val = pn->ifindex;
845                 break;
846         default:
847                 return -ENOPROTOOPT;
848         }
849
850         len = min_t(unsigned int, sizeof(int), len);
851         if (put_user(len, optlen))
852                 return -EFAULT;
853         if (put_user(val, (int __user *) optval))
854                 return -EFAULT;
855         return 0;
856 }
857
858 static int pipe_skb_send(struct sock *sk, struct sk_buff *skb)
859 {
860         struct pep_sock *pn = pep_sk(sk);
861         struct pnpipehdr *ph;
862         int err;
863
864         if (pn_flow_safe(pn->tx_fc) &&
865             !atomic_add_unless(&pn->tx_credits, -1, 0)) {
866                 kfree_skb(skb);
867                 return -ENOBUFS;
868         }
869
870         skb_push(skb, 3 + pn->aligned);
871         skb_reset_transport_header(skb);
872         ph = pnp_hdr(skb);
873         ph->utid = 0;
874         if (pn->aligned) {
875                 ph->message_id = PNS_PIPE_ALIGNED_DATA;
876                 ph->data[0] = 0; /* padding */
877         } else
878                 ph->message_id = PNS_PIPE_DATA;
879         ph->pipe_handle = pn->pipe_handle;
880
881         err = pn_skb_send(sk, skb, &pipe_srv);
882         if (err && pn_flow_safe(pn->tx_fc))
883                 atomic_inc(&pn->tx_credits);
884         return err;
885 }
886
887 static int pep_sendmsg(struct kiocb *iocb, struct sock *sk,
888                         struct msghdr *msg, size_t len)
889 {
890         struct pep_sock *pn = pep_sk(sk);
891         struct sk_buff *skb;
892         long timeo;
893         int flags = msg->msg_flags;
894         int err, done;
895
896         if ((msg->msg_flags & ~(MSG_DONTWAIT|MSG_EOR|MSG_NOSIGNAL|
897                                 MSG_CMSG_COMPAT)) ||
898                         !(msg->msg_flags & MSG_EOR))
899                 return -EOPNOTSUPP;
900
901         skb = sock_alloc_send_skb(sk, MAX_PNPIPE_HEADER + len,
902                                         flags & MSG_DONTWAIT, &err);
903         if (!skb)
904                 return err;
905
906         skb_reserve(skb, MAX_PHONET_HEADER + 3);
907         err = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len);
908         if (err < 0)
909                 goto outfree;
910
911         lock_sock(sk);
912         timeo = sock_sndtimeo(sk, flags & MSG_DONTWAIT);
913         if ((1 << sk->sk_state) & (TCPF_LISTEN|TCPF_CLOSE)) {
914                 err = -ENOTCONN;
915                 goto out;
916         }
917         if (sk->sk_state != TCP_ESTABLISHED) {
918                 /* Wait until the pipe gets to enabled state */
919 disabled:
920                 err = sk_stream_wait_connect(sk, &timeo);
921                 if (err)
922                         goto out;
923
924                 if (sk->sk_state == TCP_CLOSE_WAIT) {
925                         err = -ECONNRESET;
926                         goto out;
927                 }
928         }
929         BUG_ON(sk->sk_state != TCP_ESTABLISHED);
930
931         /* Wait until flow control allows TX */
932         done = atomic_read(&pn->tx_credits);
933         while (!done) {
934                 DEFINE_WAIT(wait);
935
936                 if (!timeo) {
937                         err = -EAGAIN;
938                         goto out;
939                 }
940                 if (signal_pending(current)) {
941                         err = sock_intr_errno(timeo);
942                         goto out;
943                 }
944
945                 prepare_to_wait(sk_sleep(sk), &wait,
946                                 TASK_INTERRUPTIBLE);
947                 done = sk_wait_event(sk, &timeo, atomic_read(&pn->tx_credits));
948                 finish_wait(sk_sleep(sk), &wait);
949
950                 if (sk->sk_state != TCP_ESTABLISHED)
951                         goto disabled;
952         }
953
954         err = pipe_skb_send(sk, skb);
955         if (err >= 0)
956                 err = len; /* success! */
957         skb = NULL;
958 out:
959         release_sock(sk);
960 outfree:
961         kfree_skb(skb);
962         return err;
963 }
964
965 int pep_writeable(struct sock *sk)
966 {
967         struct pep_sock *pn = pep_sk(sk);
968
969         return atomic_read(&pn->tx_credits);
970 }
971
972 int pep_write(struct sock *sk, struct sk_buff *skb)
973 {
974         struct sk_buff *rskb, *fs;
975         int flen = 0;
976
977         if (pep_sk(sk)->aligned)
978                 return pipe_skb_send(sk, skb);
979
980         rskb = alloc_skb(MAX_PNPIPE_HEADER, GFP_ATOMIC);
981         if (!rskb) {
982                 kfree_skb(skb);
983                 return -ENOMEM;
984         }
985         skb_shinfo(rskb)->frag_list = skb;
986         rskb->len += skb->len;
987         rskb->data_len += rskb->len;
988         rskb->truesize += rskb->len;
989
990         /* Avoid nested fragments */
991         skb_walk_frags(skb, fs)
992                 flen += fs->len;
993         skb->next = skb_shinfo(skb)->frag_list;
994         skb_frag_list_init(skb);
995         skb->len -= flen;
996         skb->data_len -= flen;
997         skb->truesize -= flen;
998
999         skb_reserve(rskb, MAX_PHONET_HEADER + 3);
1000         return pipe_skb_send(sk, rskb);
1001 }
1002
1003 struct sk_buff *pep_read(struct sock *sk)
1004 {
1005         struct sk_buff *skb = skb_dequeue(&sk->sk_receive_queue);
1006
1007         if (sk->sk_state == TCP_ESTABLISHED)
1008                 pipe_grant_credits(sk);
1009         return skb;
1010 }
1011
1012 static int pep_recvmsg(struct kiocb *iocb, struct sock *sk,
1013                         struct msghdr *msg, size_t len, int noblock,
1014                         int flags, int *addr_len)
1015 {
1016         struct sk_buff *skb;
1017         int err;
1018
1019         if (flags & ~(MSG_OOB|MSG_PEEK|MSG_TRUNC|MSG_DONTWAIT|MSG_WAITALL|
1020                         MSG_NOSIGNAL|MSG_CMSG_COMPAT))
1021                 return -EOPNOTSUPP;
1022
1023         if (unlikely(1 << sk->sk_state & (TCPF_LISTEN | TCPF_CLOSE)))
1024                 return -ENOTCONN;
1025
1026         if ((flags & MSG_OOB) || sock_flag(sk, SOCK_URGINLINE)) {
1027                 /* Dequeue and acknowledge control request */
1028                 struct pep_sock *pn = pep_sk(sk);
1029
1030                 if (flags & MSG_PEEK)
1031                         return -EOPNOTSUPP;
1032                 skb = skb_dequeue(&pn->ctrlreq_queue);
1033                 if (skb) {
1034                         pep_ctrlreq_error(sk, skb, PN_PIPE_NO_ERROR,
1035                                                 GFP_KERNEL);
1036                         msg->msg_flags |= MSG_OOB;
1037                         goto copy;
1038                 }
1039                 if (flags & MSG_OOB)
1040                         return -EINVAL;
1041         }
1042
1043         skb = skb_recv_datagram(sk, flags, noblock, &err);
1044         lock_sock(sk);
1045         if (skb == NULL) {
1046                 if (err == -ENOTCONN && sk->sk_state == TCP_CLOSE_WAIT)
1047                         err = -ECONNRESET;
1048                 release_sock(sk);
1049                 return err;
1050         }
1051
1052         if (sk->sk_state == TCP_ESTABLISHED)
1053                 pipe_grant_credits(sk);
1054         release_sock(sk);
1055 copy:
1056         msg->msg_flags |= MSG_EOR;
1057         if (skb->len > len)
1058                 msg->msg_flags |= MSG_TRUNC;
1059         else
1060                 len = skb->len;
1061
1062         err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, len);
1063         if (!err)
1064                 err = (flags & MSG_TRUNC) ? skb->len : len;
1065
1066         skb_free_datagram(sk, skb);
1067         return err;
1068 }
1069
1070 static void pep_sock_unhash(struct sock *sk)
1071 {
1072         struct pep_sock *pn = pep_sk(sk);
1073         struct sock *skparent = NULL;
1074
1075         lock_sock(sk);
1076         if ((1 << sk->sk_state) & ~(TCPF_CLOSE|TCPF_LISTEN)) {
1077                 skparent = pn->listener;
1078                 release_sock(sk);
1079
1080                 pn = pep_sk(skparent);
1081                 lock_sock(skparent);
1082                 sk_del_node_init(sk);
1083                 sk = skparent;
1084         }
1085         /* Unhash a listening sock only when it is closed
1086          * and all of its active connected pipes are closed. */
1087         if (hlist_empty(&pn->hlist))
1088                 pn_sock_unhash(&pn->pn_sk.sk);
1089         release_sock(sk);
1090
1091         if (skparent)
1092                 sock_put(skparent);
1093 }
1094
1095 static struct proto pep_proto = {
1096         .close          = pep_sock_close,
1097         .accept         = pep_sock_accept,
1098         .ioctl          = pep_ioctl,
1099         .init           = pep_init,
1100         .setsockopt     = pep_setsockopt,
1101         .getsockopt     = pep_getsockopt,
1102         .sendmsg        = pep_sendmsg,
1103         .recvmsg        = pep_recvmsg,
1104         .backlog_rcv    = pep_do_rcv,
1105         .hash           = pn_sock_hash,
1106         .unhash         = pep_sock_unhash,
1107         .get_port       = pn_sock_get_port,
1108         .obj_size       = sizeof(struct pep_sock),
1109         .owner          = THIS_MODULE,
1110         .name           = "PNPIPE",
1111 };
1112
1113 static struct phonet_protocol pep_pn_proto = {
1114         .ops            = &phonet_stream_ops,
1115         .prot           = &pep_proto,
1116         .sock_type      = SOCK_SEQPACKET,
1117 };
1118
1119 static int __init pep_register(void)
1120 {
1121         return phonet_proto_register(PN_PROTO_PIPE, &pep_pn_proto);
1122 }
1123
1124 static void __exit pep_unregister(void)
1125 {
1126         phonet_proto_unregister(PN_PROTO_PIPE, &pep_pn_proto);
1127 }
1128
1129 module_init(pep_register);
1130 module_exit(pep_unregister);
1131 MODULE_AUTHOR("Remi Denis-Courmont, Nokia");
1132 MODULE_DESCRIPTION("Phonet pipe protocol");
1133 MODULE_LICENSE("GPL");
1134 MODULE_ALIAS_NET_PF_PROTO(PF_PHONET, PN_PROTO_PIPE);