#include "util.h"

uint128_t mul64(const uint64_t a, const uint64_t b)
{
	const uint64_t ah = hi32(a), bh = hi32(b);
	const uint64_t al = lo32(a), bl = lo32(b);
	const uint64_t ahbl = ah * bl;
	const uint64_t albh = al * bh;
	const uint64_t albl = al * bl;
	const uint64_t ahbh = ah * bh;
	const uint64_t mid = hi32(albl) + lo32(ahbl) + lo32(albh);
	
	uint128_t result;
	result.hi = ahbh + hi32(ahbl) + hi32(albh) + hi32(mid);
	result.lo = lo32(albl) + (mid << 32);

	return result;
}

uint32_t bitmask32(const int begin, const int end)
{
	const uint32_t ones = uint32_t(-1);
	const uint32_t mask_begin = ones >> begin;
	const uint32_t mask_end = ones << (31 - end);
	const uint32_t mask = begin <= end ? (mask_begin & mask_end) : (mask_begin | mask_end);
	return mask;
}

uint64_t bitmask64(const int begin, const int end)
{
	const uint64_t ones = uint64_t(-1);
	const uint64_t mask_begin = ones >> begin;
	const uint64_t mask_end = ones << (63 - end);
	const uint64_t mask = begin <= end ? (mask_begin & mask_end) : (mask_begin | mask_end);
	return mask;
}

uint32_t rotl(const uint32_t x, int n)
{
	const uint32_t result = (x << n) | (x >> (32 - n));
	return result;
}

uint32_t rotr(const uint32_t x, int n)
{
	const uint32_t result = (x >> n) | (x << (32 - n));
	return result;
}

uint32_t rlwinm_bitmask(const uint32_t rs, const uint32_t sh, const uint32_t bm)
{
	const uint32_t ra = rotl(rs, sh) & bm;
	return ra;
}

uint32_t rlwinm(const uint32_t rs, const uint32_t sh, const int mb, const int me)
{
	const uint32_t mask = bitmask32(mb, me);
	const uint32_t ra = rlwinm_bitmask(rs, sh, mask);
	return ra;
}

uint32_t rlwimi(const uint32_t ra, const uint32_t rs, const uint32_t sh, const int mb, const int me)
{
	const uint32_t mask = bitmask32(mb, me);
	const uint32_t nra = (ra & ~mask) | rotl(rs, sh) & mask;
	return nra;
}

uint32_t clrlslwi(const uint32_t rs, const int b, const int n)
{
	assert(n <= b && b <= 31);
	const uint32_t ra = rlwinm(rs, n, b - n, 31 - n);
	return ra;
}

uint32_t slw(const uint32_t ra, const uint32_t rs, const int rb)
{
	const int n  = rb & 0x1F;
	const uint64_t mask = (rb & 32) == 0 ? bitmask64(32, 63 - n) : 0;
	const uint32_t nra = rotl(rs, n) & mask;
	return nra;
}

int32_t srawi(const int32_t rs, const int b)
{
	const int32_t ra = int32_t(rs) >> b;
	return ra;
}