dahdi-linux-extra/drivers/dahdi/arith.h

/*
 * Handy add/subtract functions to operate on chunks of shorts.
 * Feel free to add customizations for additional architectures
 *
 */

/*
 * See http://www.asterisk.org for more information about
 * the Asterisk project. Please do not directly contact
 * any of the maintainers of this project for assistance;
 * the project provides a web site, mailing lists and IRC
 * channels for your use.
 *
 * This program is free software, distributed under the terms of
 * the GNU General Public License Version 2 as published by the
 * Free Software Foundation. See the LICENSE file included with
 * this program for more details.
 */

#ifndef _DAHDI_ARITH_H
#define _DAHDI_ARITH_H

#ifdef CONFIG_DAHDI_MMX
#ifdef DAHDI_CHUNKSIZE
static inline void __ACSS(volatile short *dst, const short *src)
{
	__asm__ __volatile__ (
	        "movq 0(%0), %%mm0;\n"
	        "movq 0(%1), %%mm1;\n"
                "movq 8(%0), %%mm2;\n"
	        "movq 8(%1), %%mm3;\n"
	        "paddsw %%mm1, %%mm0;\n"
	        "paddsw %%mm3, %%mm2;\n"
                "movq %%mm0, 0(%0);\n"
                "movq %%mm2, 8(%0);\n"
	    : "=r" (dst)
	    : "r" (src), "0" (dst)
	    : "memory"
#ifdef CLOBBERMMX
	    , "%mm0", "%mm1", "%mm2", "%mm3"
#endif
      );

}
static inline void __SCSS(volatile short *dst, const short *src)
{
	__asm__ __volatile__ (
	        "movq 0(%0), %%mm0;\n"
	        "movq 0(%1), %%mm1;\n"
                "movq 8(%0), %%mm2;\n"
	        "movq 8(%1), %%mm3;\n"
	        "psubsw %%mm1, %%mm0;\n"
	        "psubsw %%mm3, %%mm2;\n"
                "movq %%mm0, 0(%0);\n"
                "movq %%mm2, 8(%0);\n"
	    : "=r" (dst)
	    : "r" (src), "0" (dst)
	    : "memory"
#ifdef CLOBBERMMX
	    , "%mm0", "%mm1", "%mm2", "%mm3"
#endif
      );

}

