btcore.c

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

	slist_t	*entry, *prev;

	for (prev = NULL, entry = *list; entry; prev = entry, entry = entry->next) {
		if (entry->data == data) {
			if (prev)
				prev->next = entry->next;
			else
				*list = entry->next;
			free(entry);
			break;
		}
	}
}

void s_list_remove_custom(slist_t **list, void *data, slist_sort_func *func)
{
	slist_t	*entry = *list, *prev = NULL, *next = *list;

	if (!func)
		return;

	while (next) {
		entry = next;
		next = entry->next;				
		if (func(entry->data, data) == 0) {
			if (prev)
				prev->next = entry->next;
			else
				*list = entry->next;
			free(entry);
		} else
			prev = entry;
	}
}

void s_list_destroy(slist_t **list)
{
	slist_t	*entry;
	
	while (*list) {
		entry = *list;
		*list = entry->next;
		if (entry->data)
			free(entry->data);
		free(entry);
	}
}


int s_list_length(slist_t *list)
{
	slist_t	*entry;
	int	count;

	for (count = 0, entry = list; entry; entry = entry->next, count++) ;
	return count;
}

void s_list_free(slist_t **list)
{
	slist_t	*entry;

	while (*list) {
		entry = *list;
		*list = entry->next;
		free(entry);
	}
}

void *s_list_nth_data(slist_t *list, int i)
{
	slist_t	*entry;
	int	count;

	for (count = 0, entry = list; entry; entry = entry->next, count++)
		if (count == i)
			return entry->data;
	return NULL;
}

void s_list_foreach(slist_t *list, slist_func *func, void *param)
{
	slist_t	*entry;

	if (!func)
		return;
	for (entry = list; entry; entry = entry->next)
		func(entry->data, param);
}

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

	if (!func)
		return NULL;
	for (entry = list; entry; entry = entry->next)
		if (func(entry->data, data) == 0)
			return entry;
	return NULL;
}

/* stuff */
int affix_wait_for_service_up(int port, int timeout)
{
	struct sockaddr_in	in;
	int			fd, err;
	time_t			stm, tm;
	
	in.sin_family = AF_INET;
	in.sin_port = port;
	in.sin_addr.s_addr = inet_addr("127.0.0.1");

	fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (fd < 0)
		return -1;
	stm = time(NULL);
	for (;;) {
		err = connect(fd, (struct sockaddr *)&in, sizeof(in));
		if (!err)
			break;
		tm = time(NULL);
		if (difftime(tm, stm) > timeout) {
			close(fd);
			return -1;
		}	
	}
	close(fd);
	return 0;
}

int affix_system(char *prog, int single)
{
	pid_t	pid;
	int	err;
	sighandler_t	sh;

	if (single) {
		pid = affix_pidof(prog, PIDOF_SINGLE | PIDOF_BASENAME, 0);
		if (pid)
			return 0;
	}
	/* start server */
	sh = signal(SIGCHLD, SIG_DFL);
	err = system(prog);
	signal(SIGCHLD, sh);
	return err ? -1 : 0;
}

/* device inquiry/known cache */

char *xml_element(char **buf, char **attr)
{
	char	*start = *buf, *next;

	// find first <
	start = strchr(start, '<');
	if (start == NULL)
		return NULL;
	start++;

	// find last >
	next = strchr(start, '>');
	if (next == NULL) {
		// broken
		return NULL;
	}
	*next = '\0';
	next++;
	*buf = next;

	// get first later of the element
	while (isblank(*start) && *start != '\0')
		start++;

	next = start+1;
	while (!isblank(*next) && *next != '\0')
		next++;
	
	*next = '\0';
	*attr = next+1;

	// check for "/"
	next = *buf-1;
	while (*next == '\0' || isblank(*next))
		next--;
	if (*next == '/')
		*next = '\0';

	return start;
}

char *xml_attribute(char **buf, char **value)
{
	char *start = *buf, *next;
	int flag = 0;

	//find attr name
	while (isblank(*start) && *start != '\0')
		start++;
	
	if (*start == '\0')
		return NULL;

	next = start+1;
	
	//find end
	while (!isblank(*next) && *next != '=' && *next != '\0')
		next++;

	if (*next == '=')
		flag = 1;
	
	*next = '\0';
	next++;
	
	if (flag == 0) {
		next = strchr(next, '=');
		if (next == NULL)
			return NULL;
		next++;
	}
	
	next = strchr(next, '"');
	if (next == NULL)
		return NULL;
	*value = next+1;
	next = strchr(next+1, '"');
	if (next == NULL)
		return NULL;
	*next = '\0';
	*buf = next+1;
	
	return start;
}

