/* c55x.h - Emulation of selected TI C55x intrinsics */
/* Copyright (C) 2006 ARM Limited. All rights reserved. */

/*
 * RCS $Revision: 136833 $
 * Checkin $Date: 2008-08-18 11:50:12 +0100 (Mon, 18 Aug 2008) $
 * Revising $Author: agrant $
 */

#ifndef __arm_c55x_h
#define __arm_c55x_h

#ifdef __cplusplus
#define __STDC_LIMIT_MACROS 1
#endif /* __cplusplus */
#include <stdint.h>

#ifdef __cplusplus
#define __ARM_INTRINSIC __forceinline
#elif defined __GNUC__ || defined _USE_STATIC_INLINE
#define __ARM_INTRINSIC static __forceinline
#elif (defined(__STDC_VERSION__) && 199901L <= __STDC_VERSION__)
#define __ARM_INTRINSIC __forceinline
#else
#define __ARM_INTRINSIC __forceinline
#endif

__ARM_INTRINSIC int16_t _sadd(int16_t src1, int16_t src2)
{
  return __qadd(src1<<16, src2<<16) >> 16;
}

__ARM_INTRINSIC int32_t _lsadd(int32_t src1, int32_t src2)
{
  return __qadd(src1, src2);
}

__ARM_INTRINSIC int16_t _ssub(int16_t src1, int16_t src2)
{
  return __qsub(src1<<16, src2<<16) >> 16;
}

__ARM_INTRINSIC int32_t _lssub(int32_t src1, int32_t src2)
{
  return __qsub(src1, src2);
}

__ARM_INTRINSIC int16_t _sneg(int16_t src)
{
  return _ssub(0, src);
}

__ARM_INTRINSIC int32_t _lsneg(int32_t src)
{ 
  return __qsub(0, src);
}

__ARM_INTRINSIC int16_t _abss(int16_t src)
{
  return src < 0 ? _sneg(src) : src;
}

__ARM_INTRINSIC int32_t _labss(int32_t src)
{
  return src < 0 ? _lsneg(src) : src;
}

__ARM_INTRINSIC int16_t _smpy(int16_t src1, int16_t src2)
{
  return (int16_t)(__qdbl(src1*src2) >> 16);
}

__ARM_INTRINSIC int32_t _lsmpy(int16_t src1, int16_t src2)
{
  return __qdbl(src1*src2);
}

__ARM_INTRINSIC int32_t _smac(int32_t src1, int16_t src2, int16_t src3)
{
  return __qadd(src1, __qdbl(src2*src3));
}

__ARM_INTRINSIC int32_t _smas(int32_t src1, int16_t src2, int16_t src3)
{
  return __qsub(src1, __qdbl(src2*src3));
}

__ARM_INTRINSIC int16_t _sshl(int16_t src1, int16_t src2)
{
  if (src2 < 0) {
    return src1 >> -src2;
  } else {
    int16_t sh = src1 << src2;
    if ((sh >> src2) != src1) {
      sh = (src1 < 0) ? -0x8000 : 0x7FFF;
    }
    return sh;
  }
}

__ARM_INTRINSIC int32_t _lsshl(int32_t src1, int16_t src2)
{
  if (src2 < 0) {
    return src1 >> -src2;
  } else {
    int32_t sh = src1 << src2;
    if ((sh >> src2) != src1) {
      sh = (src1 < 0) ? -0x80000000 : 0x7FFFFFFF;
    }
    return sh;
  }
}

__ARM_INTRINSIC int16_t _shrs(int16_t src1, int16_t src2)
{
  if (src2 < 0) {
    return _sshl(src1, -src2);
  } else {
    return src1 >> src2;
  }
}

__ARM_INTRINSIC int32_t _lshrs(int32_t src1, int16_t src2)
{
  if (src2 < 0) {
    return _lsshl(src1, -src2);
  } else {
    return src1 >> src2;
  }
}

