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

Commit 3596c949 authored by Mohit Aggarwal's avatar Mohit Aggarwal
Browse files

diag: Add support for time sync command 



This patch adds support for time sync command.
Also forwards the time sync control packet to all
the peripherals on receiving time sync switch command.

Change-Id: I5c42203bcf3cd830dbf937879b01d002c73c0c31
Signed-off-by: default avatarMohit Aggarwal <maggarwa@codeaurora.org>
parent 1779195e

drivers/char/diag/diag_masks.c

+36 −0
Original line number Diff line number Diff line
@@ -339,6 +339,40 @@ err: 
	mutex_unlock(&msg_mask.lock);

}



static void diag_send_time_sync_update(uint8_t peripheral)

{

	struct diag_ctrl_msg_time_sync time_sync_msg;

	int msg_size = sizeof(struct diag_ctrl_msg_time_sync);

	int err = 0;



	if (peripheral >= NUM_PERIPHERALS) {

		pr_err("diag: In %s, Invalid peripheral, %d\n",

				__func__, peripheral);

		return;

	}



	if (!driver->diagfwd_cntl[peripheral] ||

		!driver->diagfwd_cntl[peripheral]->ch_open) {

		pr_err("diag: In %s, control channel is not open, p: %d, %p\n",

			__func__, peripheral, driver->diagfwd_cntl[peripheral]);

		return;

	}



	mutex_lock(&driver->diag_cntl_mutex);

	time_sync_msg.ctrl_pkt_id = DIAG_CTRL_MSG_TIME_SYNC_PKT;

	time_sync_msg.ctrl_pkt_data_len = 5;

	time_sync_msg.version = 1;

	time_sync_msg.time_api = driver->uses_time_api;



	err = diagfwd_write(peripheral, TYPE_CNTL, &time_sync_msg, msg_size);

	if (err) {

		pr_err("diag: In %s, unable to write to peripheral: %d, type: %d, len: %d, err: %d\n",

				__func__, peripheral, TYPE_CNTL,

				msg_size, err);

	}

	mutex_unlock(&driver->diag_cntl_mutex);

}



static void diag_send_feature_mask_update(uint8_t peripheral)

{

	void *buf = driver->buf_feature_mask_update;
@@ -1498,6 +1532,8 @@ void diag_send_updates_peripheral(uint8_t peripheral) 
				driver->real_time_mode[DIAG_LOCAL_PROC]);

	diag_send_peripheral_buffering_mode(

				&driver->buffering_mode[peripheral]);

	if (driver->time_sync_enabled)

		diag_send_time_sync_update(peripheral);

}



int diag_process_apps_masks(unsigned char *buf, int len)

drivers/char/diag/diagchar.h

+34 −0
Original line number Diff line number Diff line
@@ -113,6 +113,8 @@ 
#define DIAG_DEL_RSP_WRAP	0x04

#define DIAG_DEL_RSP_WRAP_CNT	0x05

#define DIAG_EXT_MOBILE_ID	0x06

#define DIAG_GET_TIME_API	0x21B

#define DIAG_SET_TIME_API	0x21C



#define DIAG_CMD_OP_LOG_DISABLE		0

#define DIAG_CMD_OP_GET_LOG_RANGE	1
@@ -139,6 +141,10 @@ 


#define BAD_PARAM_RESPONSE_MESSAGE 20



#define PERSIST_TIME_SUCCESS 0

#define PERSIST_TIME_FAILURE 1

#define PERSIST_TIME_NOT_SUPPORTED 2



#define MODE_CMD	41

#define RESET_ID	2
@@ -249,6 +255,32 @@ struct diag_cmd_ext_mobile_rsp_t { 
	uint32_t chip_id;

} __packed;



struct diag_cmd_time_sync_query_req_t {

	struct diag_pkt_header_t header;

	uint8_t version;

};



struct diag_cmd_time_sync_query_rsp_t {

	struct diag_pkt_header_t header;

	uint8_t version;

	uint8_t time_api;

};



struct diag_cmd_time_sync_switch_req_t {

	struct diag_pkt_header_t header;

	uint8_t version;

	uint8_t time_api;

	uint8_t persist_time;

};



struct diag_cmd_time_sync_switch_rsp_t {

	struct diag_pkt_header_t header;

	uint8_t version;

	uint8_t time_api;

	uint8_t time_api_status;

	uint8_t persist_time_status;

};



struct diag_cmd_reg_entry_t {

	uint16_t cmd_code;

	uint16_t subsys_id;
@@ -527,6 +559,8 @@ struct diagchar_dev { 
	unsigned char *hdlc_encode_buf;

	int hdlc_encode_buf_len;

#endif

	int time_sync_enabled;

	uint8_t uses_time_api;

};



extern struct diagchar_dev *driver;

drivers/char/diag/diagchar_core.c

+6 −0
Original line number Diff line number Diff line
@@ -1058,6 +1058,10 @@ static int mask_request_validate(unsigned char mask_buf[]) 
				return 1;

			else if (ss_cmd == 0x218) /* HDLC Disabled Command*/

				return 0;

			else if (ss_cmd == DIAG_GET_TIME_API)

				return 1;

			else if (ss_cmd == DIAG_SET_TIME_API)

				return 1;

			break;

		case 0x13: /* DIAG_SUBSYS_FS */

			if ((ss_cmd == 0) || (ss_cmd == 0x1))
@@ -2795,6 +2799,8 @@ static int __init diagchar_init(void) 
	driver->dci_state = DIAG_DCI_NO_ERROR;

