/* Copyright (c) 2008-2014, 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/delay.h>
#include <linux/diagchar.h>
#include <linux/kmemleak.h>
#include <linux/workqueue.h>
#include <linux/uaccess.h>
#include "diagchar.h"
#include "diagfwd_cntl.h"
#include "diag_masks.h"

#define ALL_EQUIP_ID		100
#define ALL_SSID		-1

#define DIAG_SET_FEATURE_MASK(x) (feature_bytes[(x)/8] |= (1 << (x & 0x7)))

struct diag_mask_info msg_mask;
struct diag_mask_info msg_bt_mask;
struct diag_mask_info log_mask;
struct diag_mask_info event_mask;

static const struct diag_ssid_range_t msg_mask_tbl[] = {
	{ .ssid_first = MSG_SSID_0, .ssid_last = MSG_SSID_0_LAST },
	{ .ssid_first = MSG_SSID_1, .ssid_last = MSG_SSID_1_LAST },
	{ .ssid_first = MSG_SSID_2, .ssid_last = MSG_SSID_2_LAST },
	{ .ssid_first = MSG_SSID_3, .ssid_last = MSG_SSID_3_LAST },
	{ .ssid_first = MSG_SSID_4, .ssid_last = MSG_SSID_4_LAST },
	{ .ssid_first = MSG_SSID_5, .ssid_last = MSG_SSID_5_LAST },
	{ .ssid_first = MSG_SSID_6, .ssid_last = MSG_SSID_6_LAST },
	{ .ssid_first = MSG_SSID_7, .ssid_last = MSG_SSID_7_LAST },
	{ .ssid_first = MSG_SSID_8, .ssid_last = MSG_SSID_8_LAST },
	{ .ssid_first = MSG_SSID_9, .ssid_last = MSG_SSID_9_LAST },
	{ .ssid_first = MSG_SSID_10, .ssid_last = MSG_SSID_10_LAST },
	{ .ssid_first = MSG_SSID_11, .ssid_last = MSG_SSID_11_LAST },
	{ .ssid_first = MSG_SSID_12, .ssid_last = MSG_SSID_12_LAST },
	{ .ssid_first = MSG_SSID_13, .ssid_last = MSG_SSID_13_LAST },
	{ .ssid_first = MSG_SSID_14, .ssid_last = MSG_SSID_14_LAST },
	{ .ssid_first = MSG_SSID_15, .ssid_last = MSG_SSID_15_LAST },
	{ .ssid_first = MSG_SSID_16, .ssid_last = MSG_SSID_16_LAST },
	{ .ssid_first = MSG_SSID_17, .ssid_last = MSG_SSID_17_LAST },
	{ .ssid_first = MSG_SSID_18, .ssid_last = MSG_SSID_18_LAST },
	{ .ssid_first = MSG_SSID_19, .ssid_last = MSG_SSID_19_LAST },
	{ .ssid_first = MSG_SSID_20, .ssid_last = MSG_SSID_20_LAST },
	{ .ssid_first = MSG_SSID_21, .ssid_last = MSG_SSID_21_LAST },
	{ .ssid_first = MSG_SSID_22, .ssid_last = MSG_SSID_22_LAST },
	{ .ssid_first = MSG_SSID_23, .ssid_last = MSG_SSID_23_LAST },
	{ .ssid_first = MSG_SSID_24, .ssid_last = MSG_SSID_24_LAST }
};

static int diag_apps_responds(void)
{
	/*
	 * Apps processor should respond to mask commands only if the
	 * Modem channel is up, the feature mask is received from Modem
	 * and if Modem supports Mask Centralization.
	 */
	if (chk_apps_only()) {
		if (driver->smd_data[MODEM_DATA].ch &&
			driver->rcvd_feature_mask[MODEM_DATA]) {
			if (driver->mask_centralization[MODEM_DATA])
				return 1;
			return 0;
		}
		return 1;
	}
	return 0;
}

static void diag_send_log_mask_update(struct diag_smd_info *smd_info,
				      int equip_id)
{
	int i;
	int err = 0;
	int send_once = 0;
	int header_len = sizeof(struct diag_ctrl_log_mask);
	uint8_t *buf = log_mask.update_buf;
	uint8_t *temp = NULL;
	uint32_t mask_size = 0;
	struct diag_ctrl_log_mask ctrl_pkt;
	struct diag_log_mask_t *mask = (struct diag_log_mask_t *)log_mask.ptr;

	if (!smd_info)
		return;

	if (!smd_info->ch) {
		pr_debug("diag: In %s, SMD channel is closed, peripheral: %d\n",
			 __func__, smd_info->peripheral);
		return;
	}

	switch (log_mask.status) {
	case DIAG_CTRL_MASK_ALL_DISABLED:
		ctrl_pkt.equip_id = 0;
		ctrl_pkt.num_items = 0;
		ctrl_pkt.log_mask_size = 0;
		send_once = 1;
		break;
	case DIAG_CTRL_MASK_ALL_ENABLED:
		ctrl_pkt.equip_id = 0;
		ctrl_pkt.num_items = 0;
		ctrl_pkt.log_mask_size = 0;
		send_once = 1;
		break;
	case DIAG_CTRL_MASK_VALID:
		send_once = 0;
		break;
	default:
		pr_debug("diag: In %s, invalid log_mask status\n", __func__);
		return;
	}

	mutex_lock(&log_mask.lock);
	for (i = 0; i < MAX_EQUIP_ID; i++, mask++) {
		if (equip_id != i && equip_id != ALL_EQUIP_ID)
			continue;

		ctrl_pkt.cmd_type = DIAG_CTRL_MSG_LOG_MASK;
		ctrl_pkt.stream_id = 1;
		ctrl_pkt.status = log_mask.status;
		if (log_mask.status == DIAG_CTRL_MASK_VALID) {
			mask_size = LOG_ITEMS_TO_SIZE(mask->num_items);
			ctrl_pkt.equip_id = i;
			ctrl_pkt.num_items = mask->num_items;
			ctrl_pkt.log_mask_size = mask_size;
		}
		ctrl_pkt.data_len = LOG_MASK_CTRL_HEADER_LEN + mask_size;

		if (header_len + mask_size > log_mask.update_buf_len) {
			temp = krealloc(buf, header_len + mask_size,
					GFP_KERNEL);
			if (!temp) {
				pr_err("diag: Unable to realloc log update buffer, new size: %d, equip_id: %d\n",
				       header_len + mask_size, equip_id);
				break;
			}
			log_mask.update_buf = temp;
			log_mask.update_buf_len = header_len + mask_size;
		}

		memcpy(buf, &ctrl_pkt, header_len);
		if (mask_size > 0)
			memcpy(buf + header_len, mask->ptr, mask_size);

		err = diag_smd_write(smd_info, buf, header_len + mask_size);
		if (err) {
			pr_err("diag: Unable to send log masks to peripheral %d, equip_id: %d, err: %d\n",
			       smd_info->peripheral, i, err);
		}
		if (send_once || equip_id != ALL_EQUIP_ID)
			break;

	}
	mutex_unlock(&log_mask.lock);
}

