Merge "msm: rdbg: Add snapshot of remote debugger driver"

4 files changed, 1648 insertions, 7 deletions

diff --git a/Documentation/arm/msm/remote_debug_drv.txt b/Documentation/arm/msm/remote_debug_drv.txt
new file mode 100644
index 000000000000..13a35f43e86b
--- /dev/null
+++ b/Documentation/arm/msm/remote_debug_drv.txt
@@ -0,0 +1,468 @@
+Introduction
+============
+
+The goal of this debug feature is to provide a reliable, responsive,
+accurate and secure debug capability to developers interested in
+debugging MSM subsystem processor images without the use of a hardware
+debugger.
+
+The Debug Agent along with the Remote Debug Driver implements a shared
+memory based transport mechanism that allows for a debugger (ex. GDB)
+running on a host PC to communicate with a remote stub running on
+peripheral subsystems such as the ADSP, MODEM etc.
+
+The diagram below depicts end to end the components involved to
+support remote debugging:
+
+
+:               :
+:    HOST (PC)  :  MSM
+:  ,--------,   :   ,-------,
+:  |        |   :   | Debug |                         ,--------,
+:  |Debugger|<--:-->| Agent |                         | Remote |
+:  |        |   :   |  App  |                  +----->| Debug  |
+:  `--------`   :   |-------|    ,--------,    |      | Stub   |
+:               :   | Remote|    |        |<---+      `--------`
+:               :   | Debug |<-->|--------|
+:               :   | Driver|    |        |<---+      ,--------,
+:               :   `-------`    `--------`    |      | Remote |
+:               :       LA         Shared      +----->| Debug  |
+:               :                  Memory             | Stub   |
+:               :                                     `--------`
+:               :                               Peripheral Subsystems
+:               :                                 (ADSP, MODEM, ...)
+
+
+Debugger:       Debugger application running on the host PC that
+                communicates with the remote stub.
+                Examples: GDB, LLDB
+
+Debug Agent:    Software that runs on the Linux Android platform
+                that provides connectivity from the MSM to the
+                host PC. This involves two portions:
+                1) User mode Debug Agent application that discovers
+                processes running on the subsystems and creates
+                TCP/IP sockets for the host to connect to. In addition
+                to this, it creates an info (or meta) port that
+                users can connect to discover the various
+                processes and their corresponding debug ports.
+
+Remote Debug    A character based driver that the Debug
+Driver:         Agent uses to transport the payload received from the
+                host to the debug stub running on the subsystem
+                processor over shared memory and vice versa.
+
+Shared Memory:  Shared memory from the SMEM pool that is accessible
+                from the Applications Processor (AP) and the
+                subsystem processors.
+
+Remote Debug    Privileged code that runs in the kernels of the
+Stub:           subsystem processors that receives debug commands
+                from the debugger running on the host and
+                acts on these commands. These commands include reading
+                and writing to registers and memory belonging to the
+                subsystem's address space, setting breakpoints,
+                single stepping etc.
+
+Hardware description
+====================
+
+The Remote Debug Driver interfaces with the Remote Debug stubs
+running on the subsystem processors and does not drive or
+manage any hardware resources.
+
+Software description
+====================
+
+The debugger and the remote stubs use Remote Serial Protocol (RSP)
+to communicate with each other. This is widely used protocol by both
+software and hardware debuggers. RSP is an ASCII based protocol
+and used when it is not possible to run GDB server on the target under
+debug.
+
+The Debug Agent application along with the Remote Debug Driver
+is responsible for establishing a bi-directional connection from
+the debugger application running on the host to the remote debug
+stub running on a subsystem. The Debug Agent establishes connectivity
+to the host PC via TCP/IP sockets.
+
+This feature uses ADB port forwarding to establish connectivity
+between the debugger running on the host and the target under debug.
+
+Please note the Debug Agent does not expose HLOS memory to the
+remote subsystem processors.
+
+Design
+======
+
+Here is the overall flow:
+
+1) When the Debug Agent application starts up, it opens up a shared memory
+based transport channel to the various subsystem processor images.
+
+2) The Debug Agent application sends messages across to the remote stubs
+to discover the various processes that are running on the subsystem and
+creates debug sockets for each of them.
+
+3) Whenever a process running on a subsystem exits, the Debug Agent
+is notified by the stub so that the debug port and other resources
+can be reclaimed.
+
+4) The Debug Agent uses the services of the Remote Debug Driver to
+transport payload from the host debugger to the remote stub and vice versa.
+
+5) Communication between the Remote Debug Driver and the Remote Debug stub
+running on the subsystem processor is done over shared memory (see figure).
+SMEM services are used to allocate the shared memory that will
+be readable and writeable by the AP and the subsystem image under debug.
+
+A separate SMEM allocation takes place for each subsystem processor
+involved in remote debugging. The remote stub running on each of the
+subsystems allocates a SMEM buffer using a unique identifier so that both
+the AP and subsystem get the same physical block of memory. It should be
+noted that subsystem images can be restarted at any time.
+However, when a subsystem comes back up, its stub uses the same unique
+SMEM identifier to allocate the SMEM block. This would not result in a
+new allocation rather the same block of memory in the first bootup instance
+is provided back to the stub running on the subsystem.
+
+An 8KB chunk of shared memory is allocated and used for communication
+per subsystem. For multi-process capable subsystems, 16KB chunk of shared
+memory is allocated to allow for simultaneous debugging of more than one
+process running on a single subsystem.
+
+The shared memory is used as a circular ring buffer in each direction.
+Thus we have a bi-directional shared memory channel between the AP
+and a subsystem. We call this SMQ. Each memory channel contains a header,
+data and a control mechanism that is used to synchronize read and write
+of data between the AP and the remote subsystem.
+
+Overall SMQ memory view:
+:
+:    +------------------------------------------------+
+:    | SMEM buffer                                    |
+:    |-----------------------+------------------------|
+:    |Producer: LA           | Producer: Remote       |
+:    |Consumer: Remote       |           subsystem    |
+:    |          subsystem    | Consumer: LA           |
+:    |                       |                        |
+:    |               Producer|                Consumer|
+:    +-----------------------+------------------------+
+:    |                       |
+:    |                       |
+:    |                       +--------------------------------------+
+:    |                                                              |
+:    |                                                              |
+:    v                                                              v
+:    +--------------------------------------------------------------+
+:    |   Header  |       Data      |            Control             |
+:    +-----------+---+---+---+-----+----+--+--+-----+---+--+--+-----+
+:    |           | b | b | b |     | S  |n |n |     | S |n |n |     |
+:    |  Producer | l | l | l |     | M  |o |o |     | M |o |o |     |
+:    |    Ver    | o | o | o |     | Q  |d |d |     | Q |d |d |     |
+:    |-----------| c | c | c | ... |    |e |e | ... |   |e |e | ... |
+:    |           | k | k | k |     | O  |  |  |     | I |  |  |     |
+:    |  Consumer |   |   |   |     | u  |0 |1 |     | n |0 |1 |     |
+:    |    Ver    | 0 | 1 | 2 |     | t  |  |  |     |   |  |  |     |
+:    +-----------+---+---+---+-----+----+--+--+-----+---+--+--+-----+
+:                                       |           |
+:                                       +           |
+:                                                   |
+:                          +------------------------+
+:                          |
+:                          v
+:                        +----+----+----+----+
+:                        | SMQ Nodes         |
+:                        |----|----|----|----|
+:                 Node # |  0 |  1 |  2 | ...|
+:                        |----|----|----|----|
+: Starting Block Index # |  0 |  3 |  8 | ...|
+:                        |----|----|----|----|
+:            # of blocks |  3 |  5 |  1 | ...|
+:                        +----+----+----+----+
+:
+
+Header: Contains version numbers for software compatibility to ensure
+that both producers and consumers on the AP and subsystems know how to
+read from and write to the queue.
+Both the producer and consumer versions are 1.
+:     +---------+-------------------+
+:     | Size    | Field             |
+:     +---------+-------------------+
+:     | 1 byte  | Producer Version  |
+:     +---------+-------------------+
+:     | 1 byte  | Consumer Version  |
+:     +---------+-------------------+
+
+
+Data: The data portion contains multiple blocks [0..N] of a fixed size.
+The block size SM_BLOCKSIZE is fixed to 128 bytes for header version #1.
+Payload sent from the debug agent app is split (if necessary) and placed
+in these blocks. The first data block is placed at the next 8 byte aligned
+address after the header.
+
+The number of blocks for a given SMEM allocation is derived as follows:
+  Number of Blocks = ((Total Size - Alignment - Size of Header
+                      - Size of SMQIn - Size of SMQOut)/(SM_BLOCKSIZE))
+
+The producer maintains a private block map of each of these blocks to
+determine which of these blocks in the queue is available and which are free.
+
+Control:
+The control portion contains a list of nodes [0..N] where N is number
+of available data blocks. Each node identifies the data
+block indexes that contain a particular debug message to be transferred,
+and the number of blocks it took to hold the contents of the message.
+
+Each node has the following structure:
+:     +---------+-------------------+
+:     | Size    | Field             |
+:     +---------+-------------------+
+:     | 2 bytes |Staring Block Index|
+:     +---------+-------------------+
+:     | 2 bytes |Number of Blocks   |
+:     +---------+-------------------+
+
+The producer and the consumer update different parts of the control channel
+(SMQOut / SMQIn) respectively. Each of these control data structures contains
+information about the last node that was written / read, and the actual nodes
+that were written/read.
+
+SMQOut Structure (R/W by producer, R by consumer):
+:     +---------+-------------------+
+:     | Size    | Field             |
+:     +---------+-------------------+
+:     | 4 bytes | Magic Init Number |
+:     +---------+-------------------+
+:     | 4 bytes | Reset             |
+:     +---------+-------------------+
+:     | 4 bytes | Last Sent Index   |
+:     +---------+-------------------+
+:     | 4 bytes | Index Free Read   |
+:     +---------+-------------------+
+
+SMQIn Structure (R/W by consumer, R by producer):
+:     +---------+-------------------+
+:     | Size    | Field             |
+:     +---------+-------------------+
+:     | 4 bytes | Magic Init Number |
+:     +---------+-------------------+
+:     | 4 bytes | Reset ACK         |
+:     +---------+-------------------+
+:     | 4 bytes | Last Read Index   |
+:     +---------+-------------------+
+:     | 4 bytes | Index Free Write  |
+:     +---------+-------------------+
+
+Magic Init Number:
+Both SMQ Out and SMQ In initialize this field with a predefined magic
+number so as to make sure that both the consumer and producer blocks
+have fully initialized and have valid data in the shared memory control area.
+  Producer Magic #: 0xFF00FF01
+  Consumer Magic #: 0xFF00FF02
+
+SMQ Out's Last Sent Index and Index Free Read:
+  Only a producer can write to these indexes and they are updated whenever
+  there is new payload to be inserted into the SMQ in order to be sent to a
+  consumer.
+
+  The number of blocks required for the SMQ allocation is determined as:
+   (payload size + SM_BLOCKSIZE - 1) / SM_BLOCKSIZE
+
+  The private block map is searched for a large enough continuous set of blocks
+  and the user data is copied into the data blocks.
+
+  The starting index of the free block(s) is updated in the SMQOut's Last Sent
+  Index. This update keeps track of which index was last written to and the
+  producer uses it to determine where the the next allocation could be done.
+
+  Every allocation, a producer updates the Index Free Read from its
+  collaborating consumer's Index Free Write field (if they are unequal).
+  This index value indicates that the consumer has read all blocks associated
+  with allocation on the SMQ and that the producer can reuse these blocks for
+  subsquent allocations since this is a circular queue.
+
+  At cold boot and restart, these indexes are initialized to zero and all
+  blocks are marked as available for allocation.
+
+SMQ In's Last Read Index and Index Free Write:
+  These indexes are written to only by a consumer and are updated whenever
+  there is new payload to be read from the SMQ. The Last Read Index keeps
+  track of which index was last read by the consumer and using this, it
+  determines where the next read should be done.
+  After completing a read, Last Read Index is incremented to the
+  next block index. A consumer updates Index Free Write to the starting
+  index of an allocation whenever it has completed processing the blocks.
+  This is an optimization that can be used to prevent an additional copy
+  of data from the queue into a client's data buffer and the data in the queue
+  itself can be used.
+  Once Index Free Write is updated, the collaborating producer (on the next
+  data allocation) reads the updated Index Free Write value and it then
+  updates its corresponding SMQ Out's Index Free Read and marks the blocks
+  associated with that index as available for allocation. At cold boot and
+  restart, these indexes are initialized to zero.
+
+SMQ Out Reset# and SMQ In Reset ACK #:
+  Since subsystems can restart at anytime, the data blocks and control channel
+  can be in an inconsistent state when a producer or consumer comes up.
+  We use Reset and Reset ACK to manage this. At cold boot, the producer
+  initializes the Reset# to a known number ex. 1. Every other reset that the
+  producer undergoes, the Reset#1 is simply incremented by 1. All the producer
+  indexes are reset.
+  When the producer notifies the consumer of data availability, the consumer
+  reads the producers Reset # and copies that into its SMQ In Reset ACK#
+  field when they differ. When that occurs, the consumer resets its
+  indexes to 0.
+
+6) Asynchronous notifications between a producer and consumer are
+done using the SMP2P service which is interrupt based.
+
+Power Management
+================
+
+None
+
+SMP/multi-core
+==============
+
+The driver uses completion to wake up the Debug Agent client threads.
+
+Security
+========
+
+From the perspective of the subsystem, the AP is untrusted. The remote
+stubs consult the secure debug fuses to determine whether or not the
+remote debugging will be enabled at the subsystem.
+
+If the hardware debug fuses indicate that debugging is disabled, the
+remote stubs will not be functional on the subsystem. Writes to the
+queue will only be done if the driver sees that the remote stub has been
+initialized on the subsystem.
+
+Therefore even if any untrusted software running on the AP requests
+the services of the Remote Debug Driver and inject RSP messages
+into the shared memory buffer, these RSP messages will be discarded and
+an appropriate error code will be sent up to the invoking application.
+
+Performance
+===========
+
+During operation, the Remote Debug Driver copies RSP messages
+asynchronously sent from the host debugger to the remote stub and vice
+versa. The debug messages are ASCII based and relatively short
+(<25 bytes) and may once in a while go up to a maximum 700 bytes
+depending on the command the user requested. Thus we do not
+anticipate any major performance impact. Moreover, in a typical
+functional debug scenario performance should not be a concern.
+
+Interface
+=========
+
+The Remote Debug Driver is a character based device that manages
+a piece of shared memory that is used as a bi-directional
+single producer/consumer circular queue using a next fit allocator.
+Every subsystem, has its own shared memory buffer that is managed
+like a separate device.
+
+The driver distinguishes each subsystem processor's buffer by
+registering a node with a different minor number.
+
+For each subsystem that is supported, the driver exposes a user space
+interface through the following node:
+    - /dev/rdbg-<subsystem>
+    Ex. /dev/rdbg-adsp (for the ADSP subsystem)
+
+The standard open(), close(), read() and write() API set is
+implemented.
+
+The open() syscall will fail if a subsystem is not present or supported
+by the driver or a shared memory buffer cannot be allocated for the
+AP - subsystem communication. It will also fail if the subsytem has
+not initialized the queue on its side. Here are the error codes returned
+in case a call to open() fails:
+ENODEV - memory was not yet allocated for the device
+EEXIST - device is already opened
+ENOMEM - SMEM allocation failed
+ECOMM - Subsytem queue is not yet setup
+ENOMEM - Failure to initialize SMQ
+
+read() is a blocking call that will return with the number of bytes written
+by the subsystem whenever the subsystem sends it some payload. Here are the
+error codes returned in case a call to read() fails:
+EINVAL - Invalid input
+ENODEV - Device has not been opened yet
+ERESTARTSYS - call to wait_for_completion_interruptible is interrupted
+ENODATA - call to smq_receive failed
+
+write() attempts to send user mode payload out to the subsystem. It can fail
+if the SMQ is full. The number of bytes written is returned back to the user.
+Here are the error codes returned in case a call to write() fails:
+EINVAL - Invalid input
+ECOMM - SMQ send failed
+
+In the close() syscall, the control information state of the SMQ is
+initialized to zero thereby preventing any further communication between
+the AP and the subsystem. Here is the error code returned in case
+a call to close() fails:
+ENODEV - device wasn't opened/initialized
+
+The Remote Debug driver uses SMP2P for bi-directional AP to subsystem
+notification. Notifications are sent to indicate that there are new
+debug messages available for processing. Each subsystem that is
+supported will need to add a device tree entry per the usage
+specification of SMP2P driver.
+
+In case the remote stub becomes non operational or the security configuration
+on the subsystem does not permit debugging, any messages put in the SMQ will
+not be responded to. It is the responsibility of the Debug Agent app and the
+host debugger application such as GDB to timeout and notify the user of the
+non availability of remote debugging.
+
+Driver parameters
+=================
+
+None
+
+Config options
+==============
+
+The driver is configured with a device tree entry to map an SMP2P entry
+to the device. The SMP2P entry name used is "rdbg". Please see
+kernel\Documentation\arm\msm\msm_smp2p.txt for information about the
+device tree entry required to configure SMP2P.
+
+The driver uses the SMEM allocation type SMEM_LC_DEBUGGER to allocate memory
+for the queue that is used to share data with the subsystems.
+
+Dependencies
+============
+
+The Debug Agent driver requires services of SMEM to
+allocate shared memory buffers.
+
+SMP2P is used as a bi-directional notification
+mechanism between the AP and a subsystem processor.
+
+User space utilities
+====================
+
+This driver is meant to be used in conjunction with the user mode
+Remote Debug Agent application.
+
+Other
+=====
+
+None
+
+Known issues
+============
+For targets with an external subsystem, we cannot use
+shared memory for communication and would have to use the prevailing
+transport mechanisms that exists between the AP and the external subsystem.
+
+This driver cannot be leveraged for such targets.
+
+To do
+=====
+
+None
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 1046c262b46b..8e3bff9c7fe9 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -606,13 +606,20 @@ config TILE_SROM
 source "drivers/char/xillybus/Kconfig"
 
 config MSM_ADSPRPC
