drivers/char/diag/diag_pcie.c - manifest_repos/kernel - Git at Google

 /* Copyright (c) 2018-2019, 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/slab.h>
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/err.h>
 #include <linux/sched.h>
 #include <linux/ratelimit.h>
 #include <linux/workqueue.h>
 #include <linux/diagchar.h>
 #include <linux/delay.h>
 #include <linux/kmemleak.h>
 #include <linux/list.h>
 #include "diag_pcie.h"
 #include "diag_mux.h"
 #include "diagmem.h"
 #include "diag_ipc_logging.h"
 #define DIAG_LEGACY "DIAG_PCIE"

 struct diag_pcie_info diag_pcie[NUM_DIAG_PCIE_DEV] = {
 	{
 		.id = DIAG_PCIE_LOCAL,
 		.name = DIAG_LEGACY,
 		.enabled = {0},
 		.mempool = POOL_TYPE_MUX_APPS,
 		.ops = NULL,
 		.wq = NULL,
 		.read_cnt = 0,
 		.write_cnt = 0,
 		.in_chan_attr = {
 			.max_pkt_size = DIAG_MAX_PKT_SZ,
 			.nr_trbs = 1,
 			.read_buffer = NULL,
 		},
 		.out_chan_attr = {
 			.max_pkt_size = DIAG_MAX_PCIE_PKT_SZ,
 		},
 		.in_chan = MHI_CLIENT_DIAG_OUT,
 		.out_chan = MHI_CLIENT_DIAG_IN,
 	}
 };

 static void diag_pcie_event_notifier(struct mhi_dev_client_cb_reason *reason)
 {
 	int i;
 	struct diag_pcie_info *pcie_info = NULL;

 	for (i = 0; i < NUM_DIAG_PCIE_DEV; i++) {
 		pcie_info = &diag_pcie[i];
 		if (reason->reason == MHI_DEV_TRE_AVAILABLE)
 			if (reason->ch_id == pcie_info->in_chan) {
 				queue_work(pcie_info->wq,
 					&pcie_info->read_work);
 				break;
 			}
 	}
 }

 void diag_pcie_read_work_fn(struct work_struct *work)
 {
 	struct mhi_req ureq;
 	struct diag_pcie_info *pcie_info = container_of(work,
 						      struct diag_pcie_info,
 						      read_work);
 	unsigned int bytes_avail = 0;

 	if (!pcie_info || !atomic_read(&pcie_info->enabled) ||
 		!atomic_read(&pcie_info->diag_state))
 		return;

 	ureq.chan = pcie_info->in_chan;
 	ureq.client = pcie_info->in_handle;
 	ureq.mode = DMA_SYNC;
 	ureq.buf = pcie_info->in_chan_attr.read_buffer;
 	ureq.len = pcie_info->in_chan_attr.read_buffer_size;
 	ureq.transfer_len = 0;
 	bytes_avail = mhi_dev_read_channel(&ureq);
 	if (bytes_avail < 0)
 		return;
 	DIAG_LOG(DIAG_DEBUG_MUX, "read total bytes %d from chan:%d",
 		bytes_avail, pcie_info->in_chan);
 	pcie_info->read_cnt++;

 	if (pcie_info->ops && pcie_info->ops->read_done)
 		pcie_info->ops->read_done(pcie_info->in_chan_attr.read_buffer,
 					ureq.transfer_len, pcie_info->ctxt);

 }

 static void diag_pcie_buf_tbl_remove(struct diag_pcie_info *pcie_info,
 				    unsigned char *buf)
 {
 	struct diag_pcie_buf_tbl_t *temp = NULL;
 	struct diag_pcie_buf_tbl_t *entry = NULL;

 	list_for_each_entry_safe(entry, temp, &pcie_info->buf_tbl, track) {
 		if (entry->buf == buf) {
 			DIAG_LOG(DIAG_DEBUG_MUX, "ref_count-- for %pK\n", buf);
 			atomic_dec(&entry->ref_count);
 			/*
 			 * Remove reference from the table if it is the
 			 * only instance of the buffer
 			 */
 			if (atomic_read(&entry->ref_count) == 0) {
 				list_del(&entry->track);
 				kfree(entry);
 				entry = NULL;
 			}
 			break;
 		}
 	}
 }

 static struct diag_pcie_buf_tbl_t *diag_pcie_buf_tbl_get(
 				struct diag_pcie_info *pcie_info,
 				unsigned char *buf)
 {
 	struct diag_pcie_buf_tbl_t *temp = NULL;
 	struct diag_pcie_buf_tbl_t *entry = NULL;

 	list_for_each_entry_safe(entry, temp, &pcie_info->buf_tbl, track) {
 		if (entry->buf == buf) {
 			DIAG_LOG(DIAG_DEBUG_MUX, "ref_count-- for %pK\n", buf);
 			atomic_dec(&entry->ref_count);
 			return entry;
 		}
 	}

 	return NULL;
 }

 void diag_pcie_write_complete_cb(void *req)
 {
 	struct diag_pcie_context *ctxt = NULL;
 	struct diag_pcie_info *ch;
 	struct diag_pcie_buf_tbl_t *entry = NULL;
 	struct mhi_req *ureq = req;
 	unsigned long flags;

 	if (!ureq)
 		return;
 	ctxt = (struct diag_pcie_context *)ureq->context;
 	if (!ctxt)
 		return;
 	ch = ctxt->ch;
 	if (!ch)
 		return;
 	spin_lock_irqsave(&ch->write_lock, flags);
 	ch->write_cnt++;
 	entry = diag_pcie_buf_tbl_get(ch, ctxt->buf);
 	if (!entry) {
 		pr_err_ratelimited("diag: In %s, unable to find entry %pK in the table\n",
 				   __func__, ctxt->buf);
 		spin_unlock_irqrestore(&ch->write_lock, flags);
 		return;
 	}
 	if (atomic_read(&entry->ref_count) != 0) {
 		DIAG_LOG(DIAG_DEBUG_MUX, "partial write_done ref %d\n",
 			 atomic_read(&entry->ref_count));
 		diag_ws_on_copy_complete(DIAG_WS_MUX);
 		spin_unlock_irqrestore(&ch->write_lock, flags);
 		diagmem_free(driver, req, ch->mempool);
 		kfree(ctxt);
 		ctxt = NULL;
 		return;
 	}
 	DIAG_LOG(DIAG_DEBUG_MUX, "full write_done, ctxt: %pK\n",
 		 ctxt->buf);
 	list_del(&entry->track);
 	kfree(entry);
 	entry = NULL;
 	if (ch->ops && ch->ops->write_done)
 		ch->ops->write_done(ureq->buf, ureq->len,
 				ctxt->buf_ctxt, DIAG_PCIE_MODE);
 	spin_unlock_irqrestore(&ch->write_lock, flags);
 	diagmem_free(driver, req, ch->mempool);
 	kfree(ctxt);
 	ctxt = NULL;
 }

 static int diag_pcie_buf_tbl_add(struct diag_pcie_info *pcie_info,
 				unsigned char *buf, uint32_t len, int ctxt)
 {
 	struct diag_pcie_buf_tbl_t *temp = NULL;
 	struct diag_pcie_buf_tbl_t *entry = NULL;

 	list_for_each_entry_safe(entry, temp, &pcie_info->buf_tbl, track) {
 		if (entry->buf == buf) {
 			atomic_inc(&entry->ref_count);
 			return 0;
 		}
 	}

 	/* New buffer, not found in the list */
 	entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
 	if (!entry)
 		return -ENOMEM;

 	entry->buf = buf;
 	entry->ctxt = ctxt;
 	entry->len = len;
 	atomic_set(&entry->ref_count, 1);
 	INIT_LIST_HEAD(&entry->track);
 	list_add_tail(&entry->track, &pcie_info->buf_tbl);

 	return 0;
 }

 static int diag_pcie_write_ext(struct diag_pcie_info *pcie_info,
 			      unsigned char *buf, int len, int ctxt)
 {
 	int write_len = 0;
 	int bytes_remaining = len;
 	int offset = 0;
 	struct mhi_req *req;
 	struct diag_pcie_context *context;
 	int bytes_to_write;
 	unsigned long flags;

 	if (!pcie_info || !buf || len <= 0) {
 		pr_err_ratelimited("diag: In %s, pcie_info: %pK buf: %pK, len: %d\n",
 				   __func__, pcie_info, buf, len);
 		return -EINVAL;
 	}

 	while (bytes_remaining > 0) {
 		req = diagmem_alloc(driver, sizeof(struct mhi_req),
 				    pcie_info->mempool);
 		if (!req) {
 			pr_err_ratelimited("diag: In %s, cannot retrieve pcie write ptrs for pcie channel %s\n",
 					   __func__, pcie_info->name);
 			return -ENOMEM;
 		}

 		write_len = (bytes_remaining >
 				pcie_info->out_chan_attr.max_pkt_size) ?
 				pcie_info->out_chan_attr.max_pkt_size :
 				bytes_remaining;
 		req->client = pcie_info->out_handle;
 		context = kzalloc(sizeof(*context), GFP_KERNEL);
 		if (!context)
 			return -ENOMEM;

 		context->ch = pcie_info;
 		context->buf_ctxt = ctxt;
 		context->buf = buf;
 		req->context = context;
 		req->buf = buf + offset;
 		req->len = write_len;
 		req->chan = pcie_info->out_chan;
 		req->mode = DMA_ASYNC;
 		req->client_cb = diag_pcie_write_complete_cb;
 		req->snd_cmpl = 1;
 		if (!pcie_info->out_handle ||
 			!atomic_read(&pcie_info->enabled) ||
 			!atomic_read(&pcie_info->diag_state)) {
 			pr_debug_ratelimited("diag: pcie ch %s is not opened\n",
 					     pcie_info->name);
 			kfree(req->context);
 			diagmem_free(driver, req, pcie_info->mempool);
 			return -ENODEV;
 		}
 		spin_lock_irqsave(&pcie_info->write_lock, flags);
 		if (diag_pcie_buf_tbl_add(pcie_info, buf, len, ctxt)) {
 			kfree(req->context);
 			diagmem_free(driver, req, pcie_info->mempool);
 			spin_unlock_irqrestore(&pcie_info->write_lock, flags);
 			return -ENOMEM;
 		}
 		spin_unlock_irqrestore(&pcie_info->write_lock, flags);
 		diag_ws_on_read(DIAG_WS_MUX, len);
 		bytes_to_write = mhi_dev_write_channel(req);
 		diag_ws_on_copy(DIAG_WS_MUX);
 		if (bytes_to_write != write_len) {
 			pr_err_ratelimited("diag: In %s, error writing to pcie channel %s, err: %d, write_len: %d\n",
 					   __func__, pcie_info->name,
 					bytes_to_write, write_len);
 			DIAG_LOG(DIAG_DEBUG_MUX,
 				 "ERR! unable to write to pcie, err: %d, write_len: %d\n",
 				bytes_to_write, write_len);
 			diag_ws_on_copy_fail(DIAG_WS_MUX);
 			spin_lock_irqsave(&pcie_info->write_lock, flags);
 			diag_pcie_buf_tbl_remove(pcie_info, buf);
 			kfree(req->context);
 			diagmem_free(driver, req, pcie_info->mempool);
 			spin_unlock_irqrestore(&pcie_info->write_lock, flags);
 			return -EINVAL;
 		}
 		offset += write_len;
 		bytes_remaining -= write_len;
 		DIAG_LOG(DIAG_DEBUG_MUX,
 			 "bytes_remaining: %d write_len: %d, len: %d\n",
 			 bytes_remaining, write_len, len);
 	}
 	DIAG_LOG(DIAG_DEBUG_MUX, "done writing!");

 	return 0;
 }

 int diag_pcie_write(int id, unsigned char *buf, int len, int ctxt)
 {
 	struct mhi_req *req;
 	struct diag_pcie_context *context;
 	int bytes_to_write;
 	struct diag_pcie_info *pcie_info;
 	unsigned long flags;

 	pcie_info = &diag_pcie[id];

 	if (len > pcie_info->out_chan_attr.max_pkt_size) {
 		DIAG_LOG(DIAG_DEBUG_MUX, "len: %d, max_size: %zu\n",
 			 len, pcie_info->out_chan_attr.max_pkt_size);
 		return diag_pcie_write_ext(pcie_info, buf, len, ctxt);
 	}
 	req = (struct mhi_req *)diagmem_alloc(driver, sizeof(struct mhi_req),
 		    pcie_info->mempool);
 	if (!req) {
 		pr_err_ratelimited("diag: In %s, cannot retrieve pcie write ptrs for pcie channel %s\n",
 				 __func__, pcie_info->name);
 		return -ENOMEM;
 	}
 	req->client = pcie_info->out_handle;
 	context = kzalloc(sizeof(struct diag_pcie_context), GFP_KERNEL);
 	if (!context)
 		return -ENOMEM;

 	context->ch = &diag_pcie[id];
 	context->buf_ctxt = ctxt;
 	context->buf = buf;
 	req->context = context;
 	req->buf = buf;
 	req->len = len;
 	req->chan = pcie_info->out_chan;
 	req->mode = DMA_ASYNC;
 	req->client_cb = diag_pcie_write_complete_cb;
 	req->snd_cmpl = 1;
 	if (!pcie_info->out_handle || !atomic_read(&pcie_info->enabled) ||
 		!atomic_read(&pcie_info->diag_state)) {
 		pr_debug_ratelimited("diag: pcie ch %s is not opened\n",
 					pcie_info->name);
 		kfree(req->context);
 		diagmem_free(driver, req, pcie_info->mempool);
 		return -ENODEV;
 	}
 	spin_lock_irqsave(&pcie_info->write_lock, flags);
 	if (diag_pcie_buf_tbl_add(pcie_info, buf, len, ctxt)) {
 		DIAG_LOG(DIAG_DEBUG_MUX,
 			"ERR! unable to add buf %pK to table\n", buf);
 		kfree(req->context);
 		diagmem_free(driver, req, pcie_info->mempool);
 		spin_unlock_irqrestore(&pcie_info->write_lock, flags);
 		return -ENOMEM;
 	}
 	spin_unlock_irqrestore(&pcie_info->write_lock, flags);
 	diag_ws_on_read(DIAG_WS_MUX, len);
 	bytes_to_write = mhi_dev_write_channel(req);
 	diag_ws_on_copy(DIAG_WS_MUX);
 	if (bytes_to_write != len) {
 		pr_err_ratelimited("diag: In %s, error writing to pcie channel %s, err: %d len: %d\n",
 			__func__, pcie_info->name, bytes_to_write, len);
 		diag_ws_on_copy_fail(DIAG_WS_MUX);
 		DIAG_LOG(DIAG_DEBUG_MUX,
 			 "ERR! unable to write to pcie, err: %d len: %d\n",
 			bytes_to_write, len);
 		spin_lock_irqsave(&pcie_info->write_lock, flags);
 		diag_pcie_buf_tbl_remove(pcie_info, buf);
 		spin_unlock_irqrestore(&pcie_info->write_lock, flags);
 		kfree(req->context);
 		diagmem_free(driver, req, pcie_info->mempool);
 		return -EINVAL;
 	}
 	DIAG_LOG(DIAG_DEBUG_MUX, "wrote packet to pcie chan:%d, len:%d",
 		pcie_info->out_chan, len);

 	return 0;
 }

 static int pcie_init_read_chan(struct diag_pcie_info *ptr,
 		enum mhi_client_channel chan)
 {
 	int rc = 0;
 	size_t buf_size;
 	void *data_loc;

 	if (ptr == NULL) {
 		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "Bad Input data, quitting\n");
 		return -EINVAL;
 	}

 	buf_size = ptr->in_chan_attr.max_pkt_size;
 	data_loc = kzalloc(buf_size, GFP_KERNEL);
 	if (!data_loc)
 		return -ENOMEM;

 	kmemleak_not_leak(data_loc);
 	ptr->in_chan_attr.read_buffer = data_loc;
 	ptr->in_chan_attr.read_buffer_size = buf_size;

 	return rc;

 }

 void diag_pcie_client_cb(struct mhi_dev_client_cb_data *cb_data)
 {
 	struct diag_pcie_info *pcie_info = NULL;

 	if (!cb_data)
 		return;

 	pcie_info = cb_data->user_data;
 	if (!pcie_info)
 		return;

 	switch (cb_data->ctrl_info) {
 	case  MHI_STATE_CONNECTED:
 		if (cb_data->channel == pcie_info->out_chan) {
 			DIAG_LOG(DIAG_DEBUG_MUX,
 				" Received connect event from MHI for %d",
 				pcie_info->out_chan);
 			if (atomic_read(&pcie_info->enabled))
 				return;
 			queue_work(pcie_info->wq, &pcie_info->open_work);
 		}
 		break;
 	case MHI_STATE_DISCONNECTED:
 		if (cb_data->channel == pcie_info->out_chan) {
 			DIAG_LOG(DIAG_DEBUG_MUX,
 				" Received disconnect event from MHI for %d",
 				pcie_info->out_chan);
 			if (!atomic_read(&pcie_info->enabled))
 				return;
 			queue_work(pcie_info->wq, &pcie_info->close_work);
 		}
 		break;
 	default:
 		break;
 	}
 }

 static int diag_register_pcie_channels(struct diag_pcie_info *pcie_info)
 {
 	int rc = 0;

 	if (!pcie_info)
 		return -EIO;

 	pcie_info->event_notifier = diag_pcie_event_notifier;

 	DIAG_LOG(DIAG_DEBUG_MUX,
 		"Initializing inbound chan %d.\n",
 		pcie_info->in_chan);
 	rc = pcie_init_read_chan(pcie_info, pcie_info->in_chan);
 	if (rc < 0) {
 		DIAG_LOG(DIAG_DEBUG_MUX,
 			"Failed to init inbound 0x%x, ret 0x%x\n",
 			pcie_info->in_chan, rc);
 		return rc;
 	}
 	/* Register for state change notifications from mhi*/
 	rc = mhi_register_state_cb(diag_pcie_client_cb, pcie_info,
 						pcie_info->out_chan);
 	if (rc < 0)
 		return rc;

 	return 0;
 }

 static void diag_pcie_connect(struct diag_pcie_info *ch)
 {
 	if (!ch || !atomic_read(&ch->enabled))
 		return;

 	if (ch->ops && ch->ops->open)
 		if (atomic_read(&ch->diag_state))
 			ch->ops->open(ch->ctxt, DIAG_PCIE_MODE);

 	/* As soon as we open the channel, queue a read */
 	queue_work(ch->wq, &(ch->read_work));
 }

 void diag_pcie_open_work_fn(struct work_struct *work)
 {
 	int rc = 0;
 	struct diag_pcie_info *pcie_info = container_of(work,
 						      struct diag_pcie_info,
 						      open_work);

 	if (!pcie_info || atomic_read(&pcie_info->enabled))
 		return;

 	mutex_lock(&pcie_info->out_chan_lock);
 	mutex_lock(&pcie_info->in_chan_lock);
 	/* Open write channel*/
 	rc = mhi_dev_open_channel(pcie_info->out_chan,
 			&pcie_info->out_handle,
 			pcie_info->event_notifier);
 	if (rc < 0) {
 		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
 			"Failed to open chan %d, ret %d\n",
 			pcie_info->in_chan, rc);
 		goto handle_not_rdy_err;
 	}
 	DIAG_LOG(DIAG_DEBUG_MUX, "opened write channel %d",
 		pcie_info->out_chan);

 	/* Open read channel*/
 	rc = mhi_dev_open_channel(pcie_info->in_chan,
 			&pcie_info->in_handle,
 			pcie_info->event_notifier);
 	if (rc < 0) {
 		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
 			"Failed to open chan %d, ret 0x%x\n",
 			pcie_info->in_chan, rc);
 		goto handle_in_err;
 	}
 	DIAG_LOG(DIAG_DEBUG_MUX, "opened read channel %d", pcie_info->in_chan);
 	mutex_unlock(&pcie_info->in_chan_lock);
 	mutex_unlock(&pcie_info->out_chan_lock);
 	atomic_set(&pcie_info->enabled, 1);
 	atomic_set(&pcie_info->diag_state, 1);
 	diag_pcie_connect(pcie_info);
 	return;
 handle_in_err:
 	mhi_dev_close_channel(pcie_info->out_handle);
 	atomic_set(&pcie_info->enabled, 0);
 handle_not_rdy_err:
 	mutex_unlock(&pcie_info->in_chan_lock);
 	mutex_unlock(&pcie_info->out_chan_lock);
 }

 /*
  * This function performs pcie connect operations wrt Diag synchronously. It
  * doesn't translate to actual pcie connect. This is used when Diag switches
  * logging to pcie mode and wants to mimic pcie connection.
  */
 void diag_pcie_connect_all(void)
 {
 	int i = 0;
 	struct diag_pcie_info *pcie_info = NULL;

 	for (i = 0; i < NUM_DIAG_PCIE_DEV; i++) {
 		pcie_info = &diag_pcie[i];
 		if (!atomic_read(&pcie_info->enabled))
 			continue;
 		atomic_set(&pcie_info->diag_state, 1);
 		diag_pcie_connect(pcie_info);
 	}
 }

 void diag_pcie_connect_device(int id)
 {
 	struct diag_pcie_info *pcie_info = NULL;

 	pcie_info = &diag_pcie[id];
 	if (!atomic_read(&pcie_info->enabled))
 		return;
 	atomic_set(&pcie_info->diag_state, 1);
 	diag_pcie_connect(pcie_info);
 }

 static void diag_pcie_disconnect(struct diag_pcie_info *ch)
 {
 	if (!ch)
 		return;

 	if (!atomic_read(&ch->enabled) &&
 		driver->pcie_connected && diag_mask_param())
 		diag_clear_masks(0);

 	if (ch && ch->ops && ch->ops->close)
 		ch->ops->close(ch->ctxt, DIAG_PCIE_MODE);
 }

 /*
  * This function performs pcie disconnect operations wrt Diag synchronously.
  * It doesn't translate to actual pcie disconnect. This is used when Diag
  * switches logging from pcie mode and want to mimic pcie disconnect.
  */
 void diag_pcie_disconnect_all(void)
 {
 	int i = 0;
 	struct diag_pcie_info *pcie_info = NULL;

 	for (i = 0; i < NUM_DIAG_PCIE_DEV; i++) {
 		pcie_info = &diag_pcie[i];
 		if (!atomic_read(&pcie_info->enabled))
 			continue;
 		atomic_set(&pcie_info->diag_state, 0);
 		diag_pcie_disconnect(pcie_info);
 	}
 }

 void diag_pcie_disconnect_device(int id)
 {
 	struct diag_pcie_info *pcie_info = NULL;

 	pcie_info = &diag_pcie[id];
 	if (!atomic_read(&pcie_info->enabled))
 		return;
 	atomic_set(&pcie_info->diag_state, 0);
 	diag_pcie_disconnect(pcie_info);
 }

 void diag_pcie_close_work_fn(struct work_struct *work)
 {
 	struct diag_pcie_info *pcie_info = container_of(work,
 						      struct diag_pcie_info,
 						      open_work);

 	if (!pcie_info || !atomic_read(&pcie_info->enabled))
 		return;
 	mutex_lock(&pcie_info->out_chan_lock);
 	mutex_lock(&pcie_info->in_chan_lock);
 	mhi_dev_close_channel(pcie_info->in_handle);
 	DIAG_LOG(DIAG_DEBUG_MUX, " closed in bound channel %d",
 		pcie_info->in_chan);
 	mhi_dev_close_channel(pcie_info->out_handle);
 	DIAG_LOG(DIAG_DEBUG_MUX, " closed out bound channel %d",
 		pcie_info->out_chan);
 	mutex_unlock(&pcie_info->in_chan_lock);
 	mutex_unlock(&pcie_info->out_chan_lock);
 	atomic_set(&pcie_info->enabled, 0);
 	diag_pcie_disconnect(pcie_info);
 }

 int diag_pcie_register(int id, int ctxt, struct diag_mux_ops *ops)
 {
 	struct diag_pcie_info *ch = NULL;
 	int rc = 0;
 	unsigned char wq_name[DIAG_PCIE_NAME_SZ + DIAG_PCIE_STRING_SZ];

 	if (id < 0 || id >= NUM_DIAG_PCIE_DEV) {
 		pr_err("diag: Unable to register with PCIE, id: %d\n", id);
 		return -EIO;
 	}

 	if (!ops) {
 		pr_err("diag: Invalid operations for PCIE\n");
 		return -EIO;
 	}

 	ch = &diag_pcie[id];
 	ch->ops = ops;
 	ch->ctxt = ctxt;
 	atomic_set(&ch->diag_state, 0);
 	atomic_set(&ch->enabled, 0);
 	INIT_LIST_HEAD(&ch->buf_tbl);
 	spin_lock_init(&ch->write_lock);
 	INIT_WORK(&(ch->read_work), diag_pcie_read_work_fn);
 	INIT_WORK(&(ch->open_work), diag_pcie_open_work_fn);
 	INIT_WORK(&(ch->close_work), diag_pcie_close_work_fn);
 	strlcpy(wq_name, "DIAG_PCIE_", sizeof(wq_name));
 	strlcat(wq_name, ch->name, sizeof(wq_name));
 	ch->wq = create_singlethread_workqueue(wq_name);
 	if (!ch->wq)
 		return -ENOMEM;
 	diagmem_init(driver, ch->mempool);
 	mutex_init(&ch->in_chan_lock);
 	mutex_init(&ch->out_chan_lock);
 	rc = diag_register_pcie_channels(ch);
 	if (rc < 0) {
 		if (ch->wq)
 			destroy_workqueue(ch->wq);
 		kfree(ch->in_chan_attr.read_buffer);
 		return rc;
 	}
 	return 0;
 }
	/* Copyright (c) 2018-2019, 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/slab.h>
	#include <linux/init.h>
	#include <linux/module.h>
	#include <linux/device.h>
	#include <linux/kernel.h>
	#include <linux/err.h>
	#include <linux/sched.h>
	#include <linux/ratelimit.h>
	#include <linux/workqueue.h>
	#include <linux/diagchar.h>
	#include <linux/delay.h>
	#include <linux/kmemleak.h>
	#include <linux/list.h>
	#include "diag_pcie.h"
	#include "diag_mux.h"
	#include "diagmem.h"
	#include "diag_ipc_logging.h"
	#define DIAG_LEGACY "DIAG_PCIE"

	struct diag_pcie_info diag_pcie[NUM_DIAG_PCIE_DEV] = {
	{
	.id = DIAG_PCIE_LOCAL,
	.name = DIAG_LEGACY,
	.enabled = {0},
	.mempool = POOL_TYPE_MUX_APPS,
	.ops = NULL,
	.wq = NULL,
	.read_cnt = 0,
	.write_cnt = 0,
	.in_chan_attr = {
	.max_pkt_size = DIAG_MAX_PKT_SZ,
	.nr_trbs = 1,
	.read_buffer = NULL,
	},
	.out_chan_attr = {
	.max_pkt_size = DIAG_MAX_PCIE_PKT_SZ,
	},
	.in_chan = MHI_CLIENT_DIAG_OUT,
	.out_chan = MHI_CLIENT_DIAG_IN,
	}
	};

	static void diag_pcie_event_notifier(struct mhi_dev_client_cb_reason *reason)
	{
	int i;
	struct diag_pcie_info *pcie_info = NULL;

	for (i = 0; i < NUM_DIAG_PCIE_DEV; i++) {
	pcie_info = &diag_pcie[i];
	if (reason->reason == MHI_DEV_TRE_AVAILABLE)
	if (reason->ch_id == pcie_info->in_chan) {
	queue_work(pcie_info->wq,
	&pcie_info->read_work);
	break;
	}
	}
	}

	void diag_pcie_read_work_fn(struct work_struct *work)
	{
	struct mhi_req ureq;
	struct diag_pcie_info *pcie_info = container_of(work,
	struct diag_pcie_info,
	read_work);
	unsigned int bytes_avail = 0;

	if (!pcie_info \|\| !atomic_read(&pcie_info->enabled) \|\|
	!atomic_read(&pcie_info->diag_state))
	return;

	ureq.chan = pcie_info->in_chan;
	ureq.client = pcie_info->in_handle;
	ureq.mode = DMA_SYNC;
	ureq.buf = pcie_info->in_chan_attr.read_buffer;
	ureq.len = pcie_info->in_chan_attr.read_buffer_size;
	ureq.transfer_len = 0;
	bytes_avail = mhi_dev_read_channel(&ureq);
	if (bytes_avail < 0)
	return;
	DIAG_LOG(DIAG_DEBUG_MUX, "read total bytes %d from chan:%d",
	bytes_avail, pcie_info->in_chan);
	pcie_info->read_cnt++;

	if (pcie_info->ops && pcie_info->ops->read_done)
	pcie_info->ops->read_done(pcie_info->in_chan_attr.read_buffer,
	ureq.transfer_len, pcie_info->ctxt);

	}

	static void diag_pcie_buf_tbl_remove(struct diag_pcie_info *pcie_info,
	unsigned char *buf)
	{
	struct diag_pcie_buf_tbl_t *temp = NULL;
	struct diag_pcie_buf_tbl_t *entry = NULL;

	list_for_each_entry_safe(entry, temp, &pcie_info->buf_tbl, track) {
	if (entry->buf == buf) {
	DIAG_LOG(DIAG_DEBUG_MUX, "ref_count-- for %pK\n", buf);
	atomic_dec(&entry->ref_count);
	/*
	* Remove reference from the table if it is the
	* only instance of the buffer
	*/
	if (atomic_read(&entry->ref_count) == 0) {
	list_del(&entry->track);
	kfree(entry);
	entry = NULL;
	}
	break;
	}
	}
	}

	static struct diag_pcie_buf_tbl_t *diag_pcie_buf_tbl_get(
	struct diag_pcie_info *pcie_info,
	unsigned char *buf)
	{
	struct diag_pcie_buf_tbl_t *temp = NULL;
	struct diag_pcie_buf_tbl_t *entry = NULL;

	list_for_each_entry_safe(entry, temp, &pcie_info->buf_tbl, track) {
	if (entry->buf == buf) {
	DIAG_LOG(DIAG_DEBUG_MUX, "ref_count-- for %pK\n", buf);
	atomic_dec(&entry->ref_count);
	return entry;
	}
	}

	return NULL;
	}

	void diag_pcie_write_complete_cb(void *req)
	{
	struct diag_pcie_context *ctxt = NULL;
	struct diag_pcie_info *ch;
	struct diag_pcie_buf_tbl_t *entry = NULL;
	struct mhi_req *ureq = req;
	unsigned long flags;

	if (!ureq)
	return;
	ctxt = (struct diag_pcie_context *)ureq->context;
	if (!ctxt)
	return;
	ch = ctxt->ch;
	if (!ch)
	return;
	spin_lock_irqsave(&ch->write_lock, flags);
	ch->write_cnt++;
	entry = diag_pcie_buf_tbl_get(ch, ctxt->buf);
	if (!entry) {
	pr_err_ratelimited("diag: In %s, unable to find entry %pK in the table\n",
	__func__, ctxt->buf);
	spin_unlock_irqrestore(&ch->write_lock, flags);
	return;
	}
	if (atomic_read(&entry->ref_count) != 0) {
	DIAG_LOG(DIAG_DEBUG_MUX, "partial write_done ref %d\n",
	atomic_read(&entry->ref_count));
	diag_ws_on_copy_complete(DIAG_WS_MUX);
	spin_unlock_irqrestore(&ch->write_lock, flags);
	diagmem_free(driver, req, ch->mempool);
	kfree(ctxt);
	ctxt = NULL;
	return;
	}
	DIAG_LOG(DIAG_DEBUG_MUX, "full write_done, ctxt: %pK\n",
	ctxt->buf);
	list_del(&entry->track);
	kfree(entry);
	entry = NULL;
	if (ch->ops && ch->ops->write_done)
	ch->ops->write_done(ureq->buf, ureq->len,
	ctxt->buf_ctxt, DIAG_PCIE_MODE);
	spin_unlock_irqrestore(&ch->write_lock, flags);
	diagmem_free(driver, req, ch->mempool);
	kfree(ctxt);
	ctxt = NULL;
	}

	static int diag_pcie_buf_tbl_add(struct diag_pcie_info *pcie_info,
	unsigned char *buf, uint32_t len, int ctxt)
	{
	struct diag_pcie_buf_tbl_t *temp = NULL;
	struct diag_pcie_buf_tbl_t *entry = NULL;

	list_for_each_entry_safe(entry, temp, &pcie_info->buf_tbl, track) {
	if (entry->buf == buf) {
	atomic_inc(&entry->ref_count);
	return 0;
	}
	}

	/* New buffer, not found in the list */
	entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
	if (!entry)
	return -ENOMEM;

	entry->buf = buf;
	entry->ctxt = ctxt;
	entry->len = len;
	atomic_set(&entry->ref_count, 1);
	INIT_LIST_HEAD(&entry->track);
	list_add_tail(&entry->track, &pcie_info->buf_tbl);

	return 0;
	}

	static int diag_pcie_write_ext(struct diag_pcie_info *pcie_info,
	unsigned char *buf, int len, int ctxt)
	{
	int write_len = 0;
	int bytes_remaining = len;
	int offset = 0;
	struct mhi_req *req;
	struct diag_pcie_context *context;
	int bytes_to_write;
	unsigned long flags;

	if (!pcie_info \|\| !buf \|\| len <= 0) {
	pr_err_ratelimited("diag: In %s, pcie_info: %pK buf: %pK, len: %d\n",
	__func__, pcie_info, buf, len);
	return -EINVAL;
	}

	while (bytes_remaining > 0) {
	req = diagmem_alloc(driver, sizeof(struct mhi_req),
	pcie_info->mempool);
	if (!req) {
	pr_err_ratelimited("diag: In %s, cannot retrieve pcie write ptrs for pcie channel %s\n",
	__func__, pcie_info->name);
	return -ENOMEM;
	}

	write_len = (bytes_remaining >
	pcie_info->out_chan_attr.max_pkt_size) ?
	pcie_info->out_chan_attr.max_pkt_size :
	bytes_remaining;
	req->client = pcie_info->out_handle;
	context = kzalloc(sizeof(*context), GFP_KERNEL);
	if (!context)
	return -ENOMEM;

	context->ch = pcie_info;
	context->buf_ctxt = ctxt;
	context->buf = buf;
	req->context = context;
	req->buf = buf + offset;
	req->len = write_len;
	req->chan = pcie_info->out_chan;
	req->mode = DMA_ASYNC;
	req->client_cb = diag_pcie_write_complete_cb;
	req->snd_cmpl = 1;
	if (!pcie_info->out_handle \|\|
	!atomic_read(&pcie_info->enabled) \|\|
	!atomic_read(&pcie_info->diag_state)) {
	pr_debug_ratelimited("diag: pcie ch %s is not opened\n",
	pcie_info->name);
	kfree(req->context);
	diagmem_free(driver, req, pcie_info->mempool);
	return -ENODEV;
	}
	spin_lock_irqsave(&pcie_info->write_lock, flags);
	if (diag_pcie_buf_tbl_add(pcie_info, buf, len, ctxt)) {
	kfree(req->context);
	diagmem_free(driver, req, pcie_info->mempool);
	spin_unlock_irqrestore(&pcie_info->write_lock, flags);
	return -ENOMEM;
	}
	spin_unlock_irqrestore(&pcie_info->write_lock, flags);
	diag_ws_on_read(DIAG_WS_MUX, len);
	bytes_to_write = mhi_dev_write_channel(req);
	diag_ws_on_copy(DIAG_WS_MUX);
	if (bytes_to_write != write_len) {
	pr_err_ratelimited("diag: In %s, error writing to pcie channel %s, err: %d, write_len: %d\n",
	__func__, pcie_info->name,
	bytes_to_write, write_len);
	DIAG_LOG(DIAG_DEBUG_MUX,
	"ERR! unable to write to pcie, err: %d, write_len: %d\n",
	bytes_to_write, write_len);
	diag_ws_on_copy_fail(DIAG_WS_MUX);
	spin_lock_irqsave(&pcie_info->write_lock, flags);
	diag_pcie_buf_tbl_remove(pcie_info, buf);
	kfree(req->context);
	diagmem_free(driver, req, pcie_info->mempool);
	spin_unlock_irqrestore(&pcie_info->write_lock, flags);
	return -EINVAL;
	}
	offset += write_len;
	bytes_remaining -= write_len;
	DIAG_LOG(DIAG_DEBUG_MUX,
	"bytes_remaining: %d write_len: %d, len: %d\n",
	bytes_remaining, write_len, len);
	}
	DIAG_LOG(DIAG_DEBUG_MUX, "done writing!");

	return 0;
	}

	int diag_pcie_write(int id, unsigned char *buf, int len, int ctxt)
	{
	struct mhi_req *req;
	struct diag_pcie_context *context;
	int bytes_to_write;
	struct diag_pcie_info *pcie_info;
	unsigned long flags;

	pcie_info = &diag_pcie[id];

	if (len > pcie_info->out_chan_attr.max_pkt_size) {
	DIAG_LOG(DIAG_DEBUG_MUX, "len: %d, max_size: %zu\n",
	len, pcie_info->out_chan_attr.max_pkt_size);
	return diag_pcie_write_ext(pcie_info, buf, len, ctxt);
	}
	req = (struct mhi_req *)diagmem_alloc(driver, sizeof(struct mhi_req),
	pcie_info->mempool);
	if (!req) {
	pr_err_ratelimited("diag: In %s, cannot retrieve pcie write ptrs for pcie channel %s\n",
	__func__, pcie_info->name);
	return -ENOMEM;
	}
	req->client = pcie_info->out_handle;
	context = kzalloc(sizeof(struct diag_pcie_context), GFP_KERNEL);
	if (!context)
	return -ENOMEM;

	context->ch = &diag_pcie[id];
	context->buf_ctxt = ctxt;
	context->buf = buf;
	req->context = context;
	req->buf = buf;
	req->len = len;
	req->chan = pcie_info->out_chan;
	req->mode = DMA_ASYNC;
	req->client_cb = diag_pcie_write_complete_cb;
	req->snd_cmpl = 1;
	if (!pcie_info->out_handle \|\| !atomic_read(&pcie_info->enabled) \|\|
	!atomic_read(&pcie_info->diag_state)) {
	pr_debug_ratelimited("diag: pcie ch %s is not opened\n",
	pcie_info->name);
	kfree(req->context);
	diagmem_free(driver, req, pcie_info->mempool);
	return -ENODEV;
	}
	spin_lock_irqsave(&pcie_info->write_lock, flags);
	if (diag_pcie_buf_tbl_add(pcie_info, buf, len, ctxt)) {
	DIAG_LOG(DIAG_DEBUG_MUX,
	"ERR! unable to add buf %pK to table\n", buf);
	kfree(req->context);
	diagmem_free(driver, req, pcie_info->mempool);
	spin_unlock_irqrestore(&pcie_info->write_lock, flags);
	return -ENOMEM;
	}
	spin_unlock_irqrestore(&pcie_info->write_lock, flags);
	diag_ws_on_read(DIAG_WS_MUX, len);
	bytes_to_write = mhi_dev_write_channel(req);
	diag_ws_on_copy(DIAG_WS_MUX);
	if (bytes_to_write != len) {
	pr_err_ratelimited("diag: In %s, error writing to pcie channel %s, err: %d len: %d\n",
	__func__, pcie_info->name, bytes_to_write, len);
	diag_ws_on_copy_fail(DIAG_WS_MUX);
	DIAG_LOG(DIAG_DEBUG_MUX,
	"ERR! unable to write to pcie, err: %d len: %d\n",
	bytes_to_write, len);
	spin_lock_irqsave(&pcie_info->write_lock, flags);
	diag_pcie_buf_tbl_remove(pcie_info, buf);
	spin_unlock_irqrestore(&pcie_info->write_lock, flags);
	kfree(req->context);
	diagmem_free(driver, req, pcie_info->mempool);
	return -EINVAL;
	}
	DIAG_LOG(DIAG_DEBUG_MUX, "wrote packet to pcie chan:%d, len:%d",
	pcie_info->out_chan, len);

	return 0;
	}

	static int pcie_init_read_chan(struct diag_pcie_info *ptr,
	enum mhi_client_channel chan)
	{
	int rc = 0;
	size_t buf_size;
	void *data_loc;

	if (ptr == NULL) {
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "Bad Input data, quitting\n");
	return -EINVAL;
	}

	buf_size = ptr->in_chan_attr.max_pkt_size;
	data_loc = kzalloc(buf_size, GFP_KERNEL);
	if (!data_loc)
	return -ENOMEM;

	kmemleak_not_leak(data_loc);
	ptr->in_chan_attr.read_buffer = data_loc;
	ptr->in_chan_attr.read_buffer_size = buf_size;

	return rc;

	}

	void diag_pcie_client_cb(struct mhi_dev_client_cb_data *cb_data)
	{
	struct diag_pcie_info *pcie_info = NULL;

	if (!cb_data)
	return;

	pcie_info = cb_data->user_data;
	if (!pcie_info)
	return;

	switch (cb_data->ctrl_info) {
	case MHI_STATE_CONNECTED:
	if (cb_data->channel == pcie_info->out_chan) {
	DIAG_LOG(DIAG_DEBUG_MUX,
	" Received connect event from MHI for %d",
	pcie_info->out_chan);
	if (atomic_read(&pcie_info->enabled))
	return;
	queue_work(pcie_info->wq, &pcie_info->open_work);
	}
	break;
	case MHI_STATE_DISCONNECTED:
	if (cb_data->channel == pcie_info->out_chan) {
	DIAG_LOG(DIAG_DEBUG_MUX,
	" Received disconnect event from MHI for %d",
	pcie_info->out_chan);
	if (!atomic_read(&pcie_info->enabled))
	return;
	queue_work(pcie_info->wq, &pcie_info->close_work);
	}
	break;
	default:
	break;
	}
	}

	static int diag_register_pcie_channels(struct diag_pcie_info *pcie_info)
	{
	int rc = 0;

	if (!pcie_info)
	return -EIO;

	pcie_info->event_notifier = diag_pcie_event_notifier;

	DIAG_LOG(DIAG_DEBUG_MUX,
	"Initializing inbound chan %d.\n",
	pcie_info->in_chan);
	rc = pcie_init_read_chan(pcie_info, pcie_info->in_chan);
	if (rc < 0) {
	DIAG_LOG(DIAG_DEBUG_MUX,
	"Failed to init inbound 0x%x, ret 0x%x\n",
	pcie_info->in_chan, rc);
	return rc;
	}
	/* Register for state change notifications from mhi*/
	rc = mhi_register_state_cb(diag_pcie_client_cb, pcie_info,
	pcie_info->out_chan);
	if (rc < 0)
	return rc;

	return 0;
	}

	static void diag_pcie_connect(struct diag_pcie_info *ch)
	{
	if (!ch \|\| !atomic_read(&ch->enabled))
	return;

	if (ch->ops && ch->ops->open)
	if (atomic_read(&ch->diag_state))
	ch->ops->open(ch->ctxt, DIAG_PCIE_MODE);

	/* As soon as we open the channel, queue a read */
	queue_work(ch->wq, &(ch->read_work));
	}

	void diag_pcie_open_work_fn(struct work_struct *work)
	{
	int rc = 0;
	struct diag_pcie_info *pcie_info = container_of(work,
	struct diag_pcie_info,
	open_work);

	if (!pcie_info \|\| atomic_read(&pcie_info->enabled))
	return;

	mutex_lock(&pcie_info->out_chan_lock);
	mutex_lock(&pcie_info->in_chan_lock);
	/* Open write channel*/
	rc = mhi_dev_open_channel(pcie_info->out_chan,
	&pcie_info->out_handle,
	pcie_info->event_notifier);
	if (rc < 0) {
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
	"Failed to open chan %d, ret %d\n",
	pcie_info->in_chan, rc);
	goto handle_not_rdy_err;
	}
	DIAG_LOG(DIAG_DEBUG_MUX, "opened write channel %d",
	pcie_info->out_chan);

	/* Open read channel*/
	rc = mhi_dev_open_channel(pcie_info->in_chan,
	&pcie_info->in_handle,
	pcie_info->event_notifier);
	if (rc < 0) {
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
	"Failed to open chan %d, ret 0x%x\n",
	pcie_info->in_chan, rc);
	goto handle_in_err;
	}
	DIAG_LOG(DIAG_DEBUG_MUX, "opened read channel %d", pcie_info->in_chan);
	mutex_unlock(&pcie_info->in_chan_lock);
	mutex_unlock(&pcie_info->out_chan_lock);
	atomic_set(&pcie_info->enabled, 1);
	atomic_set(&pcie_info->diag_state, 1);
	diag_pcie_connect(pcie_info);
	return;
	handle_in_err:
	mhi_dev_close_channel(pcie_info->out_handle);
	atomic_set(&pcie_info->enabled, 0);
	handle_not_rdy_err:
	mutex_unlock(&pcie_info->in_chan_lock);
	mutex_unlock(&pcie_info->out_chan_lock);
	}

	/*
	* This function performs pcie connect operations wrt Diag synchronously. It
	* doesn't translate to actual pcie connect. This is used when Diag switches
	* logging to pcie mode and wants to mimic pcie connection.
	*/
	void diag_pcie_connect_all(void)
	{
	int i = 0;
	struct diag_pcie_info *pcie_info = NULL;

	for (i = 0; i < NUM_DIAG_PCIE_DEV; i++) {
	pcie_info = &diag_pcie[i];
	if (!atomic_read(&pcie_info->enabled))
	continue;
	atomic_set(&pcie_info->diag_state, 1);
	diag_pcie_connect(pcie_info);
	}
	}

	void diag_pcie_connect_device(int id)
	{
	struct diag_pcie_info *pcie_info = NULL;

	pcie_info = &diag_pcie[id];
	if (!atomic_read(&pcie_info->enabled))
	return;
	atomic_set(&pcie_info->diag_state, 1);
	diag_pcie_connect(pcie_info);
	}

	static void diag_pcie_disconnect(struct diag_pcie_info *ch)
	{
	if (!ch)
	return;

	if (!atomic_read(&ch->enabled) &&
	driver->pcie_connected && diag_mask_param())
	diag_clear_masks(0);

	if (ch && ch->ops && ch->ops->close)
	ch->ops->close(ch->ctxt, DIAG_PCIE_MODE);
	}

	/*
	* This function performs pcie disconnect operations wrt Diag synchronously.
	* It doesn't translate to actual pcie disconnect. This is used when Diag
	* switches logging from pcie mode and want to mimic pcie disconnect.
	*/
	void diag_pcie_disconnect_all(void)
	{
	int i = 0;
	struct diag_pcie_info *pcie_info = NULL;

	for (i = 0; i < NUM_DIAG_PCIE_DEV; i++) {
	pcie_info = &diag_pcie[i];
	if (!atomic_read(&pcie_info->enabled))
	continue;
	atomic_set(&pcie_info->diag_state, 0);
	diag_pcie_disconnect(pcie_info);
	}
	}

	void diag_pcie_disconnect_device(int id)
	{
	struct diag_pcie_info *pcie_info = NULL;

	pcie_info = &diag_pcie[id];
	if (!atomic_read(&pcie_info->enabled))
	return;
	atomic_set(&pcie_info->diag_state, 0);
	diag_pcie_disconnect(pcie_info);
	}

	void diag_pcie_close_work_fn(struct work_struct *work)
	{
	struct diag_pcie_info *pcie_info = container_of(work,
	struct diag_pcie_info,
	open_work);

	if (!pcie_info \|\| !atomic_read(&pcie_info->enabled))
	return;
	mutex_lock(&pcie_info->out_chan_lock);
	mutex_lock(&pcie_info->in_chan_lock);
	mhi_dev_close_channel(pcie_info->in_handle);
	DIAG_LOG(DIAG_DEBUG_MUX, " closed in bound channel %d",
	pcie_info->in_chan);
	mhi_dev_close_channel(pcie_info->out_handle);
	DIAG_LOG(DIAG_DEBUG_MUX, " closed out bound channel %d",
	pcie_info->out_chan);
	mutex_unlock(&pcie_info->in_chan_lock);
	mutex_unlock(&pcie_info->out_chan_lock);
	atomic_set(&pcie_info->enabled, 0);
	diag_pcie_disconnect(pcie_info);
	}

	int diag_pcie_register(int id, int ctxt, struct diag_mux_ops *ops)
	{
	struct diag_pcie_info *ch = NULL;
	int rc = 0;
	unsigned char wq_name[DIAG_PCIE_NAME_SZ + DIAG_PCIE_STRING_SZ];

	if (id < 0 \|\| id >= NUM_DIAG_PCIE_DEV) {
	pr_err("diag: Unable to register with PCIE, id: %d\n", id);
	return -EIO;
	}

	if (!ops) {
	pr_err("diag: Invalid operations for PCIE\n");
	return -EIO;
	}

	ch = &diag_pcie[id];
	ch->ops = ops;
	ch->ctxt = ctxt;
	atomic_set(&ch->diag_state, 0);
	atomic_set(&ch->enabled, 0);
	INIT_LIST_HEAD(&ch->buf_tbl);
	spin_lock_init(&ch->write_lock);
	INIT_WORK(&(ch->read_work), diag_pcie_read_work_fn);
	INIT_WORK(&(ch->open_work), diag_pcie_open_work_fn);
	INIT_WORK(&(ch->close_work), diag_pcie_close_work_fn);
	strlcpy(wq_name, "DIAG_PCIE_", sizeof(wq_name));
	strlcat(wq_name, ch->name, sizeof(wq_name));
	ch->wq = create_singlethread_workqueue(wq_name);
	if (!ch->wq)
	return -ENOMEM;
	diagmem_init(driver, ch->mempool);
	mutex_init(&ch->in_chan_lock);
	mutex_init(&ch->out_chan_lock);
	rc = diag_register_pcie_channels(ch);
	if (rc < 0) {
	if (ch->wq)
	destroy_workqueue(ch->wq);
	kfree(ch->in_chan_attr.read_buffer);
	return rc;
	}
	return 0;
	}