btutils.c

Affix - Bluetooth Protocol Stack for Linux has been developed at Nokia Research Center in Helsinki

				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 = codServiceClassMnemonic; 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 = codServiceClassMnemonic; 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);
}


/* device inquiry/known cache */

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

int btdev_cache_del(btdev_list *list, btdev_struct *entry)
{
	s_list_remove(&list->head, entry);
	free(entry);
	return 0;
}

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

	for (i = 0; (entry = s_list_nth_data(list->head, i)); i++) {
		if (bda_equal(bda, &entry->bda)) {
			s_list_remove(&list->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(list->head, i)); i++) {
		if (mount->state == DEVSTATE_RANGE)
			continue;
		if (mount->state == DEVSTATE_GONE || i == num)
			break;
	}
	s_list_insert(&list->head, entry, i);
	return entry;
}

int btdev_cache_reload(btdev_list *list)
{
	char		buf[256];
	FILE		*cfd;
	size_t		read;
	char		*next = NULL, *elem, *attrs, *attr, *value;
	BD_ADDR		bda;
	int		found = 0, eof = 0;

	if (btdev_cache_lock(list) < 0) {
		return -1;
	}

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

	if (list->head) {
		s_list_destroy(&list->head);
		list->head = NULL;
		list->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(list, &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);
	return 0;
}

int btdev_cache_load(char *cachefile, btdev_list *list)
{
	int	err;
	
	list->file = strdup(cachefile);
	if (!list->file)
		return -1;

	list->head = NULL;
	list->count = 0;
	list->lock = -1;

	err =  btdev_cache_reload(list);
	btdev_cache_unlock(list);

	return err;
}

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

	if (list->lock == -1 && btdev_cache_lock(list) < 0)
		return -1;

	cfd = fopen(list->file, "w");
	if (!cfd) {
		fprintf(stderr, "Unable to fdopen cache file: %s\n", list->file);
		btdev_cache_unlock(list);
		return -1;
	}
	fprintf(cfd, "<device-listing>\n");
	for (i = 0; (entry = s_list_nth_data(list->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(list);
	return 0;
}

void btdev_cache_free(btdev_list *list)
{
	if (list->head) {
		s_list_destroy(&list->head);
		list->head = NULL;
		list->count = 0;
	}
//	if (list->file)
//		free(list->file);
}

int btdev_cache_lock(btdev_list *list)
{
	list->lock = open(list->file, O_CREAT, 0644);
	if (list->lock < 0) {
		fprintf(stderr, "Unable to open cache for locking: %s\n", list->file);
		return list->lock;
	}

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

void btdev_cache_unlock(btdev_list *list)
{
	if (list->lock < 0)
		return;

	close(list->lock);
	list->lock = -1;
}

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

void btdev_cache_print(btdev_list *list, int state)
{
	btdev_struct	*entry;
	char		buf[256], *name;
	int		i;
	char		ch;
	
	for (i = 0; (entry = s_list_nth_data(list->head, i)); i++) {
		if (!(entry->state & state))
			continue;
		parse_cod(buf, entry->cod);
		if (entry->name[0] != '\0')
			name = entry->name;
		else
			name = "Unknown";
		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(btdev_list *list, BD_ADDR *bda, int id)
{
	btdev_struct 	*entry;
	
	if (id < 0)
		return -1;
	entry = s_list_nth_data(list->head, id - 1);
	if (!entry)
		return -1;
	*bda = entry->bda;
	return 0;
}

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

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

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

	return entry;
}

/* ------------------------------------------------------------- */

int btdev_get_bda(btdev_list *list, BD_ADDR *bda, char *arg)
{
	int	err;
	
	err = str2bda(bda, arg);
	if (!err) {
		/* try resolve */
		int	id;
		id = atoi(arg);
		//printf("id: %d\n", id);
		if (!id) 
			return -1;
		err = btdev_cache_resolve(list, 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 = codServiceClassMnemonic; map->str; map++) {
		if (map->value & cod) {
			count += sprintf(buf+count, "%s,", map->str);
		}
	}
	count--;	// remove ,
	count += sprintf(buf+count, "]");

	return 0;
}

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

/* 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)
{
	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;
}