text: /* Copyright 2016 The Chromium OS Authors. All rights reserved.
   * Use of this source code is governed by a BSD-style license that can be
   * found in the LICENSE file.
   */
  
  #include <math.h> /* for abs() */
  #include <stdio.h> /* for printf() */
  #include <string.h> /* for memset() */
  #include <stdint.h> /* for uint64 definition */
  #include <stdlib.h> /* for exit() definition */
  #include <time.h> /* for clock_gettime */
  
  #include "../drc_math.h"
  #include "../dsp_util.h"
  
  /* Constant for converting time to milliseconds. */
  #define BILLION 1000000000LL
  /* Number of iterations for performance testing. */
  #define ITERATIONS 400000
  
  #if defined(__aarch64__)
  int16_t float_to_short(float a)
  {
  	int32_t ret;
  	asm volatile("fcvtas %s[ret], %s[a]\n"
  		     "sqxtn %h[ret], %s[ret]\n"
  		     : [ret] "=w"(ret)
  		     : [a] "w"(a)
  		     :);
  	return (int16_t)(ret);
  }
  #else
  int16_t float_to_short(float a)
  {
  	a += (a >= 0) ? 0.5f : -0.5f;
  	return (int16_t)(max(-32768, min(32767, a)));
  }
  #endif
  
  void dsp_util_deinterleave_reference(int16_t *input, float *const *output,
  				     int channels, int frames)
  {
  	float *output_ptr[channels];
  	int i, j;
  
  	for (i = 0; i < channels; i++)
  		output_ptr[i] = output[i];
  
  	for (i = 0; i < frames; i++)
  		for (j = 0; j < channels; j++)
  			*(output_ptr[j]++) = *input++ / 32768.0f;
  }
  
  void dsp_util_interleave_reference(float *const *input, int16_t *output,
  				   int channels, int frames)
  {
  	float *input_ptr[channels];
  	int i, j;
  
  	for (i = 0; i < channels; i++)
  		input_ptr[i] = input[i];
  
  	for (i = 0; i < frames; i++)
  		for (j = 0; j < channels; j++) {
  			float f = *(input_ptr[j]++) * 32768.0f;
  			*output++ = float_to_short(f);
  		}
  }
  
  /* Use fixed size allocation to avoid performance fluctuation of allocation. */
  #define MAXSAMPLES 4096
  #define MINSAMPLES 256
  /* PAD buffer to check for overflows. */
  #define PAD 4096
  
  void TestRounding(float in, int16_t expected, int samples)
  {
  	int i;
  	int max_diff;
  	int d;
  
  	short *in_shorts = (short *)malloc(MAXSAMPLES * 2 * 2 + PAD);
  	float *out_floats_left_c = (float *)malloc(MAXSAMPLES * 4 + PAD);
  	float *out_floats_right_c = (float *)malloc(MAXSAMPLES * 4 + PAD);
  	float *out_floats_left_opt = (float *)malloc(MAXSAMPLES * 4 + PAD);
  	float *out_floats_right_opt = (float *)malloc(MAXSAMPLES * 4 + PAD);
  	short *out_shorts_c = (short *)malloc(MAXSAMPLES * 2 * 2 + PAD);
  	short *out_shorts_opt = (short *)malloc(MAXSAMPLES * 2 * 2 + PAD);
  
  	memset(in_shorts, 0xfb, MAXSAMPLES * 2 * 2 + PAD);
  	memset(out_floats_left_c, 0xfb, MAXSAMPLES * 4 + PAD);
  	memset(out_floats_right_c, 0xfb, MAXSAMPLES * 4 + PAD);
  	memset(out_floats_left_opt, 0xfb, MAXSAMPLES * 4 + PAD);
  	memset(out_floats_right_opt, 0xfb, MAXSAMPLES * 4 + PAD);
  	memset(out_shorts_c, 0xfb, MAXSAMPLES * 2 * 2 + PAD);
  	memset(out_shorts_opt, 0xfb, MAXSAMPLES * 2 * 2 + PAD);
  
  	float *out_floats_ptr_c[2];
  	float *out_floats_ptr_opt[2];
  
  	out_floats_ptr_c[0] = out_floats_left_c;
  	out_floats_ptr_c[1] = out_floats_right_c;
  	out_floats_ptr_opt[0] = out_floats_left_opt;
  	out_floats_ptr_opt[1] = out_floats_right_opt;
  
  	for (i = 0; i < MAXSAMPLES; ++i) {
  		out_floats_left_c[i] = in;
  		out_floats_right_c[i] = in;
  	}
  
  	/*  reference C interleave */
  	dsp_util_interleave_reference(out_floats_ptr_c, out_shorts_c, 2,
  				      samples);
  
  	/* measure optimized interleave */
  	for (i = 0; i < ITERATIONS; ++i) {
  		dsp_util_interleave(out_floats_ptr_c, (uint8_t *)out_shorts_opt,
  				    2, SND_PCM_FORMAT_S16_LE, samples);
  	}
  
  	max_diff = 0;
  	for (i = 0; i < (MAXSAMPLES * 2 + PAD / 2); ++i) {
  		d = abs(out_shorts_c[i] - out_shorts_opt[i]);
  		if (d > max_diff) {
  			max_diff = d;
  		}
  	}
  	printf("test interleave compare %6d, %10f %13f %6d %6d %6d %s\n",
  	       max_diff, in, in * 32768.0f, out_shorts_c[0], out_shorts_opt[0],
  	       expected,
  	       max_diff == 0 ? "PASS" :
  			       (out_shorts_opt[0] == expected ?
  					"EXPECTED DIFFERENCE" :
  					"UNEXPECTED DIFFERENCE"));
  
  	/* measure reference C deinterleave */
  	dsp_util_deinterleave_reference(in_shorts, out_floats_ptr_c, 2,
  					samples);
  
  	/* measure optimized deinterleave */
  	dsp_util_deinterleave((uint8_t *)in_shorts, out_floats_ptr_opt, 2,
  			      SND_PCM_FORMAT_S16_LE, samples);
  
  	d = memcmp(out_floats_ptr_c[0], out_floats_ptr_opt[0], samples * 4);
  	if (d)
  		printf("left compare %d, %f %f\n", d, out_floats_ptr_c[0][0],
  		       out_floats_ptr_opt[0][0]);
  	d = memcmp(out_floats_ptr_c[1], out_floats_ptr_opt[1], samples * 4);
  	if (d)
  		printf("right compare %d, %f %f\n", d, out_floats_ptr_c[1][0],
  		       out_floats_ptr_opt[1][0]);
  
  	free(in_shorts);
  	free(out_floats_left_c);
  	free(out_floats_right_c);
  	free(out_floats_left_opt);
  	free(out_floats_right_opt);
  	free(out_shorts_c);
  	free(out_shorts_opt);
  }
  
  int main(int argc, char **argv)
  {
  	float e = 0.000000001f;
  	int samples = 16;
  
  	dsp_enable_flush_denormal_to_zero();
  
  	// Print headings for TestRounding output.
  	printf("test interleave compare maxdif,     float,   float * 32k      "
  	       "C   SIMD expect pass\n");
  
  	// test clamping
  	TestRounding(1.0f, 32767, samples);
  	TestRounding(-1.0f, -32768, samples);
  	TestRounding(1.1f, 32767, samples);
  	TestRounding(-1.1f, -32768, samples);
  	TestRounding(2000000000.f / 32768.f, 32767, samples);
  	TestRounding(-2000000000.f / 32768.f, -32768, samples);
  
  	/* Infinity produces zero on arm64. */
  #if defined(__aarch64__)
  #define EXPECTED_INF_RESULT 0
  #define EXPECTED_NEGINF_RESULT 0
  #elif defined(__i386__) || defined(__x86_64__)
  #define EXPECTED_INF_RESULT -32768
  #define EXPECTED_NEGINF_RESULT 0
  #else
  #define EXPECTED_INF_RESULT 32767
  #define EXPECTED_NEGINF_RESULT -32768
  #endif
  
  	TestRounding(5000000000.f / 32768.f, EXPECTED_INF_RESULT, samples);
  	TestRounding(-5000000000.f / 32768.f, EXPECTED_NEGINF_RESULT, samples);
  
  	// test infinity
  	union ieee754_float inf;
  	inf.ieee.negative = 0;
  	inf.ieee.exponent = 0xfe;
  	inf.ieee.mantissa = 0x7fffff;
  	TestRounding(inf.f, EXPECTED_INF_RESULT, samples); // expect fail
  	inf.ieee.negative = 1;
  	inf.ieee.exponent = 0xfe;
  	inf.ieee.mantissa = 0x7fffff;
  	TestRounding(inf.f, EXPECTED_NEGINF_RESULT, samples); // expect fail
  
  	// test rounding
  	TestRounding(0.25f, 8192, samples);
  	TestRounding(-0.25f, -8192, samples);
  	TestRounding(0.50f, 16384, samples);
  	TestRounding(-0.50f, -16384, samples);
  	TestRounding(1.0f / 32768.0f, 1, samples);
  	TestRounding(-1.0f / 32768.0f, -1, samples);
  	TestRounding(1.0f / 32768.0f + e, 1, samples);
  	TestRounding(-1.0f / 32768.0f - e, -1, samples);
  	TestRounding(1.0f / 32768.0f - e, 1, samples);
  	TestRounding(-1.0f / 32768.0f + e, -1, samples);
  
  	/* Rounding on 'tie' is different for Intel. */
  #if defined(__i386__) || defined(__x86_64__)
  	TestRounding(0.5f / 32768.0f, 0, samples); /* Expect round to even */
  	TestRounding(-0.5f / 32768.0f, 0, samples);
  #else
  	TestRounding(0.5f / 32768.0f, 1, samples); /* Expect round away */
  	TestRounding(-0.5f / 32768.0f, -1, samples);
  #endif
  
  	TestRounding(0.5f / 32768.0f + e, 1, samples);
  	TestRounding(-0.5f / 32768.0f - e, 1, samples);
  	TestRounding(0.5f / 32768.0f - e, 0, samples);
  	TestRounding(-0.5f / 32768.0f + e, 0, samples);
  
  	TestRounding(1.5f / 32768.0f, 2, samples);
  	TestRounding(-1.5f / 32768.0f, -2, samples);
  	TestRounding(1.5f / 32768.0f + e, 2, samples);
  	TestRounding(-1.5f / 32768.0f - e, -2, samples);
  	TestRounding(1.5f / 32768.0f - e, 1, samples);
  	TestRounding(-1.5f / 32768.0f + e, -1, samples);
  
  	/* Test denormals */
  	union ieee754_float denorm;
  	denorm.ieee.negative = 0;
  	denorm.ieee.exponent = 0;
  	denorm.ieee.mantissa = 1;
  	TestRounding(denorm.f, 0, samples);
  	denorm.ieee.negative = 1;
  	denorm.ieee.exponent = 0;
  	denorm.ieee.mantissa = 1;
  	TestRounding(denorm.f, 0, samples);
  
  	/* Test NaNs. Caveat Results vary by implementation. */
  #if defined(__i386__) || defined(__x86_64__)
  #define EXPECTED_NAN_RESULT -32768
  #else
  #define EXPECTED_NAN_RESULT 0
  #endif
  	union ieee754_float nan; /* Quiet NaN */
  	nan.ieee.negative = 0;
  	nan.ieee.exponent = 0xff;
  	nan.ieee.mantissa = 0x400001;
  	TestRounding(nan.f, EXPECTED_NAN_RESULT, samples);
  	nan.ieee.negative = 0;
  	nan.ieee.exponent = 0xff;
  	nan.ieee.mantissa = 0x000001; /* Signalling NaN */
  	TestRounding(nan.f, EXPECTED_NAN_RESULT, samples);
  
  	/* Test Performance */
  	uint64_t diff;
  	struct timespec start, end;
  	int i;
  	int d;
  
  	short *in_shorts = (short *)malloc(MAXSAMPLES * 2 * 2 + PAD);
  	float *out_floats_left_c = (float *)malloc(MAXSAMPLES * 4 + PAD);
  	float *out_floats_right_c = (float *)malloc(MAXSAMPLES * 4 + PAD);
  	float *out_floats_left_opt = (float *)malloc(MAXSAMPLES * 4 + PAD);
  	float *out_floats_right_opt = (float *)malloc(MAXSAMPLES * 4 + PAD);
  	short *out_shorts_c = (short *)malloc(MAXSAMPLES * 2 * 2 + PAD);
  	short *out_shorts_opt = (short *)malloc(MAXSAMPLES * 2 * 2 + PAD);
  
  	memset(in_shorts, 0x11, MAXSAMPLES * 2 * 2 + PAD);
  	memset(out_floats_left_c, 0x22, MAXSAMPLES * 4 + PAD);
  	memset(out_floats_right_c, 0x33, MAXSAMPLES * 4 + PAD);
  	memset(out_floats_left_opt, 0x44, MAXSAMPLES * 4 + PAD);
  	memset(out_floats_right_opt, 0x55, MAXSAMPLES * 4 + PAD);
  	memset(out_shorts_c, 0x66, MAXSAMPLES * 2 * 2 + PAD);
  	memset(out_shorts_opt, 0x66, MAXSAMPLES * 2 * 2 + PAD);
  
  	float *out_floats_ptr_c[2];
  	float *out_floats_ptr_opt[2];
  
  	out_floats_ptr_c[0] = out_floats_left_c;
  	out_floats_ptr_c[1] = out_floats_right_c;
  	out_floats_ptr_opt[0] = out_floats_left_opt;
  	out_floats_ptr_opt[1] = out_floats_right_opt;
  
  	/* Benchmark dsp_util_interleave */
  	for (samples = MAXSAMPLES; samples >= MINSAMPLES; samples /= 2) {
  		/* measure original C interleave */
  		clock_gettime(CLOCK_MONOTONIC, &start); /* mark start time */
  		for (i = 0; i < ITERATIONS; ++i) {
  			dsp_util_interleave_reference(out_floats_ptr_c,
  						      out_shorts_c, 2, samples);
  		}
  		clock_gettime(CLOCK_MONOTONIC, &end); /* mark the end time */
  		diff = (BILLION * (end.tv_sec - start.tv_sec) + end.tv_nsec -
  			start.tv_nsec) /
  		       1000000;
  		printf("interleave   ORIG size = %6d, elapsed time = %llu ms\n",
  		       samples, (long long unsigned int)diff);
  
  		/* measure optimized interleave */
  		clock_gettime(CLOCK_MONOTONIC, &start); /* mark start time */
  		for (i = 0; i < ITERATIONS; ++i) {
  			dsp_util_interleave(out_floats_ptr_c,
  					    (uint8_t *)out_shorts_opt, 2,
  					    SND_PCM_FORMAT_S16_LE, samples);
  		}
  		clock_gettime(CLOCK_MONOTONIC, &end); /* mark the end time */
  		diff = (BILLION * (end.tv_sec - start.tv_sec) + end.tv_nsec -
  			start.tv_nsec) /
  		       1000000;
  		printf("interleave   SIMD size = %6d, elapsed time = %llu ms\n",
  		       samples, (long long unsigned int)diff);
  
  		/* Test C and SIMD output match */
  		d = memcmp(out_shorts_c, out_shorts_opt,
  			   MAXSAMPLES * 2 * 2 + PAD);
  		if (d)
  			printf("interleave compare %d, %d %d, %d %d\n", d,
  			       out_shorts_c[0], out_shorts_c[1],
  			       out_shorts_opt[0], out_shorts_opt[1]);
  	}
  
  	/* Benchmark dsp_util_deinterleave */
  	for (samples = MAXSAMPLES; samples >= MINSAMPLES; samples /= 2) {
  		/* Measure original C deinterleave */
  		clock_gettime(CLOCK_MONOTONIC, &start); /* mark start time */
  		for (i = 0; i < ITERATIONS; ++i) {
  			dsp_util_deinterleave_reference(
  				in_shorts, out_floats_ptr_c, 2, samples);
  		}
  		clock_gettime(CLOCK_MONOTONIC, &end); /* mark the end time */
  		diff = (BILLION * (end.tv_sec - start.tv_sec) + end.tv_nsec -
  			start.tv_nsec) /
  		       1000000;
  		printf("deinterleave ORIG size = %6d, "
  		       "elapsed time = %llu ms\n",
  		       samples, (long long unsigned int)diff);
  
  		/* Measure optimized deinterleave */
  		clock_gettime(CLOCK_MONOTONIC, &start); /* mark start time */
  		for (i = 0; i < ITERATIONS; ++i) {
  			dsp_util_deinterleave((uint8_t *)in_shorts,
  					      out_floats_ptr_opt, 2,
  					      SND_PCM_FORMAT_S16_LE, samples);
  		}
  		clock_gettime(CLOCK_MONOTONIC, &end); /* mark the end time */
  		diff = (BILLION * (end.tv_sec - start.tv_sec) + end.tv_nsec -
  			start.tv_nsec) /
  		       1000000;
  		printf("deinterleave SIMD size = %6d, elapsed time = %llu ms\n",
  		       samples, (long long unsigned int)diff);
  
  		/* Test C and SIMD output match */
  		d = memcmp(out_floats_ptr_c[0], out_floats_ptr_opt[0],
  			   samples * 4);
  		if (d)
  			printf("left compare %d, %f %f\n", d,
  			       out_floats_ptr_c[0][0],
  			       out_floats_ptr_opt[0][0]);
  		d = memcmp(out_floats_ptr_c[1], out_floats_ptr_opt[1],
  			   samples * 4);
  		if (d)
  			printf("right compare %d, %f %f\n", d,
  			       out_floats_ptr_c[1][0],
  			       out_floats_ptr_opt[1][0]);
  	}
  
  	free(in_shorts);
  	free(out_floats_left_c);
  	free(out_floats_right_c);
  	free(out_floats_left_opt);
  	free(out_floats_right_opt);
  	free(out_shorts_c);
  	free(out_shorts_opt);
  
  	return 0;
  }
  