From 207b7ee89cebfe4a9779014a57de2417d4d2b3f2 Mon Sep 17 00:00:00 2001 From: Sathish Ambley Date: Mon, 6 Jul 2015 14:17:40 -0700 Subject: [PATCH] msm: rdbg: Add snapshot of remote debugger driver This snapshot was taken as of msm-3.10 commit ("Merge "msm: msm_bus: Provide valid input pointer argument") Updated source and documentation to fix checkpatch warnings. Change-Id: I6e84fc016a7bea69bb4d5d1991d8b5500244307d Acked-by: Vivek Iyer Acked-by: Mahesh Rohera Acked-by: Jay Jayanna Signed-off-by: Sathish Ambley --- Documentation/arm/msm/remote_debug_drv.txt | 468 ++++++++ drivers/char/Kconfig | 21 +- drivers/char/Makefile | 1 + drivers/char/rdbg.c | 1165 ++++++++++++++++++++ 4 files changed, 1648 insertions(+), 7 deletions(-) create mode 100644 Documentation/arm/msm/remote_debug_drv.txt create mode 100644 drivers/char/rdbg.c 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- + 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#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");