btcore.c

affix是一个Open Source的蓝牙协议栈

//
// Read a file and alloc a buffer for it
//
uint8_t* easy_readfile(const char *filename, int *file_size)
{
	int actual;
	int fd;
	uint8_t *buf;


	fd = open(filename, O_RDONLY, 0);
	if (fd == -1) {
		return NULL;
	}
	*file_size = get_filesize(filename);
	DBPRT("name=%s, size=%d\n", filename, *file_size);
	if(! (buf = malloc(*file_size)) ) {
		return NULL;
	}

	actual = read(fd, buf, *file_size);
	close(fd); 

	*file_size = actual;
	return buf;
}

#endif

/* put yourself in the background */
void affix_background(void)
{
	int 	fd;
	int	nulldev_fd ;		/* file descriptor for null device */

	if (getppid() != 1) {
		/* if parent is not init */
		switch (fork()) {
			case -1:
				exit(1);
				break;
			case 0:
				break;
			default:
				exit(0); //parent exit
		}
	}
	signal(SIGTTOU, SIG_IGN);
	signal(SIGTTIN, SIG_IGN);
	signal(SIGTSTP, SIG_IGN);
	setsid();
	chdir("/");
	if ((nulldev_fd = open("/dev/null", O_RDWR)) != -1) {
		for (fd = 0; fd < 3; fd++) {
			if (isatty(fd))
				dup2(nulldev_fd, fd) ;
		}
		close(nulldev_fd) ;
	}
}

static void freeproc(void)
{
	PROC	*p, *n;
	/* Free the already existing process list. */
	for (p = plist; p; p = n) {
		n = p->next;
		if (p->fullname)
			free(p->fullname);
		if (p->statname)
			free(p->statname);
		free(p);
	}
	plist = NULL;

}

/*
 *	Read the proc filesystem.
 */
static int readproc(void)
{
	DIR *dir;
	struct dirent *d;
	char path[256];
	char buf[256];
	char *s, *q;
	FILE *fp;
	int pid, f;
	PROC *p;
	struct stat st;
	int c;

	/* Open the /proc directory. */
	if ((dir = opendir("/proc")) == NULL) {
		BTERROR("cannot opendir(/proc)");
		return -1;
	}

	freeproc();

	/* Walk through the directory. */
	while ((d = readdir(dir)) != NULL) {
		/* See if this is a process */
		if ((pid = atoi(d->d_name)) == 0) continue;
		/* Get a PROC struct . */
		p = (PROC *)malloc(sizeof(PROC));
		memset(p, 0, sizeof(PROC));
		/* Open the status file. */
		snprintf(path, sizeof(path), "/proc/%s/stat", d->d_name);

		/* Read SID & statname from it. */
		if ((fp = fopen(path, "r")) != NULL) {
			buf[0] = 0;
			fgets(buf, 256, fp);

			/* See if name starts with '(' */
			s = buf;
			while (*s != ' ') s++;
			s++;
			if (*s == '(') {
				/* Read program name. */
				q = strrchr(buf, ')');
				if (q == NULL) {
					p->sid = 0;
					BTERROR("can't get program name from %s\n", path);
					free(p);
					continue;
				}
				s++;
			} else {
				q = s;
				while (*q != ' ') q++;
			}
			*q++ = 0;
			while (*q == ' ') q++;
			p->statname = strdup(s);

			/* This could be replaced by getsid(pid) */
			if (sscanf(q, "%*c %*d %*d %d", &p->sid) != 1) {
				p->sid = 0;
				BTERROR("can't read sid from %s\n",
						path);
				free(p);
				continue;
			}
			fclose(fp);
		} else {
			/* Process disappeared.. */
			free(p);
			continue;
		}

		/* Now read argv[0] */
		snprintf(path, sizeof(path), "/proc/%s/cmdline", d->d_name);
		if ((fp = fopen(path, "r")) != NULL) {
			f = 0;
			while(f < 127 && (c = fgetc(fp)) != EOF && c)
				buf[f++] = c;
			buf[f++] = 0;
			fclose(fp);

			/* Store the name into malloced memory. */
			p->fullname = strdup(buf);

			/* Get a pointer to the basename. */
			if ((p->basename = strrchr(p->fullname, '/')) != NULL)
				p->basename++;
			else
				p->basename = p->fullname;
		} else {
			/* Process disappeared.. */
			free(p);
			continue;
		}

		/* Try to stat the executable. */
		snprintf(path, sizeof(path), "/proc/%s/exe", d->d_name);
		if (stat(path, &st) == 0) {
			p->dev = st.st_dev;
			p->ino = st.st_ino;
		}

		/* Link it into the list. */
		p->next = plist;
		plist = p;
		p->pid = pid;
	}
	closedir(dir);
	/* Done. */
	return 0;
}

