Merge "soc: qcom: smd: Add memcpy functions to handle 8-byte alignment" (73bbe129) · Commits · e / devices / android_kernel_sony_msm8994

drivers/soc/qcom/smd.c

+95 −3

Original line number	Diff line number	Diff line
		@@ -128,6 +128,11 @@ static struct interrupt_config private_intr_config[NUM_SMD_SUBSYSTEMS] = {
		},
		};

		union fifo_mem {
		uint64_t u64;
		uint8_t u8;
		};

		struct interrupt_stat interrupt_stats[NUM_SMD_SUBSYSTEMS];

		#define SMSM_STATE_ADDR(entry) (smsm_info.state + entry)
		@@ -220,6 +225,93 @@ static inline void smd_write_intr(unsigned int val,
		__raw_writel(val, addr);
		}

		/**
		* smd_memcpy_to_fifo() - copy to SMD channel FIFO
		* @dest: Destination address
		* @src: Source address
		* @num_bytes: Number of bytes to copy
		*
		* @return: Address of destination
		*
		* This function copies num_bytes from src to dest. This is used as the memcpy
		* function to copy data to SMD FIFO in case the SMD FIFO is naturally aligned.
		*/
		static void smd_memcpy_to_fifo(void dest, const void *src, size_t num_bytes)
		{
		union fifo_mem temp_dst = (union fifo_mem )dest;
		union fifo_mem temp_src = (union fifo_mem )src;
		uintptr_t mask = sizeof(union fifo_mem) - 1;

		/* Do byte copies until we hit 8-byte (double word) alignment */
		while ((uintptr_t)temp_dst & mask && num_bytes) {
		__raw_writeb_no_log(temp_src->u8, temp_dst);
		temp_src = (union fifo_mem *)((uintptr_t)temp_src + 1);
		temp_dst = (union fifo_mem *)((uintptr_t)temp_dst + 1);
		num_bytes--;
		}

		/* Do double word copies */
		while (num_bytes >= sizeof(union fifo_mem)) {
		__raw_writeq_no_log(temp_src->u64, temp_dst);
		temp_dst++;
		temp_src++;
		num_bytes -= sizeof(union fifo_mem);
		}

		/* Copy remaining bytes */
		while (num_bytes--) {
		__raw_writeb_no_log(temp_src->u8, temp_dst);
		temp_src = (union fifo_mem *)((uintptr_t)temp_src + 1);
		temp_dst = (union fifo_mem *)((uintptr_t)temp_dst + 1);
		}

		return dest;
		}

		/**
		* smd_memcpy_from_fifo() - copy from SMD channel FIFO
		* @dest: Destination address
		* @src: Source address
		* @num_bytes: Number of bytes to copy
		*
		* @return: Address of destination
		*
		* This function copies num_bytes from src to dest. This is used as the memcpy
		* function to copy data from SMD FIFO in case the SMD FIFO is naturally
		* aligned.
		*/
		static void smd_memcpy_from_fifo(void dest, const void *src, size_t num_bytes)
		{
		union fifo_mem temp_dst = (union fifo_mem )dest;
		union fifo_mem temp_src = (union fifo_mem )src;
		uintptr_t mask = sizeof(union fifo_mem) - 1;

		/* Do byte copies until we hit 8-byte (double word) alignment */
		while ((uintptr_t)temp_src & mask && num_bytes) {
		temp_dst->u8 = __raw_readb_no_log(temp_src);
		temp_src = (union fifo_mem *)((uintptr_t)temp_src + 1);
		temp_dst = (union fifo_mem *)((uintptr_t)temp_dst + 1);
		num_bytes--;
		}

		/* Do double word copies */
		while (num_bytes >= sizeof(union fifo_mem)) {
		temp_dst->u64 = __raw_readq_no_log(temp_src);
		temp_dst++;
		temp_src++;
		num_bytes -= sizeof(union fifo_mem);
		}

		/* Copy remaining bytes */
		while (num_bytes--) {
		temp_dst->u8 = __raw_readb_no_log(temp_src);
		temp_src = (union fifo_mem *)((uintptr_t)temp_src + 1);
		temp_dst = (union fifo_mem *)((uintptr_t)temp_dst + 1);
		}

		return dest;
		}

		/**
		* smd_memcpy32_to_fifo() - Copy to SMD channel FIFO
		*
		@@ -1731,11 +1823,11 @@ static int smd_alloc_channel(struct smd_alloc_elm *alloc_elm, int table_id,
		ch->read_from_fifo = smd_memcpy32_from_fifo;
		ch->write_to_fifo = smd_memcpy32_to_fifo;
		} else {
		ch->read_from_fifo = memcpy;
		ch->write_to_fifo = memcpy;
		ch->read_from_fifo = smd_memcpy_from_fifo;
		ch->write_to_fifo = smd_memcpy_to_fifo;
		}

		memcpy(ch->name, alloc_elm->name, SMD_MAX_CH_NAME_LEN);
		smd_memcpy_from_fifo(ch->name, alloc_elm->name, SMD_MAX_CH_NAME_LEN);
		ch->name[SMD_MAX_CH_NAME_LEN-1] = 0;

		ch->pdev.name = ch->name;