memcpy.c

linux-2.6.15.6

		src += 4 * sizeof(unsigned int);
		dst += 4 * sizeof(unsigned int);
		len -= 4;
	}
	while (len != 0);

do0:
	/* ((unsigned int *) dst)[0] = MERGE (a2, sh_1, a3, sh_2); */
	stw(d_space, MERGE (a2, sh_1, a3, sh_2), 0, dst, cda_stw_exc);

	preserve_branch(handle_load_error);
	preserve_branch(handle_store_error);

	return 0;

handle_load_error:
	__asm__ __volatile__ ("cda_ldw_exc:\n");
	d = &__get_cpu_var(exception_data);
	DPRINTF("cda_ldw_exc: o_len=%lu fault_addr=%lu o_src=%lu ret=%lu\n",
		o_len, d->fault_addr, o_src, o_len - d->fault_addr + o_src);
	return o_len * 4 - d->fault_addr + o_src;

handle_store_error:
	__asm__ __volatile__ ("cda_stw_exc:\n");
	d = &__get_cpu_var(exception_data);
	DPRINTF("cda_stw_exc: o_len=%lu fault_addr=%lu o_dst=%lu ret=%lu\n",
		o_len, d->fault_addr, o_dst, o_len - d->fault_addr + o_dst);
	return o_len * 4 - d->fault_addr + o_dst;
}


