// SPDX-License-Identifier: BSD-2-Clause
/*
 * Copyright (c) 2014, STMicroelectronics International N.V.
 */
#include "mpa.h"

/*************************************************************
 *
 *   HELPERS
 *
 *************************************************************/

/*------------------------------------------------------------
 *
 *  These functions have ARM assembler implementations
 *
 */
#if !defined(USE_ARM_ASM)

/*  --------------------------------------------------------------------
 *  Function:   __mpa_full_adder
 *
 *  A word_t sized full adder. Incoming carry is in *carry.
 *  The sum will be put in *sum and the
 *  outgoing carry will be returned in *carry
 */
void __mpa_full_adder(mpa_word_t a,
		      mpa_word_t b, mpa_word_t *sum, mpa_word_t *carry)
{
#if defined(MPA_SUPPORT_DWORD_T)
	mpa_dword_t _s;
	_s = (mpa_dword_t) (a) + (mpa_dword_t) (b) + (mpa_dword_t) (*carry);
	*sum = (mpa_word_t) (_s);
	*carry = ((mpa_word_t) (_s >> MPA_WORD_SIZE));
#else
	*sum = a + *carry;
	*carry = (*sum < a);
	*sum += b;
	*carry += (*sum < b);
#endif
}

/*  --------------------------------------------------------------------
 *   Function:   __mpa_full_sub
 *
 *   A word_t sized full subtraction function. Incoming carry is in *carry.
 *   The difference will be put in *diff and the outgoing carry will be returned
 *   in *carry.
 */
void __mpa_full_sub(mpa_word_t a,
		    mpa_word_t b, mpa_word_t *diff, mpa_word_t *carry)
{
#if defined(MPA_SUPPORT_DWORD_T)
	mpa_dword_t _d;
	_d = (mpa_dword_t) (a) - (mpa_dword_t) (b) - (mpa_dword_t) (*carry);
	*diff = (mpa_word_t) (_d);
	*carry = 0 - (mpa_word_t) (_d >> WORD_SIZE);
#else
	mpa_word_t _d;
	*diff = a - *carry;
	*carry = (*diff > a);
	_d = *diff - b;
	*carry += (_d > *diff);
	*diff = _d;
#endif
}

/*------------------------------------------------------------
 *
 *  __mpa_full_adder_ackum
 *
 *  A word_t sized full adder with ackumulate. *d  = *d + e + *carry
 *  Outgoing carry is in *carry
 */
void __mpa_full_adder_ackum(mpa_word_t *d, mpa_word_t e, mpa_word_t *carry)
{
#if defined(MPA_SUPPORT_DWORD_T)
	mpa_dword_t _s;
	_s = (mpa_dword_t) (*d) + (mpa_dword_t) (e) + (mpa_dword_t) (*carry);
	*d = (mpa_word_t) (_s);
	*carry = ((mpa_word_t) (_s >> MPA_WORD_SIZE));
#else
	mpa_word_t _s;
	_s = *d + *carry;
	*carry = (_s < *d);
	*d = _s + e;
	*carry += (*d < _s);
#endif
}

#endif /* of USR_ARM_ASM */

/*  --------------------------------------------------------------------
 *  Function:   __mpa_abs_add
 *
 *  Calculate |op1| + |op2|.
 *  op1 and op2 are pointers to word_t arrays holding the value.
 *  *Sum must be big enough to hold the result.
 */
void __mpa_abs_add(mpa_word_t *sum,
		   mpa_usize_t *sum_size,
		   const mpa_word_t *op1,
		   mpa_usize_t op1_size,
		   const mpa_word_t *op2, mpa_usize_t op2_size)
{
	const mpa_word_t *smaller;
	const mpa_word_t *larger;
	mpa_word_t *sum_begin;
	mpa_usize_t smaller_wsize;	/* size in words of the smallest */
	mpa_usize_t larger_wsize;	/* size in words of the largest */
	mpa_word_t carry;
	mpa_usize_t i;

	/* make sure we know which is the larger one */
	if (op1_size > op2_size) {
		larger = op1;
		smaller = op2;
		larger_wsize = op1_size;
		smaller_wsize = op2_size;
	} else {		/* op2 is the larger or same size */
		larger = op2;
		smaller = op1;
		larger_wsize = op2_size;
		smaller_wsize = op1_size;
	}

	sum_begin = sum;
	carry = 0;
	i = 0;
	while (i < smaller_wsize) {
		__mpa_full_adder(*(smaller++), *(larger++), sum++, &carry);
		i++;
	}
	while (carry && (i < larger_wsize)) {
		__mpa_full_adder(0, *(larger++), sum++, &carry);
		i++;
	}

	if (i < larger_wsize) {
		/* carry did not propagate all the way, copy the rest */
		mpa_memcpy(sum, larger, WORDS_TO_BYTES(larger_wsize - i));
		sum += larger_wsize - i;
	}

	if (carry != 0)
		*(sum++) = carry;
	*sum_size = (mpa_word_t) (sum - sum_begin);
}

