Merge "diag: Add diag ID command response support" into msm-4.8 (6e7e918e) · Commits · e / devices / android_kernel_fairphone_FP3

drivers/char/diag/diag_masks.c

+2 −1

Original line number	Diff line number	Diff line
		/* Copyright (c) 2008-2016, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2008-2017, 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
		@@ -448,6 +448,7 @@ static void diag_send_feature_mask_update(uint8_t peripheral)
		DIAG_SET_FEATURE_MASK(F_DIAG_LOG_ON_DEMAND_APPS);
		DIAG_SET_FEATURE_MASK(F_DIAG_STM);
		DIAG_SET_FEATURE_MASK(F_DIAG_DCI_EXTENDED_HEADER_SUPPORT);
		DIAG_SET_FEATURE_MASK(F_DIAG_DIAGID_SUPPORT);
		if (driver->supports_separate_cmdrsp)
		DIAG_SET_FEATURE_MASK(F_DIAG_REQ_RSP_SUPPORT);
		if (driver->supports_apps_hdlc_encoding)

drivers/char/diag/diagchar.h

+35 −1

Original line number	Diff line number	Diff line
		/* Copyright (c) 2008-2016, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2008-2017, 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
		@@ -125,6 +125,7 @@
		#define DIAG_EXT_MOBILE_ID 0x06
		#define DIAG_GET_TIME_API 0x21B
		#define DIAG_SET_TIME_API 0x21C
		#define DIAG_GET_DIAG_ID 0x222
		#define DIAG_SWITCH_COMMAND 0x081B
		#define DIAG_BUFFERING_MODE 0x080C

		@@ -257,6 +258,14 @@
		#define DIAG_CNTL_TYPE 2
		#define DIAG_DCI_TYPE 3

		/*
		* List of diag ids
		* 0 is reserved for unknown diag id, 1 for apps, diag ids
		* for remaining pds are assigned dynamically.
		*/
		#define DIAG_ID_UNKNOWN 0
		#define DIAG_ID_APPS 1

		/* List of remote processor supported */
		enum remote_procs {
		MDM = 1,
		@@ -278,6 +287,29 @@ struct diag_cmd_ext_mobile_rsp_t {
		uint32_t chip_id;
		} __packed;

		struct diag_cmd_diag_id_query_req_t {
		struct diag_pkt_header_t header;
		uint8_t version;
		} __packed;

		struct diag_id_tbl_t {
		struct list_head link;
		uint8_t diag_id;
		char *process_name;
		} __packed;
		struct diag_id_t {
		uint8_t diag_id;
		uint8_t len;
		char *process_name;
		} __packed;

		struct diag_cmd_diag_id_query_rsp_t {
		struct diag_pkt_header_t header;
		uint8_t version;
		uint8_t num_entries;
		struct diag_id_t entry;
		} __packed;

		struct diag_cmd_time_sync_query_req_t {
		struct diag_pkt_header_t header;
		uint8_t version;
		@@ -511,6 +543,8 @@ struct diagchar_dev {
		int dci_state;
		struct workqueue_struct *diag_dci_wq;
		struct list_head cmd_reg_list;
		struct list_head diag_id_list;
		struct mutex diag_id_mutex;
		struct mutex cmd_reg_mutex;
		uint32_t cmd_reg_count;
		struct mutex diagfwd_channel_mutex;

drivers/char/diag/diagchar_core.c

+4 −2

Original line number	Diff line number	Diff line
		/* Copyright (c) 2008-2016, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2008-2017, 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
		@@ -3513,7 +3513,9 @@ static int __init diagchar_init(void)
		ret = diagchar_setup_cdev(dev);
		if (ret)
		goto fail;

		mutex_init(&driver->diag_id_mutex);
		INIT_LIST_HEAD(&driver->diag_id_list);
		diag_add_diag_id_to_list(DIAG_ID_APPS, "APPS");
		pr_debug("diagchar initialized now");
		ret = diagfwd_bridge_init();
		if (ret)

drivers/char/diag/diagfwd.c

+61 −1

Original line number	Diff line number	Diff line
		/* Copyright (c) 2008-2016, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2008-2017, 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
		@@ -650,6 +650,55 @@ int diag_process_time_sync_query_cmd(unsigned char *src_buf, int src_len,
		return write_len;
		}

		int diag_process_diag_id_query_cmd(unsigned char *src_buf, int src_len,
		unsigned char *dest_buf, int dest_len)
		{
		int write_len = 0;
		struct diag_cmd_diag_id_query_req_t *req = NULL;
		struct diag_cmd_diag_id_query_rsp_t rsp;
		struct list_head *start;
		struct list_head *temp;
		struct diag_id_tbl_t *item = NULL;
		int rsp_len = 0;
		int num_entries = 0;
		uint8_t process_name_len = 0;

		if (!src_buf \|\| !dest_buf \|\| src_len <= 0 \|\| dest_len <= 0) {
		pr_err("diag: Invalid input in %s, src_buf:%pK, src_len:%d, dest_buf:%pK, dest_len:%d\n",
		__func__, src_buf, src_len, dest_buf, dest_len);
		return -EINVAL;
		}
		req = (struct diag_cmd_diag_id_query_req_t *) src_buf;
		rsp.header.cmd_code = req->header.cmd_code;
		rsp.header.subsys_id = req->header.subsys_id;
		rsp.header.subsys_cmd_code = req->header.subsys_cmd_code;
		rsp.version = req->version;
		rsp.entry.process_name = NULL;
		rsp.entry.len = 0;
		rsp.entry.diag_id = 0;
		write_len = sizeof(rsp.header) + sizeof(rsp.version) +
		sizeof(rsp.num_entries);
		rsp_len = write_len;
		mutex_lock(&driver->diag_id_mutex);
		list_for_each_safe(start, temp, &driver->diag_id_list) {
		item = list_entry(start, struct diag_id_tbl_t, link);
		memcpy(dest_buf + write_len, &item->diag_id,
		sizeof(item->diag_id));
		write_len = write_len + sizeof(item->diag_id);
		process_name_len = strlen(item->process_name) + 1;
		memcpy(dest_buf + write_len, &process_name_len,
		sizeof(process_name_len));
		write_len = write_len + sizeof(process_name_len);
		memcpy(dest_buf + write_len, item->process_name,
		strlen(item->process_name) + 1);
		write_len = write_len + strlen(item->process_name) + 1;
		num_entries++;
		}
		mutex_unlock(&driver->diag_id_mutex);
		rsp.num_entries = num_entries;
		memcpy(dest_buf, &rsp, rsp_len);
		return write_len;
		}
		int diag_process_time_sync_switch_cmd(unsigned char *src_buf, int src_len,
		unsigned char *dest_buf, int dest_len)
		{
		@@ -993,6 +1042,17 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		return 0;
		}
		/* Check for diag id command */
		else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&
		(*(buf+1) == DIAG_SS_DIAG) &&
		((uint16_t )(buf+2) == DIAG_GET_DIAG_ID)) {
		write_len = diag_process_diag_id_query_cmd(buf, len,
		driver->apps_rsp_buf,
		DIAG_MAX_RSP_SIZE);
		if (write_len > 0)
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		return 0;
		}
		/* Check for download command */
		else if ((chk_apps_master()) && (*buf == 0x3A)) {
		/* send response back */

drivers/char/diag/diagfwd_cntl.c

+88 −1

Original line number	Diff line number	Diff line
		/* Copyright (c) 2011-2016, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2011-2017, 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
		@@ -29,6 +29,7 @@

		/* tracks which peripheral is undergoing SSR */
		static uint16_t reg_dirty;
		static uint8_t diag_id = DIAG_ID_APPS;
		static void diag_notify_md_client(uint8_t peripheral, int data);

		static void diag_mask_update_work_fn(struct work_struct *work)
		@@ -648,6 +649,88 @@ static void process_build_mask_report(uint8_t *buf, uint32_t len,
		}
		}

		int diag_add_diag_id_to_list(uint8_t diag_id, char *process_name)
		{
		struct diag_id_tbl_t *new_item = NULL;

		if (!process_name \|\| diag_id == 0)
		return -EINVAL;

		new_item = kzalloc(sizeof(struct diag_id_tbl_t), GFP_KERNEL);
		if (!new_item)
		return -ENOMEM;
		kmemleak_not_leak(new_item);
		new_item->process_name = kzalloc(strlen(process_name), GFP_KERNEL);
		if (!new_item->process_name) {
		kfree(new_item);
		new_item = NULL;
		return -ENOMEM;
		}
		kmemleak_not_leak(new_item->process_name);
		new_item->diag_id = diag_id;
		strlcpy(new_item->process_name, process_name, strlen(process_name) + 1);
		INIT_LIST_HEAD(&new_item->link);
		mutex_lock(&driver->diag_id_mutex);
		list_add_tail(&new_item->link, &driver->diag_id_list);
		mutex_unlock(&driver->diag_id_mutex);
		return 0;
		}

		int diag_query_diag_id(char process_name, uint8_t diag_id)
		{
		struct list_head *start;
		struct list_head *temp;
		struct diag_id_tbl_t *item = NULL;

		if (!process_name \|\| !diag_id)
		return -EINVAL;

		mutex_lock(&driver->diag_id_mutex);
		list_for_each_safe(start, temp, &driver->diag_id_list) {
		item = list_entry(start, struct diag_id_tbl_t, link);
		if (strcmp(item->process_name, process_name) == 0) {
		*diag_id = item->diag_id;
		mutex_unlock(&driver->diag_id_mutex);
		return 1;
		}
		}
		mutex_unlock(&driver->diag_id_mutex);
		return 0;
		}
		static void process_diagid(uint8_t *buf, uint32_t len,
		uint8_t peripheral)
		{
		struct diag_ctrl_diagid *header = NULL;
		struct diag_ctrl_diagid ctrl_pkt;
		char *process_name = NULL;
		int err = 0;
		uint8_t local_diag_id = 0;

		if (!buf \|\| len == 0 \|\| peripheral >= NUM_PERIPHERALS)
		return;
		header = (struct diag_ctrl_diagid *)buf;
		process_name = (char *)&header->process_name;
		if (diag_query_diag_id(process_name, &local_diag_id))
		ctrl_pkt.diag_id = local_diag_id;
		else {
		diag_id++;
		diag_add_diag_id_to_list(diag_id, process_name);
		ctrl_pkt.diag_id = diag_id;
		}
		ctrl_pkt.pkt_id = DIAG_CTRL_MSG_DIAGID;
		ctrl_pkt.version = 1;
		strlcpy((char *)&ctrl_pkt.process_name, process_name,
		strlen(process_name) + 1);
		ctrl_pkt.len = sizeof(ctrl_pkt.diag_id) + sizeof(ctrl_pkt.version) +
		strlen(process_name) + 1;
		err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, ctrl_pkt.len +
		sizeof(ctrl_pkt.pkt_id) + sizeof(ctrl_pkt.len));
		if (err && err != -ENODEV) {
		pr_err("diag: Unable to send diag id ctrl packet to peripheral %d, err: %d\n",
		peripheral, err);
		}
		}

		void diag_cntl_process_read_data(struct diagfwd_info p_info, void buf,
		int len)
		{
		@@ -700,6 +783,10 @@ void diag_cntl_process_read_data(struct diagfwd_info p_info, void buf,
		process_pd_status(ptr, ctrl_pkt->len,
		p_info->peripheral);
		break;
		case DIAG_CTRL_MSG_DIAGID:
		process_diagid(ptr, ctrl_pkt->len,
		p_info->peripheral);
		break;
		default:
		pr_debug("diag: Control packet %d not supported\n",
		ctrl_pkt->pkt_id);