static void diag_send_event_mask_update(struct diag_smd_info *smd_info)
{
	uint8_t *buf = event_mask.update_buf;
	uint8_t *temp = NULL;
	struct diag_ctrl_event_mask header;
	int num_bytes = EVENT_COUNT_TO_BYTES(driver->last_event_id);
	int write_len = 0;
	int err = 0;
	int temp_len = 0;

	if (num_bytes <= 0 || num_bytes > driver->event_mask_size) {
		pr_debug("diag: In %s, invalid event mask length %d\n",
			 __func__, num_bytes);
		return;
	}

	if (!smd_info)
		return;

	if (!smd_info->ch) {
		pr_debug("diag: In %s, SMD channel is closed, peripheral: %d\n",
			 __func__, smd_info->peripheral);
		return;
	}

	mutex_lock(&event_mask.lock);
	header.cmd_type = DIAG_CTRL_MSG_EVENT_MASK;
	header.stream_id = 1;
	header.status = event_mask.status;

	switch (event_mask.status) {
	case DIAG_CTRL_MASK_ALL_DISABLED:
		header.event_config = 0;
		header.event_mask_size = 0;
		break;
	case DIAG_CTRL_MASK_ALL_ENABLED:
		header.event_config = 1;
		header.event_mask_size = 0;
		break;
	case DIAG_CTRL_MASK_VALID:
		header.event_config = 1;
		header.event_mask_size = num_bytes;
		if (num_bytes + sizeof(header) > event_mask.update_buf_len) {
			temp_len = num_bytes + sizeof(header);
			temp = krealloc(buf, temp_len, GFP_KERNEL);
			if (!temp) {
				pr_err("diag: Unable to realloc event mask update buffer\n");
				goto err;
			} else {
				event_mask.update_buf = temp;
				event_mask.update_buf_len = temp_len;
			}
		}
		memcpy(buf + sizeof(header), event_mask.ptr, num_bytes);
		write_len += num_bytes;
		break;
	default:
		pr_debug("diag: In %s, invalid status %d\n", __func__,
			 event_mask.status);
		goto err;
	}
	header.data_len = EVENT_MASK_CTRL_HEADER_LEN + header.event_mask_size;
	memcpy(buf, &header, sizeof(header));
	write_len += sizeof(header);

	err = diag_smd_write(smd_info, buf, write_len);
	if (err) {
		pr_err("diag: Unable to send event masks to peripheral %d\n",
		       smd_info->peripheral);
	}
err:
	mutex_unlock(&event_mask.lock);
}

static void diag_send_msg_mask_update(struct diag_smd_info *smd_info,
				      int first, int last)
{
	int i;
	int err = 0;
	int header_len = sizeof(struct diag_ctrl_msg_mask);
	int temp_len = 0;
	uint8_t *buf = msg_mask.update_buf;
	uint8_t *temp = NULL;
	uint32_t mask_size = 0;
	struct diag_msg_mask_t *mask = (struct diag_msg_mask_t *)msg_mask.ptr;
	struct diag_ctrl_msg_mask header;

	if (!smd_info)
		return;

	if (!smd_info->ch) {
		pr_debug("diag: In %s, SMD channel is closed, peripheral: %d\n",
			 __func__, smd_info->peripheral);
		return;
	}

	mutex_lock(&msg_mask.lock);
	switch (msg_mask.status) {
	case DIAG_CTRL_MASK_ALL_DISABLED:
		mask_size = 0;
		break;
	case DIAG_CTRL_MASK_ALL_ENABLED:
		mask_size = 1;
		break;
	case DIAG_CTRL_MASK_VALID:
		break;
	default:
		pr_debug("diag: In %s, invalid status: %d\n", __func__,
			 msg_mask.status);
		goto err;
	}

	for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
		if (((first < mask->ssid_first) || (last > mask->ssid_last)) &&
							first != ALL_SSID) {
			continue;
		}

		if (msg_mask.status == DIAG_CTRL_MASK_VALID) {
			mask_size = mask->ssid_last - mask->ssid_first + 1;
			temp_len = mask_size * sizeof(uint32_t);
			if (temp_len + header_len <= msg_mask.update_buf_len)
				goto proceed;
			temp = krealloc(msg_mask.update_buf, temp_len,
					GFP_KERNEL);
			if (!temp) {
				pr_err("diag: In %s, unable to realloc msg_mask update buffer\n",
				       __func__);
				mask_size = (msg_mask.update_buf_len -
					    header_len) / sizeof(uint32_t);
			} else {
				msg_mask.update_buf = temp;
				msg_mask.update_buf_len = temp_len;
				pr_debug("diag: In %s, successfully reallocated msg_mask update buffer to len: %d\n",
					 __func__, msg_mask.update_buf_len);
			}
		}
proceed:
		header.cmd_type = DIAG_CTRL_MSG_F3_MASK;
		header.status = msg_mask.status;
		header.stream_id = 1;
		header.msg_mode = 0;
		header.ssid_first = mask->ssid_first;
		header.ssid_last = mask->ssid_last;
		header.msg_mask_size = mask_size;
		mask_size *= sizeof(uint32_t);
		header.data_len = MSG_MASK_CTRL_HEADER_LEN + mask_size;
		memcpy(buf, &header, header_len);
		if (mask_size > 0)
			memcpy(buf + header_len, mask->ptr, mask_size);

		err = diag_smd_write(smd_info, buf, header_len + mask_size);
		if (err) {
			pr_err("diag: Unable to send msg masks to peripheral %d\n",
			       smd_info->peripheral);
		}

		if (first != ALL_SSID)
			break;
	}
err:
	mutex_unlock(&msg_mask.lock);
}

static void diag_send_feature_mask_update(struct diag_smd_info *smd_info)
{
	void *buf = driver->buf_feature_mask_update;
	int header_size = sizeof(struct diag_ctrl_feature_mask);
	uint8_t feature_bytes[FEATURE_MASK_LEN] = {0, 0};
	struct diag_ctrl_feature_mask feature_mask;
	int total_len = 0;
	int err = 0;

	if (!smd_info) {
		pr_err("diag: In %s, null smd info pointer\n",
			__func__);
		return;
	}

	if (!smd_info->ch) {
		pr_err("diag: In %s, smd channel not open for peripheral: %d, type: %d\n",
				__func__, smd_info->peripheral, smd_info->type);
		return;
	}

	mutex_lock(&driver->diag_cntl_mutex);
	/* send feature mask update */
	feature_mask.ctrl_pkt_id = DIAG_CTRL_MSG_FEATURE;
	feature_mask.ctrl_pkt_data_len = sizeof(uint32_t) + FEATURE_MASK_LEN;
	feature_mask.feature_mask_len = FEATURE_MASK_LEN;
	memcpy(buf, &feature_mask, header_size);
	DIAG_SET_FEATURE_MASK(F_DIAG_FEATURE_MASK_SUPPORT);
	DIAG_SET_FEATURE_MASK(F_DIAG_LOG_ON_DEMAND_APPS);
	DIAG_SET_FEATURE_MASK(F_DIAG_STM);
	if (driver->supports_separate_cmdrsp)
		DIAG_SET_FEATURE_MASK(F_DIAG_REQ_RSP_SUPPORT);
	if (driver->supports_apps_hdlc_encoding)
		DIAG_SET_FEATURE_MASK(F_DIAG_APPS_HDLC_ENCODE);
	DIAG_SET_FEATURE_MASK(F_DIAG_MASK_CENTRALIZATION);
	memcpy(buf + header_size, &feature_bytes, FEATURE_MASK_LEN);
	total_len = header_size + FEATURE_MASK_LEN;

	err = diag_smd_write(smd_info, buf, total_len);
	if (err) {
		pr_err("diag: In %s, unable to write to smd, peripheral: %d, type: %d, len: %d, err: %d\n",
		       __func__, smd_info->peripheral, smd_info->type,
		       total_len, err);
	}
	mutex_unlock(&driver->diag_cntl_mutex);
}