btdev_struct *btdev_cache_lookup(BD_ADDR *bda)
{
	btdev_struct	*entry;
	int		i;
	
	for (i = 0; (entry = s_list_nth_data(devcache.head, i)); i++) {
		if (bda_equal(bda, &entry->bda))
			return entry;
	}
	return NULL;
}

int btdev_cache_del(btdev_struct *entry)
{
	s_list_remove(&devcache.head, entry);
	free(entry);
	return 0;
}

btdev_struct *btdev_cache_add(BD_ADDR *bda)
{
	btdev_struct	*entry, *mount;
	int		i, num = -1;

	for (i = 0; (entry = s_list_nth_data(devcache.head, i)); i++) {
		if (bda_equal(bda, &entry->bda)) {
			s_list_remove(&devcache.head, entry);
			num = i;
			break;
		}
	}
	if (!entry) {
		/* create new */
		entry = malloc(sizeof(btdev_struct));
		if (!entry) {
			perror("btdev_cache allocation failed\n");
			return NULL;
		}
		memset(entry, 0, sizeof(btdev_struct));
		entry->bda = *bda;
		entry->state = DEVSTATE_UNKNOWN;
	}
	/* find linking position */
	for (i = 0; (mount = s_list_nth_data(devcache.head, i)); i++) {
		if (mount->state == DEVSTATE_RANGE)
			continue;
		if (mount->state == DEVSTATE_GONE || i == num)
			break;
	}
	s_list_insert(&devcache.head, entry, i);
	return entry;
}

int btdev_cache_init(void)
{
	devcache.file = strdup(affix_cachefile);
	if (!devcache.file)
		return -1;

	devcache.head = NULL;
	devcache.count = 0;
	devcache.lock = -1;
	devcache.stamp = 0;
	return 0;
}

int btdev_cache_load(int locked)
{
	char		buf[256];
	FILE		*cfd;
	size_t		read;
	char		*next = NULL, *elem, *attrs, *attr, *value;
	BD_ADDR		bda;
	int		found = 0, eof = 0;
	struct stat	st;

	if (!devcache.file && btdev_cache_init() < 0)
		return -1;

	if (stat(devcache.file, &st) < 0) {
		return -1;
	}
	
	if (st.st_mtime == devcache.stamp) {
		if (locked && btdev_cache_lock() < 0) {
			return -1;
		}
		return 0;
	}
	
	devcache.stamp = st.st_mtime;
	
	if (btdev_cache_lock() < 0) {
		return -1;
	}

	cfd = fopen(devcache.file, "r");
	if (!cfd){
		fprintf(stderr, "Unable to open cache: %s\n", devcache.file);
		btdev_cache_unlock();
		return -1;
	}

	if (devcache.head) {
		s_list_destroy(&devcache.head);
		devcache.head = NULL;
		devcache.count = 0;
	}
	
	for (;;) {
		int	free;
		
		if (next) {
			/* we have some info in the buffer */
			free =  next - buf;
			memmove(buf, next, sizeof(buf) - free);
		} else
			free = sizeof(buf);
			
		if (!eof) {
			//printf("reading %d butes\n", free);
			read = fread(buf + sizeof(buf) - free, 1, free, cfd);
			if (!read)
				eof = 1;
		}

		next = (void*)buf;
		elem = xml_element(&next, &attrs);
		if (!elem)
			break;

		if (!found)
			if (strcmp(elem, "device-listing") == 0) {
				found = 1;
				continue;
			}

		if (strcmp(elem, "/device-listing") == 0)
			break;
		//printf("element: %s\n", elem);
		//printf("attr left: %s\n", attrs);
		// get attributes
		if (strcmp(elem, "device") == 0) {
			btdev_struct	*entry;
			entry = NULL;
			while ((attr = xml_attribute(&attrs, &value))) {
				//printf("%s = %s\n", attr, value);
				if (!entry) {
					if (strcmp(attr, "bda") == 0) {
						str2bda(&bda, value);
						entry = btdev_cache_add(&bda);
					}
				} else if (strcmp(attr, "class") == 0) {
					sscanf(value, "%x", &entry->cod);
				} else if (strcmp(attr, "name") == 0) {
					strcpy(entry->name, value);
				} else if (strcmp(attr, "key") == 0) {
					unsigned int	val;
					int		i;
					/* convert key to binary format */
					for (i = 0; sscanf(value, "%2x", &val) > 0 && i < 16; i++, value += 2) {
						entry->link_key[i] = val;
					}
					if (i)
						entry->flags |= BTDEV_KEY;
				}
			}
		}
	}
	fclose(cfd);
	if (!locked)
		btdev_cache_unlock();
	return 0;
}

