af_rose.c

Linux Kernel 2.6.9 for OMAP1710

	SOCK_DEBUG(sk, "ROSE: sendto: Addresses built.\n");

	/* Build a packet */
	SOCK_DEBUG(sk, "ROSE: sendto: building packet.\n");
	size = len + AX25_BPQ_HEADER_LEN + AX25_MAX_HEADER_LEN + ROSE_MIN_LEN;

	if ((skb = sock_alloc_send_skb(sk, size, msg->msg_flags & MSG_DONTWAIT, &err)) == NULL)
		return err;

	skb_reserve(skb, AX25_BPQ_HEADER_LEN + AX25_MAX_HEADER_LEN + ROSE_MIN_LEN);

	/*
	 *	Put the data on the end
	 */
	SOCK_DEBUG(sk, "ROSE: Appending user data\n");

	asmptr = skb->h.raw = skb_put(skb, len);

	err = memcpy_fromiovec(asmptr, msg->msg_iov, len);
	if (err) {
		kfree_skb(skb);
		return err;
	}

	/*
	 *	If the Q BIT Include socket option is in force, the first
	 *	byte of the user data is the logical value of the Q Bit.
	 */
	if (rose->qbitincl) {
		qbit = skb->data[0];
		skb_pull(skb, 1);
	}

	/*
	 *	Push down the ROSE header
	 */
	asmptr = skb_push(skb, ROSE_MIN_LEN);

	SOCK_DEBUG(sk, "ROSE: Building Network Header.\n");

	/* Build a ROSE Network header */
	asmptr[0] = ((rose->lci >> 8) & 0x0F) | ROSE_GFI;
	asmptr[1] = (rose->lci >> 0) & 0xFF;
	asmptr[2] = ROSE_DATA;

	if (qbit)
		asmptr[0] |= ROSE_Q_BIT;

	SOCK_DEBUG(sk, "ROSE: Built header.\n");

	SOCK_DEBUG(sk, "ROSE: Transmitting buffer\n");

	if (sk->sk_state != TCP_ESTABLISHED) {
		kfree_skb(skb);
		return -ENOTCONN;
	}

#ifdef M_BIT
#define ROSE_PACLEN (256-ROSE_MIN_LEN)
	if (skb->len - ROSE_MIN_LEN > ROSE_PACLEN) {
		unsigned char header[ROSE_MIN_LEN];
		struct sk_buff *skbn;
		int frontlen;
		int lg;

		/* Save a copy of the Header */
		memcpy(header, skb->data, ROSE_MIN_LEN);
		skb_pull(skb, ROSE_MIN_LEN);

		frontlen = skb_headroom(skb);

		while (skb->len > 0) {
			if ((skbn = sock_alloc_send_skb(sk, frontlen + ROSE_PACLEN, 0, &err)) == NULL) {
				kfree_skb(skb);
				return err;
			}

			skbn->sk   = sk;
			skbn->free = 1;
			skbn->arp  = 1;

			skb_reserve(skbn, frontlen);

			lg = (ROSE_PACLEN > skb->len) ? skb->len : ROSE_PACLEN;

			/* Copy the user data */
			memcpy(skb_put(skbn, lg), skb->data, lg);
			skb_pull(skb, lg);

			/* Duplicate the Header */
			skb_push(skbn, ROSE_MIN_LEN);
			memcpy(skbn->data, header, ROSE_MIN_LEN);

			if (skb->len > 0)
				skbn->data[2] |= M_BIT;

			skb_queue_tail(&sk->sk_write_queue, skbn); /* Throw it on the queue */
		}

		skb->free = 1;
		kfree_skb(skb);
	} else {
		skb_queue_tail(&sk->sk_write_queue, skb);		/* Throw it on the queue */
	}
#else
	skb_queue_tail(&sk->sk_write_queue, skb);	/* Shove it onto the queue */
#endif

	rose_kick(sk);

	return len;
}


static int rose_recvmsg(struct kiocb *iocb, struct socket *sock,
			struct msghdr *msg, size_t size, int flags)
{
	struct sock *sk = sock->sk;
	rose_cb *rose = rose_sk(sk);
	struct sockaddr_rose *srose = (struct sockaddr_rose *)msg->msg_name;
	size_t copied;
	unsigned char *asmptr;
	struct sk_buff *skb;
	int n, er, qbit;

	/*
	 * This works for seqpacket too. The receiver has ordered the queue for
	 * us! We do one quick check first though
	 */
	if (sk->sk_state != TCP_ESTABLISHED)
		return -ENOTCONN;

