a2dpd.py

蓝牙通讯协议中A2DVP应用的程序

#! /usr/bin/env python
"""
Simple multi-client A2DP  server

*  Copyright (C) 2006  Sergei Krivov <[e-mail]>
*
*  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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
"""


"""
be sure to have installed:
libsbc (http://sbc.sf.net); check for /usr/lib/libsbc.so.
pybluez (http://org.csail.mit.edu/pybluez/)

put this in your .asoundrc file
pcm.a2dpcopy {
            type plug
            slave {
                  pcm "tee:default,'/tmp/a2dpipe', raw"
                  rate 48000
                  channels 2
             }
}

pcm.a2dp {
            type plug
            slave {
                  pcm "file:'/tmp/a2dpipe', raw"
                  rate 48000
                  channels 2
             }
}

make pipe by: mkfifo /tmp/a2dpipe
start by python a2dpd.py
stop by killing the process

select alsa output pcm device a2dpcopy
for mplayer: -ao alsa:device=a2dpcopy

In such setup the sound will be present in the alsa default device
and be copied to the headphones. The sound timing is driven by alsa,
if headphones are disconnected or out of reach the server will just
drop packets, and alsa device will notice nothing. The sound between
alsa device and the headphones is out of sync by constant delay
of about 800 ms.

To send sound just to headphones select alsa output pcm device a2dp
and set ismaster=1 close to the end of this file.

If the server uses alsa timing (i.e., ismaster==0), some dropouts
can be experienced due to (if) the timing differences
in alsa and headphones.


A2DP server works with many ACP (headphones) simultaneously if the
default link policy is set up as master, i.e., set
lm master in /etc/bluetooth/hcid.conf 

To initiate connection with the server press PLAY button on headphones.
To stop connection just switch off the headphones.
"""

import struct
import ctypes
import time
import threading
import avdtp
import bluetooth
import array
import gc
import os
import avrcp_lircd
import pyDaemon
import ConfigParser


##### useful functions ################################################################################
class timer:
  def __init__(self):
    self.time=time.time()
    self.tsend=0
  def dt(self):
    return (time.time()-self.time)*1000000
  def start(self):
    self.time=time.time()  
  def time_to_wait(self,dt):
    tc=time.time()*1000000
    if self.tsend<tc+2000: # time has come, 2000 to be in the middle of 4 ms resolution
      self.tsend+=dt
      if tc>self.tsend+dt*20:# current time > time to send next packet
        print "OUT OF SYNC tc-tsend=%g>0" %(tc-self.tsend)
        self.tsend=tc+dt
      return 0# time has come, nothing to wait
    else: return self.tsend-tc   

### A2DP server code ###############################

pipeclosed=0
class reading_thread ( threading.Thread ):
  def __init__ ( self, pipename, ldata):
    self.pipename=pipename 
    self.ldata=ldata 
    threading.Thread.__init__ ( self )

  def run (self):
    global ldata_lock
    global ismaster
    mdata=10
    mread=4096
    pipe=open(self.pipename,'rb',0)
    pipeclosed=0
    while True:
      data=pipe.read(mread)
      if len(data)==0:
        self.ldata.append(data) # to indicate the end of stream
        if deb: print "closed pipe"
        pipe.close()
        pipe=open(self.pipename,'rb',0)
        if deb: print "opened pipe"
        pipeclosed=0
      while ismaster and len(self.ldata)>mdata:time.sleep(0.0001)
      if len(self.ldata)<=mdata:
        ldata_lock.acquire()
        self.ldata.append(data)
        ldata_lock.release()
        
def update_minmax(var,val):
  if var==None:return val,val
  else: return min(var[0],val),max(var[1],val)


