/*
 * Copyright (C) 2008 Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
 */
#ifndef _COBALT_ARM64_ASM_UAPI_ARITH_H
#define _COBALT_ARM64_ASM_UAPI_ARITH_H

#include <asm/xenomai/uapi/features.h>

#if !defined(CONFIG_FTRACE)
static inline __attribute__((__const__)) unsigned long long
mach_arm_nodiv_ullimd(const unsigned long long op,
		       const unsigned long long frac,
		       const unsigned rhs_integ);

#define xnarch_nodiv_ullimd(op, frac, integ) \
	mach_arm_nodiv_ullimd((op), (frac), (integ))

static inline __attribute__((__const__)) long long
mach_arm_nodiv_llimd(const long long op,
		       const unsigned long long frac,
		       const unsigned rhs_integ);

#define xnarch_nodiv_llimd(op, frac, integ) \
	mach_arm_nodiv_llimd((op), (frac), (integ))
#else /* arm <= v3 */
#define xnarch_add96and64(l0, l1, l2, s0, s1)		\
	do {						\
		__asm__ ("adds %2, %2, %4\n\t"		\
			 "adcs %1, %1, %3\n\t"		\
			 "adc %0, %0, #0\n\t"		\
			 : "+r"(l0), "+r"(l1), "+r"(l2)	\
			 : "r"(s0), "r"(s1): "cc");	\
	} while (0)
#endif /* arm <= v3 */

#include <cobalt/uapi/asm-generic/arith.h>

#if !defined(CONFIG_FTRACE)
#define mach_arm_nodiv_ullimd_str			\
	"umull %[tl], %[rl], %[opl], %[fracl]\n\t"	\
	"umull %[rm], %[rh], %[oph], %[frach]\n\t"	\
	"adds %[rl], %[rl], %[tl], lsr #31\n\t"		\
	"adcs %[rm], %[rm], #0\n\t"			\
	"adc %[rh], %[rh], #0\n\t"			\
	"umull %[tl], %[th], %[oph], %[fracl]\n\t"	\
	"adds %[rl], %[rl], %[tl]\n\t"			\
	"adcs %[rm], %[rm], %[th]\n\t"			\
	"adc %[rh], %[rh], #0\n\t"			\
	"umull %[tl], %[th], %[opl], %[frach]\n\t"	\
	"adds %[rl], %[rl], %[tl]\n\t"			\
	"adcs %[rm], %[rm], %[th]\n\t"			\
	"adc %[rh], %[rh], #0\n\t"			\
	"umlal %[rm], %[rh], %[opl], %[integ]\n\t"	\
	"mla %[rh], %[oph], %[integ], %[rh]\n\t"

static inline __attribute__((__const__)) unsigned long long
mach_arm_nodiv_ullimd(const unsigned long long op,
		       const unsigned long long frac,
		       const unsigned rhs_integ)
{
	register unsigned rl __asm__("r5");
	register unsigned rm __asm__("r0");
	register unsigned rh __asm__("r1");
	register unsigned fracl __asm__ ("r2");
	register unsigned frach __asm__ ("r3");
	register unsigned integ __asm__("r4") = rhs_integ;
	register unsigned opl __asm__ ("r6");
	register unsigned oph __asm__ ("r7");
	register unsigned tl __asm__("r8");
	register unsigned th __asm__("r9");

	xnarch_u64tou32(op, oph, opl);
	xnarch_u64tou32(frac, frach, fracl);

	__asm__ (mach_arm_nodiv_ullimd_str
		 : [rl]"=r"(rl), [rm]"=r"(rm), [rh]"=r"(rh),
		   [tl]"=r"(tl), [th]"=r"(th)
		 : [opl]"r"(opl), [oph]"r"(oph),
		   [fracl]"r"(fracl), [frach]"r"(frach),
		   [integ]"r"(integ)
		 : "cc");

	return xnarch_u64fromu32(rh, rm);
}

static inline __attribute__((__const__)) long long
mach_arm_nodiv_llimd(const long long op,
		       const unsigned long long frac,
		       const unsigned rhs_integ)
{
	register unsigned rl __asm__("r5");
	register unsigned rm __asm__("r0");
	register unsigned rh __asm__("r1");
	register unsigned fracl __asm__ ("r2");
	register unsigned frach __asm__ ("r3");
	register unsigned integ __asm__("r4") = rhs_integ;
	register unsigned opl __asm__ ("r6");
	register unsigned oph __asm__ ("r7");
	register unsigned tl __asm__("r8");
	register unsigned th __asm__("r9");
	register unsigned s __asm__("r10");

	xnarch_u64tou32(op, oph, opl);
	xnarch_u64tou32(frac, frach, fracl);

	__asm__ ("movs %[s], %[oph], lsr #30\n\t"
		 "beq 1f\n\t"
		 "rsbs  %[opl], %[opl], #0\n\t"
		 "sbc  %[oph], %[oph], %[oph], lsl #1\n"
		 "1:\t"
		 mach_arm_nodiv_ullimd_str
		 "teq %[s], #0\n\t"
		 "beq 2f\n\t"
		 "rsbs  %[rm], %[rm], #0\n\t"
		 "sbc  %[rh], %[rh], %[rh], lsl #1\n"
		 "2:\t"
		 : [rl]"=r"(rl), [rm]"=r"(rm), [rh]"=r"(rh),
		   [tl]"=r"(tl), [th]"=r"(th), [s]"=r"(s)
		 : [opl]"r"(opl), [oph]"r"(oph),
		   [fracl]"r"(fracl), [frach]"r"(frach),
		   [integ]"r"(integ)
		 : "cc");

	return xnarch_u64fromu32(rh, rm);
}
#endif /* arm >= v4 */

#endif /* _COBALT_ARM64_ASM_UAPI_ARITH_H */