1 /*
2 * EQ.cpp
3 * ------
4 * Purpose: Mixing code for equalizer.
5 * Notes : Ugh... This should really be removed at some point.
6 * Authors: Olivier Lapicque
7 * OpenMPT Devs
8 * The OpenMPT source code is released under the BSD license. Read LICENSE for more details.
9 */
10
11
12 #include "stdafx.h"
13
14 #include "EQ.h"
15
16 #include "mpt/audio/span.hpp"
17 #include "mpt/base/numbers.hpp"
18 #include "openmpt/base/Types.hpp"
19 #include "openmpt/soundbase/MixSample.hpp"
20 #include "openmpt/soundbase/MixSampleConvert.hpp"
21
22 #ifndef NO_EQ
23 #include "../misc/mptCPU.h"
24 #endif
25
26 #include <algorithm>
27 #include <array>
28
29 #include <cstddef>
30
31 #if defined(MPT_ENABLE_ARCH_INTRINSICS_SSE)
32 #include <xmmintrin.h>
33 #endif
34
35
36 OPENMPT_NAMESPACE_BEGIN
37
38
39 #ifndef NO_EQ
40
41
42 static constexpr float EQ_BANDWIDTH = 2.0f;
43
44
45 static constexpr std::array<uint32, 33> gEqLinearToDB =
46 {
47 16, 19, 22, 25, 28, 31, 34, 37,
48 40, 43, 46, 49, 52, 55, 58, 61,
49 64, 76, 88, 100, 112, 124, 136, 148,
50 160, 172, 184, 196, 208, 220, 232, 244, 256
51 };
52
53
54 static constexpr std::array<EQBANDSETTINGS, MAX_EQ_BANDS> gEQDefaults =
55 {{
56 // Default: Flat EQ
57 {0,0,0,0,0, 1, 120},
58 {0,0,0,0,0, 1, 600},
59 {0,0,0,0,0, 1, 1200},
60 {0,0,0,0,0, 1, 3000},
61 {0,0,0,0,0, 1, 6000},
62 {0,0,0,0,0, 1, 10000}
63 }};
64
65
66 template <std::size_t channels, typename Tbuf>
EQFilter(Tbuf & buf,const std::array<EQBANDSETTINGS,MAX_EQ_BANDS> & bands,std::array<std::array<EQBANDSTATE,MAX_EQ_BANDS>,MAX_EQ_CHANNELS> & states)67 static void EQFilter(Tbuf & buf, const std::array<EQBANDSETTINGS, MAX_EQ_BANDS> &bands, std::array<std::array<EQBANDSTATE, MAX_EQ_BANDS>, MAX_EQ_CHANNELS> &states)
68 {
69 for(std::size_t frame = 0; frame < buf.size_frames(); ++frame)
70 {
71 for(std::size_t channel = 0; channel < channels; ++channel)
72 {
73 float sample = mix_sample_cast<float>(buf(channel, frame));
74 for(std::size_t b = 0; b < std::size(bands); ++b)
75 {
76 const EQBANDSETTINGS &band = bands[b];
77 if(band.Gain != 1.0f)
78 {
79 EQBANDSTATE &bandState = states[channel][b];
80 float x = sample;
81 float y = band.a1 * bandState.x1 + band.a2 * bandState.x2 + band.a0 * x + band.b1 * bandState.y1 + band.b2 * bandState.y2;
82 bandState.x2 = bandState.x1;
83 bandState.y2 = bandState.y1;
84 bandState.x1 = x;
85 bandState.y1 = y;
86 sample = y;
87 }
88 }
89 buf(channel, frame) = mix_sample_cast<typename Tbuf::sample_type>(sample);
90 }
91 }
92 }
93
94
95 template <typename TMixSample>
ProcessTemplate(TMixSample * frontBuffer,TMixSample * rearBuffer,std::size_t countFrames,std::size_t numChannels)96 void CEQ::ProcessTemplate(TMixSample *frontBuffer, TMixSample *rearBuffer, std::size_t countFrames, std::size_t numChannels)
97 {
98 #if defined(MPT_ENABLE_ARCH_INTRINSICS_SSE)
99 unsigned int old_csr = 0;
100 if(CPU::HasFeatureSet(CPU::feature::sse))
101 {
102 old_csr = _mm_getcsr();
103 _mm_setcsr((old_csr & ~(_MM_DENORMALS_ZERO_MASK | _MM_FLUSH_ZERO_MASK)) | _MM_DENORMALS_ZERO_ON | _MM_FLUSH_ZERO_ON);
104 }
105 #endif
106 if(numChannels == 1)
107 {
108 mpt::audio_span_interleaved<TMixSample> buf{ frontBuffer, 1, countFrames };
109 EQFilter<1>(buf, m_Bands, m_ChannelState);
110 } else if(numChannels == 2)
111 {
112 mpt::audio_span_interleaved<TMixSample> buf{ frontBuffer, 2, countFrames };
113 EQFilter<2>(buf, m_Bands, m_ChannelState);
114 } else if(numChannels == 4)
115 {
116 std::array<TMixSample*, 4> buffers = { &frontBuffer[0], &frontBuffer[1], &rearBuffer[0], &rearBuffer[1] };
117 mpt::audio_span_planar_strided<TMixSample> buf{ buffers.data(), 4, countFrames, 2 };
118 EQFilter<4>(buf, m_Bands, m_ChannelState);
119 }
120 #if defined(MPT_ENABLE_ARCH_INTRINSICS_SSE)
121 if(CPU::HasFeatureSet(CPU::feature::sse))
122 {
123 _mm_setcsr(old_csr);
124 }
125 #endif
126 }
127
128
Process(MixSampleInt * frontBuffer,MixSampleInt * rearBuffer,std::size_t countFrames,std::size_t numChannels)129 void CEQ::Process(MixSampleInt *frontBuffer, MixSampleInt *rearBuffer, std::size_t countFrames, std::size_t numChannels)
130 {
131 ProcessTemplate<MixSampleInt>(frontBuffer, rearBuffer, countFrames, numChannels);
132 }
133
134
Process(MixSampleFloat * frontBuffer,MixSampleFloat * rearBuffer,std::size_t countFrames,std::size_t numChannels)135 void CEQ::Process(MixSampleFloat *frontBuffer, MixSampleFloat *rearBuffer, std::size_t countFrames, std::size_t numChannels)
136 {
137 ProcessTemplate<MixSampleFloat>(frontBuffer, rearBuffer, countFrames, numChannels);
138 }
139
140
CEQ()141 CEQ::CEQ()
142 : m_Bands(gEQDefaults)
143 {
144 return;
145 }
146
147
Initialize(bool bReset,uint32 MixingFreq)148 void CEQ::Initialize(bool bReset, uint32 MixingFreq)
149 {
150 float fMixingFreq = static_cast<float>(MixingFreq);
151 // Gain = 0.5 (-6dB) .. 2 (+6dB)
152 for(std::size_t band = 0; band < MAX_EQ_BANDS; ++band)
153 {
154 float k, k2, r, f;
155 float v0, v1;
156 bool b = bReset;
157 f = m_Bands[band].CenterFrequency / fMixingFreq;
158 if(f > 0.45f)
159 {
160 m_Bands[band].Gain = 1.0f;
161 }
162 k = f * mpt::numbers::pi_v<float>;
163 k = k + k*f;
164 k2 = k*k;
165 v0 = m_Bands[band].Gain;
166 v1 = 1;
167 if(m_Bands[band].Gain < 1.0f)
168 {
169 v0 *= (0.5f/EQ_BANDWIDTH);
170 v1 *= (0.5f/EQ_BANDWIDTH);
171 } else
172 {
173 v0 *= (1.0f/EQ_BANDWIDTH);
174 v1 *= (1.0f/EQ_BANDWIDTH);
175 }
176 r = (1 + v0*k + k2) / (1 + v1*k + k2);
177 if(r != m_Bands[band].a0)
178 {
179 m_Bands[band].a0 = r;
180 b = true;
181 }
182 r = 2 * (k2 - 1) / (1 + v1*k + k2);
183 if(r != m_Bands[band].a1)
184 {
185 m_Bands[band].a1 = r;
186 b = true;
187 }
188 r = (1 - v0*k + k2) / (1 + v1*k + k2);
189 if(r != m_Bands[band].a2)
190 {
191 m_Bands[band].a2 = r;
192 b = true;
193 }
194 r = - 2 * (k2 - 1) / (1 + v1*k + k2);
195 if(r != m_Bands[band].b1)
196 {
197 m_Bands[band].b1 = r;
198 b = true;
199 }
200 r = - (1 - v1*k + k2) / (1 + v1*k + k2);
201 if(r != m_Bands[band].b2)
202 {
203 m_Bands[band].b2 = r;
204 b = true;
205 }
206 if(b)
207 {
208 for(std::size_t channel = 0; channel < MAX_EQ_CHANNELS; ++channel)
209 {
210 m_ChannelState[channel][band] = EQBANDSTATE{};
211 }
212 }
213 }
214 }
215
216
SetEQGains(const uint32 * pGains,const uint32 * pFreqs,bool bReset,uint32 MixingFreq)217 void CEQ::SetEQGains(const uint32 *pGains, const uint32 *pFreqs, bool bReset, uint32 MixingFreq)
218 {
219 for(std::size_t i = 0; i < MAX_EQ_BANDS; ++i)
220 {
221 m_Bands[i].Gain = static_cast<float>(gEqLinearToDB[std::clamp(pGains[i], static_cast<uint32>(0), static_cast<uint32>(std::size(gEqLinearToDB) - 1))]) / 64.0f;
222 m_Bands[i].CenterFrequency = static_cast<float>(pFreqs[i]);
223 }
224 Initialize(bReset, MixingFreq);
225 }
226
227
228 #else
229
230
231 MPT_MSVC_WORKAROUND_LNK4221(EQ)
232
233
234 #endif // !NO_EQ
235
236
237 OPENMPT_NAMESPACE_END
238