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

Commit 14a18d8e authored by Eric Holmberg's avatar Eric Holmberg Committed by Gerrit - the friendly Code Review server
Browse files

soc: qcom: smd: Add memcpy functions to handle 8-byte alignment 



The standard memcpy implementation is not intended to be used for device
memory and hence does not handle the more restrictive device-memory
alignment constraints.  This may lead to a data abort when transferring
unaligned data to or from device memory when the transfer size is
greater than 1 byte.

Implement memcpy that will do byte-aligned copies until the address is
8-byte aligned, do multi-byte aligned copies, and then finish with
byte-aligned copies again as necessary.

Change-Id: I2da9050ca81a8dfaf24ce1b63b8339058e74ba0b
Signed-off-by: default avatarEric Holmberg <eholmber@codeaurora.org>
parent e432f9a6

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;