-        tristate "Qualcomm ADSP RPC driver"
-        depends on MSM_SMD
-        help
-          Provides a communication mechanism that allows for clients to
-          make remote method invocations across processor boundary to
-          applications DSP processor. Say M if you want to enable this
-          module.
+	tristate "QTI ADSP RPC driver"
+	depends on MSM_SMD
+	help
+	  Provides a communication mechanism that allows for clients to
+	  make remote method invocations across processor boundary to
+	  applications DSP processor. Say M if you want to enable this
+	  module.
+
+config MSM_RDBG
+	tristate "QTI Remote debug driver"
+	help
+	  Implements a shared memory based transport mechanism that allows
+	  for a debugger running on a host PC to communicate with a remote
+	  stub running on peripheral subsystems such as the ADSP, MODEM etc.
 
 endmenu
 
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index e180562c725e..7b0bd5408324 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -65,3 +65,4 @@ obj-$(CONFIG_MSM_ADSPRPC)       += adsprpc.o
 ifdef CONFIG_COMPAT
 obj-$(CONFIG_MSM_ADSPRPC)       += adsprpc_compat.o
 endif
+obj-$(CONFIG_MSM_RDBG)		+= rdbg.o
diff --git a/drivers/char/rdbg.c b/drivers/char/rdbg.c
new file mode 100644
index 000000000000..0823ed78485e
--- /dev/null
+++ b/drivers/char/rdbg.c
@@ -0,0 +1,1165 @@
+/*
+ * Copyright (c) 2013-2016, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * 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/cdev.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/of_gpio.h>
+#include <soc/qcom/smsm.h>
+#include <linux/uaccess.h>
+#include <linux/interrupt.h>
+
+#define SMP2P_NUM_PROCS				8
+#define MAX_RETRIES				20
+
+#define SM_VERSION				1
+#define SM_BLOCKSIZE				128
+
+#define SMQ_MAGIC_INIT				0xFF00FF00
+#define SMQ_MAGIC_PRODUCER			(SMQ_MAGIC_INIT | 0x1)
+#define SMQ_MAGIC_CONSUMER			(SMQ_MAGIC_INIT | 0x2)
+
+enum SMQ_STATUS {
+	SMQ_SUCCESS    =  0,
+	SMQ_ENOMEMORY  = -1,
+	SMQ_EBADPARM   = -2,
+	SMQ_UNDERFLOW  = -3,
+	SMQ_OVERFLOW   = -4
+};
+
+enum smq_type {
+	PRODUCER = 1,
+	CONSUMER = 2,
+	INVALID  = 3
+};
+
+struct smq_block_map {
+	uint32_t index_read;
+	uint32_t num_blocks;
+	uint8_t *map;
+};
+
+struct smq_node {
+	uint16_t index_block;
+	uint16_t num_blocks;
+} __attribute__ ((__packed__));
+
+struct smq_hdr {
+	uint8_t producer_version;
+	uint8_t consumer_version;
+} __attribute__ ((__packed__));
+
+struct smq_out_state {
+	uint32_t init;
+	uint32_t index_check_queue_for_reset;
+	uint32_t index_sent_write;
+	uint32_t index_free_read;
+} __attribute__ ((__packed__));
+
+struct smq_out {
+	struct smq_out_state s;
+	struct smq_node sent[1];
+};
+
+struct smq_in_state {
+	uint32_t init;
+	uint32_t index_check_queue_for_reset_ack;
+	uint32_t index_sent_read;
+	uint32_t index_free_write;
+} __attribute__ ((__packed__));
+
+struct smq_in {
+	struct smq_in_state s;
+	struct smq_node free[1];
+};
+
+struct smq {
+	struct smq_hdr *hdr;
+	struct smq_out *out;
+	struct smq_in *in;
+	uint8_t *blocks;
+	uint32_t num_blocks;
+	struct mutex *lock;
+	uint32_t initialized;
+	struct smq_block_map block_map;
+	enum smq_type type;
+};
+
+struct gpio_info {
+	int gpio_base_id;
+	int irq_base_id;
+};
+
+struct rdbg_data {
+	struct device *device;
+	struct completion work;
+	struct gpio_info in;
+	struct gpio_info out;
+	bool   device_initialized;
+	int    gpio_out_offset;
+	bool   device_opened;
+	void   *smem_addr;
+	size_t smem_size;
+	struct smq    producer_smrb;
+	struct smq    consumer_smrb;
+	struct mutex  write_mutex;
+};
+
+struct rdbg_device {
+	struct cdev cdev;
+	struct class *class;
+	dev_t dev_no;
+	int num_devices;
+	struct rdbg_data *rdbg_data;
+};
+
+static struct rdbg_device g_rdbg_instance = {
+	{ {0} },
+	NULL,
+	0,
+	SMP2P_NUM_PROCS,
+	NULL
+};
+
+struct processor_specific_info {
+	char *name;
+	unsigned int smem_buffer_addr;
+	size_t smem_buffer_size;
+};
+
+static struct processor_specific_info proc_info[SMP2P_NUM_PROCS] = {
+		{0},	/*APPS*/
+		{"rdbg_modem", 0, 0},	/*MODEM*/
+		{"rdbg_adsp", SMEM_LC_DEBUGGER, 16*1024},	/*ADSP*/
+		{0},	/*SMP2P_RESERVED_PROC_1*/
+		{"rdbg_wcnss", 0, 0},		/*WCNSS*/
+		{0},	/*SMP2P_RESERVED_PROC_2*/
+		{0},	/*SMP2P_POWER_PROC*/
+		{0}		/*SMP2P_REMOTE_MOCK_PROC*/
+};
+
+static int smq_blockmap_get(struct smq_block_map *block_map,
+	uint32_t *block_index, uint32_t n)
+{
+	uint32_t start;
+	uint32_t mark = 0;
+	uint32_t found = 0;
+	uint32_t i = 0;
+
+	start = block_map->index_read;
+
+	if (n == 1) {
+		do {
+			if (!block_map->map[block_map->index_read]) {
+				*block_index = block_map->index_read;
+				block_map->map[block_map->index_read] = 1;
+				block_map->index_read++;
+				block_map->index_read %= block_map->num_blocks;
+				return SMQ_SUCCESS;
+			}
+			block_map->index_read++;
+		} while (start != (block_map->index_read %=
+			block_map->num_blocks));
+	} else {
+		mark = block_map->num_blocks;
+
+		do {
+			if (!block_map->map[block_map->index_read]) {
+				if (mark > block_map->index_read) {
+					mark = block_map->index_read;
+					start = block_map->index_read;
+					found = 0;
+				}
+
+				found++;
+				if (found == n) {
+					*block_index = mark;
+					for (i = 0; i < n; i++)
+						block_map->map[mark + i] =
+							(uint8_t)(n - i);
+					block_map->index_read += block_map->map
+						[block_map->index_read] - 1;
+					return SMQ_SUCCESS;
+				}
+			} else {
+				found = 0;
+				block_map->index_read += block_map->map
+					[block_map->index_read] - 1;
+				mark = block_map->num_blocks;
+			}
+			block_map->index_read++;
+		} while (start != (block_map->index_read %=
+			block_map->num_blocks));
+	}
+
+	return SMQ_ENOMEMORY;
+}
+
+static void smq_blockmap_put(struct smq_block_map *block_map, uint32_t i)
+{
+	uint32_t num_blocks = block_map->map[i];
+
+	while (num_blocks--) {
+		block_map->map[i] = 0;
+		i++;
+	}
+}
+
+static int smq_blockmap_reset(struct smq_block_map *block_map)
+{
+	if (!block_map->map)
+		return SMQ_ENOMEMORY;
+	memset(block_map->map, 0, block_map->num_blocks + 1);
+	block_map->index_read = 0;
+
+	return SMQ_SUCCESS;
+}
+
+static int smq_blockmap_ctor(struct smq_block_map *block_map,
+	uint32_t num_blocks)
+{
+	if (num_blocks <= 1)
+		return SMQ_ENOMEMORY;
+
+	block_map->map = kcalloc(num_blocks, sizeof(uint8_t), GFP_KERNEL);
+	if (!block_map->map)
+		return SMQ_ENOMEMORY;
+
+	block_map->num_blocks = num_blocks - 1;
+	smq_blockmap_reset(block_map);
+
+	return SMQ_SUCCESS;
+}
+
+static void smq_blockmap_dtor(struct smq_block_map *block_map)
+{
+	kfree(block_map->map);
+	block_map->map = NULL;
+}
+
+static int smq_free(struct smq *smq, void *data)
+{
+	struct smq_node node;
+	uint32_t index_block;
+	int err = SMQ_SUCCESS;
+
+	if (smq->lock)
+		mutex_lock(smq->lock);
+
+	if ((smq->hdr->producer_version != SM_VERSION) &&
+		(smq->out->s.init != SMQ_MAGIC_PRODUCER)) {
+		err = SMQ_UNDERFLOW;
+		goto bail;
+	}
+
+	index_block = ((uint8_t *)data - smq->blocks) / SM_BLOCKSIZE;
+	if (index_block >= smq->num_blocks) {
+		err = SMQ_EBADPARM;
+		goto bail;
+	}
+
+	node.index_block = (uint16_t)index_block;
+	node.num_blocks = 0;
+	*((struct smq_node *)(smq->in->free + smq->in->
+		s.index_free_write)) = node;
+
+	smq->in->s.index_free_write = (smq->in->s.index_free_write + 1)
+		% smq->num_blocks;
+
+bail:
+	if (smq->lock)
+		mutex_unlock(smq->lock);
+	return err;
+}
+
+static int smq_receive(struct smq *smq, void **pp, int *pnsize, int *pbmore)
+{
+	struct smq_node *node;
+	int err = SMQ_SUCCESS;
+	int more = 0;
+
+	if ((smq->hdr->producer_version != SM_VERSION) &&
+		(smq->out->s.init != SMQ_MAGIC_PRODUCER))
+		return SMQ_UNDERFLOW;
+
+	if (smq->in->s.index_sent_read == smq->out->s.index_sent_write) {
+		err = SMQ_UNDERFLOW;
+		goto bail;
+	}
+
+	node = (struct smq_node *)(smq->out->sent + smq->in->s.index_sent_read);
+	if (node->index_block >= smq->num_blocks) {
+		err = SMQ_EBADPARM;
+		goto bail;
+	}
+
+	smq->in->s.index_sent_read = (smq->in->s.index_sent_read + 1)
+		% smq->num_blocks;
+
+	*pp = smq->blocks + (node->index_block * SM_BLOCKSIZE);
+	*pnsize = SM_BLOCKSIZE * node->num_blocks;
+
+	/* Ensure that the reads and writes are updated in the memory
+	* when they are done and not cached. Also, ensure that the reads
+	* and writes are not reordered as they are shared between two cores.
+	*/
+	rmb();
+	if (smq->in->s.index_sent_read != smq->out->s.index_sent_write)
+		more = 1;
+
+bail:
+	*pbmore = more;
+	return err;
+}
+
+static int smq_alloc_send(struct smq *smq, const uint8_t *pcb, int nsize)
+{
+	void *pv = 0;
+	int num_blocks;
+	uint32_t index_block = 0;
+	int err = SMQ_SUCCESS;
+	struct smq_node *node = NULL;
+
+	mutex_lock(smq->lock);
+
+	if ((smq->in->s.init == SMQ_MAGIC_CONSUMER) &&
+	 (smq->hdr->consumer_version == SM_VERSION)) {
+		if (smq->out->s.index_check_queue_for_reset ==
+			smq->in->s.index_check_queue_for_reset_ack) {
+			while (smq->out->s.index_free_read !=
+				smq->in->s.index_free_write) {
+				node = (struct smq_node *)(
+					smq->in->free +
+					smq->out->s.index_free_read);
+				if (node->index_block >= smq->num_blocks) {
+					err = SMQ_EBADPARM;
+					goto bail;
+				}
+
+				smq->out->s.index_free_read =
+					(smq->out->s.index_free_read + 1)
+						% smq->num_blocks;
+
+				smq_blockmap_put(&smq->block_map,
+					node->index_block);
+				/* Ensure that the reads and writes are
+				* updated in the memory when they are done
+				* and not cached. Also, ensure that the reads
+				* and writes are not reordered as they are
+				* shared between two cores.
+				*/
+				rmb();
+			}
+		}
+	}
+
+	num_blocks = ALIGN(nsize, SM_BLOCKSIZE)/SM_BLOCKSIZE;
+	err = smq_blockmap_get(&smq->block_map, &index_block, num_blocks);
+	if (err != SMQ_SUCCESS)
+		goto bail;
+
+	pv = smq->blocks + (SM_BLOCKSIZE * index_block);
+
+	err = copy_from_user((void *)pv, (void *)pcb, nsize);
+	if (err != 0)
+		goto bail;
+
+	((struct smq_node *)(smq->out->sent +
+		smq->out->s.index_sent_write))->index_block
+			= (uint16_t)index_block;
+	((struct smq_node *)(smq->out->sent +
+		smq->out->s.index_sent_write))->num_blocks
+			= (uint16_t)num_blocks;
+
+	smq->out->s.index_sent_write = (smq->out->s.index_sent_write + 1)
+		% smq->num_blocks;
+
+bail:
+	if (err != SMQ_SUCCESS) {
+		if (pv)
+			smq_blockmap_put(&smq->block_map, index_block);
+	}
+	mutex_unlock(smq->lock);
+	return err;
+}
+
+static int smq_reset_producer_queue_internal(struct smq *smq,
+	uint32_t reset_num)
+{
+	int retval = 0;
+	uint32_t i;
+
+	if (smq->type != PRODUCER)
+		goto bail;
+
+	mutex_lock(smq->lock);
+	if (smq->out->s.index_check_queue_for_reset != reset_num) {
+		smq->out->s.index_check_queue_for_reset = reset_num;
+		for (i = 0; i < smq->num_blocks; i++)
+			(smq->out->sent + i)->index_block = 0xFFFF;
+
+		smq_blockmap_reset(&smq->block_map);
+		smq->out->s.index_sent_write = 0;
+		smq->out->s.index_free_read = 0;
+		retval = 1;
+	}
+	mutex_unlock(smq->lock);
+
+bail:
+	return retval;
+}
+
+static int smq_check_queue_reset(struct smq *p_cons, struct smq *p_prod)
+{
+	int retval = 0;
+	uint32_t reset_num, i;
+
+	if ((p_cons->type != CONSUMER) ||
+		(p_cons->out->s.init != SMQ_MAGIC_PRODUCER) ||
+		(p_cons->hdr->producer_version != SM_VERSION))
+		goto bail;
+
+	reset_num = p_cons->out->s.index_check_queue_for_reset;
+	if (p_cons->in->s.index_check_queue_for_reset_ack != reset_num) {
+		p_cons->in->s.index_check_queue_for_reset_ack = reset_num;
+		for (i = 0; i < p_cons->num_blocks; i++)
+			(p_cons->in->free + i)->index_block = 0xFFFF;
+
+		p_cons->in->s.index_sent_read = 0;
+		p_cons->in->s.index_free_write = 0;
+
+		retval = smq_reset_producer_queue_internal(p_prod, reset_num);
+	}
+
+bail:
+	return retval;
+}
+
+static int check_subsystem_debug_enabled(void *base_addr, int size)
+{
+	int num_blocks;
+	uint8_t *pb_orig;
+	uint8_t *pb;
+	struct smq smq;
+	int err = 0;
+
+	pb = pb_orig = (uint8_t *)base_addr;
+	pb += sizeof(struct smq_hdr);
+	pb = PTR_ALIGN(pb, 8);
+	size -= pb - (uint8_t *)pb_orig;
+	num_blocks = (int)((size - sizeof(struct smq_out_state) -
+		sizeof(struct smq_in_state))/(SM_BLOCKSIZE +
+		sizeof(struct smq_node) * 2));
+	if (num_blocks <= 0) {
+		err = SMQ_EBADPARM;
+		goto bail;
+	}
+
+	pb += num_blocks * SM_BLOCKSIZE;
+	smq.out = (struct smq_out *)pb;
+	pb += sizeof(struct smq_out_state) + (num_blocks *
+		sizeof(struct smq_node));
+	smq.in = (struct smq_in *)pb;
+
+	if (smq.in->s.init != SMQ_MAGIC_CONSUMER) {
+		pr_err("%s, smq in consumer not initialized", __func__);
+		err = -ECOMM;
+	}
+
+bail:
+	return err;
+}
+
+static void smq_dtor(struct smq *smq)
+{
+	if (smq->initialized == SMQ_MAGIC_INIT) {
+		switch (smq->type) {
+		case PRODUCER:
+			smq->out->s.init = 0;
+			smq_blockmap_dtor(&smq->block_map);
+			break;
+		case CONSUMER:
+			smq->in->s.init = 0;
+			break;
+		default:
+		case INVALID:
+			break;
+		}
+
+		smq->initialized = 0;
+	}
+}
+
+/*
+ * The shared memory is used as a circular ring buffer in each direction.
+ * Thus we have a bi-directional shared memory channel between the AP
+ * and a subsystem. We call this SMQ. Each memory channel contains a header,
+ * data and a control mechanism that is used to synchronize read and write
+ * of data between the AP and the remote subsystem.
+ *
+ * Overall SMQ memory view:
+ *
+ *    +------------------------------------------------+
+ *    | SMEM buffer                                    |
+ *    |-----------------------+------------------------|
+ *    |Producer: LA           | Producer: Remote       |
+ *    |Consumer: Remote       |           subsystem    |
+ *    |          subsystem    | Consumer: LA           |
+ *    |                       |                        |
+ *    |               Producer|                Consumer|
+ *    +-----------------------+------------------------+
+ *    |                       |
+ *    |                       |
+ *    |                       +--------------------------------------+
+ *    |                                                              |
+ *    |                                                              |
+ *    v                                                              v
+ *    +--------------------------------------------------------------+
+ *    |   Header  |       Data      |            Control             |
+ *    +-----------+---+---+---+-----+----+--+--+-----+---+--+--+-----+
+ *    |           | b | b | b |     | S  |n |n |     | S |n |n |     |
+ *    |  Producer | l | l | l |     | M  |o |o |     | M |o |o |     |
+ *    |    Ver    | o | o | o |     | Q  |d |d |     | Q |d |d |     |
+ *    |-----------| c | c | c | ... |    |e |e | ... |   |e |e | ... |
+ *    |           | k | k | k |     | O  |  |  |     | I |  |  |     |
+ *    |  Consumer |   |   |   |     | u  |0 |1 |     | n |0 |1 |     |
+ *    |    Ver    | 0 | 1 | 2 |     | t  |  |  |     |   |  |  |     |
+ *    +-----------+---+---+---+-----+----+--+--+-----+---+--+--+-----+
+ *                                       |           |
+ *                                       +           |
+ *                                                   |
+ *                          +------------------------+
+ *                          |
+ *                          v
+ *                        +----+----+----+----+
+ *                        | SMQ Nodes         |
+ *                        |----|----|----|----|
+ *                 Node # |  0 |  1 |  2 | ...|
+ *                        |----|----|----|----|
+ * Starting Block Index # |  0 |  3 |  8 | ...|
+ *                        |----|----|----|----|
+ *            # of blocks |  3 |  5 |  1 | ...|
+ *                        +----+----+----+----+
+ *
+ * Header: Contains version numbers for software compatibility to ensure
+ * that both producers and consumers on the AP and subsystems know how to
+ * read from and write to the queue.
+ * Both the producer and consumer versions are 1.
+ *     +---------+-------------------+
+ *     | Size    | Field             |
+ *     +---------+-------------------+
+ *     | 1 byte  | Producer Version  |
+ *     +---------+-------------------+
+ *     | 1 byte  | Consumer Version  |
+ *     +---------+-------------------+
+ *
+ * Data: The data portion contains multiple blocks [0..N] of a fixed size.
+ * The block size SM_BLOCKSIZE is fixed to 128 bytes for header version #1.
+ * Payload sent from the debug agent app is split (if necessary) and placed
+ * in these blocks. The first data block is placed at the next 8 byte aligned
+ * address after the header.
+ *
+ * The number of blocks for a given SMEM allocation is derived as follows:
+ *   Number of Blocks = ((Total Size - Alignment - Size of Header
+ *		- Size of SMQIn - Size of SMQOut)/(SM_BLOCKSIZE))
+ *
+ * The producer maintains a private block map of each of these blocks to
+ * determine which of these blocks in the queue is available and which are free.
+ *
+ * Control:
+ * The control portion contains a list of nodes [0..N] where N is number
+ * of available data blocks. Each node identifies the data
+ * block indexes that contain a particular debug message to be transferred,
+ * and the number of blocks it took to hold the contents of the message.
+ *
+ * Each node has the following structure:
+ *     +---------+-------------------+
+ *     | Size    | Field             |
+ *     +---------+-------------------+
+ *     | 2 bytes |Staring Block Index|
+ *     +---------+-------------------+
+ *     | 2 bytes |Number of Blocks   |
+ *     +---------+-------------------+
+ *
+ * The producer and the consumer update different parts of the control channel
+ * (SMQOut / SMQIn) respectively. Each of these control data structures contains
+ * information about the last node that was written / read, and the actual nodes
+ * that were written/read.
+ *
+ * SMQOut Structure (R/W by producer, R by consumer):
+ *     +---------+-------------------+
+ *     | Size    | Field             |
+ *     +---------+-------------------+
+ *     | 4 bytes | Magic Init Number |
+ *     +---------+-------------------+
+ *     | 4 bytes | Reset             |
+ *     +---------+-------------------+
+ *     | 4 bytes | Last Sent Index   |
+ *     +---------+-------------------+
+ *     | 4 bytes | Index Free Read   |
+ *     +---------+-------------------+
+ *
+ * SMQIn Structure (R/W by consumer, R by producer):
+ *     +---------+-------------------+
+ *     | Size    | Field             |
+ *     +---------+-------------------+
+ *     | 4 bytes | Magic Init Number |
+ *     +---------+-------------------+
+ *     | 4 bytes | Reset ACK         |
+ *     +---------+-------------------+
+ *     | 4 bytes | Last Read Index   |
+ *     +---------+-------------------+
+ *     | 4 bytes | Index Free Write  |
+ *     +---------+-------------------+
+ *
+ * Magic Init Number:
+ * Both SMQ Out and SMQ In initialize this field with a predefined magic
+ * number so as to make sure that both the consumer and producer blocks
+ * have fully initialized and have valid data in the shared memory control area.
+ *	Producer Magic #: 0xFF00FF01
+ *	Consumer Magic #: 0xFF00FF02
+ */
+static int smq_ctor(struct smq *smq, void *base_addr, int size,
+	enum smq_type type, struct mutex *lock_ptr)
+{
+	int num_blocks;
+	uint8_t *pb_orig;
+	uint8_t *pb;
+	uint32_t i;
+	int err;
+
+	if (smq->initialized == SMQ_MAGIC_INIT) {
+		err = SMQ_EBADPARM;
+		goto bail;
+	}
+
+	if (!base_addr || !size) {
+		err = SMQ_EBADPARM;
+		goto bail;
+	}
+
+	if (type == PRODUCER)
+		smq->lock = lock_ptr;
+
+	pb_orig = (uint8_t *)base_addr;
+	smq->hdr = (struct smq_hdr *)pb_orig;
+	pb = pb_orig;
+	pb += sizeof(struct smq_hdr);
+	pb = PTR_ALIGN(pb, 8);
+	size -= pb - (uint8_t *)pb_orig;
+	num_blocks = (int)((size - sizeof(struct smq_out_state) -
+		sizeof(struct smq_in_state))/(SM_BLOCKSIZE +
+		sizeof(struct smq_node) * 2));
+	if (num_blocks <= 0) {
+		err = SMQ_ENOMEMORY;
+		goto bail;
+	}
+
+	smq->blocks = pb;
+	smq->num_blocks = num_blocks;
+	pb += num_blocks * SM_BLOCKSIZE;
+	smq->out = (struct smq_out *)pb;
+	pb += sizeof(struct smq_out_state) + (num_blocks *
+		sizeof(struct smq_node));
+	smq->in = (struct smq_in *)pb;
+	smq->type = type;
+	if (type == PRODUCER) {
+		smq->hdr->producer_version = SM_VERSION;
+		for (i = 0; i < smq->num_blocks; i++)
+			(smq->out->sent + i)->index_block = 0xFFFF;
+
+		err = smq_blockmap_ctor(&smq->block_map, smq->num_blocks);
+		if (err != SMQ_SUCCESS)
+			goto bail;
+
+		smq->out->s.index_sent_write = 0;
+		smq->out->s.index_free_read = 0;
+		if (smq->out->s.init == SMQ_MAGIC_PRODUCER) {
+			smq->out->s.index_check_queue_for_reset += 1;
+		} else {
+			smq->out->s.index_check_queue_for_reset = 1;
+			smq->out->s.init = SMQ_MAGIC_PRODUCER;
+		}
+	} else {
+		smq->hdr->consumer_version = SM_VERSION;
+		for (i = 0; i < smq->num_blocks; i++)
+			(smq->in->free + i)->index_block = 0xFFFF;
+
+		smq->in->s.index_sent_read = 0;
+		smq->in->s.index_free_write = 0;
+		if (smq->out->s.init == SMQ_MAGIC_PRODUCER) {
+			smq->in->s.index_check_queue_for_reset_ack =
+				smq->out->s.index_check_queue_for_reset;
+		} else {
+			smq->in->s.index_check_queue_for_reset_ack = 0;
+		}
+
+		smq->in->s.init = SMQ_MAGIC_CONSUMER;
+	}
+	smq->initialized = SMQ_MAGIC_INIT;
+	err = SMQ_SUCCESS;
+
+bail:
+	return err;
+}
+
+static void send_interrupt_to_subsystem(struct rdbg_data *rdbgdata)
+{
+	int offset = rdbgdata->gpio_out_offset;
+	int val = 1 ^ gpio_get_value(rdbgdata->out.gpio_base_id + offset);
+
+	gpio_set_value(rdbgdata->out.gpio_base_id + offset, val);
+	rdbgdata->gpio_out_offset = (offset + 1) % 32;
+
+	dev_dbg(rdbgdata->device, "%s: sent interrupt %d to subsystem",
+		__func__, val);
+}
+
+static irqreturn_t on_interrupt_from(int irq, void *ptr)
+{
+	struct rdbg_data *rdbgdata = (struct rdbg_data *) ptr;
+
+	dev_dbg(rdbgdata->device, "%s: Received interrupt %d from subsystem",
+		__func__, irq);
+
+	complete(&(rdbgdata->work));
+	return IRQ_HANDLED;
+}
+
+static int initialize_smq(struct rdbg_data *rdbgdata)
+{
+	int err = 0;
+	unsigned char *smem_consumer_buffer = rdbgdata->smem_addr;
+
+	smem_consumer_buffer += (rdbgdata->smem_size/2);
+
+	if (smq_ctor(&(rdbgdata->producer_smrb), (void *)(rdbgdata->smem_addr),
+		((rdbgdata->smem_size)/2), PRODUCER, &rdbgdata->write_mutex)) {
+		dev_err(rdbgdata->device, "%s: smq producer allocation failed",
+			__func__);
+		err = -ENOMEM;
+		goto bail;
+	}
+
+	if (smq_ctor(&(rdbgdata->consumer_smrb), (void *)smem_consumer_buffer,
+		((rdbgdata->smem_size)/2), CONSUMER, NULL)) {
+		dev_err(rdbgdata->device, "%s: smq conmsumer allocation failed",
+			__func__);
+		err = -ENOMEM;
+	}
+
+bail:
+	return err;
+
+}
+
+static int rdbg_open(struct inode *inode, struct file *filp)
+{
+	int device_id = -1;
+	struct rdbg_device *device = &g_rdbg_instance;
+	struct rdbg_data *rdbgdata = NULL;
+	int err = 0;
+
+	if (!inode || !device->rdbg_data) {
+		pr_err("Memory not allocated yet");
+		err = -ENODEV;
+		goto bail;
+	}
+
+	device_id = MINOR(inode->i_rdev);
+	rdbgdata = &device->rdbg_data[device_id];
+
+	if (rdbgdata->device_opened) {
+		dev_err(rdbgdata->device, "%s: Device already opened",
+			__func__);
+		err = -EEXIST;
+		goto bail;
+	}
+
+	rdbgdata->smem_size = proc_info[device_id].smem_buffer_size;
+	if (!rdbgdata->smem_size) {
+		dev_err(rdbgdata->device, "%s: smem not initialized", __func__);
+		err = -ENOMEM;
+		goto bail;
+	}
+
+	rdbgdata->smem_addr = smem_find(proc_info[device_id].smem_buffer_addr,
+		rdbgdata->smem_size, 0, SMEM_ANY_HOST_FLAG);
+	if (!rdbgdata->smem_addr) {
+		dev_err(rdbgdata->device, "%s: Could not allocate smem memory",
+			__func__);
+		err = -ENOMEM;
+		goto bail;
+	}
+	dev_dbg(rdbgdata->device, "%s: SMEM address=0x%lx smem_size=%d",
+		__func__, (unsigned long)rdbgdata->smem_addr,
+		(unsigned int)rdbgdata->smem_size);
+
+	if (check_subsystem_debug_enabled(rdbgdata->smem_addr,
+		rdbgdata->smem_size/2)) {
+		dev_err(rdbgdata->device, "%s: Subsystem %s is not debug enabled",
+			__func__, proc_info[device_id].name);
+		err = -ECOMM;
+		goto bail;
+	}
+
+	init_completion(&rdbgdata->work);
+
+	err = request_irq(rdbgdata->in.irq_base_id, on_interrupt_from,
+			IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			proc_info[device_id].name,
+			(void *)&device->rdbg_data[device_id]);
+	if (err) {
+		dev_err(rdbgdata->device,
+			"%s: Failed to register interrupt.Err=%d,irqid=%d.",
+			__func__, err, rdbgdata->in.irq_base_id);
+		goto irq_bail;
+	}
+
+	err = enable_irq_wake(rdbgdata->in.irq_base_id);
+	if (err < 0) {
+		dev_dbg(rdbgdata->device, "enable_irq_wake() failed with err=%d",
+			err);
+		err = 0;
+	}
+
+	mutex_init(&rdbgdata->write_mutex);
+
+	err = initialize_smq(rdbgdata);
+	if (err) {
+		dev_err(rdbgdata->device, "Error initializing smq. Err=%d",
+			err);
+		goto smq_bail;
+	}
+
+	rdbgdata->device_opened = 1;
+
+	filp->private_data = (void *)rdbgdata;
+
+	return 0;
+
+smq_bail:
+	smq_dtor(&(rdbgdata->producer_smrb));
+	smq_dtor(&(rdbgdata->consumer_smrb));
+	mutex_destroy(&rdbgdata->write_mutex);
+irq_bail:
+	free_irq(rdbgdata->in.irq_base_id, (void *)
+		&device->rdbg_data[device_id]);
+bail:
+	return err;
+}
+
+static int rdbg_release(struct inode *inode, struct file *filp)
+{
+	int device_id = -1;
+	struct rdbg_device *rdbgdevice = &g_rdbg_instance;
+	struct rdbg_data *rdbgdata = NULL;
+	int err = 0;
+
+	if (!inode || !rdbgdevice->rdbg_data) {
+		pr_err("Memory not allocated yet");
+		err = -ENODEV;
+		goto bail;
+	}
+
+	device_id = MINOR(inode->i_rdev);
+	rdbgdata = &rdbgdevice->rdbg_data[device_id];
+
+	if (rdbgdata->device_opened == 1) {
+		dev_dbg(rdbgdata->device, "%s: Destroying %s.", __func__,
+			proc_info[device_id].name);
+		rdbgdata->device_opened = 0;
+		complete(&(rdbgdata->work));
+		free_irq(rdbgdata->in.irq_base_id, (void *)
+			&rdbgdevice->rdbg_data[device_id]);
+		if (rdbgdevice->rdbg_data[device_id].producer_smrb.initialized)
+			smq_dtor(&(rdbgdevice->rdbg_data[device_id].
+				producer_smrb));
+		if (rdbgdevice->rdbg_data[device_id].consumer_smrb.initialized)
+			smq_dtor(&(rdbgdevice->rdbg_data[device_id].
+				consumer_smrb));
+		mutex_destroy(&rdbgdata->write_mutex);
+	}
+
+	filp->private_data = NULL;
+
+bail:
+	return err;
+}
+
+static ssize_t rdbg_read(struct file *filp, char __user *buf, size_t size,
+	loff_t *offset)
+{
+	int err = 0;
+	struct rdbg_data *rdbgdata = filp->private_data;
+	void *p_sent_buffer = NULL;
+	int nsize = 0;
+	int more = 0;
+
+	if (!rdbgdata) {
+		pr_err("Invalid argument");
+		err = -EINVAL;
+		goto bail;
+	}
+
+	dev_dbg(rdbgdata->device, "%s: In receive", __func__);
+	err = wait_for_completion_interruptible(&(rdbgdata->work));
+	if (err) {
+		dev_err(rdbgdata->device, "%s: Error in wait", __func__);
+		goto bail;
+	}
+
+	smq_check_queue_reset(&(rdbgdata->consumer_smrb),
+		&(rdbgdata->producer_smrb));
+	if (smq_receive(&(rdbgdata->consumer_smrb), &p_sent_buffer,
+			&nsize, &more) != SMQ_SUCCESS) {
+		dev_err(rdbgdata->device, "%s: Error in smq_recv(). Err code = %d",
+			__func__, err);
+		err = -ENODATA;
+		goto bail;
+	}
+
+	size = ((size < nsize) ? size : nsize);
+	err = copy_to_user(buf, p_sent_buffer, size);
+	if (err != 0) {
+		dev_err(rdbgdata->device, "%s: Error in copy_to_user(). Err code = %d",
+			__func__, err);
+		err = -ENODATA;
+		goto bail;
+	}
+
+	smq_free(&(rdbgdata->consumer_smrb), p_sent_buffer);
+	err = size;
+	dev_dbg(rdbgdata->device, "%s: Read data to buffer with address 0x%lx",
+		__func__, (unsigned long) buf);
+
+bail:
+	return err;
+}
+
+static ssize_t rdbg_write(struct file *filp, const char __user *buf,
+	size_t size, loff_t *offset)
+{
+	int err = 0;
+	int num_retries = 0;
+	struct rdbg_data *rdbgdata = filp->private_data;
+
+	if (!rdbgdata) {
+		pr_err("Invalid argument");
+		err = -EINVAL;
+		goto bail;
+	}
+
+	do {
+		err = smq_alloc_send(&(rdbgdata->producer_smrb), buf, size);
+		dev_dbg(rdbgdata->device, "%s, smq_alloc_send returned %d.",
+			__func__, err);
+	} while (err != 0 && num_retries++ < MAX_RETRIES);
+
+	if (err != 0) {
+		err = -ECOMM;
+		goto bail;
+	}
+
+	send_interrupt_to_subsystem(rdbgdata);
+
+	err = size;
+
+bail:
+	return err;
+}
+
+
+static const struct file_operations rdbg_fops = {
+	.open = rdbg_open,
+	.read =  rdbg_read,
+	.write =  rdbg_write,
+	.release = rdbg_release,
+};
+
+static int register_smp2p(char *node_name, struct gpio_info *gpio_info_ptr)
+{
+	struct device_node *node = NULL;
+	int cnt = 0;
+	int id = 0;
+
+	node = of_find_compatible_node(NULL, NULL, node_name);
+	if (node) {
+		cnt = of_gpio_count(node);
+		if (cnt && gpio_info_ptr) {
+			id = of_get_gpio(node, 0);
+			gpio_info_ptr->gpio_base_id = id;
+			gpio_info_ptr->irq_base_id = gpio_to_irq(id);
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int __init rdbg_init(void)
+{
+	int err = 0;
+	struct rdbg_device *rdbgdevice = &g_rdbg_instance;
+	int minor = 0;
+	int major = 0;
+	int minor_nodes_created = 0;
+
+	char *rdbg_compatible_string = "qcom,smp2pgpio_client_rdbg_";
+	int max_len = strlen(rdbg_compatible_string) + strlen("xx_out");
+
+	char *node_name = kcalloc(max_len, sizeof(char), GFP_KERNEL);
+
+	if (!node_name) {
+		err = -ENOMEM;
+		goto bail;
+	}
+
+	if (rdbgdevice->num_devices < 1 ||
+		rdbgdevice->num_devices > SMP2P_NUM_PROCS) {
+		pr_err("rgdb: invalid num_devices");
+		err = -EDOM;
+		goto name_bail;
+	}
+
+	rdbgdevice->rdbg_data = kcalloc(rdbgdevice->num_devices,
+		sizeof(struct rdbg_data), GFP_KERNEL);
+	if (!rdbgdevice->rdbg_data) {
+		err = -ENOMEM;
+		goto name_bail;
+	}
+
+	err = alloc_chrdev_region(&rdbgdevice->dev_no, 0,
+		rdbgdevice->num_devices, "rdbgctl");
+	if (err) {
+		pr_err("Error in alloc_chrdev_region.");
+		goto data_bail;
+	}
+	major = MAJOR(rdbgdevice->dev_no);
+
+	cdev_init(&rdbgdevice->cdev, &rdbg_fops);
+	rdbgdevice->cdev.owner = THIS_MODULE;
+	err = cdev_add(&rdbgdevice->cdev, MKDEV(major, 0),
+		rdbgdevice->num_devices);
+	if (err) {
+		pr_err("Error in cdev_add");
+		goto chrdev_bail;
+	}
+
+	rdbgdevice->class = class_create(THIS_MODULE, "rdbg");
+	if (IS_ERR(rdbgdevice->class)) {
+		err = PTR_ERR(rdbgdevice->class);
+		pr_err("Error in class_create");
+		goto cdev_bail;
+	}
+
+	for (minor = 0; minor < rdbgdevice->num_devices; minor++) {
+		if (!proc_info[minor].name)
+			continue;
+
+		if (snprintf(node_name, max_len, "%s%d_in",
+			rdbg_compatible_string, minor) <= 0) {
+			pr_err("Error in snprintf");
+			err = -ENOMEM;
+			goto device_bail;
+		}
+
+		if (register_smp2p(node_name,
+			&rdbgdevice->rdbg_data[minor].in)) {
+			pr_debug("No incoming device tree entry found for %s",
+				proc_info[minor].name);
+			continue;
+		}
+
+		if (snprintf(node_name, max_len, "%s%d_out",
+			rdbg_compatible_string, minor) <= 0) {
+			pr_err("Error in snprintf");
+			err = -ENOMEM;
+			goto device_bail;
+		}
+
+		if (register_smp2p(node_name,
+			&rdbgdevice->rdbg_data[minor].out)) {
+			pr_err("No outgoing device tree entry found for %s",
+				proc_info[minor].name);
+			err = -EINVAL;
+			goto device_bail;
+		}
+
+		rdbgdevice->rdbg_data[minor].device = device_create(
+			rdbgdevice->class, NULL, MKDEV(major, minor),
+			NULL, "%s", proc_info[minor].name);
+		if (IS_ERR(rdbgdevice->rdbg_data[minor].device)) {
+			err = PTR_ERR(rdbgdevice->rdbg_data[minor].device);
+			pr_err("Error in device_create");
+			goto device_bail;
+		}
+		rdbgdevice->rdbg_data[minor].device_initialized = 1;
+		minor_nodes_created++;
+		dev_dbg(rdbgdevice->rdbg_data[minor].device,
+			"%s: created /dev/%s c %d %d'", __func__,
+			proc_info[minor].name, major, minor);
+	}
+
+	if (!minor_nodes_created) {
+		pr_err("No device tree entries found");
+		err = -EINVAL;
+		goto class_bail;
+	}
+
+	goto name_bail;
+
+device_bail:
+	for (--minor; minor >= 0; minor--) {
+		if (rdbgdevice->rdbg_data[minor].device_initialized)
+			device_destroy(rdbgdevice->class,
+				MKDEV(MAJOR(rdbgdevice->dev_no), minor));
+	}
+class_bail:
+	class_destroy(rdbgdevice->class);
+cdev_bail:
+	cdev_del(&rdbgdevice->cdev);
+chrdev_bail:
+	unregister_chrdev_region(rdbgdevice->dev_no, rdbgdevice->num_devices);
+data_bail:
+	kfree(rdbgdevice->rdbg_data);
+name_bail:
+	kfree(node_name);
+bail:
+	return err;
+}
+
+static void __exit rdbg_exit(void)
+{
+	struct rdbg_device *rdbgdevice = &g_rdbg_instance;
+	int minor;
+
+	for (minor = 0; minor < rdbgdevice->num_devices; minor++) {
+		if (rdbgdevice->rdbg_data[minor].device_initialized) {
+			device_destroy(rdbgdevice->class,
+				MKDEV(MAJOR(rdbgdevice->dev_no), minor));
+		}
+	}
+	class_destroy(rdbgdevice->class);
+	cdev_del(&rdbgdevice->cdev);
+	unregister_chrdev_region(rdbgdevice->dev_no, 1);
+	kfree(rdbgdevice->rdbg_data);
+}
+
+module_init(rdbg_init);
+module_exit(rdbg_exit);
+
+MODULE_DESCRIPTION("rdbg module");
+MODULE_LICENSE("GPL v2");