From d216ec6c4211eeb0995b25171f7fa36bf77d9fb9 Mon Sep 17 00:00:00 2001 From: Padraig O Conbhui Date: Sat, 11 Jun 2016 22:12:07 +0100 Subject: [PATCH] Basig audio working. Many kinks. --- ds4drv/__main__.py | 35 ++- ds4drv/actions/__init__.py | 1 + ds4drv/actions/audio.py | 128 +++++++++++ ds4drv/audio/__init__.py | 2 + ds4drv/audio/gst_pulse_to_sbc_pipeline.py | 218 ++++++++++++++++++ ds4drv/audio/sbc_headers.py | 266 ++++++++++++++++++++++ ds4drv/backends/hidraw.py | 69 +++++- ds4drv/device.py | 28 ++- setup.py | 3 +- 9 files changed, 734 insertions(+), 16 deletions(-) create mode 100644 ds4drv/actions/audio.py create mode 100644 ds4drv/audio/__init__.py create mode 100644 ds4drv/audio/gst_pulse_to_sbc_pipeline.py create mode 100644 ds4drv/audio/sbc_headers.py diff --git a/ds4drv/__main__.py b/ds4drv/__main__.py index fa915a2..6ed1d36 100644 --- a/ds4drv/__main__.py +++ b/ds4drv/__main__.py @@ -9,10 +9,11 @@ from .daemon import Daemon from .eventloop import EventLoop from .exceptions import BackendError +from .audio import GstPulseToSBCPipeline class DS4Controller(object): - def __init__(self, index, options, dynamic=False): + def __init__(self, index, options, audio_pipeline, dynamic=False): self.index = index self.dynamic = dynamic self.logger = Daemon.logger.new_module("controller {0}".format(index)) @@ -33,6 +34,8 @@ def __init__(self, index, options, dynamic=False): if self.profiles: self.profiles.append("default") + self.audio_pipeline = audio_pipeline + self.load_options(self.options) def fire_event(self, event, *args): @@ -120,8 +123,10 @@ def exit(self, *args, error = True): self.logger.info(*args) -def create_controller_thread(index, controller_options, dynamic=False): - controller = DS4Controller(index, controller_options, dynamic=dynamic) +def create_controller_thread(index, controller_options, audio_pipeline, + dynamic=False): + controller = DS4Controller(index, controller_options, audio_pipeline, + dynamic=dynamic) thread = Thread(target=controller.run) thread.controller = controller @@ -131,8 +136,9 @@ def create_controller_thread(index, controller_options, dynamic=False): class SigintHandler(object): - def __init__(self, threads): + def __init__(self, threads, audio_pipeline): self.threads = threads + self.audio_pipeline = audio_pipeline def cleanup_controller_threads(self): for thread in self.threads: @@ -140,17 +146,23 @@ def cleanup_controller_threads(self): thread.controller.loop.stop() thread.join() + def cleanup_audio_pipeline(self): + self.audio_pipeline.stop() + def __call__(self, signum, frame): signal.signal(signum, signal.SIG_DFL) + self.cleanup_audio_pipeline() self.cleanup_controller_threads() + sys.exit(0) def main(): threads = [] + audio_pipeline = GstPulseToSBCPipeline() - sigint_handler = SigintHandler(threads) + sigint_handler = SigintHandler(threads, audio_pipeline) signal.signal(signal.SIGINT, sigint_handler) try: @@ -172,10 +184,20 @@ def main(): Daemon.fork(options.daemon_log, options.daemon_pid) for index, controller_options in enumerate(options.controllers): - thread = create_controller_thread(index + 1, controller_options) + thread = create_controller_thread( + index + 1, controller_options, audio_pipeline + ) threads.append(thread) + audio_pipeline.start() + for device in backend.devices: + print("-----") + from multiprocessing import Pool + p = Pool(processes=1) + def f(d): + print("f:", d) + p.apply_async(f, (device,)) connected_devices = [] for thread in threads: # Controller has received a fatal error, exit @@ -199,6 +221,7 @@ def main(): else: thread = create_controller_thread(len(threads) + 1, options.default_controller, + audio_pipeline, dynamic=True) threads.append(thread) diff --git a/ds4drv/actions/__init__.py b/ds4drv/actions/__init__.py index 2d37a0c..65ccab9 100644 --- a/ds4drv/actions/__init__.py +++ b/ds4drv/actions/__init__.py @@ -7,3 +7,4 @@ from . import input from . import led from . import status +from . import audio diff --git a/ds4drv/actions/audio.py b/ds4drv/actions/audio.py new file mode 100644 index 0000000..1d95363 --- /dev/null +++ b/ds4drv/actions/audio.py @@ -0,0 +1,128 @@ +from ..action import Action +from ..audio import SBCHeaders + + +class AudioCallbacks(): + callbacks = [] + + def __call__(self, buffer, data): + import os + #print("cbpid: ", os.getpid()) + for callback in self.callbacks: + callback(data) + +import os +from io import FileIO +hidraw_device = "/dev/hidraw3" +report_fd = os.open(hidraw_device, os.O_RDWR | os.O_NONBLOCK) +fd = FileIO(report_fd, "rb+", closefd=False) +class AudioAction(Action): + """Plays audio through the device""" + + frame_number = 0 + audio_buffer = b'' + + def setup(self, device): + self.audio_pipeline = self.controller.audio_pipeline + + if not isinstance(self.audio_pipeline.get_callback(), AudioCallbacks): + self.audio_pipeline.set_callback(AudioCallbacks()) + + self.audio_pipeline.get_callback().callbacks.append(self.play_audio) + + self.audio_pipeline.restart() + + def play_audio(self, data): + pos = 0 + sbc_headers = SBCHeaders() + + #print() + #print("ld: ", len(data)) + import os + #print("lpid:", os.getpid()) + while pos != len(data): + sbc_headers.parse_header(data) + frame_length = sbc_headers.calculate_frame_length() + #print("fl: ", frame_length) + + self.controller.device.play_audio(sbc_headers, + data[pos:pos + frame_length]) + #self.lplay_audio(None, data[pos:(pos+frame_length)]) + + pos += frame_length + #print("done") + + def lplay_audio(self, sbc_headers, data): + print() + print(len(self.audio_buffer)) + print(len(data)) + print() + if len(self.audio_buffer) + len(data) <= 448: + self.audio_buffer += data + return + print("running: ", len(self.audio_buffer)) + + rumble_weak = 0 + rumble_strong = 0 + r = 0 + g = 0 + b = 10 + crc = b'\x00\x00\x00\x00' + volume_speaker = 80 + volume_l = 60 + volume_r = 60 + unk2 = 100 + unk3 = 100 + flash_bright = 0 + flash_dark = 0 + #audio_header = b'\x24' + audio_header = b'\x24' + + + def frame_number(inc): + import struct + res = struct.pack(" 0xffff: + self.frame_number = 0 + return res + + def joy_data(): + data = [0xff,0x4,0x00] + #global volume_r,volume_unk2, unk3 + data.extend([rumble_weak,rumble_strong,r,g,b,flash_bright,flash_dark]) + data.extend([0]*8) + data.extend([volume_l,volume_r,unk2,volume_speaker,unk3]) + return data + + + def _11_report(): + data = joy_data() + data.extend([0]*(48)) + return b'\x11\xC0\x20' + bytearray(data) + crc + + try: + if self.reported_11 == True: pass + except AttributeError: + fd.write(_11_report()) + + def _17_report(audo_data): + return ( + b'\x17\x40\xA0' + + frame_number(4) + + audio_header + + audo_data + + bytearray(452 - len(audo_data)) + crc + ) + report = _17_report(self.audio_buffer) + + print(data[0]) + self.audio_buffer = data + + #if self._volume_r == 0: + # self.set_volume(60, 60, 0) + # self._control() + #self.write_report(report[0], report[1:]) + print(report) + print(len(report)) + fd.write(report) diff --git a/ds4drv/audio/__init__.py b/ds4drv/audio/__init__.py new file mode 100644 index 0000000..37c5d98 --- /dev/null +++ b/ds4drv/audio/__init__.py @@ -0,0 +1,2 @@ +from .sbc_headers import SBCHeaders +from .gst_pulse_to_sbc_pipeline import GstPulseToSBCPipeline diff --git a/ds4drv/audio/gst_pulse_to_sbc_pipeline.py b/ds4drv/audio/gst_pulse_to_sbc_pipeline.py new file mode 100644 index 0000000..ee31074 --- /dev/null +++ b/ds4drv/audio/gst_pulse_to_sbc_pipeline.py @@ -0,0 +1,218 @@ +import gi +gi.require_version('Gst', '1.0') +gi.require_version('GstBase', '1.0') +gi.require_version('Gtk', '3.0') +from gi.repository import Gst, GstBase, GObject, Gtk + +from multiprocessing import Process, Event +from threading import Thread, Event as tEvent +import subprocess + +from .sbc_headers import SBCHeaders + +Gst.init(None) +GObject.threads_init() + +class CallbackSink(GstBase.BaseSink): + __gstmetadata__ = ( + 'CustomSink', 'Sink', 'custom test sink element', 'poconbhui' + ) + __gsttemplates__ = Gst.PadTemplate.new( + 'sink', + Gst.PadDirection.SINK, + Gst.PadPresence.ALWAYS, + Gst.Caps.new_any() + ) + + def __init__(self, callback = None, *args, **kwargs): + super().__init__(*args, **kwargs) + + self.set_callback(callback) + + def do_render(self, buffer): + data = buffer.extract_dup(0, buffer.get_size()) + self.callback(buffer, data) + return Gst.FlowReturn.OK + + def set_callback(self, callback): + if callback == None: + self.callback = lambda b, d: None + else: + self.callback = callback + + def get_callback(self): + return self.callback + + +class ProcessWithWatcher(object): + def __init__(self, target = None): + self.target = target + self.process = Process(target = target) + self.end_watch = tEvent() + + def watch_target(self): + while self.end_watch.is_set() != True: + self.process.join() + self.process = Process(target = self.target) + self.process.start() + + def process(self): + return self.process + + def start(self): + self.process.start() + self.start_watch() + + def join(self): + self.stop_watch() + self.process.join() + + def start_watch(self): + self.watchman = Thread(target = self.watch_target) + self.end_watch.clear() + self.watchman.start() + + def stop_watch(self): + self.end_watch.set() + + +class GstPulseToSBCPipeline(object): + def __init__(self, pulse_sink_name='ds4'): + self.pulse_sink_name = pulse_sink_name + self.sink = CallbackSink() + self.gtk_quit_main = Event() + + def run(self): + import os + print("fpsp:", os.getpid()) + + self.player = Gst.Pipeline.new('player') + + + self.pulse_source = Gst.ElementFactory.make( + 'pulsesrc', instance_name='pulse-source' + ) + self.pulse_source.set_property( + 'device', self.pulse_sink_name + '.monitor' + ) + + self.player.add(self.pulse_source) + + + self.pulse_buffer = Gst.ElementFactory.make( + 'queue', 'pulse-buffer' + ) + + self.player.add(self.pulse_buffer) + self.pulse_source.link(self.pulse_buffer) + + + self.pulse_resampler = Gst.ElementFactory.make( + 'audioresample', 'pulse-resampler' + ) + + self.player.add(self.pulse_resampler) + self.pulse_buffer.link(self.pulse_resampler) + + + self.pulse_resampler_caps = Gst.ElementFactory.make( + 'capsfilter', 'pulse-resampler-caps' + ) + self.pulse_resampler_caps.set_property( + 'caps', Gst.Caps.from_string("audio/x-raw, rate=32000") + ) + + self.player.add(self.pulse_resampler_caps) + self.pulse_resampler.link(self.pulse_resampler_caps) + + + self.sbc_encoder = Gst.ElementFactory.make( + 'sbcenc', 'sbc-encoder' + ) + + self.player.add(self.sbc_encoder) + self.pulse_resampler_caps.link(self.sbc_encoder) + + + self.sbc_encoder_caps = Gst.ElementFactory.make( + 'capsfilter', 'sbc-encoder-caps' + ) + self.sbc_encoder_caps.set_property( + 'caps', Gst.Caps.from_string(SBCHeaders().gst_sbc_caps()) + ) + + self.player.add(self.sbc_encoder_caps) + self.sbc_encoder.link(self.sbc_encoder_caps) + + + self.sbc_encoder_buffer = Gst.ElementFactory.make( + 'queue', 'sbc-encoder-buffer' + ) + + self.player.add(self.sbc_encoder_buffer) + self.sbc_encoder_caps.link(self.sbc_encoder_buffer) + + + self.player.add(self.sink) + self.sbc_encoder_buffer.link(self.sink) + + + + self.player.set_state(Gst.State.PLAYING) + + + print("here: ", self.gtk_quit_main.is_set()) + while self.gtk_quit_main.is_set() != True: + Gtk.main_iteration_do(False) + print("Ran gtk main") + + + def start(self): + # Create pulse source + pulse_sink_id = subprocess.check_output([ + 'pactl', 'load-module', 'module-null-sink', + 'sink_name="{}"'.format(self.pulse_sink_name), + 'sink_properties=device.description="{}"'.format( + "DualShock\ 4" + ), + ]) + self.pulse_sink_id = int(pulse_sink_id) + print("pulse sink id:", pulse_sink_id) + + import os + print("sis:", os.getpid()) + self.gst_process = ProcessWithWatcher(target = self.run) + self.gst_process.start() + print("sgsp:", self.gst_process) + + + def stop(self): + + import os + print("qid:", os.getpid()) + print("qgsp:", self.gst_process) + self.gtk_quit_main.set() + print("qran") + self.gst_process.join() + print("qgst_joined") + + subprocess.check_output([ + 'pactl', 'unload-module', str(self.pulse_sink_id) + ]) + + def restart(self): + print("starting restart") + self.gtk_quit_main.set() + print("a: ", self.gtk_quit_main.is_set()) + self.gst_process.join() + self.gst_process = ProcessWithWatcher(target = self.run) + self.gtk_quit_main.clear() + print("b: ", self.gtk_quit_main.is_set()) + self.gst_process.start() + print("restarted") + + def set_callback(self, callback): + self.sink.set_callback(callback) + + def get_callback(self): + return self.sink.get_callback() diff --git a/ds4drv/audio/sbc_headers.py b/ds4drv/audio/sbc_headers.py new file mode 100644 index 0000000..0c21ccb --- /dev/null +++ b/ds4drv/audio/sbc_headers.py @@ -0,0 +1,266 @@ +class SBCHeaders(object): + + MONO = 0 + DUAL_CHANNEL = 1 + STEREO = 2 + JOINT_STEREO = 3 + + + def __init__( + self, + sampling_frequency = 32000, + bitpool = 50, + channel_mode = None, + nrof_blocks = 16, + nrof_subbands = 8 + ): + if channel_mode == None: + channel_mode = SBCHeaders.STEREO + + self.syncword = 156 + + self.nrof_subbands = nrof_subbands + self.channel_mode = channel_mode + self.nrof_channels = 2 + if self.channel_mode == SBCHeaders.MONO: + self.nrof_channels = 1 + self.nrof_blocks = nrof_blocks + self.join = 0 + if self.channel_mode == SBCHeaders.JOINT_STEREO: + self.join = 1 + self.bitpool = bitpool + self.sampling_frequency = sampling_frequency + + + self.frame_length = None + self.bitrate = None + + + def calculate_frame_length(self): + # Calculate frame length + def ceildiv(a, b): + return -(-a // b) + + if ( + (self.channel_mode == SBCHeaders.MONO) + or (self.channel_mode == + SBCHeaders.DUAL_CHANNEL) + ): + + self.frame_length = ( + 4 + ( + 4 + * self.nrof_subbands + * self.nrof_channels + )//8 + + ceildiv( + self.nrof_blocks + * self.nrof_channels + * self.bitpool, + 8 + ) + ) + else: + self.frame_length = ( + 4 + ( + 4 + * self.nrof_subbands + * self.nrof_channels + )//8 + + ceildiv( + self.join + * self.nrof_subbands + + self.nrof_blocks + * self.bitpool, + 8 + ) + ) + + return self.frame_length + + + def calculate_bit_rate(self): + if self.frame_length == None: + self.calculate_frame_length() + + # Calculate bit rate + self.bit_rate = ( + 8 * self.frame_length * self.sampling_frequency + // self.nrof_subbands // self.nrof_blocks + ) + + return self.bit_rate + + + def gst_sbc_caps(self): + + if self.channel_mode == SBCHeaders.MONO: + channel_mode_str = "mono" + elif self.channel_mode == SBCHeaders.DUAL_CHANNEL: + channel_mode_str = "dual" + elif self.channel_mode == SBCHeaders.STEREO: + channel_mode_str = "stereo" + elif self.channel_mode == SBCHeaders.JOINT_STEREO: + channel_mode_str = "joint" + + return ( + 'audio/x-sbc, ' + + 'channels=' + str(self.nrof_channels) + ', ' + + 'rate=' + str(self.sampling_frequency) + ', ' + + 'channel-mode=' + channel_mode_str + ', ' + + 'blocks=' + str(self.nrof_blocks) + ', ' + + 'subbands=' + str(self.nrof_subbands) + ', ' + + 'bitpool=' + str(self.bitpool) + ) + + + def parse_header(self, raw_header): + # Info in SBC headers from + # https://tools.ietf.org/html/draft-ietf-avt-rtp-sbc-01#section-6.3 + + # Syncword should be 0x9C + self.syncword = raw_header[0] + + self.nrof_subbands = \ + SBCHeaders.parse_number_of_subbands( + raw_header + ) + self.channel_mode = SBCHeaders.parse_channel_mode( + raw_header + ) + self.nrof_channels = 2 + if self.channel_mode == SBCHeaders.MONO: + self.nrof_channels = 1 + self.nrof_blocks = SBCHeaders.parse_number_of_blocks( + raw_header + ) + self.join = 0 + if self.channel_mode == SBCHeaders.JOINT_STEREO: + self.join = 1 + self.nrof_subbands = \ + SBCHeaders.parse_number_of_subbands( + raw_header + ) + self.bitpool = SBCHeaders.parse_bitpool(raw_header) + self.sampling_frequency = SBCHeaders.parse_sampling( + raw_header + ) + + + def print_values(self): + # Info in SBC headers from + # https://tools.ietf.org/html/draft-ietf-avt-rtp-sbc-01#section-6.3 + + print("syncword: ", self.syncword) + + print("nrof_subbands", self.nrof_subbands) + print("channel_mode", [ + "MONO", "DUAL_CHANNEL", "STEREO", "JOINT_STEREO" + ][self.channel_mode] + ) + print("nrof_channels", self.nrof_channels) + print("nrof_blocks", self.nrof_blocks) + print("join: ", self.join) + print("nrof_subbands", self.nrof_subbands) + print("bitpool", self.bitpool) + print("sampling_frequency", self.sampling_frequency) + print("frame_length", self.frame_length) + print("bit_rate", self.bit_rate) + + + @staticmethod + def parse_sampling(raw_header): + + sf_word = raw_header[1] + + # Find sampling frequency from rightmost 2 bits + if sf_word & 0x80 == 0x80: + bit_0 = 1 + else: + bit_0 = 0 + + if sf_word & 0x40 == 0x40: + bit_1 = 1 + else: + bit_1 = 0 + + if (bit_0 == 0) and (bit_1 == 0): + sampling_frequency = 16000 + elif (bit_0 == 0) and (bit_1 == 1): + sampling_frequency = 32000 + elif (bit_0 == 1) and (bit_1 == 0): + sampling_frequency = 44100 + elif (bit_0 == 1) and (bit_1 == 1): + sampling_frequency = 48000 + + return sampling_frequency + + + @staticmethod + def parse_number_of_blocks(raw_header): + + nb_word = raw_header[1] + + if nb_word & 0x20 == 0x20: + bit_0 = 1 + else: + bit_0 = 0 + + if nb_word & 0x10 == 0x10: + bit_1 = 1 + else: + bit_1 = 0 + + + if (bit_0 == 0) and (bit_1 == 0): + number_of_blocks = 4 + elif (bit_0 == 0) and (bit_1 == 1): + number_of_blocks = 8 + elif (bit_0 == 1) and (bit_1 == 0): + number_of_blocks = 12 + elif (bit_0 == 1) and (bit_1 == 1): + number_of_blocks = 16 + + return number_of_blocks + + + @staticmethod + def parse_channel_mode(raw_header): + + ch_word = raw_header[1] + + if ch_word & 0x08 == 0x08: + bit_0 = 1 + else: + bit_0 = 0 + + if ch_word & 0x04 == 0x04: + bit_1 = 1 + else: + bit_1 = 0 + + if (bit_0 == 0) and (bit_1 == 0): + channel_mode = SBCHeaders.MONO + elif (bit_0 == 0) and (bit_1 == 1): + channel_mode = SBCHeaders.DUAL_CHANNEL + elif (bit_0 == 1) and (bit_1 == 0): + channel_mode = SBCHeaders.STEREO + elif (bit_0 == 1) and (bit_1 == 1): + channel_mode = SBCHeaders.JOINT_STEREO + + return channel_mode + + + @staticmethod + def parse_number_of_subbands(raw_header): + if raw_header[1] & 0x01 == 0x01: + number_of_subbands = 8 + else: + number_of_subbands = 4 + + return number_of_subbands + + + @staticmethod + def parse_bitpool(raw_header): + return int(raw_header[2]) diff --git a/ds4drv/backends/hidraw.py b/ds4drv/backends/hidraw.py index ae1cd04..3ada2ef 100644 --- a/ds4drv/backends/hidraw.py +++ b/ds4drv/backends/hidraw.py @@ -1,6 +1,8 @@ import fcntl import itertools import os +import struct +import signal from io import FileIO from time import sleep @@ -62,14 +64,30 @@ def read_feature_report(self, report_id, size): return fcntl.ioctl(self.fd, op, bytes(buf)) - def write_report(self, report_id, data): - if self.type == "bluetooth": - # TODO: Add a check for a kernel that supports writing - # output reports when such a kernel has been released. - return + @staticmethod + def sigalrm_handler(signum, frame): + raise TimeoutError + + def write_report(self, report_id, data, timeout = None): + #if self.type == "bluetooth": + # # TODO: Add a check for a kernel that supports writing + # # output reports when such a kernel has been released. + # return - hid = bytearray((report_id,)) - self.fd.write(hid + data) + #if timeout != None: + # old_sigalrm_handler = signal.getsignal(signal.SIGALRM) + # signal.signal(signal.SIGALRM, HidrawDS4Device.sigalrm_handler) + # signal.setitimer(self.ITIMER_REAL, timeout) + + try: + hid = bytearray((report_id,)) + self.fd.write(hid + data) + except TimeoutError: + pass + + #if timeout != None: + # signal.setitimer(self.ITIMER_REAL, 0) + # signal.signal(signal.SIGALRM, old_sigalrm_handler) def close(self): try: @@ -85,9 +103,46 @@ class HidrawBluetoothDS4Device(HidrawDS4Device): report_size = 78 valid_report_id = 0x11 + audio_buffer_size = 448 + audio_buffer = b'' + frame_number = 0 + def set_operational(self): self.read_feature_report(0x02, 37) + def increment_frame_number(self, inc): + self.frame_number += inc + if self.frame_number > 0xffff: + self.frame_number = 0 + + def play_audio(self, headers, data): + if len(self.audio_buffer) + len(data) <= self.audio_buffer_size: + self.audio_buffer += data + return + + crc = b'\x00\x00\x00\x00' + audio_header = b'\x24' + + self.increment_frame_number(4) + + report_id = 0x17 + report = ( + b'\x40\xA0' + + struct.pack("=0.3.0", "pyudev>=0.16"], classifiers=[ "Development Status :: 4 - Beta",