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