C言語によりRaspberry Pi 3とSensorTag間をBLEで接続

「Bluez を使用したSensorTagへのアクセス 」でBluez を使用してSensorTagへアクセスしましたが、今回はRaspberry Pi 3に実装されているBluetoothを使用して、Bluetooth Low Energy（BLE）プロトコルでC言語プログラムからSensorTagへアクセスし、Raspberry Pi 3をサービス提供するセントラルとして動作させます。

BlueZ 5.40 のインストール

Rasbian 上で BlueZ をインストールする際に apt-get を使うと バージョン5.23 がインストールされてしまうのでソースからコンパイルしてインストールします。

$ sudo apt-get install libdbus-1-dev libdbus-glib-1-dev libglib2.0-dev libical-dev libreadline-dev libudev-dev libusb-dev make
$ mkdir -p work/bluetooth
$ cd work/bluetooth
$ wget https://www.kernel.org/pub/linux/bluetooth/bluez-5.40.tar.xz
$ tar xvf bluez-5.40.tar.xz
$ cd bluez-5.40
$ ./configure --disable-systemd --enable-library
$ make
$ sudo make install

続けて make でインストールされないものを手動でインストールします。

$ sudo cp attrib/gatttool /usr/local/bin/

あと、libbluetooth-dev の各種ヘッダファイルをビルドしたもので置き換えます。既存のincludeはbluetooth.5.23としてバックアップし、コンパイルしたlib/配下をbluetoothとしてリンクします。

$ sudo cp -ipr lib/ /usr/include/bluetooth.5.40
$ cd /usr/include
$ sudo mv bluetooth bluetooth.5.23
$ sudo ln -s bluetooth.5.40/ bluetooth

BLEプログラムのコンパイル環境の構築

インストールしたBlueZのファイルの内、gatttoolコマンドに関連するファイルを流用して、SentorTagと接続できるBLEプログラムを作成します。

コンパイルするために必要なincludeファイルを参照するリンクを次に示します。includeファイルは、先にインストールしたBlueZのインストール先をリンクしています。

makeファイルを次に示します。フォルダ「_build」にオブジェクトが作成され、実行ファイル「gatool」がカレントディレクトに作成されます。

PROJECT_NAME := gatool
BLUEZ_PATH = .
PRJ_PATH = .
OBJECT_DIRECTORY = _build
OUTPUT_BINARY_DIRECTORY = .
OUTPUT_FILENAME := $(PROJECT_NAME)
　　　　　　・
　　　　　　・
　　　　　　・
#sources project
C_SOURCE_FILES += $(PRJ_PATH)/gatttool.c
C_SOURCE_FILES += $(PRJ_PATH)/att.c
C_SOURCE_FILES += $(PRJ_PATH)/gatt.c
C_SOURCE_FILES += $(PRJ_PATH)/gattrib.c
C_SOURCE_FILES += $(PRJ_PATH)/btio.c

C_SOURCE_FILES += $(BLUEZ_PATH)/src/log.c
C_SOURCE_FILES += $(BLUEZ_PATH)/client/display.c
　　　　　　・
　　　　　　・
　　　　　　・
#Link Library
LIBS += $(BLUEZ_PATH)/lib/.libs/libbluetooth-internal.a
LIBS += $(BLUEZ_PATH)/src/.libs/libshared-glib.a
LIBS += -lreadline
LIBS += `pkg-config --libs glib-2.0`

BLEプログラムの作成

BLEプログラムは、BlueZのファイルとそのソースの一部を切り出して作成します。ここでは、BlueZのgatttoolコマンドで、SensorTagのアドレスとinteractiveのパラメータを与えて起動し、「connect」をキー入力させた状態を、BlueZのファイルをベースにしてプログラム作成し、Raspberry Pi 3とSensorTagをBLEで接続します。作成手順を次に示します。

1. BLEプログラムを実行すると、main関数から実行する。最初に、本来パラメータとして渡されるopt_dst変数に、SensorTagのアドレスを設置し、キーボードから入力される”connect”をパラメータにして、parse_line関数を呼び出します。
2. parse_line関数では、パラメータ「connect”」をキーにしてcommands配列を検索し、一致したcmd_connect関数を実行します。
3. cmd_connect関数は、gatt_connect関数を呼び出し、gatt_connect関数は、bt_io_connect関数を呼び出します。connectのコールバック関数「connect_cb」は、bt_io_connect関数を呼び出すときにパラメータとして与えられます。
4. SentorTagをアドバタイズすると、コールバック関数「connect_cb」が実行され、connect_cb_main関数に制御が移されます。
5. connect_cb_main関数は、bt_io_get関数でensorTagからのデータを受け取けとります。
6. The Main Event Loopにより、SentorTagからのイベントが発生するまでプログラムを待たせます。ここでは、g_main_loop_run関数により、SentorTagがアドバタイズされるまで待たせています。

