1*f843863dSArend van Spriel // SPDX-License-Identifier: ISC
205491d2cSKalle Valo /*
305491d2cSKalle Valo * Copyright (c) 2010 Broadcom Corporation
405491d2cSKalle Valo */
505491d2cSKalle Valo
605491d2cSKalle Valo #include "phy_qmath.h"
705491d2cSKalle Valo
805491d2cSKalle Valo /*
905491d2cSKalle Valo * Description: This function make 16 bit unsigned multiplication.
1005491d2cSKalle Valo * To fit the output into 16 bits the 32 bit multiplication result is right
1105491d2cSKalle Valo * shifted by 16 bits.
1205491d2cSKalle Valo */
qm_mulu16(u16 op1,u16 op2)1305491d2cSKalle Valo u16 qm_mulu16(u16 op1, u16 op2)
1405491d2cSKalle Valo {
1505491d2cSKalle Valo return (u16) (((u32) op1 * (u32) op2) >> 16);
1605491d2cSKalle Valo }
1705491d2cSKalle Valo
1805491d2cSKalle Valo /*
1905491d2cSKalle Valo * Description: This function make 16 bit multiplication and return the result
2005491d2cSKalle Valo * in 16 bits. To fit the multiplication result into 16 bits the multiplication
2105491d2cSKalle Valo * result is right shifted by 15 bits. Right shifting 15 bits instead of 16 bits
2205491d2cSKalle Valo * is done to remove the extra sign bit formed due to the multiplication.
2305491d2cSKalle Valo * When both the 16bit inputs are 0x8000 then the output is saturated to
2405491d2cSKalle Valo * 0x7fffffff.
2505491d2cSKalle Valo */
qm_muls16(s16 op1,s16 op2)2605491d2cSKalle Valo s16 qm_muls16(s16 op1, s16 op2)
2705491d2cSKalle Valo {
2805491d2cSKalle Valo s32 result;
2905491d2cSKalle Valo if (op1 == (s16) 0x8000 && op2 == (s16) 0x8000)
3005491d2cSKalle Valo result = 0x7fffffff;
3105491d2cSKalle Valo else
3205491d2cSKalle Valo result = ((s32) (op1) * (s32) (op2));
3305491d2cSKalle Valo
3405491d2cSKalle Valo return (s16) (result >> 15);
3505491d2cSKalle Valo }
3605491d2cSKalle Valo
3705491d2cSKalle Valo /*
3805491d2cSKalle Valo * Description: This function add two 32 bit numbers and return the 32bit
3905491d2cSKalle Valo * result. If the result overflow 32 bits, the output will be saturated to
4005491d2cSKalle Valo * 32bits.
4105491d2cSKalle Valo */
qm_add32(s32 op1,s32 op2)4205491d2cSKalle Valo s32 qm_add32(s32 op1, s32 op2)
4305491d2cSKalle Valo {
4405491d2cSKalle Valo s32 result;
4505491d2cSKalle Valo result = op1 + op2;
4605491d2cSKalle Valo if (op1 < 0 && op2 < 0 && result > 0)
4705491d2cSKalle Valo result = 0x80000000;
4805491d2cSKalle Valo else if (op1 > 0 && op2 > 0 && result < 0)
4905491d2cSKalle Valo result = 0x7fffffff;
5005491d2cSKalle Valo
5105491d2cSKalle Valo return result;
5205491d2cSKalle Valo }
5305491d2cSKalle Valo
5405491d2cSKalle Valo /*
5505491d2cSKalle Valo * Description: This function add two 16 bit numbers and return the 16bit
5605491d2cSKalle Valo * result. If the result overflow 16 bits, the output will be saturated to
5705491d2cSKalle Valo * 16bits.
5805491d2cSKalle Valo */
qm_add16(s16 op1,s16 op2)5905491d2cSKalle Valo s16 qm_add16(s16 op1, s16 op2)
6005491d2cSKalle Valo {
6105491d2cSKalle Valo s16 result;
6205491d2cSKalle Valo s32 temp = (s32) op1 + (s32) op2;
6305491d2cSKalle Valo if (temp > (s32) 0x7fff)
6405491d2cSKalle Valo result = (s16) 0x7fff;
6505491d2cSKalle Valo else if (temp < (s32) 0xffff8000)
6605491d2cSKalle Valo result = (s16) 0xffff8000;
6705491d2cSKalle Valo else
6805491d2cSKalle Valo result = (s16) temp;
6905491d2cSKalle Valo
7005491d2cSKalle Valo return result;
7105491d2cSKalle Valo }
7205491d2cSKalle Valo
7305491d2cSKalle Valo /*
7405491d2cSKalle Valo * Description: This function make 16 bit subtraction and return the 16bit
7505491d2cSKalle Valo * result. If the result overflow 16 bits, the output will be saturated to
7605491d2cSKalle Valo * 16bits.
7705491d2cSKalle Valo */
qm_sub16(s16 op1,s16 op2)7805491d2cSKalle Valo s16 qm_sub16(s16 op1, s16 op2)
7905491d2cSKalle Valo {
8005491d2cSKalle Valo s16 result;
8105491d2cSKalle Valo s32 temp = (s32) op1 - (s32) op2;
8205491d2cSKalle Valo if (temp > (s32) 0x7fff)
8305491d2cSKalle Valo result = (s16) 0x7fff;
8405491d2cSKalle Valo else if (temp < (s32) 0xffff8000)
8505491d2cSKalle Valo result = (s16) 0xffff8000;
8605491d2cSKalle Valo else
8705491d2cSKalle Valo result = (s16) temp;
8805491d2cSKalle Valo
8905491d2cSKalle Valo return result;
9005491d2cSKalle Valo }
9105491d2cSKalle Valo
9205491d2cSKalle Valo /*
9305491d2cSKalle Valo * Description: This function make a 32 bit saturated left shift when the
9405491d2cSKalle Valo * specified shift is +ve. This function will make a 32 bit right shift when
9505491d2cSKalle Valo * the specified shift is -ve. This function return the result after shifting
9605491d2cSKalle Valo * operation.
9705491d2cSKalle Valo */
qm_shl32(s32 op,int shift)9805491d2cSKalle Valo s32 qm_shl32(s32 op, int shift)
9905491d2cSKalle Valo {
10005491d2cSKalle Valo int i;
10105491d2cSKalle Valo s32 result;
10205491d2cSKalle Valo result = op;
10305491d2cSKalle Valo if (shift > 31)
10405491d2cSKalle Valo shift = 31;
10505491d2cSKalle Valo else if (shift < -31)
10605491d2cSKalle Valo shift = -31;
10705491d2cSKalle Valo if (shift >= 0) {
10805491d2cSKalle Valo for (i = 0; i < shift; i++)
10905491d2cSKalle Valo result = qm_add32(result, result);
11005491d2cSKalle Valo } else {
11105491d2cSKalle Valo result = result >> (-shift);
11205491d2cSKalle Valo }
11305491d2cSKalle Valo
11405491d2cSKalle Valo return result;
11505491d2cSKalle Valo }
11605491d2cSKalle Valo
11705491d2cSKalle Valo /*
11805491d2cSKalle Valo * Description: This function make a 16 bit saturated left shift when the
11905491d2cSKalle Valo * specified shift is +ve. This function will make a 16 bit right shift when
12005491d2cSKalle Valo * the specified shift is -ve. This function return the result after shifting
12105491d2cSKalle Valo * operation.
12205491d2cSKalle Valo */
qm_shl16(s16 op,int shift)12305491d2cSKalle Valo s16 qm_shl16(s16 op, int shift)
12405491d2cSKalle Valo {
12505491d2cSKalle Valo int i;
12605491d2cSKalle Valo s16 result;
12705491d2cSKalle Valo result = op;
12805491d2cSKalle Valo if (shift > 15)
12905491d2cSKalle Valo shift = 15;
13005491d2cSKalle Valo else if (shift < -15)
13105491d2cSKalle Valo shift = -15;
13205491d2cSKalle Valo if (shift > 0) {
13305491d2cSKalle Valo for (i = 0; i < shift; i++)
13405491d2cSKalle Valo result = qm_add16(result, result);
13505491d2cSKalle Valo } else {
13605491d2cSKalle Valo result = result >> (-shift);
13705491d2cSKalle Valo }
13805491d2cSKalle Valo
13905491d2cSKalle Valo return result;
14005491d2cSKalle Valo }
14105491d2cSKalle Valo
14205491d2cSKalle Valo /*
14305491d2cSKalle Valo * Description: This function make a 16 bit right shift when shift is +ve.
14405491d2cSKalle Valo * This function make a 16 bit saturated left shift when shift is -ve. This
14505491d2cSKalle Valo * function return the result of the shift operation.
14605491d2cSKalle Valo */
qm_shr16(s16 op,int shift)14705491d2cSKalle Valo s16 qm_shr16(s16 op, int shift)
14805491d2cSKalle Valo {
14905491d2cSKalle Valo return qm_shl16(op, -shift);
15005491d2cSKalle Valo }
15105491d2cSKalle Valo
15205491d2cSKalle Valo /*
15305491d2cSKalle Valo * Description: This function return the number of redundant sign bits in a
15405491d2cSKalle Valo * 32 bit number. Example: qm_norm32(0x00000080) = 23
15505491d2cSKalle Valo */
qm_norm32(s32 op)15605491d2cSKalle Valo s16 qm_norm32(s32 op)
15705491d2cSKalle Valo {
15805491d2cSKalle Valo u16 u16extraSignBits;
15905491d2cSKalle Valo if (op == 0) {
16005491d2cSKalle Valo return 31;
16105491d2cSKalle Valo } else {
16205491d2cSKalle Valo u16extraSignBits = 0;
16305491d2cSKalle Valo while ((op >> 31) == (op >> 30)) {
16405491d2cSKalle Valo u16extraSignBits++;
16505491d2cSKalle Valo op = op << 1;
16605491d2cSKalle Valo }
16705491d2cSKalle Valo }
16805491d2cSKalle Valo return u16extraSignBits;
16905491d2cSKalle Valo }
17005491d2cSKalle Valo
1714c0bfeaaSTobias Regnery /* This table is log2(1+(i/32)) where i=[0:1:32], in q.15 format */
17205491d2cSKalle Valo static const s16 log_table[] = {
17305491d2cSKalle Valo 0,
17405491d2cSKalle Valo 1455,
17505491d2cSKalle Valo 2866,
17605491d2cSKalle Valo 4236,
17705491d2cSKalle Valo 5568,
17805491d2cSKalle Valo 6863,
17905491d2cSKalle Valo 8124,
18005491d2cSKalle Valo 9352,
18105491d2cSKalle Valo 10549,
18205491d2cSKalle Valo 11716,
18305491d2cSKalle Valo 12855,
18405491d2cSKalle Valo 13968,
18505491d2cSKalle Valo 15055,
18605491d2cSKalle Valo 16117,
18705491d2cSKalle Valo 17156,
18805491d2cSKalle Valo 18173,
18905491d2cSKalle Valo 19168,
19005491d2cSKalle Valo 20143,
19105491d2cSKalle Valo 21098,
19205491d2cSKalle Valo 22034,
19305491d2cSKalle Valo 22952,
19405491d2cSKalle Valo 23852,
19505491d2cSKalle Valo 24736,
19605491d2cSKalle Valo 25604,
19705491d2cSKalle Valo 26455,
19805491d2cSKalle Valo 27292,
19905491d2cSKalle Valo 28114,
20005491d2cSKalle Valo 28922,
20105491d2cSKalle Valo 29717,
20205491d2cSKalle Valo 30498,
20305491d2cSKalle Valo 31267,
2044c0bfeaaSTobias Regnery 32024,
205c9a61469SStefan Agner 32767
20605491d2cSKalle Valo };
20705491d2cSKalle Valo
20805491d2cSKalle Valo #define LOG_TABLE_SIZE 32 /* log_table size */
20905491d2cSKalle Valo #define LOG2_LOG_TABLE_SIZE 5 /* log2(log_table size) */
21005491d2cSKalle Valo #define Q_LOG_TABLE 15 /* qformat of log_table */
21105491d2cSKalle Valo #define LOG10_2 19728 /* log10(2) in q.16 */
21205491d2cSKalle Valo
21305491d2cSKalle Valo /*
21405491d2cSKalle Valo * Description:
21505491d2cSKalle Valo * This routine takes the input number N and its q format qN and compute
21605491d2cSKalle Valo * the log10(N). This routine first normalizes the input no N. Then N is in
21705491d2cSKalle Valo * mag*(2^x) format. mag is any number in the range 2^30-(2^31 - 1).
21805491d2cSKalle Valo * Then log2(mag * 2^x) = log2(mag) + x is computed. From that
21905491d2cSKalle Valo * log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed.
22005491d2cSKalle Valo * This routine looks the log2 value in the table considering
22105491d2cSKalle Valo * LOG2_LOG_TABLE_SIZE+1 MSBs. As the MSB is always 1, only next
22205491d2cSKalle Valo * LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup. Next 16 MSBs are used
22305491d2cSKalle Valo * for interpolation.
22405491d2cSKalle Valo * Inputs:
22505491d2cSKalle Valo * N - number to which log10 has to be found.
22605491d2cSKalle Valo * qN - q format of N
22705491d2cSKalle Valo * log10N - address where log10(N) will be written.
22805491d2cSKalle Valo * qLog10N - address where log10N qformat will be written.
22905491d2cSKalle Valo * Note/Problem:
23005491d2cSKalle Valo * For accurate results input should be in normalized or near normalized form.
23105491d2cSKalle Valo */
qm_log10(s32 N,s16 qN,s16 * log10N,s16 * qLog10N)23205491d2cSKalle Valo void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N)
23305491d2cSKalle Valo {
23405491d2cSKalle Valo s16 s16norm, s16tableIndex, s16errorApproximation;
23505491d2cSKalle Valo u16 u16offset;
23605491d2cSKalle Valo s32 s32log;
23705491d2cSKalle Valo
23805491d2cSKalle Valo /* normalize the N. */
23905491d2cSKalle Valo s16norm = qm_norm32(N);
24005491d2cSKalle Valo N = N << s16norm;
24105491d2cSKalle Valo
24205491d2cSKalle Valo /* The qformat of N after normalization.
24305491d2cSKalle Valo * -30 is added to treat the no as between 1.0 to 2.0
24405491d2cSKalle Valo * i.e. after adding the -30 to the qformat the decimal point will be
24505491d2cSKalle Valo * just rigtht of the MSB. (i.e. after sign bit and 1st MSB). i.e.
24605491d2cSKalle Valo * at the right side of 30th bit.
24705491d2cSKalle Valo */
24805491d2cSKalle Valo qN = qN + s16norm - 30;
24905491d2cSKalle Valo
25005491d2cSKalle Valo /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the
25105491d2cSKalle Valo * MSB */
25205491d2cSKalle Valo s16tableIndex = (s16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE)));
25305491d2cSKalle Valo
25405491d2cSKalle Valo /* remove the MSB. the MSB is always 1 after normalization. */
25505491d2cSKalle Valo s16tableIndex =
25605491d2cSKalle Valo s16tableIndex & (s16) ((1 << LOG2_LOG_TABLE_SIZE) - 1);
25705491d2cSKalle Valo
25805491d2cSKalle Valo /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */
25905491d2cSKalle Valo N = N & ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE))) - 1);
26005491d2cSKalle Valo
26105491d2cSKalle Valo /* take the offset as the 16 MSBS after table index.
26205491d2cSKalle Valo */
26305491d2cSKalle Valo u16offset = (u16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE + 16)));
26405491d2cSKalle Valo
26505491d2cSKalle Valo /* look the log value in the table. */
26605491d2cSKalle Valo s32log = log_table[s16tableIndex]; /* q.15 format */
26705491d2cSKalle Valo
26805491d2cSKalle Valo /* interpolate using the offset. q.15 format. */
26905491d2cSKalle Valo s16errorApproximation = (s16) qm_mulu16(u16offset,
27005491d2cSKalle Valo (u16) (log_table[s16tableIndex + 1] -
27105491d2cSKalle Valo log_table[s16tableIndex]));
27205491d2cSKalle Valo
27305491d2cSKalle Valo /* q.15 format */
27405491d2cSKalle Valo s32log = qm_add16((s16) s32log, s16errorApproximation);
27505491d2cSKalle Valo
27605491d2cSKalle Valo /* adjust for the qformat of the N as
27705491d2cSKalle Valo * log2(mag * 2^x) = log2(mag) + x
27805491d2cSKalle Valo */
27905491d2cSKalle Valo s32log = qm_add32(s32log, ((s32) -qN) << 15); /* q.15 format */
28005491d2cSKalle Valo
28105491d2cSKalle Valo /* normalize the result. */
28205491d2cSKalle Valo s16norm = qm_norm32(s32log);
28305491d2cSKalle Valo
28405491d2cSKalle Valo /* bring all the important bits into lower 16 bits */
28505491d2cSKalle Valo /* q.15+s16norm-16 format */
28605491d2cSKalle Valo s32log = qm_shl32(s32log, s16norm - 16);
28705491d2cSKalle Valo
28805491d2cSKalle Valo /* compute the log10(N) by multiplying log2(N) with log10(2).
28905491d2cSKalle Valo * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2)
29005491d2cSKalle Valo * log10N in q.15+s16norm-16+1 (LOG10_2 is in q.16)
29105491d2cSKalle Valo */
29205491d2cSKalle Valo *log10N = qm_muls16((s16) s32log, (s16) LOG10_2);
29305491d2cSKalle Valo
29405491d2cSKalle Valo /* write the q format of the result. */
29505491d2cSKalle Valo *qLog10N = 15 + s16norm - 16 + 1;
29605491d2cSKalle Valo
29705491d2cSKalle Valo return;
29805491d2cSKalle Valo }
299