static int diag_cmd_get_ssid_range(unsigned char *src_buf, int src_len,
				   unsigned char *dest_buf, int dest_len)
{
	int i;
	int write_len = 0;
	struct diag_msg_mask_t *mask_ptr = NULL;
	struct diag_msg_ssid_query_t rsp;
	struct diag_ssid_range_t ssid_range;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	if (!diag_apps_responds())
		return 0;

	rsp.cmd_code = DIAG_CMD_MSG_CONFIG;
	rsp.sub_cmd = DIAG_CMD_OP_GET_SSID_RANGE;
	rsp.status = MSG_STATUS_SUCCESS;
	rsp.padding = 0;
	rsp.count = driver->msg_mask_tbl_count;
	memcpy(dest_buf, &rsp, sizeof(rsp));
	write_len += sizeof(rsp);

	mask_ptr = (struct diag_msg_mask_t *)msg_mask.ptr;
	for (i = 0; i <  driver->msg_mask_tbl_count; i++, mask_ptr++) {
		if (write_len + sizeof(ssid_range) > dest_len) {
			pr_err("diag: In %s, Truncating response due to size limitations of rsp buffer\n",
			       __func__);
			break;
		}
		ssid_range.ssid_first = mask_ptr->ssid_first;
		ssid_range.ssid_last = mask_ptr->ssid_last;
		memcpy(dest_buf + write_len, &ssid_range, sizeof(ssid_range));
		write_len += sizeof(ssid_range);
	}

	return write_len;
}

static int diag_cmd_get_build_mask(unsigned char *src_buf, int src_len,
				   unsigned char *dest_buf, int dest_len)
{
	int i = 0;
	int write_len = 0;
	int num_entries = 0;
	int copy_len = 0;
	struct diag_msg_mask_t *build_mask = NULL;
	struct diag_build_mask_req_t *req = NULL;
	struct diag_msg_build_mask_t rsp;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	if (!diag_apps_responds())
		return 0;

	req = (struct diag_build_mask_req_t *)src_buf;
	rsp.cmd_code = DIAG_CMD_MSG_CONFIG;
	rsp.sub_cmd = DIAG_CMD_OP_GET_BUILD_MASK;
	rsp.ssid_first = req->ssid_first;
	rsp.ssid_last = req->ssid_last;
	rsp.status = MSG_STATUS_FAIL;
	rsp.padding = 0;

	build_mask = (struct diag_msg_mask_t *)msg_bt_mask.ptr;
	for (i = 0; i < driver->msg_mask_tbl_count; i++, build_mask++) {
		if (build_mask->ssid_first != req->ssid_first)
			continue;
		num_entries = req->ssid_last - req->ssid_first + 1;
		if (num_entries > build_mask->range) {
			pr_warn("diag: In %s, truncating ssid range for ssid_first: %d ssid_last %d\n",
				__func__, req->ssid_first, req->ssid_last);
			num_entries = build_mask->range;
			req->ssid_last = req->ssid_first + build_mask->range;
		}
		copy_len = num_entries * sizeof(uint32_t);
		if (copy_len + sizeof(rsp) > dest_len)
			copy_len = dest_len - sizeof(rsp);
		memcpy(dest_buf + sizeof(rsp), build_mask->ptr, copy_len);
		write_len += copy_len;
		rsp.ssid_last = build_mask->ssid_last;
		rsp.status = MSG_STATUS_SUCCESS;
		break;
	}
	memcpy(dest_buf, &rsp, sizeof(rsp));
	write_len += sizeof(rsp);

	return write_len;
}

static int diag_cmd_get_msg_mask(unsigned char *src_buf, int src_len,
				 unsigned char *dest_buf, int dest_len)
{
	int i;
	int write_len = 0;
	uint32_t mask_size = 0;
	struct diag_msg_mask_t *mask = NULL;
	struct diag_build_mask_req_t *req = NULL;
	struct diag_msg_build_mask_t rsp;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	if (!diag_apps_responds())
		return 0;

	req = (struct diag_build_mask_req_t *)src_buf;
	rsp.cmd_code = DIAG_CMD_MSG_CONFIG;
	rsp.sub_cmd = DIAG_CMD_OP_GET_MSG_MASK;
	rsp.ssid_first = req->ssid_first;
	rsp.ssid_last = req->ssid_last;
	rsp.status = MSG_STATUS_FAIL;
	rsp.padding = 0;

	mask = (struct diag_msg_mask_t *)msg_mask.ptr;
	for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
		if ((req->ssid_first < mask->ssid_first) ||
		    (req->ssid_first > mask->ssid_last)) {
			continue;
		}
		mask_size = mask->range * sizeof(uint32_t);
		/* Copy msg mask only till the end of the rsp buffer */
		if (mask_size + sizeof(rsp) > dest_len)
			mask_size = dest_len - sizeof(rsp);
		memcpy(dest_buf + sizeof(rsp), mask->ptr, mask_size);
		write_len += mask_size;
		rsp.status = MSG_STATUS_SUCCESS;
		break;
	}
	memcpy(dest_buf, &rsp, sizeof(rsp));
	write_len += sizeof(rsp);

	return write_len;
}