gatttool.c

#include <errno.h>
#include <stdio.h>
#include <signal.h>
#include <sys/signalfd.h>
#include <glib.h>

#include <readline/readline.h>
#include <readline/history.h>

#include "lib/bluetooth.h"
#include "lib/sdp.h"
#include "lib/uuid.h"
#include "lib/hci.h"
#include "lib/hci_lib.h"

#include "src/shared/util.h"
#include "btio/btio.h"
#include "att.h"
#include "gattrib.h"
#include "gatt.h"
#include "gatttool.h"
#include "client/display.h"

static char *opt_src = NULL;
static char *opt_dst = NULL;
static char *opt_dst_type = NULL;
static char *opt_sec_level = NULL;
static bt_uuid_t *opt_uuid = NULL;
static int opt_psm = 0;
static gboolean opt_interactive = FALSE;
static gboolean got_error = FALSE;

struct characteristic_data {
	GAttrib *attrib;
	uint16_t start;
	uint16_t end;
};

static GIOChannel *iochannel = NULL;
static GAttrib *attrib = NULL;
static int opt_mtu = 0;

static volatile enum state
{
	STATE_DISCONNECTED,
	STATE_CONNECTING,
	STATE_CONNECTED
}
conn_state;


#define error(fmt, arg...) \
	rl_printf(COLOR_RED "Error: " COLOR_OFF fmt, ## arg)

static GString *prompt;
static GMainLoop *event_loop;

static void cmd_exit(int argcp, char **argvp)
{
	printf("cmd_exit\n");
	rl_callback_handler_remove();
	g_main_loop_quit(event_loop);
}

static gboolean signal_handler(GIOChannel *channel, GIOCondition condition,
							gpointer user_data)
{
	static unsigned int __terminated = 0;
	struct signalfd_siginfo si;

	printf("signal_handler\n");
	cmd_exit(0, NULL);
	if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP)) {
		g_main_loop_quit(event_loop);
		return FALSE;
	}

	switch (si.ssi_signo) {
	case SIGINT:
		rl_replace_line("", 0);
		rl_crlf();
		rl_on_new_line();
		rl_redisplay();
		break;
	case SIGTERM:
		if (__terminated == 0) {
			rl_replace_line("", 0);
			rl_crlf();
			g_main_loop_quit(event_loop);
		}

		__terminated = 1;
		break;
	}
	return TRUE;
}

static guint setup_signalfd(void)
{
	GIOChannel *channel;
	guint source;
	sigset_t mask;
	int fd;

	printf("setup_signalfd\n");
	sigemptyset(&mask);
	sigaddset(&mask, SIGINT);
	sigaddset(&mask, SIGTERM);

	if (sigprocmask(SIG_BLOCK, &mask, NULL) < 0) {
		perror("Failed to set signal mask");
		return 0;
	}

	fd = signalfd(-1, &mask, 0);
	if (fd < 0) {
		perror("Failed to create signal descriptor");
		return 0;
	}

	channel = g_io_channel_unix_new(fd);

	g_io_channel_set_close_on_unref(channel, TRUE);
	g_io_channel_set_encoding(channel, NULL, NULL);
	g_io_channel_set_buffered(channel, FALSE);

	source = g_io_add_watch(channel,
				G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
				signal_handler, NULL);

	g_io_channel_unref(channel);

	return source;
}

static char *get_prompt(void)
{
	printf("get_prompt\n");
	if (conn_state == STATE_CONNECTED)
		g_string_assign(prompt, COLOR_BLUE);
	else
		g_string_assign(prompt, "");

	if (conn_state == STATE_CONNECTED)
		g_string_append(prompt, COLOR_OFF);

	return prompt->str;
}
static void set_state(enum state st)
{
	printf("set_state\n");
	conn_state = st;
	rl_set_prompt(get_prompt());
}

static void disconnect_io()
{
	printf("disconnect_io\n");
	if (conn_state == STATE_DISCONNECTED)
		return;

	g_attrib_unref(attrib);
	attrib = NULL;
	opt_mtu = 0;

	g_io_channel_shutdown(iochannel, FALSE, NULL);
	g_io_channel_unref(iochannel);
	iochannel = NULL;

	set_state(STATE_DISCONNECTED);
}