	/* Now we can treat all alike */
	if ((skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT, flags & MSG_DONTWAIT, &er)) == NULL)
		return er;

	qbit = (skb->data[0] & ROSE_Q_BIT) == ROSE_Q_BIT;

	skb_pull(skb, ROSE_MIN_LEN);

	if (rose->qbitincl) {
		asmptr  = skb_push(skb, 1);
		*asmptr = qbit;
	}

	skb->h.raw = skb->data;
	copied     = skb->len;

	if (copied > size) {
		copied = size;
		msg->msg_flags |= MSG_TRUNC;
	}

	skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);

	if (srose != NULL) {
		srose->srose_family = AF_ROSE;
		srose->srose_addr   = rose->dest_addr;
		srose->srose_call   = rose->dest_call;
		srose->srose_ndigis = rose->dest_ndigis;
		if (msg->msg_namelen >= sizeof(struct full_sockaddr_rose)) {
			struct full_sockaddr_rose *full_srose = (struct full_sockaddr_rose *)msg->msg_name;
			for (n = 0 ; n < rose->dest_ndigis ; n++)
				full_srose->srose_digis[n] = rose->dest_digis[n];
			msg->msg_namelen = sizeof(struct full_sockaddr_rose);
		} else {
			if (rose->dest_ndigis >= 1) {
				srose->srose_ndigis = 1;
				srose->srose_digi = rose->dest_digis[0];
			}
			msg->msg_namelen = sizeof(struct sockaddr_rose);
		}
	}

	skb_free_datagram(sk, skb);

	return copied;
}


static int rose_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
{
	struct sock *sk = sock->sk;
	rose_cb *rose = rose_sk(sk);
	void __user *argp = (void __user *)arg;

	switch (cmd) {
	case TIOCOUTQ: {
		long amount;
		amount = sk->sk_sndbuf - atomic_read(&sk->sk_wmem_alloc);
		if (amount < 0)
			amount = 0;
		return put_user(amount, (unsigned int __user *)argp);
	}

	case TIOCINQ: {
		struct sk_buff *skb;
		long amount = 0L;
		/* These two are safe on a single CPU system as only user tasks fiddle here */
		if ((skb = skb_peek(&sk->sk_receive_queue)) != NULL)
			amount = skb->len;
		return put_user(amount, (unsigned int __user *)argp);
	}

	case SIOCGSTAMP:
		if (sk != NULL) 
			return sock_get_timestamp(sk, (struct timeval __user *)argp);
		return -EINVAL;

	case SIOCGIFADDR:
	case SIOCSIFADDR:
	case SIOCGIFDSTADDR:
	case SIOCSIFDSTADDR:
	case SIOCGIFBRDADDR:
	case SIOCSIFBRDADDR:
	case SIOCGIFNETMASK:
	case SIOCSIFNETMASK:
	case SIOCGIFMETRIC:
	case SIOCSIFMETRIC:
		return -EINVAL;

	case SIOCADDRT:
	case SIOCDELRT:
	case SIOCRSCLRRT:
		if (!capable(CAP_NET_ADMIN))
			return -EPERM;
		return rose_rt_ioctl(cmd, argp);

	case SIOCRSGCAUSE: {
		struct rose_cause_struct rose_cause;
		rose_cause.cause      = rose->cause;
		rose_cause.diagnostic = rose->diagnostic;
		return copy_to_user(argp, &rose_cause, sizeof(struct rose_cause_struct)) ? -EFAULT : 0;
	}

	case SIOCRSSCAUSE: {
		struct rose_cause_struct rose_cause;
		if (copy_from_user(&rose_cause, argp, sizeof(struct rose_cause_struct)))
			return -EFAULT;
		rose->cause      = rose_cause.cause;
		rose->diagnostic = rose_cause.diagnostic;
		return 0;
	}

	case SIOCRSSL2CALL:
		if (!capable(CAP_NET_ADMIN)) return -EPERM;
		if (ax25cmp(&rose_callsign, &null_ax25_address) != 0)
			ax25_listen_release(&rose_callsign, NULL);
		if (copy_from_user(&rose_callsign, argp, sizeof(ax25_address)))
			return -EFAULT;
		if (ax25cmp(&rose_callsign, &null_ax25_address) != 0)
			ax25_listen_register(&rose_callsign, NULL);
		return 0;

	case SIOCRSGL2CALL:
		return copy_to_user(argp, &rose_callsign, sizeof(ax25_address)) ? -EFAULT : 0;

	case SIOCRSACCEPT:
		if (rose->state == ROSE_STATE_5) {
			rose_write_internal(sk, ROSE_CALL_ACCEPTED);
			rose_start_idletimer(sk);
			rose->condition = 0x00;
			rose->vs        = 0;
			rose->va        = 0;
			rose->vr        = 0;
			rose->vl        = 0;
			rose->state     = ROSE_STATE_3;
		}
		return 0;

	default:
		return dev_ioctl(cmd, argp);
	}

	return 0;
}

