1// Copyright 2016 The Fuchsia Authors
2// Copyright (c) 2013, Google Inc. All rights reserved.
3//
4// Use of this source code is governed by a MIT-style
5// license that can be found in the LICENSE file or at
6// https://opensource.org/licenses/MIT
7
8#pragma once
9
10#include <stdint.h>
11
12#ifndef DEBUG_FIXED_POINT
13#define DEBUG_FIXED_POINT 0
14#endif
15
16struct fp_32_64 {
17    uint32_t l0;  /* unshifted value */
18    uint32_t l32; /* value shifted left 32 bits (or bit -1 to -32) */
19    uint32_t l64; /* value shifted left 64 bits (or bit -33 to -64) */
20};
21
22#include "fixed_point_debug.h"
23
24static void
25fp_32_64_div_32_32(struct fp_32_64* result, uint32_t dividend, uint32_t divisor) {
26    uint64_t tmp;
27    uint32_t rem;
28
29    tmp = ((uint64_t)dividend << 32) / divisor;
30    rem = (uint32_t)(((uint64_t)dividend << 32) % divisor);
31    result->l0 = (uint32_t)(tmp >> 32);
32    result->l32 = (uint32_t)tmp;
33    tmp = ((uint64_t)rem << 32) / divisor;
34    result->l64 = (uint32_t)tmp;
35}
36
37static uint64_t
38mul_u32_u32(uint32_t a, uint32_t b, int a_shift, int b_shift) {
39    uint64_t ret = (uint64_t)a * b;
40    debug_mul_u32_u32(a, b, a_shift, b_shift, ret);
41    return ret;
42}
43
44static uint64_t
45u64_mul_u32_fp32_64(uint32_t a, struct fp_32_64 b) {
46    uint64_t tmp;
47    uint64_t res_0;
48    uint64_t res_l32;
49    uint32_t res_l32_32;
50    uint64_t ret;
51
52    res_0 = mul_u32_u32(a, b.l0, 0, 0);
53    tmp = mul_u32_u32(a, b.l32, 0, -32);
54    res_0 += tmp >> 32;
55    res_l32 = (uint32_t)tmp;
56    res_l32 += mul_u32_u32(a, b.l64, 0, -64) >> 32; /* Improve rounding accuracy */
57    res_0 += res_l32 >> 32;
58    res_l32_32 = (uint32_t)res_l32;
59    ret = res_0 + (res_l32_32 >> 31); /* Round to nearest integer */
60
61    debug_u64_mul_u32_fp32_64(a, b, res_0, res_l32_32, ret);
62
63    return ret;
64}
65
66static uint32_t
67u32_mul_u64_fp32_64(uint64_t a, struct fp_32_64 b) {
68    uint32_t a_r32 = (uint32_t)(a >> 32);
69    uint32_t a_0 = (uint32_t)a;
70    uint64_t res_l32;
71    uint32_t ret;
72
73    /* mul_u32_u32(a_r32, b.l0, 32, 0) does not affect result */
74    res_l32 = mul_u32_u32(a_0, b.l0, 0, 0) << 32;
75    res_l32 += mul_u32_u32(a_r32, b.l32, 32, -32) << 32;
76    res_l32 += mul_u32_u32(a_0, b.l32, 0, -32);
77    res_l32 += mul_u32_u32(a_r32, b.l64, 32, -64);
78    res_l32 += mul_u32_u32(a_0, b.l64, 0, -64) >> 32;              /* Improve rounding accuracy */
79    ret = (uint32_t)((res_l32 >> 32) + ((uint32_t)res_l32 >> 31)); /* Round to nearest integer */
80
81    debug_u32_mul_u64_fp32_64(a, b, res_l32, ret);
82
83    return ret;
84}
85
86static uint64_t
87u64_mul_u64_fp32_64(uint64_t a, struct fp_32_64 b) {
88    uint32_t a_r32 = (uint32_t)(a >> 32);
89    uint32_t a_0 = (uint32_t)a;
90    uint64_t res_0;
91    uint64_t res_l32;
92    uint32_t res_l32_32;
93    uint64_t tmp;
94    uint64_t ret;
95
96    tmp = mul_u32_u32(a_r32, b.l0, 32, 0);
97    res_0 = tmp << 32;
98    tmp = mul_u32_u32(a_0, b.l0, 0, 0);
99    res_0 += tmp;
100    tmp = mul_u32_u32(a_r32, b.l32, 32, -32);
101    res_0 += tmp;
102    tmp = mul_u32_u32(a_0, b.l32, 0, -32);
103    res_0 += tmp >> 32;
104    res_l32 = (uint32_t)tmp;
105    tmp = mul_u32_u32(a_r32, b.l64, 32, -64);
106    res_0 += tmp >> 32;
107    res_l32 += (uint32_t)tmp;
108    tmp = mul_u32_u32(a_0, b.l64, 0, -64); /* Improve rounding accuracy */
109    res_l32 += tmp >> 32;
110    res_0 += res_l32 >> 32;
111    res_l32_32 = (uint32_t)(res_l32);
112    ret = res_0 + (res_l32_32 >> 31); /* Round to nearest integer */
113
114    debug_u64_mul_u64_fp32_64(a, b, res_0, res_l32_32, ret);
115
116    return ret;
117}
118