Staging: HTC Dream: add rpcrouter driver

rpcrouter code is neccessarry for communication with QDSP and thus
many hardware components on HTC Dream, including camera hardware.


Cc: Brian Swetland <swetland@google.com>
Cc: Iliyan Malchev <ibm@android.com>
Cc: San Mehat <san@android.com>
Signed-off-by: Pavel Machek <pavel@ucw.cz>

drivers/staging/dream/smd/Kconfig

+config MSM_SMD
+	default y
+	bool "MSM Shared Memory Driver (SMD)"
+	help
+	  Support for the shared memory interface between the apps
+	  processor and the baseband processor.  Provides access to
+	  the "shared heap", as well as virtual serial channels
+	  used to communicate with various services on the baseband
+	  processor.
+
+config MSM_ONCRPCROUTER
+	depends on MSM_SMD
+	default y
+	bool "MSM ONCRPC router support"
+	help
+	  Support for the MSM ONCRPC router for communication between
+	  the ARM9 and ARM11
+
+config MSM_RPCSERVERS
+	depends on MSM_ONCRPCROUTER
+	default y
+	bool "Kernel side RPC server bundle"
+	help
+	  none
+

drivers/staging/dream/smd/Makefile

+obj-$(CONFIG_MSM_SMD) += smd.o smd_tty.o smd_qmi.o
+obj-$(CONFIG_MSM_ONCRPCROUTER) += smd_rpcrouter.o
+obj-$(CONFIG_MSM_ONCRPCROUTER) += smd_rpcrouter_device.o
+obj-$(CONFIG_MSM_ONCRPCROUTER) += smd_rpcrouter_servers.o
+obj-$(CONFIG_MSM_RPCSERVERS) += rpc_server_dog_keepalive.o
+obj-$(CONFIG_MSM_RPCSERVERS) += rpc_server_time_remote.o

drivers/staging/dream/smd/rpc_server_dog_keepalive.c

+/* arch/arm/mach-msm/rpc_server_dog_keepalive.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ * Author: Iliyan Malchev <ibm@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <mach/msm_rpcrouter.h>
+
+/* dog_keepalive server definitions */
+
+#define DOG_KEEPALIVE_PROG 0x30000015
+#if CONFIG_MSM_AMSS_VERSION==6210
+#define DOG_KEEPALIVE_VERS 0
+#define RPC_DOG_KEEPALIVE_BEACON 1
+#elif (CONFIG_MSM_AMSS_VERSION==6220) || (CONFIG_MSM_AMSS_VERSION==6225)
+#define DOG_KEEPALIVE_VERS 0x731fa727
+#define RPC_DOG_KEEPALIVE_BEACON 2
+#elif CONFIG_MSM_AMSS_VERSION==6350
+#define DOG_KEEPALIVE_VERS 0x00010000
+#define RPC_DOG_KEEPALIVE_BEACON 2
+#else
+#error "Unsupported AMSS version"
+#endif
+#define RPC_DOG_KEEPALIVE_NULL 0
+
+
+/* TODO: Remove server registration with _VERS when modem is upated with _COMP*/
+
+static int handle_rpc_call(struct msm_rpc_server *server,
+			   struct rpc_request_hdr *req, unsigned len)
+{
+	switch (req->procedure) {
+	case RPC_DOG_KEEPALIVE_NULL:
+		return 0;
+	case RPC_DOG_KEEPALIVE_BEACON:
+		printk(KERN_INFO "DOG KEEPALIVE PING\n");
+		return 0;
+	default:
+		return -ENODEV;
+	}
+}
+
+static struct msm_rpc_server rpc_server = {
+	.prog = DOG_KEEPALIVE_PROG,
+	.vers = DOG_KEEPALIVE_VERS,
+	.rpc_call = handle_rpc_call,
+};
+
+static int __init rpc_server_init(void)
+{
+	/* Dual server registration to support backwards compatibility vers */
+	return msm_rpc_create_server(&rpc_server);
+}
+
+
+module_init(rpc_server_init);

