X-Git-Url: http://www.pilppa.org/gitweb/gitweb.cgi?p=linux-2.6-omap-h63xx.git;a=blobdiff_plain;f=drivers%2Fmisc%2Fsti%2Fsti-netlink.c;fp=drivers%2Fmisc%2Fsti%2Fsti-netlink.c;h=dbd6a03ec207d3c4f6df17233f37b5142ac7413c;hp=0000000000000000000000000000000000000000;hb=0d5f2216edfeb9904e0d6512f803b0f93871baaf;hpb=14fc69723d3442ef46f8f82b3f481e82f06a346d

diff --git a/drivers/misc/sti/sti-netlink.c b/drivers/misc/sti/sti-netlink.c
new file mode 100644
index 00000000000..dbd6a03ec20
--- /dev/null
+++ b/drivers/misc/sti/sti-netlink.c
@@ -0,0 +1,157 @@
+/*
+ * OMAP STI/XTI communications interface via netlink socket.
+ *
+ * Copyright (C) 2004, 2005, 2006 Nokia Corporation
+ * 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>
+#include <linux/mutex.h>
+#include <net/sock.h>
+#include <mach/sti.h>
+
+static struct sock *sti_sock;
+static DEFINE_MUTEX(sti_netlink_mutex);
+
+enum {
+	STI_READ,
+	STI_WRITE,
+};
+
+#if defined(CONFIG_ARCH_OMAP1) || defined(CONFIG_ARCH_OMAP2)
+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;
+}
+#endif
+
+/*
+ * 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;
+	int 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;
+#if defined(CONFIG_ARCH_OMAP1) || defined(CONFIG_ARCH_OMAP2)
+	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;
+#endif
+	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 sk_buff *skb)
+{
+	if (!mutex_trylock(&sti_netlink_mutex))
+		return;
+
+	sti_netlink_receive_skb(skb);
+	mutex_unlock(&sti_netlink_mutex);
+}
+
+static int __init sti_netlink_init(void)
+{
+	sti_sock = netlink_kernel_create(&init_net, NETLINK_USERSOCK, 0,
+					 sti_netlink_receive, NULL,
+					 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");