1/*
  2 * Copyright 2012-15 Advanced Micro Devices, Inc.
  3 *
  4 * Permission is hereby granted, free of charge, to any person obtaining a
  5 * copy of this software and associated documentation files (the "Software"),
  6 * to deal in the Software without restriction, including without limitation
  7 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
  8 * and/or sell copies of the Software, and to permit persons to whom the
  9 * Software is furnished to do so, subject to the following conditions:
 10 *
 11 * The above copyright notice and this permission notice shall be included in
 12 * all copies or substantial portions of the Software.
 13 *
 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 17 * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
 18 * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 19 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 20 * OTHER DEALINGS IN THE SOFTWARE.
 21 *
 22 * Authors: AMD
 23 *
 24 */
 25
 26#include "dm_services.h"
 27#include "include/fixed32_32.h"
 28
 29static uint64_t u64_div(uint64_t n, uint64_t d)
 30{
 31	uint32_t i = 0;
 32	uint64_t r;
 33	uint64_t q = div64_u64_rem(n, d, &r);
 34
 35	for (i = 0; i < 32; ++i) {
 36		uint64_t sbit = q & (1ULL<<63);
 37
 38		r <<= 1;
 39		r |= sbit ? 1 : 0;
 40		q <<= 1;
 41		if (r >= d) {
 42			r -= d;
 43			q |= 1;
 44		}
 45	}
 46
 47	if (2*r >= d)
 48		q += 1;
 49	return q;
 50}
 51
 52struct fixed32_32 dal_fixed32_32_from_fraction(uint32_t n, uint32_t d)
 53{
 54	struct fixed32_32 fx;
 55
 56	fx.value = u64_div((uint64_t)n << 32, (uint64_t)d << 32);
 57	return fx;
 58}
 59
 60struct fixed32_32 dal_fixed32_32_add(
 61	struct fixed32_32 lhs,
 62	struct fixed32_32 rhs)
 63{
 64	struct fixed32_32 fx = {lhs.value + rhs.value};
 65
 66	ASSERT(fx.value >= rhs.value);
 67	return fx;
 68}
 69
 70struct fixed32_32 dal_fixed32_32_add_int(struct fixed32_32 lhs, uint32_t rhs)
 71{
 72	struct fixed32_32 fx = {lhs.value + ((uint64_t)rhs << 32)};
 73
 74	ASSERT(fx.value >= (uint64_t)rhs << 32);
 75	return fx;
 76
 77}
 78struct fixed32_32 dal_fixed32_32_sub(
 79	struct fixed32_32 lhs,
 80	struct fixed32_32 rhs)
 81{
 82	struct fixed32_32 fx;
 83
 84	ASSERT(lhs.value >= rhs.value);
 85	fx.value = lhs.value - rhs.value;
 86	return fx;
 87}
 88
 89struct fixed32_32 dal_fixed32_32_sub_int(struct fixed32_32 lhs, uint32_t rhs)
 90{
 91	struct fixed32_32 fx;
 92
 93	ASSERT(lhs.value >= ((uint64_t)rhs<<32));
 94	fx.value = lhs.value - ((uint64_t)rhs<<32);
 95	return fx;
 96}
 97
 98struct fixed32_32 dal_fixed32_32_mul(
 99	struct fixed32_32 lhs,
100	struct fixed32_32 rhs)
101{
102	struct fixed32_32 fx;
103	uint64_t lhs_int = lhs.value>>32;
104	uint64_t lhs_frac = (uint32_t)lhs.value;
105	uint64_t rhs_int = rhs.value>>32;
106	uint64_t rhs_frac = (uint32_t)rhs.value;
107	uint64_t ahbh = lhs_int * rhs_int;
108	uint64_t ahbl = lhs_int * rhs_frac;
109	uint64_t albh = lhs_frac * rhs_int;
110	uint64_t albl = lhs_frac * rhs_frac;
111
112	ASSERT((ahbh>>32) == 0);
113
114	fx.value = (ahbh<<32) + ahbl + albh + (albl>>32);
115	return fx;
116
117}
118
119struct fixed32_32 dal_fixed32_32_mul_int(struct fixed32_32 lhs, uint32_t rhs)
120{
121	struct fixed32_32 fx;
122	uint64_t lhsi = (lhs.value>>32) * (uint64_t)rhs;
123	uint64_t lhsf;
124
125	ASSERT((lhsi>>32) == 0);
126	lhsf = ((uint32_t)lhs.value) * (uint64_t)rhs;
127	ASSERT((lhsi<<32) + lhsf >= lhsf);
128	fx.value = (lhsi<<32) + lhsf;
129	return fx;
130}
131
132struct fixed32_32 dal_fixed32_32_div(
133	struct fixed32_32 lhs,
134	struct fixed32_32 rhs)
135{
136	struct fixed32_32 fx;
137
138	fx.value = u64_div(lhs.value, rhs.value);
139	return fx;
140}
141
142struct fixed32_32 dal_fixed32_32_div_int(struct fixed32_32 lhs, uint32_t rhs)
143{
144	struct fixed32_32 fx;
145
146	fx.value = u64_div(lhs.value, (uint64_t)rhs << 32);
147	return fx;
148}
149
150uint32_t dal_fixed32_32_ceil(struct fixed32_32 v)
151{
152	ASSERT((uint32_t)v.value ? (v.value >> 32) + 1 >= 1 : true);
153	return (v.value>>32) + ((uint32_t)v.value ? 1 : 0);
154}
155
156uint32_t dal_fixed32_32_round(struct fixed32_32 v)
157{
158	ASSERT(v.value + (1ULL<<31) >= (1ULL<<31));
159	return (v.value + (1ULL<<31))>>32;
160}
161