#ifdef CONFIG_PROC_FS
static void *rose_info_start(struct seq_file *seq, loff_t *pos)
{
	int i;
	struct sock *s;
	struct hlist_node *node;

	spin_lock_bh(&rose_list_lock);
	if (*pos == 0)
		return SEQ_START_TOKEN;
	
	i = 1;
	sk_for_each(s, node, &rose_list) {
		if (i == *pos)
			return s;
		++i;
	}
	return NULL;
}

static void *rose_info_next(struct seq_file *seq, void *v, loff_t *pos)
{
	++*pos;

	return (v == SEQ_START_TOKEN) ? sk_head(&rose_list) 
		: sk_next((struct sock *)v);
}
	
static void rose_info_stop(struct seq_file *seq, void *v)
{
	spin_unlock_bh(&rose_list_lock);
}

static int rose_info_show(struct seq_file *seq, void *v)
{
	if (v == SEQ_START_TOKEN)
		seq_puts(seq, 
			 "dest_addr  dest_call src_addr   src_call  dev   lci neigh st vs vr va   t  t1  t2  t3  hb    idle Snd-Q Rcv-Q inode\n");

	else {
		struct sock *s = v;
		rose_cb *rose = rose_sk(s);
		const char *devname, *callsign;
		const struct net_device *dev = rose->device;

		if (!dev)
			devname = "???";
		else
			devname = dev->name;
		
		seq_printf(seq, "%-10s %-9s ",
			rose2asc(&rose->dest_addr),
			ax2asc(&rose->dest_call));

		if (ax25cmp(&rose->source_call, &null_ax25_address) == 0)
			callsign = "??????-?";
		else
			callsign = ax2asc(&rose->source_call);

		seq_printf(seq,
			   "%-10s %-9s %-5s %3.3X %05d  %d  %d  %d  %d %3lu %3lu %3lu %3lu %3lu %3lu/%03lu %5d %5d %ld\n",
			rose2asc(&rose->source_addr),
			callsign,
			devname,
			rose->lci & 0x0FFF,
			(rose->neighbour) ? rose->neighbour->number : 0,
			rose->state,
			rose->vs,
			rose->vr,
			rose->va,
			ax25_display_timer(&rose->timer) / HZ,
			rose->t1 / HZ,
			rose->t2 / HZ,
			rose->t3 / HZ,
			rose->hb / HZ,
			ax25_display_timer(&rose->idletimer) / (60 * HZ),
			rose->idle / (60 * HZ),
			atomic_read(&s->sk_wmem_alloc),
			atomic_read(&s->sk_rmem_alloc),
			s->sk_socket ? SOCK_INODE(s->sk_socket)->i_ino : 0L);
	}

	return 0;
}

static struct seq_operations rose_info_seqops = {
	.start = rose_info_start,
	.next = rose_info_next,
	.stop = rose_info_stop,
	.show = rose_info_show,
};

static int rose_info_open(struct inode *inode, struct file *file)
{
	return seq_open(file, &rose_info_seqops);
}

static struct file_operations rose_info_fops = {
	.owner = THIS_MODULE,
	.open = rose_info_open,
	.read = seq_read,
	.llseek = seq_lseek,
	.release = seq_release,
};
#endif	/* CONFIG_PROC_FS */

static struct net_proto_family rose_family_ops = {
	.family		=	PF_ROSE,
	.create		=	rose_create,
	.owner		=	THIS_MODULE,
};

