osmo-cc-misdn-endpoint/src/libmisdn/socket.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 *
 * Author	Karsten Keil <kkeil@novell.com>
 *
 * Copyright 2008  by Karsten Keil <kkeil@novell.com>
 */

#include "mISDNif.h"
#include "core.h"
#include <stdio.h>
#include <stdarg.h>
#include <sys/socket.h>
#include "socket.h"

static u_int	*debug;

static struct mISDN_sock_list	data_sockets = {
	.lock = 0 //__RW_LOCK_UNLOCKED(data_sockets.lock)
};

static struct mISDN_sock_list	base_sockets = {
	.lock = 0 //__RW_LOCK_UNLOCKED(base_sockets.lock)
};

#define L2_HEADER_LEN	4

static inline struct sk_buff *
_l2_alloc_skb(unsigned int len, gfp_t gfp_mask)
{
	struct sk_buff  *skb;

	skb = alloc_skb(len + L2_HEADER_LEN, gfp_mask);
	if (likely(skb))
		skb_reserve(skb, L2_HEADER_LEN);
	return skb;
}

static void
mISDN_sock_link(struct mISDN_sock_list *l, struct mISDN_sock *msk)
{
	write_lock_bh(&l->lock);
	hlist_add_head(&msk->list, &l->head);
	write_unlock_bh(&l->lock);
}

static void mISDN_sock_unlink(struct mISDN_sock_list __attribute__((unused)) *l, struct mISDN_sock *msk)
{
	write_lock_bh(&l->lock);
	hlist_del(&msk->list);
	write_unlock_bh(&l->lock);
}

static int
mISDN_send(struct mISDNchannel *ch, struct sk_buff *skb)
{
	struct mISDN_sock *msk;

	msk = container_of(ch, struct mISDN_sock, ch);
	if (*debug & DEBUG_SOCKET)
		printk(KERN_DEBUG "%s len %d %p\n", __func__, skb->len, skb);
	if (msk->sk_state == MISDN_CLOSED)
		return -EUNATCH;
	__net_timestamp(skb);
	skb_queue_tail(&msk->sk_receive_queue, skb);
	return 0;
}

static int
mISDN_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
{
	struct mISDN_sock *msk;

	msk = container_of(ch, struct mISDN_sock, ch);
	if (*debug & DEBUG_SOCKET)
		printk(KERN_DEBUG "%s(%p, %x, %p)\n", __func__, ch, cmd, arg);
	switch (cmd) {
	case CLOSE_CHANNEL:
		msk->sk_state = MISDN_CLOSED;
		break;
	}
	return 0;
}

int mISDN_data_recvmsg(void *inst, unsigned char *data, size_t len, int flags, struct sockaddr_mISDN *maddr)
{
	struct mISDN_sock	*msk = (struct mISDN_sock *)inst;
	struct sk_buff		*skb;

	size_t			copied;

	if (flags & (MSG_OOB))
		return -EOPNOTSUPP;

	if (msk->sk_state == MISDN_CLOSED)
		return 0;

	skb = skb_dequeue(&msk->sk_receive_queue);
	if (!skb)
		return -EAGAIN;

	if (*debug & DEBUG_SOCKET)
		printk(KERN_DEBUG "%s: len %d, flags %x ch.nr %d, proto %x\n",
		       __func__, (int)len, flags, msk->ch.nr,
		       msk->sk_protocol);

	if (maddr) {
		maddr->family = AF_ISDN;
		maddr->dev = msk->dev->id;
		if ((msk->sk_protocol == ISDN_P_LAPD_TE) ||
		    (msk->sk_protocol == ISDN_P_LAPD_NT)) {
			maddr->channel = (mISDN_HEAD_ID(skb) >> 16) & 0xff;
			maddr->tei =  (mISDN_HEAD_ID(skb) >> 8) & 0xff;
			maddr->sapi = mISDN_HEAD_ID(skb) & 0xff;
		} else {
			maddr->channel = msk->ch.nr;
			maddr->sapi = msk->ch.addr & 0xFF;
			maddr->tei =  (msk->ch.addr >> 8) & 0xFF;
		}
	}

	copied = skb->len + MISDN_HEADER_LEN;
	if (len < copied) {
		if (flags & MSG_PEEK)
			/*refcount_dec(&skb->users)*/;
		else
			skb_queue_head(&msk->sk_receive_queue, skb);
		return -ENOSPC;
	}
	memcpy(skb_push(skb, MISDN_HEADER_LEN), mISDN_HEAD_P(skb),
	       MISDN_HEADER_LEN);

	memcpy(data, skb->data, skb->len);

	dev_kfree_skb(skb);

	return copied;
}

