1 /* 2 * CDDL HEADER START 3 * 4 * The contents of this file are subject to the terms of the 5 * Common Development and Distribution License (the "License"). 6 * You may not use this file except in compliance with the License. 7 * 8 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE 9 * or http://www.opensolaris.org/os/licensing. 10 * See the License for the specific language governing permissions 11 * and limitations under the License. 12 * 13 * When distributing Covered Code, include this CDDL HEADER in each 14 * file and include the License file at usr/src/OPENSOLARIS.LICENSE. 15 * If applicable, add the following below this CDDL HEADER, with the 16 * fields enclosed by brackets "[]" replaced with your own identifying 17 * information: Portions Copyright [yyyy] [name of copyright owner] 18 * 19 * CDDL HEADER END 20 */ 21 /* 22 * Copyright (C) 4Front Technologies 1996-2008. 23 * 24 * Copyright 2009 Sun Microsystems, Inc. All rights reserved. 25 * Use is subject to license terms. 26 */ 27 28 /* 29 * Purpose: GRC3 Sample Rate Converter 30 * 31 * GRC library version 3.1 32 */ 33 34 #include <sys/types.h> 35 #include "audio_grc3.h" 36 37 extern const int32_t filter_data_L[]; 38 extern const int32_t filter_data_M[]; 39 extern const int32_t filter_data_H[]; 40 extern const int32_t filter_data_P[]; 41 42 #define filter_data_HX filter_data_H 43 #define filter_data_PX filter_data_P 44 45 static int32_t 46 _muldivu64(uint32_t a, uint32_t val1, uint32_t val2) 47 { 48 uint64_t v = ((uint64_t)a) * val1 / val2; 49 return ((uint32_t)(v)); 50 } 51 52 53 static int32_t 54 _grc_sat6(int32_t a, int32_t b) 55 { 56 int64_t v = ((int64_t)a) * b + (1 << 5); 57 return ((int32_t)(v >> 6)); 58 } 59 60 static int32_t 61 _grc_sat31(int32_t a, int32_t b) 62 { 63 int64_t v = ((int64_t)a) * b + (1 << 30); 64 return ((int32_t)(v >> 31)); 65 } 66 67 68 #define DEFINE_FILTER(T) \ 69 static int32_t \ 70 _filt31_##T(int32_t a, int32_t idx) \ 71 { \ 72 int64_t v = ((int64_t)a) * filter_data_##T[idx >> 15]; \ 73 return ((int32_t)(v >> 31)); \ 74 } 75 76 #define DEFINE_FILTER_HQ(T) \ 77 static int32_t \ 78 _filt31_##T(int32_t a, int32_t idx) \ 79 { \ 80 int32_t idx2 = idx>>15; \ 81 int64_t v = ((int64_t)a) * \ 82 \ 83 (filter_data_##T[idx2] + \ 84 (((int64_t)(idx & 32767)) * (filter_data_##T[idx2 + 1] - \ 85 filter_data_##T[idx2]) >> 15)); \ 86 return ((int32_t)(v>>31)); \ 87 } 88 89 90 DEFINE_FILTER(L) 91 DEFINE_FILTER(M) 92 DEFINE_FILTER(H) 93 DEFINE_FILTER_HQ(HX) 94 DEFINE_FILTER(P) 95 DEFINE_FILTER_HQ(PX) 96 97 #define DEFINE_CONVD(T, SZ) \ 98 static int32_t \ 99 _conv31d_##T(int32_t *history, uint32_t filter, uint32_t incv) \ 100 { \ 101 int32_t accum = 0; \ 102 \ 103 filter = (1024 << 15) - filter; \ 104 \ 105 while (filter < ((uint32_t)(SZ << 15))) { \ 106 accum += _filt31_##T(*history, filter); \ 107 filter += incv; \ 108 history--; \ 109 } \ 110 \ 111 return (accum); \ 112 } 113 114 DEFINE_CONVD(L, 4096) 115 DEFINE_CONVD(M, 8192) 116 DEFINE_CONVD(H, 16384) 117 DEFINE_CONVD(HX, 16384) 118 DEFINE_CONVD(P, 32768) 119 DEFINE_CONVD(PX, 32768) 120 121 static int32_t 122 _conv31_L(int32_t *history, uint32_t filter) 123 { 124 int32_t accum = 0; 125 126 #define ITERATION(p) \ 127 accum += _filt31_##p(*history, filter); \ 128 filter += (1024 << 15); \ 129 history-- 130 131 ITERATION(L); ITERATION(L); ITERATION(L); ITERATION(L); 132 return (accum); 133 } 134 135 136 static int32_t 137 _conv31_M(int32_t *history, uint32_t filter) 138 { 139 int32_t accum = 0; 140 141 ITERATION(M); ITERATION(M); ITERATION(M); ITERATION(M); 142 ITERATION(M); ITERATION(M); ITERATION(M); ITERATION(M); 143 return (accum); 144 } 145 146 static int32_t 147 _conv31_H(int32_t *history, uint32_t filter) 148 { 149 int32_t accum = 0; 150 151 ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H); 152 ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H); 153 ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H); 154 ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H); 155 return (accum); 156 } 157 158 static int32_t 159 _conv31_HX(int32_t *history, uint32_t filter) 160 { 161 int32_t accum = 0; 162 163 ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX); 164 ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX); 165 ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX); 166 ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX); 167 return (accum); 168 } 169 170 static int32_t 171 _conv31_P(int32_t *history, uint32_t filter) 172 { 173 int32_t accum = 0; 174 175 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P); 176 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P); 177 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P); 178 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P); 179 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P); 180 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P); 181 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P); 182 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P); 183 return (accum); 184 } 185 186 static int32_t 187 _conv31_PX(int32_t *history, uint32_t filter) 188 { 189 int32_t accum = 0; 190 191 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX); 192 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX); 193 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX); 194 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX); 195 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX); 196 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX); 197 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX); 198 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX); 199 return (accum); 200 } 201 202 #define GRC3_RESAMPLE(QUAL) \ 203 static void \ 204 grc3_upsample_##QUAL(grc3state_t *grc, const int32_t *src, \ 205 int32_t *dst, uint32_t sz, uint32_t bufsz, int inc, int offset) \ 206 { \ 207 int32_t ptr = grc->ptr; \ 208 int32_t srcrate = grc->srcrate; \ 209 int32_t dstrate = grc->dstrate; \ 210 int32_t *history = grc->historyptr; \ 211 int32_t filtfactor = grc->filtfactor; \ 212 uint32_t dstsz = 0; \ 213 \ 214 src += offset; \ 215 dst += offset; \ 216 \ 217 while (sz > 0) { \ 218 while (ptr < dstrate) { \ 219 if (dstsz >= bufsz) \ 220 goto endloop; \ 221 dst[0] = (_conv31_##QUAL(history, \ 222 _grc_sat6(ptr, filtfactor))); \ 223 ptr += srcrate; \ 224 dst += inc; \ 225 dstsz++; \ 226 } \ 227 \ 228 history++; \ 229 if (history >= (grc->history + GRC3_MAXHISTORY * 2)) \ 230 history -= GRC3_MAXHISTORY; \ 231 \ 232 history[0] = history[-GRC3_MAXHISTORY] = (*src); \ 233 \ 234 ptr -= dstrate; \ 235 \ 236 sz--; \ 237 src += inc; \ 238 } \ 239 endloop: \ 240 \ 241 grc->ptr = ptr; \ 242 grc->historyptr = history; \ 243 grc->outsz = dstsz; \ 244 } \ 245 \ 246 static void \ 247 grc3_dnsample_##QUAL(grc3state_t *grc, const int32_t *src, \ 248 int32_t *dst, uint32_t sz, uint32_t bufsz, int inc, int offset) \ 249 { \ 250 int32_t ptr = grc->ptr; \ 251 int32_t srcrate = grc->srcrate; \ 252 int32_t dstrate = grc->dstrate; \ 253 int32_t sat = grc->sat; \ 254 int32_t *history = grc->historyptr; \ 255 int32_t filtfactor = grc->filtfactor; \ 256 uint32_t dstsz = 0; \ 257 \ 258 src += offset; \ 259 dst += offset; \ 260 \ 261 while (sz > 0) { \ 262 while (ptr >= srcrate) { \ 263 if (dstsz >= bufsz) \ 264 goto endloop; \ 265 ptr -= srcrate; \ 266 dst[0] = (_conv31d_##QUAL(history, \ 267 _grc_sat6(ptr, filtfactor), \ 268 grc->ptr_incv)); \ 269 dst += inc; \ 270 dstsz++; \ 271 } \ 272 \ 273 history++; \ 274 if (history >= (grc->history + GRC3_MAXHISTORY * 2)) \ 275 history -= GRC3_MAXHISTORY; \ 276 \ 277 /* \ 278 * TODO: for better quality multiplier is worth moving \ 279 * to output cascade \ 280 */ \ 281 history[0] = history[-GRC3_MAXHISTORY] = \ 282 _grc_sat31((*src), sat); \ 283 \ 284 ptr += dstrate; \ 285 \ 286 sz--; \ 287 src += inc; \ 288 } \ 289 endloop: \ 290 \ 291 grc->ptr = ptr; \ 292 grc->historyptr = history; \ 293 grc->outsz = dstsz; \ 294 } \ 295 \ 296 static void \ 297 grc3_resample_##QUAL(grc3state_t *grc, const void *src, void *dst, \ 298 uint32_t sz, uint32_t bufsz, int inc, int offset) \ 299 { \ 300 if (grc->srcrate <= grc->dstrate) \ 301 grc3_upsample_##QUAL(grc, src, dst, sz, \ 302 bufsz, inc, offset); \ 303 else \ 304 grc3_dnsample_##QUAL(grc, src, dst, sz, \ 305 bufsz, inc, offset); \ 306 } 307 308 GRC3_RESAMPLE(L) 309 GRC3_RESAMPLE(M) 310 GRC3_RESAMPLE(H) 311 GRC3_RESAMPLE(HX) 312 GRC3_RESAMPLE(P) 313 GRC3_RESAMPLE(PX) 314 315 /* 316 * For performance reasons, we only support 24-bit SRC. 317 */ 318 void 319 grc3_convert(grc3state_t *grc, int quality, const void *src, 320 void *dst, int sz, int bufsz, int inc, int offset) 321 { 322 323 switch (quality) { 324 default: 325 case 0: 326 case 1: 327 grc3_resample_L(grc, src, dst, sz, bufsz, inc, offset); 328 break; 329 case 2: 330 grc3_resample_M(grc, src, dst, sz, bufsz, inc, offset); 331 break; 332 case 3: 333 grc3_resample_H(grc, src, dst, sz, bufsz, inc, offset); 334 break; 335 case 4: 336 grc3_resample_HX(grc, src, dst, sz, bufsz, inc, offset); 337 break; 338 case 5: 339 grc3_resample_P(grc, src, dst, sz, bufsz, inc, offset); 340 break; 341 case 6: 342 grc3_resample_PX(grc, src, dst, sz, bufsz, inc, offset); 343 break; 344 } 345 } 346 347 void 348 grc3_reset(grc3state_t *grc) 349 { 350 int32_t t; 351 grc->ptr = 0; 352 grc->historyptr = grc->history + GRC3_MAXHISTORY; 353 354 for (t = 0; t < GRC3_MAXHISTORY * 2; t++) 355 grc->history[t] = 0; 356 } 357 358 static void 359 grc3_setup_up(grc3state_t *grc, uint32_t fromRate, uint32_t toRate) 360 { 361 grc->srcrate = fromRate; 362 grc->dstrate = toRate; 363 grc->filtfactor = 0x80000000U / toRate; 364 } 365 366 static void 367 grc3_setup_dn(grc3state_t *grc, uint32_t fromRate, uint32_t toRate) 368 { 369 grc->srcrate = fromRate; 370 grc->dstrate = toRate; 371 grc->filtfactor = 0x80000000U / fromRate; 372 grc->ptr_incv = _muldivu64(1024 << 15, toRate, fromRate); 373 grc->sat = _muldivu64(0x80000000U, toRate, fromRate); 374 } 375 376 void 377 grc3_setup(grc3state_t *grc, uint32_t fromRate, uint32_t toRate) 378 { 379 while ((!(fromRate & 1)) && (!(toRate & 1)) && (fromRate > 0)) { 380 fromRate >>= 1; 381 toRate >>= 1; 382 } 383 384 if (fromRate <= toRate) 385 grc3_setup_up(grc, fromRate, toRate); 386 else 387 grc3_setup_dn(grc, fromRate, toRate); 388 } 389