/* Copyright (c) 2015-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/slab.h>
#include <linux/platform_device.h>
#include <linux/device.h>
#include <linux/err.h>
#include <linux/sched.h>
#include <linux/ratelimit.h>
#include <linux/workqueue.h>
#include <linux/msm_ipc.h>
#include <linux/socket.h>
#include <linux/pm_runtime.h>
#include <linux/delay.h>
#include <linux/diagchar.h>
#include <linux/of.h>
#include <linux/kmemleak.h>
#include <asm/current.h>
#include <net/sock.h>
#include <linux/ipc_router.h>
#include <linux/notifier.h>
#include "diagchar.h"
#include "diagfwd.h"
#include "diagfwd_peripheral.h"
#include "diagfwd_socket.h"
#include "diag_ipc_logging.h"

#define DIAG_SVC_ID		0x1001

#define MODEM_INST_BASE		0
#define LPASS_INST_BASE		64
#define WCNSS_INST_BASE		128
#define SENSORS_INST_BASE	192
#define WDSP_INST_BASE	256
#define CDSP_INST_BASE  320

#define INST_ID_CNTL		0
#define INST_ID_CMD		1
#define INST_ID_DATA		2
#define INST_ID_DCI_CMD		3
#define INST_ID_DCI		4

struct diag_cntl_socket_info *cntl_socket;

struct diag_socket_info socket_data[NUM_PERIPHERALS] = {
	{
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_DATA,
		.name = "MODEM_DATA"
	},
	{
		.peripheral = PERIPHERAL_LPASS,
		.type = TYPE_DATA,
		.name = "LPASS_DATA"
	},
	{
		.peripheral = PERIPHERAL_WCNSS,
		.type = TYPE_DATA,
		.name = "WCNSS_DATA"
	},
	{
		.peripheral = PERIPHERAL_SENSORS,
		.type = TYPE_DATA,
		.name = "SENSORS_DATA"
	},
	{
		.peripheral = PERIPHERAL_WDSP,
		.type = TYPE_DATA,
		.name = "DIAG_DATA"
	},
	{
		.peripheral = PERIPHERAL_CDSP,
		.type = TYPE_DATA,
		.name = "CDSP_DATA"
	}
};

struct diag_socket_info socket_cntl[NUM_PERIPHERALS] = {
	{
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_CNTL,
		.name = "MODEM_CNTL"
	},
	{
		.peripheral = PERIPHERAL_LPASS,
		.type = TYPE_CNTL,
		.name = "LPASS_CNTL"
	},
	{
		.peripheral = PERIPHERAL_WCNSS,
		.type = TYPE_CNTL,
		.name = "WCNSS_CNTL"
	},
	{
		.peripheral = PERIPHERAL_SENSORS,
		.type = TYPE_CNTL,
		.name = "SENSORS_CNTL"
	},
	{
		.peripheral = PERIPHERAL_WDSP,
		.type = TYPE_CNTL,
		.name = "DIAG_CTRL"
	},
	{
		.peripheral = PERIPHERAL_CDSP,
		.type = TYPE_CNTL,
		.name = "CDSP_CNTL"
	}
};

struct diag_socket_info socket_dci[NUM_PERIPHERALS] = {
	{
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_DCI,
		.name = "MODEM_DCI"
	},
	{
		.peripheral = PERIPHERAL_LPASS,
		.type = TYPE_DCI,
		.name = "LPASS_DCI"
	},
	{
		.peripheral = PERIPHERAL_WCNSS,
		.type = TYPE_DCI,
		.name = "WCNSS_DCI"
	},
	{
		.peripheral = PERIPHERAL_SENSORS,
		.type = TYPE_DCI,
		.name = "SENSORS_DCI"
	},
	{
		.peripheral = PERIPHERAL_WDSP,
		.type = TYPE_DCI,
		.name = "DIAG_DCI_DATA"
	},
	{
		.peripheral = PERIPHERAL_CDSP,
		.type = TYPE_DCI,
		.name = "CDSP_DCI"
	}
};

struct diag_socket_info socket_cmd[NUM_PERIPHERALS] = {
	{
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_CMD,
		.name = "MODEM_CMD"
	},
	{
		.peripheral = PERIPHERAL_LPASS,
		.type = TYPE_CMD,
		.name = "LPASS_CMD"
	},
	{
		.peripheral = PERIPHERAL_WCNSS,
		.type = TYPE_CMD,
		.name = "WCNSS_CMD"
	},
	{
		.peripheral = PERIPHERAL_SENSORS,
		.type = TYPE_CMD,
		.name = "SENSORS_CMD"
	},
	{
		.peripheral = PERIPHERAL_WDSP,
		.type = TYPE_CMD,
		.name = "DIAG_CMD"
	},
	{
		.peripheral = PERIPHERAL_CDSP,
		.type = TYPE_CMD,
		.name = "CDSP_CMD"
	}

};

struct diag_socket_info socket_dci_cmd[NUM_PERIPHERALS] = {
	{
		.peripheral = PERIPHERAL_MODEM,
		.type = TYPE_DCI_CMD,
		.name = "MODEM_DCI_CMD"
	},
	{
		.peripheral = PERIPHERAL_LPASS,
		.type = TYPE_DCI_CMD,
		.name = "LPASS_DCI_CMD"
	},
	{
		.peripheral = PERIPHERAL_WCNSS,
		.type = TYPE_DCI_CMD,
		.name = "WCNSS_DCI_CMD"
	},
	{
		.peripheral = PERIPHERAL_SENSORS,
		.type = TYPE_DCI_CMD,
		.name = "SENSORS_DCI_CMD"
	},
	{
		.peripheral = PERIPHERAL_WDSP,
		.type = TYPE_DCI_CMD,
		.name = "DIAG_DCI_CMD"
	},
	{
		.peripheral = PERIPHERAL_CDSP,
		.type = TYPE_DCI_CMD,
		.name = "CDSP_DCI_CMD"
	},
};

static void diag_state_open_socket(void *ctxt);
static void diag_state_close_socket(void *ctxt);
static int diag_socket_write(void *ctxt, unsigned char *buf, int len);
static int diag_socket_read(void *ctxt, unsigned char *buf, int buf_len);
static void diag_socket_queue_read(void *ctxt);
static void socket_init_work_fn(struct work_struct *work);
static int socket_ready_notify(struct notifier_block *nb,
			       unsigned long action, void *data);

static struct diag_peripheral_ops socket_ops = {
	.open = diag_state_open_socket,
	.close = diag_state_close_socket,
	.write = diag_socket_write,
	.read = diag_socket_read,
	.queue_read = diag_socket_queue_read
};

static struct notifier_block socket_notify = {
	.notifier_call = socket_ready_notify,
};

static void diag_state_open_socket(void *ctxt)
{
	struct diag_socket_info *info = NULL;

	if (!ctxt)
		return;

	info = (struct diag_socket_info *)(ctxt);
	atomic_set(&info->diag_state, 1);
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		 "%s setting diag state to 1", info->name);
}

static void diag_state_close_socket(void *ctxt)
{
	struct diag_socket_info *info = NULL;

	if (!ctxt)
		return;

	info = (struct diag_socket_info *)(ctxt);
	atomic_set(&info->diag_state, 0);
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
		 "%s setting diag state to 0", info->name);
	wake_up_interruptible(&info->read_wait_q);
	flush_workqueue(info->wq);
}

static void socket_data_ready(struct sock *sk_ptr)
{
	unsigned long flags;
	struct diag_socket_info *info = NULL;

	if (!sk_ptr) {
		pr_err_ratelimited("diag: In %s, invalid sk_ptr", __func__);
		return;
	}

	info = (struct diag_socket_info *)(sk_ptr->sk_user_data);
	if (!info) {
		pr_err_ratelimited("diag: In %s, invalid info\n", __func__);
		return;
	}

	spin_lock_irqsave(&info->lock, flags);
	info->data_ready++;
	spin_unlock_irqrestore(&info->lock, flags);
	diag_ws_on_notify();

	/*
	 * Initialize read buffers for the servers. The servers must read data
	 * first to get the address of its clients.
	 */
	if (!atomic_read(&info->opened) && info->port_type == PORT_TYPE_SERVER)
		diagfwd_buffers_init(info->fwd_ctxt);

	queue_work(info->wq, &(info->read_work));
	wake_up_interruptible(&info->read_wait_q);
}