static inline PIDQ *init_pid_q(PIDQ *q)
{
	q->front =  q->next = q->rear = NULL;
	q->proc = NULL;
	return q;
}

static inline int empty_q(PIDQ *q)
{
	return (q->front == NULL);
}

static inline int add_pid_to_q(PIDQ *q, PROC *p)
{
	PIDQ *tmp;

	tmp = (PIDQ *)malloc(sizeof(PIDQ));

	tmp->proc = p;
	tmp->next = NULL;

	if (empty_q(q)) {
		q->front = tmp;
		q->rear  = tmp;
	} else {
		q->rear->next = tmp;
		q->rear = tmp;
	}
	return 0;
}

static inline PROC *get_next_from_pid_q(PIDQ *q)
{
	PROC *p;
	PIDQ *tmp = q->front;

	if (!empty_q(q)) {
		p = q->front->proc;
		q->front = tmp->next;
		free(tmp);
		return p;
	}

	return NULL;
}

/* Try to get the process ID of a given process. */
static PIDQ *pidof(char *prog)
{
	struct stat st;
	int dostat = 0;
	PROC *p;
	PIDQ *q;
	char *s;
	int foundone = 0;
	int ok = 0;

	/* Try to stat the executable. */
	if (prog[0] == '/' && stat(prog, &st) == 0) dostat++;

	/* Get basename of program. */
	if ((s = strrchr(prog, '/')) == NULL)
		s = prog;
	else
		s++;

	q = (PIDQ *)malloc(sizeof(PIDQ));
	q = init_pid_q(q);

	/* First try to find a match based on dev/ino pair. */
	if (dostat) {
		for (p = plist; p; p = p->next) {
			if (p->dev == st.st_dev && p->ino == st.st_ino) {
				add_pid_to_q(q, p);
				foundone++;
			}
		}
	}

	/* If we didn't find a match based on dev/ino, try the name. */
	if (!foundone) {
		for (p = plist; p; p = p->next) {
			ok = 0;

			ok += (strcmp(p->fullname, prog) == 0);
			ok += (strcmp(p->basename, s) == 0);

			if (p->fullname[0] == 0 ||
					strchr(p->fullname, ' ') ||
					scripts_too)
				ok += (strcmp(p->statname, s) == 0);

			if (ok) add_pid_to_q(q, p);
		}
	}

	return q;
}

#define PIDOF_OMITSZ	5

/*
 *	Pidof functionality.
 */
int affix_pidof(char *name, int flags, pid_t pid)
{
	PROC	*p;
	PIDQ	*q;
	int	i,oind;
	pid_t	opid[PIDOF_OMITSZ], spid = 0;
	char	*basec = NULL;

	for (oind = PIDOF_OMITSZ-1; oind > 0; oind--)
		opid[oind] = 0;

	if (flags&PIDOF_SCRIPTS)
		scripts_too++;

	if (flags&PIDOF_OMIT) {
		opid[oind] = pid;
		oind++;
	}
	if (flags&PIDOF_POMIT) {
		opid[oind] = getppid();
		oind++;
	}
	if (flags&PIDOF_BASENAME) {
		char	*ch;

		basec = strdup(name);
		name = basename(basec);
		if ((ch = strchr(name, ' '))) {
			*ch = '\0';
		}
	}
	/* Print out process-ID's one by one. */
	readproc();
	if ((q = pidof(name)) == NULL)
		goto exit;
	while ((p = get_next_from_pid_q(q))) {
		if (flags & PIDOF_OMIT) {
			for (i = 0; i < oind; i++) {
				if (opid[i] == p->pid)
					break;
			}
			/*
			 *	On a match, continue with
			 *	the for loop above.
			 */
			if (i < oind)
				continue;
		}
		if (flags & PIDOF_SINGLE) {
			if (spid)
				continue;
			else
				spid = p->pid;
		}
	}
exit:
	free(basec);
	freeproc();
	return spid;
}



