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 "crossover.h" 7 #include "biquad.h" 8 9 static void lr4_set(struct lr4 *lr4, enum biquad_type type, float freq) 10 { 11 struct biquad q; 12 biquad_set(&q, type, freq, 0, 0); 13 lr4->b0 = q.b0; 14 lr4->b1 = q.b1; 15 lr4->b2 = q.b2; 16 lr4->a1 = q.a1; 17 lr4->a2 = q.a2; 18 lr4->x1 = 0; 19 lr4->x2 = 0; 20 lr4->y1 = 0; 21 lr4->y2 = 0; 22 lr4->z1 = 0; 23 lr4->z2 = 0; 24 } 25 26 /* Split input data using two LR4 filters, put the result into the input array 27 * and another array. 28 * 29 * data0 --+-- lp --> data0 30 * | 31 * \-- hp --> data1 32 */ 33 static void lr4_split(struct lr4 *lp, struct lr4 *hp, int count, float *data0, 34 float *data1) 35 { 36 float lx1 = lp->x1; 37 float lx2 = lp->x2; 38 float ly1 = lp->y1; 39 float ly2 = lp->y2; 40 float lz1 = lp->z1; 41 float lz2 = lp->z2; 42 float lb0 = lp->b0; 43 float lb1 = lp->b1; 44 float lb2 = lp->b2; 45 float la1 = lp->a1; 46 float la2 = lp->a2; 47 48 float hx1 = hp->x1; 49 float hx2 = hp->x2; 50 float hy1 = hp->y1; 51 float hy2 = hp->y2; 52 float hz1 = hp->z1; 53 float hz2 = hp->z2; 54 float hb0 = hp->b0; 55 float hb1 = hp->b1; 56 float hb2 = hp->b2; 57 float ha1 = hp->a1; 58 float ha2 = hp->a2; 59 60 int i; 61 for (i = 0; i < count; i++) { 62 float x, y, z; 63 x = data0[i]; 64 y = lb0*x + lb1*lx1 + lb2*lx2 - la1*ly1 - la2*ly2; 65 z = lb0*y + lb1*ly1 + lb2*ly2 - la1*lz1 - la2*lz2; 66 lx2 = lx1; 67 lx1 = x; 68 ly2 = ly1; 69 ly1 = y; 70 lz2 = lz1; 71 lz1 = z; 72 data0[i] = z; 73 74 y = hb0*x + hb1*hx1 + hb2*hx2 - ha1*hy1 - ha2*hy2; 75 z = hb0*y + hb1*hy1 + hb2*hy2 - ha1*hz1 - ha2*hz2; 76 hx2 = hx1; 77 hx1 = x; 78 hy2 = hy1; 79 hy1 = y; 80 hz2 = hz1; 81 hz1 = z; 82 data1[i] = z; 83 } 84 85 lp->x1 = lx1; 86 lp->x2 = lx2; 87 lp->y1 = ly1; 88 lp->y2 = ly2; 89 lp->z1 = lz1; 90 lp->z2 = lz2; 91 92 hp->x1 = hx1; 93 hp->x2 = hx2; 94 hp->y1 = hy1; 95 hp->y2 = hy2; 96 hp->z1 = hz1; 97 hp->z2 = hz2; 98 } 99 100 /* Split input data using two LR4 filters and sum them back to the original 101 * data array. 102 * 103 * data --+-- lp --+--> data 104 * | | 105 * \-- hp --/ 106 */ 107 static void lr4_merge(struct lr4 *lp, struct lr4 *hp, int count, float *data) 108 { 109 float lx1 = lp->x1; 110 float lx2 = lp->x2; 111 float ly1 = lp->y1; 112 float ly2 = lp->y2; 113 float lz1 = lp->z1; 114 float lz2 = lp->z2; 115 float lb0 = lp->b0; 116 float lb1 = lp->b1; 117 float lb2 = lp->b2; 118 float la1 = lp->a1; 119 float la2 = lp->a2; 120 121 float hx1 = hp->x1; 122 float hx2 = hp->x2; 123 float hy1 = hp->y1; 124 float hy2 = hp->y2; 125 float hz1 = hp->z1; 126 float hz2 = hp->z2; 127 float hb0 = hp->b0; 128 float hb1 = hp->b1; 129 float hb2 = hp->b2; 130 float ha1 = hp->a1; 131 float ha2 = hp->a2; 132 133 int i; 134 for (i = 0; i < count; i++) { 135 float x, y, z; 136 x = data[i]; 137 y = lb0*x + lb1*lx1 + lb2*lx2 - la1*ly1 - la2*ly2; 138 z = lb0*y + lb1*ly1 + lb2*ly2 - la1*lz1 - la2*lz2; 139 lx2 = lx1; 140 lx1 = x; 141 ly2 = ly1; 142 ly1 = y; 143 lz2 = lz1; 144 lz1 = z; 145 146 y = hb0*x + hb1*hx1 + hb2*hx2 - ha1*hy1 - ha2*hy2; 147 z = hb0*y + hb1*hy1 + hb2*hy2 - ha1*hz1 - ha2*hz2; 148 hx2 = hx1; 149 hx1 = x; 150 hy2 = hy1; 151 hy1 = y; 152 hz2 = hz1; 153 hz1 = z; 154 data[i] = z + lz1; 155 } 156 157 lp->x1 = lx1; 158 lp->x2 = lx2; 159 lp->y1 = ly1; 160 lp->y2 = ly2; 161 lp->z1 = lz1; 162 lp->z2 = lz2; 163 164 hp->x1 = hx1; 165 hp->x2 = hx2; 166 hp->y1 = hy1; 167 hp->y2 = hy2; 168 hp->z1 = hz1; 169 hp->z2 = hz2; 170 } 171 172 void crossover_init(struct crossover *xo, float freq1, float freq2) 173 { 174 int i; 175 for (i = 0; i < 3; i++) { 176 float f = (i == 0) ? freq1 : freq2; 177 lr4_set(&xo->lp[i], BQ_LOWPASS, f); 178 lr4_set(&xo->hp[i], BQ_HIGHPASS, f); 179 } 180 } 181 182 void crossover_process(struct crossover *xo, int count, float *data0, 183 float *data1, float *data2) 184 { 185 lr4_split(&xo->lp[0], &xo->hp[0], count, data0, data1); 186 lr4_merge(&xo->lp[1], &xo->hp[1], count, data0); 187 lr4_split(&xo->lp[2], &xo->hp[2], count, data1, data2); 188 } 189