int btdev_cache_reload(void)
{
	return btdev_cache_load(1);
}

	
int btdev_cache_save(void)
{
	btdev_struct	*entry;
	FILE		*cfd;
	int		i, k;

	if (devcache.lock == -1 && btdev_cache_lock() < 0)
		return -1;

	cfd = fopen(devcache.file, "w");
	if (!cfd) {
		fprintf(stderr, "Unable to fopen cache file: %s\n", devcache.file);
		btdev_cache_unlock();
		return -1;
	}
	fprintf(cfd, "<device-listing>\n");
	for (i = 0; (entry = s_list_nth_data(devcache.head, i)); i++) {
		fprintf(cfd, "<device bda=\"%s\"", bda2str(&entry->bda));
		if (entry->cod)
			fprintf(cfd, " class=\"%x\"", entry->cod);
		if (entry->name[0] != '\0')
			fprintf(cfd, " name=\"%s\"", entry->name);
		if (entry->flags & BTDEV_KEY) {
			fprintf(cfd, " key=\"");
			for (k = 0; k < 16; k++)
				fprintf(cfd, "%02x", entry->link_key[k]);
			fprintf(cfd, "\"");
		}

		fprintf(cfd, "/>\n");
	}
	fprintf(cfd, "</device-listing>\n");
	fclose(cfd);
	btdev_cache_unlock();
	return 0;
}

void btdev_cache_free(void)
{
	if (devcache.head) {
		s_list_destroy(&devcache.head);
		devcache.head = NULL;
		devcache.count = 0;
	}
	if (devcache.file) {
		free(devcache.file);
		devcache.file = NULL;
	}
}

void btdev_cache_purge(void)
{
	if (devcache.head) {
		s_list_destroy(&devcache.head);
		devcache.head = NULL;
		devcache.count = 0;
	}
	if (!devcache.file)
		btdev_cache_init();
	btdev_cache_save();
}

int btdev_cache_lock(void)
{
	devcache.lock = open(devcache.file, O_CREAT, 0644);
	if (devcache.lock < 0) {
		fprintf(stderr, "Unable to open cache for locking: %s\n", devcache.file);
		return devcache.lock;
	}

	if (flock(devcache.lock, LOCK_EX) < 0) {
		fprintf(stderr, "Unable to lock cache\n");
		close(devcache.lock);
		devcache.lock = -1;
		return -1;
	}
	return 0;
}

void btdev_cache_unlock(void)
{
	if (devcache.lock < 0)
		return;

	close(devcache.lock);
	devcache.lock = -1;
}

/*
 * Inquiry Cache Stuff
 */
void btdev_cache_retire(void)
{
	btdev_struct	*entry;
	int		i;
	
	for (i = 0; (entry = s_list_nth_data(devcache.head, i)); i++)
		entry->state = DEVSTATE_GONE;
}

void btdev_cache_print(int state)
{
	btdev_struct	*entry;
	char		buf[256], *name;
	int		i;
	char		ch;

	for (i = 0; (entry = s_list_nth_data(devcache.head, i)); i++) {
		if (!(entry->state & state))
			continue;
		parse_cod(buf, entry->cod);
		if (entry->name[0] != '\0')
			name = entry->name;
		else
			name = "(none)";
		switch (entry->state) {
			case DEVSTATE_RANGE:
				ch = '+';
				break;
			case DEVSTATE_GONE:
				ch = '-';
				break;
			case DEVSTATE_UNKNOWN:
			default:
				ch = ' ';
		}
		printf("%c%d: Address: %s, Class: 0x%06X, Key: \"%s\"", 
				ch, i+1, bda2str(&entry->bda), entry->cod, (entry->flags & BTDEV_KEY)?"yes":"no");
		printf(", Name: \"%s\"\n", name);
		printf("    %s\n", buf);
	}
}

int btdev_cache_resolve(BD_ADDR *bda, int id)
{
	btdev_struct 	*entry;
	
	if (id < 0)
		return -1;
	entry = s_list_nth_data(devcache.head, id - 1);
	if (!entry)
		return -1;
	*bda = entry->bda;
	return 0;
}