int rmkdir(char *new_dir, int mode)
{
	size_t i = 0;

	DBPRT("new_dir: %s\n", new_dir);	
	if (new_dir == NULL || new_dir[0] == '\0')
		return -1;

	if (access(new_dir, R_OK|X_OK) == 0)
		return 0;
	
	if (new_dir[0] == '/')
		i++;
	
	for (; new_dir[i] != '\0'; i++) {
		if (new_dir[i] == '/') {
			char tmpdir[PATH_MAX + 1];

			strncpy (tmpdir, new_dir, i);
			tmpdir[i] = '\0';

			if ((mkdir(tmpdir, mode) == -1) && (errno != EEXIST))
				return -1;
		}	
	}

	if (mkdir(new_dir, mode) == -1 && errno != EEXIST)
		return -1;

	return 0;
}

/* Mapping */

char *val2str(struct affix_tupla *map, int value)
{
	for (;map; map++) {
		if (map->value == value) {
			if (map->str == NULL)
				return map->match;
			return map->str;
		}
	}
	return "";
}


int str2val(struct affix_tupla *map, char *str, unsigned int *val)
{
	char			*fp, *tmp;
	struct affix_tupla	*m;
	int			found;

	if (!str || !(str = strdup(str)))
		return 0;

	*val = 0;
	found = 1;

	for (fp = strtok_r(str, ", ", &tmp); fp; fp = strtok_r(NULL, ", ", &tmp)) {
		//printf("arg: [%s]\n", fp);
		for (m = map; m->match || m->str || (found = 0); m++) {
			if (m->match && strcasecmp(fp, m->match) == 0) {
				*val = m->value;
				free(str);
				return 1;
			}
		}
		if (!found) {
			free(str);
			return 0;
		}
	}
	free(str);
	return 0;
}


int str2mask(struct affix_tupla *map, char *str, unsigned int *mask)
{
	char			*fp, *tmp;
	struct affix_tupla	*m;
	int			found;

	if (!str || !(str = strdup(str)))
		return 0;

	*mask = 0;
	found = 1;

	for (fp = strtok_r(str, ", ", &tmp); fp; fp = strtok_r(NULL, ", ", &tmp)) {
		//printf("arg: [%s]\n", fp);
		for (m = map; m->match || (found = 0); m++) {
			if (strcasecmp(fp, m->match) == 0) {
				*mask |= m->value;
				break;
			}
		}
		if (!found) {
			free(str);
			return 0;
		}
	}
	free(str);
	return 1;
}

int mask2str(struct affix_tupla *map, char *str, unsigned int mask)
{
	int			i = 0;
	struct affix_tupla	*m;

	str[0] = '\0';
	for (m = map; m->match; m++) {
		if (m->value & mask)
			i += sprintf(str + i, "%s ", m->match);
	}
	if (i)
		str[i-1] = '\0';
	return 0;
}

int mask2str_comma(struct affix_tupla *map, char *str, unsigned int mask)
{
	int			i = 0;
	struct affix_tupla	*m;

	str[0] = '\0';
	for (m = map; m->match; m++) {
		if (m->value & mask)
			i += sprintf(str + i, "%s, ", m->match);
	}
	if (i)
		str[i-2] = '\0';
	return 0;
}