static void cntl_socket_data_ready(struct sock *sk_ptr)
{
	if (!sk_ptr || !cntl_socket) {
		pr_err_ratelimited("diag: In %s, invalid ptrs. sk_ptr: %pK cntl_socket: %pK\n",
				   __func__, sk_ptr, cntl_socket);
		return;
	}

	atomic_inc(&cntl_socket->data_ready);
	wake_up_interruptible(&cntl_socket->read_wait_q);
	queue_work(cntl_socket->wq, &(cntl_socket->read_work));
}

static void socket_flow_cntl(struct sock *sk_ptr)
{
	struct diag_socket_info *info = NULL;

	if (!sk_ptr)
		return;

	info = (struct diag_socket_info *)(sk_ptr->sk_user_data);
	if (!info) {
		pr_err_ratelimited("diag: In %s, invalid info\n", __func__);
		return;
	}

	atomic_inc(&info->flow_cnt);
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s flow controlled\n", info->name);
	pr_debug("diag: In %s, channel %s flow controlled\n",
		 __func__, info->name);
}

static int lookup_server(struct diag_socket_info *info)
{
	int ret = 0;
	struct server_lookup_args *args = NULL;
	struct sockaddr_msm_ipc *srv_addr = NULL;

	if (!info)
		return -EINVAL;

	args = kzalloc((sizeof(struct server_lookup_args) +
			sizeof(struct msm_ipc_server_info)), GFP_KERNEL);
	if (!args)
		return -ENOMEM;
	kmemleak_not_leak(args);

	args->lookup_mask = 0xFFFFFFFF;
	args->port_name.service = info->svc_id;
	args->port_name.instance = info->ins_id;
	args->num_entries_in_array = 1;
	args->num_entries_found = 0;

	ret = kernel_sock_ioctl(info->hdl, IPC_ROUTER_IOCTL_LOOKUP_SERVER,
				(unsigned long)args);
	if (ret < 0) {
		pr_err("diag: In %s, cannot find service for %s\n", __func__,
		       info->name);
		kfree(args);
		return -EFAULT;
	}

	srv_addr = &info->remote_addr;
	srv_addr->family = AF_MSM_IPC;
	srv_addr->address.addrtype = MSM_IPC_ADDR_ID;
	srv_addr->address.addr.port_addr.node_id = args->srv_info[0].node_id;
	srv_addr->address.addr.port_addr.port_id = args->srv_info[0].port_id;
	ret = args->num_entries_found;
	kfree(args);
	if (ret < 1)
		return -EIO;
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s found server node: %d port: %d",
		 info->name, srv_addr->address.addr.port_addr.node_id,
		 srv_addr->address.addr.port_addr.port_id);
	return 0;
}