static int diag_cmd_set_msg_mask(unsigned char *src_buf, int src_len,
				 unsigned char *dest_buf, int dest_len)
{
	int i;
	int write_len = 0;
	int header_len = sizeof(struct diag_msg_build_mask_t);
	int found = 0;
	uint32_t mask_size = 0;
	uint32_t offset = 0;
	struct diag_msg_mask_t *mask = NULL;
	struct diag_msg_build_mask_t *req = NULL;
	struct diag_msg_build_mask_t rsp;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	req = (struct diag_msg_build_mask_t *)src_buf;

	mutex_lock(&msg_mask.lock);
	mask = (struct diag_msg_mask_t *)msg_mask.ptr;
	for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
		if ((req->ssid_first < mask->ssid_first) ||
		    (req->ssid_first > mask->ssid_last)) {
			continue;
		}
		found = 1;
		if (req->ssid_last > mask->ssid_last) {
			pr_debug("diag: Msg SSID range mismatch\n");
			mask->ssid_last = req->ssid_last;
		}
		mask_size = req->ssid_last - req->ssid_first + 1;
		if (mask_size > mask->range) {
			pr_warn("diag: In %s, truncating ssid range, %d-%d to max allowed: %d\n",
				__func__, mask->ssid_first, mask->ssid_last,
				mask->range);
			mask_size = mask->range;
			mask->ssid_last = mask->ssid_first + mask->range;
		}
		offset = req->ssid_first - mask->ssid_first;
		if (offset + mask_size > mask->range) {
			pr_err("diag: In %s, Not enough space for msg mask, mask_size: %d\n",
			       __func__, mask_size);
			break;
		}
		mask_size = mask_size * sizeof(uint32_t);
		memcpy(mask->ptr + offset, src_buf + header_len, mask_size);
		msg_mask.status = DIAG_CTRL_MASK_VALID;
		break;
	}
	mutex_unlock(&msg_mask.lock);

	diag_update_userspace_clients(MSG_MASKS_TYPE);

	/*
	 * Apps processor must send the response to this command. Frame the
	 * response.
	 */
	rsp.cmd_code = DIAG_CMD_MSG_CONFIG;
	rsp.sub_cmd = DIAG_CMD_OP_SET_MSG_MASK;
	rsp.ssid_first = req->ssid_first;
	rsp.ssid_last = req->ssid_last;
	rsp.status = found;
	rsp.padding = 0;
	memcpy(dest_buf, &rsp, header_len);
	write_len += header_len;
	if (!found)
		goto end;
	if (mask_size + write_len > dest_len)
		mask_size = dest_len - write_len;
	memcpy(dest_buf + write_len, src_buf + header_len, mask_size);
	write_len += mask_size;
	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
		diag_send_msg_mask_update(&driver->smd_cntl[i],
					  req->ssid_first, req->ssid_last);
	}
end:
	return write_len;
}

static int diag_cmd_set_all_msg_mask(unsigned char *src_buf, int src_len,
				     unsigned char *dest_buf, int dest_len)
{
	int i;
	int write_len = 0;
	int header_len = sizeof(struct diag_msg_config_rsp_t);
	struct diag_msg_config_rsp_t rsp;
	struct diag_msg_config_rsp_t *req = NULL;
	struct diag_msg_mask_t *mask = (struct diag_msg_mask_t *)msg_mask.ptr;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	req = (struct diag_msg_config_rsp_t *)src_buf;

	mutex_lock(&msg_mask.lock);
	msg_mask.status = (req->rt_mask) ? DIAG_CTRL_MASK_ALL_ENABLED :
					   DIAG_CTRL_MASK_ALL_DISABLED;
	for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
		memset(mask->ptr, req->rt_mask,
		       mask->range * sizeof(uint32_t));
	}
	mutex_unlock(&msg_mask.lock);

	diag_update_userspace_clients(MSG_MASKS_TYPE);

	/*
	 * Apps processor must send the response to this command. Frame the
	 * response.
	 */
	rsp.cmd_code = DIAG_CMD_MSG_CONFIG;
	rsp.sub_cmd = DIAG_CMD_OP_SET_ALL_MSG_MASK;
	rsp.status = MSG_STATUS_SUCCESS;
	rsp.padding = 0;
	rsp.rt_mask = req->rt_mask;
	memcpy(dest_buf, &rsp, header_len);
	write_len += header_len;

	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
		diag_send_msg_mask_update(&driver->smd_cntl[i], ALL_SSID,
					  ALL_SSID);
	}

	return write_len;
}

static int diag_cmd_get_event_mask(unsigned char *src_buf, int src_len,
				   unsigned char *dest_buf, int dest_len)
{
	int write_len = 0;
	uint32_t mask_size;
	struct diag_event_mask_config_t rsp;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	if (!diag_apps_responds())
		return 0;

	mask_size = EVENT_COUNT_TO_BYTES(driver->last_event_id);
	if (mask_size + sizeof(rsp) > dest_len) {
		pr_err("diag: In %s, invalid mask size: %d\n", __func__,
		       mask_size);
		return -ENOMEM;
	}

	rsp.cmd_code = DIAG_CMD_GET_EVENT_MASK;
	rsp.status = EVENT_STATUS_SUCCESS;
	rsp.padding = 0;
	rsp.num_bits = driver->last_event_id + 1;
	memcpy(dest_buf, &rsp, sizeof(rsp));
	write_len += sizeof(rsp);
	memcpy(dest_buf + write_len, event_mask.ptr, mask_size);
	write_len += mask_size;

	return write_len;
}

static int diag_cmd_update_event_mask(unsigned char *src_buf, int src_len,
				      unsigned char *dest_buf, int dest_len)
{
	int i;
	int write_len = 0;
	int mask_len = 0;
	int header_len = sizeof(struct diag_event_mask_config_t);
	struct diag_event_mask_config_t rsp;
	struct diag_event_mask_config_t *req;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	req = (struct diag_event_mask_config_t *)src_buf;
	mask_len = EVENT_COUNT_TO_BYTES(req->num_bits);
	if (mask_len <= 0 || mask_len > event_mask.mask_len) {
		pr_err("diag: In %s, invalid event mask len: %d\n", __func__,
		       mask_len);
		return -EIO;
	}

	mutex_lock(&event_mask.lock);
	memcpy(event_mask.ptr, src_buf + header_len, mask_len);
	event_mask.status = DIAG_CTRL_MASK_VALID;
	mutex_unlock(&event_mask.lock);
	diag_update_userspace_clients(EVENT_MASKS_TYPE);

	/*
	 * Apps processor must send the response to this command. Frame the
	 * response.
	 */
	rsp.cmd_code = DIAG_CMD_SET_EVENT_MASK;
	rsp.status = EVENT_STATUS_SUCCESS;
	rsp.padding = 0;
	rsp.num_bits = driver->last_event_id + 1;
	memcpy(dest_buf, &rsp, header_len);
	write_len += header_len;
	memcpy(dest_buf + write_len, event_mask.ptr, mask_len);
	write_len += mask_len;

	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		diag_send_event_mask_update(&driver->smd_cntl[i]);

	return write_len;
}

static int diag_cmd_toggle_events(unsigned char *src_buf, int src_len,
				  unsigned char *dest_buf, int dest_len)
{
	int i;
	int write_len = 0;
	uint8_t toggle = 0;
	struct diag_event_report_t header;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	toggle = *(src_buf + 1);
	mutex_lock(&event_mask.lock);
	if (toggle) {
		event_mask.status = DIAG_CTRL_MASK_ALL_ENABLED;
		memset(event_mask.ptr, 0xFF, event_mask.mask_len);
	} else {
		event_mask.status = DIAG_CTRL_MASK_ALL_DISABLED;
		memset(event_mask.ptr, 0, event_mask.mask_len);
	}
	mutex_unlock(&event_mask.lock);
	diag_update_userspace_clients(EVENT_MASKS_TYPE);

	/*
	 * Apps processor must send the response to this command. Frame the
	 * response.
	 */
	header.cmd_code = DIAG_CMD_EVENT_TOGGLE;
	header.padding = 0;
	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		diag_send_event_mask_update(&driver->smd_cntl[i]);
	memcpy(dest_buf, &header, sizeof(header));
	write_len += sizeof(header);

	return write_len;
}