#if (DAHDI_CHUNKSIZE == 8)
#define ACSS(a,b) __ACSS(a,b)
#define SCSS(a,b) __SCSS(a,b)
#elif (DAHDI_CHUNKSIZE > 8)
static inline void ACSS(volatile short *dst, const short *src)
{
	int x;
	for (x=0;x<DAHDI_CHUNKSIZE;x+=8)
		__ACSS(dst + x, src + x);
}
static inline void SCSS(volatile short *dst, const short *src)
{
	int x;
	for (x=0;x<DAHDI_CHUNKSIZE;x+=8)
		__SCSS(dst + x, src + x);
}
#else
#error No MMX for DAHDI_CHUNKSIZE < 8
#endif
#endif
static inline int CONVOLVE(const int *coeffs, const short *hist, int len)
{
	int sum;
	/* Divide length by 16 */
	len >>= 4;
	
	/* Clear our accumulator, mm4 */
	
	/* 
	
	   For every set of eight...
	
	   Load 16 coefficients into four registers...
	   Shift each word right 16 to make them shorts...
	   Pack the resulting shorts into two registers...
	   With the coefficients now in mm0 and mm2, load the 
	        history into mm1 and mm3...
	   Multiply/add mm1 into mm0, and mm3 into mm2...
	   Add mm2 into mm0 (without saturation, alas).  Now we have two half-results.
	   Accumulate in mm4 (again, without saturation, alas)
	*/
	__asm__ (
		"pxor %%mm4, %%mm4;\n"
		"mov %1, %%edi;\n"
		"mov %2, %%esi;\n"
		"mov %3, %%ecx;\n"
		"1:"
			"movq  0(%%edi), %%mm0;\n"
			"movq  8(%%edi), %%mm1;\n"
			"movq 16(%%edi), %%mm2;\n"
			"movq 24(%%edi), %%mm3;\n"
			/* can't use 4/5 since 4 is the accumulator for us */
			"movq 32(%%edi), %%mm6;\n"
			"movq 40(%%edi), %%mm7;\n"
			"psrad $16, %%mm0;\n"
			"psrad $16, %%mm1;\n"
			"psrad $16, %%mm2;\n"
			"psrad $16, %%mm3;\n"
			"psrad $16, %%mm6;\n"
			"psrad $16, %%mm7;\n"
			"packssdw %%mm1, %%mm0;\n"
			"packssdw %%mm3, %%mm2;\n"
			"packssdw %%mm7, %%mm6;\n"
			"movq 0(%%esi), %%mm1;\n"
			"movq 8(%%esi), %%mm3;\n"
			"movq 16(%%esi), %%mm7;\n"
			"pmaddwd %%mm1, %%mm0;\n"
			"pmaddwd %%mm3, %%mm2;\n"
			"pmaddwd %%mm7, %%mm6;\n"
			"paddd %%mm6, %%mm4;\n"
			"paddd %%mm2, %%mm4;\n"
			"paddd %%mm0, %%mm4;\n"
			/* Come back and do for the last few bytes */
			"movq 48(%%edi), %%mm6;\n"
			"movq 56(%%edi), %%mm7;\n"
			"psrad $16, %%mm6;\n"
			"psrad $16, %%mm7;\n"
			"packssdw %%mm7, %%mm6;\n"
			"movq 24(%%esi), %%mm7;\n"
			"pmaddwd %%mm7, %%mm6;\n"
			"paddd %%mm6, %%mm4;\n"
			"add $64, %%edi;\n"
			"add $32, %%esi;\n"
			"dec %%ecx;\n"
		"jnz 1b;\n"
		"movq %%mm4, %%mm0;\n"
		"psrlq $32, %%mm0;\n"
		"paddd %%mm0, %%mm4;\n"
		"movd %%mm4, %0;\n"
		: "=r" (sum)
		: "r" (coeffs), "r" (hist), "r" (len)
		: "%ecx", "%edi", "%esi"
	);
		
	return sum;
}

static inline void UPDATE(volatile int *taps, const short *history, const int nsuppr, const int ntaps)
{
	int i;
	int correction;
	for (i=0;i<ntaps;i++) {
		correction = history[i] * nsuppr;
		taps[i] += correction;
	}
}

static inline void UPDATE2(volatile int *taps, volatile short *taps_short, const short *history, const int nsuppr, const int ntaps)
{
	int i;
	int correction;
#if 0
	ntaps >>= 4;
	/* First, load up taps, */
	__asm__ (
		"pxor %%mm4, %%mm4;\n"
		"mov %0, %%edi;\n"
		"mov %1, %%esi;\n"
		"mov %3, %%ecx;\n"
		"1:"
		"jnz 1b;\n"
		"movq %%mm4, %%mm0;\n"
		"psrlq $32, %%mm0;\n"
		"paddd %%mm0, %%mm4;\n"
		"movd %%mm4, %0;\n"
		: "=r" (taps), "=r" (taps_short)
		: "r" (history), "r" (nsuppr), "r" (ntaps), "0" (taps)
		: "%ecx", "%edi", "%esi"
	);
#endif
#if 1
	for (i=0;i<ntaps;i++) {
		correction = history[i] * nsuppr;
		taps[i] += correction;
		taps_short[i] = taps[i] >> 16;
	}
#endif	
}