static void __socket_open_channel(struct diag_socket_info *info)
{
	if (!info)
		return;

	if (!info->inited) {
		pr_debug("diag: In %s, socket %s is not initialized\n",
			 __func__, info->name);
		return;
	}

	if (atomic_read(&info->opened)) {
		pr_debug("diag: In %s, socket %s already opened\n",
			 __func__, info->name);
		return;
	}

	atomic_set(&info->opened, 1);
	diagfwd_channel_open(info->fwd_ctxt);
}

static void socket_open_client(struct diag_socket_info *info)
{
	int ret = 0;

	if (!info || info->port_type != PORT_TYPE_CLIENT)
		return;

	ret = sock_create(AF_MSM_IPC, SOCK_DGRAM, 0, &info->hdl);
	if (ret < 0 || !info->hdl) {
		pr_err("diag: In %s, socket not initialized for %s\n", __func__,
		       info->name);
		return;
	}

	write_lock_bh(&info->hdl->sk->sk_callback_lock);
	info->hdl->sk->sk_user_data = (void *)(info);
	info->hdl->sk->sk_data_ready = socket_data_ready;
	info->hdl->sk->sk_write_space = socket_flow_cntl;
	write_unlock_bh(&info->hdl->sk->sk_callback_lock);
	ret = lookup_server(info);
	if (ret) {
		pr_err("diag: In %s, failed to lookup server, ret: %d\n",
		       __func__, ret);
		return;
	}
	__socket_open_channel(info);
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n", info->name);
}