/*  --------------------------------------------------------------------
 *  Function:   __mpa_abs_add_ackum
 *
 *  Calculate dest = dest + src. Both dest and src must be positive
 *  dest must be big enough to hold the result.
 */
void __mpa_abs_add_ackum(mpanum dest, const mpanum src)
{
	const mpa_word_t *sdig;	/* source digits */
	mpa_word_t *ddig;	/* dest digits */
	mpa_word_t *dest_begin;

	mpa_word_t carry;
	mpa_usize_t i;

	i = src->size - dest->size;
	if (i <= 0)
		i = 0;
	mpa_memset(dest->d + dest->size, 0, (1 + i) * BYTES_PER_WORD);

	dest_begin = dest->d;
	sdig = src->d;
	ddig = dest->d;
	i = 0;
	carry = 0;
	while (i < src->size) {
		__mpa_full_adder_ackum(ddig++, *(sdig++), &carry);
		/*
		 * _s = (mpa_dword_t)*(ddig) + (mpa_dword_t)*(sdig++) +
		 *	(mpa_dword_t)carry;
		 * *(ddig++) = (mpa_word_t)(_s);
		 * carry = (mpa_word_t)(_s >> WORD_SIZE);
		 */
		i++;
	}
	while (carry) {
		__mpa_full_adder_ackum(ddig++, 0, &carry);
		/*
		 * _s = (mpa_dword_t)*(ddig) + (mpa_dword_t)carry;
		 * *(ddig++) = (mpa_word_t)(_s);
		 * carry = (mpa_word_t)(_s >> WORD_SIZE);
		 */
	}
	i = (mpa_word_t) (ddig - dest_begin);
	if (i > dest->size)
		dest->size = i;
}

/*  --------------------------------------------------------------------
 *  Function:   __mpa_abs_sub
 *
 *  Calculate |op1| - |op2|, where  |op1| >= |op2| must hold.
 *  op1 and op2 are pointers to word_t arrays holding the value.
 *  *Diff must be big enough to hold the result. diff_size is
 *  updated with the number of significant words in *diff.
 */
void __mpa_abs_sub(mpa_word_t *diff,
		   mpa_usize_t *diff_size,
		   const mpa_word_t *op1,
		   mpa_usize_t op1_size,
		   const mpa_word_t *op2, mpa_usize_t op2_size)
{
	mpa_word_t carry;
	mpa_usize_t i;

	carry = 0;
	i = 0;
	while (i < op2_size) {
		__mpa_full_sub(*(op1++), *(op2++), diff++, &carry);
		i++;
	}
	/*
	 * Here we have no more digits in op2, we only need to keep on
	 * subtracting 0 from op1, and deal with carry.
	 */
	while (carry && (i < op1_size)) {
		__mpa_full_sub(*(op1++), 0, diff++, &carry);
		i++;
	}

	if (i < op1_size) {
		/*
		 * Carry did not propagate all the way, now we only need to
		 * copy the rest.
		 */
		mpa_memcpy(diff, op1, WORDS_TO_BYTES(op1_size - i));
		diff += op1_size - i;
	}
	/* check size of diff */
	i = op1_size;
	while ((i > 0) && (*(--diff) == 0))
		i--;
	*diff_size = i;
}

/*************************************************************
 *
 *   LIB FUNCTIONS
 *
 *************************************************************/

/*  --------------------------------------------------------------------
 *  Function:   mpa_add
 *
 *  Adds op1 and op2 and puts the result in dest. Dest could be one of the
 *  operands.
 */