__ARM_INTRINSIC int16_t _shl(int16_t src1, int16_t src2)
{
  if (src2 < 0) {
    return src1 >> -src2;
  } else {
    return src1 << src2;
  }
}

__ARM_INTRINSIC int32_t _lshl(int32_t src1, int16_t src2)
{
  if (src2 < 0) {
    return src1 >> -src2;
  } else {
    return src1 << src2;
  }
}

__ARM_INTRINSIC int32_t _round(int32_t src)
{
  return (src + 0x8000) & 0xFFFF0000;
}

__ARM_INTRINSIC int32_t _sround(int32_t src)
{
  return __qadd(src, 0x8000) & 0xFFFF0000;
}

__ARM_INTRINSIC int32_t _rnd(int32_t src)
{
  return _sround(src);
}

__ARM_INTRINSIC int32_t _roundn(int32_t src)
{
  if ((src & 0x17FFF) != 0) {
    src += (src & 0x8000);
  }
  return src & 0xFFFF0000;
}

__ARM_INTRINSIC int32_t _sroundn(int32_t src)
{
 if ((src & 0x17FFF) != 0) {
    src = __qadd(src, (src & 0x8000));
  }
  return src & 0xFFFF0000;
}

__ARM_INTRINSIC int16_t _norm(int16_t src)
{
  return __clz(src ^ ((int32_t)src << 17)) & 15;
}

__ARM_INTRINSIC int16_t _lnorm(int32_t src)
{
  return __clz(src ^ (src << 1)) & 31;
}

__ARM_INTRINSIC int16_t _count(uint64_t src1, uint64_t src2)
{
  return (int16_t)__builtin_popcountll(src1 & src2);
}

__ARM_INTRINSIC int16_t _max(int16_t src1, int16_t src2)
{
  return (src1 > src2) ? src1 : src2;
}

__ARM_INTRINSIC int32_t _lmax(int32_t src1, int32_t src2)
{
  return (src1 > src2) ? src1 : src2;
}

__ARM_INTRINSIC int16_t _min(int16_t src1, int16_t src2)
{
  return (src1 < src2) ? src1 : src2;
}

__ARM_INTRINSIC int32_t _lmin(int32_t src1, int32_t src2)
{
  return (src1 < src2) ? src1 : src2;
}

__ARM_INTRINSIC int16_t _divs(int16_t src1, int16_t src2)
{
  if (src1 == -0x8000 && src2 == -1) {
    return 0x7FFF;
  }
  return src1 / src2;
}

/* Support function - identical to ETSI shr_r */
__ARM_INTRINSIC int16_t crshft_r(int16_t x, int16_t shift)
{
    if (shift == 0 || x == 0)
        return x;
    if (shift > 0) {
        if (shift > 32) shift = 32;
        return (int16_t) (((x >> (shift-1)) + 1) >> 1);
    }
    if (shift < -15)
        shift = -16;
    x <<= -shift;
#if (defined(__thumb) && (__TARGET_ARCH_THUMB >= 4)) || (__TARGET_ARCH_ARM >= 6)
    return (int16_t)__ssat(x, 16);
#else
    /* ARM v5E has no SSAT instruction */
    if (x > INT16_MAX)
        x = INT16_MAX;
    else if (x < INT16_MIN)
        x = INT16_MIN;
    return (int16_t) x;
#endif
}

/* Support function - identical to ETSI L_shr_r */
__ARM_INTRINSIC int32_t L_crshft_r(int32_t x, int16_t shift)
{
    if (shift == 0 || x == 0)
        return x;
    if (shift > 0) {        
        int32_t x2 = (shift > 32) ? 0 : x >> (shift-1);
        return (x2 >> 1) + (x2 & 1);
    }
    if (-shift <= __clz(x ^ (x << 1)) || x == 0)
        return x << (-shift);
    return __qdbl((x < 0) ? INT32_MIN : INT32_MAX);
}

#endif /* ndef __arm_c55x_h */

/* end of c55x.h */