static void socket_open_server(struct diag_socket_info *info)
{
	int ret = 0;
	struct sockaddr_msm_ipc srv_addr = { 0 };

	if (!info)
		return;

	ret = sock_create(AF_MSM_IPC, SOCK_DGRAM, 0, &info->hdl);
	if (ret < 0 || !info->hdl) {
		pr_err("diag: In %s, socket not initialized for %s\n", __func__,
		       info->name);
		return;
	}

	write_lock_bh(&info->hdl->sk->sk_callback_lock);
	info->hdl->sk->sk_user_data = (void *)(info);
	info->hdl->sk->sk_data_ready = socket_data_ready;
	info->hdl->sk->sk_write_space = socket_flow_cntl;
	write_unlock_bh(&info->hdl->sk->sk_callback_lock);

	srv_addr.family = AF_MSM_IPC;
	srv_addr.address.addrtype = MSM_IPC_ADDR_NAME;
	srv_addr.address.addr.port_name.service = info->svc_id;
	srv_addr.address.addr.port_name.instance = info->ins_id;

	ret = kernel_bind(info->hdl, (struct sockaddr *)&srv_addr,
			  sizeof(srv_addr));
	if (ret) {
		pr_err("diag: In %s, failed to bind, ch: %s, svc_id: %d ins_id: %d, err: %d\n",
		       __func__, info->name, info->svc_id, info->ins_id, ret);
		return;
	}
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s opened server svc: %d ins: %d",
		 info->name, info->svc_id, info->ins_id);
}

static void socket_init_work_fn(struct work_struct *work)
{
	struct diag_socket_info *info = container_of(work,
						     struct diag_socket_info,
						     init_work);
	if (!info)
		return;

	if (!info->inited) {
		pr_debug("diag: In %s, socket %s is not initialized\n",
			 __func__, info->name);
		return;
	}

	switch (info->port_type) {
	case PORT_TYPE_SERVER:
		socket_open_server(info);
		break;
	case PORT_TYPE_CLIENT:
		socket_open_client(info);
		break;
	default:
		pr_err("diag: In %s, unknown type %d\n", __func__,
		       info->port_type);
		break;
	}
}

static void __socket_close_channel(struct diag_socket_info *info)
{
	if (!info || !info->hdl)
		return;

	if (!atomic_read(&info->opened))
		return;

	memset(&info->remote_addr, 0, sizeof(struct sockaddr_msm_ipc));
	diagfwd_channel_close(info->fwd_ctxt);

	atomic_set(&info->opened, 0);

	/* Don't close the server. Server should always remain open */
	if (info->port_type != PORT_TYPE_SERVER) {
		write_lock_bh(&info->hdl->sk->sk_callback_lock);
		info->hdl->sk->sk_user_data = NULL;
		info->hdl->sk->sk_data_ready = NULL;
		write_unlock_bh(&info->hdl->sk->sk_callback_lock);
		sock_release(info->hdl);
		info->hdl = NULL;
		wake_up_interruptible(&info->read_wait_q);
	}
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n", info->name);
}

static void socket_close_channel(struct diag_socket_info *info)
{
	if (!info)
		return;

	__socket_close_channel(info);

	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n", info->name);
}

