Merge "diag: Fix non-hdlc encoding of packets sent to remote proc" (9c5c2d20) · Commits · e / devices / android_kernel_fairphone_FP4

drivers/char/diag/diagchar.h

+3 −0

Original line number	Diff line number	Diff line
		@@ -59,6 +59,8 @@
		#define DIAG_CTRL_MSG_LOG_MS_MASK 37
		#define DIAG_CTRL_MSG_EVENT_MS_MASK 38

		#define NON_HDLC_VERSION 1
		#define NON_HDLC_HEADER_SIZE 4
		#define CONTROL_CHAR 0x7E

		#define DIAG_ID_ROOT_STRING "root"
		@@ -834,6 +836,7 @@ struct diagchar_dev {
		unsigned char *buf_feature_mask_update;
		uint8_t hdlc_disabled;
		uint8_t p_hdlc_disabled[NUM_MD_SESSIONS];
		uint8_t proc_hdlc_disabled[NUM_DIAG_MD_DEV];
		struct mutex hdlc_disable_mutex;
		struct mutex hdlc_recovery_mutex;
		struct timer_list hdlc_reset_timer;

drivers/char/diag/diagchar_core.c

+76 −57

Original line number	Diff line number	Diff line
		@@ -1030,11 +1030,13 @@ static int diag_send_raw_data_remote(int proc, void *buf, int len,
		int max_len = 0;
		uint8_t retry_count = 0;
		uint8_t max_retries = 50;
		uint16_t payload = 0;
		uint16_t payload_len = 0;
		struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
		struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
		int bridge_index = proc - 1;
		uint8_t hdlc_disabled = 0;
		unsigned char non_hdlc_header[NON_HDLC_HEADER_SIZE] = {
		CONTROL_CHAR, NON_HDLC_VERSION, 0, 0 };
		unsigned char end_byte[1] = { CONTROL_CHAR };

		if (!buf)
		return -EINVAL;
		@@ -1059,39 +1061,6 @@ static int diag_send_raw_data_remote(int proc, void *buf, int len,

		if (driver->hdlc_encode_buf_len != 0)
		return -EAGAIN;
		mutex_lock(&driver->hdlc_disable_mutex);
		hdlc_disabled = driver->p_hdlc_disabled[APPS_DATA];
		mutex_unlock(&driver->hdlc_disable_mutex);
		if (hdlc_disabled) {
		if (len < 4) {
		pr_err("diag: In %s, invalid len: %d of non_hdlc pkt\n",
		__func__, len);
		return -EBADMSG;
		}
		payload = (uint16_t )(buf + 2);
		if (payload > DIAG_MAX_HDLC_BUF_SIZE) {
		pr_err("diag: Dropping packet, payload size is %d\n",
		payload);
		return -EBADMSG;
		}
		driver->hdlc_encode_buf_len = payload;
		/*
		* Adding 5 bytes for start (1 byte), version (1 byte),
		* payload (2 bytes) and end (1 byte)
		*/
		if (len == (payload + 5)) {
		/*
		* Adding 4 bytes for start (1 byte), version (1 byte)
		* and payload (2 bytes)
		*/
		memcpy(driver->hdlc_encode_buf, buf + 4, payload);
		goto send_data;
		} else {
		pr_err("diag: In %s, invalid len: %d of non_hdlc pkt\n",
		__func__, len);
		return -EBADMSG;
		}
		}

		if (hdlc_flag) {
		if (len > DIAG_MAX_HDLC_BUF_SIZE) {
		@@ -1104,8 +1073,26 @@ static int diag_send_raw_data_remote(int proc, void *buf, int len,
		goto send_data;
		}

		if (driver->proc_hdlc_disabled[proc]) {
		/*
		* The worst case length will be twice as the incoming packet length.
		* Adding 4 bytes for start (1 byte), version (1 byte)
		* and payload (2 bytes)
		*/
		payload_len = len;
		memcpy(non_hdlc_header+2, &payload_len, 2);
		memcpy(driver->hdlc_encode_buf,
		non_hdlc_header, NON_HDLC_HEADER_SIZE);
		memcpy(driver->hdlc_encode_buf +
		NON_HDLC_HEADER_SIZE, buf, len);
		memcpy((driver->hdlc_encode_buf +
		NON_HDLC_HEADER_SIZE + len), end_byte, 1);
		driver->hdlc_encode_buf_len =
		len + NON_HDLC_HEADER_SIZE + 1;
		goto send_data;
		} else {
		/*
		* The worst case length will be twice as the incoming packet
		* length.
		* Add 3 bytes for CRC bytes (2 bytes) and delimiter (1 byte)
		*/
		max_len = (2 * len) + 3;
		@@ -1126,6 +1113,7 @@ static int diag_send_raw_data_remote(int proc, void *buf, int len,
		diag_hdlc_encode(&send, &enc);
		driver->hdlc_encode_buf_len = (int)(enc.dest -
		(void *)driver->hdlc_encode_buf);
		}

		send_data:
		err = diagfwd_bridge_write(bridge_index, driver->hdlc_encode_buf,
		@@ -2269,10 +2257,13 @@ static int diag_ioctl_hdlc_toggle(unsigned long ioarg)
		mutex_lock(&driver->hdlc_disable_mutex);
		mutex_lock(&driver->md_session_lock);
		session_info = diag_md_session_get_pid(current->tgid);
		if (session_info)
		if (session_info) {
		session_info->hdlc_disabled = hdlc_support;
		else
		driver->proc_hdlc_disabled[DIAG_LOCAL_PROC] =
		hdlc_support;
		} else {
		driver->hdlc_disabled = hdlc_support;
		}

		peripheral =
		diag_md_session_match_pid_peripheral(DIAG_LOCAL_PROC,
		@@ -3026,6 +3017,8 @@ long diagchar_compat_ioctl(struct file *filp,
		unsigned int iocmd, unsigned long ioarg)
		{
		int result = -EINVAL;
		uint8_t hdlc_support, i;
		struct diag_md_session_t *session_info = NULL;

		if (iocmd == DIAG_IOCTL_COMMAND_REG) {
		result = diag_ioctl_cmd_reg_compat(ioarg);
		@@ -3049,6 +3042,18 @@ long diagchar_compat_ioctl(struct file *filp,
		} else if (iocmd >= DIAG_IOCTL_QUERY_PD_FEATUREMASK &&
		iocmd <= DIAG_IOCTL_PASSTHRU_CONTROL) {
		result = diagchar_ioctl_hw_accel(filp, iocmd, ioarg);
		} else if (iocmd == DIAG_IOCTL_MDM_HDLC_TOGGLE) {
		if (copy_from_user(&hdlc_support, (void __user *)ioarg,
		sizeof(uint8_t)))
		return -EFAULT;
		mutex_lock(&driver->md_session_lock);
		session_info = diag_md_session_get_pid(current->tgid);
		if (session_info) {
		for (i = DIAG_LOCAL_PROC + 1; i < NUM_DIAG_MD_DEV; i++)
		driver->proc_hdlc_disabled[i] = hdlc_support;
		}
		mutex_unlock(&driver->md_session_lock);
		result = 0;
		} else {
		result = -EINVAL;
		}
		@@ -3060,6 +3065,8 @@ long diagchar_ioctl(struct file *filp,
		unsigned int iocmd, unsigned long ioarg)
		{
		int result = -EINVAL;
		uint8_t hdlc_support, i;
		struct diag_md_session_t *session_info = NULL;

		if (iocmd == DIAG_IOCTL_COMMAND_REG) {
		result = diag_ioctl_cmd_reg(ioarg);
		@@ -3083,6 +3090,18 @@ long diagchar_ioctl(struct file *filp,
		} else if (iocmd >= DIAG_IOCTL_QUERY_PD_FEATUREMASK &&
		iocmd <= DIAG_IOCTL_PASSTHRU_CONTROL) {
		result = diagchar_ioctl_hw_accel(filp, iocmd, ioarg);
		} else if (iocmd == DIAG_IOCTL_MDM_HDLC_TOGGLE) {
		if (copy_from_user(&hdlc_support, (void __user *)ioarg,
		sizeof(uint8_t)))
		return -EFAULT;
		mutex_lock(&driver->md_session_lock);
		session_info = diag_md_session_get_pid(current->tgid);
		if (session_info) {
		for (i = DIAG_LOCAL_PROC + 1; i < NUM_DIAG_MD_DEV; i++)
		driver->proc_hdlc_disabled[i] = hdlc_support;
		}
		mutex_unlock(&driver->md_session_lock);
		result = 0;
		} else {
		result = -EINVAL;
		}

include/linux/diagchar.h

+1 −0

Original line number	Diff line number	Diff line
		@@ -62,6 +62,7 @@
		#define DIAG_IOCTL_QUERY_MD_PID 41
		#define DIAG_IOCTL_QUERY_PD_FEATUREMASK 42
		#define DIAG_IOCTL_PASSTHRU_CONTROL 43
		#define DIAG_IOCTL_MDM_HDLC_TOGGLE 44

		/* PC Tools IDs */
		#define APQ8060_TOOLS_ID 4062