class encoding_thread( threading.Thread ):
  def __init__ ( self, ldata, lpackets,codec,commands=[]):
    self.ldata=ldata 
    self.lpackets=lpackets
    self.codec=codec
    self.commands=commands
    threading.Thread.__init__ ( self )

    
  def run (self):
    global ldata_lock
    global lpackets_lock
    packet_header_size=13
    min_encoding_size=512
    mtu=672
    mpackets=5
    seq_number=0
    frame_count=0
    timestamp=0
    buf=(ctypes.c_ubyte*mtu)()
    lenbuf=0
    data=""
    elapsedtime=0
    mmdata=None
    mmpackets=None
    timer1=timer()
    igc=0
    gc.disable()
    pipeclosed=0
    while True:
      igc+=1
      if (igc>100 and len(lpackets)==0) or igc>150:
        gc.collect()
        igc=0
      time.sleep(0.00001) # some sleep
      if 'quit' in commands: break
      mmdata=update_minmax(mmdata,len(ldata))
      mmpackets=update_minmax(mmpackets,len(lpackets))
      if len(ldata)>0 and len(lpackets)<mpackets:
        ldata_lock.acquire()
        rdata=ldata[0]
        del ldata[0]
        ldata_lock.release()
        if len(rdata)==0:pipeclosed=1
        adata=array.array('H')#change endiannes
        adata.fromstring(rdata)
        adata.byteswap()
        rdata=adata.tostring()
        data+=rdata
#      while len(data)>=min_encoding_size: #encode
      dry=pipeclosed and len(data)==0
      while len(data)>0 or dry: #encode
        dry=pipeclosed and len(data)==0 and lenbuf>0
        l,pendata,lendata,duration=self.codec.encode(data)
        data=data[l:]
        if lenbuf+lendata+packet_header_size>mtu or dry: #enough for packet
          packetdata=(ctypes.c_ubyte*lenbuf)()
          ctypes.memmove(packetdata,buf,lenbuf)
          packet=avdtp.media_packet(packetdata,int(timestamp),frame_count,seq_number)
          par=self.codec.par
          duration2=1000000.*par.subbands*par.block_length/par.frequency
          duration2=duration2*frame_count
          lpackets_lock.acquire()
          self.lpackets.append((duration2,packet))
          lpackets_lock.release()
          timestamp=elapsedtime # for the following packet
          seq_number=(seq_number+1) & 0xffff
          lenbuf=0
          frame_count=0
          if seq_number%1024==0:
            if deb: print "%i packets,dt per packet=%g<>%g ms,ldata=[%i:%i],lpackets=[%i:%i]"\
              %(1024,timer1.dt()/1024,duration2,mmdata[0],mmdata[1],mmpackets[0],mmpackets[1])
            timer1.start()
            mmdata=None
            mmpackets=None
        ctypes.memmove(ctypes.addressof(buf)+lenbuf,pendata,lendata)
        lenbuf+=lendata
        frame_count+=1
        elapsedtime+=duration
        if dry and len(data)==0 and lenbuf==0:pipeclosed=0


class sending_thread(threading.Thread):
  def __init__ ( self, lpackets,lacp,commands=[]):
    self.lpackets=lpackets
    self.lacp=lacp
    self.commands=commands
    threading.Thread.__init__ ( self )

  def run (self):
    global lpackets_lock
    timer0=timer()
    while True:
      while len(self.lpackets)==0 or len(self.lacp)==0: time.sleep(0.0001)
      lpackets_lock.acquire()
      dt,packet=self.lpackets[0]
      del self.lpackets[0]
      lpackets_lock.release()
      while timer0.time_to_wait(dt):time.sleep(0.001)
      for acp in self.lacp:
          try:
            avdtp.send_packet(acp.stream,packet)
          except bluetooth.BluetoothError: # close and remove dead ACP
            if deb: print "stream error ",acp.addr
            acp.stream.close()
            acp.sock.close()
            if deb: print "close stream ",acp.addr
            try: lacp.remove(acp)
            except: pass
            if deb: print "remove stream, number of ACP ",len(lacp)
 


class acp_listen_thread(threading.Thread):
  def __init__ ( self, acp):
    self.acp=acp
    threading.Thread.__init__ ( self )
  def run(self):
    while True:
      try:
        resp=avdtp.receive_response(self.acp.sock,avdtp.message_single)
        if resp.header.signal_id==avdtp.AVDTP_CLOSE:
          if deb:
            print "received AVDTP_CLOSE ", self.acp.addr 
            break
        avdtp.print_fields(resp)
      except bluetooth.BluetoothError: # close and remove dead ACP
        break
    if deb: print "removing ACP ", self.acp.addr 
    self.acp.stream.close()
    self.acp.sock.close()
    try: self.acp.lacp.remove(self.acp)
    except: pass
    if deb:print 'number of ACP', len(lacp)

  