btdev_struct *__btdev_cache_add(BD_ADDR bda, uint32_t cod, char *name)
{
	btdev_struct *entry;

	entry = btdev_cache_add(&bda);
	if (!entry)
		return NULL;

	entry->state = DEVSTATE_RANGE;
	entry->cod = cod;
	if (name)
		strcpy(entry->name, name);

	return entry;
}


int btdev_get_bda(BD_ADDR *bda, char *peer)
{
	int	err;
	
	err = str2bda(bda, peer);
	if (!err) {
		/* try resolve */
		int	id;
		id = atoi(peer);
		//printf("id: %d\n", id);
		if (!id) 
			return -1;
		if (btdev_cache_load(0) < 0)
			return -1;
		err = btdev_cache_resolve(bda, id);
		if (err)
			return -1;
	}
	return 0;
}

/*
 * CLASS Of Device stuff
 */
int parse_cod(char *buf, uint32_t  cod)
{
	int			count = 0, found = 1;
	struct affix_tupla	*map;

	switch (cod & HCI_COD_MAJOR) {
		case HCI_COD_COMPUTER:
			for (map = codMinorComputerMnemonic; map->str || (found=0); map++) {
				if (map->value == (cod & HCI_COD_MINOR)) {
					count += sprintf(buf+count, "Computer (%s)", map->str);
					break;
				}
			}
			if (!found)
				count += sprintf(buf+count, "Computer (Unclassified)");
			break;
		case HCI_COD_PHONE:
			for (map = codMinorPhoneMnemonic; map->str || (found=0); map++) {
				if (map->value == (cod & HCI_COD_MINOR)) {
					count += sprintf(buf+count, "Phone (%s)", map->str);
					break;
				}
			}
			if (!found)
				count += sprintf(buf+count, "Phone (Unclassified)");
			break;
		case HCI_COD_MAUDIO:
			for (map = codMinorAudioMnemonic; map->str || (found=0); map++) {
				if (map->value == (cod & HCI_COD_MINOR)) {
					count += sprintf(buf+count, "Audio (%s)", map->str);
					break;
				}
			}
			if (!found)
				count += sprintf(buf+count, "Audio (Unclassified)");
			break;
		default:
			for (map = codMajorClassMnemonic; map->str || (found=0); map++) {
				if (map->value == (cod & HCI_COD_MAJOR)) {
					count += sprintf(buf+count, "%s (Unclassified)", map->str);
					break;
				}
			}
			if (!found)
				count += sprintf(buf+count, "Unclassified (Unclassified)");
	}
	count += sprintf(buf+count, " [");
	for (map = codsdp_service_map; map->str; map++) {
		if (map->value & cod) {
			count += sprintf(buf+count, "%s,", map->str);
		}
	}
	count--;	// remove ,
	count += sprintf(buf+count, "]");

	return 0;
}

/* commands */
void print_usage(struct command *cmd)
{
	printf("usage: %s %s\n", cmd->name, cmd->arg ? cmd->arg : "");
}

void print_full_usage(struct command *cmd)
{
	if (cmd->name)
		printf("usage: %s %s\n", cmd->name, cmd->arg ? cmd->arg : "");
	if (cmd->msg)
		printf("description:\n%s", cmd->msg);

}

int print_command_usage(struct command *cmds, char *command)
{
	struct command	*cmd;
	struct command	nullcmd = {NULL, NULL, 0, NULL, NULL};

	for (cmd = cmds; memcmp(cmd, &nullcmd, sizeof(nullcmd)) != 0; cmd++) {
		if (cmd->name)
			if (strcmp(cmd->name, command) == 0) {
				print_full_usage(cmd);
				return 0;
			}
	}
	printf("invalid command: %s\n", command);
	return 	1;
}

void print_all_usage(struct command *cmds)
{
	struct command	*cmd;
	struct command	nullcmd = {NULL, NULL, 0, NULL, NULL};
	
	for (cmd = cmds; memcmp(cmd, &nullcmd, sizeof(nullcmd)) != 0; cmd++) {
		if (cmd->name && cmd->arg)
			printf("%s %s\n", cmd->name, cmd->arg);
		else if (cmd->msg)
			printf("%s", cmd->msg);
	}
}

int call_command(struct command *cmds, char *command)
{
	struct command	*cmd;
	struct command	nullcmd = {NULL, NULL, 0, NULL, NULL};
	
	for (cmd = cmds; memcmp(cmd, &nullcmd, sizeof(nullcmd)) != 0; cmd++) {
		if (cmd->name && strcasecmp(cmd->name, command) == 0) {
			return cmd->func(cmd);
		}
	}
	printf("Invalid command: %s\n", command);
	return -1;
}