static void connect_cb_main(GIOChannel *io, GError *err, gpointer user_data)
{
	uint16_t mtu;
	uint16_t cid;

	printf("connect_cb_main\n");
	if (err) {
		set_state(STATE_DISCONNECTED);
		error("%s\n", err->message);
		return;
	}

	bt_io_get(io, &err, BT_IO_OPT_IMTU, &mtu,
	          BT_IO_OPT_CID, &cid, BT_IO_OPT_INVALID);

	if (err) {
		g_printerr("Can't detect MTU, using default: %s", err->message);
		g_error_free(err);
		mtu = ATT_DEFAULT_LE_MTU;
	}

	if (cid == ATT_CID)
		mtu = ATT_DEFAULT_LE_MTU;

	attrib = g_attrib_new(iochannel, mtu);
	g_attrib_register(attrib, ATT_OP_HANDLE_NOTIFY, GATTRIB_ALL_HANDLES,
	                  events_handler, attrib, NULL);
	g_attrib_register(attrib, ATT_OP_HANDLE_IND, GATTRIB_ALL_HANDLES,
	                  events_handler, attrib, NULL);
	set_state(STATE_CONNECTED);
	rl_printf("Connection successful\n");
}

static void connect_cb(GIOChannel *io, GError *err, gpointer user_data)
{
	printf("connect_cb\n");
	connect_cb_main(io, err, user_data);
}

static gboolean channel_watcher(GIOChannel *chan, GIOCondition cond,
                                gpointer user_data)
{
	printf("channel_watcher\n");
	disconnect_io();
	return FALSE;
}


GIOChannel *gatt_connect(const char *src, const char *dst,
				const char *dst_type, const char *sec_level,
				int psm, int mtu, BtIOConnect connect_cb,
				GError **gerr)
{
	GIOChannel *chan;
	bdaddr_t sba, dba;
	uint8_t dest_type;
	GError *tmp_err = NULL;
	BtIOSecLevel sec;

	printf("gatt_connect\n");
	
	str2ba(dst, &dba);

	/* Local adapter */
	if (src != NULL) {
		if (!strncmp(src, "hci", 3))
			hci_devba(atoi(src + 3), &sba);
		else
			str2ba(src, &sba);
	} else
		bacpy(&sba, BDADDR_ANY);

	/* Not used for BR/EDR */
	if (strcmp(dst_type, "random") == 0)
		dest_type = BDADDR_LE_RANDOM;
	else
		dest_type = BDADDR_LE_PUBLIC;

	if (strcmp(sec_level, "medium") == 0)
		sec = BT_IO_SEC_MEDIUM;
	else if (strcmp(sec_level, "high") == 0)
		sec = BT_IO_SEC_HIGH;
	else
		sec = BT_IO_SEC_LOW;

	printf("gatt_connect1 psm:%x\n",psm);
	if (psm == 0)
		chan = bt_io_connect(connect_cb, NULL, NULL, &tmp_err,
				BT_IO_OPT_SOURCE_BDADDR, &sba,
				BT_IO_OPT_SOURCE_TYPE, BDADDR_LE_PUBLIC,
				BT_IO_OPT_DEST_BDADDR, &dba,
				BT_IO_OPT_DEST_TYPE, dest_type,
				BT_IO_OPT_CID, ATT_CID,
				BT_IO_OPT_SEC_LEVEL, sec,
				BT_IO_OPT_INVALID);
	else
		chan = bt_io_connect(connect_cb, NULL, NULL, &tmp_err,
				BT_IO_OPT_SOURCE_BDADDR, &sba,
				BT_IO_OPT_DEST_BDADDR, &dba,
				BT_IO_OPT_PSM, psm,
				BT_IO_OPT_IMTU, mtu,
				BT_IO_OPT_SEC_LEVEL, sec,
				BT_IO_OPT_INVALID);

	if (tmp_err) {
		g_propagate_error(gerr, tmp_err);
		return NULL;
	}

	return chan;
}

static void cmd_connect(int argcp, char **argvp)
{
	GError *gerr = NULL;

	printf("cmd_connect\n");
	if (conn_state != STATE_DISCONNECTED)
		return;

	if (argcp > 1) {
		g_free(opt_dst);
		opt_dst = g_strdup(argvp[1]);

		g_free(opt_dst_type);
		if (argcp > 2)
			opt_dst_type = g_strdup(argvp[2]);
		else
			opt_dst_type = g_strdup("public");
		}

	if (opt_dst == NULL) {
		error("Remote Bluetooth address required\n"); 
		return;
	}

	set_state(STATE_CONNECTING);
	iochannel = gatt_connect(opt_src, opt_dst, opt_dst_type, opt_sec_level,
	                         opt_psm, opt_mtu, connect_cb, &gerr);
	if (iochannel == NULL) {
		set_state(STATE_DISCONNECTED);
		error("%s\n", gerr->message);
		g_error_free(gerr);
	} else
		g_io_add_watch(iochannel, G_IO_HUP, channel_watcher, NULL);
}

static void cmd_disconnect(int argcp, char **argvp)
{
	printf("cmd_disconnect\n");
	disconnect_io();
}

static struct {
	const char *cmd;
	void (*func)(int argcp, char **argvp);
	const char *params;
	const char *desc;
} commands[] = {
	{
		"connect",		cmd_connect,	"[address [address type]]",
		"Connect to a remote device"
	},
	{ NULL, NULL, NULL}
};