drivers/staging/dream/smd/rpc_server_time_remote.c

+/* arch/arm/mach-msm/rpc_server_time_remote.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ * Author: Iliyan Malchev <ibm@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <mach/msm_rpcrouter.h>
+
+/* time_remote_mtoa server definitions. */
+
+#define TIME_REMOTE_MTOA_PROG 0x3000005d
+#if CONFIG_MSM_AMSS_VERSION==6210
+#define TIME_REMOTE_MTOA_VERS 0
+#elif (CONFIG_MSM_AMSS_VERSION==6220) || (CONFIG_MSM_AMSS_VERSION==6225)
+#define TIME_REMOTE_MTOA_VERS 0x9202a8e4
+#elif CONFIG_MSM_AMSS_VERSION==6350
+#define TIME_REMOTE_MTOA_VERS 0x00010000
+#else
+#error "Unknown AMSS version"
+#endif
+#define RPC_TIME_REMOTE_MTOA_NULL   0
+#define RPC_TIME_TOD_SET_APPS_BASES 2
+
+struct rpc_time_tod_set_apps_bases_args {
+	uint32_t tick;
+	uint64_t stamp;
+};
+
+static int handle_rpc_call(struct msm_rpc_server *server,
+			   struct rpc_request_hdr *req, unsigned len)
+{
+	switch (req->procedure) {
+	case RPC_TIME_REMOTE_MTOA_NULL:
+		return 0;
+
+	case RPC_TIME_TOD_SET_APPS_BASES: {
+		struct rpc_time_tod_set_apps_bases_args *args;
+		args = (struct rpc_time_tod_set_apps_bases_args *)(req + 1);
+		args->tick = be32_to_cpu(args->tick);
+		args->stamp = be64_to_cpu(args->stamp);
+		printk(KERN_INFO "RPC_TIME_TOD_SET_APPS_BASES:\n"
+		       "\ttick = %d\n"
+		       "\tstamp = %lld\n",
+		       args->tick, args->stamp);
+		return 0;
+	}
+	default:
+		return -ENODEV;
+	}
+}
+
+static struct msm_rpc_server rpc_server = {
+	.prog = TIME_REMOTE_MTOA_PROG,
+	.vers = TIME_REMOTE_MTOA_VERS,
+	.rpc_call = handle_rpc_call,
+};
+
+static int __init rpc_server_init(void)
+{
+	/* Dual server registration to support backwards compatibility vers */
+	return msm_rpc_create_server(&rpc_server);
+}
+
+
+module_init(rpc_server_init);

drivers/staging/dream/smd/smd_rpcrouter.h

