a2dpd_tools.c

linux蓝牙剖面实现

/*
*
*  A2DPD - Bluetooth A2DP daemon for Linux
*
*  Copyright (C) 2006  Frédéric DALLEAU <[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.
*/
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/stat.h>

// Only macro DBG
#include "a2dpd_protocol.h"
#include "a2dpd_tools.h"

#define CHECKVAL ((uint32_t)0xFDFDFDFD)

void* mymalloc(int size)
{
	char* buffer = malloc(size+8);
	
	if(buffer)
	{
		*((uint32_t*)buffer) = ((uint32_t)size);
		buffer+=4;
		*((uint32_t*)(buffer+size)) = CHECKVAL;
	}
	return buffer;
}

void myfree(void* p, int line)
{
	char* buffer = p;
	if(buffer)
	{
		uint32_t size  = *((uint32_t*)(buffer-4));
		uint32_t check = *((uint32_t*)(buffer+size));
		if(check != CHECKVAL || size>16384)
			DBG("buffer overflow line %d (size=%d check=%X)", line, size, check);
		buffer-=4;
		free(buffer);
	}
}

int checkbuffer__(void* p, int line)
{
	int result = 0;
	char* buffer = p;
	if(buffer)
	{
		uint32_t size  = *((uint32_t*)(buffer-4));
		uint32_t check = *((uint32_t*)(buffer+size));
		if(check != CHECKVAL || size>16384)
		{
			DBG("buffer failed check line %d (size=%d check=%X)", line, size, check);
			result = 1;
		}
	}
	return result;
}

/*
int state=0;
int count=0;
int total=0;

for(i=0; i<pcm_buffer_size/2; i+=2) {
	if(state==0) {
		//DBG("%08X | %08X   %d | %d", ((int16_t*)lpConvert->lpVoid)[i], ((int16_t*)lpConvert->lpVoid)[i+1], ((int16_t*)lpConvert->lpVoid)[i], ((int16_t*)lpConvert->lpVoid)[i+1]);
		if(((int16_t*)pcm_buffer)[i]==(int16_t)0xFAFA) {
			state=1;
			count++;
			total++;
		} else {
			state=0;
		}
	} else if(state==1) {
		if(((int16_t*)pcm_buffer)[i]==(int16_t)0xFAFA) {
			count++;
			total++;
		} else {
			//DBG("Gap in the data %d,%d", count, i);
			state=0;
			count=0;
		}
	}
}
if(state==1) {
	//DBG("Gap in the data: %d, total=%d", count, total);
}
*/

// This function is needed to destroy zombies processes
// On Unix, any forked process which terminate before its parent create a zombie until parent call waitpid()
// We do not want to wait as we just need to "fire and forget" processes
// Found that on the web, hope it works
// http://www.erlenstar.demon.co.uk/unix/faq_2.html
void ignore_child_processes_return_values()
{
	struct sigaction sa;
	sa.sa_handler = SIG_IGN;
#ifdef SA_NOCLDWAIT
	sa.sa_flags = SA_NOCLDWAIT;
#else
	sa.sa_flags = 0;
#endif
	sigemptyset(&sa.sa_mask);
	sigaction(SIGCHLD, &sa, NULL);
}

