Context Navigation

source: src/linux/universal/linux-3.2/net/phonet/pep.c @ 18171

Last change on this file since 18171 was 18171, checked in by BrainSlayer, 16 months ago
this kernel will be maintained for all targets, so target specific kernel trees will not be neccessary anymore in future
File size: 28.8 KB

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

Note: See TracBrowser for help on using the repository browser.

Download in other formats: