Merge "diag: Make changes to diag control packets" (1bfd3d1e) · Commits · e / devices / android_kernel_sony_msm8994

drivers/char/diag/diag_masks.c

+17 −25

Original line number	Diff line number	Diff line
		@@ -29,7 +29,7 @@ int diag_event_num_bytes;
		#define ALL_EQUIP_ID 100
		#define ALL_SSID -1

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

		struct mask_info {
		int equip_id;
		@@ -561,7 +561,8 @@ 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_BYTES] = {0, 0};
		uint8_t feature_bytes[FEATURE_MASK_LEN] = {0, 0};
		struct diag_ctrl_feature_mask feature_mask;
		int total_len = 0;
		int err = 0;

		@@ -579,19 +580,19 @@ void diag_send_feature_mask_update(struct diag_smd_info *smd_info)

		mutex_lock(&driver->diag_cntl_mutex);
		/* send feature mask update */
		driver->feature_mask->ctrl_pkt_id = DIAG_CTRL_MSG_FEATURE;
		driver->feature_mask->ctrl_pkt_data_len = 4 + FEATURE_MASK_LEN_BYTES;
		driver->feature_mask->feature_mask_len = FEATURE_MASK_LEN_BYTES;
		memcpy(buf, driver->feature_mask, header_size);
		feature_bytes[0] \|= F_DIAG_INT_FEATURE_MASK;
		feature_bytes[0] \|= F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
		feature_bytes[0] \|= driver->supports_separate_cmdrsp ?
		F_DIAG_REQ_RSP_CHANNEL : 0;
		feature_bytes[0] \|= driver->supports_apps_hdlc_encoding ?
		F_DIAG_HDLC_ENCODE_IN_APPS_MASK : 0;
		feature_bytes[1] \|= F_DIAG_OVER_STM;
		memcpy(buf+header_size, &feature_bytes, FEATURE_MASK_LEN_BYTES);
		total_len = header_size + FEATURE_MASK_LEN_BYTES;
		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);
		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) {
		@@ -911,18 +912,11 @@ int diag_masks_init(void)
		if (driver->buf_feature_mask_update == NULL) {
		driver->buf_feature_mask_update = kzalloc(sizeof(
		struct diag_ctrl_feature_mask) +
		FEATURE_MASK_LEN_BYTES, GFP_KERNEL);
		FEATURE_MASK_LEN, GFP_KERNEL);
		if (driver->buf_feature_mask_update == NULL)
		goto err;
		kmemleak_not_leak(driver->buf_feature_mask_update);
		}
		if (driver->feature_mask == NULL) {
		driver->feature_mask = kzalloc(sizeof(
		struct diag_ctrl_feature_mask), GFP_KERNEL);
		if (driver->feature_mask == NULL)
		goto err;
		kmemleak_not_leak(driver->feature_mask);
		}
		diag_create_msg_mask_table();
		diag_event_num_bytes = 0;
		if (driver->log_masks == NULL) {
		@@ -947,7 +941,6 @@ err:
		kfree(driver->msg_masks);
		kfree(driver->log_masks);
		kfree(driver->event_masks);
		kfree(driver->feature_mask);
		kfree(driver->buf_feature_mask_update);
		return -ENOMEM;
		}
		@@ -960,6 +953,5 @@ void diag_masks_exit(void)
		kfree(driver->msg_masks);
		kfree(driver->log_masks);
		kfree(driver->event_masks);
		kfree(driver->feature_mask);
		kfree(driver->buf_feature_mask_update);
		}

drivers/char/diag/diagchar.h

+3 −1