static int diag_cmd_log_on_demand(unsigned char *src_buf, int src_len,
				  unsigned char *dest_buf, int dest_len)
{
	int write_len = 0;
	struct diag_log_on_demand_rsp_t header;

	if (driver->smd_cntl[MODEM_DATA].ch && !driver->log_on_demand_support)
		return 0;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	header.cmd_code = DIAG_CMD_LOG_ON_DMND;
	header.log_code = *(uint16_t *)(src_buf + 1);
	header.status = 1;
	memcpy(dest_buf, &header, sizeof(struct diag_log_on_demand_rsp_t));
	write_len += sizeof(struct diag_log_on_demand_rsp_t);

	return write_len;
}

static int diag_cmd_get_log_mask(unsigned char *src_buf, int src_len,
				 unsigned char *dest_buf, int dest_len)
{
	int i;
	int status = LOG_STATUS_INVALID;
	int write_len = 0;
	int read_len = 0;
	int req_header_len = sizeof(struct diag_log_config_req_t);
	int rsp_header_len = sizeof(struct diag_log_config_rsp_t);
	uint32_t mask_size = 0;
	struct diag_log_mask_t *log_item = NULL;
	struct diag_log_config_req_t *req;
	struct diag_log_config_rsp_t rsp;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	if (!diag_apps_responds())
		return 0;

	req = (struct diag_log_config_req_t *)src_buf;
	read_len += req_header_len;

	rsp.cmd_code = DIAG_CMD_LOG_CONFIG;
	rsp.padding[0] = 0;
	rsp.padding[1] = 0;
	rsp.padding[2] = 0;
	rsp.sub_cmd = DIAG_CMD_OP_GET_LOG_MASK;
	/*
	 * Don't copy the response header now. Copy at the end after
	 * calculating the status field value
	 */
	write_len += rsp_header_len;

	log_item = (struct diag_log_mask_t *)log_mask.ptr;
	for (i = 0; i < MAX_EQUIP_ID; i++, log_item++) {
		if (log_item->equip_id != req->equip_id)
			continue;
		mask_size = LOG_ITEMS_TO_SIZE(log_item->num_items);
		/*
		 * Make sure we have space to fill the response in the buffer.
		 * Destination buffer should atleast be able to hold equip_id
		 * (uint32_t), num_items(uint32_t), mask (mask_size) and the
		 * response header.
		 */
		if ((mask_size + (2 * sizeof(uint32_t)) + rsp_header_len) >
								dest_len) {
			pr_err("diag: In %s, invalid length: %d, max rsp_len: %d\n",
				__func__, mask_size, dest_len);
			status = LOG_STATUS_FAIL;
			break;
		}
		*(uint32_t *)(dest_buf + write_len) = log_item->equip_id;
		write_len += sizeof(uint32_t);
		*(uint32_t *)(dest_buf + write_len) = log_item->num_items;
		write_len += sizeof(uint32_t);
		if (mask_size > 0) {
			memcpy(dest_buf + write_len, log_item->ptr, mask_size);
			write_len += mask_size;
		}
		status = LOG_STATUS_SUCCESS;
		break;
	}

	rsp.status = status;
	memcpy(dest_buf, &rsp, rsp_header_len);

	return write_len;
}

static int diag_cmd_get_log_range(unsigned char *src_buf, int src_len,
				  unsigned char *dest_buf, int dest_len)
{
	int i;
	int write_len = 0;
	struct diag_log_config_rsp_t rsp;
	struct diag_log_mask_t *mask = (struct diag_log_mask_t *)log_mask.ptr;

	if (!diag_apps_responds())
		return 0;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	rsp.cmd_code = DIAG_CMD_LOG_CONFIG;
	rsp.padding[0] = 0;
	rsp.padding[1] = 0;
	rsp.padding[2] = 0;
	rsp.sub_cmd = DIAG_CMD_OP_GET_LOG_RANGE;
	rsp.status = LOG_STATUS_SUCCESS;
	memcpy(dest_buf, &rsp, sizeof(rsp));
	write_len += sizeof(rsp);

	for (i = 0; i < MAX_EQUIP_ID && write_len < dest_len; i++, mask++) {
		*(uint32_t *)(dest_buf + write_len) = mask->num_items;
		write_len += sizeof(uint32_t);
	}

	return write_len;
}

static int diag_cmd_set_log_mask(unsigned char *src_buf, int src_len,
				 unsigned char *dest_buf, int dest_len)
{
	int i;
	int write_len = 0;
	int status = LOG_STATUS_SUCCESS;
	int read_len = 0;
	int payload_len = 0;
	int req_header_len = sizeof(struct diag_log_config_req_t);
	int rsp_header_len = sizeof(struct diag_log_config_set_rsp_t);
	uint32_t mask_size = 0;
	struct diag_log_mask_t *mask = (struct diag_log_mask_t *)log_mask.ptr;
	struct diag_log_config_req_t *req;
	struct diag_log_config_set_rsp_t rsp;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	req = (struct diag_log_config_req_t *)src_buf;
	read_len += req_header_len;

	if (req->equip_id >= MAX_EQUIP_ID) {
		pr_err("diag: In %s, Invalid logging mask request, equip_id: %d\n",
		       __func__, req->equip_id);
		status = LOG_STATUS_INVALID;
	}
	mutex_lock(&log_mask.lock);
	for (i = 0; i < MAX_EQUIP_ID && !status; i++, mask++) {
		if (mask->equip_id != req->equip_id)
			continue;
		if (mask->num_items < req->num_items)
			mask->num_items = req->num_items;
		mask_size = LOG_ITEMS_TO_SIZE(req->num_items);
		if (mask_size > mask->range) {
			/*
			 * If the size of the log mask cannot fit into our
			 * buffer, trim till we have space left in the buffer.
			 * num_items should then reflect the items that we have
			 * in our buffer.
			 */
			mask_size = mask->range;
			mask->num_items = LOG_SIZE_TO_ITEMS(mask_size);
			req->num_items = mask->num_items;
		}
		if (mask_size > 0)
			memcpy(mask->ptr, src_buf + read_len, mask_size);
		log_mask.status = DIAG_CTRL_MASK_VALID;
		break;
	}
	mutex_unlock(&log_mask.lock);
	diag_update_userspace_clients(LOG_MASKS_TYPE);

	/*
	 * Apps processor must send the response to this command. Frame the
	 * response.
	 */
	payload_len = LOG_ITEMS_TO_SIZE(req->num_items);
	if (payload_len + rsp_header_len > dest_len) {
		pr_err("diag: In %s, invalid length, payload_len: %d, header_len: %d, dest_len: %d\n",
		       __func__, payload_len, rsp_header_len , dest_len);
		status = LOG_STATUS_FAIL;
	}
	rsp.cmd_code = DIAG_CMD_LOG_CONFIG;
	rsp.padding[0] = 0;
	rsp.padding[1] = 0;
	rsp.padding[2] = 0;
	rsp.sub_cmd = DIAG_CMD_OP_SET_LOG_MASK;
	rsp.status = status;
	rsp.equip_id = req->equip_id;
	rsp.num_items = req->num_items;
	memcpy(dest_buf, &rsp, rsp_header_len);
	write_len += rsp_header_len;
	if (status != LOG_STATUS_SUCCESS)
		goto end;
	memcpy(dest_buf + write_len, src_buf + read_len, payload_len);
	write_len += payload_len;

	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		diag_send_log_mask_update(&driver->smd_cntl[i], req->equip_id);
end:
	return write_len;
}

