Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (c) 2010 Broadcom Corporation |
| 3 | * |
| 4 | * Permission to use, copy, modify, and/or distribute this software for any |
| 5 | * purpose with or without fee is hereby granted, provided that the above |
| 6 | * copyright notice and this permission notice appear in all copies. |
| 7 | * |
| 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES |
| 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF |
| 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY |
| 11 | * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES |
| 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION |
| 13 | * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN |
| 14 | * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. |
| 15 | */ |
| 16 | |
| 17 | #include "qmath.h" |
| 18 | |
| 19 | /* |
| 20 | Description: This function saturate input 32 bit number into a 16 bit number. |
| 21 | If input number is greater than 0x7fff then output is saturated to 0x7fff. |
| 22 | else if input number is less than 0xffff8000 then output is saturated to 0xffff8000 |
| 23 | else output is same as input. |
| 24 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 25 | s16 qm_sat32(s32 op) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 26 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 27 | s16 result; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 28 | if (op > (s32) 0x7fff) { |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 29 | result = 0x7fff; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 30 | } else if (op < (s32) 0xffff8000) { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 31 | result = (s16) (0x8000); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 32 | } else { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 33 | result = (s16) op; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 34 | } |
| 35 | return result; |
| 36 | } |
| 37 | |
| 38 | /* |
| 39 | Description: This function multiply two input 16 bit numbers and return the 32 bit result. |
| 40 | This multiplication is similar to compiler multiplication. This operation is defined if |
| 41 | 16 bit multiplication on the processor platform is cheaper than 32 bit multiplication (as |
| 42 | the most of qmath functions can be replaced with processor intrinsic instructions). |
| 43 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 44 | s32 qm_mul321616(s16 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 45 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 46 | return (s32) (op1) * (s32) (op2); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 47 | } |
| 48 | |
| 49 | /* |
| 50 | Description: This function make 16 bit multiplication and return the result in 16 bits. |
| 51 | To fit the result into 16 bits the 32 bit multiplication result is right |
| 52 | shifted by 16 bits. |
| 53 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 54 | s16 qm_mul16(s16 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 55 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 56 | s32 result; |
| 57 | result = ((s32) (op1) * (s32) (op2)); |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 58 | return (s16) (result >> 16); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 59 | } |
| 60 | |
| 61 | /* |
| 62 | Description: This function multiply two 16 bit numbers and return the result in 32 bits. |
| 63 | This function remove the extra sign bit created by the multiplication by leftshifting the |
| 64 | 32 bit multiplication result by 1 bit before returning the result. So the output is |
| 65 | twice that of compiler multiplication. (i.e. qm_muls321616(2,3)=12). |
| 66 | When both input 16 bit numbers are 0x8000, then the result is saturated to 0x7fffffff. |
| 67 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 68 | s32 qm_muls321616(s16 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 69 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 70 | s32 result; |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 71 | if (op1 == (s16) (0x8000) && op2 == (s16) (0x8000)) { |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 72 | result = 0x7fffffff; |
| 73 | } else { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 74 | result = ((s32) (op1) * (s32) (op2)); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 75 | result = result << 1; |
| 76 | } |
| 77 | return result; |
| 78 | } |
| 79 | |
| 80 | /* |
| 81 | Description: This function make 16 bit unsigned multiplication. To fit the output into |
| 82 | 16 bits the 32 bit multiplication result is right shifted by 16 bits. |
| 83 | */ |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 84 | u16 qm_mulu16(u16 op1, u16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 85 | { |
Greg Kroah-Hartman | 66cbd3a | 2010-10-08 11:05:47 -0700 | [diff] [blame] | 86 | return (u16) (((u32) op1 * (u32) op2) >> 16); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 87 | } |
| 88 | |
| 89 | /* |
| 90 | Description: This function make 16 bit multiplication and return the result in 16 bits. |
| 91 | To fit the multiplication result into 16 bits the multiplication result is right shifted by |
| 92 | 15 bits. Right shifting 15 bits instead of 16 bits is done to remove the extra sign bit formed |
| 93 | due to the multiplication. |
| 94 | When both the 16bit inputs are 0x8000 then the output is saturated to 0x7fffffff. |
| 95 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 96 | s16 qm_muls16(s16 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 97 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 98 | s32 result; |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 99 | if (op1 == (s16) 0x8000 && op2 == (s16) 0x8000) { |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 100 | result = 0x7fffffff; |
| 101 | } else { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 102 | result = ((s32) (op1) * (s32) (op2)); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 103 | } |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 104 | return (s16) (result >> 15); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 105 | } |
| 106 | |
| 107 | /* |
| 108 | Description: This function add two 32 bit numbers and return the 32bit result. |
| 109 | If the result overflow 32 bits, the output will be saturated to 32bits. |
| 110 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 111 | s32 qm_add32(s32 op1, s32 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 112 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 113 | s32 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 114 | result = op1 + op2; |
| 115 | if (op1 < 0 && op2 < 0 && result > 0) { |
| 116 | result = 0x80000000; |
| 117 | } else if (op1 > 0 && op2 > 0 && result < 0) { |
| 118 | result = 0x7fffffff; |
| 119 | } |
| 120 | return result; |
| 121 | } |
| 122 | |
| 123 | /* |
| 124 | Description: This function add two 16 bit numbers and return the 16bit result. |
| 125 | If the result overflow 16 bits, the output will be saturated to 16bits. |
| 126 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 127 | s16 qm_add16(s16 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 128 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 129 | s16 result; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 130 | s32 temp = (s32) op1 + (s32) op2; |
| 131 | if (temp > (s32) 0x7fff) { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 132 | result = (s16) 0x7fff; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 133 | } else if (temp < (s32) 0xffff8000) { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 134 | result = (s16) 0xffff8000; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 135 | } else { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 136 | result = (s16) temp; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 137 | } |
| 138 | return result; |
| 139 | } |
| 140 | |
| 141 | /* |
| 142 | Description: This function make 16 bit subtraction and return the 16bit result. |
| 143 | If the result overflow 16 bits, the output will be saturated to 16bits. |
| 144 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 145 | s16 qm_sub16(s16 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 146 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 147 | s16 result; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 148 | s32 temp = (s32) op1 - (s32) op2; |
| 149 | if (temp > (s32) 0x7fff) { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 150 | result = (s16) 0x7fff; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 151 | } else if (temp < (s32) 0xffff8000) { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 152 | result = (s16) 0xffff8000; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 153 | } else { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 154 | result = (s16) temp; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 155 | } |
| 156 | return result; |
| 157 | } |
| 158 | |
| 159 | /* |
| 160 | Description: This function make 32 bit subtraction and return the 32bit result. |
| 161 | If the result overflow 32 bits, the output will be saturated to 32bits. |
| 162 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 163 | s32 qm_sub32(s32 op1, s32 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 164 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 165 | s32 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 166 | result = op1 - op2; |
| 167 | if (op1 >= 0 && op2 < 0 && result < 0) { |
| 168 | result = 0x7fffffff; |
| 169 | } else if (op1 < 0 && op2 > 0 && result > 0) { |
| 170 | result = 0x80000000; |
| 171 | } |
| 172 | return result; |
| 173 | } |
| 174 | |
| 175 | /* |
| 176 | Description: This function multiply input 16 bit numbers and accumulate the result |
| 177 | into the input 32 bit number and return the 32 bit accumulated result. |
| 178 | If the accumulation result in overflow, then the output will be saturated. |
| 179 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 180 | s32 qm_mac321616(s32 acc, s16 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 181 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 182 | s32 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 183 | result = qm_add32(acc, qm_mul321616(op1, op2)); |
| 184 | return result; |
| 185 | } |
| 186 | |
| 187 | /* |
| 188 | Description: This function make a 32 bit saturated left shift when the specified shift |
| 189 | is +ve. This function will make a 32 bit right shift when the specified shift is -ve. |
| 190 | This function return the result after shifting operation. |
| 191 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 192 | s32 qm_shl32(s32 op, int shift) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 193 | { |
| 194 | int i; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 195 | s32 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 196 | result = op; |
| 197 | if (shift > 31) |
| 198 | shift = 31; |
| 199 | else if (shift < -31) |
| 200 | shift = -31; |
| 201 | if (shift >= 0) { |
| 202 | for (i = 0; i < shift; i++) { |
| 203 | result = qm_add32(result, result); |
| 204 | } |
| 205 | } else { |
| 206 | result = result >> (-shift); |
| 207 | } |
| 208 | return result; |
| 209 | } |
| 210 | |
| 211 | /* |
| 212 | Description: This function make a 32 bit right shift when shift is +ve. |
| 213 | This function make a 32 bit saturated left shift when shift is -ve. This function |
| 214 | return the result of the shift operation. |
| 215 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 216 | s32 qm_shr32(s32 op, int shift) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 217 | { |
| 218 | return qm_shl32(op, -shift); |
| 219 | } |
| 220 | |
| 221 | /* |
| 222 | Description: This function make a 16 bit saturated left shift when the specified shift |
| 223 | is +ve. This function will make a 16 bit right shift when the specified shift is -ve. |
| 224 | This function return the result after shifting operation. |
| 225 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 226 | s16 qm_shl16(s16 op, int shift) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 227 | { |
| 228 | int i; |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 229 | s16 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 230 | result = op; |
| 231 | if (shift > 15) |
| 232 | shift = 15; |
| 233 | else if (shift < -15) |
| 234 | shift = -15; |
| 235 | if (shift > 0) { |
| 236 | for (i = 0; i < shift; i++) { |
| 237 | result = qm_add16(result, result); |
| 238 | } |
| 239 | } else { |
| 240 | result = result >> (-shift); |
| 241 | } |
| 242 | return result; |
| 243 | } |
| 244 | |
| 245 | /* |
| 246 | Description: This function make a 16 bit right shift when shift is +ve. |
| 247 | This function make a 16 bit saturated left shift when shift is -ve. This function |
| 248 | return the result of the shift operation. |
| 249 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 250 | s16 qm_shr16(s16 op, int shift) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 251 | { |
| 252 | return qm_shl16(op, -shift); |
| 253 | } |
| 254 | |
| 255 | /* |
| 256 | Description: This function return the number of redundant sign bits in a 16 bit number. |
| 257 | Example: qm_norm16(0x0080) = 7. |
| 258 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 259 | s16 qm_norm16(s16 op) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 260 | { |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 261 | u16 u16extraSignBits; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 262 | if (op == 0) { |
| 263 | return 15; |
| 264 | } else { |
| 265 | u16extraSignBits = 0; |
| 266 | while ((op >> 15) == (op >> 14)) { |
| 267 | u16extraSignBits++; |
| 268 | op = op << 1; |
| 269 | } |
| 270 | } |
| 271 | return u16extraSignBits; |
| 272 | } |
| 273 | |
| 274 | /* |
| 275 | Description: This function return the number of redundant sign bits in a 32 bit number. |
| 276 | Example: qm_norm32(0x00000080) = 23 |
| 277 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 278 | s16 qm_norm32(s32 op) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 279 | { |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 280 | u16 u16extraSignBits; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 281 | if (op == 0) { |
| 282 | return 31; |
| 283 | } else { |
| 284 | u16extraSignBits = 0; |
| 285 | while ((op >> 31) == (op >> 30)) { |
| 286 | u16extraSignBits++; |
| 287 | op = op << 1; |
| 288 | } |
| 289 | } |
| 290 | return u16extraSignBits; |
| 291 | } |
| 292 | |
| 293 | /* |
| 294 | Description: This function divide two 16 bit unsigned numbers. |
| 295 | The numerator should be less than denominator. So the quotient is always less than 1. |
| 296 | This function return the quotient in q.15 format. |
| 297 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 298 | s16 qm_div_s(s16 num, s16 denom) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 299 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 300 | s16 var_out; |
| 301 | s16 iteration; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 302 | s32 L_num; |
| 303 | s32 L_denom; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 304 | L_num = (num) << 15; |
| 305 | L_denom = (denom) << 15; |
| 306 | for (iteration = 0; iteration < 15; iteration++) { |
| 307 | L_num <<= 1; |
| 308 | if (L_num >= L_denom) { |
| 309 | L_num = qm_sub32(L_num, L_denom); |
| 310 | L_num = qm_add32(L_num, 1); |
| 311 | } |
| 312 | } |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 313 | var_out = (s16) (L_num & 0x7fff); |
Jason Cooper | 90ea229 | 2010-09-14 09:45:32 -0400 | [diff] [blame] | 314 | return var_out; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 315 | } |
| 316 | |
| 317 | /* |
| 318 | Description: This function compute the absolute value of a 16 bit number. |
| 319 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 320 | s16 qm_abs16(s16 op) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 321 | { |
| 322 | if (op < 0) { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 323 | if (op == (s16) 0xffff8000) { |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 324 | return 0x7fff; |
| 325 | } else { |
| 326 | return -op; |
| 327 | } |
| 328 | } else { |
| 329 | return op; |
| 330 | } |
| 331 | } |
| 332 | |
| 333 | /* |
| 334 | Description: This function divide two 16 bit numbers. |
| 335 | The quotient is returned through return value. |
| 336 | The qformat of the quotient is returned through the pointer (qQuotient) passed |
| 337 | to this function. The qformat of quotient is adjusted appropriately such that |
| 338 | the quotient occupies all 16 bits. |
| 339 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 340 | s16 qm_div16(s16 num, s16 denom, s16 *qQuotient) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 341 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 342 | s16 sign; |
| 343 | s16 nNum, nDenom; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 344 | sign = num ^ denom; |
| 345 | num = qm_abs16(num); |
| 346 | denom = qm_abs16(denom); |
| 347 | nNum = qm_norm16(num); |
| 348 | nDenom = qm_norm16(denom); |
| 349 | num = qm_shl16(num, nNum - 1); |
| 350 | denom = qm_shl16(denom, nDenom); |
| 351 | *qQuotient = nNum - 1 - nDenom + 15; |
| 352 | if (sign >= 0) { |
| 353 | return qm_div_s(num, denom); |
| 354 | } else { |
| 355 | return -qm_div_s(num, denom); |
| 356 | } |
| 357 | } |
| 358 | |
| 359 | /* |
| 360 | Description: This function compute absolute value of a 32 bit number. |
| 361 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 362 | s32 qm_abs32(s32 op) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 363 | { |
| 364 | if (op < 0) { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 365 | if (op == (s32) 0x80000000) { |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 366 | return 0x7fffffff; |
| 367 | } else { |
| 368 | return -op; |
| 369 | } |
| 370 | } else { |
| 371 | return op; |
| 372 | } |
| 373 | } |
| 374 | |
| 375 | /* |
| 376 | Description: This function divide two 32 bit numbers. The division is performed |
| 377 | by considering only important 16 bits in 32 bit numbers. |
| 378 | The quotient is returned through return value. |
| 379 | The qformat of the quotient is returned through the pointer (qquotient) passed |
| 380 | to this function. The qformat of quotient is adjusted appropriately such that |
| 381 | the quotient occupies all 16 bits. |
| 382 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 383 | s16 qm_div163232(s32 num, s32 denom, s16 *qquotient) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 384 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 385 | s32 sign; |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 386 | s16 nNum, nDenom; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 387 | sign = num ^ denom; |
| 388 | num = qm_abs32(num); |
| 389 | denom = qm_abs32(denom); |
| 390 | nNum = qm_norm32(num); |
| 391 | nDenom = qm_norm32(denom); |
| 392 | num = qm_shl32(num, nNum - 1); |
| 393 | denom = qm_shl32(denom, nDenom); |
| 394 | *qquotient = nNum - 1 - nDenom + 15; |
| 395 | if (sign >= 0) { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 396 | return qm_div_s((s16) (num >> 16), (s16) (denom >> 16)); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 397 | } else { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 398 | return -qm_div_s((s16) (num >> 16), (s16) (denom >> 16)); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 399 | } |
| 400 | } |
| 401 | |
| 402 | /* |
| 403 | Description: This function multiply a 32 bit number with a 16 bit number. |
| 404 | The multiplicaton result is right shifted by 16 bits to fit the result |
| 405 | into 32 bit output. |
| 406 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 407 | s32 qm_mul323216(s32 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 408 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 409 | s16 hi; |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 410 | u16 lo; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 411 | s32 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 412 | hi = op1 >> 16; |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 413 | lo = (s16) (op1 & 0xffff); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 414 | result = qm_mul321616(hi, op2); |
| 415 | result = result + (qm_mulsu321616(op2, lo) >> 16); |
| 416 | return result; |
| 417 | } |
| 418 | |
| 419 | /* |
| 420 | Description: This function multiply signed 16 bit number with unsigned 16 bit number and return |
| 421 | the result in 32 bits. |
| 422 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 423 | s32 qm_mulsu321616(s16 op1, u16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 424 | { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 425 | return (s32) (op1) * op2; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 426 | } |
| 427 | |
| 428 | /* |
| 429 | Description: This function multiply 32 bit number with 16 bit number. The multiplication result is |
| 430 | right shifted by 15 bits to fit the result into 32 bits. Right shifting by only 15 bits instead of |
| 431 | 16 bits is done to remove the extra sign bit formed by multiplication from the return value. |
| 432 | When the input numbers are 0x80000000, 0x8000 the return value is saturated to 0x7fffffff. |
| 433 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 434 | s32 qm_muls323216(s32 op1, s16 op2) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 435 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 436 | s16 hi; |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 437 | u16 lo; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 438 | s32 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 439 | hi = op1 >> 16; |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 440 | lo = (s16) (op1 & 0xffff); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 441 | result = qm_muls321616(hi, op2); |
| 442 | result = qm_add32(result, (qm_mulsu321616(op2, lo) >> 15)); |
| 443 | return result; |
| 444 | } |
| 445 | |
| 446 | /* |
| 447 | Description: This function multiply two 32 bit numbers. The multiplication result is right |
| 448 | shifted by 32 bits to fit the multiplication result into 32 bits. The right shifted |
| 449 | multiplication result is returned as output. |
| 450 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 451 | s32 qm_mul32(s32 a, s32 b) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 452 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 453 | s16 hi1, hi2; |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 454 | u16 lo1, lo2; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 455 | s32 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 456 | hi1 = a >> 16; |
| 457 | hi2 = b >> 16; |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 458 | lo1 = (u16) (a & 0xffff); |
| 459 | lo2 = (u16) (b & 0xffff); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 460 | result = qm_mul321616(hi1, hi2); |
| 461 | result = result + (qm_mulsu321616(hi1, lo2) >> 16); |
| 462 | result = result + (qm_mulsu321616(hi2, lo1) >> 16); |
| 463 | return result; |
| 464 | } |
| 465 | |
| 466 | /* |
| 467 | Description: This function multiply two 32 bit numbers. The multiplication result is |
| 468 | right shifted by 31 bits to fit the multiplication result into 32 bits. The right |
| 469 | shifted multiplication result is returned as output. Right shifting by only 31 bits |
| 470 | instead of 32 bits is done to remove the extra sign bit formed by multiplication. |
| 471 | When the input numbers are 0x80000000, 0x80000000 the return value is saturated to |
| 472 | 0x7fffffff. |
| 473 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 474 | s32 qm_muls32(s32 a, s32 b) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 475 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 476 | s16 hi1, hi2; |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 477 | u16 lo1, lo2; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 478 | s32 result; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 479 | hi1 = a >> 16; |
| 480 | hi2 = b >> 16; |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 481 | lo1 = (u16) (a & 0xffff); |
| 482 | lo2 = (u16) (b & 0xffff); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 483 | result = qm_muls321616(hi1, hi2); |
| 484 | result = qm_add32(result, (qm_mulsu321616(hi1, lo2) >> 15)); |
| 485 | result = qm_add32(result, (qm_mulsu321616(hi2, lo1) >> 15)); |
| 486 | result = qm_add32(result, (qm_mulu16(lo1, lo2) >> 15)); |
| 487 | return result; |
| 488 | } |
| 489 | |
| 490 | /* This table is log2(1+(i/32)) where i=[0:1:31], in q.15 format */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 491 | static const s16 log_table[] = { |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 492 | 0, |
| 493 | 1455, |
| 494 | 2866, |
| 495 | 4236, |
| 496 | 5568, |
| 497 | 6863, |
| 498 | 8124, |
| 499 | 9352, |
| 500 | 10549, |
| 501 | 11716, |
| 502 | 12855, |
| 503 | 13968, |
| 504 | 15055, |
| 505 | 16117, |
| 506 | 17156, |
| 507 | 18173, |
| 508 | 19168, |
| 509 | 20143, |
| 510 | 21098, |
| 511 | 22034, |
| 512 | 22952, |
| 513 | 23852, |
| 514 | 24736, |
| 515 | 25604, |
| 516 | 26455, |
| 517 | 27292, |
| 518 | 28114, |
| 519 | 28922, |
| 520 | 29717, |
| 521 | 30498, |
| 522 | 31267, |
| 523 | 32024 |
| 524 | }; |
| 525 | |
| 526 | #define LOG_TABLE_SIZE 32 /* log_table size */ |
| 527 | #define LOG2_LOG_TABLE_SIZE 5 /* log2(log_table size) */ |
| 528 | #define Q_LOG_TABLE 15 /* qformat of log_table */ |
| 529 | #define LOG10_2 19728 /* log10(2) in q.16 */ |
| 530 | |
| 531 | /* |
| 532 | Description: |
| 533 | This routine takes the input number N and its q format qN and compute |
| 534 | the log10(N). This routine first normalizes the input no N. Then N is in mag*(2^x) format. |
| 535 | mag is any number in the range 2^30-(2^31 - 1). Then log2(mag * 2^x) = log2(mag) + x is computed. |
| 536 | From that log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed. |
| 537 | This routine looks the log2 value in the table considering LOG2_LOG_TABLE_SIZE+1 MSBs. |
| 538 | As the MSB is always 1, only next LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup. |
| 539 | Next 16 MSBs are used for interpolation. |
| 540 | Inputs: |
| 541 | N - number to which log10 has to be found. |
| 542 | qN - q format of N |
| 543 | log10N - address where log10(N) will be written. |
| 544 | qLog10N - address where log10N qformat will be written. |
| 545 | Note/Problem: |
| 546 | For accurate results input should be in normalized or near normalized form. |
| 547 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 548 | void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 549 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 550 | s16 s16norm, s16tableIndex, s16errorApproximation; |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 551 | u16 u16offset; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 552 | s32 s32log; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 553 | |
| 554 | /* Logerithm of negative values is undefined. |
| 555 | * assert N is greater than 0. |
| 556 | */ |
| 557 | /* ASSERT(N > 0); */ |
| 558 | |
| 559 | /* normalize the N. */ |
| 560 | s16norm = qm_norm32(N); |
| 561 | N = N << s16norm; |
| 562 | |
| 563 | /* The qformat of N after normalization. |
| 564 | * -30 is added to treat the no as between 1.0 to 2.0 |
| 565 | * i.e. after adding the -30 to the qformat the decimal point will be |
| 566 | * just rigtht of the MSB. (i.e. after sign bit and 1st MSB). i.e. |
| 567 | * at the right side of 30th bit. |
| 568 | */ |
| 569 | qN = qN + s16norm - 30; |
| 570 | |
| 571 | /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the MSB */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 572 | s16tableIndex = (s16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE))); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 573 | |
| 574 | /* remove the MSB. the MSB is always 1 after normalization. */ |
| 575 | s16tableIndex = |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 576 | s16tableIndex & (s16) ((1 << LOG2_LOG_TABLE_SIZE) - 1); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 577 | |
| 578 | /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */ |
| 579 | N = N & ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE))) - 1); |
| 580 | |
| 581 | /* take the offset as the 16 MSBS after table index. |
| 582 | */ |
Greg Kroah-Hartman | 7d4df48 | 2010-10-07 17:04:47 -0700 | [diff] [blame] | 583 | u16offset = (u16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE + 16))); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 584 | |
| 585 | /* look the log value in the table. */ |
| 586 | s32log = log_table[s16tableIndex]; /* q.15 format */ |
| 587 | |
| 588 | /* interpolate using the offset. */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 589 | s16errorApproximation = (s16) qm_mulu16(u16offset, (u16) (log_table[s16tableIndex + 1] - log_table[s16tableIndex])); /* q.15 */ |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 590 | |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 591 | s32log = qm_add16((s16) s32log, s16errorApproximation); /* q.15 format */ |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 592 | |
| 593 | /* adjust for the qformat of the N as |
| 594 | * log2(mag * 2^x) = log2(mag) + x |
| 595 | */ |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 596 | s32log = qm_add32(s32log, ((s32) -qN) << 15); /* q.15 format */ |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 597 | |
| 598 | /* normalize the result. */ |
| 599 | s16norm = qm_norm32(s32log); |
| 600 | |
| 601 | /* bring all the important bits into lower 16 bits */ |
| 602 | s32log = qm_shl32(s32log, s16norm - 16); /* q.15+s16norm-16 format */ |
| 603 | |
| 604 | /* compute the log10(N) by multiplying log2(N) with log10(2). |
| 605 | * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2) |
| 606 | * log10N in q.15+s16norm-16+1 (LOG10_2 is in q.16) |
| 607 | */ |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 608 | *log10N = qm_muls16((s16) s32log, (s16) LOG10_2); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 609 | |
| 610 | /* write the q format of the result. */ |
| 611 | *qLog10N = 15 + s16norm - 16 + 1; |
| 612 | |
| 613 | return; |
| 614 | } |
| 615 | |
| 616 | /* |
| 617 | Description: |
| 618 | This routine compute 1/N. |
| 619 | This routine reformates the given no N as N * 2^qN where N is in between 0.5 and 1.0 |
| 620 | in q.15 format in 16 bits. So the problem now boils down to finding the inverse of a |
| 621 | q.15 no in 16 bits which is in the range of 0.5 to 1.0. The output is always between |
| 622 | 2.0 to 1. So the output is 2.0 to 1.0 in q.30 format. Once the final output format is found |
| 623 | by taking the qN into account. Inverse is found with newton rapson method. Initially |
| 624 | inverse (x) is guessed as 1/0.75 (with appropriate sign). The new guess is calculated |
| 625 | using the formula x' = 2*x - N*x*x. After 4 or 5 iterations the inverse is very close to |
| 626 | inverse of N. |
| 627 | Inputs: |
| 628 | N - number to which 1/N has to be found. |
| 629 | qn - q format of N. |
| 630 | sqrtN - address where 1/N has to be written. |
| 631 | qsqrtN - address where q format of 1/N has to be written. |
| 632 | */ |
| 633 | #define qx 29 |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 634 | void qm_1byN(s32 N, s16 qN, s32 *result, s16 *qResult) |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 635 | { |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 636 | s16 normN; |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 637 | s32 s32firstTerm, s32secondTerm, x; |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 638 | int i; |
| 639 | |
| 640 | normN = qm_norm32(N); |
| 641 | |
| 642 | /* limit N to least significant 16 bits. 15th bit is the sign bit. */ |
| 643 | N = qm_shl32(N, normN - 16); |
| 644 | qN = qN + normN - 16 - 15; |
| 645 | /* -15 is added to treat N as 16 bit q.15 number in the range from 0.5 to 1 */ |
| 646 | |
| 647 | /* Take the initial guess as 1/0.75 in qx format with appropriate sign. */ |
| 648 | if (N >= 0) { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 649 | x = (s32) ((1 / 0.75) * (1 << qx)); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 650 | /* input no is in the range 0.5 to 1. So 1/0.75 is taken as initial guess. */ |
| 651 | } else { |
Greg Kroah-Hartman | 3e26416 | 2010-10-08 11:11:13 -0700 | [diff] [blame] | 652 | x = (s32) ((1 / -0.75) * (1 << qx)); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 653 | /* input no is in the range -0.5 to -1. So 1/-0.75 is taken as initial guess. */ |
| 654 | } |
| 655 | |
| 656 | /* iterate the equation x = 2*x - N*x*x for 4 times. */ |
| 657 | for (i = 0; i < 4; i++) { |
| 658 | s32firstTerm = qm_shl32(x, 1); /* s32firstTerm = 2*x in q.29 */ |
| 659 | s32secondTerm = |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 660 | qm_muls321616((s16) (s32firstTerm >> 16), |
| 661 | (s16) (s32firstTerm >> 16)); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 662 | /* s32secondTerm = x*x in q.(29+1-16)*2+1 */ |
| 663 | s32secondTerm = |
Greg Kroah-Hartman | e59fe08 | 2010-10-07 17:08:21 -0700 | [diff] [blame] | 664 | qm_muls321616((s16) (s32secondTerm >> 16), (s16) N); |
Henry Ptasinski | a9533e7 | 2010-09-08 21:04:42 -0700 | [diff] [blame] | 665 | /* s32secondTerm = N*x*x in q.((29+1-16)*2+1)-16+15+1 i.e. in q.29 */ |
| 666 | x = qm_sub32(s32firstTerm, s32secondTerm); |
| 667 | /* can be added directly as both are in q.29 */ |
| 668 | } |
| 669 | |
| 670 | /* Bring the x to q.30 format. */ |
| 671 | *result = qm_shl32(x, 1); |
| 672 | /* giving the output in q.30 format for q.15 input in 16 bits. */ |
| 673 | |
| 674 | /* compute the final q format of the result. */ |
| 675 | *qResult = -qN + 30; /* adjusting the q format of actual output */ |
| 676 | |
| 677 | return; |
| 678 | } |
| 679 | |
| 680 | #undef qx |