Merge "diag: Update Diag over STM to support new command protocol" (1fe7c416) · Commits · e / devices / android_kernel_sony_msm8994

drivers/char/diag/diagfwd.c

+25 −41

Original line number	Diff line number	Diff line
		@@ -45,12 +45,9 @@
		#define STM_CMD_DATA_OFFSET 6
		#define STM_CMD_NUM_BYTES 7

		#define STM_RSP_VALID_INDEX 7
		#define STM_RSP_SUPPORTED_INDEX 8
		#define STM_RSP_SMD_COMPLY_INDEX 9
		#define STM_RSP_NUM_BYTES 10

		#define STM_COMMAND_VALID 1
		#define STM_RSP_SUPPORTED_INDEX 7
		#define STM_RSP_SMD_STATUS_INDEX 8
		#define STM_RSP_NUM_BYTES 9

		#define SMD_DRAIN_BUF_SIZE 4096

		@@ -1185,8 +1182,7 @@ int diag_send_data(struct diag_master_table entry, unsigned char *buf,
		return success;
		}

		void diag_process_stm_mask(uint8_t cmd, uint8_t data_mask, int data_type,
		uint8_t rsp_supported, uint8_t rsp_smd_comply)
		void diag_process_stm_mask(uint8_t cmd, uint8_t data_mask, int data_type)
		{
		int status = 0;
		if (data_type >= MODEM_DATA && data_type <= WCNSS_DATA) {
		@@ -1194,19 +1190,10 @@ void diag_process_stm_mask(uint8_t cmd, uint8_t data_mask, int data_type,
		status = diag_send_stm_state(
		&driver->smd_cntl[data_type], cmd);
		if (status == 1)
		*rsp_smd_comply \|= data_mask;
		*rsp_supported \|= data_mask;
		} else if (driver->smd_cntl[data_type].ch) {
		*rsp_smd_comply \|= data_mask;
		}
		if ((*rsp_smd_comply & data_mask) &&
		(*rsp_supported & data_mask))
		driver->stm_state[data_type] = cmd;

		}
		driver->stm_state_requested[data_type] = cmd;
		} else if (data_type == APPS_DATA) {
		*rsp_supported \|= data_mask;
		*rsp_smd_comply \|= data_mask;
		driver->stm_state[data_type] = cmd;
		driver->stm_state_requested[data_type] = cmd;
		}
		@@ -1216,7 +1203,7 @@ int diag_process_stm_cmd(unsigned char buf, unsigned char dest_buf)
		{
		uint8_t version, mask, cmd;
		uint8_t rsp_supported = 0;
		uint8_t rsp_smd_comply = 0;
		uint8_t rsp_smd_status = 0;
		int i;

		if (!buf \|\| !dest_buf) {
		@@ -1233,47 +1220,44 @@ int diag_process_stm_cmd(unsigned char buf, unsigned char dest_buf)
		* Check if command is valid. If the command is asking for
		* status, then the processor mask field is to be ignored.
		*/
		if ((version != 1) \|\| (cmd > STATUS_STM) \|\|
		if ((version != 2) \|\| (cmd > STATUS_STM) \|\|
		((cmd != STATUS_STM) && ((mask == 0) \|\| (0 != (mask >> 4))))) {
		/* Command is invalid. Send bad param message response */
		dest_buf[0] = BAD_PARAM_RESPONSE_MESSAGE;
		for (i = 0; i < STM_CMD_NUM_BYTES; i++)
		dest_buf[i+1] = *(buf + i);
		return STM_CMD_NUM_BYTES+1;
		} else if (cmd == STATUS_STM) {
		/*
		* Only the status is being queried, so fill in whether diag
		* over stm is supported or not
		*/
		for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		if (driver->peripheral_supports_stm[i])
		rsp_supported \|= 1 << i;

		rsp_supported \|= DIAG_STM_APPS;
		} else {
		} else if (cmd != STATUS_STM) {
		if (mask & DIAG_STM_MODEM)
		diag_process_stm_mask(cmd, DIAG_STM_MODEM, MODEM_DATA,
		&rsp_supported, &rsp_smd_comply);
		diag_process_stm_mask(cmd, DIAG_STM_MODEM, MODEM_DATA);

		if (mask & DIAG_STM_LPASS)
		diag_process_stm_mask(cmd, DIAG_STM_LPASS, LPASS_DATA,
		&rsp_supported, &rsp_smd_comply);
		diag_process_stm_mask(cmd, DIAG_STM_LPASS, LPASS_DATA);

		if (mask & DIAG_STM_WCNSS)
		diag_process_stm_mask(cmd, DIAG_STM_WCNSS, WCNSS_DATA,
		&rsp_supported, &rsp_smd_comply);
		diag_process_stm_mask(cmd, DIAG_STM_WCNSS, WCNSS_DATA);

		if (mask & DIAG_STM_APPS)
		diag_process_stm_mask(cmd, DIAG_STM_APPS, APPS_DATA,
		&rsp_supported, &rsp_smd_comply);
		diag_process_stm_mask(cmd, DIAG_STM_APPS, APPS_DATA);
		}

		for (i = 0; i < STM_CMD_NUM_BYTES; i++)
		dest_buf[i] = *(buf + i);

		dest_buf[STM_RSP_VALID_INDEX] = STM_COMMAND_VALID;
		/* Set mask denoting which peripherals support STM */
		for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
		if (driver->peripheral_supports_stm[i])
		rsp_supported \|= 1 << i;

		rsp_supported \|= DIAG_STM_APPS;

		/* Set mask denoting STM state/status for each peripheral/APSS */
		for (i = 0; i < NUM_STM_PROCESSORS; i++)
		if (driver->stm_state[i])
		rsp_smd_status \|= 1 << i;

		dest_buf[STM_RSP_SUPPORTED_INDEX] = rsp_supported;
		dest_buf[STM_RSP_SMD_COMPLY_INDEX] = rsp_smd_comply;
		dest_buf[STM_RSP_SMD_STATUS_INDEX] = rsp_smd_status;

		return STM_RSP_NUM_BYTES;
		}

drivers/char/diag/diagfwd_cntl.c

+8 −1

Original line number	Diff line number	Diff line
		@@ -75,9 +75,16 @@ void diag_cntl_stm_notify(struct diag_smd_info *smd_info, int action)
		if (!smd_info \|\| smd_info->type != SMD_CNTL_TYPE)
		return;

		if (action == CLEAR_PERIPHERAL_STM_STATE)
		if (action == CLEAR_PERIPHERAL_STM_STATE) {
		driver->peripheral_supports_stm[smd_info->peripheral] =
		DISABLE_STM;
		/*
		* Turn off STM for now until such time as the
		* tools can support SSR
		*/
		driver->stm_state[smd_info->peripheral] = DISABLE_STM;
		driver->stm_state_requested[smd_info->peripheral] = DISABLE_STM;
		}
		}

		static void process_stm_feature(struct diag_smd_info *smd_info,