static int diag_cmd_disable_log_mask(unsigned char *src_buf, int src_len,
				     unsigned char *dest_buf, int dest_len)
{
	struct diag_log_mask_t *mask = (struct diag_log_mask_t *)log_mask.ptr;
	struct diag_log_config_rsp_t header;
	int write_len = 0;
	int i;

	if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
		       __func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
	}

	mutex_lock(&log_mask.lock);
	for (i = 0; i < MAX_EQUIP_ID; i++, mask++)
		memset(mask->ptr, 0, mask->range);
	log_mask.status = DIAG_CTRL_MASK_ALL_DISABLED;
	mutex_unlock(&log_mask.lock);
	diag_update_userspace_clients(LOG_MASKS_TYPE);

	/*
	 * Apps processor must send the response to this command. Frame the
	 * response.
	 */
	header.cmd_code = DIAG_CMD_LOG_CONFIG;
	header.padding[0] = 0;
	header.padding[1] = 0;
	header.padding[2] = 0;
	header.sub_cmd = DIAG_CMD_OP_LOG_DISABLE;
	header.status = LOG_STATUS_SUCCESS;
	memcpy(dest_buf, &header, sizeof(struct diag_log_config_rsp_t));
	write_len += sizeof(struct diag_log_config_rsp_t);
	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		diag_send_log_mask_update(&driver->smd_cntl[i], ALL_EQUIP_ID);

	return write_len;
}

int diag_create_msg_mask_table_entry(struct diag_msg_mask_t *msg_mask,
				     struct diag_ssid_range_t *range)
{
	if (!msg_mask || !range)
		return -EIO;
	if (range->ssid_last < range->ssid_first)
		return -EINVAL;
	msg_mask->ssid_first = range->ssid_first;
	msg_mask->ssid_last = range->ssid_last;
	msg_mask->range = msg_mask->ssid_last - msg_mask->ssid_first + 1;
	if (msg_mask->range < MAX_SSID_PER_RANGE)
		msg_mask->range = MAX_SSID_PER_RANGE;
	if (msg_mask->range > 0) {
		msg_mask->ptr = kzalloc(msg_mask->range * sizeof(uint32_t),
					GFP_KERNEL);
		if (!msg_mask->ptr)
			return -ENOMEM;
	}
	return 0;
}

static int diag_create_msg_mask_table(void)
{
	int i;
	int err = 0;
	struct diag_msg_mask_t *mask = (struct diag_msg_mask_t *)msg_mask.ptr;
	struct diag_ssid_range_t range;

	mutex_lock(&msg_mask.lock);
	driver->msg_mask_tbl_count = MSG_MASK_TBL_CNT;
	for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
		range.ssid_first = msg_mask_tbl[i].ssid_first;
		range.ssid_last = msg_mask_tbl[i].ssid_last;
		err = diag_create_msg_mask_table_entry(mask, &range);
		if (err)
			break;
	}
	mutex_unlock(&msg_mask.lock);
	return err;
}

static int diag_create_build_time_mask(void)
{
	int i;
	int err = 0;
	const uint32_t *tbl = NULL;
	uint32_t tbl_size = 0;
	struct diag_msg_mask_t *build_mask = NULL;
	struct diag_ssid_range_t range;

	mutex_lock(&msg_bt_mask.lock);
	build_mask = (struct diag_msg_mask_t *)msg_bt_mask.ptr;
	for (i = 0; i < driver->msg_mask_tbl_count; i++, build_mask++) {
		range.ssid_first = msg_mask_tbl[i].ssid_first;
		range.ssid_last = msg_mask_tbl[i].ssid_last;
		err = diag_create_msg_mask_table_entry(build_mask, &range);
		if (err)
			break;
		switch (build_mask->ssid_first) {
		case MSG_SSID_0:
			tbl = msg_bld_masks_0;
			tbl_size = sizeof(msg_bld_masks_0);
			break;
		case MSG_SSID_1:
			tbl = msg_bld_masks_1;
			tbl_size = sizeof(msg_bld_masks_1);
			break;
		case MSG_SSID_2:
			tbl = msg_bld_masks_2;
			tbl_size = sizeof(msg_bld_masks_2);
			break;
		case MSG_SSID_3:
			tbl = msg_bld_masks_3;
			tbl_size = sizeof(msg_bld_masks_3);
			break;
		case MSG_SSID_4:
			tbl = msg_bld_masks_4;
			tbl_size = sizeof(msg_bld_masks_4);
			break;
		case MSG_SSID_5:
			tbl = msg_bld_masks_5;
			tbl_size = sizeof(msg_bld_masks_5);
			break;
		case MSG_SSID_6:
			tbl = msg_bld_masks_6;
			tbl_size = sizeof(msg_bld_masks_6);
			break;
		case MSG_SSID_7:
			tbl = msg_bld_masks_7;
			tbl_size = sizeof(msg_bld_masks_7);
			break;
		case MSG_SSID_8:
			tbl = msg_bld_masks_8;
			tbl_size = sizeof(msg_bld_masks_8);
			break;
		case MSG_SSID_9:
			tbl = msg_bld_masks_9;
			tbl_size = sizeof(msg_bld_masks_9);
			break;
		case MSG_SSID_10:
			tbl = msg_bld_masks_10;
			tbl_size = sizeof(msg_bld_masks_10);
			break;
		case MSG_SSID_11:
			tbl = msg_bld_masks_11;
			tbl_size = sizeof(msg_bld_masks_11);
			break;
		case MSG_SSID_12:
			tbl = msg_bld_masks_12;
			tbl_size = sizeof(msg_bld_masks_12);
			break;
		case MSG_SSID_13:
			tbl = msg_bld_masks_13;
			tbl_size = sizeof(msg_bld_masks_13);
			break;
		case MSG_SSID_14:
			tbl = msg_bld_masks_14;
			tbl_size = sizeof(msg_bld_masks_14);
			break;
		case MSG_SSID_15:
			tbl = msg_bld_masks_15;
			tbl_size = sizeof(msg_bld_masks_15);
			break;
		case MSG_SSID_16:
			tbl = msg_bld_masks_16;
			tbl_size = sizeof(msg_bld_masks_16);
			break;
		case MSG_SSID_17:
			tbl = msg_bld_masks_17;
			tbl_size = sizeof(msg_bld_masks_17);
			break;
		case MSG_SSID_18:
			tbl = msg_bld_masks_18;
			tbl_size = sizeof(msg_bld_masks_18);
			break;
		case MSG_SSID_19:
			tbl = msg_bld_masks_19;
			tbl_size = sizeof(msg_bld_masks_19);
			break;
		case MSG_SSID_20:
			tbl = msg_bld_masks_20;
			tbl_size = sizeof(msg_bld_masks_20);
			break;
		case MSG_SSID_21:
			tbl = msg_bld_masks_21;
			tbl_size = sizeof(msg_bld_masks_21);
			break;
		case MSG_SSID_22:
			tbl = msg_bld_masks_22;
			tbl_size = sizeof(msg_bld_masks_22);
			break;
		}
		if (!tbl)
			continue;
		if (tbl_size > build_mask->range * sizeof(uint32_t)) {
			pr_warn("diag: In %s, table %d has more ssid than max, ssid_first: %d, ssid_last: %d\n",
				__func__, i, build_mask->ssid_first,
				build_mask->ssid_last);
			tbl_size = build_mask->range * sizeof(uint32_t);
		}
		memcpy(build_mask->ptr, tbl, tbl_size);
	}
	mutex_unlock(&msg_bt_mask.lock);

	return err;
}