void mpa_add(mpanum dest,
	     const mpanum op1, const mpanum op2, mpa_scratch_mem pool)
{
	int sign_1;
	int sign_2;
	mpa_word_t size_1;
	mpa_word_t size_2;
	mpanum tmp_dest;
	int mem_marker;

	size_1 = __mpanum_size(op1);
	size_2 = __mpanum_size(op2);

	sign_1 = __mpanum_sign(op1);
	sign_2 = __mpanum_sign(op2);

	/* Handle the case when dest is one of the operands */
	mem_marker = ((dest == op1) || (dest == op2));
	if (mem_marker)
		mpa_alloc_static_temp_var(&tmp_dest, pool);
	else
		tmp_dest = dest;

	/*  Check if we must do a subtraction or a addition.
	 *  Remember, we're not allowed to modify op1 or op2.
	 */

	if (sign_1 == sign_2) {	/* same signs */
		/* tmp_dest = |op1| + |op2|  or tmp_dest = -(|op1| + |op2|) */
		__mpa_abs_add(tmp_dest->d, &(tmp_dest->size), op1->d, size_1,
			      op2->d, size_2);

		if (sign_1 == MPA_NEG_SIGN)
			__mpanum_neg(tmp_dest);

	} else {		/* different signs */
		if (sign_1 == MPA_POS_SIGN) {	/* op1 positive, op1 + (-op2) */
			if (__mpa_abs_greater_than(op1, op2)) {
				/* |op1| > |op2| */

				/* tmp_dest = |op1| - |op2| */
				__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size),
					      op1->d, size_1, op2->d, size_2);

			} else {	/* |op2| >= |op1| */
				/* tmp_dest = - ( |op2| - |op1|) */
				__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size),
					      op2->d, size_2, op1->d, size_1);
				__mpanum_neg(tmp_dest);
			}

		} else {	/* op2 positive, (-op1) + op2 */
			if (__mpa_abs_greater_than(op1, op2)) {
				/* |op1| > |op2| */

				/* tmp_dest = - (|op1| - |op2|) */
				__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size),
					      op1->d, size_1, op2->d, size_2);
				__mpanum_neg(tmp_dest);
			} else {	/* |op2| >= |op1| */
				/* tmp_dest = |op2| - |op1| */
				__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size),
					      op2->d, size_2, op1->d, size_1);
			}
		}
	}

	mpa_copy(dest, tmp_dest);
	if (mem_marker)
		mpa_free_static_temp_var(&tmp_dest, pool);
}

/*  --------------------------------------------------------------------
 *  Function:   mpa_sub
 *
 *  Calculated op1 - op2 and stores the result in dest. Dest could be one of
 *  the operands.
 */
void mpa_sub(mpanum dest,
	     const mpanum op1, const mpanum op2, mpa_scratch_mem pool)
{
	int sign_1;
	int sign_2;
	mpa_word_t size_1;
	mpa_word_t size_2;
	mpanum tmp_dest;
	int mem_marker;

	size_1 = __mpanum_size(op1);
	size_2 = __mpanum_size(op2);

	sign_1 = __mpanum_sign(op1);
	sign_2 = __mpanum_sign(op2);

	/* Handle the case when dest is one of the operands */
	mem_marker = ((dest == op1) || (dest == op2));
	if (mem_marker)
		mpa_alloc_static_temp_var(&tmp_dest, pool);
	else
		tmp_dest = dest;

	/*
	 * Check if we must do a subtraction or a addition. Remember,
	 * we're not allowed to modify op1 or op2.
	 */
	if (sign_1 == sign_2) {	/* same signs */
		if (sign_1 == MPA_POS_SIGN) {	/* both positive, op1 - op2 */
			if (__mpa_abs_greater_than(op1, op2)) {
				/* |op1| > |op2| */

				__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size),
					      op1->d, size_1, op2->d, size_2);
			} else {	/* |op1| <= |op2| */
				/* tmp_dest = - (|op2| - |op1|) */
				__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size),
					      op2->d, size_2, op1->d, size_1);
				__mpanum_neg(tmp_dest);
			}
		} else {
			/* both negative, (-op1) - (-op2) = -(op1 - op2) */

			if (__mpa_abs_greater_than(op1, op2)) {
				/* |op1| > |op2| */

				/* tmp_dest = -(|op1| - |op2|) */
				__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size),
					      op1->d, size_1, op2->d, size_2);
				__mpanum_neg(tmp_dest);
			} else {	/* |op1| <= |op2| */
				/* tmp_dest = |op2| - |op1| */
				__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size),
					      op2->d, size_2, op1->d, size_1);
			}
		}
	} else {		/* different signs */
		if (sign_1 == MPA_POS_SIGN) {	/* op1 positive, op1 - (-op2) */
			/* tmp_dest = |op1| + |op2| */
			__mpa_abs_add(tmp_dest->d, &(tmp_dest->size), op1->d,
				      size_1, op2->d, size_2);
		} else {	/* op2 positive, (-op1) - op2 = - (op1 + op2) */
			/* tmp_dest = -(|op1| + |op2|) */
			__mpa_abs_add(tmp_dest->d, &(tmp_dest->size), op1->d,
				      size_1, op2->d, size_2);
			__mpanum_neg(tmp_dest);
		}
	}

	mpa_copy(dest, tmp_dest);
	if (mem_marker)
		mpa_free_static_temp_var(&tmp_dest, pool);
}