int mISDN_data_sendmsg(void *inst, unsigned char *data, size_t len, int __attribute__((unused)) flags, struct sockaddr_mISDN *maddr)
{
	struct mISDN_sock *msk = (struct mISDN_sock *)inst;
	struct sk_buff		*skb;
	int			err = -ENOMEM;

	if (*debug & DEBUG_SOCKET)
		printk(KERN_DEBUG "%s: len %d ch %d proto %x\n",
		       __func__, (int)len, msk->ch.nr,
		       msk->sk_protocol);

	if (len < MISDN_HEADER_LEN)
		return -EINVAL;

	if (msk->sk_state != MISDN_BOUND)
		return -EBADFD;

	skb = _l2_alloc_skb(len, GFP_KERNEL);
	if (!skb)
		goto done;

	memcpy(skb_put(skb, len), data, len);

	memcpy(mISDN_HEAD_P(skb), skb->data, MISDN_HEADER_LEN);
	skb_pull(skb, MISDN_HEADER_LEN);

	if (maddr) {
		/* if we have a address, we use it */
		mISDN_HEAD_ID(skb) = maddr->channel;
	} else { /* use default for L2 messages */
		if ((msk->sk_protocol == ISDN_P_LAPD_TE) ||
		    (msk->sk_protocol == ISDN_P_LAPD_NT))
			mISDN_HEAD_ID(skb) = msk->ch.nr;
	}

	if (*debug & DEBUG_SOCKET)
		printk(KERN_DEBUG "%s: ID:%x\n",
		       __func__, mISDN_HEAD_ID(skb));

	err = -ENODEV;
	if (!msk->ch.peer)
		goto done;
	err = msk->ch.recv(msk->ch.peer, skb);
	if (err)
		goto done;
	else {
		skb = NULL;
		err = len;
	}

done:
	kfree_skb(skb);
	return err;
}

int mISDN_data_release(void *inst)
{
	struct mISDN_sock *msk = (struct mISDN_sock *)inst;

	if (*debug & DEBUG_SOCKET)
		printk(KERN_DEBUG "%s msk=%p\n", __func__, msk);
	if (!msk)
		return 0;
	switch (msk->sk_protocol) {
	case ISDN_P_TE_S0:
	case ISDN_P_NT_S0:
	case ISDN_P_TE_E1:
	case ISDN_P_NT_E1:
		if (msk->sk_state == MISDN_BOUND)
			delete_channel(&msk->ch);
		else
			mISDN_sock_unlink(&data_sockets, msk);
		break;
	case ISDN_P_LAPD_TE:
	case ISDN_P_LAPD_NT:
	case ISDN_P_B_RAW:
	case ISDN_P_B_HDLC:
	case ISDN_P_B_X75SLP:
	case ISDN_P_B_L2DTMF:
	case ISDN_P_B_L2DSP:
	case ISDN_P_B_L2DSPHDLC:
		delete_channel(&msk->ch);
		mISDN_sock_unlink(&data_sockets, msk);
		break;
	}

	skb_queue_purge(&msk->sk_receive_queue);

	free(msk);

	return 0;
}