static int diag_create_log_mask_table(void)
{
	struct diag_log_mask_t *mask = NULL;
	uint8_t i;
	int err = 0;

	mutex_lock(&log_mask.lock);
	mask = (struct diag_log_mask_t *)(log_mask.ptr);
	for (i = 0; i < MAX_EQUIP_ID; i++, mask++) {
		mask->equip_id = i;
		mask->num_items = LOG_GET_ITEM_NUM(log_code_last_tbl[i]);
		if (LOG_ITEMS_TO_SIZE(mask->num_items) > MAX_ITEMS_PER_EQUIP_ID)
			mask->range = LOG_ITEMS_TO_SIZE(mask->num_items);
		else
			mask->range = MAX_ITEMS_PER_EQUIP_ID;
		mask->ptr = kzalloc(mask->range, GFP_KERNEL);
		if (!mask->ptr) {
			err = -ENOMEM;
			break;
		}
	}
	mutex_unlock(&log_mask.lock);
	return err;
}

static int __diag_mask_init(struct diag_mask_info *mask_info, int mask_len,
			    int update_buf_len)
{
	if (!mask_info || mask_len < 0 || update_buf_len < 0)
		return -EINVAL;

	mask_info->status = DIAG_CTRL_MASK_INVALID;
	mask_info->mask_len = mask_len;
	mask_info->update_buf_len = update_buf_len;
	if (mask_len > 0) {
		mask_info->ptr = kzalloc(mask_len, GFP_KERNEL);
		if (!mask_info->ptr)
			return -ENOMEM;
		kmemleak_not_leak(mask_info->ptr);
	}
	if (update_buf_len > 0) {
		mask_info->update_buf = kzalloc(update_buf_len, GFP_KERNEL);
		if (!mask_info->update_buf) {
			kfree(mask_info->ptr);
			return -ENOMEM;
		}
		kmemleak_not_leak(mask_info->update_buf);
	}
	mutex_init(&mask_info->lock);
	return 0;
}

static int diag_msg_mask_init(void)
{
	int err = 0;
	int i;

	err = __diag_mask_init(&msg_mask, MSG_MASK_SIZE, APPS_BUF_SIZE);
	if (err)
		return err;
	err = diag_create_msg_mask_table();
	if (err) {
		pr_err("diag: Unable to create msg masks, err: %d\n", err);
		return err;
	}
	driver->msg_mask = &msg_mask;

	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		driver->max_ssid_count[i] = 0;

	return 0;
}

static void diag_msg_mask_exit(void)
{
	int i;
	struct diag_msg_mask_t *mask = NULL;

	mask = (struct diag_msg_mask_t *)(msg_mask.ptr);
	if (mask) {
		for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++)
			kfree(mask->ptr);
		kfree(msg_mask.ptr);
	}

	kfree(msg_mask.update_buf);
}

static int diag_build_time_mask_init(void)
{
	int err = 0;

	/* There is no need for update buffer for Build Time masks */
	err = __diag_mask_init(&msg_bt_mask, MSG_MASK_SIZE, 0);
	if (err)
		return err;
	err = diag_create_build_time_mask();
	if (err) {
		pr_err("diag: Unable to create msg build time masks, err: %d\n",
		       err);
		return err;
	}
	driver->build_time_mask = &msg_bt_mask;
	return 0;
}

static void diag_build_time_mask_exit(void)
{
	int i;
	struct diag_msg_mask_t *mask = NULL;

	mask = (struct diag_msg_mask_t *)(msg_bt_mask.ptr);
	if (mask) {
		for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++)
			kfree(mask->ptr);
		kfree(msg_mask.ptr);
	}
}

static int diag_log_mask_init(void)
{
	int err = 0;
	int i;

	err = __diag_mask_init(&log_mask, LOG_MASK_SIZE, APPS_BUF_SIZE);
	if (err)
		return err;
	err = diag_create_log_mask_table();
	if (err)
		return err;
	driver->log_mask = &log_mask;

	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		driver->num_equip_id[i] = 0;

	return 0;
}

static void diag_log_mask_exit(void)
{
	int i;
	struct diag_log_mask_t *mask = NULL;

	mask = (struct diag_log_mask_t *)(log_mask.ptr);
	if (mask) {
		for (i = 0; i < MAX_EQUIP_ID; i++, mask++)
			kfree(mask->ptr);
		kfree(log_mask.ptr);
	}

	kfree(log_mask.update_buf);
}

static int diag_event_mask_init(void)
{
	int err = 0;
	int i;

	err = __diag_mask_init(&event_mask, EVENT_MASK_SIZE, APPS_BUF_SIZE);
	if (err)
		return err;
	driver->event_mask_size = EVENT_MASK_SIZE;
	driver->last_event_id = APPS_EVENT_LAST_ID;
	driver->event_mask = &event_mask;

	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		driver->num_event_id[i] = 0;

	return 0;
}

static void diag_event_mask_exit(void)
{
	kfree(event_mask.ptr);
	kfree(event_mask.update_buf);
}

int diag_copy_to_user_msg_mask(char __user *buf, size_t count)
{
	int i;
	int err = 0;
	int len = 0;
	int copy_len = 0;
	int total_len = 0;
	struct diag_msg_mask_userspace_t header;
	struct diag_msg_mask_t *mask = NULL;
	unsigned char *ptr = NULL;

	if (!buf || count == 0)
		return -EINVAL;

	mutex_lock(&msg_mask.lock);
	mask = (struct diag_msg_mask_t *)(msg_mask.ptr);
	for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
		ptr = msg_mask.update_buf;
		len = 0;
		header.ssid_first = mask->ssid_first;
		header.ssid_last = mask->ssid_last;
		header.range = mask->range;
		memcpy(ptr, &header, sizeof(header));
		len += sizeof(header);
		copy_len = (sizeof(uint32_t) * mask->range);
		if ((len + copy_len) > msg_mask.update_buf_len) {
			pr_err("diag: In %s, no space to update msg mask, first: %d, last: %d\n",
			       __func__, mask->ssid_first, mask->ssid_last);
			continue;
		}
		memcpy(ptr + len, mask->ptr, copy_len);
		len += copy_len;
		/* + sizeof(int) to account for data_type already in buf */
		if (total_len + sizeof(int) + len > count) {
			pr_err("diag: In %s, unable to send msg masks to user space, total_len: %d, count: %zu\n",
			       __func__, total_len, count);
			err = -ENOMEM;
			break;
		}
		err = copy_to_user(buf + total_len, (void *)ptr, len);
		if (err) {
			pr_err("diag: In %s Unable to send msg masks to user space clients, err: %d\n",
			       __func__, err);
			break;
		}
		total_len += len;
	}
	mutex_unlock(&msg_mask.lock);

	return err ? err : total_len;
}