Original line number	Diff line number	Diff line
		@@ -134,6 +134,8 @@
		#define MODE_CMD 41
		#define RESET_ID 2

		#define FEATURE_MASK_LEN 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
		@@ -405,7 +407,6 @@ struct diagchar_dev {
		struct diag_ctrl_event_mask *event_mask;
		struct diag_ctrl_log_mask *log_mask;
		struct diag_ctrl_msg_mask *msg_mask;
		struct diag_ctrl_feature_mask *feature_mask;
		struct mutex log_mask_mutex;
		/* Members for Sending response */
		unsigned char *encoded_rsp_buf;
		@@ -421,6 +422,7 @@ struct diagchar_dev {
		int rcvd_feature_mask[NUM_SMD_CONTROL_CHANNELS];
		int separate_cmdrsp[NUM_SMD_CONTROL_CHANNELS];
		unsigned char *usb_buf_out;
		uint8_t peripheral_feature[NUM_SMD_CONTROL_CHANNELS][FEATURE_MASK_LEN];
		unsigned char *apps_rsp_buf;
		unsigned char *user_space_data_buf;
		/* buffer for updating mask to peripherals */

drivers/char/diag/diagfwd_cntl.c

+150 −134

Original line number	Diff line number	Diff line
		@@ -21,9 +21,11 @@
		#include "diagfwd_hsic.h"
		#include "diag_dci.h"
		#include "diagmem.h"

		#define FEATURE_SUPPORTED(x) ((feature_mask << (i * 8)) & (1 << x))

		/* tracks which peripheral is undergoing SSR */
		static uint16_t reg_dirty;
		#define HDR_SIZ 8

		void diag_clean_reg_fn(struct work_struct *work)
		{
		@@ -87,30 +89,20 @@ void diag_cntl_stm_notify(struct diag_smd_info *smd_info, int action)
		}
		}

		static void process_stm_feature(struct diag_smd_info *smd_info,
		uint8_t feature_mask)
		static void enable_stm_feature(struct diag_smd_info *smd_info)
		{
		if (feature_mask & F_DIAG_OVER_STM) {
		driver->peripheral_supports_stm[smd_info->peripheral] =
		ENABLE_STM;
		driver->peripheral_supports_stm[smd_info->peripheral] = ENABLE_STM;
		smd_info->general_context = UPDATE_PERIPHERAL_STM_STATE;
		queue_work(driver->diag_cntl_wq,
		&(smd_info->diag_general_smd_work));
		} else {
		driver->peripheral_supports_stm[smd_info->peripheral] =
		DISABLE_STM;
		}
		queue_work(driver->diag_cntl_wq, &(smd_info->diag_general_smd_work));
		}

		static void process_hdlc_encoding_feature(struct diag_smd_info *smd_info,
		uint8_t feature_mask)
		static void process_hdlc_encoding_feature(struct diag_smd_info *smd_info)
		{
		/*
		* Check if apps supports hdlc encoding and the
		* peripheral supports apps hdlc encoding
		*/
		if (driver->supports_apps_hdlc_encoding &&
		(feature_mask & F_DIAG_HDLC_ENCODE_IN_APPS_MASK)) {
		if (driver->supports_apps_hdlc_encoding) {
		driver->smd_data[smd_info->peripheral].encode_hdlc =
		ENABLE_APPS_HDLC_ENCODING;
		if (driver->separate_cmdrsp[smd_info->peripheral] &&
		@@ -127,134 +119,158 @@ static void process_hdlc_encoding_feature(struct diag_smd_info *smd_info,
		}
		}

		/* Process the data read from the smd control channel */
		int diag_process_smd_cntl_read_data(struct diag_smd_info smd_info, void buf,
		int total_recd)
		static void process_command_registration(uint8_t *buf, uint32_t len,
		struct diag_smd_info *smd_info)
		{
		int data_len = 0, type = -1, count_bytes = 0, j, flag = 0;
		struct bindpkt_params_per_process *pkt_params =
		kzalloc(sizeof(struct bindpkt_params_per_process), GFP_KERNEL);
		struct diag_ctrl_msg *msg;
		struct cmd_code_range *range;
		struct bindpkt_params *temp;

		if (pkt_params == NULL) {
		pr_alert("diag: In %s, Memory allocation failure\n",
		__func__);
		return 0;
		}
		uint8_t *ptr = buf;
		int i;
		int header_len = sizeof(struct diag_ctrl_cmd_reg);
		int read_len = 0;
		struct bindpkt_params_per_process *pkt_params = NULL;
		struct bindpkt_params *temp = NULL;
		struct diag_ctrl_cmd_reg *reg = NULL;
		struct cmd_code_range *range = NULL;

		if (!smd_info) {
		pr_err("diag: In %s, No smd info. Not able to read.\n",
		__func__);
		kfree(pkt_params);
		return 0;
		}
		/*
		* Perform Basic sanity. The len field is the size of the data payload.
		* This doesn't include the header size.
		*/
		if (!buf \|\| !smd_info \|\| len == 0)
		return;

		while (count_bytes + HDR_SIZ <= total_recd) {
		type = (uint32_t )(buf);
		data_len = (uint32_t )(buf + 4);
		if (type < DIAG_CTRL_MSG_REG \|\|
		type > DIAG_CTRL_MSG_LAST) {
		pr_alert("diag: In %s, Invalid Msg type %d proc %d",
		__func__, type, smd_info->peripheral);
		break;
		}
		if (data_len < 0 \|\| data_len > total_recd) {
		pr_alert("diag: In %s, Invalid data len %d, total_recd: %d, proc %d",
		__func__, data_len, total_recd,
		/* Peripheral undergoing SSR should not record new registration */
		if (reg_dirty & smd_info->peripheral_mask) {
		pr_err("diag: dropping command registration from peripheral %d\n",
		smd_info->peripheral);
		break;
		return;
		}
		count_bytes = count_bytes+HDR_SIZ+data_len;
		if (type == DIAG_CTRL_MSG_REG && total_recd >= count_bytes) {
		msg = buf+HDR_SIZ;
		range = buf+HDR_SIZ+
		sizeof(struct diag_ctrl_msg);
		if (msg->count_entries == 0) {

		reg = (struct diag_ctrl_cmd_reg *)ptr;
		ptr += header_len;

		if (reg->count_entries == 0) {
		pr_debug("diag: In %s, received reg tbl with no entries\n",
		__func__);
		buf = buf + HDR_SIZ + data_len;
		continue;
		return;
		}

		pkt_params = kzalloc(sizeof(struct bindpkt_params_per_process),
		GFP_KERNEL);
		if (!pkt_params) {
		pr_err("diag: In %s, unable to allocate memory for new command table entry\n",
		__func__);
		return;
		}
		pkt_params->count = msg->count_entries;
		pkt_params->count = reg->count_entries;
		pkt_params->params = kzalloc(pkt_params->count *
		sizeof(struct bindpkt_params), GFP_KERNEL);
		sizeof(struct bindpkt_params),
		GFP_KERNEL);
		if (!pkt_params->params) {
		pr_alert("diag: In %s, Memory alloc fail for cmd_code: %d, subsys: %d\n",
		__func__, msg->cmd_code,
		msg->subsysid);
		buf = buf + HDR_SIZ + data_len;
		continue;
		pr_err("diag: In %s, Memory alloc fail for cmd_code: %d, subsys: %d\n",
		__func__, reg->cmd_code, reg->subsysid);
		kfree(pkt_params);
		return;
		}

		temp = pkt_params->params;
		for (j = 0; j < pkt_params->count; j++) {
		temp->cmd_code = msg->cmd_code;
		temp->subsys_id = msg->subsysid;
		for (i = 0; i < reg->count_entries && read_len < len; i++, temp++) {
		temp->cmd_code = reg->cmd_code;
		temp->subsys_id = reg->subsysid;
		temp->client_id = smd_info->peripheral;
		temp->proc_id = NON_APPS_PROC;
		range = (struct cmd_code_range *)ptr;
		temp->cmd_code_lo = range->cmd_code_lo;
		temp->cmd_code_hi = range->cmd_code_hi;
		range++;
		temp++;
		ptr += sizeof(struct cmd_code_range);
		read_len += sizeof(struct cmd_code_range);
		}
		flag = 1;
		/* peripheral undergoing SSR should not
		* record new registration
		*/
		if (!(reg_dirty & smd_info->peripheral_mask))
		diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG,
		(unsigned long)pkt_params);
		else
		pr_err("diag: drop reg proc %d\n",
		smd_info->peripheral);

		diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG, (unsigned long)pkt_params);
		kfree(pkt_params->params);
		} else if (type == DIAG_CTRL_MSG_FEATURE &&
		total_recd >= count_bytes) {
		uint8_t feature_mask = 0;
		int feature_mask_len = (int )(buf+8);
		if (feature_mask_len > 0) {
		int periph = smd_info->peripheral;
		driver->rcvd_feature_mask[smd_info->peripheral]
		= 1;
		feature_mask = (uint8_t )(buf+12);
		if (periph == MODEM_DATA)
		driver->log_on_demand_support =
		feature_mask &
		F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
		/*
		* If apps supports separate cmd/rsp channels
		* and the peripheral supports separate cmd/rsp
		* channels
		*/
		if (driver->supports_separate_cmdrsp &&
		(feature_mask & F_DIAG_REQ_RSP_CHANNEL))
		driver->separate_cmdrsp[periph] =
		ENABLE_SEPARATE_CMDRSP;
		else
		driver->separate_cmdrsp[periph] =
		DISABLE_SEPARATE_CMDRSP;
		/*
		* Check if apps supports hdlc encoding and the
		* peripheral supports apps hdlc encoding
		*/
		process_hdlc_encoding_feature(smd_info,
		feature_mask);
		if (feature_mask_len > 1) {
		feature_mask = (uint8_t )(buf+13);
		process_stm_feature(smd_info,
		feature_mask);
		kfree(pkt_params);
		}

		static void process_incoming_feature_mask(uint8_t *buf, uint32_t len,
		struct diag_smd_info *smd_info)
		{
		int i;
		int header_len = sizeof(struct diag_ctrl_feature_mask);
		int read_len = 0;
		struct diag_ctrl_feature_mask *header = NULL;
		uint32_t feature_mask_len = 0;
		uint32_t feature_mask = 0;
		uint8_t *ptr = buf;

		if (!buf \|\| !smd_info \|\| len == 0)
		return;

		header = (struct diag_ctrl_feature_mask *)ptr;
		ptr += header_len;
		feature_mask_len = header->feature_mask_len;

		if (feature_mask_len == 0) {
		pr_debug("diag: In %s, received invalid feature mask from peripheral %d\n",
		__func__, smd_info->peripheral);
		return;
		}
		flag = 1;
		} else if (type != DIAG_CTRL_MSG_REG) {
		flag = 1;

		if (feature_mask_len > FEATURE_MASK_LEN) {
		pr_alert("diag: Receiving feature mask length more than Apps support\n");
		feature_mask_len = FEATURE_MASK_LEN;
		}
		buf = buf + HDR_SIZ + data_len;

		driver->rcvd_feature_mask[smd_info->peripheral] = 1;

		for (i = 0; i < feature_mask_len && read_len < len; i++) {
		feature_mask = (uint8_t )ptr;
		driver->peripheral_feature[smd_info->peripheral][i] =
		feature_mask;
		ptr += sizeof(uint8_t);
		read_len += sizeof(uint8_t);

		if (FEATURE_SUPPORTED(F_DIAG_LOG_ON_DEMAND_APPS))
		driver->log_on_demand_support = 1;
		if (FEATURE_SUPPORTED(F_DIAG_REQ_RSP_SUPPORT))
		driver->separate_cmdrsp[smd_info->peripheral] = 1;
		if (FEATURE_SUPPORTED(F_DIAG_APPS_HDLC_ENCODE))
		process_hdlc_encoding_feature(smd_info);
		if (FEATURE_SUPPORTED(F_DIAG_STM))
		enable_stm_feature(smd_info);
		}
		}

		/* Process the data read from the smd control channel */
		int diag_process_smd_cntl_read_data(struct diag_smd_info smd_info, void buf,
		int total_recd)
		{
		int read_len = 0;
		int header_len = sizeof(struct diag_ctrl_pkt_header_t);
		uint8_t *ptr = buf;
		struct diag_ctrl_pkt_header_t *ctrl_pkt = NULL;

		if (!smd_info \|\| !buf \|\| total_recd <= 0)
		return -EIO;

		while (read_len + header_len < total_recd) {
		ctrl_pkt = (struct diag_ctrl_pkt_header_t *)ptr;
		switch (ctrl_pkt->pkt_id) {
		case DIAG_CTRL_MSG_REG:
		process_command_registration(ptr, ctrl_pkt->len,
		smd_info);
		break;
		case DIAG_CTRL_MSG_FEATURE:
		process_incoming_feature_mask(ptr, ctrl_pkt->len,
		smd_info);
		break;
		default:
		pr_debug("diag: Control packet %d not supported\n",
		ctrl_pkt->pkt_id);
		}
		ptr += header_len + ctrl_pkt->len;
		read_len += header_len + ctrl_pkt->len;
		}
		kfree(pkt_params);

		return flag;
		return 0;
		}

		static int diag_compute_real_time(int idx)

drivers/char/diag/diagfwd_cntl.h

+22 −13

Original line number	Diff line number	Diff line
		@@ -37,20 +37,22 @@
		#define DIAG_CTRL_MSG_DCI_CONNECTION_STATUS 20
		#define DIAG_CTRL_MSG_LAST DIAG_CTRL_MSG_DCI_CONNECTION_STATUS

		/* Denotes that we support sending/receiving the feature mask */
		#define F_DIAG_INT_FEATURE_MASK 0x01
		/* Denotes that we support responding to "Log on Demand" */
		#define F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER 0x04
		/*
		* Supports dedicated main request/response on
		* new Data Rx and DCI Rx channels
		* Feature Mask Definitions: Feature mask is used to sepcify Diag features
		* supported by the Apps processor
		*
		* F_DIAG_FEATURE_MASK_SUPPORT - Denotes we support sending and receiving
		* feature masks
		* F_DIAG_LOG_ON_DEMAND_APPS - Apps responds to Log on Demand request
		* F_DIAG_REQ_RSP_SUPPORT - Apps supported dedicated request response Channel
		* F_DIAG_APPS_HDLC_ENCODE - HDLC encoding is done on the forward channel
		* F_DIAG_STM - Denotes Apps supports Diag over STM
		*/
		#define F_DIAG_REQ_RSP_CHANNEL 0x10
		/* Denotes we support diag over stm */
		#define F_DIAG_OVER_STM 0x02

		/* Perform hdlc encoding of data coming from smd channel */
		#define F_DIAG_HDLC_ENCODE_IN_APPS_MASK 0x40
		#define F_DIAG_FEATURE_MASK_SUPPORT 0
		#define F_DIAG_LOG_ON_DEMAND_APPS 2
		#define F_DIAG_REQ_RSP_SUPPORT 4
		#define F_DIAG_APPS_HDLC_ENCODE 6
		#define F_DIAG_STM 9

		#define ENABLE_SEPARATE_CMDRSP 1
		#define DISABLE_SEPARATE_CMDRSP 0
		@@ -67,13 +69,20 @@

		#define DIAG_MODE_PKT_LEN 36

		struct diag_ctrl_pkt_header_t {
		uint32_t pkt_id;
		uint32_t len;
		};

		struct cmd_code_range {
		uint16_t cmd_code_lo;
		uint16_t cmd_code_hi;
		uint32_t data;
		};

		struct diag_ctrl_msg {
		struct diag_ctrl_cmd_reg {
		uint32_t pkt_id;
		uint32_t len;
		uint32_t version;
		uint16_t cmd_code;
		uint16_t subsysid;