static int
data_sock_ioctl_bound(struct mISDN_sock *msk, unsigned int cmd, void *p)
{
	struct mISDN_ctrl_req	cq;
	int			err = -EINVAL, val[2];
	struct mISDNchannel	*bchan, *next;

	if (!msk->dev) {
		err = -ENODEV;
		goto done;
	}
	switch (cmd) {
	case IMCTRLREQ:
		if (memcpy(&cq, p, sizeof(cq))) {
			err = -EFAULT;
			break;
		}
		if ((msk->sk_protocol & ~ISDN_P_B_MASK) == ISDN_P_B_START) {
			list_for_each_entry_safe(bchan, next,
						 &msk->dev->bchannels, list) {
				if ((int)bchan->nr == cq.channel) {
					err = bchan->ctrl(bchan,
							  CONTROL_CHANNEL, &cq);
					break;
				}
			}
		} else
			err = msk->dev->D.ctrl(&msk->dev->D,
						    CONTROL_CHANNEL, &cq);
		if (err)
			break;
		if (memcpy(p, &cq, sizeof(cq)))
			err = -EFAULT;
		break;
	case IMCLEAR_L2:
		if (msk->sk_protocol != ISDN_P_LAPD_NT) {
			err = -EINVAL;
			break;
		}
		val[0] = cmd;
		val[1] = *((int *)p);
		err = msk->dev->teimgr->ctrl(msk->dev->teimgr,
						  CONTROL_CHANNEL, val);
		break;
	case IMHOLD_L1:
		if (msk->sk_protocol != ISDN_P_LAPD_NT
		    && msk->sk_protocol != ISDN_P_LAPD_TE) {
			err = -EINVAL;
			break;
		}
		val[0] = cmd;
		val[1] = *((int *)p);
		err = msk->dev->teimgr->ctrl(msk->dev->teimgr,
						  CONTROL_CHANNEL, val);
		break;
	default:
		err = -EINVAL;
		break;
	}
done:
	return err;
}

int mISDN_data_ioctl(void *inst, unsigned int cmd, void *arg)
{
	int			err = 0, id;
	struct mISDN_sock	*msk = (struct mISDN_sock *)inst;
	struct mISDNdevice	*dev;
	struct mISDNversion	ver;

	switch (cmd) {
	case IMGETVERSION:
		ver.major = MISDN_MAJOR_VERSION;
		ver.minor = MISDN_MINOR_VERSION;
		ver.release = MISDN_RELEASE;
		memcpy((void *)arg, &ver, sizeof(ver));
		break;
	case IMGETCOUNT:
		id = get_mdevice_count();
		*((int *)arg) = id;
		break;
	case IMGETDEVINFO:
		id = *((int *)arg);
		dev = get_mdevice(id);
		if (dev) {
			struct mISDN_devinfo di;

			memset(&di, 0, sizeof(di));
			di.id = dev->id;
			di.Dprotocols = dev->Dprotocols;
			di.Bprotocols = dev->Bprotocols | get_all_Bprotocols();
			di.protocol = dev->D.protocol;
			memcpy(di.channelmap, dev->channelmap,
			       sizeof(di.channelmap));
			di.nrbchan = dev->nrbchan;
			strncpy(di.name, dev_name(dev), sizeof(di.name));
			di.name[sizeof(di.name) - 1] = '\0';
			memcpy((void *)arg, &di, sizeof(di));
		} else
			err = -ENODEV;
		break;
	default:
		if (msk->sk_state == MISDN_BOUND)
			err = data_sock_ioctl_bound(msk, cmd, arg);
		else
			err = -ENOTCONN;
	}
	return err;
}

#if 0
int mISDN_data_setopt(void *inst, int level, int optname,
		sockptr_t optval, unsigned int len)
{
	struct mISDN_sock *msk = (struct mISDN_sock *)inst;
	int err = 0, opt = 0;

	if (*debug & DEBUG_SOCKET)
		printk(KERN_DEBUG "%s(%p, %d, %x, optval, %d)\n", __func__, sock,
		       level, optname, len);

	switch (optname) {
	case MISDN_TIME_STAMP:
		if (copy_from_sockptr(&opt, optval, sizeof(int))) {
			err = -EFAULT;
			break;
		}

		if (opt)
			msk->cmask |= MISDN_TIME_STAMP;
		else
			msk->cmask &= ~MISDN_TIME_STAMP;
		break;
	default:
		err = -ENOPROTOOPT;
		break;
	}
	return err;
}