static int cntl_socket_process_msg_server(uint32_t cmd, uint32_t svc_id,
					  uint32_t ins_id)
{
	uint8_t peripheral;
	uint8_t found = 0;
	struct diag_socket_info *info = NULL;

	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
		info = &socket_cmd[peripheral];
		if ((svc_id == info->svc_id) &&
		    (ins_id == info->ins_id)) {
			found = 1;
			break;
		}

		info = &socket_dci_cmd[peripheral];
		if ((svc_id == info->svc_id) &&
		    (ins_id == info->ins_id)) {
			found = 1;
			break;
		}
	}

	if (!found)
		return -EIO;

	switch (cmd) {
	case CNTL_CMD_NEW_SERVER:
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s received new server\n",
			 info->name);
		diagfwd_register(TRANSPORT_SOCKET, info->peripheral,
				 info->type, (void *)info, &socket_ops,
				 &info->fwd_ctxt);
		queue_work(info->wq, &(info->init_work));
		break;
	case CNTL_CMD_REMOVE_SERVER:
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s received remove server\n",
			 info->name);
		socket_close_channel(info);
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static int cntl_socket_process_msg_client(uint32_t cmd, uint32_t node_id,
					  uint32_t port_id)
{
	uint8_t peripheral;
	uint8_t found = 0;
	struct diag_socket_info *info = NULL;
	struct msm_ipc_port_addr remote_port = {0};

	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
		info = &socket_data[peripheral];
		remote_port = info->remote_addr.address.addr.port_addr;
		if ((remote_port.node_id == node_id) &&
		    (remote_port.port_id == port_id)) {
			found = 1;
			break;
		}

		info = &socket_cntl[peripheral];
		remote_port = info->remote_addr.address.addr.port_addr;
		if ((remote_port.node_id == node_id) &&
		    (remote_port.port_id == port_id)) {
			found = 1;
			break;
		}

		info = &socket_dci[peripheral];
		remote_port = info->remote_addr.address.addr.port_addr;
		if ((remote_port.node_id == node_id) &&
		    (remote_port.port_id == port_id)) {
			found = 1;
			break;
		}
	}

	if (!found)
		return -EIO;

	switch (cmd) {
	case CNTL_CMD_REMOVE_CLIENT:
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s received remove client\n",
			 info->name);
		socket_close_channel(info);
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static void cntl_socket_read_work_fn(struct work_struct *work)
{
	union cntl_port_msg msg;
	int ret = 0;
	struct kvec iov = { 0 };
	struct msghdr read_msg = { 0 };


	if (!cntl_socket)
		return;

	ret = wait_event_interruptible(cntl_socket->read_wait_q,
				(atomic_read(&cntl_socket->data_ready) > 0));
	if (ret)
		return;

	do {
		iov.iov_base = &msg;
		iov.iov_len = sizeof(msg);
		read_msg.msg_name = NULL;
		read_msg.msg_namelen = 0;
		ret = kernel_recvmsg(cntl_socket->hdl, &read_msg, &iov, 1,
				     sizeof(msg), MSG_DONTWAIT);
		if (ret < 0) {
			pr_debug("diag: In %s, Error recving data %d\n",
				 __func__, ret);
			break;
		}

		atomic_dec(&cntl_socket->data_ready);

		switch (msg.srv.cmd) {
		case CNTL_CMD_NEW_SERVER:
		case CNTL_CMD_REMOVE_SERVER:
			cntl_socket_process_msg_server(msg.srv.cmd,
						       msg.srv.service,
						       msg.srv.instance);
			break;
		case CNTL_CMD_REMOVE_CLIENT:
			cntl_socket_process_msg_client(msg.cli.cmd,
						       msg.cli.node_id,
						       msg.cli.port_id);
			break;
		}
	} while (atomic_read(&cntl_socket->data_ready) > 0);
}

static void socket_read_work_fn(struct work_struct *work)
{
	struct diag_socket_info *info = container_of(work,
						     struct diag_socket_info,
						     read_work);

	if (!info)
		return;

	diagfwd_channel_read(info->fwd_ctxt);
}

static void diag_socket_queue_read(void *ctxt)
{
	struct diag_socket_info *info = NULL;

	if (!ctxt)
		return;

	info = (struct diag_socket_info *)ctxt;
	if (info->hdl && info->wq)
		queue_work(info->wq, &(info->read_work));
}

void diag_socket_invalidate(void *ctxt, struct diagfwd_info *fwd_ctxt)
{
	struct diag_socket_info *info = NULL;

	if (!ctxt || !fwd_ctxt)
		return;

	info = (struct diag_socket_info *)ctxt;
	info->fwd_ctxt = fwd_ctxt;
}

int diag_socket_check_state(void *ctxt)
{
	struct diag_socket_info *info = NULL;

	if (!ctxt)
		return 0;

	info = (struct diag_socket_info *)ctxt;
	return (int)(atomic_read(&info->diag_state));
}