static inline int CONVOLVE2(const short *coeffs, const short *hist, int len)
{
	int sum;
	/* Divide length by 16 */
	len >>= 4;
	
	/* Clear our accumulator, mm4 */
	
	/* 
	
	   For every set of eight...
	   Load in eight coefficients and eight historic samples, multliply add and
	   accumulate the result
	*/
	__asm__ (
		"pxor %%mm4, %%mm4;\n"
		"mov %1, %%edi;\n"
		"mov %2, %%esi;\n"
		"mov %3, %%ecx;\n"
		"1:"
			"movq  0(%%edi), %%mm0;\n"
			"movq  8(%%edi), %%mm2;\n"
			"movq 0(%%esi), %%mm1;\n"
			"movq 8(%%esi), %%mm3;\n"
			"pmaddwd %%mm1, %%mm0;\n"
			"pmaddwd %%mm3, %%mm2;\n"
			"paddd %%mm2, %%mm4;\n"
			"paddd %%mm0, %%mm4;\n"
			"movq  16(%%edi), %%mm0;\n"
			"movq  24(%%edi), %%mm2;\n"
			"movq 16(%%esi), %%mm1;\n"
			"movq 24(%%esi), %%mm3;\n"
			"pmaddwd %%mm1, %%mm0;\n"
			"pmaddwd %%mm3, %%mm2;\n"
			"paddd %%mm2, %%mm4;\n"
			"paddd %%mm0, %%mm4;\n"
			"add $32, %%edi;\n"
			"add $32, %%esi;\n"
			"dec %%ecx;\n"
		"jnz 1b;\n"
		"movq %%mm4, %%mm0;\n"
		"psrlq $32, %%mm0;\n"
		"paddd %%mm0, %%mm4;\n"
		"movd %%mm4, %0;\n"
		: "=r" (sum)
		: "r" (coeffs), "r" (hist), "r" (len)
		: "%ecx", "%edi", "%esi"
	);
		
	return sum;
}
static inline short MAX16(const short *y, int len, int *pos)
{
	int k;
	short max = 0;
	int bestpos = 0;
	for (k=0;k<len;k++) {
		if (max < y[k]) {
			bestpos = k;
			max = y[k];
		}
	}
	*pos = (len - 1 - bestpos);
	return max;
}



#else

#ifdef DAHDI_CHUNKSIZE
static inline void ACSS(short *dst, short *src)
{
	int x;

	/* Add src to dst with saturation, storing in dst */

#ifdef BFIN
	for (x = 0; x < DAHDI_CHUNKSIZE; x++)
		dst[x] = __builtin_bfin_add_fr1x16(dst[x], src[x]);
#else
	int sum;

	for (x = 0; x < DAHDI_CHUNKSIZE; x++) {
		sum = dst[x] + src[x];
		if (sum > 32767)
			sum = 32767;
		else if (sum < -32768)
			sum = -32768;
		dst[x] = sum;
	}
#endif
}

static inline void SCSS(short *dst, short *src)
{
	int x;

	/* Subtract src from dst with saturation, storing in dst */
#ifdef BFIN
	for (x = 0; x < DAHDI_CHUNKSIZE; x++)
		dst[x] = __builtin_bfin_sub_fr1x16(dst[x], src[x]);
#else
	int sum;

	for (x = 0; x < DAHDI_CHUNKSIZE; x++) {
		sum = dst[x] - src[x];
		if (sum > 32767)
			sum = 32767;
		else if (sum < -32768)
			sum = -32768;
		dst[x] = sum;
	}
#endif
}

#endif	/* DAHDI_CHUNKSIZE */

static inline int CONVOLVE(const int *coeffs, const short *hist, int len)
{
	int x;
	int sum = 0;
	for (x=0;x<len;x++)
		sum += (coeffs[x] >> 16) * hist[x];
	return sum;
}

static inline int CONVOLVE2(const short *coeffs, const short *hist, int len)
{
	int x;
	int sum = 0;
	for (x=0;x<len;x++)
		sum += coeffs[x] * hist[x];
	return sum;
}

static inline void UPDATE(int *taps, const short *history, const int nsuppr, const int ntaps)
{
	int i;
	int correction;
	for (i=0;i<ntaps;i++) {
		correction = history[i] * nsuppr;
		taps[i] += correction;
	}
}

static inline void UPDATE2(int *taps, short *taps_short, const short *history, const int nsuppr, const int ntaps)
{
	int i;
	int correction;
	for (i=0;i<ntaps;i++) {
		correction = history[i] * nsuppr;
		taps[i] += correction;
		taps_short[i] = taps[i] >> 16;
	}
}

static inline short MAX16(const short *y, int len, int *pos)
{
	int k;
	short max = 0;
	int bestpos = 0;
	for (k=0;k<len;k++) {
		if (max < y[k]) {
			bestpos = k;
			max = y[k];
		}
	}
	*pos = (len - 1 - bestpos);
	return max;
}

#endif	/* MMX */
#endif	/* _DAHDI_ARITH_H */