int mISDN_data_getopt(void *inst, int level, int optname,
		char *optval, int *optlen)
{
	struct mISDN_sock *msk = (struct mISDN_sock *)inst;
	int len, opt;

	len = *optlen;

	if (len != sizeof(char))
		return -EINVAL;

	switch (optname) {
	case MISDN_TIME_STAMP:
		if (msk->cmask & MISDN_TIME_STAMP)
			opt = 1;
		else
			opt = 0;

		*((int *)optval) = opt;
			return -EFAULT;
		break;
	default:
		return -ENOPROTOOPT;
	}

	return 0;
}
#endif

int mISDN_data_bind(void *inst, struct sockaddr *addr, int addr_len)
{
	struct sockaddr_mISDN *maddr = (struct sockaddr_mISDN *) addr;
	struct mISDN_sock *msk = (struct mISDN_sock *)inst;
	struct mISDN_sock *cmsk;
	int err = 0;

	if (*debug & DEBUG_SOCKET)
		printk(KERN_DEBUG "%s msk=%p\n", __func__, msk);
	if (addr_len != sizeof(struct sockaddr_mISDN))
		return -EINVAL;
	if (!maddr || maddr->family != AF_ISDN)
		return -EINVAL;

	if (msk->dev) {
		err = -EALREADY;
		goto done;
	}
	msk->dev = get_mdevice(maddr->dev);
	if (!msk->dev) {
		err = -ENODEV;
		goto done;
	}

	if (msk->sk_protocol < ISDN_P_B_START) {
		read_lock_bh(&data_sockets.lock);
		hlist_for_each_entry(cmsk, &data_sockets.head, list) {
			if (msk == cmsk)
				continue;
			if (cmsk->dev != msk->dev)
				continue;
			if (cmsk->sk_protocol >= ISDN_P_B_START)
				continue;
			if (IS_ISDN_P_TE(cmsk->sk_protocol)
			    == IS_ISDN_P_TE(msk->sk_protocol))
				continue;
			read_unlock_bh(&data_sockets.lock);
			err = -EBUSY;
			goto done;
		}
		read_unlock_bh(&data_sockets.lock);
	}

	msk->ch.send = mISDN_send;
	msk->ch.ctrl = mISDN_ctrl;

	switch (msk->sk_protocol) {
	case ISDN_P_TE_S0:
	case ISDN_P_NT_S0:
	case ISDN_P_TE_E1:
	case ISDN_P_NT_E1:
		mISDN_sock_unlink(&data_sockets, msk);
		err = connect_layer1(msk->dev, &msk->ch,
				     msk->sk_protocol, maddr);
		if (err)
			mISDN_sock_link(&data_sockets, msk);
		break;
	case ISDN_P_LAPD_TE:
	case ISDN_P_LAPD_NT:
		err = create_l2entity(msk->dev, &msk->ch,
				      msk->sk_protocol, maddr);
		break;
	case ISDN_P_B_RAW:
	case ISDN_P_B_HDLC:
	case ISDN_P_B_X75SLP:
	case ISDN_P_B_L2DTMF:
	case ISDN_P_B_L2DSP:
	case ISDN_P_B_L2DSPHDLC:
		err = connect_Bstack(msk->dev, &msk->ch,
				     msk->sk_protocol, maddr);
		break;
	default:
		err = -EPROTONOSUPPORT;
	}
	if (err)
		goto done;
	msk->sk_state = MISDN_BOUND;
	msk->ch.protocol = msk->sk_protocol;

done:
	return err;
}

int mISDN_data_getname(void *inst, struct sockaddr *addr, int __attribute__((unused)) peer)
{
	struct sockaddr_mISDN	*maddr = (struct sockaddr_mISDN *) addr;
	struct mISDN_sock *msk = (struct mISDN_sock *)inst;

	if (!msk->dev)
		return -EBADFD;

	maddr->family = AF_ISDN;
	maddr->dev = msk->dev->id;
	maddr->channel = msk->ch.nr;
	maddr->sapi = msk->ch.addr & 0xff;
	maddr->tei = (msk->ch.addr >> 8) & 0xff;
	return sizeof(*maddr);
}