static void __diag_socket_init(struct diag_socket_info *info)
{
	uint16_t ins_base = 0;
	uint16_t ins_offset = 0;

	char wq_name[DIAG_SOCKET_NAME_SZ + 10];

	if (!info)
		return;

	init_waitqueue_head(&info->wait_q);
	info->inited = 0;
	atomic_set(&info->opened, 0);
	atomic_set(&info->diag_state, 0);
	info->pkt_len = 0;
	info->pkt_read = 0;
	info->hdl = NULL;
	info->fwd_ctxt = NULL;
	info->data_ready = 0;
	atomic_set(&info->flow_cnt, 0);
	spin_lock_init(&info->lock);
	strlcpy(wq_name, "DIAG_SOCKET_", 10);
	strlcat(wq_name, info->name, sizeof(info->name));
	init_waitqueue_head(&info->read_wait_q);
	info->wq = create_singlethread_workqueue(wq_name);
	if (!info->wq) {
		pr_err("diag: In %s, unable to create workqueue for socket channel %s\n",
		       __func__, info->name);
		return;
	}
	INIT_WORK(&(info->init_work), socket_init_work_fn);
	INIT_WORK(&(info->read_work), socket_read_work_fn);

	switch (info->peripheral) {
	case PERIPHERAL_MODEM:
		ins_base = MODEM_INST_BASE;
		break;
	case PERIPHERAL_LPASS:
		ins_base = LPASS_INST_BASE;
		break;
	case PERIPHERAL_WCNSS:
		ins_base = WCNSS_INST_BASE;
		break;
	case PERIPHERAL_SENSORS:
		ins_base = SENSORS_INST_BASE;
		break;
	case PERIPHERAL_WDSP:
		ins_base = WDSP_INST_BASE;
		break;
	case PERIPHERAL_CDSP:
		ins_base = CDSP_INST_BASE;
		break;
	}

	switch (info->type) {
	case TYPE_DATA:
		ins_offset = INST_ID_DATA;
		info->port_type = PORT_TYPE_SERVER;
		break;
	case TYPE_CNTL:
		ins_offset = INST_ID_CNTL;
		info->port_type = PORT_TYPE_SERVER;
		break;
	case TYPE_DCI:
		ins_offset = INST_ID_DCI;
		info->port_type = PORT_TYPE_SERVER;
		break;
	case TYPE_CMD:
		ins_offset = INST_ID_CMD;
		info->port_type = PORT_TYPE_CLIENT;
		break;
	case TYPE_DCI_CMD:
		ins_offset = INST_ID_DCI_CMD;
		info->port_type = PORT_TYPE_CLIENT;
		break;
	}

	info->svc_id = DIAG_SVC_ID;
	info->ins_id = ins_base + ins_offset;
	info->inited = 1;
}

static void cntl_socket_init_work_fn(struct work_struct *work)
{
	int ret = 0;

	if (!cntl_socket)
		return;

	ret = sock_create(AF_MSM_IPC, SOCK_DGRAM, 0, &cntl_socket->hdl);
	if (ret < 0 || !cntl_socket->hdl) {
		pr_err("diag: In %s, cntl socket is not initialized, ret: %d\n",
		       __func__, ret);
		return;
	}

	write_lock_bh(&cntl_socket->hdl->sk->sk_callback_lock);
	cntl_socket->hdl->sk->sk_user_data = (void *)cntl_socket;
	cntl_socket->hdl->sk->sk_data_ready = cntl_socket_data_ready;
	write_unlock_bh(&cntl_socket->hdl->sk->sk_callback_lock);

	ret = kernel_sock_ioctl(cntl_socket->hdl,
				IPC_ROUTER_IOCTL_BIND_CONTROL_PORT, 0);
	if (ret < 0) {
		pr_err("diag: In %s Could not bind as control port, ret: %d\n",
		       __func__, ret);
	}

	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "Initialized control sockets");
}

static int __diag_cntl_socket_init(void)
{
	cntl_socket = kzalloc(sizeof(struct diag_cntl_socket_info), GFP_KERNEL);
	if (!cntl_socket)
		return -ENOMEM;

	cntl_socket->svc_id = DIAG_SVC_ID;
	cntl_socket->ins_id = 1;
	atomic_set(&cntl_socket->data_ready, 0);
	init_waitqueue_head(&cntl_socket->read_wait_q);
	cntl_socket->wq = create_singlethread_workqueue("DIAG_CNTL_SOCKET");
	INIT_WORK(&(cntl_socket->read_work), cntl_socket_read_work_fn);
	INIT_WORK(&(cntl_socket->init_work), cntl_socket_init_work_fn);

	return 0;
}