/* Returns 0 for success, otherwise, returns number of bytes not transferred. */
unsigned long pa_memcpy(void *dstp, const void *srcp, unsigned long len)
{
	register unsigned long src, dst, t1, t2, t3;
	register unsigned char *pcs, *pcd;
	register unsigned int *pws, *pwd;
	register double *pds, *pdd;
	unsigned long ret = 0;
	unsigned long o_dst, o_src, o_len;
	struct exception_data *d;

	src = (unsigned long)srcp;
	dst = (unsigned long)dstp;
	pcs = (unsigned char *)srcp;
	pcd = (unsigned char *)dstp;

	o_dst = dst; o_src = src; o_len = len;

	/* prefetch_src((const void *)srcp); */

	if (len < THRESHOLD)
		goto byte_copy;

	/* Check alignment */
	t1 = (src ^ dst);
	if (unlikely(t1 & (sizeof(double)-1)))
		goto unaligned_copy;

	/* src and dst have same alignment. */

	/* Copy bytes till we are double-aligned. */
	t2 = src & (sizeof(double) - 1);
	if (unlikely(t2 != 0)) {
		t2 = sizeof(double) - t2;
		while (t2 && len) {
			/* *pcd++ = *pcs++; */
			ldbma(s_space, pcs, t3, pmc_load_exc);
			len--;
			stbma(d_space, t3, pcd, pmc_store_exc);
			t2--;
		}
	}

	pds = (double *)pcs;
	pdd = (double *)pcd;

#if 0
	/* Copy 8 doubles at a time */
	while (len >= 8*sizeof(double)) {
		register double r1, r2, r3, r4, r5, r6, r7, r8;
		/* prefetch_src((char *)pds + L1_CACHE_BYTES); */
		flddma(s_space, pds, r1, pmc_load_exc);
		flddma(s_space, pds, r2, pmc_load_exc);
		flddma(s_space, pds, r3, pmc_load_exc);
		flddma(s_space, pds, r4, pmc_load_exc);
		fstdma(d_space, r1, pdd, pmc_store_exc);
		fstdma(d_space, r2, pdd, pmc_store_exc);
		fstdma(d_space, r3, pdd, pmc_store_exc);
		fstdma(d_space, r4, pdd, pmc_store_exc);

#if 0
		if (L1_CACHE_BYTES <= 32)
			prefetch_src((char *)pds + L1_CACHE_BYTES);
#endif
		flddma(s_space, pds, r5, pmc_load_exc);
		flddma(s_space, pds, r6, pmc_load_exc);
		flddma(s_space, pds, r7, pmc_load_exc);
		flddma(s_space, pds, r8, pmc_load_exc);
		fstdma(d_space, r5, pdd, pmc_store_exc);
		fstdma(d_space, r6, pdd, pmc_store_exc);
		fstdma(d_space, r7, pdd, pmc_store_exc);
		fstdma(d_space, r8, pdd, pmc_store_exc);
		len -= 8*sizeof(double);
	}
#endif

	pws = (unsigned int *)pds;
	pwd = (unsigned int *)pdd;

word_copy:
	while (len >= 8*sizeof(unsigned int)) {
		register unsigned int r1,r2,r3,r4,r5,r6,r7,r8;
		/* prefetch_src((char *)pws + L1_CACHE_BYTES); */
		ldwma(s_space, pws, r1, pmc_load_exc);
		ldwma(s_space, pws, r2, pmc_load_exc);
		ldwma(s_space, pws, r3, pmc_load_exc);
		ldwma(s_space, pws, r4, pmc_load_exc);
		stwma(d_space, r1, pwd, pmc_store_exc);
		stwma(d_space, r2, pwd, pmc_store_exc);
		stwma(d_space, r3, pwd, pmc_store_exc);
		stwma(d_space, r4, pwd, pmc_store_exc);

		ldwma(s_space, pws, r5, pmc_load_exc);
		ldwma(s_space, pws, r6, pmc_load_exc);
		ldwma(s_space, pws, r7, pmc_load_exc);
		ldwma(s_space, pws, r8, pmc_load_exc);
		stwma(d_space, r5, pwd, pmc_store_exc);
		stwma(d_space, r6, pwd, pmc_store_exc);
		stwma(d_space, r7, pwd, pmc_store_exc);
		stwma(d_space, r8, pwd, pmc_store_exc);
		len -= 8*sizeof(unsigned int);
	}

	while (len >= 4*sizeof(unsigned int)) {
		register unsigned int r1,r2,r3,r4;
		ldwma(s_space, pws, r1, pmc_load_exc);
		ldwma(s_space, pws, r2, pmc_load_exc);
		ldwma(s_space, pws, r3, pmc_load_exc);
		ldwma(s_space, pws, r4, pmc_load_exc);
		stwma(d_space, r1, pwd, pmc_store_exc);
		stwma(d_space, r2, pwd, pmc_store_exc);
		stwma(d_space, r3, pwd, pmc_store_exc);
		stwma(d_space, r4, pwd, pmc_store_exc);
		len -= 4*sizeof(unsigned int);
	}

	pcs = (unsigned char *)pws;
	pcd = (unsigned char *)pwd;

byte_copy:
	while (len) {
		/* *pcd++ = *pcs++; */
		ldbma(s_space, pcs, t3, pmc_load_exc);
		stbma(d_space, t3, pcd, pmc_store_exc);
		len--;
	}

	return 0;

unaligned_copy:
	/* possibly we are aligned on a word, but not on a double... */
	if (likely(t1 & (sizeof(unsigned int)-1)) == 0) {
		t2 = src & (sizeof(unsigned int) - 1);

		if (unlikely(t2 != 0)) {
			t2 = sizeof(unsigned int) - t2;
			while (t2) {
				/* *pcd++ = *pcs++; */
				ldbma(s_space, pcs, t3, pmc_load_exc);
				stbma(d_space, t3, pcd, pmc_store_exc);
				len--;
				t2--;
			}
		}

		pws = (unsigned int *)pcs;
		pwd = (unsigned int *)pcd;
		goto word_copy;
	}

	/* Align the destination.  */
	if (unlikely((dst & (sizeof(unsigned int) - 1)) != 0)) {
		t2 = sizeof(unsigned int) - (dst & (sizeof(unsigned int) - 1));
		while (t2) {
			/* *pcd++ = *pcs++; */
			ldbma(s_space, pcs, t3, pmc_load_exc);
			stbma(d_space, t3, pcd, pmc_store_exc);
			len--;
			t2--;
		}
		dst = (unsigned long)pcd;
		src = (unsigned long)pcs;
	}

	ret = copy_dstaligned(dst, src, len / sizeof(unsigned int), 
		o_dst, o_src, o_len);
	if (ret)
		return ret;

	pcs += (len & -sizeof(unsigned int));
	pcd += (len & -sizeof(unsigned int));
	len %= sizeof(unsigned int);

	preserve_branch(handle_load_error);
	preserve_branch(handle_store_error);

	goto byte_copy;

handle_load_error:
	__asm__ __volatile__ ("pmc_load_exc:\n");
	d = &__get_cpu_var(exception_data);
	DPRINTF("pmc_load_exc: o_len=%lu fault_addr=%lu o_src=%lu ret=%lu\n",
		o_len, d->fault_addr, o_src, o_len - d->fault_addr + o_src);
	return o_len - d->fault_addr + o_src;

handle_store_error:
	__asm__ __volatile__ ("pmc_store_exc:\n");
	d = &__get_cpu_var(exception_data);
	DPRINTF("pmc_store_exc: o_len=%lu fault_addr=%lu o_dst=%lu ret=%lu\n",
		o_len, d->fault_addr, o_dst, o_len - d->fault_addr + o_dst);
	return o_len - d->fault_addr + o_dst;
}

#ifdef __KERNEL__
unsigned long copy_to_user(void __user *dst, const void *src, unsigned long len)
{
	mtsp(get_kernel_space(), 1);
	mtsp(get_user_space(), 2);
	return pa_memcpy((void __force *)dst, src, len);
}

unsigned long copy_from_user(void *dst, const void __user *src, unsigned long len)
{
	mtsp(get_user_space(), 1);
	mtsp(get_kernel_space(), 2);
	return pa_memcpy(dst, (void __force *)src, len);
}

unsigned long copy_in_user(void __user *dst, const void __user *src, unsigned long len)
{
	mtsp(get_user_space(), 1);
	mtsp(get_user_space(), 2);
	return pa_memcpy((void __force *)dst, (void __force *)src, len);
}


void * memcpy(void * dst,const void *src, size_t count)
{
	mtsp(get_kernel_space(), 1);
	mtsp(get_kernel_space(), 2);
	pa_memcpy(dst, src, count);
	return dst;
}

EXPORT_SYMBOL(copy_to_user);
EXPORT_SYMBOL(copy_from_user);
EXPORT_SYMBOL(copy_in_user);
EXPORT_SYMBOL(memcpy);
#endif