void make_daemon_process(int bFork, int bVerbose, char *output_file_name)
{
	// Fork to background process if needed
	if (bFork == 1) {
		switch (fork()) {
		case -1:
			exit(-1);
		case 0:
			break;
		default:
			exit(0);
		}

		setsid();
		chdir("/");
	}
	// Redirect output to file (default /dev/null) in silent mode, verbose will print output to stdin/out/err
	if (!bVerbose) {
		int fd;
		if ((fd = open(output_file_name, O_CREAT | O_APPEND | O_RDWR, 0)) != -1) {
			fchmod(fd, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
			(void) dup2(fd, STDIN_FILENO);
			(void) dup2(fd, STDOUT_FILENO);
			(void) dup2(fd, STDERR_FILENO);
			if (fd > 2)
				(void) close(fd);
		} else {
			DBG("Couldn't redirect output");
		}
	}

	DBG("a2dpd [%s %s] starting ...", __DATE__, __TIME__);
}

static int lock_fd(int fd)
{
	struct flock lock;
	lock.l_type = F_WRLCK;
	lock.l_whence = SEEK_SET;
	lock.l_start = 0;
	lock.l_len = 0;

	return fcntl(fd, F_SETLK, &lock);
}

int lockfile(int bKill)
{
	int ret = 0;
	FILE* fp = NULL;
	pid_t pid;

	// Generate the lockfile
	int fd = open(PIDFILE, O_RDWR | O_CREAT | O_EXCL, S_IWUSR | S_IRUSR | S_IRGRP | S_IROTH);

	if (fd == -1) {
		if (errno != EEXIST)	// If we can't write the lock, then ignore
			goto post_lock;

		if ((fd = open(PIDFILE, O_RDWR)) < 0)
			goto post_lock;

		fp = fdopen(fd, "rw");
		if (fp == NULL)
			goto post_lock;

		pid = -1;
		if ((fscanf(fp, "%d", &pid) != 1) || (pid == getpid())
		|| (lock_fd(fileno(fp)) == 0)) {
			unlink(PIDFILE);
		} else {
			if (bKill) {
				int i = 0;
				// let the other daemon die
				while(i<5 && kill(pid, SIGTERM) >= 0) {
					DBG("Killing (%d/5)...", i);
					sleep(1);
					i++;
				}
				// Force termination
				kill(pid, SIGSTOP); 
			} else {
				DBG("A2DPD already running (pid=%d)", (int)pid);
				ret = -1;
				goto post_lock;
			}
		}
		fclose(fp);

		unlink(PIDFILE);
		fd = open(PIDFILE, O_RDWR | O_CREAT | O_EXCL, S_IWUSR | S_IRUSR | S_IRGRP | S_IROTH);

		if (fd == -1)
			goto post_lock;
	}

	lock_fd(fd);

	fp = fdopen(fd, "w");
	fprintf(fp, "%d\n", getpid());
	fflush(fp);
	fcntl(fd, F_SETFD, (long) 1);

post_lock:
	//fclose(fd);
	return ret;
}


sdp_session_t *sdp_connect_async(const bdaddr_t *dst)
{
	sdp_session_t *session = NULL;
#if BLUEZ_37
	session = sdp_connect(BDADDR_ANY, dst, SDP_NON_BLOCKING);
#else
	const bdaddr_t *src=BDADDR_ANY;
	int flags=0;
	int err;
	session = malloc(sizeof(sdp_session_t));
	if (!session)
		return session;
	memset(session, 0, sizeof(*session));
	session->flags = flags;
	{
		struct sockaddr_l2 sa;

		// create L2CAP connection
		session->sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
		session->local = 0;
		if (session->sock >= 0) {
			sa.l2_family = AF_BLUETOOTH;
			sa.l2_psm = 0;
			if (bacmp(src, BDADDR_ANY) != 0)
			{
				sa.l2_bdaddr = *src;
				if (bind(session->sock, (struct sockaddr *) &sa, sizeof(sa)) < 0)
					goto fail;
			}
			if (flags & SDP_WAIT_ON_CLOSE) {
				struct linger l = { .l_onoff = 1, .l_linger = 1 };
				setsockopt(session->sock, SOL_SOCKET, SO_LINGER, &l, sizeof(l));
			}
			sa.l2_psm = htobs(SDP_PSM);
			sa.l2_bdaddr = *dst;
			if(fcntl(session->sock, F_SETFL, O_NONBLOCK) < 0) {
				goto fail;
			}
			if ( (connect(session->sock, (struct sockaddr *) &sa, sizeof(sa)) == 0) || (errno == EAGAIN) || (errno == EINPROGRESS) )
				return session;
		}
	}
fail:
	err = errno;
	if (session->sock >= 0)
		close(session->sock);
	free(session);
	errno = err;
	session=NULL;
#endif
	return session;
}

void dump_raw(const char* data, int size)
{
	int i;
	for (i = 0; i < size; i++) {
		unsigned char c = data[i];
		if (i % 16 == 0)
			printf("%05d:", i);
		if (i % 4 == 0)
			printf(" ");
		printf("%02X", c);
		if (i % 16 == 15)
			printf("\r\n");
	}
	printf("\r\n");
}

void dump_stream(const short* data, int size)
{
	int i;
	for (i = 0; i < min(size,32); i+=2) {
		unsigned short s0 = data[i+0];
		unsigned short s1 = data[i+1];
		if (i % 16 == 0)
			printf("%03d:", i);
		printf("%04X-%04X", s0, s1);
		printf(" ");
		if (i % 16 == 14)
			printf("\r\n");
	}
	printf("\r\n");
}