+/** arch/arm/mach-msm/smd_rpcrouter.h
+ *
+ * Copyright (C) 2007 Google, Inc.
+ * Copyright (c) 2007-2008 QUALCOMM Incorporated.
+ * Author: San Mehat <san@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _ARCH_ARM_MACH_MSM_SMD_RPCROUTER_H
+#define _ARCH_ARM_MACH_MSM_SMD_RPCROUTER_H
+
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/cdev.h>
+#include <linux/platform_device.h>
+#include <linux/wakelock.h>
+
+#include <mach/msm_smd.h>
+#include <mach/msm_rpcrouter.h>
+
+/* definitions for the R2R wire protcol */
+
+#define RPCROUTER_VERSION			1
+#define RPCROUTER_PROCESSORS_MAX		4
+#define RPCROUTER_MSGSIZE_MAX			512
+
+#define RPCROUTER_CLIENT_BCAST_ID		0xffffffff
+#define RPCROUTER_ROUTER_ADDRESS		0xfffffffe
+
+#define RPCROUTER_PID_LOCAL			1
+#define RPCROUTER_PID_REMOTE			0
+
+#define RPCROUTER_CTRL_CMD_DATA			1
+#define RPCROUTER_CTRL_CMD_HELLO		2
+#define RPCROUTER_CTRL_CMD_BYE			3
+#define RPCROUTER_CTRL_CMD_NEW_SERVER		4
+#define RPCROUTER_CTRL_CMD_REMOVE_SERVER	5
+#define RPCROUTER_CTRL_CMD_REMOVE_CLIENT	6
+#define RPCROUTER_CTRL_CMD_RESUME_TX		7
+#define RPCROUTER_CTRL_CMD_EXIT			8
+
+#define RPCROUTER_DEFAULT_RX_QUOTA	5
+
+union rr_control_msg {
+	uint32_t cmd;
+	struct {
+		uint32_t cmd;
+		uint32_t prog;
+		uint32_t vers;
+		uint32_t pid;
+		uint32_t cid;
+	} srv;
+	struct {
+		uint32_t cmd;
+		uint32_t pid;
+		uint32_t cid;
+	} cli;
+};
+
+struct rr_header {
+	uint32_t version;
+	uint32_t type;
+	uint32_t src_pid;
+	uint32_t src_cid;
+	uint32_t confirm_rx;
+	uint32_t size;
+	uint32_t dst_pid;
+	uint32_t dst_cid;
+};
+
+/* internals */
+
+#define RPCROUTER_MAX_REMOTE_SERVERS		100
+
+struct rr_fragment {
+	unsigned char data[RPCROUTER_MSGSIZE_MAX];
+	uint32_t length;
+	struct rr_fragment *next;
+};
+
+struct rr_packet {
+	struct list_head list;
+	struct rr_fragment *first;
+	struct rr_fragment *last;
+	struct rr_header hdr;
+	uint32_t mid;
+	uint32_t length;
+};
+
+#define PACMARK_LAST(n) ((n) & 0x80000000)
+#define PACMARK_MID(n)  (((n) >> 16) & 0xFF)
+#define PACMARK_LEN(n)  ((n) & 0xFFFF)
+
+static inline uint32_t PACMARK(uint32_t len, uint32_t mid, uint32_t first,
+			       uint32_t last)
+{
+	return (len & 0xFFFF) |
+	  ((mid & 0xFF) << 16) |
+	  ((!!first) << 30) |
+	  ((!!last) << 31);
+}
+
+struct rr_server {
+	struct list_head list;
+
+	uint32_t pid;
+	uint32_t cid;
+	uint32_t prog;
+	uint32_t vers;
+
+	dev_t device_number;
+	struct cdev cdev;
+	struct device *device;
+	struct rpcsvr_platform_device p_device;
+	char pdev_name[32];
+};
+
+struct rr_remote_endpoint {
+	uint32_t pid;
+	uint32_t cid;
+
+	int tx_quota_cntr;
+	spinlock_t quota_lock;
+	wait_queue_head_t quota_wait;
+
+	struct list_head list;
+};
+
+struct msm_rpc_endpoint {
+	struct list_head list;
+
+	/* incomplete packets waiting for assembly */
+	struct list_head incomplete;
+
+	/* complete packets waiting to be read */
+	struct list_head read_q;
+	spinlock_t read_q_lock;
+	struct wake_lock read_q_wake_lock;
+	wait_queue_head_t wait_q;
+	unsigned flags;
+
+	/* endpoint address */
+	uint32_t pid;
+	uint32_t cid;
+
+	/* bound remote address
+	 * if not connected (dst_pid == 0xffffffff) RPC_CALL writes fail
+	 * RPC_CALLs must be to the prog/vers below or they will fail
+	 */
+	uint32_t dst_pid;
+	uint32_t dst_cid;
+	uint32_t dst_prog; /* be32 */
+	uint32_t dst_vers; /* be32 */
+
+	/* reply remote address
+	 * if reply_pid == 0xffffffff, none available
+	 * RPC_REPLY writes may only go to the pid/cid/xid of the
+	 * last RPC_CALL we received.
+	 */
+	uint32_t reply_pid;
+	uint32_t reply_cid;
+	uint32_t reply_xid; /* be32 */
+	uint32_t next_pm;   /* Pacmark sequence */
+
+	/* device node if this endpoint is accessed via userspace */
+	dev_t dev;
+};
+
+/* shared between smd_rpcrouter*.c */
+
+int __msm_rpc_read(struct msm_rpc_endpoint *ept,
+		   struct rr_fragment **frag,
+		   unsigned len, long timeout);
+
+struct msm_rpc_endpoint *msm_rpcrouter_create_local_endpoint(dev_t dev);
+int msm_rpcrouter_destroy_local_endpoint(struct msm_rpc_endpoint *ept);
+
+int msm_rpcrouter_create_server_cdev(struct rr_server *server);
+int msm_rpcrouter_create_server_pdev(struct rr_server *server);
+
+int msm_rpcrouter_init_devices(void);
+void msm_rpcrouter_exit_devices(void);
+
+extern dev_t msm_rpcrouter_devno;
+extern struct class *msm_rpcrouter_class;
+#endif

