1 /****************************************************************************** 2 * 3 * Copyright 2022 Google LLC 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at: 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 * 17 ******************************************************************************/ 18 19 #include "sns.h" 20 #include "tables.h" 21 22 23 /* ---------------------------------------------------------------------------- 24 * DCT-16 25 * -------------------------------------------------------------------------- */ 26 27 /** 28 * Matrix of DCT-16 coefficients 29 * 30 * M[n][k] = 2f cos( Pi k (2n + 1) / 2N ) 31 * 32 * k = [0..N-1], n = [0..N-1], N = 16 33 * f = sqrt(1/4N) for k=0, sqrt(1/2N) otherwise 34 */ 35 static const float dct16_m[16][16] = { 36 37 { 2.50000000e-01, 3.51850934e-01, 3.46759961e-01, 3.38329500e-01, 38 3.26640741e-01, 3.11806253e-01, 2.93968901e-01, 2.73300467e-01, 39 2.50000000e-01, 2.24291897e-01, 1.96423740e-01, 1.66663915e-01, 40 1.35299025e-01, 1.02631132e-01, 6.89748448e-02, 3.46542923e-02 }, 41 42 { 2.50000000e-01, 3.38329500e-01, 2.93968901e-01, 2.24291897e-01, 43 1.35299025e-01, 3.46542923e-02, -6.89748448e-02, -1.66663915e-01, 44 -2.50000000e-01, -3.11806253e-01, -3.46759961e-01, -3.51850934e-01, 45 -3.26640741e-01, -2.73300467e-01, -1.96423740e-01, -1.02631132e-01 }, 46 47 { 2.50000000e-01, 3.11806253e-01, 1.96423740e-01, 3.46542923e-02, 48 -1.35299025e-01, -2.73300467e-01, -3.46759961e-01, -3.38329500e-01, 49 -2.50000000e-01, -1.02631132e-01, 6.89748448e-02, 2.24291897e-01, 50 3.26640741e-01, 3.51850934e-01, 2.93968901e-01, 1.66663915e-01 }, 51 52 { 2.50000000e-01, 2.73300467e-01, 6.89748448e-02, -1.66663915e-01, 53 -3.26640741e-01, -3.38329500e-01, -1.96423740e-01, 3.46542923e-02, 54 2.50000000e-01, 3.51850934e-01, 2.93968901e-01, 1.02631132e-01, 55 -1.35299025e-01, -3.11806253e-01, -3.46759961e-01, -2.24291897e-01 }, 56 57 { 2.50000000e-01, 2.24291897e-01, -6.89748448e-02, -3.11806253e-01, 58 -3.26640741e-01, -1.02631132e-01, 1.96423740e-01, 3.51850934e-01, 59 2.50000000e-01, -3.46542923e-02, -2.93968901e-01, -3.38329500e-01, 60 -1.35299025e-01, 1.66663915e-01, 3.46759961e-01, 2.73300467e-01 }, 61 62 { 2.50000000e-01, 1.66663915e-01, -1.96423740e-01, -3.51850934e-01, 63 -1.35299025e-01, 2.24291897e-01, 3.46759961e-01, 1.02631132e-01, 64 -2.50000000e-01, -3.38329500e-01, -6.89748448e-02, 2.73300467e-01, 65 3.26640741e-01, 3.46542923e-02, -2.93968901e-01, -3.11806253e-01 }, 66 67 { 2.50000000e-01, 1.02631132e-01, -2.93968901e-01, -2.73300467e-01, 68 1.35299025e-01, 3.51850934e-01, 6.89748448e-02, -3.11806253e-01, 69 -2.50000000e-01, 1.66663915e-01, 3.46759961e-01, 3.46542923e-02, 70 -3.26640741e-01, -2.24291897e-01, 1.96423740e-01, 3.38329500e-01 }, 71 72 { 2.50000000e-01, 3.46542923e-02, -3.46759961e-01, -1.02631132e-01, 73 3.26640741e-01, 1.66663915e-01, -2.93968901e-01, -2.24291897e-01, 74 2.50000000e-01, 2.73300467e-01, -1.96423740e-01, -3.11806253e-01, 75 1.35299025e-01, 3.38329500e-01, -6.89748448e-02, -3.51850934e-01 }, 76 77 { 2.50000000e-01, -3.46542923e-02, -3.46759961e-01, 1.02631132e-01, 78 3.26640741e-01, -1.66663915e-01, -2.93968901e-01, 2.24291897e-01, 79 2.50000000e-01, -2.73300467e-01, -1.96423740e-01, 3.11806253e-01, 80 1.35299025e-01, -3.38329500e-01, -6.89748448e-02, 3.51850934e-01 }, 81 82 { 2.50000000e-01, -1.02631132e-01, -2.93968901e-01, 2.73300467e-01, 83 1.35299025e-01, -3.51850934e-01, 6.89748448e-02, 3.11806253e-01, 84 -2.50000000e-01, -1.66663915e-01, 3.46759961e-01, -3.46542923e-02, 85 -3.26640741e-01, 2.24291897e-01, 1.96423740e-01, -3.38329500e-01 }, 86 87 { 2.50000000e-01, -1.66663915e-01, -1.96423740e-01, 3.51850934e-01, 88 -1.35299025e-01, -2.24291897e-01, 3.46759961e-01, -1.02631132e-01, 89 -2.50000000e-01, 3.38329500e-01, -6.89748448e-02, -2.73300467e-01, 90 3.26640741e-01, -3.46542923e-02, -2.93968901e-01, 3.11806253e-01 }, 91 92 { 2.50000000e-01, -2.24291897e-01, -6.89748448e-02, 3.11806253e-01, 93 -3.26640741e-01, 1.02631132e-01, 1.96423740e-01, -3.51850934e-01, 94 2.50000000e-01, 3.46542923e-02, -2.93968901e-01, 3.38329500e-01, 95 -1.35299025e-01, -1.66663915e-01, 3.46759961e-01, -2.73300467e-01 }, 96 97 { 2.50000000e-01, -2.73300467e-01, 6.89748448e-02, 1.66663915e-01, 98 -3.26640741e-01, 3.38329500e-01, -1.96423740e-01, -3.46542923e-02, 99 2.50000000e-01, -3.51850934e-01, 2.93968901e-01, -1.02631132e-01, 100 -1.35299025e-01, 3.11806253e-01, -3.46759961e-01, 2.24291897e-01 }, 101 102 { 2.50000000e-01, -3.11806253e-01, 1.96423740e-01, -3.46542923e-02, 103 -1.35299025e-01, 2.73300467e-01, -3.46759961e-01, 3.38329500e-01, 104 -2.50000000e-01, 1.02631132e-01, 6.89748448e-02, -2.24291897e-01, 105 3.26640741e-01, -3.51850934e-01, 2.93968901e-01, -1.66663915e-01 }, 106 107 { 2.50000000e-01, -3.38329500e-01, 2.93968901e-01, -2.24291897e-01, 108 1.35299025e-01, -3.46542923e-02, -6.89748448e-02, 1.66663915e-01, 109 -2.50000000e-01, 3.11806253e-01, -3.46759961e-01, 3.51850934e-01, 110 -3.26640741e-01, 2.73300467e-01, -1.96423740e-01, 1.02631132e-01 }, 111 112 { 2.50000000e-01, -3.51850934e-01, 3.46759961e-01, -3.38329500e-01, 113 3.26640741e-01, -3.11806253e-01, 2.93968901e-01, -2.73300467e-01, 114 2.50000000e-01, -2.24291897e-01, 1.96423740e-01, -1.66663915e-01, 115 1.35299025e-01, -1.02631132e-01, 6.89748448e-02, -3.46542923e-02 }, 116 117 }; 118 119 /** 120 * Forward DCT-16 transformation 121 * x, y Input and output 16 values 122 */ 123 LC3_HOT static void dct16_forward(const float *x, float *y) 124 { 125 for (int i = 0, j; i < 16; i++) 126 for (y[i] = 0, j = 0; j < 16; j++) 127 y[i] += x[j] * dct16_m[j][i]; 128 } 129 130 /** 131 * Inverse DCT-16 transformation 132 * x, y Input and output 16 values 133 */ 134 LC3_HOT static void dct16_inverse(const float *x, float *y) 135 { 136 for (int i = 0, j; i < 16; i++) 137 for (y[i] = 0, j = 0; j < 16; j++) 138 y[i] += x[j] * dct16_m[i][j]; 139 } 140 141 142 /* ---------------------------------------------------------------------------- 143 * Scale factors 144 * -------------------------------------------------------------------------- */ 145 146 /** 147 * Scale factors 148 * dt, sr Duration and samplerate of the frame 149 * nbytes Size in bytes of the frame 150 * eb Energy estimation per bands 151 * att 1: Attack detected 0: Otherwise 152 * scf Output 16 scale factors 153 */ 154 LC3_HOT static void compute_scale_factors( 155 enum lc3_dt dt, enum lc3_srate sr, int nbytes, 156 const float *eb, bool att, float *scf) 157 { 158 /* Pre-emphasis gain table : 159 * Ge[b] = 10 ^ (b * g_tilt) / 630 , b = [0..63] */ 160 161 static const float ge_14[LC3_MAX_BANDS] = { /* g_tilt = 14 */ 162 1.00000000e+00, 1.05250029e+00, 1.10775685e+00, 1.16591440e+00, 163 1.22712524e+00, 1.29154967e+00, 1.35935639e+00, 1.43072299e+00, 164 1.50583635e+00, 1.58489319e+00, 1.66810054e+00, 1.75567629e+00, 165 1.84784980e+00, 1.94486244e+00, 2.04696827e+00, 2.15443469e+00, 166 2.26754313e+00, 2.38658979e+00, 2.51188643e+00, 2.64376119e+00, 167 2.78255940e+00, 2.92864456e+00, 3.08239924e+00, 3.24422608e+00, 168 3.41454887e+00, 3.59381366e+00, 3.78248991e+00, 3.98107171e+00, 169 4.19007911e+00, 4.41005945e+00, 4.64158883e+00, 4.88527357e+00, 170 5.14175183e+00, 5.41169527e+00, 5.69581081e+00, 5.99484250e+00, 171 6.30957344e+00, 6.64082785e+00, 6.98947321e+00, 7.35642254e+00, 172 7.74263683e+00, 8.14912747e+00, 8.57695899e+00, 9.02725178e+00, 173 9.50118507e+00, 1.00000000e+01, 1.05250029e+01, 1.10775685e+01, 174 1.16591440e+01, 1.22712524e+01, 1.29154967e+01, 1.35935639e+01, 175 1.43072299e+01, 1.50583635e+01, 1.58489319e+01, 1.66810054e+01, 176 1.75567629e+01, 1.84784980e+01, 1.94486244e+01, 2.04696827e+01, 177 2.15443469e+01, 2.26754313e+01, 2.38658979e+01, 2.51188643e+01 }; 178 179 static const float ge_18[LC3_MAX_BANDS] = { /* g_tilt = 18 */ 180 1.00000000e+00, 1.06800043e+00, 1.14062492e+00, 1.21818791e+00, 181 1.30102522e+00, 1.38949549e+00, 1.48398179e+00, 1.58489319e+00, 182 1.69266662e+00, 1.80776868e+00, 1.93069773e+00, 2.06198601e+00, 183 2.20220195e+00, 2.35195264e+00, 2.51188643e+00, 2.68269580e+00, 184 2.86512027e+00, 3.05994969e+00, 3.26802759e+00, 3.49025488e+00, 185 3.72759372e+00, 3.98107171e+00, 4.25178630e+00, 4.54090961e+00, 186 4.84969343e+00, 5.17947468e+00, 5.53168120e+00, 5.90783791e+00, 187 6.30957344e+00, 6.73862717e+00, 7.19685673e+00, 7.68624610e+00, 188 8.20891416e+00, 8.76712387e+00, 9.36329209e+00, 1.00000000e+01, 189 1.06800043e+01, 1.14062492e+01, 1.21818791e+01, 1.30102522e+01, 190 1.38949549e+01, 1.48398179e+01, 1.58489319e+01, 1.69266662e+01, 191 1.80776868e+01, 1.93069773e+01, 2.06198601e+01, 2.20220195e+01, 192 2.35195264e+01, 2.51188643e+01, 2.68269580e+01, 2.86512027e+01, 193 3.05994969e+01, 3.26802759e+01, 3.49025488e+01, 3.72759372e+01, 194 3.98107171e+01, 4.25178630e+01, 4.54090961e+01, 4.84969343e+01, 195 5.17947468e+01, 5.53168120e+01, 5.90783791e+01, 6.30957344e+01 }; 196 197 static const float ge_22[LC3_MAX_BANDS] = { /* g_tilt = 22 */ 198 1.00000000e+00, 1.08372885e+00, 1.17446822e+00, 1.27280509e+00, 199 1.37937560e+00, 1.49486913e+00, 1.62003281e+00, 1.75567629e+00, 200 1.90267705e+00, 2.06198601e+00, 2.23463373e+00, 2.42173704e+00, 201 2.62450630e+00, 2.84425319e+00, 3.08239924e+00, 3.34048498e+00, 202 3.62017995e+00, 3.92329345e+00, 4.25178630e+00, 4.60778348e+00, 203 4.99358789e+00, 5.41169527e+00, 5.86481029e+00, 6.35586411e+00, 204 6.88803330e+00, 7.46476041e+00, 8.08977621e+00, 8.76712387e+00, 205 9.50118507e+00, 1.02967084e+01, 1.11588399e+01, 1.20931568e+01, 206 1.31057029e+01, 1.42030283e+01, 1.53922315e+01, 1.66810054e+01, 207 1.80776868e+01, 1.95913107e+01, 2.12316686e+01, 2.30093718e+01, 208 2.49359200e+01, 2.70237760e+01, 2.92864456e+01, 3.17385661e+01, 209 3.43959997e+01, 3.72759372e+01, 4.03970086e+01, 4.37794036e+01, 210 4.74450028e+01, 5.14175183e+01, 5.57226480e+01, 6.03882412e+01, 211 6.54444792e+01, 7.09240702e+01, 7.68624610e+01, 8.32980665e+01, 212 9.02725178e+01, 9.78309319e+01, 1.06022203e+02, 1.14899320e+02, 213 1.24519708e+02, 1.34945600e+02, 1.46244440e+02, 1.58489319e+02 }; 214 215 static const float ge_26[LC3_MAX_BANDS] = { /* g_tilt = 26 */ 216 1.00000000e+00, 1.09968890e+00, 1.20931568e+00, 1.32987103e+00, 217 1.46244440e+00, 1.60823388e+00, 1.76855694e+00, 1.94486244e+00, 218 2.13874364e+00, 2.35195264e+00, 2.58641621e+00, 2.84425319e+00, 219 3.12779366e+00, 3.43959997e+00, 3.78248991e+00, 4.15956216e+00, 220 4.57422434e+00, 5.03022373e+00, 5.53168120e+00, 6.08312841e+00, 221 6.68954879e+00, 7.35642254e+00, 8.08977621e+00, 8.89623710e+00, 222 9.78309319e+00, 1.07583590e+01, 1.18308480e+01, 1.30102522e+01, 223 1.43072299e+01, 1.57335019e+01, 1.73019574e+01, 1.90267705e+01, 224 2.09235283e+01, 2.30093718e+01, 2.53031508e+01, 2.78255940e+01, 225 3.05994969e+01, 3.36499270e+01, 3.70044512e+01, 4.06933843e+01, 226 4.47500630e+01, 4.92111475e+01, 5.41169527e+01, 5.95118121e+01, 227 6.54444792e+01, 7.19685673e+01, 7.91430346e+01, 8.70327166e+01, 228 9.57089124e+01, 1.05250029e+02, 1.15742288e+02, 1.27280509e+02, 229 1.39968963e+02, 1.53922315e+02, 1.69266662e+02, 1.86140669e+02, 230 2.04696827e+02, 2.25102829e+02, 2.47543082e+02, 2.72220379e+02, 231 2.99357729e+02, 3.29200372e+02, 3.62017995e+02, 3.98107171e+02 }; 232 233 static const float ge_30[LC3_MAX_BANDS] = { /* g_tilt = 30 */ 234 1.00000000e+00, 1.11588399e+00, 1.24519708e+00, 1.38949549e+00, 235 1.55051578e+00, 1.73019574e+00, 1.93069773e+00, 2.15443469e+00, 236 2.40409918e+00, 2.68269580e+00, 2.99357729e+00, 3.34048498e+00, 237 3.72759372e+00, 4.15956216e+00, 4.64158883e+00, 5.17947468e+00, 238 5.77969288e+00, 6.44946677e+00, 7.19685673e+00, 8.03085722e+00, 239 8.96150502e+00, 1.00000000e+01, 1.11588399e+01, 1.24519708e+01, 240 1.38949549e+01, 1.55051578e+01, 1.73019574e+01, 1.93069773e+01, 241 2.15443469e+01, 2.40409918e+01, 2.68269580e+01, 2.99357729e+01, 242 3.34048498e+01, 3.72759372e+01, 4.15956216e+01, 4.64158883e+01, 243 5.17947468e+01, 5.77969288e+01, 6.44946677e+01, 7.19685673e+01, 244 8.03085722e+01, 8.96150502e+01, 1.00000000e+02, 1.11588399e+02, 245 1.24519708e+02, 1.38949549e+02, 1.55051578e+02, 1.73019574e+02, 246 1.93069773e+02, 2.15443469e+02, 2.40409918e+02, 2.68269580e+02, 247 2.99357729e+02, 3.34048498e+02, 3.72759372e+02, 4.15956216e+02, 248 4.64158883e+02, 5.17947468e+02, 5.77969288e+02, 6.44946677e+02, 249 7.19685673e+02, 8.03085722e+02, 8.96150502e+02, 1.00000000e+03 }; 250 251 #if LC3_PLUS_HR 252 253 static const float ge_34[LC3_MAX_BANDS] = { /* g_tilt = 34 */ 254 1.00000000e+00, 1.13231759e+00, 1.28214312e+00, 1.45179321e+00, 255 1.64389099e+00, 1.86140669e+00, 2.10770353e+00, 2.38658979e+00, 256 2.70237760e+00, 3.05994969e+00, 3.46483486e+00, 3.92329345e+00, 257 4.44241419e+00, 5.03022373e+00, 5.69581081e+00, 6.44946677e+00, 258 7.30284467e+00, 8.26913948e+00, 9.36329209e+00, 1.06022203e+01, 259 1.20050806e+01, 1.35935639e+01, 1.53922315e+01, 1.74288945e+01, 260 1.97350438e+01, 2.23463373e+01, 2.53031508e+01, 2.86512027e+01, 261 3.24422608e+01, 3.67349426e+01, 4.15956216e+01, 4.70994540e+01, 262 5.33315403e+01, 6.03882412e+01, 6.83786677e+01, 7.74263683e+01, 263 8.76712387e+01, 9.92716858e+01, 1.12407076e+02, 1.27280509e+02, 264 1.44121960e+02, 1.63191830e+02, 1.84784980e+02, 2.09235283e+02, 265 2.36920791e+02, 2.68269580e+02, 3.03766364e+02, 3.43959997e+02, 266 3.89471955e+02, 4.41005945e+02, 4.99358789e+02, 5.65432741e+02, 267 6.40249439e+02, 7.24965701e+02, 8.20891416e+02, 9.29509790e+02, 268 1.05250029e+03, 1.19176459e+03, 1.34945600e+03, 1.52801277e+03, 269 1.73019574e+03, 1.95913107e+03, 2.21835857e+03, 2.51188643e+03 }; 270 271 #endif /* LC3_PLUS_HR */ 272 273 static const float *ge_table[LC3_NUM_SRATE] = { 274 [LC3_SRATE_8K ] = ge_14, [LC3_SRATE_16K ] = ge_18, 275 [LC3_SRATE_24K ] = ge_22, [LC3_SRATE_32K ] = ge_26, 276 [LC3_SRATE_48K ] = ge_30, 277 278 #if LC3_PLUS_HR 279 [LC3_SRATE_48K_HR] = ge_30, [LC3_SRATE_96K_HR] = ge_34, 280 #endif /* LC3_PLUS_HR */ 281 282 }; 283 284 float e[LC3_MAX_BANDS]; 285 286 /* --- Copy and padding --- */ 287 288 int nb = lc3_num_bands[dt][sr]; 289 int n4 = nb < 32 ? 32 % nb : 0; 290 int n2 = nb < 32 ? nb - n4 : LC3_MAX_BANDS - nb; 291 292 for (int i4 = 0; i4 < n4; i4++) 293 e[4*i4 + 0] = e[4*i4 + 1] = 294 e[4*i4 + 2] = e[4*i4 + 3] = eb[i4]; 295 296 for (int i2 = n4; i2 < n4+n2; i2++) 297 e[2*(n4+i2) + 0] = e[2*(n4+i2) + 1] = eb[i2]; 298 299 memcpy(e + 4*n4 + 2*n2, eb + n4 + n2, (nb - n4 - n2) * sizeof(float)); 300 301 /* --- Smoothing, pre-emphasis and logarithm --- */ 302 303 const float *ge = ge_table[sr]; 304 305 float e0 = e[0], e1 = e[0], e2; 306 float e_sum = 0; 307 308 for (int i = 0; i < LC3_MAX_BANDS-1; ) { 309 e[i] = (e0 * 0.25f + e1 * 0.5f + (e2 = e[i+1]) * 0.25f) * ge[i]; 310 e_sum += e[i++]; 311 312 e[i] = (e1 * 0.25f + e2 * 0.5f + (e0 = e[i+1]) * 0.25f) * ge[i]; 313 e_sum += e[i++]; 314 315 e[i] = (e2 * 0.25f + e0 * 0.5f + (e1 = e[i+1]) * 0.25f) * ge[i]; 316 e_sum += e[i++]; 317 } 318 319 e[LC3_MAX_BANDS-1] = (e0 * 0.25f + e1 * 0.75f) * ge[LC3_MAX_BANDS-1]; 320 e_sum += e[LC3_MAX_BANDS-1]; 321 322 float noise_floor = fmaxf(e_sum * (1e-4f / 64), 0x1p-32f); 323 324 for (int i = 0; i < LC3_MAX_BANDS; i++) 325 e[i] = lc3_log2f(fmaxf(e[i], noise_floor)) * 0.5f; 326 327 /* --- Grouping & scaling --- */ 328 329 float scf_sum; 330 331 scf[0] = (e[0] + e[4]) * 1.f/12 + 332 (e[0] + e[3]) * 2.f/12 + 333 (e[1] + e[2]) * 3.f/12 ; 334 scf_sum = scf[0]; 335 336 for (int i = 1; i < 15; i++) { 337 scf[i] = (e[4*i-1] + e[4*i+4]) * 1.f/12 + 338 (e[4*i ] + e[4*i+3]) * 2.f/12 + 339 (e[4*i+1] + e[4*i+2]) * 3.f/12 ; 340 scf_sum += scf[i]; 341 } 342 343 scf[15] = (e[59] + e[63]) * 1.f/12 + 344 (e[60] + e[63]) * 2.f/12 + 345 (e[61] + e[62]) * 3.f/12 ; 346 scf_sum += scf[15]; 347 348 float cf = lc3_hr(sr) ? 0.6f : 0.85f; 349 if (lc3_hr(sr) && 8 * nbytes > 350 (dt < LC3_DT_10M ? 1150 * (int)(1 + dt) : 4400)) 351 cf *= dt < LC3_DT_10M ? 0.25f : 0.35f; 352 353 for (int i = 0; i < 16; i++) 354 scf[i] = cf * (scf[i] - scf_sum * 1.f/16); 355 356 /* --- Attack handling --- */ 357 358 if (!att) 359 return; 360 361 float s0, s1 = scf[0], s2 = scf[1], s3 = scf[2], s4 = scf[3]; 362 float sn = s1 + s2; 363 364 scf[0] = (sn += s3) * 1.f/3; 365 scf[1] = (sn += s4) * 1.f/4; 366 scf_sum = scf[0] + scf[1]; 367 368 for (int i = 2; i < 14; i++, sn -= s0) { 369 s0 = s1, s1 = s2, s2 = s3, s3 = s4, s4 = scf[i+2]; 370 scf[i] = (sn += s4) * 1.f/5; 371 scf_sum += scf[i]; 372 } 373 374 scf[14] = (sn ) * 1.f/4; 375 scf[15] = (sn -= s1) * 1.f/3; 376 scf_sum += scf[14] + scf[15]; 377 378 for (int i = 0; i < 16; i++) 379 scf[i] = (dt == LC3_DT_7M5 ? 0.3f : 0.5f) * 380 (scf[i] - scf_sum * 1.f/16); 381 } 382 383 /** 384 * Codebooks 385 * scf Input 16 scale factors 386 * lf/hfcb_idx Output the low and high frequency codebooks index 387 */ 388 LC3_HOT static void resolve_codebooks( 389 const float *scf, int *lfcb_idx, int *hfcb_idx) 390 { 391 float dlfcb_max = 0, dhfcb_max = 0; 392 *lfcb_idx = *hfcb_idx = 0; 393 394 for (int icb = 0; icb < 32; icb++) { 395 const float *lfcb = lc3_sns_lfcb[icb]; 396 const float *hfcb = lc3_sns_hfcb[icb]; 397 float dlfcb = 0, dhfcb = 0; 398 399 for (int i = 0; i < 8; i++) { 400 dlfcb += (scf[ i] - lfcb[i]) * (scf[ i] - lfcb[i]); 401 dhfcb += (scf[8+i] - hfcb[i]) * (scf[8+i] - hfcb[i]); 402 } 403 404 if (icb == 0 || dlfcb < dlfcb_max) 405 *lfcb_idx = icb, dlfcb_max = dlfcb; 406 407 if (icb == 0 || dhfcb < dhfcb_max) 408 *hfcb_idx = icb, dhfcb_max = dhfcb; 409 } 410 } 411 412 /** 413 * Unit energy normalize pulse configuration 414 * c Pulse configuration 415 * cn Normalized pulse configuration 416 */ 417 LC3_HOT static void normalize(const int *c, float *cn) 418 { 419 int c2_sum = 0; 420 for (int i = 0; i < 16; i++) 421 c2_sum += c[i] * c[i]; 422 423 float c_norm = 1.f / sqrtf(c2_sum); 424 425 for (int i = 0; i < 16; i++) 426 cn[i] = c[i] * c_norm; 427 } 428 429 /** 430 * Sub-procedure of `quantize()`, add unit pulse 431 * x, y, n Transformed residual, and vector of pulses with length 432 * start, end Current number of pulses, limit to reach 433 * corr, energy Correlation (x,y) and y energy, updated at output 434 */ 435 LC3_HOT static void add_pulse(const float *x, int *y, int n, 436 int start, int end, float *corr, float *energy) 437 { 438 for (int k = start; k < end; k++) { 439 float best_c2 = (*corr + x[0]) * (*corr + x[0]); 440 float best_e = *energy + 2*y[0] + 1; 441 int nbest = 0; 442 443 for (int i = 1; i < n; i++) { 444 float c2 = (*corr + x[i]) * (*corr + x[i]); 445 float e = *energy + 2*y[i] + 1; 446 447 if (c2 * best_e > e * best_c2) 448 best_c2 = c2, best_e = e, nbest = i; 449 } 450 451 *corr += x[nbest]; 452 *energy += 2*y[nbest] + 1; 453 y[nbest]++; 454 } 455 } 456 457 /** 458 * Quantization of codebooks residual 459 * scf Input 16 scale factors, output quantized version 460 * lf/hfcb_idx Codebooks index 461 * c, cn Output 4 pulse configurations candidates, normalized 462 * shape/gain_idx Output selected shape/gain indexes 463 */ 464 LC3_HOT static void quantize(const float *scf, int lfcb_idx, int hfcb_idx, 465 int (*c)[16], float (*cn)[16], int *shape_idx, int *gain_idx) 466 { 467 /* --- Residual --- */ 468 469 const float *lfcb = lc3_sns_lfcb[lfcb_idx]; 470 const float *hfcb = lc3_sns_hfcb[hfcb_idx]; 471 float r[16], x[16]; 472 473 for (int i = 0; i < 8; i++) { 474 r[ i] = scf[ i] - lfcb[i]; 475 r[8+i] = scf[8+i] - hfcb[i]; 476 } 477 478 dct16_forward(r, x); 479 480 /* --- Shape 3 candidate --- 481 * Project to or below pyramid N = 16, K = 6, 482 * then add unit pulses until you reach K = 6, over N = 16 */ 483 484 float xm[16]; 485 float xm_sum = 0; 486 487 for (int i = 0; i < 16; i++) { 488 xm[i] = fabsf(x[i]); 489 xm_sum += xm[i]; 490 } 491 492 float proj_factor = (6 - 1) / fmaxf(xm_sum, 1e-31f); 493 float corr = 0, energy = 0; 494 int npulses = 0; 495 496 for (int i = 0; i < 16; i++) { 497 c[3][i] = floorf(xm[i] * proj_factor); 498 npulses += c[3][i]; 499 corr += c[3][i] * xm[i]; 500 energy += c[3][i] * c[3][i]; 501 } 502 503 add_pulse(xm, c[3], 16, npulses, 6, &corr, &energy); 504 npulses = 6; 505 506 /* --- Shape 2 candidate --- 507 * Add unit pulses until you reach K = 8 on shape 3 */ 508 509 memcpy(c[2], c[3], sizeof(c[2])); 510 511 add_pulse(xm, c[2], 16, npulses, 8, &corr, &energy); 512 npulses = 8; 513 514 /* --- Shape 1 candidate --- 515 * Remove any unit pulses from shape 2 that are not part of 0 to 9 516 * Update energy and correlation terms accordingly 517 * Add unit pulses until you reach K = 10, over N = 10 */ 518 519 memcpy(c[1], c[2], sizeof(c[1])); 520 521 for (int i = 10; i < 16; i++) { 522 c[1][i] = 0; 523 npulses -= c[2][i]; 524 corr -= c[2][i] * xm[i]; 525 energy -= c[2][i] * c[2][i]; 526 } 527 528 add_pulse(xm, c[1], 10, npulses, 10, &corr, &energy); 529 npulses = 10; 530 531 /* --- Shape 0 candidate --- 532 * Add unit pulses until you reach K = 1, on shape 1 */ 533 534 memcpy(c[0], c[1], sizeof(c[0])); 535 536 add_pulse(xm + 10, c[0] + 10, 6, 0, 1, &corr, &energy); 537 538 /* --- Add sign and unit energy normalize --- */ 539 540 for (int j = 0; j < 16; j++) 541 for (int i = 0; i < 4; i++) 542 c[i][j] = x[j] < 0 ? -c[i][j] : c[i][j]; 543 544 for (int i = 0; i < 4; i++) 545 normalize(c[i], cn[i]); 546 547 /* --- Determe shape & gain index --- 548 * Search the Mean Square Error, within (shape, gain) combinations */ 549 550 float mse_min = FLT_MAX; 551 *shape_idx = *gain_idx = 0; 552 553 for (int ic = 0; ic < 4; ic++) { 554 const struct lc3_sns_vq_gains *cgains = lc3_sns_vq_gains + ic; 555 float cmse_min = FLT_MAX; 556 int cgain_idx = 0; 557 558 for (int ig = 0; ig < cgains->count; ig++) { 559 float g = cgains->v[ig]; 560 561 float mse = 0; 562 for (int i = 0; i < 16; i++) 563 mse += (x[i] - g * cn[ic][i]) * (x[i] - g * cn[ic][i]); 564 565 if (mse < cmse_min) { 566 cgain_idx = ig, 567 cmse_min = mse; 568 } 569 } 570 571 if (cmse_min < mse_min) { 572 *shape_idx = ic, *gain_idx = cgain_idx; 573 mse_min = cmse_min; 574 } 575 } 576 } 577 578 /** 579 * Unquantization of codebooks residual 580 * lf/hfcb_idx Low and high frequency codebooks index 581 * c Table of normalized pulse configuration 582 * shape/gain Selected shape/gain indexes 583 * scf Return unquantized scale factors 584 */ 585 LC3_HOT static void unquantize(int lfcb_idx, int hfcb_idx, 586 const float *c, int shape, int gain, float *scf) 587 { 588 const float *lfcb = lc3_sns_lfcb[lfcb_idx]; 589 const float *hfcb = lc3_sns_hfcb[hfcb_idx]; 590 float g = lc3_sns_vq_gains[shape].v[gain]; 591 592 dct16_inverse(c, scf); 593 594 for (int i = 0; i < 8; i++) 595 scf[i] = lfcb[i] + g * scf[i]; 596 597 for (int i = 8; i < 16; i++) 598 scf[i] = hfcb[i-8] + g * scf[i]; 599 } 600 601 /** 602 * Sub-procedure of `sns_enumerate()`, enumeration of a vector 603 * c, n Table of pulse configuration, and length 604 * idx, ls Return enumeration set 605 */ 606 static void enum_mvpq(const int *c, int n, int *idx, bool *ls) 607 { 608 int ci, i; 609 610 /* --- Scan for 1st significant coeff --- */ 611 612 for (i = 0, c += n; (ci = *(--c)) == 0 && i < 15; i++); 613 614 *idx = 0; 615 *ls = ci < 0; 616 617 /* --- Scan remaining coefficients --- */ 618 619 unsigned j = LC3_ABS(ci); 620 621 for (i++; i < n; i++, j += LC3_ABS(ci)) { 622 623 if ((ci = *(--c)) != 0) { 624 *idx = (*idx << 1) | *ls; 625 *ls = ci < 0; 626 } 627 628 *idx += lc3_sns_mpvq_offsets[i][LC3_MIN(j, 10)]; 629 } 630 } 631 632 /** 633 * Sub-procedure of `sns_deenumerate()`, deenumeration of a vector 634 * idx, ls Enumeration set 635 * npulses Number of pulses in the set 636 * c, n Table of pulses configuration, and length 637 */ 638 static void deenum_mvpq(int idx, bool ls, int npulses, int *c, int n) 639 { 640 int i; 641 642 /* --- Scan for coefficients --- */ 643 644 for (i = n-1; i >= 0 && idx; i--) { 645 646 int ci = 0; 647 648 for (ci = 0; idx < lc3_sns_mpvq_offsets[i][npulses - ci]; ci++); 649 idx -= lc3_sns_mpvq_offsets[i][npulses - ci]; 650 651 *(c++) = ls ? -ci : ci; 652 npulses -= ci; 653 if (ci > 0) { 654 ls = idx & 1; 655 idx >>= 1; 656 } 657 } 658 659 /* --- Set last significant --- */ 660 661 int ci = npulses; 662 663 if (i-- >= 0) 664 *(c++) = ls ? -ci : ci; 665 666 while (i-- >= 0) 667 *(c++) = 0; 668 } 669 670 /** 671 * SNS Enumeration of PVQ configuration 672 * shape Selected shape index 673 * c Selected pulse configuration 674 * idx_a, ls_a Return enumeration set A 675 * idx_b, ls_b Return enumeration set B (shape = 0) 676 */ 677 static void enumerate(int shape, const int *c, 678 int *idx_a, bool *ls_a, int *idx_b, bool *ls_b) 679 { 680 enum_mvpq(c, shape < 2 ? 10 : 16, idx_a, ls_a); 681 682 if (shape == 0) 683 enum_mvpq(c + 10, 6, idx_b, ls_b); 684 } 685 686 /** 687 * SNS Deenumeration of PVQ configuration 688 * shape Selected shape index 689 * idx_a, ls_a enumeration set A 690 * idx_b, ls_b enumeration set B (shape = 0) 691 * c Return pulse configuration 692 */ 693 static void deenumerate(int shape, 694 int idx_a, bool ls_a, int idx_b, bool ls_b, int *c) 695 { 696 int npulses_a = (const int []){ 10, 10, 8, 6 }[shape]; 697 698 deenum_mvpq(idx_a, ls_a, npulses_a, c, shape < 2 ? 10 : 16); 699 700 if (shape == 0) 701 deenum_mvpq(idx_b, ls_b, 1, c + 10, 6); 702 else if (shape == 1) 703 memset(c + 10, 0, 6 * sizeof(*c)); 704 } 705 706 707 /* ---------------------------------------------------------------------------- 708 * Filtering 709 * -------------------------------------------------------------------------- */ 710 711 /** 712 * Spectral shaping 713 * dt, sr Duration and samplerate of the frame 714 * scf_q Quantized scale factors 715 * inv True on inverse shaping, False otherwise 716 * x Spectral coefficients 717 * y Return shapped coefficients 718 * 719 * `x` and `y` can be the same buffer 720 */ 721 LC3_HOT static void spectral_shaping(enum lc3_dt dt, enum lc3_srate sr, 722 const float *scf_q, bool inv, const float *x, float *y) 723 { 724 /* --- Interpolate scale factors --- */ 725 726 float scf[LC3_MAX_BANDS]; 727 float s0, s1 = inv ? -scf_q[0] : scf_q[0]; 728 729 scf[0] = scf[1] = s1; 730 for (int i = 0; i < 15; i++) { 731 s0 = s1, s1 = inv ? -scf_q[i+1] : scf_q[i+1]; 732 scf[4*i+2] = s0 + 0.125f * (s1 - s0); 733 scf[4*i+3] = s0 + 0.375f * (s1 - s0); 734 scf[4*i+4] = s0 + 0.625f * (s1 - s0); 735 scf[4*i+5] = s0 + 0.875f * (s1 - s0); 736 } 737 scf[62] = s1 + 0.125f * (s1 - s0); 738 scf[63] = s1 + 0.375f * (s1 - s0); 739 740 int nb = lc3_num_bands[dt][sr]; 741 int n4 = nb < 32 ? 32 % nb : 0; 742 int n2 = nb < 32 ? nb - n4 : LC3_MAX_BANDS - nb; 743 744 for (int i4 = 0; i4 < n4; i4++) 745 scf[i4] = 0.25f * (scf[4*i4+0] + scf[4*i4+1] + 746 scf[4*i4+2] + scf[4*i4+3]); 747 748 for (int i2 = n4; i2 < n4+n2; i2++) 749 scf[i2] = 0.5f * (scf[2*(n4+i2)] + scf[2*(n4+i2)+1]); 750 751 memmove(scf + n4 + n2, scf + 4*n4 + 2*n2, (nb - n4 - n2) * sizeof(float)); 752 753 /* --- Spectral shaping --- */ 754 755 const int *lim = lc3_band_lim[dt][sr]; 756 757 for (int i = 0, ib = 0; ib < nb; ib++) { 758 float g_sns = lc3_exp2f(-scf[ib]); 759 760 for ( ; i < lim[ib+1]; i++) 761 y[i] = x[i] * g_sns; 762 } 763 } 764 765 766 /* ---------------------------------------------------------------------------- 767 * Interface 768 * -------------------------------------------------------------------------- */ 769 770 /** 771 * SNS analysis 772 */ 773 void lc3_sns_analyze( 774 enum lc3_dt dt, enum lc3_srate sr, int nbytes, 775 const float *eb, bool att, struct lc3_sns_data *data, 776 const float *x, float *y) 777 { 778 /* Processing steps : 779 * - Determine 16 scale factors from bands energy estimation 780 * - Get codebooks indexes that match thoses scale factors 781 * - Quantize the residual with the selected codebook 782 * - The pulse configuration `c[]` is enumerated 783 * - Finally shape the spectrum coefficients accordingly */ 784 785 float scf[16], cn[4][16]; 786 int c[4][16]; 787 788 compute_scale_factors(dt, sr, nbytes, eb, att, scf); 789 790 resolve_codebooks(scf, &data->lfcb, &data->hfcb); 791 792 quantize(scf, data->lfcb, data->hfcb, 793 c, cn, &data->shape, &data->gain); 794 795 unquantize(data->lfcb, data->hfcb, 796 cn[data->shape], data->shape, data->gain, scf); 797 798 enumerate(data->shape, c[data->shape], 799 &data->idx_a, &data->ls_a, &data->idx_b, &data->ls_b); 800 801 spectral_shaping(dt, sr, scf, false, x, y); 802 } 803 804 /** 805 * SNS synthesis 806 */ 807 void lc3_sns_synthesize( 808 enum lc3_dt dt, enum lc3_srate sr, 809 const lc3_sns_data_t *data, const float *x, float *y) 810 { 811 float scf[16], cn[16]; 812 int c[16]; 813 814 deenumerate(data->shape, 815 data->idx_a, data->ls_a, data->idx_b, data->ls_b, c); 816 817 normalize(c, cn); 818 819 unquantize(data->lfcb, data->hfcb, cn, data->shape, data->gain, scf); 820 821 spectral_shaping(dt, sr, scf, true, x, y); 822 } 823 824 /** 825 * Return number of bits coding the bitstream data 826 */ 827 int lc3_sns_get_nbits(void) 828 { 829 return 38; 830 } 831 832 /** 833 * Put bitstream data 834 */ 835 void lc3_sns_put_data(lc3_bits_t *bits, const struct lc3_sns_data *data) 836 { 837 /* --- Codebooks --- */ 838 839 lc3_put_bits(bits, data->lfcb, 5); 840 lc3_put_bits(bits, data->hfcb, 5); 841 842 /* --- Shape, gain and vectors --- * 843 * Write MSB bit of shape index, next LSB bits of shape and gain, 844 * and MVPQ vectors indexes are muxed */ 845 846 int shape_msb = data->shape >> 1; 847 lc3_put_bit(bits, shape_msb); 848 849 if (shape_msb == 0) { 850 const int size_a = 2390004; 851 int submode = data->shape & 1; 852 853 int mux_high = submode == 0 ? 854 2 * (data->idx_b + 1) + data->ls_b : data->gain & 1; 855 int mux_code = mux_high * size_a + data->idx_a; 856 857 lc3_put_bits(bits, data->gain >> submode, 1); 858 lc3_put_bits(bits, data->ls_a, 1); 859 lc3_put_bits(bits, mux_code, 25); 860 861 } else { 862 const int size_a = 15158272; 863 int submode = data->shape & 1; 864 865 int mux_code = submode == 0 ? 866 data->idx_a : size_a + 2 * data->idx_a + (data->gain & 1); 867 868 lc3_put_bits(bits, data->gain >> submode, 2); 869 lc3_put_bits(bits, data->ls_a, 1); 870 lc3_put_bits(bits, mux_code, 24); 871 } 872 } 873 874 /** 875 * Get bitstream data 876 */ 877 int lc3_sns_get_data(lc3_bits_t *bits, struct lc3_sns_data *data) 878 { 879 /* --- Codebooks --- */ 880 881 *data = (struct lc3_sns_data){ 882 .lfcb = lc3_get_bits(bits, 5), 883 .hfcb = lc3_get_bits(bits, 5) 884 }; 885 886 /* --- Shape, gain and vectors --- */ 887 888 int shape_msb = lc3_get_bit(bits); 889 data->gain = lc3_get_bits(bits, 1 + shape_msb); 890 data->ls_a = lc3_get_bit(bits); 891 892 int mux_code = lc3_get_bits(bits, 25 - shape_msb); 893 894 if (shape_msb == 0) { 895 const int size_a = 2390004; 896 897 if (mux_code >= size_a * 14) 898 return -1; 899 900 data->idx_a = mux_code % size_a; 901 mux_code = mux_code / size_a; 902 903 data->shape = (mux_code < 2); 904 905 if (data->shape == 0) { 906 data->idx_b = (mux_code - 2) / 2; 907 data->ls_b = (mux_code - 2) % 2; 908 } else { 909 data->gain = (data->gain << 1) + (mux_code % 2); 910 } 911 912 } else { 913 const int size_a = 15158272; 914 915 if (mux_code >= size_a + 1549824) 916 return -1; 917 918 data->shape = 2 + (mux_code >= size_a); 919 if (data->shape == 2) { 920 data->idx_a = mux_code; 921 } else { 922 mux_code -= size_a; 923 data->idx_a = mux_code / 2; 924 data->gain = (data->gain << 1) + (mux_code % 2); 925 } 926 } 927 928 return 0; 929 } 930