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