int diag_socket_init(void)
{
	int err = 0;
	int peripheral = 0;
	struct diag_socket_info *info = NULL;

	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
		info = &socket_cntl[peripheral];
		__diag_socket_init(&socket_cntl[peripheral]);

		diagfwd_cntl_register(TRANSPORT_SOCKET, peripheral,
			(void *)info, &socket_ops, &(info->fwd_ctxt));

		__diag_socket_init(&socket_data[peripheral]);
		__diag_socket_init(&socket_cmd[peripheral]);
		__diag_socket_init(&socket_dci[peripheral]);
		__diag_socket_init(&socket_dci_cmd[peripheral]);
	}

	err = __diag_cntl_socket_init();
	if (err) {
		pr_err("diag: Unable to open control sockets, err: %d\n", err);
		goto fail;
	}

	register_ipcrtr_af_init_notifier(&socket_notify);
fail:
	return err;
}

static int socket_ready_notify(struct notifier_block *nb,
			       unsigned long action, void *data)
{
	uint8_t peripheral;
	struct diag_socket_info *info = NULL;

	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "received notification from IPCR");

	if (action != IPCRTR_AF_INIT) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
			 "action not recognized by diag %lu\n", action);
		return 0;
	}

	/* Initialize only the servers */
	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
		info = &socket_cntl[peripheral];
		queue_work(info->wq, &(info->init_work));
		info = &socket_data[peripheral];
		queue_work(info->wq, &(info->init_work));
		info = &socket_dci[peripheral];
		queue_work(info->wq, &(info->init_work));
	}
	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "Initialized all servers");

	queue_work(cntl_socket->wq, &(cntl_socket->init_work));

	return 0;
}

int diag_socket_init_peripheral(uint8_t peripheral)
{
	struct diag_socket_info *info = NULL;

	if (peripheral >= NUM_PERIPHERALS)
		return -EINVAL;

	info = &socket_data[peripheral];
	diagfwd_register(TRANSPORT_SOCKET, info->peripheral,
			 info->type, (void *)info, &socket_ops,
			 &info->fwd_ctxt);

	info = &socket_dci[peripheral];
	diagfwd_register(TRANSPORT_SOCKET, info->peripheral,
			 info->type, (void *)info, &socket_ops,
			 &info->fwd_ctxt);
	return 0;
}

static void __diag_socket_exit(struct diag_socket_info *info)
{
	if (!info)
		return;

	diagfwd_deregister(info->peripheral, info->type, (void *)info);
	info->fwd_ctxt = NULL;
	info->hdl = NULL;
	if (info->wq)
		destroy_workqueue(info->wq);

}

void diag_socket_early_exit(void)
{
	int i = 0;

	for (i = 0; i < NUM_PERIPHERALS; i++)
		__diag_socket_exit(&socket_cntl[i]);
}

void diag_socket_exit(void)
{
	int i = 0;

	for (i = 0; i < NUM_PERIPHERALS; i++) {
		__diag_socket_exit(&socket_data[i]);
		__diag_socket_exit(&socket_cmd[i]);
		__diag_socket_exit(&socket_dci[i]);
		__diag_socket_exit(&socket_dci_cmd[i]);
	}
}