drivers/staging/dream/smd/smd_rpcrouter_device.c

+/* arch/arm/mach-msm/smd_rpcrouter_device.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ * Copyright (c) 2007-2009 QUALCOMM Incorporated.
+ * Author: San Mehat <san@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/cdev.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+#include <linux/sched.h>
+#include <linux/poll.h>
+#include <linux/platform_device.h>
+#include <linux/msm_rpcrouter.h>
+
+#include <asm/uaccess.h>
+#include <asm/byteorder.h>
+
+#include "smd_rpcrouter.h"
+
+#define SAFETY_MEM_SIZE 65536
+
+/* Next minor # available for a remote server */
+static int next_minor = 1;
+
+struct class *msm_rpcrouter_class;
+dev_t msm_rpcrouter_devno;
+
+static struct cdev rpcrouter_cdev;
+static struct device *rpcrouter_device;
+
+static int rpcrouter_open(struct inode *inode, struct file *filp)
+{
+	int rc;
+	struct msm_rpc_endpoint *ept;
+
+	rc = nonseekable_open(inode, filp);
+	if (rc < 0)
+		return rc;
+
+	ept = msm_rpcrouter_create_local_endpoint(inode->i_rdev);
+	if (!ept)
+		return -ENOMEM;
+
+	filp->private_data = ept;
+	return 0;
+}
+
+static int rpcrouter_release(struct inode *inode, struct file *filp)
+{
+	struct msm_rpc_endpoint *ept;
+	ept = (struct msm_rpc_endpoint *) filp->private_data;
+
+	return msm_rpcrouter_destroy_local_endpoint(ept);
+}
+
+static ssize_t rpcrouter_read(struct file *filp, char __user *buf,
+			      size_t count, loff_t *ppos)
+{
+	struct msm_rpc_endpoint *ept;
+	struct rr_fragment *frag, *next;
+	int rc;
+
+	ept = (struct msm_rpc_endpoint *) filp->private_data;
+
+	rc = __msm_rpc_read(ept, &frag, count, -1);
+	if (rc < 0)
+		return rc;
+
+	count = rc;
+
+	while (frag != NULL) {
+		if (copy_to_user(buf, frag->data, frag->length)) {
+			printk(KERN_ERR
+			       "rpcrouter: could not copy all read data to user!\n");
+			rc = -EFAULT;
+		}
+		buf += frag->length;
+		next = frag->next;
+		kfree(frag);
+		frag = next;
+	}
+
+	return rc;
+}
+
+static ssize_t rpcrouter_write(struct file *filp, const char __user *buf,
+				size_t count, loff_t *ppos)
+{
+	struct msm_rpc_endpoint	*ept;
+	int rc = 0;
+	void *k_buffer;
+
+	ept = (struct msm_rpc_endpoint *) filp->private_data;
+
+	/* A check for safety, this seems non-standard */
+	if (count > SAFETY_MEM_SIZE)
+		return -EINVAL;
+
+	k_buffer = kmalloc(count, GFP_KERNEL);
+	if (!k_buffer)
+		return -ENOMEM;
+
+	if (copy_from_user(k_buffer, buf, count)) {
+		rc = -EFAULT;
+		goto write_out_free;
+	}
+
+	rc = msm_rpc_write(ept, k_buffer, count);
+	if (rc < 0)
+		goto write_out_free;
+
+	rc = count;
+write_out_free:
+	kfree(k_buffer);
+	return rc;
+}
+
+static unsigned int rpcrouter_poll(struct file *filp,
+				   struct poll_table_struct *wait)
+{
+	struct msm_rpc_endpoint *ept;
+	unsigned mask = 0;
+	ept = (struct msm_rpc_endpoint *) filp->private_data;
+
+	/* If there's data already in the read queue, return POLLIN.
+	 * Else, wait for the requested amount of time, and check again.
+	 */
+
+	if (!list_empty(&ept->read_q))
+		mask |= POLLIN;
+
+	if (!mask) {
+		poll_wait(filp, &ept->wait_q, wait);
+		if (!list_empty(&ept->read_q))
+			mask |= POLLIN;
+	}
+
+	return mask;
+}
+
+static long rpcrouter_ioctl(struct file *filp, unsigned int cmd,
+			    unsigned long arg)
+{
+	struct msm_rpc_endpoint *ept;
+	struct rpcrouter_ioctl_server_args server_args;
+	int rc = 0;
+	uint32_t n;
+
+	ept = (struct msm_rpc_endpoint *) filp->private_data;
+	switch (cmd) {
+
+	case RPC_ROUTER_IOCTL_GET_VERSION:
+		n = RPC_ROUTER_VERSION_V1;
+		rc = put_user(n, (unsigned int *) arg);
+		break;
+
+	case RPC_ROUTER_IOCTL_GET_MTU:
+		/* the pacmark word reduces the actual payload
+		 * possible per message
+		 */
+		n = RPCROUTER_MSGSIZE_MAX - sizeof(uint32_t);
+		rc = put_user(n, (unsigned int *) arg);
+		break;
+
+	case RPC_ROUTER_IOCTL_REGISTER_SERVER:
+		rc = copy_from_user(&server_args, (void *) arg,
+				    sizeof(server_args));
+		if (rc < 0)
+			break;
+		msm_rpc_register_server(ept,
+					server_args.prog,
+					server_args.vers);
+		break;
+
+	case RPC_ROUTER_IOCTL_UNREGISTER_SERVER:
+		rc = copy_from_user(&server_args, (void *) arg,
+				    sizeof(server_args));
+		if (rc < 0)
+			break;
+
+		msm_rpc_unregister_server(ept,
+					  server_args.prog,
+					  server_args.vers);
+		break;
+
+	case RPC_ROUTER_IOCTL_GET_MINOR_VERSION:
+		n = MSM_RPC_GET_MINOR(msm_rpc_get_vers(ept));
+		rc = put_user(n, (unsigned int *)arg);
+		break;
+
+	default:
+		rc = -EINVAL;
+		break;
+	}
+
+	return rc;
+}
+
+static struct file_operations rpcrouter_server_fops = {
+	.owner	 = THIS_MODULE,
+	.open	 = rpcrouter_open,
+	.release = rpcrouter_release,
+	.read	 = rpcrouter_read,
+	.write	 = rpcrouter_write,
+	.poll    = rpcrouter_poll,
+	.unlocked_ioctl	 = rpcrouter_ioctl,
+};
+
+static struct file_operations rpcrouter_router_fops = {
+	.owner	 = THIS_MODULE,
+	.open	 = rpcrouter_open,
+	.release = rpcrouter_release,
+	.read	 = rpcrouter_read,
+	.write	 = rpcrouter_write,
+	.poll    = rpcrouter_poll,
+	.unlocked_ioctl = rpcrouter_ioctl,
+};
+
+int msm_rpcrouter_create_server_cdev(struct rr_server *server)
+{
+	int rc;
+	uint32_t dev_vers;
+
+	if (next_minor == RPCROUTER_MAX_REMOTE_SERVERS) {
+		printk(KERN_ERR
+		       "rpcrouter: Minor numbers exhausted - Increase "
+		       "RPCROUTER_MAX_REMOTE_SERVERS\n");
+		return -ENOBUFS;
+	}
+
+#if CONFIG_MSM_AMSS_VERSION >= 6350
+	/* Servers with bit 31 set are remote msm servers with hashkey version.
+	 * Servers with bit 31 not set are remote msm servers with
+	 * backwards compatible version type in which case the minor number
+	 * (lower 16 bits) is set to zero.
+	 *
+	 */
+	if ((server->vers & RPC_VERSION_MODE_MASK))
+		dev_vers = server->vers;
+	else
+		dev_vers = server->vers & RPC_VERSION_MAJOR_MASK;
+#else
+	dev_vers = server->vers;
+#endif
+
+	server->device_number =
+		MKDEV(MAJOR(msm_rpcrouter_devno), next_minor++);
+
+	server->device =
+		device_create(msm_rpcrouter_class, rpcrouter_device,
+			      server->device_number, NULL, "%.8x:%.8x",
+			      server->prog, dev_vers);
+	if (IS_ERR(server->device)) {
+		printk(KERN_ERR
+		       "rpcrouter: Unable to create device (%ld)\n",
+		       PTR_ERR(server->device));
+		return PTR_ERR(server->device);;
+	}
+
+	cdev_init(&server->cdev, &rpcrouter_server_fops);
+	server->cdev.owner = THIS_MODULE;
+
+	rc = cdev_add(&server->cdev, server->device_number, 1);
+	if (rc < 0) {
+		printk(KERN_ERR
+		       "rpcrouter: Unable to add chrdev (%d)\n", rc);
+		device_destroy(msm_rpcrouter_class, server->device_number);
+		return rc;
+	}
+	return 0;
+}
+
+/* for backward compatible version type (31st bit cleared)
+ * clearing minor number (lower 16 bits) in device name
+ * is neccessary for driver binding
+ */
+int msm_rpcrouter_create_server_pdev(struct rr_server *server)
+{
+	sprintf(server->pdev_name, "rs%.8x:%.8x",
+		server->prog,
+#if CONFIG_MSM_AMSS_VERSION >= 6350
+		(server->vers & RPC_VERSION_MODE_MASK) ? server->vers :
+		(server->vers & RPC_VERSION_MAJOR_MASK));
+#else
+		server->vers);
+#endif
+
+	server->p_device.base.id = -1;
+	server->p_device.base.name = server->pdev_name;
+
+	server->p_device.prog = server->prog;
+	server->p_device.vers = server->vers;
+
+	platform_device_register(&server->p_device.base);
+	return 0;
+}
+
+int msm_rpcrouter_init_devices(void)
+{
+	int rc;
+	int major;
+
+	/* Create the device nodes */
+	msm_rpcrouter_class = class_create(THIS_MODULE, "oncrpc");
+	if (IS_ERR(msm_rpcrouter_class)) {
+		rc = -ENOMEM;
+		printk(KERN_ERR
+		       "rpcrouter: failed to create oncrpc class\n");
+		goto fail;
+	}
+
+	rc = alloc_chrdev_region(&msm_rpcrouter_devno, 0,
+				 RPCROUTER_MAX_REMOTE_SERVERS + 1,
+				 "oncrpc");
+	if (rc < 0) {
+		printk(KERN_ERR
+		       "rpcrouter: Failed to alloc chardev region (%d)\n", rc);
+		goto fail_destroy_class;
+	}
+
+	major = MAJOR(msm_rpcrouter_devno);
+	rpcrouter_device = device_create(msm_rpcrouter_class, NULL,
+					 msm_rpcrouter_devno, NULL, "%.8x:%d",
+					 0, 0);
+	if (IS_ERR(rpcrouter_device)) {
+		rc = -ENOMEM;
+		goto fail_unregister_cdev_region;
+	}
+
+	cdev_init(&rpcrouter_cdev, &rpcrouter_router_fops);
+	rpcrouter_cdev.owner = THIS_MODULE;
+
+	rc = cdev_add(&rpcrouter_cdev, msm_rpcrouter_devno, 1);
+	if (rc < 0)
+		goto fail_destroy_device;
+
+	return 0;
+
+fail_destroy_device:
+	device_destroy(msm_rpcrouter_class, msm_rpcrouter_devno);
+fail_unregister_cdev_region:
+	unregister_chrdev_region(msm_rpcrouter_devno,
+				 RPCROUTER_MAX_REMOTE_SERVERS + 1);
+fail_destroy_class:
+	class_destroy(msm_rpcrouter_class);
+fail:
+	return rc;
+}
+
+void msm_rpcrouter_exit_devices(void)
+{
+	cdev_del(&rpcrouter_cdev);
+	device_destroy(msm_rpcrouter_class, msm_rpcrouter_devno);
+	unregister_chrdev_region(msm_rpcrouter_devno,
+				 RPCROUTER_MAX_REMOTE_SERVERS + 1);
+	class_destroy(msm_rpcrouter_class);
+}
+