static struct proto_ops rose_proto_ops = {
	.family		=	PF_ROSE,
	.owner		=	THIS_MODULE,
	.release	=	rose_release,
	.bind		=	rose_bind,
	.connect	=	rose_connect,
	.socketpair	=	sock_no_socketpair,
	.accept		=	rose_accept,
	.getname	=	rose_getname,
	.poll		=	datagram_poll,
	.ioctl		=	rose_ioctl,
	.listen		=	rose_listen,
	.shutdown	=	sock_no_shutdown,
	.setsockopt	=	rose_setsockopt,
	.getsockopt	=	rose_getsockopt,
	.sendmsg	=	rose_sendmsg,
	.recvmsg	=	rose_recvmsg,
	.mmap		=	sock_no_mmap,
	.sendpage	=	sock_no_sendpage,
};

static struct notifier_block rose_dev_notifier = {
	.notifier_call	=	rose_device_event,
};

static struct net_device **dev_rose;

static const char banner[] = KERN_INFO "F6FBB/G4KLX ROSE for Linux. Version 0.62 for AX25.037 Linux 2.4\n";

static int __init rose_proto_init(void)
{
	int i;

	rose_callsign = null_ax25_address;

	if (rose_ndevs > 0x7FFFFFFF/sizeof(struct net_device *)) {
		printk(KERN_ERR "ROSE: rose_proto_init - rose_ndevs parameter to large\n");
		return -1;
	}

	dev_rose = kmalloc(rose_ndevs * sizeof(struct net_device *), GFP_KERNEL);
	if (dev_rose == NULL) {
		printk(KERN_ERR "ROSE: rose_proto_init - unable to allocate device structure\n");
		return -1;
	}

	memset(dev_rose, 0x00, rose_ndevs * sizeof(struct net_device*));
	for (i = 0; i < rose_ndevs; i++) {
		struct net_device *dev;
		char name[IFNAMSIZ];

		sprintf(name, "rose%d", i);
		dev = alloc_netdev(sizeof(struct net_device_stats), 
				   name, rose_setup);
		if (!dev) {
			printk(KERN_ERR "ROSE: rose_proto_init - unable to allocate memory\n");
			goto fail;
		}
		if (register_netdev(dev)) {
			printk(KERN_ERR "ROSE: netdevice regeistration failed\n");
			free_netdev(dev);
			goto fail;
		}
		dev_rose[i] = dev;
	}

	sock_register(&rose_family_ops);
	register_netdevice_notifier(&rose_dev_notifier);
	printk(banner);

	ax25_protocol_register(AX25_P_ROSE, rose_route_frame);
	ax25_linkfail_register(rose_link_failed);

#ifdef CONFIG_SYSCTL
	rose_register_sysctl();
#endif
	rose_loopback_init();

	rose_add_loopback_neigh();

	proc_net_fops_create("rose", S_IRUGO, &rose_info_fops);
	proc_net_fops_create("rose_neigh", S_IRUGO, &rose_neigh_fops);
	proc_net_fops_create("rose_nodes", S_IRUGO, &rose_nodes_fops);
	proc_net_fops_create("rose_routes", S_IRUGO, &rose_routes_fops);

	return 0;
fail:
	while (--i >= 0) {
		unregister_netdev(dev_rose[i]);
		free_netdev(dev_rose[i]);
	}
	kfree(dev_rose);
	return -ENOMEM;
}
module_init(rose_proto_init);

module_param(rose_ndevs, int, 0);
MODULE_PARM_DESC(rose_ndevs, "number of ROSE devices");

MODULE_AUTHOR("Jonathan Naylor G4KLX <[e-mail]>");
MODULE_DESCRIPTION("The amateur radio ROSE network layer protocol");
MODULE_LICENSE("GPL");
MODULE_ALIAS_NETPROTO(PF_ROSE);

static void __exit rose_exit(void)
{
	int i;

	proc_net_remove("rose");
	proc_net_remove("rose_neigh");
	proc_net_remove("rose_nodes");
	proc_net_remove("rose_routes");
	rose_loopback_clear();

	rose_rt_free();

	ax25_protocol_release(AX25_P_ROSE);
	ax25_linkfail_release(rose_link_failed);

	if (ax25cmp(&rose_callsign, &null_ax25_address) != 0)
		ax25_listen_release(&rose_callsign, NULL);

#ifdef CONFIG_SYSCTL
	rose_unregister_sysctl();
#endif
	unregister_netdevice_notifier(&rose_dev_notifier);

	sock_unregister(PF_ROSE);

	for (i = 0; i < rose_ndevs; i++) {
		struct net_device *dev = dev_rose[i];

		if (dev) {
			unregister_netdev(dev);
			free_netdev(dev);
		}
	}

	kfree(dev_rose);
}

module_exit(rose_exit);