/*
 * SPDX-License-Identifier: BSD-2-Clause
 *
 * Copyright (c) 2010-2019 Red Hat, Inc.
 */

#define _GNU_SOURCE        // for FE_NOMASK_ENV

#include <errno.h>

/*  Mask and shift amount for rounding bits.  */
#define FE_CW_ROUND_MASK	(0x0c00)
#define FE_CW_ROUND_SHIFT	(10)
/*  Same, for SSE MXCSR.  */
#define FE_MXCSR_ROUND_MASK	(0x6000)
#define FE_MXCSR_ROUND_SHIFT	(13)

#define FE_DOWNWARD     (1)
#define FE_TONEAREST    (0)
#define FE_TOWARDZERO   (3)
#define FE_UPWARD       (2)

static inline int use_sse(void)
{
  unsigned int edx, eax;

  /* Check for presence of SSE: invoke CPUID #1, check EDX bit 25.  */
  eax = 1;
  __asm__ volatile ("cpuid" : "=d" (edx), "+a" (eax) :: "%ecx", "%ebx");
  /* If this flag isn't set we'll avoid trying to execute any SSE.  */
  if ((edx & (1 << 25)) != 0)
    return 1;

  return 0;
}

/*  Returns the currently selected rounding mode, represented by one of the
   values of the defined rounding mode macros.  */
int
fegetround (void)
{
  unsigned short cw;

  /* Get control word.  We assume SSE and x87 stay in sync.  */
  __asm__ volatile ("fnstcw %0" : "=m" (*&cw) : );

  return (cw & FE_CW_ROUND_MASK) >> FE_CW_ROUND_SHIFT;
}

/*  Changes the currently selected rounding mode to round. If round does
   not correspond to one of the supported rounding modes nothing is changed.
   fesetround returns zero if it changed the rounding mode, a nonzero value
   if the mode is not supported.  */
int
fesetround (int round)
{
  unsigned short cw;
  unsigned int mxcsr = 0;

  /* Will succeed for any valid value of the input parameter.  */
  if (round < FE_TONEAREST || round > FE_TOWARDZERO)
    return EINVAL;

  /* Get control words.  */
  __asm__ volatile ("fnstcw %0" : "=m" (cw) : );
if (use_sse())
    __asm__ volatile ("stmxcsr %0" : "=m" (mxcsr) : );

  /* Twiddle bits.  */
  cw &= ~FE_CW_ROUND_MASK;
  cw |= (round << FE_CW_ROUND_SHIFT);
  

  mxcsr &= ~FE_MXCSR_ROUND_MASK;
  mxcsr |= (round << FE_MXCSR_ROUND_SHIFT);
/* Set back into FPU state.  */
  __asm__ volatile ("fldcw %0" :: "m" (cw));
  if (use_sse())
    __asm__ volatile ("ldmxcsr %0" :: "m" (mxcsr));

  /* Indicate success.  */
  return 0;
}