diag: dci: Add DCI command response support on APSS (ac10fd2a) · Commits · e / devices / android_kernel_sony_msm8994

drivers/char/diag/diag_dci.c

+324 −99

Original line number	Diff line number	Diff line
		/* Copyright (c) 2012-2013, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2012-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
		@@ -23,7 +23,9 @@
		#include <linux/pm_wakeup.h>
		#include <linux/spinlock.h>
		#include <linux/ratelimit.h>
		#include <linux/reboot.h>
		#include <asm/current.h>
		#include <mach/restart.h>
		#ifdef CONFIG_DIAG_OVER_USB
		#include <mach/usbdiag.h>
		#endif
		@@ -282,19 +284,31 @@ void diag_process_apps_dci_read_data(int data_type, void *buf, int recd_bytes)
		return;
		}

		if (data_type != DATA_TYPE_DCI_LOG && data_type != DATA_TYPE_DCI_EVENT)
		if (data_type != DATA_TYPE_DCI_LOG && data_type != DATA_TYPE_DCI_EVENT
		&& data_type != DCI_PKT_TYPE) {
		pr_err("diag: In %s, unsupported data_type: 0x%x\n",
		__func__, (unsigned int)data_type);
		return;
		}

		cmd_code = (uint8_t )buf;

		if (cmd_code == LOG_CMD_CODE) {
		switch (cmd_code) {
		case LOG_CMD_CODE:
		extract_dci_log(buf, recd_bytes, APPS_DATA);
		} else if (cmd_code == EVENT_CMD_CODE) {
		break;
		case EVENT_CMD_CODE:
		extract_dci_events(buf, recd_bytes, APPS_DATA);
		} else {
		break;
		case DCI_PKT_RSP_CODE:
		case DCI_DELAYED_RSP_CODE:
		extract_dci_pkt_rsp(buf, recd_bytes, APPS_DATA, NULL);
		break;
		default:
		pr_err("diag: In %s, unsupported command code: 0x%x, not log or event\n",
		__func__, cmd_code);
		return;

		}

		/* wake up all sleeping DCI clients which have some data */
		@@ -352,7 +366,8 @@ int diag_process_smd_dci_read_data(struct diag_smd_info smd_info, void buf,
		extract_dci_events(buf + 4, recd_bytes - 4,
		smd_info->peripheral);
		} else
		extract_dci_pkt_rsp(smd_info, buf, recd_bytes);
		extract_dci_pkt_rsp(buf + 4, dci_pkt_len,
		smd_info->peripheral, smd_info);
		read_bytes += 5 + dci_pkt_len;
		buf += 5 + dci_pkt_len; /* advance to next DCI pkt */
		}
		@@ -541,48 +556,61 @@ static int diag_dci_remove_req_entry(unsigned char *buf, int len,
		return 0;
		}

		void extract_dci_pkt_rsp(struct diag_smd_info smd_info, unsigned char buf,
		int len)
		void extract_dci_pkt_rsp(unsigned char *buf, int len, int data_source,
		struct diag_smd_info *smd_info)
		{
		int cmd_code_len = 1;
		int curr_client_pid = 0, write_len, *tag = NULL;
		int tag, curr_client_pid = 0;
		struct diag_dci_client_tbl *entry = NULL;
		void *temp_buf = NULL;
		uint8_t recv_pkt_cmd_code, delete_flag = 0;
		uint8_t dci_cmd_code, cmd_code_len, delete_flag = 0;
		uint32_t rsp_len = 0;
		struct diag_dci_buffer_t *rsp_buf = NULL;
		struct dci_pkt_req_entry_t *req_entry = NULL;
		unsigned char *temp = buf;

		recv_pkt_cmd_code = (uint8_t )(buf+4);
		if (recv_pkt_cmd_code != DCI_PKT_RSP_CODE)
		cmd_code_len = 4; /* delayed response */
		if (!buf) {
		pr_err("diag: Invalid pointer in %s\n", __func__);
		return;
		}
		dci_cmd_code = (uint8_t )(temp);
		if (dci_cmd_code == DCI_PKT_RSP_CODE) {
		cmd_code_len = sizeof(uint8_t);
		} else if (dci_cmd_code == DCI_DELAYED_RSP_CODE) {
		cmd_code_len = sizeof(uint32_t);
		} else {
		pr_err("diag: In %s, invalid command code %d\n", __func__,
		dci_cmd_code);
		return;
		}
		temp += cmd_code_len;
		tag = (int )temp;
		temp += sizeof(int);

		/* Skip the Start(1) and the version(1) bytes */
		write_len = (int)((uint16_t )(buf+2));
		/* Check if the length embedded in the packet is correct.
		/*
		* The size of the response is (total length) - (length of the command
		* code, the tag (int)
		*/
		rsp_len = len - (cmd_code_len + sizeof(int));
		/*
		* Check if the length embedded in the packet is correct.
		* Include the start (1), version (1), length (2) and the end
		* (1) bytes while checking. Total = 5 bytes
		*/
		write_len -= cmd_code_len;
		if ((write_len <= 0) \|\| (write_len > (len - 5))) {
		pr_err("diag: Invalid length in %s, len: %d, write_len: %d",
		__func__, len, write_len);
		if ((rsp_len == 0) \|\| (rsp_len > (len - 5))) {
		pr_err("diag: Invalid length in %s, len: %d, rsp_len: %d",
		__func__, len, rsp_len);
		return;
		}
		pr_debug("diag: len = %d\n", write_len);

		tag = (int )(buf + (4 + cmd_code_len)); / Retrieve the Tag field */
		req_entry = diag_dci_get_request_entry(*tag);
		req_entry = diag_dci_get_request_entry(tag);
		if (!req_entry) {
		pr_err("diag: No matching PID for DCI data\n");
		return;
		}
		tag = req_entry->uid; / Replace the tag field with UID */
		curr_client_pid = req_entry->pid;

		/* Remove the headers and send only the response to this function */
		delete_flag = diag_dci_remove_req_entry(buf + 8 + cmd_code_len,
		len - (8 + cmd_code_len),
		req_entry);
		delete_flag = diag_dci_remove_req_entry(temp, rsp_len, req_entry);
		if (delete_flag < 0)
		return;

		@@ -592,7 +620,7 @@ void extract_dci_pkt_rsp(struct diag_smd_info smd_info, unsigned char buf,
		return;
		}

		rsp_buf = entry->buffers[smd_info->peripheral].buf_cmd;
		rsp_buf = entry->buffers[data_source].buf_cmd;

		mutex_lock(&rsp_buf->data_mutex);
		/*
		@@ -600,9 +628,9 @@ void extract_dci_pkt_rsp(struct diag_smd_info smd_info, unsigned char buf,
		* the rsp is the rsp length (write_len) + DCI_PKT_RSP_TYPE header (int)
		* + field for length (int) + delete_flag (uint8_t)
		*/
		if ((rsp_buf->data_len + 9 + write_len) > rsp_buf->capacity) {
		if ((rsp_buf->data_len + 9 + rsp_len) > rsp_buf->capacity) {
		pr_alert("diag: create capacity for pkt rsp\n");
		rsp_buf->capacity += 9 + write_len;
		rsp_buf->capacity += 9 + rsp_len;
		temp_buf = krealloc(rsp_buf->data, rsp_buf->capacity,
		GFP_KERNEL);
		if (!temp_buf) {
		@@ -615,13 +643,17 @@ void extract_dci_pkt_rsp(struct diag_smd_info smd_info, unsigned char buf,
		}
		(int )(rsp_buf->data + rsp_buf->data_len) = DCI_PKT_RSP_TYPE;
		rsp_buf->data_len += sizeof(int);
		(int )(rsp_buf->data + rsp_buf->data_len) = write_len;
		/* Packet Length = Response Length + Length of uid field (int) */
		(int )(rsp_buf->data + rsp_buf->data_len) = rsp_len + sizeof(int);
		rsp_buf->data_len += sizeof(int);
		(uint8_t )(rsp_buf->data + rsp_buf->data_len) = delete_flag;
		rsp_buf->data_len += sizeof(uint8_t);
		memcpy(rsp_buf->data+rsp_buf->data_len, buf+4+cmd_code_len, write_len);
		rsp_buf->data_len += write_len;
		rsp_buf->data_source = smd_info->peripheral;
		(int )(rsp_buf->data + rsp_buf->data_len) = req_entry->uid;
		rsp_buf->data_len += sizeof(int);
		memcpy(rsp_buf->data + rsp_buf->data_len, temp, rsp_len);
		rsp_buf->data_len += rsp_len;
		rsp_buf->data_source = data_source;
		if (smd_info)
		smd_info->in_busy_1 = 1;
		mutex_unlock(&rsp_buf->data_mutex);

		@@ -956,7 +988,7 @@ void diag_dci_notify_client(int peripheral_mask, int data)
		static int diag_send_dci_pkt(struct diag_master_table entry,
		unsigned char *buf, int len, int tag)
		{
		int i, status = 0;
		int i, status = DIAG_DCI_NO_ERROR;
		unsigned int read_len = 0;

		/* The first 4 bytes is the uid tag and the next four bytes is
		@@ -990,6 +1022,14 @@ static int diag_send_dci_pkt(struct diag_master_table entry,
		mutex_unlock(&driver->dci_mutex);
		return -EIO;
		}
		/* This command is registered locally on the Apps */
		if (entry.client_id == APPS_DATA) {
		driver->dci_pkt_length = len + 10;
		diag_update_pkt_buffer(driver->apps_dci_buf, DCI_PKT_TYPE);
		diag_update_sleeping_process(entry.process_id, DCI_PKT_TYPE);
		mutex_unlock(&driver->dci_mutex);
		return DIAG_DCI_NO_ERROR;
		}

		for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++)
		if (entry.client_id == i) {
		@@ -1011,34 +1051,163 @@ static int diag_send_dci_pkt(struct diag_master_table entry,
		return status;
		}

		int diag_process_dci_transaction(unsigned char *buf, int len)
		static int diag_dci_process_apps_pkt(struct diag_pkt_header_t *pkt_header,
		unsigned char *req_buf, int tag)
		{
		uint8_t cmd_code, subsys_id, i, goto_download = 0;
		uint8_t header_len = sizeof(struct diag_dci_pkt_header_t);
		uint16_t ss_cmd_code;
		uint32_t write_len = 0;
		unsigned char *dest_buf = driver->apps_dci_buf;
		unsigned char *payload_ptr = driver->apps_dci_buf + header_len;
		struct diag_dci_pkt_header_t dci_header;

		if (!pkt_header \|\| !req_buf \|\| tag < 0)
		return -EIO;

		cmd_code = pkt_header->cmd_code;
		subsys_id = pkt_header->subsys_id;
		ss_cmd_code = pkt_header->subsys_cmd_code;

		if (cmd_code == DIAG_CMD_DOWNLOAD) {
		*payload_ptr = DIAG_CMD_DOWNLOAD;
		write_len = sizeof(uint8_t);
		goto_download = 1;
		goto fill_buffer;
		} else if (cmd_code == DIAG_CMD_VERSION) {
		if (chk_polling_response()) {
		for (i = 0; i < 55; i++, write_len++, payload_ptr++)
		*(payload_ptr) = 0;
		goto fill_buffer;
		}
		} else if (cmd_code == DIAG_CMD_EXT_BUILD) {
		if (chk_polling_response()) {
		*payload_ptr = DIAG_CMD_EXT_BUILD;
		write_len = sizeof(uint8_t);
		payload_ptr += sizeof(uint8_t);
		for (i = 0; i < 8; i++, write_len++, payload_ptr++)
		*(payload_ptr) = 0;
		(int )(payload_ptr) = chk_config_get_id();
		write_len += sizeof(int);
		goto fill_buffer;
		}
		} else if (cmd_code == DIAG_CMD_LOG_ON_DMND) {
		if (driver->log_on_demand_support) {
		*payload_ptr = DIAG_CMD_LOG_ON_DMND;
		write_len = sizeof(uint8_t);
		payload_ptr += sizeof(uint8_t);
		(uint16_t )(payload_ptr) = (uint16_t )(req_buf + 1);
		write_len += sizeof(uint16_t);
		payload_ptr += sizeof(uint16_t);
		payload_ptr = 0x1; / Unknown */
		write_len += sizeof(uint8_t);
		goto fill_buffer;
		}
		} else if (cmd_code != DIAG_CMD_DIAG_SUBSYS) {
		return DIAG_DCI_TABLE_ERR;
		}

		if (subsys_id == DIAG_SS_DIAG) {
		if (ss_cmd_code == DIAG_DIAG_MAX_PKT_SZ) {
		memcpy(payload_ptr, pkt_header,
		sizeof(struct diag_pkt_header_t));
		write_len = sizeof(struct diag_pkt_header_t);
		(uint32_t )(payload_ptr + write_len) = PKT_SIZE;
		write_len += sizeof(uint32_t);
		} else if (ss_cmd_code == DIAG_DIAG_STM) {
		write_len = diag_process_stm_cmd(req_buf, payload_ptr);
		}
		} else if (subsys_id == DIAG_SS_PARAMS) {
		if (ss_cmd_code == DIAG_DIAG_POLL) {
		if (chk_polling_response()) {
		memcpy(payload_ptr, pkt_header,
		sizeof(struct diag_pkt_header_t));
		write_len = sizeof(struct diag_pkt_header_t);
		payload_ptr += write_len;
		for (i = 0; i < 12; i++, write_len++) {
		*(payload_ptr) = 0;
		payload_ptr++;
		}
		}
		} else if (ss_cmd_code == DIAG_DEL_RSP_WRAP) {
		memcpy(payload_ptr, pkt_header,
		sizeof(struct diag_pkt_header_t));
		write_len = sizeof(struct diag_pkt_header_t);
		(int )(payload_ptr + write_len) = wrap_enabled;
		write_len += sizeof(int);
		} else if (ss_cmd_code == DIAG_DEL_RSP_WRAP_CNT) {
		wrap_enabled = true;
		memcpy(payload_ptr, pkt_header,
		sizeof(struct diag_pkt_header_t));
		write_len = sizeof(struct diag_pkt_header_t);
		(uint16_t )(payload_ptr + write_len) = wrap_count;
		write_len += sizeof(uint16_t);
		}
		}

		fill_buffer:
		if (write_len > 0) {
		/* Check if we are within the range of the buffer*/
		if (write_len + header_len > PKT_SIZE) {
		pr_err("diag: In %s, invalid length %d\n", __func__,
		write_len + header_len);
		return -ENOMEM;
		}
		dci_header.start = CONTROL_CHAR;
		dci_header.version = 1;
		/*
		* Length of the rsp pkt = actual data len + pkt rsp code
		* (uint8_t) + tag (int)
		*/
		dci_header.len = write_len + sizeof(uint8_t) + sizeof(int);
		dci_header.pkt_code = DCI_PKT_RSP_CODE;
		dci_header.tag = tag;
		driver->in_busy_dcipktdata = 1;
		memcpy(dest_buf, &dci_header, header_len);
		diag_process_apps_dci_read_data(DCI_PKT_TYPE, dest_buf + 4,
		dci_header.len);
		driver->in_busy_dcipktdata = 0;

		if (goto_download) {
		/*
		* Sleep for sometime so that the response reaches the
		* client. The value 5000 empirically as an optimum
		* time for the response to reach the client.
		*/
		usleep_range(5000, 5100);
		/* call download API */
		msm_set_restart_mode(RESTART_DLOAD);
		pr_alert("diag: download mode set, Rebooting SoC..\n");
		kernel_restart(NULL);
		}
		return DIAG_DCI_NO_ERROR;
		}

		return DIAG_DCI_TABLE_ERR;
		}

		static int diag_process_dci_pkt_rsp(unsigned char *buf, int len)
		{
		int req_uid, ret = DIAG_DCI_TABLE_ERR, i;
		struct diag_pkt_header_t *header = NULL;
		unsigned char *temp = buf;
		uint16_t log_code, item_num;
		int ret = -1, found = 0, req_uid;
		unsigned char *req_buf = NULL;
		uint8_t retry_count = 0, max_retries = 3, found = 0;
		uint32_t read_len = 0;
		struct diag_master_table entry;
		int count, set_mask, num_codes, bit_index, event_id, offset = 0, i;
		unsigned int byte_index, read_len = 0;
		uint8_t equip_id, log_mask_ptr, head_log_mask_ptr, byte_mask;
		uint8_t *event_mask_ptr;
		struct diag_dci_client_tbl *dci_entry = NULL;
		struct dci_pkt_req_entry_t *req_entry = NULL;
		struct diag_pkt_header_t *header = NULL;

		if (!temp) {
		pr_err("diag: Invalid buffer in %s\n", __func__);
		return -ENOMEM;
		}
		if (!buf)
		return -EIO;

		/* This is Pkt request/response transaction */
		if ((int )temp > 0) {
		if (len < DCI_PKT_REQ_MIN_LEN \|\| len > USER_SPACE_DATA) {
		pr_err("diag: dci: Invalid length %d len in %s", len,
		__func__);
		pr_err("diag: dci: Invalid length %d len in %s", len, __func__);
		return -EIO;
		}
		req_uid = (int )temp;

		req_uid = (int )temp; /* UID of the request */
		temp += sizeof(int);
		req_buf = temp; /* Start of the Request */
		header = (struct diag_pkt_header_t *)temp;
		temp += sizeof(struct diag_pkt_header_t);
		read_len = sizeof(int) + sizeof(struct diag_pkt_header_t);
		@@ -1046,21 +1215,47 @@ int diag_process_dci_transaction(unsigned char *buf, int len)
		pr_err("diag: dci: Invalid length in %s\n", __func__);
		return -EIO;
		}
		/* check if the command is allowed on DCI */

		/* Check if the command is allowed on DCI */
		if (diag_dci_filter_commands(header)) {
		pr_debug("diag: command not supported %d %d %d",
		header->cmd_code,
		header->subsys_id,
		header->cmd_code, header->subsys_id,
		header->subsys_cmd_code);
		return DIAG_DCI_SEND_DATA_FAIL;
		}
		/* enter this UID into kernel table */

		/*
		* Previous packet is yet to be consumed by the client. Wait
		* till the buffer is free.
		*/
		while (retry_count < max_retries) {
		retry_count++;
		if (driver->in_busy_dcipktdata)
		usleep_range(10000, 10100);
		else
		break;
		}
		/* The buffer is still busy */
		if (driver->in_busy_dcipktdata) {
		pr_err("diag: In %s, apps dci buffer is still busy. Dropping packet\n",
		__func__);
		return -EAGAIN;
		}

		/* Register this new DCI packet */
		req_entry = diag_register_dci_transaction(req_uid);
		if (!req_entry) {
		pr_alert("diag: registering new DCI transaction failed\n");
		return DIAG_DCI_NO_REG;
		}
		for (i = 0; i < diag_max_reg; i++) {

		/* Check if it is a dedicated Apps command */
		ret = diag_dci_process_apps_pkt(header, req_buf, req_entry->tag);
		if (ret == DIAG_DCI_NO_ERROR \|\| ret < 0)
		return ret;

		/* Check the registration table for command entries */
		for (i = 0; i < diag_max_reg && !found; i++) {
		entry = driver->table[i];
		if (entry.process_id == NO_PROCESS)
		continue;
		@@ -1070,25 +1265,55 @@ int diag_process_dci_transaction(unsigned char *buf, int len)
		entry.cmd_code_hi >= header->subsys_cmd_code) {
		ret = diag_send_dci_pkt(entry, buf, len,
		req_entry->tag);
		} else if (entry.cmd_code == 255 &&
		header->cmd_code == 75) {
		found = 1;
		} else if (entry.cmd_code == 255 && header->cmd_code == 75) {
		if (entry.subsys_id == header->subsys_id &&
		entry.cmd_code_lo <=
		header->subsys_cmd_code &&
		entry.cmd_code_hi >=
		header->subsys_cmd_code) {
		entry.cmd_code_lo <= header->subsys_cmd_code &&
		entry.cmd_code_hi >= header->subsys_cmd_code) {
		ret = diag_send_dci_pkt(entry, buf, len,
		req_entry->tag);
		found = 1;
		}
		} else if (entry.cmd_code == 255 &&
		entry.subsys_id == 255) {
		} else if (entry.cmd_code == 255 && entry.subsys_id == 255) {
		if (entry.cmd_code_lo <= header->cmd_code &&
		entry.cmd_code_hi >= header->cmd_code) {
		/*
		* If its a Mode reset command, make sure it is
		* registered on the Apps Processor
		*/
		if (entry.cmd_code_lo == MODE_CMD &&
		entry.cmd_code_hi == MODE_CMD)
		if (entry.client_id != APPS_DATA)
		continue;
		ret = diag_send_dci_pkt(entry, buf, len,
		req_entry->tag);
		found = 1;
		}
		}
		}

		return ret;
		}

		int diag_process_dci_transaction(unsigned char *buf, int len)
		{
		unsigned char *temp = buf;
		uint16_t log_code, item_num;
		int ret = -1, found = 0;
		int count, set_mask, num_codes, bit_index, event_id, offset = 0;
		unsigned int byte_index, read_len = 0;
		uint8_t equip_id, log_mask_ptr, head_log_mask_ptr, byte_mask;
		uint8_t *event_mask_ptr;
		struct diag_dci_client_tbl *dci_entry = NULL;

		if (!temp) {
		pr_err("diag: Invalid buffer in %s\n", __func__);
		return -ENOMEM;
		}

		/* This is Pkt request/response transaction */
		if ((int )temp > 0) {
		return diag_process_dci_pkt_rsp(buf, len);
		} else if ((int )temp == DCI_LOG_TYPE) {
		/* Minimum length of a log mask config is 12 + 2 bytes for
		atleast one log code to be set or reset */

drivers/char/diag/diag_dci.h

+11 −3

Original line number	Diff line number	Diff line
		/* Copyright (c) 2012-2013, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2012-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
		@@ -128,6 +128,14 @@ struct diag_log_event_stats {
		int is_set;
		};

		struct diag_dci_pkt_header_t {
		uint8_t start;
		uint8_t version;
		uint16_t len;
		uint8_t pkt_code;
		int tag;
		} __packed;

		enum {
		DIAG_DCI_NO_ERROR = 1001, /* No error */
		DIAG_DCI_NO_REG, /* Could not register */
		@@ -163,8 +171,8 @@ void diag_process_apps_dci_read_data(int data_type, void *buf, int recd_bytes);
		int diag_process_smd_dci_read_data(struct diag_smd_info smd_info, void buf,
		int recd_bytes);
		int diag_process_dci_transaction(unsigned char *buf, int len);
		void extract_dci_pkt_rsp(struct diag_smd_info smd_info, unsigned char buf,
		int len);
		void extract_dci_pkt_rsp(unsigned char *buf, int len, int data_source,
		struct diag_smd_info *smd_info);
		struct diag_dci_client_tbl *diag_dci_get_client_entry(void);
		/* DCI Log streaming functions */
		void create_dci_log_mask_tbl(unsigned char *tbl_buf);

drivers/char/diag/diagchar.h

+22 −1

Original line number	Diff line number	Diff line
		/* Copyright (c) 2008-2013, The Linux Foundation. All rights reserved.
		/* 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
		@@ -111,6 +111,24 @@
		#define DIAG_STM_WCNSS 0x04
		#define DIAG_STM_APPS 0x08

		#define DIAG_CMD_VERSION 0
		#define DIAG_CMD_DOWNLOAD 0x3A
		#define DIAG_CMD_DIAG_SUBSYS 0x4B
		#define DIAG_CMD_LOG_ON_DMND 0x78
		#define DIAG_CMD_EXT_BUILD 0x7c

		#define DIAG_SS_DIAG 0x12
		#define DIAG_SS_PARAMS 0x32

		#define DIAG_DIAG_MAX_PKT_SZ 0x55
		#define DIAG_DIAG_STM 0x20E
		#define DIAG_DIAG_POLL 0x03
		#define DIAG_DEL_RSP_WRAP 0x04
		#define DIAG_DEL_RSP_WRAP_CNT 0x05

		#define MODE_CMD 41
		#define RESET_ID 2

		/*
		* The status bit masks when received in a signal handler are to be
		* used in conjunction with the peripheral list bit mask to determine the
		@@ -413,6 +431,9 @@ struct diagchar_dev {
		struct diag_master_table *table;
		uint8_t *pkt_buf;
		int pkt_length;
		uint8_t dci_pkt_buf; / For Apps DCI packets */
		uint32_t dci_pkt_length;
		int in_busy_dcipktdata;
		struct diag_request *usb_read_ptr;
		struct diag_request *write_ptr_svc;
		int logging_mode;

drivers/char/diag/diagchar_core.c

+24 −8

Original line number	Diff line number	Diff line
		/* Copyright (c) 2008-2013, The Linux Foundation. All rights reserved.
		/* 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
		@@ -1367,6 +1367,17 @@ drop:
		goto exit;
		}

		if (driver->data_ready[index] & DCI_PKT_TYPE) {
		/* Copy the type of data being passed */
		data_type = driver->data_ready[index] & DCI_PKT_TYPE;
		COPY_USER_SPACE_OR_EXIT(buf, data_type, 4);
		COPY_USER_SPACE_OR_EXIT(buf+4, *(driver->dci_pkt_buf),
		driver->dci_pkt_length);
		driver->data_ready[index] ^= DCI_PKT_TYPE;
		driver->in_busy_dcipktdata = 0;
		goto exit;
		}

		if (driver->data_ready[index] & DCI_EVENT_MASKS_TYPE) {
		/Copy the type of data being passed/
		data_type = driver->data_ready[index] & DCI_EVENT_MASKS_TYPE;
		@@ -1436,7 +1447,7 @@ static ssize_t diagchar_write(struct file file, const char __user buf,
		size_t count, loff_t *ppos)
		{
		int err, ret = 0, pkt_type, token_offset = 0;
		int remote_proc = 0;
		int remote_proc = 0, data_type;
		uint8_t index;
		#ifdef DIAG_DEBUG
		int length = 0, i;
		@@ -1750,16 +1761,20 @@ static ssize_t diagchar_write(struct file file, const char __user buf,
		return -EFAULT;
		}

		if (pkt_type & (DATA_TYPE_DCI_LOG \| DATA_TYPE_DCI_EVENT)) {
		int data_type = pkt_type &
		(DATA_TYPE_DCI_LOG \| DATA_TYPE_DCI_EVENT);
		data_type = pkt_type &
		(DATA_TYPE_DCI_LOG \| DATA_TYPE_DCI_EVENT \| DCI_PKT_TYPE);
		if (data_type) {
		diag_process_apps_dci_read_data(data_type, buf_copy,
		payload_size);

		if (pkt_type & DATA_TYPE_DCI_LOG)
		pkt_type ^= DATA_TYPE_DCI_LOG;
		else
		else if (pkt_type & DATA_TYPE_DCI_EVENT) {
		pkt_type ^= DATA_TYPE_DCI_EVENT;
		} else {
		pkt_type ^= DCI_PKT_TYPE;
		diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
		return 0;
		}

		/*
		* If the data is not headed for normal processing or the usb
		@@ -2123,6 +2138,7 @@ static int __init diagchar_init(void)
		driver->callback_process = NULL;
		driver->mask_check = 0;
		driver->in_busy_pktdata = 0;
		driver->in_busy_dcipktdata = 0;
		mutex_init(&driver->diagchar_mutex);
		init_waitqueue_head(&driver->wait_q);
		init_waitqueue_head(&driver->smd_wait_q);

drivers/char/diag/diagfwd.c

+68 −23

Original line number	Diff line number	Diff line
		/* Copyright (c) 2008-2013, The Linux Foundation. All rights reserved.
		/* 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
		@@ -43,9 +43,6 @@
		#include "diag_masks.h"
		#include "diagfwd_bridge.h"

		#define MODE_CMD 41
		#define RESET_ID 2

		#define STM_CMD_VERSION_OFFSET 4
		#define STM_CMD_MASK_OFFSET 5
		#define STM_CMD_DATA_OFFSET 6
		@@ -1060,17 +1057,46 @@ int diag_device_write(void buf, int data_type, struct diag_request write_ptr)
		return err;
		}

		static void diag_update_pkt_buffer(unsigned char *buf)
		void diag_update_pkt_buffer(unsigned char *buf, int type)
		{
		unsigned char *ptr = driver->pkt_buf;
		unsigned char *ptr = NULL;
		unsigned char *temp = buf;
		unsigned int length;
		int *in_busy = NULL;

		if (!buf) {
		pr_err("diag: Invalid buffer in %s\n", __func__);
		return;
		}

		switch (type) {
		case PKT_TYPE:
		ptr = driver->pkt_buf;
		length = driver->pkt_length;
		in_busy = &driver->in_busy_pktdata;
		break;
		case DCI_PKT_TYPE:
		ptr = driver->dci_pkt_buf;
		length = driver->dci_pkt_length;
		in_busy = &driver->in_busy_dcipktdata;
		break;
		default:
		pr_err("diag: Invalid type %d in %s\n", type, __func__);
		return;
		}

		if (!ptr \|\| length == 0) {
		pr_err("diag: Invalid ptr %p and length %d in %s",
		ptr, length, __func__);
		return;
		}
		mutex_lock(&driver->diagchar_mutex);
		if (CHK_OVERFLOW(ptr, ptr, ptr + PKT_SIZE, driver->pkt_length)) {
		memcpy(ptr, temp , driver->pkt_length);
		driver->in_busy_pktdata = 1;
		} else
		if (CHK_OVERFLOW(ptr, ptr, ptr + PKT_SIZE, length)) {
		memcpy(ptr, temp , length);
		*in_busy = 1;
		} else {
		printk(KERN_CRIT " Not enough buffer space for PKT_RESP\n");
		}
		mutex_unlock(&driver->diagchar_mutex);
		}

		@@ -1119,7 +1145,7 @@ int diag_send_data(struct diag_master_table entry, unsigned char *buf,
		if (entry.process_id != NON_APPS_PROC) {
		/* If the message is to be sent to the apps process */
		if (type != MODEM_DATA) {
		diag_update_pkt_buffer(buf);
		diag_update_pkt_buffer(buf, PKT_TYPE);
		diag_update_sleeping_process(entry.process_id,
		PKT_TYPE);
		}
		@@ -1195,16 +1221,24 @@ void diag_process_stm_mask(uint8_t cmd, uint8_t data_mask, int data_type,
		}
		}

		int diag_process_stm_cmd(unsigned char *buf)
		int diag_process_stm_cmd(unsigned char buf, unsigned char dest_buf)
		{
		uint8_t version = *(buf+STM_CMD_VERSION_OFFSET);
		uint8_t mask = *(buf+STM_CMD_MASK_OFFSET);
		uint8_t cmd = *(buf+STM_CMD_DATA_OFFSET);
		uint8_t version, mask, cmd;
		uint8_t rsp_supported = 0;
		uint8_t rsp_smd_comply = 0;
		int valid_command = 1;
		int i;

		if (!buf \|\| !dest_buf) {
		pr_err("diag: Invalid pointers buf: %p, dest_buf %p in %s\n",
		buf, dest_buf, __func__);
		return -EIO;
		}

		version = *(buf + STM_CMD_VERSION_OFFSET);
		mask = *(buf + STM_CMD_MASK_OFFSET);
		cmd = *(buf + STM_CMD_DATA_OFFSET);

		/* Check if command is valid */
		if ((version != 1) \|\| (mask == 0) \|\| (0 != (mask >> 4)) \|\|
		(cmd != ENABLE_STM && cmd != DISABLE_STM)) {
		@@ -1228,15 +1262,13 @@ int diag_process_stm_cmd(unsigned char *buf)
		}

		for (i = 0; i < STM_CMD_NUM_BYTES; i++)
		driver->apps_rsp_buf[i] = *(buf+i);
		dest_buf[i] = *(buf + i);

		driver->apps_rsp_buf[STM_RSP_VALID_INDEX] = valid_command;
		driver->apps_rsp_buf[STM_RSP_SUPPORTED_INDEX] = rsp_supported;
		driver->apps_rsp_buf[STM_RSP_SMD_COMPLY_INDEX] = rsp_smd_comply;
		dest_buf[STM_RSP_VALID_INDEX] = valid_command;
		dest_buf[STM_RSP_SUPPORTED_INDEX] = rsp_supported;
		dest_buf[STM_RSP_SMD_COMPLY_INDEX] = rsp_smd_comply;

		encode_rsp_and_send(STM_RSP_NUM_BYTES-1);

		return 0;
		return STM_RSP_NUM_BYTES;
		}

		int diag_apps_responds()
		@@ -1333,7 +1365,12 @@ int diag_process_apps_pkt(unsigned char *buf, int len)
		return 0;
		} else if ((buf == 0x4b) && ((buf+1) == 0x12) &&
		((uint16_t )(buf+2) == 0x020E)) {
		return diag_process_stm_cmd(buf);
		len = diag_process_stm_cmd(buf, driver->apps_rsp_buf);
		if (len > 0) {
		encode_rsp_and_send(len - 1);
		return 0;
		}
		return len;
		}
		/* Check for Apps Only & get event mask request */
		else if (diag_apps_responds() && *buf == 0x81) {
		@@ -2641,6 +2678,12 @@ void diagfwd_init(void)
		GFP_KERNEL)) == NULL)
		goto err;
		kmemleak_not_leak(driver->pkt_buf);
		if (driver->dci_pkt_buf == NULL) {
		driver->dci_pkt_buf = kzalloc(PKT_SIZE, GFP_KERNEL);
		if (!driver->dci_pkt_buf)
		goto err;
		}
		kmemleak_not_leak(driver->dci_pkt_buf);
		if (driver->apps_rsp_buf == NULL) {
		driver->apps_rsp_buf = kzalloc(APPS_BUF_SIZE, GFP_KERNEL);
		if (driver->apps_rsp_buf == NULL)
		@@ -2691,6 +2734,7 @@ err:
		kfree(driver->data_ready);
		kfree(driver->table);
		kfree(driver->pkt_buf);
		kfree(driver->dci_pkt_buf);
		kfree(driver->usb_read_ptr);
		kfree(driver->apps_rsp_buf);
		kfree(driver->user_space_data_buf);
		@@ -2731,6 +2775,7 @@ void diagfwd_exit(void)
		kfree(driver->data_ready);
		kfree(driver->table);
		kfree(driver->pkt_buf);
		kfree(driver->dci_pkt_buf);
		kfree(driver->usb_read_ptr);
		kfree(driver->apps_rsp_buf);
		kfree(driver->user_space_data_buf);