185 lines
6.8 KiB
Python
185 lines
6.8 KiB
Python
## Copyright (C) 2012-2013 Daniel Pavel
|
|
## Copyright (C) 2014-2024 Solaar Contributors https://pwr-solaar.github.io/Solaar/
|
|
##
|
|
## This program is free software; you can redistribute it and/or modify
|
|
## it under the terms of the GNU General Public License as published by
|
|
## the Free Software Foundation; either version 2 of the License, or
|
|
## (at your option) any later version.
|
|
##
|
|
## This program is distributed in the hope that it will be useful,
|
|
## but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
## GNU General Public License for more details.
|
|
##
|
|
## You should have received a copy of the GNU General Public License along
|
|
## with this program; if not, write to the Free Software Foundation, Inc.,
|
|
## 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
|
|
|
"""OnboardEQ (0x0636) biquad coefficient math and payload builders.
|
|
|
|
Pure computation — no device or transport dependencies beyond feature_request().
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
import struct
|
|
|
|
from .hidpp20_constants import SupportedFeature
|
|
|
|
# Opaque bytes observed between band params and coefficient header. First
|
|
# byte matches band_count; bytes 2-3 look like LE16 coeff blob size. Keep
|
|
# verbatim until a device counter-example forces a re-derivation.
|
|
_EQ_MYSTERY_BYTES = b"\x05\x5a\xe3\x00"
|
|
|
|
|
|
def _peaking_eq_biquad(freq_hz, gain_db, Q, sample_rate=48000.0):
|
|
"""Compute peaking EQ biquad coefficients (Audio EQ Cookbook).
|
|
|
|
Returns (b0/a0, b1/a0, b2/a0, a1/a0, a2/a0) normalised coefficients.
|
|
"""
|
|
A = 10.0 ** (gain_db / 40.0)
|
|
w0 = 2.0 * math.pi * freq_hz / sample_rate
|
|
cos_w0 = math.cos(w0)
|
|
alpha = math.sin(w0) / (2.0 * Q)
|
|
a0 = 1.0 + alpha / A
|
|
return (
|
|
(1.0 + alpha * A) / a0,
|
|
(-2.0 * cos_w0) / a0,
|
|
(1.0 - alpha * A) / a0,
|
|
(-2.0 * cos_w0) / a0,
|
|
(1.0 - alpha / A) / a0,
|
|
)
|
|
|
|
|
|
def _quantize_coeffs(b0, b1, b2, a1, a2):
|
|
"""Quantize biquad coefficients to mixed Q1.31 / Q2.30 fixed-point.
|
|
|
|
b0, b2, a2 use Q1.31 (x 2^31); b1, a1 use Q2.30 (x 2^30).
|
|
Values are truncated to 24-bit precision (low byte zeroed) matching
|
|
the device DSP's internal format.
|
|
Returns list of 10 uint16 values (5 coefficients x 2 LE words each,
|
|
high word first).
|
|
"""
|
|
scales = [2**31, 2**30, 2**31, 2**30, 2**31] # b0, b1, b2, a1, a2
|
|
words = []
|
|
for val, scale in zip([b0, b1, b2, a1, a2], scales):
|
|
q = int(round(val * scale))
|
|
q = max(-(1 << 31), min((1 << 31) - 1, q))
|
|
q = q & 0xFFFFFF00 # 24-bit precision (low byte always zero)
|
|
words.append((q >> 16) & 0xFFFF) # high word
|
|
words.append(q & 0xFFFF) # low word
|
|
return words
|
|
|
|
|
|
def _build_coeff_section(bands, sample_rate, section_type=1):
|
|
"""Build one coefficient section for a DSP processing block.
|
|
|
|
Returns bytes: 4-byte section header + coefficient data as LE uint16 words.
|
|
Section header: [type, 0x00, count_lo, count_hi].
|
|
|
|
Coefficients are normalized by a rescale factor to prevent Q1.31 overflow.
|
|
Only feedforward coefficients (b0, b1, b2) are divided by rescale; feedback
|
|
coefficients (a1, a2) are left unchanged. The DSP multiplies the output by
|
|
rescale to restore correct gain.
|
|
"""
|
|
_HEADROOM = 1.19 # 19% headroom margin before quantization
|
|
num_bands = len(bands)
|
|
all_words = [num_bands] # first uint16 = num_bands
|
|
|
|
# First pass: compute raw biquad coefficients for all bands
|
|
raw_coeffs = []
|
|
for freq, gain, Q in bands:
|
|
raw_coeffs.append(_peaking_eq_biquad(freq, gain, max(Q, 0.1), sample_rate))
|
|
|
|
# Compute rescale: ensure max |b0| fits in Q1.31 with headroom
|
|
max_b0 = max(abs(c[0]) for c in raw_coeffs)
|
|
rescale = max(1.0, max_b0) * _HEADROOM
|
|
|
|
# Second pass: normalize b-coefficients and quantize
|
|
for b0, b1, b2, a1, a2 in raw_coeffs:
|
|
all_words.extend(_quantize_coeffs(b0 / rescale, b1 / rescale, b2 / rescale, a1, a2))
|
|
|
|
# Rescale factor as Q6.26, 24-bit precision
|
|
rs = int(round(rescale * (1 << 26)))
|
|
rs = max(-(1 << 31), min((1 << 31) - 1, rs)) & 0xFFFFFF00
|
|
all_words.append((rs >> 16) & 0xFFFF)
|
|
all_words.append(rs & 0xFFFF)
|
|
|
|
coeff_count = num_bands * 10 + 3 # num_bands word + 10 per band + 2 rescale words
|
|
hdr = bytes([section_type, 0x00, coeff_count & 0xFF, (coeff_count >> 8) & 0xFF])
|
|
data = struct.pack(f"<{len(all_words)}H", *all_words)
|
|
return hdr + data
|
|
|
|
|
|
def _build_eq_coeffs_payload(bands):
|
|
"""Build the full EQCoeffs wire payload for SetEQParameters.
|
|
|
|
Two coefficient sections: type=1 (48 kHz playback) and type=2 (16 kHz mic).
|
|
Returns bytes: 7-byte header + sections (no trailing padding).
|
|
"""
|
|
section_count = 2
|
|
header = bytes([0x03, 0x0E, 0x00, section_count, 0x00, 0x00, 0x00])
|
|
sections = _build_coeff_section(bands, 48000.0, section_type=1)
|
|
sections += _build_coeff_section(bands, 16000.0, section_type=2)
|
|
return header + sections
|
|
|
|
|
|
def _build_set_eq_payload(slot, bands):
|
|
"""Build complete SetEQParameters payload: band params + biquad coefficients.
|
|
|
|
bands: list of (freq_hz, gain_db, Q) tuples.
|
|
Returns bytes ready to send as sub-device params.
|
|
"""
|
|
params = bytes([slot, len(bands)])
|
|
for freq, gain, Q in bands:
|
|
params += struct.pack(">H", freq) + bytes([gain & 0xFF, Q & 0xFF])
|
|
params += _EQ_MYSTERY_BYTES
|
|
params += _build_eq_coeffs_payload(bands)
|
|
return params
|
|
|
|
|
|
def get_onboard_eq_info(device):
|
|
"""Query HEADSET_ONBOARD_EQ GetEQInfos (function 0).
|
|
|
|
Returns (has_hw_eq, num_bands) or None.
|
|
"""
|
|
result = device.feature_request(SupportedFeature.HEADSET_ONBOARD_EQ, 0x00)
|
|
if result is None or len(result) < 5:
|
|
return None
|
|
has_hw_eq = bool(result[0] & 0x80)
|
|
num_bands = result[4]
|
|
return (has_hw_eq, num_bands)
|
|
|
|
|
|
def get_onboard_eq_params(device, slot=0x00):
|
|
"""Query HEADSET_ONBOARD_EQ GetEQParameters (function 0x10).
|
|
|
|
Returns list of (freq_hz, gain_db, q) tuples, or None.
|
|
"""
|
|
result = device.feature_request(SupportedFeature.HEADSET_ONBOARD_EQ, 0x10, slot)
|
|
if result is None or len(result) < 2:
|
|
return None
|
|
band_count = result[1]
|
|
bands = []
|
|
offset = 2
|
|
for _i in range(band_count):
|
|
if offset + 4 > len(result):
|
|
break
|
|
freq_hz = struct.unpack(">H", result[offset : offset + 2])[0]
|
|
gain_db = struct.unpack("b", bytes([result[offset + 2]]))[0] # signed
|
|
q = result[offset + 3]
|
|
bands.append((freq_hz, gain_db, q))
|
|
offset += 4
|
|
return bands
|
|
|
|
|
|
def set_onboard_eq_params(device, bands, slot=0x00):
|
|
"""Send HEADSET_ONBOARD_EQ SetEQParameters (function 0x20).
|
|
|
|
bands: list of (freq_hz, gain_db, Q) tuples.
|
|
Returns response or None.
|
|
"""
|
|
payload = _build_set_eq_payload(slot, bands)
|
|
return device.feature_request(SupportedFeature.HEADSET_ONBOARD_EQ, 0x20, payload)
|