class acp: # container class to keep information about ACP
  def __init__(self,addr,codecs,lacp):
    self.addr=addr
    self.lacp=lacp
    self.stream,self.sock,self.seid,self.codec=connect(addr,codecs)
    self.listen_thread=acp_listen_thread(self)  
    self.listen_thread.start()
    
    

def connect(addr,codecs):
  """connects to a2dp sink on address addr """
  sock=avdtp.avdtp_connect(addr,25) # connect to the ACP side
  lsep=avdtp.avdtp_discover(sock) # discover set of codecs on ACP side
  seid,codec=avdtp.avdtp_set_configuration(sock,lsep,codecs) # select and set codec parameters
  stream=avdtp.avdtp_open(addr,sock,seid) # open audio stream
  avdtp.avdtp_start(sock,seid) #start audio stream
  return stream,sock,seid,codec

def avdtp_discover_abort(sock):
  cmd=avdtp.receive_response(sock,avdtp.message_single)
  cmd.header.message_type=avdtp.MESSAGE_TYPE_COMMAND
  cmd.header.signal_id=avdtp.AVDTP_ABORT
  avdtp.send_packet(sock,cmd)

def advertise_a2dp():
  """advertise a2dp source"""
  server_sock=bluetooth.BluetoothSocket( bluetooth.L2CAP )
  port=25
  server_sock.bind(("",port))
  server_sock.listen(1)
  if deb:print "listening on port %d" % port
  uuid = "110a"
  profile=[bluetooth.ADVANCED_AUDIO_PROFILE]
  classes=[bluetooth.AUDIO_SOURCE_CLASS,]
  bluetooth.advertise_service( server_sock, "Audio Source", service_id = "", service_classes = [], \
        profiles = [], provider = "", description = "")
  client_sock,(address,port) = server_sock.accept()
  if deb: print "Accepted connection from ",address
  return client_sock,address,port


def set_realtime():
  libc=ctypes.CDLL('libc.so.6')
  SCHED_FIFO=1
  schp=ctypes.c_int(libc.sched_get_priority_max(SCHED_FIFO))
  if libc.sched_setscheduler(0,SCHED_FIFO,ctypes.byref(schp)):
    print "can not set realtime privs, do you have root privs?"
    return 0
  else:
    print "successfully set realtime privs"
    return 1  
  

if __name__=="__main__":
  pyDaemon.createDaemon()
  cfg=ConfigParser.ConfigParser()
  cfg.read('/etc/a2dpd.conf')
  def getval(var,default):
    if cfg.has_option('a2dpd',var): return eval(cfg.get('a2dpd',var))
    else: return default
  ismaster=getval('ismaster',0)
  deb=getval('deb',0)
  bitpool=getval('bitpool',32)
  freq=getval('freq',48000)
  myaddr=getval('myaddr',[])

  RT=set_realtime()
  avdtp.deb=deb
  lacp=[]
  commands=[]
  ldata=[]
  lpackets=[]
  ldata_lock=threading.Lock()
  lpackets_lock=threading.Lock()
  sbc_codec=avdtp.sbc(freq,2,bitpool=bitpool) # initialize sbc codec
  codecs={avdtp.SBC_MEDIA_CODEC_TYPE:sbc_codec} # list the available codecs

  avrcp_lircd.deb=deb
  lircd=avrcp_lircd.lircd_emu()
  avrcp=avrcp_lircd.avrcp_server(lircd.send)
  avrcp.start()  
  
  reader=reading_thread('/tmp/a2dpipe',ldata)
  reader.start()
  encoder=encoding_thread(ldata,lpackets,sbc_codec,commands)
  encoder.start()
  for addr in myaddr:
    try: lacp.append(acp(addr,codecs,lacp)) 
    except bluetooth.BluetoothError,what:
      if deb:print 'BluetoothError: %s '  %(what)  
    
  sender=sending_thread(lpackets,lacp,commands)
  sender.start()
  while True: 
#  dynamically manages new ACP
#  to initiate connection press PLAY button on the headphones
    sock2,addr,port=advertise_a2dp()
    avdtp_discover_abort(sock2)  
    sock2.close()
    try:
      lacp.append(acp(addr,codecs,lacp))
    except bluetooth.BluetoothError,what:
      if deb: print 'BluetoothError: %s, try again', what  
    if deb: print 'number of ACP', len(lacp)