3 * Author Karsten Keil <kkeil@novell.com>
5 * Copyright 2008 by Karsten Keil <kkeil@novell.com>
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License version 2 as
9 * published by the Free Software Foundation.
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
18 #include <linux/mISDNif.h>
19 #include <linux/slab.h>
20 #include <linux/export.h>
25 static struct proto mISDN_proto
= {
28 .obj_size
= sizeof(struct mISDN_sock
)
31 #define _pms(sk) ((struct mISDN_sock *)sk)
33 static struct mISDN_sock_list data_sockets
= {
34 .lock
= __RW_LOCK_UNLOCKED(data_sockets
.lock
)
37 static struct mISDN_sock_list base_sockets
= {
38 .lock
= __RW_LOCK_UNLOCKED(base_sockets
.lock
)
41 #define L2_HEADER_LEN 4
43 static inline struct sk_buff
*
44 _l2_alloc_skb(unsigned int len
, gfp_t gfp_mask
)
48 skb
= alloc_skb(len
+ L2_HEADER_LEN
, gfp_mask
);
50 skb_reserve(skb
, L2_HEADER_LEN
);
55 mISDN_sock_link(struct mISDN_sock_list
*l
, struct sock
*sk
)
57 write_lock_bh(&l
->lock
);
58 sk_add_node(sk
, &l
->head
);
59 write_unlock_bh(&l
->lock
);
62 static void mISDN_sock_unlink(struct mISDN_sock_list
*l
, struct sock
*sk
)
64 write_lock_bh(&l
->lock
);
66 write_unlock_bh(&l
->lock
);
70 mISDN_send(struct mISDNchannel
*ch
, struct sk_buff
*skb
)
72 struct mISDN_sock
*msk
;
75 msk
= container_of(ch
, struct mISDN_sock
, ch
);
76 if (*debug
& DEBUG_SOCKET
)
77 printk(KERN_DEBUG
"%s len %d %p\n", __func__
, skb
->len
, skb
);
78 if (msk
->sk
.sk_state
== MISDN_CLOSED
)
81 err
= sock_queue_rcv_skb(&msk
->sk
, skb
);
83 printk(KERN_WARNING
"%s: error %d\n", __func__
, err
);
88 mISDN_ctrl(struct mISDNchannel
*ch
, u_int cmd
, void *arg
)
90 struct mISDN_sock
*msk
;
92 msk
= container_of(ch
, struct mISDN_sock
, ch
);
93 if (*debug
& DEBUG_SOCKET
)
94 printk(KERN_DEBUG
"%s(%p, %x, %p)\n", __func__
, ch
, cmd
, arg
);
97 msk
->sk
.sk_state
= MISDN_CLOSED
;
104 mISDN_sock_cmsg(struct sock
*sk
, struct msghdr
*msg
, struct sk_buff
*skb
)
108 if (_pms(sk
)->cmask
& MISDN_TIME_STAMP
) {
109 skb_get_timestamp(skb
, &tv
);
110 put_cmsg(msg
, SOL_MISDN
, MISDN_TIME_STAMP
, sizeof(tv
), &tv
);
115 mISDN_sock_recvmsg(struct kiocb
*iocb
, struct socket
*sock
,
116 struct msghdr
*msg
, size_t len
, int flags
)
119 struct sock
*sk
= sock
->sk
;
120 struct sockaddr_mISDN
*maddr
;
124 if (*debug
& DEBUG_SOCKET
)
125 printk(KERN_DEBUG
"%s: len %d, flags %x ch.nr %d, proto %x\n",
126 __func__
, (int)len
, flags
, _pms(sk
)->ch
.nr
,
128 if (flags
& (MSG_OOB
))
131 if (sk
->sk_state
== MISDN_CLOSED
)
134 skb
= skb_recv_datagram(sk
, flags
, flags
& MSG_DONTWAIT
, &err
);
138 if (msg
->msg_namelen
>= sizeof(struct sockaddr_mISDN
)) {
139 msg
->msg_namelen
= sizeof(struct sockaddr_mISDN
);
140 maddr
= (struct sockaddr_mISDN
*)msg
->msg_name
;
141 maddr
->family
= AF_ISDN
;
142 maddr
->dev
= _pms(sk
)->dev
->id
;
143 if ((sk
->sk_protocol
== ISDN_P_LAPD_TE
) ||
144 (sk
->sk_protocol
== ISDN_P_LAPD_NT
)) {
145 maddr
->channel
= (mISDN_HEAD_ID(skb
) >> 16) & 0xff;
146 maddr
->tei
= (mISDN_HEAD_ID(skb
) >> 8) & 0xff;
147 maddr
->sapi
= mISDN_HEAD_ID(skb
) & 0xff;
149 maddr
->channel
= _pms(sk
)->ch
.nr
;
150 maddr
->sapi
= _pms(sk
)->ch
.addr
& 0xFF;
151 maddr
->tei
= (_pms(sk
)->ch
.addr
>> 8) & 0xFF;
154 if (msg
->msg_namelen
)
155 printk(KERN_WARNING
"%s: too small namelen %d\n",
156 __func__
, msg
->msg_namelen
);
157 msg
->msg_namelen
= 0;
160 copied
= skb
->len
+ MISDN_HEADER_LEN
;
162 if (flags
& MSG_PEEK
)
163 atomic_dec(&skb
->users
);
165 skb_queue_head(&sk
->sk_receive_queue
, skb
);
168 memcpy(skb_push(skb
, MISDN_HEADER_LEN
), mISDN_HEAD_P(skb
),
171 err
= skb_copy_datagram_iovec(skb
, 0, msg
->msg_iov
, copied
);
173 mISDN_sock_cmsg(sk
, msg
, skb
);
175 skb_free_datagram(sk
, skb
);
177 return err
? : copied
;
181 mISDN_sock_sendmsg(struct kiocb
*iocb
, struct socket
*sock
,
182 struct msghdr
*msg
, size_t len
)
184 struct sock
*sk
= sock
->sk
;
187 struct sockaddr_mISDN
*maddr
;
189 if (*debug
& DEBUG_SOCKET
)
190 printk(KERN_DEBUG
"%s: len %d flags %x ch %d proto %x\n",
191 __func__
, (int)len
, msg
->msg_flags
, _pms(sk
)->ch
.nr
,
194 if (msg
->msg_flags
& MSG_OOB
)
197 if (msg
->msg_flags
& ~(MSG_DONTWAIT
| MSG_NOSIGNAL
| MSG_ERRQUEUE
))
200 if (len
< MISDN_HEADER_LEN
)
203 if (sk
->sk_state
!= MISDN_BOUND
)
208 skb
= _l2_alloc_skb(len
, GFP_KERNEL
);
212 if (memcpy_fromiovec(skb_put(skb
, len
), msg
->msg_iov
, len
)) {
217 memcpy(mISDN_HEAD_P(skb
), skb
->data
, MISDN_HEADER_LEN
);
218 skb_pull(skb
, MISDN_HEADER_LEN
);
220 if (msg
->msg_namelen
>= sizeof(struct sockaddr_mISDN
)) {
221 /* if we have a address, we use it */
222 maddr
= (struct sockaddr_mISDN
*)msg
->msg_name
;
223 mISDN_HEAD_ID(skb
) = maddr
->channel
;
224 } else { /* use default for L2 messages */
225 if ((sk
->sk_protocol
== ISDN_P_LAPD_TE
) ||
226 (sk
->sk_protocol
== ISDN_P_LAPD_NT
))
227 mISDN_HEAD_ID(skb
) = _pms(sk
)->ch
.nr
;
230 if (*debug
& DEBUG_SOCKET
)
231 printk(KERN_DEBUG
"%s: ID:%x\n",
232 __func__
, mISDN_HEAD_ID(skb
));
235 if (!_pms(sk
)->ch
.peer
)
237 err
= _pms(sk
)->ch
.recv(_pms(sk
)->ch
.peer
, skb
);
253 data_sock_release(struct socket
*sock
)
255 struct sock
*sk
= sock
->sk
;
257 if (*debug
& DEBUG_SOCKET
)
258 printk(KERN_DEBUG
"%s(%p) sk=%p\n", __func__
, sock
, sk
);
261 switch (sk
->sk_protocol
) {
266 if (sk
->sk_state
== MISDN_BOUND
)
267 delete_channel(&_pms(sk
)->ch
);
269 mISDN_sock_unlink(&data_sockets
, sk
);
275 case ISDN_P_B_X75SLP
:
276 case ISDN_P_B_L2DTMF
:
278 case ISDN_P_B_L2DSPHDLC
:
279 delete_channel(&_pms(sk
)->ch
);
280 mISDN_sock_unlink(&data_sockets
, sk
);
287 skb_queue_purge(&sk
->sk_receive_queue
);
296 data_sock_ioctl_bound(struct sock
*sk
, unsigned int cmd
, void __user
*p
)
298 struct mISDN_ctrl_req cq
;
299 int err
= -EINVAL
, val
[2];
300 struct mISDNchannel
*bchan
, *next
;
303 if (!_pms(sk
)->dev
) {
309 if (copy_from_user(&cq
, p
, sizeof(cq
))) {
313 if ((sk
->sk_protocol
& ~ISDN_P_B_MASK
) == ISDN_P_B_START
) {
314 list_for_each_entry_safe(bchan
, next
,
315 &_pms(sk
)->dev
->bchannels
, list
) {
316 if (bchan
->nr
== cq
.channel
) {
317 err
= bchan
->ctrl(bchan
,
318 CONTROL_CHANNEL
, &cq
);
323 err
= _pms(sk
)->dev
->D
.ctrl(&_pms(sk
)->dev
->D
,
324 CONTROL_CHANNEL
, &cq
);
327 if (copy_to_user(p
, &cq
, sizeof(cq
)))
331 if (sk
->sk_protocol
!= ISDN_P_LAPD_NT
) {
336 if (get_user(val
[1], (int __user
*)p
)) {
340 err
= _pms(sk
)->dev
->teimgr
->ctrl(_pms(sk
)->dev
->teimgr
,
341 CONTROL_CHANNEL
, val
);
344 if (sk
->sk_protocol
!= ISDN_P_LAPD_NT
345 && sk
->sk_protocol
!= ISDN_P_LAPD_TE
) {
350 if (get_user(val
[1], (int __user
*)p
)) {
354 err
= _pms(sk
)->dev
->teimgr
->ctrl(_pms(sk
)->dev
->teimgr
,
355 CONTROL_CHANNEL
, val
);
367 data_sock_ioctl(struct socket
*sock
, unsigned int cmd
, unsigned long arg
)
370 struct sock
*sk
= sock
->sk
;
371 struct mISDNdevice
*dev
;
372 struct mISDNversion ver
;
376 ver
.major
= MISDN_MAJOR_VERSION
;
377 ver
.minor
= MISDN_MINOR_VERSION
;
378 ver
.release
= MISDN_RELEASE
;
379 if (copy_to_user((void __user
*)arg
, &ver
, sizeof(ver
)))
383 id
= get_mdevice_count();
384 if (put_user(id
, (int __user
*)arg
))
388 if (get_user(id
, (int __user
*)arg
)) {
392 dev
= get_mdevice(id
);
394 struct mISDN_devinfo di
;
396 memset(&di
, 0, sizeof(di
));
398 di
.Dprotocols
= dev
->Dprotocols
;
399 di
.Bprotocols
= dev
->Bprotocols
| get_all_Bprotocols();
400 di
.protocol
= dev
->D
.protocol
;
401 memcpy(di
.channelmap
, dev
->channelmap
,
402 sizeof(di
.channelmap
));
403 di
.nrbchan
= dev
->nrbchan
;
404 strcpy(di
.name
, dev_name(&dev
->dev
));
405 if (copy_to_user((void __user
*)arg
, &di
, sizeof(di
)))
411 if (sk
->sk_state
== MISDN_BOUND
)
412 err
= data_sock_ioctl_bound(sk
, cmd
,
420 static int data_sock_setsockopt(struct socket
*sock
, int level
, int optname
,
421 char __user
*optval
, unsigned int len
)
423 struct sock
*sk
= sock
->sk
;
424 int err
= 0, opt
= 0;
426 if (*debug
& DEBUG_SOCKET
)
427 printk(KERN_DEBUG
"%s(%p, %d, %x, %p, %d)\n", __func__
, sock
,
428 level
, optname
, optval
, len
);
433 case MISDN_TIME_STAMP
:
434 if (get_user(opt
, (int __user
*)optval
)) {
440 _pms(sk
)->cmask
|= MISDN_TIME_STAMP
;
442 _pms(sk
)->cmask
&= ~MISDN_TIME_STAMP
;
452 static int data_sock_getsockopt(struct socket
*sock
, int level
, int optname
,
453 char __user
*optval
, int __user
*optlen
)
455 struct sock
*sk
= sock
->sk
;
458 if (get_user(len
, optlen
))
461 if (len
!= sizeof(char))
465 case MISDN_TIME_STAMP
:
466 if (_pms(sk
)->cmask
& MISDN_TIME_STAMP
)
471 if (put_user(opt
, optval
))
482 data_sock_bind(struct socket
*sock
, struct sockaddr
*addr
, int addr_len
)
484 struct sockaddr_mISDN
*maddr
= (struct sockaddr_mISDN
*) addr
;
485 struct sock
*sk
= sock
->sk
;
489 if (*debug
& DEBUG_SOCKET
)
490 printk(KERN_DEBUG
"%s(%p) sk=%p\n", __func__
, sock
, sk
);
491 if (addr_len
!= sizeof(struct sockaddr_mISDN
))
493 if (!maddr
|| maddr
->family
!= AF_ISDN
)
502 _pms(sk
)->dev
= get_mdevice(maddr
->dev
);
503 if (!_pms(sk
)->dev
) {
508 if (sk
->sk_protocol
< ISDN_P_B_START
) {
509 read_lock_bh(&data_sockets
.lock
);
510 sk_for_each(csk
, &data_sockets
.head
) {
513 if (_pms(csk
)->dev
!= _pms(sk
)->dev
)
515 if (csk
->sk_protocol
>= ISDN_P_B_START
)
517 if (IS_ISDN_P_TE(csk
->sk_protocol
)
518 == IS_ISDN_P_TE(sk
->sk_protocol
))
520 read_unlock_bh(&data_sockets
.lock
);
524 read_unlock_bh(&data_sockets
.lock
);
527 _pms(sk
)->ch
.send
= mISDN_send
;
528 _pms(sk
)->ch
.ctrl
= mISDN_ctrl
;
530 switch (sk
->sk_protocol
) {
535 mISDN_sock_unlink(&data_sockets
, sk
);
536 err
= connect_layer1(_pms(sk
)->dev
, &_pms(sk
)->ch
,
537 sk
->sk_protocol
, maddr
);
539 mISDN_sock_link(&data_sockets
, sk
);
543 err
= create_l2entity(_pms(sk
)->dev
, &_pms(sk
)->ch
,
544 sk
->sk_protocol
, maddr
);
548 case ISDN_P_B_X75SLP
:
549 case ISDN_P_B_L2DTMF
:
551 case ISDN_P_B_L2DSPHDLC
:
552 err
= connect_Bstack(_pms(sk
)->dev
, &_pms(sk
)->ch
,
553 sk
->sk_protocol
, maddr
);
556 err
= -EPROTONOSUPPORT
;
560 sk
->sk_state
= MISDN_BOUND
;
561 _pms(sk
)->ch
.protocol
= sk
->sk_protocol
;
569 data_sock_getname(struct socket
*sock
, struct sockaddr
*addr
,
570 int *addr_len
, int peer
)
572 struct sockaddr_mISDN
*maddr
= (struct sockaddr_mISDN
*) addr
;
573 struct sock
*sk
= sock
->sk
;
580 *addr_len
= sizeof(*maddr
);
581 maddr
->dev
= _pms(sk
)->dev
->id
;
582 maddr
->channel
= _pms(sk
)->ch
.nr
;
583 maddr
->sapi
= _pms(sk
)->ch
.addr
& 0xff;
584 maddr
->tei
= (_pms(sk
)->ch
.addr
>> 8) & 0xff;
589 static const struct proto_ops data_sock_ops
= {
591 .owner
= THIS_MODULE
,
592 .release
= data_sock_release
,
593 .ioctl
= data_sock_ioctl
,
594 .bind
= data_sock_bind
,
595 .getname
= data_sock_getname
,
596 .sendmsg
= mISDN_sock_sendmsg
,
597 .recvmsg
= mISDN_sock_recvmsg
,
598 .poll
= datagram_poll
,
599 .listen
= sock_no_listen
,
600 .shutdown
= sock_no_shutdown
,
601 .setsockopt
= data_sock_setsockopt
,
602 .getsockopt
= data_sock_getsockopt
,
603 .connect
= sock_no_connect
,
604 .socketpair
= sock_no_socketpair
,
605 .accept
= sock_no_accept
,
610 data_sock_create(struct net
*net
, struct socket
*sock
, int protocol
)
614 if (sock
->type
!= SOCK_DGRAM
)
615 return -ESOCKTNOSUPPORT
;
617 sk
= sk_alloc(net
, PF_ISDN
, GFP_KERNEL
, &mISDN_proto
);
621 sock_init_data(sock
, sk
);
623 sock
->ops
= &data_sock_ops
;
624 sock
->state
= SS_UNCONNECTED
;
625 sock_reset_flag(sk
, SOCK_ZAPPED
);
627 sk
->sk_protocol
= protocol
;
628 sk
->sk_state
= MISDN_OPEN
;
629 mISDN_sock_link(&data_sockets
, sk
);
635 base_sock_release(struct socket
*sock
)
637 struct sock
*sk
= sock
->sk
;
639 printk(KERN_DEBUG
"%s(%p) sk=%p\n", __func__
, sock
, sk
);
643 mISDN_sock_unlink(&base_sockets
, sk
);
651 base_sock_ioctl(struct socket
*sock
, unsigned int cmd
, unsigned long arg
)
654 struct mISDNdevice
*dev
;
655 struct mISDNversion ver
;
659 ver
.major
= MISDN_MAJOR_VERSION
;
660 ver
.minor
= MISDN_MINOR_VERSION
;
661 ver
.release
= MISDN_RELEASE
;
662 if (copy_to_user((void __user
*)arg
, &ver
, sizeof(ver
)))
666 id
= get_mdevice_count();
667 if (put_user(id
, (int __user
*)arg
))
671 if (get_user(id
, (int __user
*)arg
)) {
675 dev
= get_mdevice(id
);
677 struct mISDN_devinfo di
;
679 memset(&di
, 0, sizeof(di
));
681 di
.Dprotocols
= dev
->Dprotocols
;
682 di
.Bprotocols
= dev
->Bprotocols
| get_all_Bprotocols();
683 di
.protocol
= dev
->D
.protocol
;
684 memcpy(di
.channelmap
, dev
->channelmap
,
685 sizeof(di
.channelmap
));
686 di
.nrbchan
= dev
->nrbchan
;
687 strcpy(di
.name
, dev_name(&dev
->dev
));
688 if (copy_to_user((void __user
*)arg
, &di
, sizeof(di
)))
695 struct mISDN_devrename dn
;
696 if (copy_from_user(&dn
, (void __user
*)arg
,
701 dev
= get_mdevice(dn
.id
);
703 err
= device_rename(&dev
->dev
, dn
.name
);
715 base_sock_bind(struct socket
*sock
, struct sockaddr
*addr
, int addr_len
)
717 struct sockaddr_mISDN
*maddr
= (struct sockaddr_mISDN
*) addr
;
718 struct sock
*sk
= sock
->sk
;
721 if (!maddr
|| maddr
->family
!= AF_ISDN
)
731 _pms(sk
)->dev
= get_mdevice(maddr
->dev
);
732 if (!_pms(sk
)->dev
) {
736 sk
->sk_state
= MISDN_BOUND
;
743 static const struct proto_ops base_sock_ops
= {
745 .owner
= THIS_MODULE
,
746 .release
= base_sock_release
,
747 .ioctl
= base_sock_ioctl
,
748 .bind
= base_sock_bind
,
749 .getname
= sock_no_getname
,
750 .sendmsg
= sock_no_sendmsg
,
751 .recvmsg
= sock_no_recvmsg
,
752 .poll
= sock_no_poll
,
753 .listen
= sock_no_listen
,
754 .shutdown
= sock_no_shutdown
,
755 .setsockopt
= sock_no_setsockopt
,
756 .getsockopt
= sock_no_getsockopt
,
757 .connect
= sock_no_connect
,
758 .socketpair
= sock_no_socketpair
,
759 .accept
= sock_no_accept
,
765 base_sock_create(struct net
*net
, struct socket
*sock
, int protocol
)
769 if (sock
->type
!= SOCK_RAW
)
770 return -ESOCKTNOSUPPORT
;
772 sk
= sk_alloc(net
, PF_ISDN
, GFP_KERNEL
, &mISDN_proto
);
776 sock_init_data(sock
, sk
);
777 sock
->ops
= &base_sock_ops
;
778 sock
->state
= SS_UNCONNECTED
;
779 sock_reset_flag(sk
, SOCK_ZAPPED
);
780 sk
->sk_protocol
= protocol
;
781 sk
->sk_state
= MISDN_OPEN
;
782 mISDN_sock_link(&base_sockets
, sk
);
788 mISDN_sock_create(struct net
*net
, struct socket
*sock
, int proto
, int kern
)
790 int err
= -EPROTONOSUPPORT
;
794 err
= base_sock_create(net
, sock
, proto
);
804 case ISDN_P_B_X75SLP
:
805 case ISDN_P_B_L2DTMF
:
807 case ISDN_P_B_L2DSPHDLC
:
808 err
= data_sock_create(net
, sock
, proto
);
817 static const struct net_proto_family mISDN_sock_family_ops
= {
818 .owner
= THIS_MODULE
,
820 .create
= mISDN_sock_create
,
824 misdn_sock_init(u_int
*deb
)
829 err
= sock_register(&mISDN_sock_family_ops
);
831 printk(KERN_ERR
"%s: error(%d)\n", __func__
, err
);
836 misdn_sock_cleanup(void)
838 sock_unregister(PF_ISDN
);