static int diag_socket_read(void *ctxt, unsigned char *buf, int buf_len)
{
	int err = 0;
	int pkt_len = 0;
	int read_len = 0;
	int bytes_remaining = 0;
	int total_recd = 0;
	int loop_count = 0;
	uint8_t buf_full = 0;
	unsigned char *temp = NULL;
	struct kvec iov = {0};
	struct msghdr read_msg = {0};
	struct sockaddr_msm_ipc src_addr = {0};
	struct diag_socket_info *info = NULL;
	unsigned long flags;

	info = (struct diag_socket_info *)(ctxt);
	if (!info)
		return -ENODEV;

	if (!buf || !ctxt || buf_len <= 0)
		return -EINVAL;

	temp = buf;
	bytes_remaining = buf_len;

	err = wait_event_interruptible(info->read_wait_q,
				      (info->data_ready > 0) || (!info->hdl) ||
				      (atomic_read(&info->diag_state) == 0));
	if (err) {
		mutex_lock(&driver->diagfwd_channel_mutex);
		diagfwd_channel_read_done(info->fwd_ctxt, buf, 0);
		mutex_unlock(&driver->diagfwd_channel_mutex);
		return -ERESTARTSYS;
	}

	/*
	 * There is no need to continue reading over peripheral in this case.
	 * Release the wake source hold earlier.
	 */
	if (atomic_read(&info->diag_state) == 0) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
			 "%s closing read thread. diag state is closed\n",
			 info->name);
		mutex_lock(&driver->diagfwd_channel_mutex);
		diagfwd_channel_read_done(info->fwd_ctxt, buf, 0);
		mutex_unlock(&driver->diagfwd_channel_mutex);
		return 0;
	}

	if (!info->hdl) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s closing read thread\n",
			 info->name);
		goto fail;
	}

	do {
		loop_count++;
		iov.iov_base = temp;
		iov.iov_len = bytes_remaining;
		read_msg.msg_name = &src_addr;
		read_msg.msg_namelen = sizeof(src_addr);

		pkt_len = kernel_recvmsg(info->hdl, &read_msg, &iov, 1, 0,
					 MSG_PEEK);
		if (pkt_len <= 0)
			break;

		if (pkt_len > bytes_remaining) {
			buf_full = 1;
			break;
		}

		spin_lock_irqsave(&info->lock, flags);
		info->data_ready--;
		spin_unlock_irqrestore(&info->lock, flags);

		read_len = kernel_recvmsg(info->hdl, &read_msg, &iov, 1,
					  pkt_len, 0);
		if (read_len <= 0)
			goto fail;

		if (!atomic_read(&info->opened) &&
		    info->port_type == PORT_TYPE_SERVER) {
			/*
			 * This is the first packet from the client. Copy its
			 * address to the connection object. Consider this
			 * channel open for communication.
			 */
			memcpy(&info->remote_addr, &src_addr, sizeof(src_addr));
			if (info->ins_id == INST_ID_DCI)
				atomic_set(&info->opened, 1);
			else
				__socket_open_channel(info);
		}

		if (read_len < 0) {
			pr_err_ratelimited("diag: In %s, error receiving data, err: %d\n",
					   __func__, pkt_len);
			err = read_len;
			goto fail;
		}
		temp += read_len;
		total_recd += read_len;
		bytes_remaining -= read_len;
	} while (info->data_ready > 0);

	if (buf_full || (info->type == TYPE_DATA && pkt_len))
		err = queue_work(info->wq, &(info->read_work));

	if (total_recd > 0) {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s read total bytes: %d\n",
			 info->name, total_recd);
		mutex_lock(&driver->diagfwd_channel_mutex);
		err = diagfwd_channel_read_done(info->fwd_ctxt,
						buf, total_recd);
		mutex_unlock(&driver->diagfwd_channel_mutex);
		if (err)
			goto fail;
	} else {
		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s error in read, err: %d\n",
			 info->name, total_recd);
		goto fail;
	}

	diag_socket_queue_read(info);
	return 0;

fail:
	mutex_lock(&driver->diagfwd_channel_mutex);
	diagfwd_channel_read_done(info->fwd_ctxt, buf, 0);
	mutex_unlock(&driver->diagfwd_channel_mutex);
	return -EIO;
}

static int diag_socket_write(void *ctxt, unsigned char *buf, int len)
{
	int err = 0;
	int write_len = 0;
	struct kvec iov = {0};
	struct msghdr write_msg = {0};
	struct diag_socket_info *info = NULL;

	if (!ctxt || !buf || len <= 0)
		return -EIO;

	info = (struct diag_socket_info *)(ctxt);
	if (!atomic_read(&info->opened) || !info->hdl)
		return -ENODEV;

	iov.iov_base = buf;
	iov.iov_len = len;
	write_msg.msg_name = &info->remote_addr;
	write_msg.msg_namelen = sizeof(info->remote_addr);
	write_msg.msg_flags |= MSG_DONTWAIT;
	write_len = kernel_sendmsg(info->hdl, &write_msg, &iov, 1, len);
	if (write_len < 0) {
		err = write_len;
		/*
		 * -EAGAIN means that the number of packets in flight is at
		 * max capactity and the peripheral hasn't read the data.
		 */
		if (err != -EAGAIN) {
			pr_err_ratelimited("diag: In %s, error sending data, err: %d, ch: %s\n",
					   __func__, err, info->name);
		}
	} else if (write_len != len) {
		err = write_len;
		pr_err_ratelimited("diag: In %s, wrote partial packet to %s, len: %d, wrote: %d\n",
				   __func__, info->name, len, write_len);
	}

	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s wrote to socket, len: %d\n",
		 info->name, write_len);

	return err;
}