int str2cod(char *str, uint32_t *cod)
{
	struct affix_tupla	*map;
	int			found = 1;
	char			*arg, *tmp;

	if (!str || !(str = strdup(str)))
		return -1;

	/* get Major */
	arg = strtok_r(str, ", ", &tmp);
	if (arg == NULL) {
		free(str);
		return -1;
	}

	*cod = 0;
	for (map = codMajorClassMnemonic; map->match || (found=0); map++) {
		if (strncasecmp(map->match, arg, 3) == 0) {
			*cod |= map->value;
			break;
		}
	}
	if (!found){
		free(str);
		return -1;
	}

	/* get minor */
	arg = strtok_r(NULL, ", ", &tmp);
	if (arg == NULL) {
		free(str);
		return -1;
	}

	switch (*cod & HCI_COD_MAJOR) {
		case HCI_COD_COMPUTER:
			for (map = codMinorComputerMnemonic; map->match || (found=0); map++) {
				if (strncasecmp(map->match, arg, 3) == 0) {
					*cod |= map->value;
					break;
				}
			}
			break;
		case HCI_COD_PHONE:
			for (map = codMinorPhoneMnemonic; map->match || (found=0); map++) {
				if (strncasecmp(map->match, arg, 3) == 0) {
					*cod |= map->value;
					break;
				}
			}
			break;
		case HCI_COD_MAUDIO:
			for (map = codMinorAudioMnemonic; map->match || (found=0); map++) {
				if (strncasecmp(map->match, arg, 3) == 0) {
					*cod |= map->value;
					break;
				}
			}
			break;
		default:
			found = 0;
	}

	if (!found) {
		free(str);
		return -1;
	}

	/* get services */
	while ((arg = strtok_r(NULL, ", ", &tmp))) {
		for (map = codsdp_service_map; map->match || (found=0); map++) {
			if (strncasecmp(map->match, arg, 3) == 0) {
				*cod |= map->value;
				break;
			}
		}
		if (!found) {
			free(str);
			return -1;
		}
	}

	free(str);
	return 0;
}

int str2cod_svc(char *str, uint32_t *cod)
{
	struct affix_tupla	*map;
	int			found = 1;
	char			*arg, *tmp;

	if (!str || !(str = strdup(str)))
		return -1;

	/* get services */
	for (arg = strtok_r(str, ", ", &tmp); arg; arg = strtok_r(NULL, ", ", &tmp)) {
		for (map = codsdp_service_map; map->match || (found=0); map++) {
			if (strncasecmp(map->match, arg, 3) == 0) {
				*cod |= map->value;
				break;
			}
		}
		if (!found) {
			free(str);
			return -1;
		}
	}

	free(str);
	return 0;
}


int str2pkt_type(char *str, unsigned int *pkt_type)
{
	return str2mask(pkt_type_map, str, pkt_type);
}

int str2sec_level(char *str, unsigned int *sec_level)
{
	return str2mask(sec_level_map, str, sec_level);
}


char *argv2str(char *argv[])
{
	int	i = 0;
	char	*arg;
	static char	str[128];
	
	while ((arg = *argv++)) {
		i += sprintf(str + i, "%s ", arg);
	}
	str[i - 1] = '\0'; // remove last space
	return str;
}

/* slist_t */

slist_t *s_list_append(slist_t **list, void *data)
{
	slist_t	*entry, *prev;

	entry = (slist_t*)malloc(sizeof(slist_t));
	if (!entry)
		return NULL;
	entry->data = data;
	entry->next = NULL;
	
	if (!(*list)) {
		*list = entry;
		return entry;
	}
	
	for (prev = *list; prev->next; prev = prev->next) ;
	
	prev->next = entry;
	return entry;
}

slist_t *s_list_insert(slist_t **list, void *data, int i)
{
	slist_t	*entry, *prev;
	int	count;

	for (count = 0, prev = NULL, entry = *list; entry; 
			prev = entry, entry = entry->next, count++)
		if (count == i)
			break;

	entry = (slist_t*)malloc(sizeof(slist_t));
	if (!entry)
		return NULL;
	entry->data = data;
	if (!prev) {
		entry->next = *list;
		*list = entry;
	} else {
		entry->next = prev->next;
		prev->next = entry;
	}
	return entry;
}

slist_t *s_list_insert_sorted(slist_t **list, void *data, slist_sort_func *func)
{
	slist_t	*entry, *prev;

	if (!func)
		return NULL;
	for (prev = NULL, entry = *list; entry; prev = entry, entry = entry->next) {
		if (func(data, entry->data) < 0)
			break;
	}
	entry = (slist_t*)malloc(sizeof(slist_t));
	if (!entry)
		return NULL;
	entry->data = data;
	if (!prev) {
		entry->next = *list;
		*list = entry;
	} else {
		entry->next = prev->next;
		prev->next = entry;
	}
	return entry;
}

void *s_list_dequeue(slist_t **list)
{
	void	*data;
	slist_t	*entry = *list;
	
	if (entry == NULL)
		return NULL;
	*list = entry->next;
	data = entry->data;
	free(entry);
	return data;
}

void s_list_remove(slist_t **list, void *data)
{