Merge "diag: Add mdlog session to apps pkt response path" (95e27189) · Commits · e / devices / android_kernel_fairphone_FP3

drivers/char/diag/diagchar_core.c

+2 −1

Original line number	Diff line number	Diff line
		@@ -2776,7 +2776,8 @@ static int diag_user_process_raw_data(const char __user *buf, int len)
		mutex_unlock(&driver->md_session_lock);
		ret = diag_process_apps_pkt(user_space_data, len, info);
		if (ret == 1)
		diag_send_error_rsp((void *)(user_space_data), len);
		diag_send_error_rsp((void *)(user_space_data), len,
		info);
		}
		fail:
		diagmem_free(driver, user_space_data, mempool);

drivers/char/diag/diagfwd.c

+84 −39

Original line number	Diff line number	Diff line
		@@ -240,10 +240,11 @@ void chk_logging_wakeup(void)
		}
		}

		static void pack_rsp_and_send(unsigned char *buf, int len)
		static void pack_rsp_and_send(unsigned char *buf, int len,
		struct diag_md_session_t *info)
		{
		int err;
		int retry_count = 0;
		int retry_count = 0, i, rsp_ctxt;
		uint32_t write_len = 0;
		unsigned long flags;
		unsigned char *rsp_ptr = driver->encoded_rsp_buf;
		@@ -258,6 +259,26 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
		return;
		}

		/*
		* Explicitly check for the Peripheral Modem here
		* is necessary till a way to identify a peripheral
		* if its supporting qshrink4 feature.
		*/
		if (info && info->peripheral_mask) {
		if (info->peripheral_mask == DIAG_CON_ALL \|\|
		(info->peripheral_mask & (1 << APPS_DATA)) \|\|
		(info->peripheral_mask & (1 << PERIPHERAL_MODEM))) {
		rsp_ctxt = SET_BUF_CTXT(APPS_DATA, TYPE_CMD, 1);
		} else {
		for (i = 0; i < NUM_MD_SESSIONS; i++) {
		if (info->peripheral_mask & (1 << i))
		break;
		}
		rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, 1);
		}
		} else
		rsp_ctxt = driver->rsp_buf_ctxt;

		/*
		* Keep trying till we get the buffer back. It should probably
		* take one or two iterations. When this loops till UINT_MAX, it
		@@ -299,8 +320,7 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
		(uint8_t )(rsp_ptr + write_len) = CONTROL_CHAR;
		write_len += sizeof(uint8_t);

		err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, write_len,
		driver->rsp_buf_ctxt);
		err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, write_len, rsp_ctxt);
		if (err) {
		pr_err("diag: In %s, unable to write to mux, err: %d\n",
		__func__, err);
		@@ -310,12 +330,13 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
		}
		}

		static void encode_rsp_and_send(unsigned char *buf, int len)
		static void encode_rsp_and_send(unsigned char *buf, int len,
		struct diag_md_session_t *info)
		{
		struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
		struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
		unsigned char *rsp_ptr = driver->encoded_rsp_buf;
		int err, retry_count = 0;
		int err, i, rsp_ctxt, retry_count = 0;
		unsigned long flags;

		if (!rsp_ptr \|\| !buf)
		@@ -327,6 +348,26 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
		return;
		}

		/*
		* Explicitly check for the Peripheral Modem here
		* is necessary till a way to identify a peripheral
		* if its supporting qshrink4 feature.
		*/
		if (info && info->peripheral_mask) {
		if (info->peripheral_mask == DIAG_CON_ALL \|\|
		(info->peripheral_mask & (1 << APPS_DATA)) \|\|
		(info->peripheral_mask & (1 << PERIPHERAL_MODEM))) {
		rsp_ctxt = SET_BUF_CTXT(APPS_DATA, TYPE_CMD, 1);
		} else {
		for (i = 0; i < NUM_MD_SESSIONS; i++) {
		if (info->peripheral_mask & (1 << i))
		break;
		}
		rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, 1);
		}
		} else
		rsp_ctxt = driver->rsp_buf_ctxt;

		/*
		* Keep trying till we get the buffer back. It should probably
		* take one or two iterations. When this loops till UINT_MAX, it
		@@ -370,7 +411,7 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
		diag_hdlc_encode(&send, &enc);
		driver->encoded_rsp_len = (int)(enc.dest - (void *)rsp_ptr);
		err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, driver->encoded_rsp_len,
		driver->rsp_buf_ctxt);
		rsp_ctxt);
		if (err) {
		pr_err("diag: In %s, Unable to write to device, err: %d\n",
		__func__, err);
		@@ -381,21 +422,23 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
		memset(buf, '\0', DIAG_MAX_RSP_SIZE);
		}

		void diag_send_rsp(unsigned char *buf, int len)
		static void diag_send_rsp(unsigned char *buf, int len,
		struct diag_md_session_t *info)
		{
		struct diag_md_session_t *session_info = NULL;
		uint8_t hdlc_disabled;

		session_info = diag_md_session_get_peripheral(APPS_DATA);
		session_info = (info) ? info :
		diag_md_session_get_peripheral(APPS_DATA);
		if (session_info)
		hdlc_disabled = session_info->hdlc_disabled;
		else
		hdlc_disabled = driver->hdlc_disabled;

		if (hdlc_disabled)
		pack_rsp_and_send(buf, len);
		pack_rsp_and_send(buf, len, session_info);
		else
		encode_rsp_and_send(buf, len);
		encode_rsp_and_send(buf, len, session_info);
		}

		void diag_update_pkt_buffer(unsigned char *buf, uint32_t len, int type)
		@@ -926,7 +969,8 @@ static int diag_cmd_disable_hdlc(unsigned char *src_buf, int src_len,
		return write_len;
		}

		void diag_send_error_rsp(unsigned char *buf, int len)
		void diag_send_error_rsp(unsigned char *buf, int len,
		struct diag_md_session_t *info)
		{
		/* -1 to accommodate the first byte 0x13 */
		if (len > (DIAG_MAX_RSP_SIZE - 1)) {
		@@ -936,7 +980,7 @@ void diag_send_error_rsp(unsigned char *buf, int len)

		(uint8_t )driver->apps_rsp_buf = DIAG_CMD_ERROR;
		memcpy((driver->apps_rsp_buf + sizeof(uint8_t)), buf, len);
		diag_send_rsp(driver->apps_rsp_buf, len + 1);
		diag_send_rsp(driver->apps_rsp_buf, len + 1, info);
		}

		int diag_process_apps_pkt(unsigned char *buf, int len,
		@@ -956,7 +1000,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		/* Check if the command is a supported mask command */
		mask_ret = diag_process_apps_masks(buf, len, info);
		if (mask_ret > 0) {
		diag_send_rsp(driver->apps_rsp_buf, mask_ret);
		diag_send_rsp(driver->apps_rsp_buf, mask_ret, info);
		return 0;
		}

		@@ -978,7 +1022,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		driver->apps_rsp_buf,
		DIAG_MAX_RSP_SIZE);
		if (write_len > 0)
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
		return 0;
		}

		@@ -988,13 +1032,15 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		reg_item = container_of(temp_entry, struct diag_cmd_reg_t,
		entry);
		if (info) {
		if (MD_PERIPHERAL_MASK(reg_item->proc) &
		info->peripheral_mask)
		if ((MD_PERIPHERAL_MASK(reg_item->proc) &
		info->peripheral_mask) \|\|
		(MD_PERIPHERAL_PD_MASK(reg_item->proc) &
		info->peripheral_mask))
		write_len = diag_send_data(reg_item, buf, len);
		} else {
		if (MD_PERIPHERAL_MASK(reg_item->proc) &
		driver->logging_mask)
		diag_send_error_rsp(buf, len);
		diag_send_error_rsp(buf, len, info);
		else
		write_len = diag_send_data(reg_item, buf, len);
		}
		@@ -1010,13 +1056,13 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		for (i = 0; i < 4; i++)
		(driver->apps_rsp_buf+i) = (buf+i);
		(uint32_t )(driver->apps_rsp_buf+4) = DIAG_MAX_REQ_SIZE;
		diag_send_rsp(driver->apps_rsp_buf, 8);
		diag_send_rsp(driver->apps_rsp_buf, 8, info);
		return 0;
		} else if ((buf == 0x4b) && ((buf+1) == 0x12) &&
		((uint16_t )(buf+2) == DIAG_DIAG_STM)) {
		len = diag_process_stm_cmd(buf, driver->apps_rsp_buf);
		if (len > 0) {
		diag_send_rsp(driver->apps_rsp_buf, len);
		diag_send_rsp(driver->apps_rsp_buf, len, info);
		return 0;
		}
		return len;
		@@ -1029,7 +1075,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		driver->apps_rsp_buf,
		DIAG_MAX_RSP_SIZE);
		if (write_len > 0)
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
		return 0;
		}
		/* Check for time sync switch command */
		@@ -1040,7 +1086,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		driver->apps_rsp_buf,
		DIAG_MAX_RSP_SIZE);
		if (write_len > 0)
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
		return 0;
		}
		/* Check for diag id command */
		@@ -1051,14 +1097,14 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		driver->apps_rsp_buf,
		DIAG_MAX_RSP_SIZE);
		if (write_len > 0)
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
		return 0;
		}
		/* Check for download command */
		else if ((chk_apps_master()) && (*buf == 0x3A)) {
		/* send response back */
		driver->apps_rsp_buf[0] = *buf;
		diag_send_rsp(driver->apps_rsp_buf, 1);
		diag_send_rsp(driver->apps_rsp_buf, 1, info);
		msleep(5000);
		/* call download API */
		msm_set_restart_mode(RESTART_DLOAD);
		@@ -1078,7 +1124,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		for (i = 0; i < 13; i++)
		driver->apps_rsp_buf[i+3] = 0;

		diag_send_rsp(driver->apps_rsp_buf, 16);
		diag_send_rsp(driver->apps_rsp_buf, 16, info);
		return 0;
		}
		}
		@@ -1087,7 +1133,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		((buf+2) == 0x04) && ((buf+3) == 0x0)) {
		memcpy(driver->apps_rsp_buf, buf, 4);
		driver->apps_rsp_buf[4] = wrap_enabled;
		diag_send_rsp(driver->apps_rsp_buf, 5);
		diag_send_rsp(driver->apps_rsp_buf, 5, info);
		return 0;
		}
		/* Wrap the Delayed Rsp ID */
		@@ -1096,7 +1142,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		wrap_enabled = true;
		memcpy(driver->apps_rsp_buf, buf, 4);
		driver->apps_rsp_buf[4] = wrap_count;
		diag_send_rsp(driver->apps_rsp_buf, 6);
		diag_send_rsp(driver->apps_rsp_buf, 6, info);
		return 0;
		}
		/* Mobile ID Rsp */
		@@ -1107,7 +1153,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		driver->apps_rsp_buf,
		DIAG_MAX_RSP_SIZE);
		if (write_len > 0) {
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
		return 0;
		}
		}
		@@ -1127,7 +1173,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		for (i = 0; i < 55; i++)
		driver->apps_rsp_buf[i] = 0;

		diag_send_rsp(driver->apps_rsp_buf, 55);
		diag_send_rsp(driver->apps_rsp_buf, 55, info);
		return 0;
		}
		/* respond to 0x7c command */
		@@ -1140,14 +1186,14 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		chk_config_get_id();
		(unsigned char )(driver->apps_rsp_buf + 12) = '\0';
		(unsigned char )(driver->apps_rsp_buf + 13) = '\0';
		diag_send_rsp(driver->apps_rsp_buf, 14);
		diag_send_rsp(driver->apps_rsp_buf, 14, info);
		return 0;
		}
		}
		write_len = diag_cmd_chk_stats(buf, len, driver->apps_rsp_buf,
		DIAG_MAX_RSP_SIZE);
		if (write_len > 0) {
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
		return 0;
		}
		write_len = diag_cmd_disable_hdlc(buf, len, driver->apps_rsp_buf,
		@@ -1159,7 +1205,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
		* before disabling HDLC encoding on Apps processor.
		*/
		mutex_lock(&driver->hdlc_disable_mutex);
		diag_send_rsp(driver->apps_rsp_buf, write_len);
		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
		/*
		* Set the value of hdlc_disabled after sending the response to
		* the tools. This is required since the tools is expecting a
		@@ -1179,7 +1225,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,

		/* We have now come to the end of the function. */
		if (chk_apps_only())
		diag_send_error_rsp(buf, len);
		diag_send_error_rsp(buf, len, info);

		return 0;
		}
		@@ -1262,7 +1308,7 @@ void diag_process_hdlc_pkt(void *data, unsigned int len,
		* recovery algorithm. Send an error response if the
		* packet is not in expected format.
		*/
		diag_send_error_rsp(driver->hdlc_buf, driver->hdlc_buf_len);
		diag_send_error_rsp(driver->hdlc_buf, driver->hdlc_buf_len, info);
		driver->hdlc_buf_len = 0;
		end:
		mutex_unlock(&driver->diag_hdlc_mutex);
		@@ -1535,7 +1581,7 @@ void diag_process_non_hdlc_pkt(unsigned char *buf, int len,

		if (actual_pkt->start != CONTROL_CHAR) {
		diag_hdlc_start_recovery(buf, len, info);
		diag_send_error_rsp(buf, len);
		diag_send_error_rsp(buf, len, info);
		goto end;
		}
		mutex_lock(&driver->hdlc_recovery_mutex);
		@@ -1625,15 +1671,14 @@ static int diagfwd_mux_write_done(unsigned char *buf, int len, int buf_ctxt,
		case TYPE_CMD:
		if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) {
		diagfwd_write_done(peripheral, type, num);
		} else if (peripheral == APPS_DATA) {
		}
		if (peripheral == APPS_DATA \|\|
		ctxt == DIAG_MEMORY_DEVICE_MODE) {
		spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
		driver->rsp_buf_busy = 0;
		driver->encoded_rsp_len = 0;
		spin_unlock_irqrestore(&driver->rsp_buf_busy_lock,
		flags);
		} else {
		pr_err_ratelimited("diag: Invalid peripheral %d in %s, type: %d\n",
		peripheral, __func__, type);
		}
		break;
		default:

drivers/char/diag/diagfwd.h

+2 −1

Original line number	Diff line number	Diff line
		@@ -47,7 +47,8 @@ void diag_update_userspace_clients(unsigned int type);
		void diag_update_sleeping_process(int process_id, int data_type);
		int diag_process_apps_pkt(unsigned char *buf, int len,
		struct diag_md_session_t *info);
		void diag_send_error_rsp(unsigned char *buf, int len);
		void diag_send_error_rsp(unsigned char *buf, int len,
		struct diag_md_session_t *info);
		void diag_update_pkt_buffer(unsigned char *buf, uint32_t len, int type);
		int diag_process_stm_cmd(unsigned char buf, unsigned char dest_buf);
		void diag_md_hdlc_reset_timer_func(unsigned long pid);

drivers/char/diag/diagfwd_peripheral.c

+11 −1

Original line number	Diff line number	Diff line
		@@ -222,12 +222,22 @@ int diag_md_get_peripheral(int ctxt)
		struct diagfwd_info *fwd_info = NULL;

		peripheral = GET_BUF_PERIPHERAL(ctxt);
		if (peripheral < 0 \|\| peripheral > NUM_PERIPHERALS)

		/* Check for peripheral value within bounds
		* of peripherals and UPD combined.
		*/
		if (peripheral < 0 \|\| peripheral > NUM_MD_SESSIONS)
		return -EINVAL;

		if (peripheral == APPS_DATA)
		return peripheral;

		/* With peripheral value bound checked
		* return user pd value.
		*/
		if (peripheral > NUM_PERIPHERALS)
		return peripheral;

		type = GET_BUF_TYPE(ctxt);
		if (type < 0 \|\| type >= NUM_TYPES)
		return -EINVAL;