	setup_timer(&drain_timer, drain_timer_func, 1234);

	driver->supports_sockets = 1;

	driver->time_sync_enabled = 0;

	driver->uses_time_api = 0;

	driver->poolsize = poolsize;

	driver->poolsize_hdlc = poolsize_hdlc;

	driver->poolsize_dci = poolsize_dci;

drivers/char/diag/diagfwd.c

+120 −0
Original line number Diff line number Diff line
@@ -48,6 +48,9 @@ 
#define STM_RSP_STATUS_INDEX		8

#define STM_RSP_NUM_BYTES		9



int timestamp_switch;

module_param(timestamp_switch, int, 0644);



int wrap_enabled;

uint16_t wrap_count;

static struct diag_hdlc_decode_type *hdlc_decode;
@@ -565,6 +568,101 @@ int diag_process_stm_cmd(unsigned char *buf, unsigned char *dest_buf) 
	return STM_RSP_NUM_BYTES;

}



int diag_process_time_sync_query_cmd(unsigned char *src_buf, int src_len,

				      unsigned char *dest_buf, int dest_len)

{

	int write_len = 0;

	struct diag_cmd_time_sync_query_req_t *req = NULL;

	struct diag_cmd_time_sync_query_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_cmd_time_sync_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.time_api = driver->uses_time_api;

	memcpy(dest_buf, &rsp, sizeof(rsp));

	write_len = sizeof(rsp);

	return write_len;

}



int diag_process_time_sync_switch_cmd(unsigned char *src_buf, int src_len,

				      unsigned char *dest_buf, int dest_len)

{

	uint8_t peripheral, status;

	struct diag_cmd_time_sync_switch_req_t *req = NULL;

	struct diag_cmd_time_sync_switch_rsp_t rsp;

	struct diag_ctrl_msg_time_sync time_sync_msg;

	int msg_size = sizeof(struct diag_ctrl_msg_time_sync);

	int err = 0, write_len = 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;

	}



	req = (struct diag_cmd_time_sync_switch_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.time_api = req->time_api;

	if ((req->version > 1) || (req->time_api > 1) ||

					(req->persist_time > 0)) {

		dest_buf[0] = BAD_PARAM_RESPONSE_MESSAGE;

		rsp.time_api_status = 0;

		rsp.persist_time_status = PERSIST_TIME_NOT_SUPPORTED;

		memcpy(dest_buf + 1, &rsp, sizeof(rsp));

		write_len = sizeof(rsp) + 1;

		timestamp_switch = 0;

		return write_len;

	}



	time_sync_msg.ctrl_pkt_id = DIAG_CTRL_MSG_TIME_SYNC_PKT;

	time_sync_msg.ctrl_pkt_data_len = 5;

	time_sync_msg.version = 1;

	time_sync_msg.time_api = req->time_api;



	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {

		err = diagfwd_write(peripheral, TYPE_CNTL, &time_sync_msg,

					msg_size);

		if (err && err != -ENODEV) {

			pr_err("diag: In %s, unable to write to peripheral: %d, type: %d, len: %d, err: %d\n",

				__func__, peripheral, TYPE_CNTL,

				msg_size, err);

			status |= (1 << peripheral);

		}

	}



	driver->time_sync_enabled = 1;

	driver->uses_time_api = req->time_api;



	switch (req->time_api) {

	case 0:

		timestamp_switch = 0;

		break;

	case 1:

		timestamp_switch = 1;

		break;

	default:

		timestamp_switch = 0;

		break;

	}



	rsp.time_api_status = status;

	rsp.persist_time_status = PERSIST_TIME_NOT_SUPPORTED;

	memcpy(dest_buf, &rsp, sizeof(rsp));

	write_len = sizeof(rsp);

	return write_len;

}



int diag_cmd_log_on_demand(unsigned char *src_buf, int src_len,

			   unsigned char *dest_buf, int dest_len)

{
@@ -804,6 +902,28 @@ int diag_process_apps_pkt(unsigned char *buf, int len) 
		}

		return len;

	}

	/* Check for time sync query command */

	else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&

		(*(buf+1) == DIAG_SS_DIAG) &&

		(*(uint16_t *)(buf+2) == DIAG_GET_TIME_API)) {

		write_len = diag_process_time_sync_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 time sync switch command */

	else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&

		(*(buf+1) == DIAG_SS_DIAG) &&

		(*(uint16_t *)(buf+2) == DIAG_SET_TIME_API)) {

		write_len = diag_process_time_sync_switch_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 ((cpu_is_msm8x60() || chk_apps_master()) && (*buf == 0x3A)) {

		/* send response back */

drivers/char/diag/diagfwd_cntl.h

+8 −0
Original line number Diff line number Diff line
@@ -45,6 +45,7 @@ 
#define DIAG_CTRL_MSG_DEREG		27

#define DIAG_CTRL_MSG_DCI_HANDSHAKE_PKT		29

#define DIAG_CTRL_MSG_PD_STATUS			30

#define DIAG_CTRL_MSG_TIME_SYNC_PKT		31



/*

 * Feature Mask Definitions: Feature mask is used to sepcify Diag features
@@ -172,6 +173,13 @@ struct diag_ctrl_msg_stm { 
	uint8_t  control_data;

} __packed;



struct diag_ctrl_msg_time_sync {

	uint32_t ctrl_pkt_id;

	uint32_t ctrl_pkt_data_len;

	uint32_t version;

	uint8_t  time_api;

} __packed;



struct diag_ctrl_dci_status {

	uint32_t ctrl_pkt_id;

	uint32_t ctrl_pkt_data_len;