Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit e3dff7b5 authored by Ravi Aravamudhan's avatar Ravi Aravamudhan
Browse files

diag: Make changes to diag control packets 



Refactor handling of diag control packets and make them robust
by adding more error handling.

Change-Id: I90d0212a10269ac00cc1bd3731f38f20e3918b2e
Signed-off-by: default avatarRavi Aravamudhan <aravamud@codeaurora.org>
parent 4d9cdfe4

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;