static void parse_line(char *line_read)
{
	char **argvp;
	int argcp;
	int i;

	if (line_read == NULL) {
		rl_printf("\n");
		cmd_exit(0, NULL);
		return;
	}

	line_read = g_strstrip(line_read);

	if (*line_read == '\0')
		goto done;

	add_history(line_read);

	if (g_shell_parse_argv(line_read, &argcp, &argvp, NULL) == FALSE)
		goto done;

	for (i = 0; commands[i].cmd; i++)
		if (strcasecmp(commands[i].cmd, argvp[0]) == 0)
			break;

	if (commands[i].cmd)
		commands[i].func(argcp, argvp);
	else
		error("%s: command not found\n", argvp[0]);

	g_strfreev(argvp);

done:
	free(line_read);
}

static char *btComm = NULL;

int main(int argc, char *argv[])
{
	guint signal;

	printf("main\n");
	
	//強制interactive
	opt_src = g_strdup("hci0");
	opt_dst = g_strdup("B4:99:4C:64:CD:DF");
	opt_interactive = TRUE;

	opt_dst_type = g_strdup("public");
	opt_sec_level = g_strdup("low");

	printf("sensortag\n");
	opt_sec_level = g_strdup("low");

	opt_src = g_strdup(opt_src);
	opt_dst = g_strdup(opt_dst);
	opt_dst_type = g_strdup(opt_dst_type);
	opt_psm = opt_psm;

	prompt = g_string_new(NULL);

	printf("sensortag1\n");
	event_loop = g_main_loop_new(NULL, FALSE);

	signal = setup_signalfd();


	printf("sensortag3\n");
	
	btComm = g_strdup("connect");
	parse_line(btComm);
	g_main_loop_run(event_loop);
	
	printf("sensortag4\n");
	rl_callback_handler_remove();
	cmd_disconnect(0, NULL);
	g_source_remove(signal);
	g_main_loop_unref(event_loop);
	g_string_free(prompt, TRUE);

	g_free(opt_src);
	g_free(opt_dst);
	g_free(opt_uuid);
	g_free(opt_sec_level);

	if (got_error)
		exit(EXIT_FAILURE);
	else
		exit(EXIT_SUCCESS);
}

BLEプログラムの実行

作成したBLEプログラムをRaspberry Pi 3で実行した結果を次に示します。SentorTagをアドバタイズすると、Raspberry Pi 3がSentorTagから通知を受信し、「connect_cb」に制御が移り、ここから、「connect_cb_main」に制御が移って、SentorTagからデータを受信します。受信したデータをチェックした結果正常だったことが、「Connection successful」の表示でわかります。プログラムの終了はCNTL-Cをキーインしています。CNTL-Cをキーインすると、「signal_handler」が動作し、「disconnect_io」でSentorTagとの接続を切断していることがわかります。

$ sudo ./gatool
main
sensortag
sensortag1
setup_signalfd
sensortag3
cmd_connect
set_state
get_prompt
gatt_connect
gatt_connect1 psm:0
bt_io_connect
bt_io_connect1
sockaddr_l2: 1f 00 00 00 df cd 64 4c 99 b4 04 00 01 00
bt_io_connect4
bt_io_connect5
connect_cb
connect_cb_main
set_state
get_prompt
Connection successful
^Csignal_handler
cmd_exit
sensortag4
cmd_disconnect
disconnect_io

(process:1041): GLib-WARNING **: Invalid file descriptor.

set_state
get_prompt

tshark使用してBLEで接続で使用するパケットの解析

パケットキャプチャソフト「tshark」使用して、Raspberry Pi 3とSentorTag間の接続で使用するパケットを解析しました。接続要求時に最初にRaspberry Pi 3とSentorTag間で送信されるパケット「LE Create Connection (0x000d)」の詳細情報を次に示します。

RaspberryPi で Error: connect: Connection refused (111)

OSとkernelを古いままでgatttoolコマンドによりBLEのConnectを行うと、次のように、「Error: connect: Connection refused (111)」のエラーメッセージが表示されることがあります。

$ sudo gatttool -b B4:99:4C:64:CD:DF --interactive
[B4:99:4C:64:CD:DF][LE]> connect
Attempting to connect to B4:99:4C:64:CD:DF
Error: connect: Connection refused (111)
[B4:99:4C:64:CD:DF][LE]>

この場合、OSとkernelを次のコマンドでアップグレードすると私の場合は正常に戻りました。

$ sudo apt-get dist-upgrade

Jessie になると bluez-utils というパッケージがなくなって bluez に統一されるなど、いくつか変更されているようで、BlueZの最新バージョンのソースを使用すると不整合が起こるようです。