int mISDN_data_create(void **inst, int protocol)
{
	struct mISDN_sock *msk;

	msk = calloc(1, sizeof(*msk));
	if (!msk)
		return -ENOMEM;
	printk(KERN_DEBUG "%s msk=%p\n", __func__, msk);

	skb_queue_head_init(&msk->sk_receive_queue);

	msk->sk_protocol = protocol;
	msk->sk_state    = MISDN_OPEN;
	mISDN_sock_link(&data_sockets, msk);

	*inst = msk;
	return 0;
}

int mISDN_base_release(void *inst)
{
	struct mISDN_sock *msk = (struct mISDN_sock *)inst;

	printk(KERN_DEBUG "%s msk=%p\n", __func__, msk);
	if (!msk)
		return 0;

	mISDN_sock_unlink(&base_sockets, msk);
	free(msk);

	return 0;
}

int mISDN_base_ioctl(void *inst __attribute__((unused)), unsigned int cmd, void *arg)
{
	int			err = 0, id;
	struct mISDNdevice	*dev;
	struct mISDNversion	ver;

	switch (cmd) {
	case IMGETVERSION:
		ver.major = MISDN_MAJOR_VERSION;
		ver.minor = MISDN_MINOR_VERSION;
		ver.release = MISDN_RELEASE;
		memcpy((void *)arg, &ver, sizeof(ver));
		break;
	case IMGETCOUNT:
		id = get_mdevice_count();
		*((int *)arg) = id;
		break;
	case IMGETDEVINFO:
		id = *((int *)arg);
		dev = get_mdevice(id);
		if (dev) {
			struct mISDN_devinfo di;

			memset(&di, 0, sizeof(di));
			di.id = dev->id;
			di.Dprotocols = dev->Dprotocols;
			di.Bprotocols = dev->Bprotocols | get_all_Bprotocols();
			di.protocol = dev->D.protocol;
			memcpy(di.channelmap, dev->channelmap,
			       sizeof(di.channelmap));
			di.nrbchan = dev->nrbchan;
			strncpy(di.name, dev_name(dev), sizeof(di.name));
			di.name[sizeof(di.name) - 1] = '\0';
			memcpy((void *)arg, &di, sizeof(di));
		} else
			err = -ENODEV;
		break;
	case IMSETDEVNAME:
	{
		struct mISDN_devrename dn;
		memcpy(&dn, (void *)arg, sizeof(dn));
		dn.name[sizeof(dn.name) - 1] = '\0';
		dev = get_mdevice(dn.id);
		if (dev)
			dev_set_name(dev, "%s", dn.name);
		else
			err = -ENODEV;
		err = -EINVAL;
	}
	break;
	default:
		err = -EINVAL;
	}
	return err;
}

int mISDN_base_bind(void *inst, struct sockaddr *addr, int addr_len)
{
	struct sockaddr_mISDN *maddr = (struct sockaddr_mISDN *) addr;
	struct mISDN_sock *msk = (struct mISDN_sock *)inst;
	int err = 0;

	if (addr_len < (int)sizeof(struct sockaddr_mISDN))
		return -EINVAL;

	if (!maddr || maddr->family != AF_ISDN)
		return -EINVAL;

	if (msk->dev) {
		err = -EALREADY;
		goto done;
	}

	msk->dev = get_mdevice(maddr->dev);
	if (!msk->dev) {
		err = -ENODEV;
		goto done;
	}
	msk->sk_state = MISDN_BOUND;

done:
	return err;
}

int mISDN_base_create(void **inst, int protocol)
{
	struct mISDN_sock *msk;

	msk = calloc(1, sizeof(*msk));
	if (!msk)
		return -ENOMEM;
	printk(KERN_DEBUG "%s msk=%p\n", __func__, msk);

	skb_queue_head_init(&msk->sk_receive_queue);

	msk->sk_protocol = protocol;
	msk->sk_state    = MISDN_OPEN;
	mISDN_sock_link(&base_sockets, msk);

	*inst = msk;
	return 0;
}

int misdn_sock_init(u_int *deb)
{
        debug = deb;
        return 0;
}

void misdn_sock_cleanup(void)
{
}