/*  --------------------------------------------------------------------
 *  Function:   mpa_neg
 *
 *  Assigns dest the value of src, but with a change of sign. Dest and src
 *  could be the same variable.
 */
void mpa_neg(mpanum dest, const mpanum src)
{
	mpa_copy(dest, src);
	__mpanum_neg(dest);
}

/*  --------------------------------------------------------------------
 *  Function:   mpa_add_word
 *
 *  Add a word_t (op2) to op1 and put result in dest
 */
void mpa_add_word(mpanum dest,
		 const mpanum op1, mpa_word_t op2, mpa_scratch_mem pool)
{
	int sign_1;
	mpanum tmp_dest;
	mpa_word_t size_1;
	int mem_marker;

	if (op2 == 0) {
		mpa_copy(dest, op1);
		return;
	}

	if (__mpanum_is_zero(op1)) {
		dest->size = 1;
		dest->d[0] = op2;
		return;
	}

	sign_1 = __mpanum_sign(op1);
	size_1 = __mpanum_size(op1);

	/* handle the case when dest is the operand */
	mem_marker = (dest == op1);
	if (mem_marker)
		mpa_alloc_static_temp_var(&tmp_dest, pool);
	else
		tmp_dest = dest;

	/* find out if we should do an add or a sub, op2 is always positive */
	if (sign_1 == MPA_POS_SIGN) {	/* add */
		/* tmp_dest = |op1| + op2 */
		__mpa_abs_add(tmp_dest->d, &(tmp_dest->size), op1->d, size_1,
			      &op2, 1);
	} else {		/* sub, op1 is negative: (-op1) + op2 */
		if (__mpanum_size(op1) > 1 || __mpanum_lsw(op1) > op2) {
			/* |op1| > |op2| */

			/* tmp_dest = - (|op1| - op2) */
			__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size), op1->d,
				      size_1, &op2, 1);
			__mpanum_neg(tmp_dest);
		} else {	/* op2 >= |op1| */
			/* tmp_dest = op2 - |op1| */
			tmp_dest->d[0] = op2 - op1->d[0];
			tmp_dest->size = (tmp_dest->d[0] == 0) ? 0 : 1;
		}
	}

	mpa_copy(dest, tmp_dest);
	if (mem_marker)
		mpa_free_static_temp_var(&tmp_dest, pool);
}

/*  --------------------------------------------------------------------
 *  Function:   mpa_sub_word
 *
 *  Calculate op1 - op2, op2 is a word_t and always positive.
 */
void mpa_sub_word(mpanum dest,
		 const mpanum op1, mpa_word_t op2, mpa_scratch_mem pool)
{
	int sign_1;
	mpanum tmp_dest;
	mpa_word_t size_1;
	char mem_marker;

	if (op2 == 0) {
		mpa_copy(dest, op1);
		return;
	}

	if (__mpanum_is_zero(op1)) {
		dest->size = -1;
		dest->d[0] = op2;
		return;
	}

	sign_1 = __mpanum_sign(op1);
	size_1 = __mpanum_size(op1);

	/* handle the case when dest is the operand */
	mem_marker = (dest == op1);
	if (mem_marker)
		mpa_alloc_static_temp_var(&tmp_dest, pool);
	else
		tmp_dest = dest;

	/*
	 * Find out if we should do an add or a sub, op2 is always positive
	 *
	 *  dest = op1 - op2        if op1 > op2 >= 0
	 *  dest = -(op2 - op1)     if op2 >= op1 >= 0
	 *  dest = -(|op1| + op2)   if op1 < 0
	 *
	 */
	if (sign_1 == MPA_POS_SIGN) {
		if (__mpanum_size(op1) > 1 || __mpanum_lsw(op1) > op2) {
			__mpa_abs_sub(tmp_dest->d, &(tmp_dest->size), op1->d,
				      size_1, &op2, 1);
		} else {
			tmp_dest->d[0] = op2 - op1->d[0];
			tmp_dest->size = (tmp_dest->d[0] == 0) ? 0 : -1;
		}
	} else {
		__mpa_abs_add(tmp_dest->d, &(tmp_dest->size), op1->d, size_1,
			      &op2, 1);
		__mpanum_neg(tmp_dest);
	}

	mpa_copy(dest, tmp_dest);
	if (mem_marker)
		mpa_free_static_temp_var(&tmp_dest, pool);
}