drivers/staging/dream/smd/smd_rpcrouter_servers.c

+/* arch/arm/mach-msm/rpc_servers.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ * Author: Iliyan Malchev <ibm@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/cdev.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/fs.h>
+#include <linux/kthread.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/wakelock.h>
+
+#include <linux/msm_rpcrouter.h>
+#include <linux/uaccess.h>
+
+#include <mach/msm_rpcrouter.h>
+#include "smd_rpcrouter.h"
+
+static struct msm_rpc_endpoint *endpoint;
+
+#define FLAG_REGISTERED 0x0001
+
+static LIST_HEAD(rpc_server_list);
+static DEFINE_MUTEX(rpc_server_list_lock);
+static int rpc_servers_active;
+static struct wake_lock rpc_servers_wake_lock;
+
+static void rpc_server_register(struct msm_rpc_server *server)
+{
+	int rc;
+	rc = msm_rpc_register_server(endpoint, server->prog, server->vers);
+	if (rc < 0)
+		printk(KERN_ERR "[rpcserver] error registering %p @ %08x:%d\n",
+		       server, server->prog, server->vers);
+}
+
+static struct msm_rpc_server *rpc_server_find(uint32_t prog, uint32_t vers)
+{
+	struct msm_rpc_server *server;
+
+	mutex_lock(&rpc_server_list_lock);
+	list_for_each_entry(server, &rpc_server_list, list) {
+		if ((server->prog == prog) &&
+#if CONFIG_MSM_AMSS_VERSION >= 6350
+		    msm_rpc_is_compatible_version(server->vers, vers)) {
+#else
+		    server->vers == vers) {
+#endif
+			mutex_unlock(&rpc_server_list_lock);
+			return server;
+		}
+	}
+	mutex_unlock(&rpc_server_list_lock);
+	return NULL;
+}
+
+static void rpc_server_register_all(void)
+{
+	struct msm_rpc_server *server;
+
+	mutex_lock(&rpc_server_list_lock);
+	list_for_each_entry(server, &rpc_server_list, list) {
+		if (!(server->flags & FLAG_REGISTERED)) {
+			rpc_server_register(server);
+			server->flags |= FLAG_REGISTERED;
+		}
+	}
+	mutex_unlock(&rpc_server_list_lock);
+}
+
+int msm_rpc_create_server(struct msm_rpc_server *server)
+{
+	/* make sure we're in a sane state first */
+	server->flags = 0;
+	INIT_LIST_HEAD(&server->list);
+
+	mutex_lock(&rpc_server_list_lock);
+	list_add(&server->list, &rpc_server_list);
+	if (rpc_servers_active) {
+		rpc_server_register(server);
+		server->flags |= FLAG_REGISTERED;
+	}
+	mutex_unlock(&rpc_server_list_lock);
+
+	return 0;
+}
+
+static int rpc_send_accepted_void_reply(struct msm_rpc_endpoint *client,
+					uint32_t xid, uint32_t accept_status)
+{
+	int rc = 0;
+	uint8_t reply_buf[sizeof(struct rpc_reply_hdr)];
+	struct rpc_reply_hdr *reply = (struct rpc_reply_hdr *)reply_buf;
+
+	reply->xid = cpu_to_be32(xid);
+	reply->type = cpu_to_be32(1); /* reply */
+	reply->reply_stat = cpu_to_be32(RPCMSG_REPLYSTAT_ACCEPTED);
+
+	reply->data.acc_hdr.accept_stat = cpu_to_be32(accept_status);
+	reply->data.acc_hdr.verf_flavor = 0;
+	reply->data.acc_hdr.verf_length = 0;
+
+	rc = msm_rpc_write(client, reply_buf, sizeof(reply_buf));
+	if (rc < 0)
+		printk(KERN_ERR
+		       "%s: could not write response: %d\n",
+		       __FUNCTION__, rc);
+
+	return rc;
+}
+
+static int rpc_servers_thread(void *data)
+{
+	void *buffer;
+	struct rpc_request_hdr *req;
+	struct msm_rpc_server *server;
+	int rc;
+
+	for (;;) {
+		wake_unlock(&rpc_servers_wake_lock);
+		rc = wait_event_interruptible(endpoint->wait_q,
+						!list_empty(&endpoint->read_q));
+		wake_lock(&rpc_servers_wake_lock);
+		rc = msm_rpc_read(endpoint, &buffer, -1, -1);
+		if (rc < 0) {
+			printk(KERN_ERR "%s: could not read: %d\n",
+			       __FUNCTION__, rc);
+			break;
+		}
+		req = (struct rpc_request_hdr *)buffer;
+
+		req->type = be32_to_cpu(req->type);
+		req->xid = be32_to_cpu(req->xid);
+		req->rpc_vers = be32_to_cpu(req->rpc_vers);
+		req->prog = be32_to_cpu(req->prog);
+		req->vers = be32_to_cpu(req->vers);
+		req->procedure = be32_to_cpu(req->procedure);
+
+		server = rpc_server_find(req->prog, req->vers);
+
+		if (req->rpc_vers != 2)
+			continue;
+		if (req->type != 0)
+			continue;
+		if (!server) {
+			rpc_send_accepted_void_reply(
+				endpoint, req->xid,
+				RPC_ACCEPTSTAT_PROG_UNAVAIL);
+			continue;
+		}
+
+		rc = server->rpc_call(server, req, rc);
+
+		switch (rc) {
+		case 0:
+			rpc_send_accepted_void_reply(
+				endpoint, req->xid,
+				RPC_ACCEPTSTAT_SUCCESS);
+			break;
+		default:
+			rpc_send_accepted_void_reply(
+				endpoint, req->xid,
+				RPC_ACCEPTSTAT_PROG_UNAVAIL);
+			break;
+		}
+
+		kfree(buffer);
+	}
+
+	do_exit(0);
+}
+
+static int rpcservers_probe(struct platform_device *pdev)
+{
+	struct task_struct *server_thread;
+
+	endpoint = msm_rpc_open();
+	if (IS_ERR(endpoint))
+		return PTR_ERR(endpoint);
+
+	/* we're online -- register any servers installed beforehand */
+	rpc_servers_active = 1;
+	rpc_server_register_all();
+
+	/* start the kernel thread */
+	server_thread = kthread_run(rpc_servers_thread, NULL, "krpcserversd");
+	if (IS_ERR(server_thread))
+		return PTR_ERR(server_thread);
+
+	return 0;
+}
+
+static struct platform_driver rpcservers_driver = {
+	.probe	= rpcservers_probe,
+	.driver	= {
+		.name	= "oncrpc_router",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init rpc_servers_init(void)
+{
+	wake_lock_init(&rpc_servers_wake_lock, WAKE_LOCK_SUSPEND, "rpc_server");
+	return platform_driver_register(&rpcservers_driver);
+}
+
+module_init(rpc_servers_init);
+
+MODULE_DESCRIPTION("MSM RPC Servers");
+MODULE_AUTHOR("Iliyan Malchev <ibm@android.com>");
+MODULE_LICENSE("GPL");