sti-netlink.c 3.41 KB
Newer Older
1 2 3
/*
 * OMAP STI/XTI communications interface via netlink socket.
 *
4
 * Copyright (C) 2004, 2005, 2006 Nokia Corporation
5 6 7 8 9 10 11 12 13 14 15
 * Written by: Paul Mundt <paul.mundt@nokia.com>
 *
 * This file is subject to the terms and conditions of the GNU General Public
 * License.  See the file "COPYING" in the main directory of this archive
 * for more details.
 */
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/netlink.h>
#include <linux/socket.h>
#include <linux/skbuff.h>
16
#include <linux/mutex.h>
17 18 19 20
#include <net/sock.h>
#include <asm/arch/sti.h>

static struct sock *sti_sock;
21
static DEFINE_MUTEX(sti_netlink_mutex);
22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129

enum {
	STI_READ,
	STI_WRITE,
};

static int sti_netlink_read(int pid, int seq, void *payload, int size)
{
	struct sk_buff *skb;
	struct nlmsghdr *nlh;
	int ret, len = NLMSG_SPACE(size);
	unsigned char *tail;

	skb = alloc_skb(len, GFP_KERNEL);
	if (!skb)
		return -ENOMEM;

	tail = skb->tail;
	nlh = NLMSG_PUT(skb, pid, seq, STI_READ,
			len - (sizeof(struct nlmsghdr)));
	nlh->nlmsg_flags = 0;
	memcpy(NLMSG_DATA(nlh), payload, size);
	nlh->nlmsg_len = skb->tail - tail;

	ret = netlink_unicast(sti_sock, skb, pid, MSG_DONTWAIT);
	if (ret > 0)
		ret = 0;

	return ret;

nlmsg_failure:
	if (skb)
		kfree_skb(skb);

	return -EINVAL;
}

/*
 * We abuse nlmsg_type and nlmsg_flags for our purposes.
 *
 * The ID is encoded into the upper 8 bits of the nlmsg_type, while the
 * channel number is encoded into the upper 8 bits of the nlmsg_flags.
 */
static int sti_netlink_receive_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
{
	void *data;
	u8 chan, id;
	int size, ret = 0, len = 0;

	data	= NLMSG_DATA(nlh);
	chan	= (nlh->nlmsg_flags >> 8) & 0xff;
	id	= (nlh->nlmsg_type  >> 8) & 0xff;
	size	= (int)(nlh->nlmsg_len - ((char *)data - (char *)nlh));

	switch (nlh->nlmsg_type & 0xff) {
	case STI_WRITE:
		sti_channel_write_trace(size, id, data, chan);
		break;
	case STI_READ:
		data = kmalloc(size, GFP_KERNEL);
		if (!data)
			return -ENOMEM;
		memset(data, 0, size);

		len = sti_read_packet(data, size);
		ret = sti_netlink_read(NETLINK_CB(skb).pid, nlh->nlmsg_seq,
				       data, len);
		kfree(data);
		break;
	default:
		return -ENOTTY;
	}

	return ret;
}

static int sti_netlink_receive_skb(struct sk_buff *skb)
{
	while (skb->len >= NLMSG_SPACE(0)) {
		struct nlmsghdr *nlh;
		u32 rlen;
		int ret;

		nlh = (struct nlmsghdr *)skb->data;
		if (nlh->nlmsg_len < sizeof(struct nlmsghdr) ||
		    skb->len < nlh->nlmsg_len)
			break;

		rlen = NLMSG_ALIGN(nlh->nlmsg_len);
		if (rlen > skb->len)
			rlen = skb->len;

		ret = sti_netlink_receive_msg(skb, nlh);
		if (ret)
			netlink_ack(skb, nlh, -ret);
		else if (nlh->nlmsg_flags & NLM_F_ACK)
			netlink_ack(skb, nlh, 0);

		skb_pull(skb, rlen);
	}

	return 0;
}

static void sti_netlink_receive(struct sock *sk, int len)
{
	struct sk_buff *skb;

130
	if (!mutex_trylock(&sti_netlink_mutex))
131 132 133 134 135 136 137 138
		return;

	while ((skb = skb_dequeue(&sk->sk_receive_queue)))
		if (sti_netlink_receive_skb(skb) && skb->len)
			skb_queue_head(&sk->sk_receive_queue, skb);
		else
			kfree_skb(skb);

139
	mutex_unlock(&sti_netlink_mutex);
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158
}

static int __init sti_netlink_init(void)
{
	sti_sock = netlink_kernel_create(NETLINK_USERSOCK, 0,
					 sti_netlink_receive, THIS_MODULE);
	if (!sti_sock) {
		printk(KERN_ERR "STI: Failed to create netlink socket\n");
		return -ENODEV;
	}

	return 0;
}

module_init(sti_netlink_init);

MODULE_AUTHOR("Paul Mundt");
MODULE_LICENSE("GPL");
MODULE_DESCRIPTION("STI netlink-driven communications interface");