Add OnboardEQ (0x0636) support for Centurion headsets
Implement host-computed biquad EQ coefficient generation and multi-fragment bridge writes for the PRO X 2 LIGHTSPEED headset's 5-band parametric EQ. The coefficient algorithm uses standard Audio EQ Cookbook peaking EQ formulas with a simplified rescale normalization (max_b0 × 1.19 headroom). This is our own implementation — not an exact replica of LGHUB's ~350-line per-band cascade normalization — but it produces functionally correct results. The DSP compensates via the rescale factor, and the EQ changes are audible and working on real hardware. Wire format verified against 38 LGHUB pcap writes: - 4-byte LE section headers, LE uint16 coefficient words - Mixed Q1.31/Q2.30 fixed-point with 24-bit precision - Only b-coefficients divided by rescale; a-coefficients unchanged - Two sections: 48kHz playback + 16kHz mic - No trailing padding, no extra words between sections Changes: - base.py: Add flags parameter to write_centurion_cpl() for multi-fragment CPL - device.py: Rewrite multi-fragment bridge send — proper CPL fragmentation with fragment 0 carrying bridge prefix/hdr and continuations carrying raw sub_msg, all fragments sent back-to-back without intermediate ACKs - hidpp20.py: Replace placeholder coefficient code with full biquad math, mixed Q-format quantization, rescale normalization, and dual-section output - settings_templates.py: Persist EQ to slot 0x80 after writing to slot 0x00 so settings survive power cycle - tests: Update expected SetEQParameters payloads for new coefficient format
This commit is contained in:
Ken Sanislo
parent
0cc3b01aec
commit
14807fdf28
|
|
@ -363,17 +363,21 @@ def write(handle, devnumber, data, long_message=False):
|
|||
raise exceptions.NoReceiver(reason=reason) from reason
|
||||
|
||||
|
||||
def write_centurion_cpl(handle, layer3_payload):
|
||||
def write_centurion_cpl(handle, layer3_payload, flags=0x00):
|
||||
"""Send a Centurion CPL frame with the given Layer 3+ payload.
|
||||
|
||||
Builds: [0x51, cpl_length, flags=0x00, layer3_payload..., zero-pad to 64 bytes]
|
||||
Builds: [0x51, cpl_length, flags, layer3_payload..., zero-pad to 64 bytes]
|
||||
where cpl_length = len(layer3_payload) + 1 (the +1 counts the flags byte).
|
||||
|
||||
For multi-fragment sends, flags encodes fragment index and continuation:
|
||||
flags = (fragment_index << 1) | (1 if more_fragments else 0)
|
||||
Single-frame messages use flags=0x00 (default).
|
||||
"""
|
||||
ihandle = int(handle)
|
||||
if ihandle not in _centurion_handles:
|
||||
raise ValueError("write_centurion_cpl called on non-Centurion handle")
|
||||
cpl_length = len(layer3_payload) + 1 # +1 for flags byte
|
||||
wdata = struct.pack("!BBB", CENTURION_REPORT_ID, cpl_length, 0x00) + layer3_payload
|
||||
wdata = struct.pack("!BBB", CENTURION_REPORT_ID, cpl_length, flags) + layer3_payload
|
||||
wdata = wdata + b"\x00" * (CENTURION_FRAME_SIZE - len(wdata))
|
||||
if logger.isEnabledFor(logging.DEBUG):
|
||||
logger.debug("(%s) <= centurion_cpl[%s]", handle, common.strhex(wdata[: cpl_length + 2]))
|
||||
|
|
|
|||
|
|
@ -914,21 +914,28 @@ class Device:
|
|||
return self.centurion_bridge_request(sub_idx, function, *params, no_reply=no_reply)
|
||||
return hidpp20.feature_request(self, feature, function, *params, no_reply=no_reply)
|
||||
|
||||
# Maximum sub-message bytes per bridge frame: 64 - 1 (report ID) - 2 (CPL) - 4 (bridge hdr) = 57
|
||||
_MAX_BRIDGE_CHUNK = 57
|
||||
# Max sub-message bytes in the first bridge fragment:
|
||||
# 64 - 1 (report ID) - 1 (cpl_len) - 1 (flags) - 2 (bridge prefix) - 2 (bridge hdr) = 57
|
||||
# LGHUB uses 56 for first fragment (60 byte payload - 4 bridge overhead)
|
||||
_BRIDGE_FIRST_CHUNK = 56
|
||||
# Continuation fragments carry raw sub_msg data (no bridge prefix/hdr):
|
||||
# 64 - 1 (report ID) - 1 (cpl_len) - 1 (flags) = 61, but LGHUB uses 60
|
||||
_BRIDGE_CONT_CHUNK = 60
|
||||
|
||||
def centurion_bridge_request(self, sub_feat_idx, sub_function=0x00, *params, no_reply=False):
|
||||
"""Send a request to a Centurion sub-device via CentPPBridge.
|
||||
|
||||
Builds the 4-layer nested message:
|
||||
Layer 1: [0x51]
|
||||
Layer 2: [cpl_length, flags=0x00]
|
||||
Layer 2: [cpl_length, flags]
|
||||
Layer 3: [bridge_idx, sendFragment_func|swid, bridge_hdr...]
|
||||
Layer 4: [sub_cpl=0x00, sub_feat_idx, sub_func|swid, params...]
|
||||
|
||||
When sub_msg exceeds 57 bytes, splits into multiple fragments.
|
||||
Each fragment carries the same bridge_hdr (total sub_len).
|
||||
The device accumulates until complete.
|
||||
For multi-fragment sends, only the first fragment includes the bridge
|
||||
prefix and header. Continuation fragments carry raw sub_msg data.
|
||||
The CPL flags byte encodes fragment index and continuation:
|
||||
flags = (fragment_index << 1) | (1 if more_fragments else 0)
|
||||
Single-frame messages use flags=0x00.
|
||||
|
||||
Returns the sub-device response data (after bridge header), or None.
|
||||
"""
|
||||
|
|
@ -956,21 +963,35 @@ class Device:
|
|||
|
||||
timeout = base.DEFAULT_TIMEOUT
|
||||
with base.acquire_timeout(base.handle_lock(handle), handle, timeout):
|
||||
if sub_len <= self._MAX_BRIDGE_CHUNK:
|
||||
if sub_len <= self._BRIDGE_FIRST_CHUNK:
|
||||
# Single-frame path
|
||||
layer3 = bridge_prefix + bridge_hdr + sub_msg
|
||||
base.write_centurion_cpl(handle, layer3)
|
||||
else:
|
||||
# Multi-fragment send: split sub_msg into chunks
|
||||
for offset in range(0, sub_len, self._MAX_BRIDGE_CHUNK):
|
||||
chunk = sub_msg[offset : offset + self._MAX_BRIDGE_CHUNK]
|
||||
layer3 = bridge_prefix + bridge_hdr + chunk
|
||||
base.write_centurion_cpl(handle, layer3)
|
||||
# Wait for ACK between fragments (not after the last one)
|
||||
if offset + self._MAX_BRIDGE_CHUNK < sub_len:
|
||||
if not self._wait_for_bridge_ack(handle, bridge_idx, sw_id, timeout):
|
||||
logger.warning("centurion_bridge_request: no ACK for fragment at offset %d", offset)
|
||||
return None
|
||||
# Multi-fragment send
|
||||
# Fragment 0: bridge_prefix + bridge_hdr + first chunk of sub_msg
|
||||
# Fragments 1+: raw sub_msg continuation data (no bridge overhead)
|
||||
# CPL flags = (frag_index << 1) | (1 if more_fragments else 0)
|
||||
# All fragments are sent back-to-back without waiting for
|
||||
# intermediate ACKs (verified via LGHUB pcap). The device
|
||||
# reassembles internally and sends a single ACK + MessageEvent
|
||||
# after the last fragment.
|
||||
frag_index = 0
|
||||
offset = 0
|
||||
while offset < sub_len:
|
||||
if frag_index == 0:
|
||||
chunk_size = self._BRIDGE_FIRST_CHUNK
|
||||
chunk = sub_msg[offset : offset + chunk_size]
|
||||
layer3 = bridge_prefix + bridge_hdr + chunk
|
||||
else:
|
||||
chunk_size = self._BRIDGE_CONT_CHUNK
|
||||
chunk = sub_msg[offset : offset + chunk_size]
|
||||
layer3 = chunk
|
||||
has_more = (offset + chunk_size) < sub_len
|
||||
flags = (frag_index << 1) | (1 if has_more else 0)
|
||||
base.write_centurion_cpl(handle, layer3, flags=flags)
|
||||
offset += len(chunk)
|
||||
frag_index += 1
|
||||
|
||||
if no_reply:
|
||||
return None
|
||||
|
|
|
|||
|
|
@ -2402,6 +2402,13 @@ class ForceSensingButtonArray(UserDict):
|
|||
|
||||
# --- OnboardEQ (0x0636) biquad coefficient math and helpers ---
|
||||
|
||||
# Mystery bytes observed in every LGHUB pcap EQ write between band params
|
||||
# and coefficient header. Purpose not fully understood — possibly a null-band
|
||||
# terminator for DSPs that support >5 bands (advanced 10-band mode).
|
||||
# First byte matches band_count; bytes 2-3 look like LE16 coeff blob size.
|
||||
# Hardcoded from pcap for initial bring-up; revisit once device-tested.
|
||||
_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).
|
||||
|
|
@ -2422,34 +2429,77 @@ def _peaking_eq_biquad(freq_hz, gain_db, Q, sample_rate=48000.0):
|
|||
)
|
||||
|
||||
|
||||
def _float_to_q626(value):
|
||||
"""Convert a float to Q6.26 fixed-point unsigned 32-bit representation."""
|
||||
scaled = max(-(1 << 31), min((1 << 31) - 1, int(round(value * (1 << 26)))))
|
||||
return scaled & 0xFFFFFFFF
|
||||
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 (matches LGHUB)
|
||||
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 biquad coefficient blob for SetEQParameters.
|
||||
"""Build the full EQCoeffs wire payload for SetEQParameters.
|
||||
|
||||
bands: list of (freq_hz, gain_db, Q) tuples.
|
||||
Returns bytes containing header + section + per-band coefficients + global gain.
|
||||
Two coefficient sections: type=1 (48 kHz playback) and type=2 (16 kHz mic).
|
||||
Returns bytes: 7-byte header + sections (no trailing padding).
|
||||
"""
|
||||
num_bands = len(bands)
|
||||
# 7-byte header
|
||||
payload = bytes([0x03, 0x0E, 0x00, 0x01, 0x00, 0x00, 0x00])
|
||||
# Section header: filter_type=0x01, reserved=0x00, then coeff count as uint16 LE
|
||||
total_words = num_bands * 10 + 3 # 5 coeffs * 2 words each per band, plus 2 global gain words + 1 count
|
||||
payload += struct.pack("<BBH", 0x01, 0x00, total_words)
|
||||
# Per-band: 5 coefficients, each as Q6.26 split into 2 uint16 LE words
|
||||
for freq, gain, Q in bands:
|
||||
coeffs = _peaking_eq_biquad(freq, gain, Q)
|
||||
for c in coeffs:
|
||||
q626 = _float_to_q626(c)
|
||||
payload += struct.pack("<HH", q626 & 0xFFFF, (q626 >> 16) & 0xFFFF)
|
||||
# Global gain: Q6.26(1.0) = 0x04000000
|
||||
unity = _float_to_q626(1.0)
|
||||
payload += struct.pack("<HH", unity & 0xFFFF, (unity >> 16) & 0xFFFF)
|
||||
return payload
|
||||
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):
|
||||
|
|
@ -2461,6 +2511,7 @@ def _build_set_eq_payload(slot, bands):
|
|||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -1750,8 +1750,21 @@ class HeadsetOnboardEQ(settings.RangeFieldSetting):
|
|||
q = self._band_qs[i] if i < len(self._band_qs) else 10
|
||||
gain = new_values.get(i, 0)
|
||||
bands.append((freq, gain, q))
|
||||
self._pending_bands = bands # stash for persist step
|
||||
return hidpp20._build_set_eq_payload(0x00, bands)
|
||||
|
||||
def write(self, map, save=True):
|
||||
result = super().write(map, save)
|
||||
# Also persist to device flash (slot 0x80) so EQ survives power cycle
|
||||
if result is not None and hasattr(self._validator, "_pending_bands"):
|
||||
bands = self._validator._pending_bands
|
||||
del self._validator._pending_bands
|
||||
try:
|
||||
self._device.feature_request(_F.HEADSET_ONBOARD_EQ, 0x20, hidpp20._build_set_eq_payload(0x80, bands))
|
||||
except Exception:
|
||||
logger.warning("HeadsetOnboardEQ: failed to persist EQ to slot 0x80")
|
||||
return result
|
||||
|
||||
|
||||
class BrightnessControl(settings.Setting):
|
||||
name = "brightness_control"
|
||||
|
|
|
|||
|
|
@ -643,17 +643,53 @@ key_tests = [
|
|||
[-12, 12],
|
||||
fake_hidpp.Response("8000000002", 0x0400), # GetEQInfos: has_hw_eq, 2 bands
|
||||
fake_hidpp.Response("00020080FE0A0100030A", 0x0410, "00"), # GetEQParameters
|
||||
fake_hidpp.Response( # SetEQParameters: write initial values back
|
||||
fake_hidpp.Response( # SetEQParameters: write initial values back (slot 0x00)
|
||||
"00",
|
||||
0x0420,
|
||||
"00020080FE0A0100030A030E0001000000010017005FCDFF03DB3502F84C46FE03DB3502F8AA13FE03"
|
||||
"37980004E10704F8D685FC03E10704F80D1EFD0300000004",
|
||||
"00020080FE0A0100030A"
|
||||
"055AE300" # mystery bytes (from pcap)
|
||||
"030E00020000000100170002"
|
||||
"007A6B00D69D94008C516B00C82380005DC27F0075"
|
||||
"906B0022B6940002226B00B44080007EA37F00C1C3040044"
|
||||
"0200170002"
|
||||
"00506B00900F95006ED56A00D185800060477F00CA"
|
||||
"906B00229D950052496A00A12E810085EC7E0075C40400AC",
|
||||
),
|
||||
fake_hidpp.Response( # SetEQParameters: write updated band 1 gain=5
|
||||
fake_hidpp.Response( # SetEQParameters: persist initial values (slot 0x80)
|
||||
"00",
|
||||
0x0420,
|
||||
"00020080FE0A0100050A030E0001000000010017005FCDFF03DB3502F84C46FE03DB3502F8AA13FE03"
|
||||
"FAFF0004C6B703F83A6EFC03C6B703F8346EFD0300000004",
|
||||
"80020080FE0A0100030A"
|
||||
"055AE300"
|
||||
"030E00020000000100170002"
|
||||
"007A6B00D69D94008C516B00C82380005DC27F0075"
|
||||
"906B0022B6940002226B00B44080007EA37F00C1C3040044"
|
||||
"0200170002"
|
||||
"00506B00900F95006ED56A00D185800060477F00CA"
|
||||
"906B00229D950052496A00A12E810085EC7E0075C40400AC",
|
||||
),
|
||||
fake_hidpp.Response( # SetEQParameters: write updated band 1 gain=5 (slot 0x00)
|
||||
"00",
|
||||
0x0420,
|
||||
"00020080FE0A0100050A"
|
||||
"055AE300"
|
||||
"030E00020000000100170002"
|
||||
"006F6B00F5A894006A466B00EB2380005DC27F0075"
|
||||
"906B0022BC9400AA156B00613B80007CAD7F00C6C30400BF"
|
||||
"0200170002"
|
||||
"00306B00272F9500BAB56A008C85800060477F00CA"
|
||||
"906B0022B1950002226A000E1F8100AB0A7F004FC604001D",
|
||||
),
|
||||
fake_hidpp.Response( # SetEQParameters: persist updated values (slot 0x80)
|
||||
"00",
|
||||
0x0420,
|
||||
"80020080FE0A0100050A"
|
||||
"055AE300"
|
||||
"030E00020000000100170002"
|
||||
"006F6B00F5A894006A466B00EB2380005DC27F0075"
|
||||
"906B0022BC9400AA156B00613B80007CAD7F00C6C30400BF"
|
||||
"0200170002"
|
||||
"00306B00272F9500BAB56A008C85800060477F00CA"
|
||||
"906B0022B1950002226A000E1F8100AB0A7F004FC604001D",
|
||||
),
|
||||
),
|
||||
Setup(
|
||||
|
|
|
|||
Loading…
Reference in New Issue