int diag_copy_to_user_log_mask(char __user *buf, size_t count)
{
	int i;
	int err = 0;
	int len = 0;
	int copy_len = 0;
	int total_len = 0;
	struct diag_log_mask_userspace_t header;
	struct diag_log_mask_t *mask = NULL;
	unsigned char *ptr = NULL;

	if (!buf || count == 0)
		return -EINVAL;

	mutex_lock(&log_mask.lock);
	mask = (struct diag_log_mask_t *)(log_mask.ptr);
	for (i = 0; i < MAX_EQUIP_ID; i++, mask++) {
		ptr = log_mask.update_buf;
		len = 0;
		header.equip_id = mask->equip_id;
		header.num_items = mask->num_items;
		memcpy(ptr, &header, sizeof(header));
		len += sizeof(header);
		copy_len = LOG_ITEMS_TO_SIZE(header.num_items);
		if ((len + copy_len) > log_mask.update_buf_len) {
			pr_err("diag: In %s, no space to update log mask, equip_id: %d\n",
			       __func__, mask->equip_id);
			continue;
		}
		memcpy(ptr + len, mask->ptr, copy_len);
		len += copy_len;
		/* + sizeof(int) to account for data_type already in buf */
		if (total_len + sizeof(int) + len > count) {
			pr_err("diag: In %s, unable to send log masks to user space, total_len: %d, count: %zu\n",
			       __func__, total_len, count);
			err = -ENOMEM;
			break;
		}
		err = copy_to_user(buf + total_len, (void *)ptr, len);
		if (err) {
			pr_err("diag: In %s Unable to send log masks to user space clients, err: %d\n",
			       __func__, err);
			break;
		}
		total_len += len;
	}
	mutex_unlock(&log_mask.lock);

	return err ? err : total_len;
}

void diag_mask_update_fn(struct work_struct *work)
{
	struct diag_smd_info *smd_info = container_of(work,
						struct diag_smd_info,
						diag_notify_update_smd_work);
	if (!smd_info) {
		pr_err("diag: In %s, smd info is null, cannot update masks for the peripheral\n",
			__func__);
		return;
	}

	diag_send_feature_mask_update(smd_info);
	diag_send_msg_mask_update(smd_info, ALL_SSID, ALL_SSID);
	diag_send_log_mask_update(smd_info, ALL_EQUIP_ID);
	diag_send_event_mask_update(smd_info);

	if (smd_info->notify_context == SMD_EVENT_OPEN)
		diag_send_diag_mode_update_by_smd(smd_info,
				driver->real_time_mode[DIAG_LOCAL_PROC]);

	smd_info->notify_context = 0;
}

int diag_process_apps_masks(unsigned char *buf, int len)
{
	int size = 0;
	int sub_cmd = 0;
	int (*hdlr)(unsigned char *src_buf, int src_len,
		    unsigned char *dest_buf, int dest_len) = NULL;

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

	if (*buf == DIAG_CMD_LOG_CONFIG) {
		sub_cmd = *(int *)(buf + sizeof(int));
		switch (sub_cmd) {
		case DIAG_CMD_OP_LOG_DISABLE:
			hdlr = diag_cmd_disable_log_mask;
			break;
		case DIAG_CMD_OP_GET_LOG_RANGE:
			hdlr = diag_cmd_get_log_range;
			break;
		case DIAG_CMD_OP_SET_LOG_MASK:
			hdlr = diag_cmd_set_log_mask;
			break;
		case DIAG_CMD_OP_GET_LOG_MASK:
			hdlr = diag_cmd_get_log_mask;
			break;
		}
	} else if (*buf == DIAG_CMD_MSG_CONFIG) {
		sub_cmd = *(uint8_t *)(buf + sizeof(uint8_t));
		switch (sub_cmd) {
		case DIAG_CMD_OP_GET_SSID_RANGE:
			hdlr = diag_cmd_get_ssid_range;
			break;
		case DIAG_CMD_OP_GET_BUILD_MASK:
			hdlr = diag_cmd_get_build_mask;
			break;
		case DIAG_CMD_OP_GET_MSG_MASK:
			hdlr = diag_cmd_get_msg_mask;
			break;
		case DIAG_CMD_OP_SET_MSG_MASK:
			hdlr = diag_cmd_set_msg_mask;
			break;
		case DIAG_CMD_OP_SET_ALL_MSG_MASK:
			hdlr = diag_cmd_set_all_msg_mask;
			break;
		}
	} else if (*buf == DIAG_CMD_GET_EVENT_MASK) {
		hdlr = diag_cmd_get_event_mask;
	} else if (*buf == DIAG_CMD_SET_EVENT_MASK) {
		hdlr = diag_cmd_update_event_mask;
	} else if (*buf == DIAG_CMD_EVENT_TOGGLE) {
		hdlr = diag_cmd_toggle_events;
	} else if (*buf == DIAG_CMD_LOG_ON_DMND) {
		hdlr = diag_cmd_log_on_demand;
	}

	if (hdlr)
		size = hdlr(buf, len, driver->apps_rsp_buf, APPS_BUF_SIZE);

	return (size > 0) ? size : 0;
}

int diag_masks_init(void)
{
	int err = 0;
	err = diag_msg_mask_init();
	if (err)
		goto fail;

	err = diag_build_time_mask_init();
	if (err)
		goto fail;

	err = diag_log_mask_init();
	if (err)
		goto fail;

	err = diag_event_mask_init();
	if (err)
		goto fail;

	if (driver->buf_feature_mask_update == NULL) {
		driver->buf_feature_mask_update = kzalloc(sizeof(
					struct diag_ctrl_feature_mask) +
					FEATURE_MASK_LEN, GFP_KERNEL);
		if (driver->buf_feature_mask_update == NULL)
			goto fail;
		kmemleak_not_leak(driver->buf_feature_mask_update);
	}

	return 0;
fail:
	pr_err("diag: Could not initialize diag mask buffers\n");
	diag_masks_exit();
	return -ENOMEM;
}

void diag_masks_exit(void)
{
	diag_msg_mask_exit();
	diag_build_time_mask_exit();
	diag_log_mask_exit();
	diag_event_mask_exit();
	kfree(driver->buf_feature_mask_update);
}