1 /* Copyright (c) 2013 The Chromium OS Authors. All rights reserved. 2 * Use of this source code is governed by a BSD-style license that can be 3 * found in the LICENSE file. 4 */ 5 6 #include <stdlib.h> 7 #include "eq.h" 8 9 struct eq { 10 int n; 11 struct biquad biquad[MAX_BIQUADS_PER_EQ]; 12 }; 13 14 struct eq *eq_new() 15 { 16 struct eq *eq = (struct eq *)calloc(1, sizeof(*eq)); 17 return eq; 18 } 19 20 void eq_free(struct eq *eq) 21 { 22 free(eq); 23 } 24 25 int eq_append_biquad(struct eq *eq, enum biquad_type type, float freq, float Q, 26 float gain) 27 { 28 if (eq->n >= MAX_BIQUADS_PER_EQ) 29 return -1; 30 biquad_set(&eq->biquad[eq->n++], type, freq, Q, gain); 31 return 0; 32 } 33 34 int eq_append_biquad_direct(struct eq *eq, const struct biquad *biquad) 35 { 36 if (eq->n >= MAX_BIQUADS_PER_EQ) 37 return -1; 38 eq->biquad[eq->n++] = *biquad; 39 return 0; 40 } 41 42 /* This is the prototype of the processing loop. */ 43 void eq_process1(struct eq *eq, float *data, int count) 44 { 45 int i, j; 46 for (i = 0; i < eq->n; i++) { 47 struct biquad *q = &eq->biquad[i]; 48 float x1 = q->x1; 49 float x2 = q->x2; 50 float y1 = q->y1; 51 float y2 = q->y2; 52 float b0 = q->b0; 53 float b1 = q->b1; 54 float b2 = q->b2; 55 float a1 = q->a1; 56 float a2 = q->a2; 57 for (j = 0; j < count; j++) { 58 float x = data[j]; 59 float y = b0*x 60 + b1*x1 + b2*x2 61 - a1*y1 - a2*y2; 62 data[j] = y; 63 x2 = x1; 64 x1 = x; 65 y2 = y1; 66 y1 = y; 67 } 68 q->x1 = x1; 69 q->x2 = x2; 70 q->y1 = y1; 71 q->y2 = y2; 72 } 73 } 74 75 /* This is the actual processing loop used. It is the unrolled version of the 76 * above prototype. */ 77 void eq_process(struct eq *eq, float *data, int count) 78 { 79 int i, j; 80 for (i = 0; i < eq->n; i += 2) { 81 if (i + 1 == eq->n) { 82 struct biquad *q = &eq->biquad[i]; 83 float x1 = q->x1; 84 float x2 = q->x2; 85 float y1 = q->y1; 86 float y2 = q->y2; 87 float b0 = q->b0; 88 float b1 = q->b1; 89 float b2 = q->b2; 90 float a1 = q->a1; 91 float a2 = q->a2; 92 for (j = 0; j < count; j++) { 93 float x = data[j]; 94 float y = b0*x 95 + b1*x1 + b2*x2 96 - a1*y1 - a2*y2; 97 data[j] = y; 98 x2 = x1; 99 x1 = x; 100 y2 = y1; 101 y1 = y; 102 } 103 q->x1 = x1; 104 q->x2 = x2; 105 q->y1 = y1; 106 q->y2 = y2; 107 } else { 108 struct biquad *q = &eq->biquad[i]; 109 struct biquad *r = &eq->biquad[i+1]; 110 float x1 = q->x1; 111 float x2 = q->x2; 112 float y1 = q->y1; 113 float y2 = q->y2; 114 float qb0 = q->b0; 115 float qb1 = q->b1; 116 float qb2 = q->b2; 117 float qa1 = q->a1; 118 float qa2 = q->a2; 119 120 float z1 = r->y1; 121 float z2 = r->y2; 122 float rb0 = r->b0; 123 float rb1 = r->b1; 124 float rb2 = r->b2; 125 float ra1 = r->a1; 126 float ra2 = r->a2; 127 128 for (j = 0; j < count; j++) { 129 float x = data[j]; 130 float y = qb0*x 131 + qb1*x1 + qb2*x2 132 - qa1*y1 - qa2*y2; 133 float z = rb0*y 134 + rb1*y1 + rb2*y2 135 - ra1*z1 - ra2*z2; 136 data[j] = z; 137 x2 = x1; 138 x1 = x; 139 y2 = y1; 140 y1 = y; 141 z2 = z1; 142 z1 = z; 143 } 144 q->x1 = x1; 145 q->x2 = x2; 146 q->y1 = y1; 147 q->y2 = y2; 148 r->y1 = z1; 149 r->y2 = z2; 150 } 151 } 152 } 153
