1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

HCIを使ってみる ESP32-S3 + Linux

Posted at

1.概要

HCI (Host Controller Interface)とは、Bluetoothの規格で、ホストとコントローラの間の通信プロトコルです。
今回は、HCIを理解するためにコントローラにESP32を使用しホストにUbuntuを使用して、どのようなコマンドでスキャンやアドバタイズが実施されているのかを確認する。

ざっくり構成

2.環境

  • ESP32-S3-DevKitC
  • FT232RQ USBシリアル変換
  • Ubuntu 24.04 LTS

3.初期設定

コントローラ側のESP32-S3にサンプルを書き込んで使用します。
controller_hci_uart_esp32c3_and_esp32s3
書き込み方法はここで説明はしません。
Tx Rx RTS CTSをシリアルUSB変換と接続します。

書き込むとこんなシリアルログが出てきます。

I (346) BLE_INIT: BT controller compile version [2edb0b0]
I (346) BLE_INIT: Using main XTAL as clock source
I (346) BLE_INIT: Feature Config, ADV:1, BLE_50:1, DTM:1, SCAN:1, CCA:0, SMP:1, CONNECT:1
I (356) BLE_INIT: Bluetooth MAC: f4:12:fa:84:72:e2
I (356) phy_init: phy_version 701,f4f1da3a,Mar  3 2025,15:50:10
I (396) phy_init: Saving new calibration data due to checksum failure or outdated calibration data, mode(0)
I (466) UHCI: HCI messages can be communicated over UART1:
--PINs: TxD 4, RxD 5, RTS 6, CTS 7
--Baudrate: 115200
I (466) main_task: Returned from app_main()

4.アドバタイズを送信

Bluetoothのアドバタイズを送信するようにHCIを使用して、ESP32にコマンドを送ります。

4-1 コマンド

HCI Reset
LE Set Advertising Params
LE Set Advertising/Data
Enable Advertising

4-2 実行

c言語のソースなのでgccでビルドして実行します。
起動オプションでUART変換のデバイスとボーレート、アドバタイズ時の表示名を入れて下さい。

4-2-1 ビルド実行

gcc -O2 -Wall -o esp32_hci esp32_hci_linux_advertise.c
./esp32_hci -d /dev/ttyUSB0 -b 115200 -n "ESP32-HCI"

4-2-2 実行結果

iPhoneのアプリBLE Scannerを使用して、アドバタイズを確認することができました。

4-3 コード

コード全文
esp32_hci_linux_advertise.c
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <sys/select.h>
#include <sys/time.h>
#include <signal.h>

// H4 パケット種別
#define H4_CMD   0x01
#define H4_ACL   0x02
#define H4_SCO   0x03
#define H4_EVT   0x04

// HCI イベントコード
#define EVT_COMMAND_COMPLETE 0x0E
#define EVT_COMMAND_STATUS   0x0F
#define EVT_LE_META          0x3E

// HCI OGF/OCF ヘルパ
#define OGF_LINK_CTL        0x01
#define OGF_CTRL_BB         0x03
#define OGF_INFO_PARAM      0x04
#define OGF_STATUS_PARAM    0x05
#define OGF_LE_CTL          0x08
#define OGF_VENDOR          0x3F

#define cmd_opcode_pack(ogf, ocf) (uint16_t)((ocf & 0x03FF) | (ogf << 10))

// 代表的なオペコード
#define HCI_OP_RESET                          cmd_opcode_pack(OGF_CTRL_BB, 0x0003)
#define HCI_OP_LE_SET_ADV_PARAMS              cmd_opcode_pack(OGF_LE_CTL,   0x0006)
#define HCI_OP_LE_SET_ADV_DATA                cmd_opcode_pack(OGF_LE_CTL,   0x0008)
#define HCI_OP_LE_SET_SCAN_RSP_DATA           cmd_opcode_pack(OGF_LE_CTL,   0x0009)
#define HCI_OP_LE_SET_ADV_ENABLE              cmd_opcode_pack(OGF_LE_CTL,   0x000A)
#define HCI_OP_LE_READ_LOCAL_SUPPORTED_FEATURES cmd_opcode_pack(OGF_LE_CTL, 0x0003)

static volatile sig_atomic_t g_stop = 0;
static void on_sigint(int s){ (void)s; g_stop = 1; }

void usage(const char *progname) {
    printf("Usage: %s -d <device> [-b baudrate]\n", progname);
    printf("Example: %s -d /dev/ttyUSB0 -b 115200\n", progname);
    exit(1);
}

static int set_interface_attribs(int fd, int speed)
{
    struct termios tty;
    if (tcgetattr(fd, &tty) != 0) {
        perror("tcgetattr");
        return -1;
    }

    cfmakeraw(&tty);

    // 入出力速度設定
    speed_t spd;
    switch (speed) {
        case 9600: spd = B9600; break;
        case 19200: spd = B19200; break;
        case 38400: spd = B38400; break;
        case 57600: spd = B57600; break;
        case 115200: spd = B115200; break;
#ifdef B230400
        case 230400: spd = B230400; break;
#endif
#ifdef B460800
        case 460800: spd = B460800; break;
#endif
#ifdef B921600
        case 921600: spd = B921600; break;
#endif
        default:
            fprintf(stderr, "Unsupported baud: %d\n", speed);
            return -1;
    }
    cfsetospeed(&tty, spd);
    cfsetispeed(&tty, spd);

    // 8N1, フロー制御なし
    tty.c_cflag &= ~PARENB;
    tty.c_cflag &= ~CSTOPB;
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= CS8;
    tty.c_cflag &= ~CRTSCTS;
    tty.c_cflag |= (CLOCAL | CREAD);

    // ブロッキング read の最小バイト数/タイムアウト (raw にしているので VMIN/VTIME を設定)
    tty.c_cc[VMIN]  = 0;   // 0 で非ブロッキング
    tty.c_cc[VTIME] = 0;   // タイムアウト 0.1s 単位

    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        perror("tcsetattr");
        return -1;
    }
    return 0;
}

static int open_serial(const char* dev, int baud)
{
    int fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (fd < 0) {
        fprintf(stderr, "open %s failed: %s\n", dev, strerror(errno));
        return -1;
    }
    if (set_interface_attribs(fd, baud) < 0) {
        close(fd);
        return -1;
    }
    return fd;
}

static int write_all(int fd, const void* buf, size_t len)
{
    const uint8_t* p = (const uint8_t*)buf;
    size_t left = len;
    while (left) {
        ssize_t n = write(fd, p, left);
        if (n < 0) {
            if (errno == EAGAIN || errno == EINTR) continue;
            return -1;
        }
        left -= (size_t)n;
        p += n;
    }
    return 0;
}

static int hci_send_cmd(int fd, uint16_t opcode, const uint8_t* params, uint8_t plen)
{
    uint8_t hdr[4];
    hdr[0] = H4_CMD;
    hdr[1] = (uint8_t)(opcode & 0xFF);
    hdr[2] = (uint8_t)(opcode >> 8);
    hdr[3] = plen;

    if (write_all(fd, hdr, sizeof(hdr)) < 0) return -1;
    if (plen && write_all(fd, params, plen) < 0) return -1;

    return 0;
}

static int read_hci_event(int fd, uint8_t* buf, size_t buflen, int timeout_ms)
{
    // H4 EVT: [0x04][event_code][param_len][params...]
    fd_set rfds; FD_ZERO(&rfds); FD_SET(fd, &rfds);
    struct timeval tv; tv.tv_sec = timeout_ms / 1000; tv.tv_usec = (timeout_ms % 1000)*1000;
    int r = select(fd+1, &rfds, NULL, NULL, &tv);
    if (r <= 0) return r; // 0: timeout, -1: error

    // まず H4 種別
    uint8_t type;
    ssize_t n = read(fd, &type, 1);
    if (n <= 0) return -1;
    if (type != H4_EVT) {
        // 他のパケットを破棄
        uint8_t dump[260];
        n = read(fd, dump, sizeof(dump)); (void)n;
        return -2; // unexpected packet
    }
    // イベントコード + パラメータ長
    uint8_t ehdr[2];
    n = read(fd, ehdr, 2);
    if (n != 2) return -1;
    uint8_t evt = ehdr[0];
    uint8_t plen = ehdr[1];
    if (plen > buflen) plen = (uint8_t)buflen;
    n = read(fd, buf, plen);
    if (n != plen) return -1;

    // デバッグ表示
    fprintf(stderr, "EVT 0x%02X len=%u\n", evt, ehdr[1]);
    return ((int)evt) | (plen << 8); // 上位8bitに実際読んだ長さ
}

static int wait_cmd_complete(int fd, uint16_t opcode, uint8_t* ret_params, size_t* ret_len, int timeout_ms)
{
    uint8_t buf[260];
    int deadline_ms = timeout_ms;
    const int step = 200; // 200ms 刻み

    while (deadline_ms > 0 && !g_stop) {
        int r = read_hci_event(fd, buf, sizeof(buf), step);
        deadline_ms -= step;
        if (r <= 0) continue; // timeout or error -> ループ継続

        uint8_t evt = (uint8_t)(r & 0xFF);
        uint8_t plen = (uint8_t)((r >> 8) & 0xFF);

        if (evt == EVT_COMMAND_STATUS) {
            if (plen < 4) continue;
            uint8_t status = buf[0];
            uint8_t ncmd = buf[1]; (void)ncmd;
            uint16_t op = buf[2] | (buf[3] << 8);
            if (op == opcode) {
                if (status) {
                    fprintf(stderr, "Command Status error: 0x%02X for opcode 0x%04X\n", status, opcode);
                    return -1;
                }
                // 成功。完了イベント待ちへ続く
            }
        } else if (evt == EVT_COMMAND_COMPLETE) {
            if (plen < 3) continue;
            uint8_t ncmd = buf[0]; (void)ncmd;
            uint16_t op = buf[1] | (buf[2] << 8);
            if (op == opcode) {
                uint8_t status = buf[3];
                if (status) {
                    fprintf(stderr, "Command Complete status error: 0x%02X for opcode 0x%04X\n", status, opcode);
                    return -1;
                }
                size_t rlen = (size_t)(plen - 4);
                if (ret_params && ret_len && *ret_len >= rlen) {
                    memcpy(ret_params, &buf[4], rlen);
                    *ret_len = rlen;
                }
                return 0;
            }
        } else if (evt == EVT_LE_META) {
            // 必要なら受信広告等をログ
            fprintf(stderr, "LE Meta subevent=0x%02X\n", buf[0]);
        }
    }
    fprintf(stderr, "Timeout waiting CC for opcode 0x%04X\n", opcode);
    return -1;
}

static void build_adv_data(uint8_t out[31], uint8_t* out_len, const char* name)
{
    // Flags (LE General Discoverable | BR/EDR Not Supported)
    uint8_t p = 0;
    out[p++] = 2;      // length
    out[p++] = 0x01;   // Flags
    out[p++] = 0x06;   // LE General | BR/EDR Not Supported

    size_t nlen = name ? strlen(name) : 0;
    if (nlen > 29) nlen = 29; // 31 - (2+1) = 28 だが flags 3bytes なので 31-3=28, type 分を含めて 29 に制限
    if (nlen) {
        out[p++] = (uint8_t)(nlen + 1);
        out[p++] = 0x09; // Complete Local Name
        memcpy(&out[p], name, nlen);
        p += (uint8_t)nlen;
    }
    // 残りは 0x00 でパディング
    while (p < 31) out[p++] = 0x00;
    *out_len = p > 31 ? 31 : p;
}

static int enable_advertising(int fd, const char* name)
{
    int rc;

    // 1) Reset
    rc = hci_send_cmd(fd, HCI_OP_RESET, NULL, 0);
    if (rc < 0) return rc;
    rc = wait_cmd_complete(fd, HCI_OP_RESET, NULL, NULL, 5000);
    if (rc < 0) return rc;

    // 2) (任意) LE 機能読み出し
    uint8_t lefeat[8]; size_t lelen = sizeof(lefeat);
    rc = hci_send_cmd(fd, HCI_OP_LE_READ_LOCAL_SUPPORTED_FEATURES, NULL, 0);
    if (rc == 0) wait_cmd_complete(fd, HCI_OP_LE_READ_LOCAL_SUPPORTED_FEATURES, lefeat, &lelen, 2000);

    // 3) Advertising Parameters
    struct __attribute__((packed)) {
        uint16_t min_interval; // 0x00A0 = 100ms
        uint16_t max_interval; // 0x00A0 = 100ms
        uint8_t  adv_type;     // 0x00 ADV_IND
        uint8_t  own_addr_type;// 0x00 Public
        uint8_t  peer_addr_type;// 0x00 Public
        uint8_t  peer_addr[6]; // 00..00
        uint8_t  ch_map;       // 0x07 (37,38,39)
        uint8_t  filter_policy;// 0x00
    } advp = {0};
    advp.min_interval = 0x00A0; // little-endian OK そのまま
    advp.max_interval = 0x00A0;
    advp.adv_type = 0x00;
    advp.own_addr_type = 0x00;
    advp.peer_addr_type = 0x00;
    memset(advp.peer_addr, 0, 6);
    advp.ch_map = 0x07;
    advp.filter_policy = 0x00;

    rc = hci_send_cmd(fd, HCI_OP_LE_SET_ADV_PARAMS, (uint8_t*)&advp, sizeof(advp));
    if (rc < 0) return rc;
    rc = wait_cmd_complete(fd, HCI_OP_LE_SET_ADV_PARAMS, NULL, NULL, 2000);
    if (rc < 0) return rc;

    // 4) Advertising Data
    uint8_t adv_data[31]; uint8_t adv_len = 0;
    build_adv_data(adv_data, &adv_len, name);
    uint8_t set_adv_params[32] = {0};
    set_adv_params[0] = 31; // adv data 長(常に 31 指定が一般的、未使用は 0x00)
    memcpy(&set_adv_params[1], adv_data, 31);

    rc = hci_send_cmd(fd, HCI_OP_LE_SET_ADV_DATA, set_adv_params, sizeof(set_adv_params));
    if (rc < 0) return rc;
    rc = wait_cmd_complete(fd, HCI_OP_LE_SET_ADV_DATA, NULL, NULL, 2000);
    if (rc < 0) return rc;

    // 5) (任意) Scan Response を空で
    uint8_t scanrsp[32] = {0}; scanrsp[0] = 0; // 長さ 0
    rc = hci_send_cmd(fd, HCI_OP_LE_SET_SCAN_RSP_DATA, scanrsp, sizeof(scanrsp));
    if (rc == 0) wait_cmd_complete(fd, HCI_OP_LE_SET_SCAN_RSP_DATA, NULL, NULL, 2000);

    // 6) Enable Advertising
    uint8_t en = 0x01;
    rc = hci_send_cmd(fd, HCI_OP_LE_SET_ADV_ENABLE, &en, 1);
    if (rc < 0) return rc;
    rc = wait_cmd_complete(fd, HCI_OP_LE_SET_ADV_ENABLE, NULL, NULL, 2000);
    if (rc < 0) return rc;

    fprintf(stderr, "Advertising enabled.\n");
    return 0;
}

static int disable_advertising(int fd)
{
    uint8_t en = 0x00;
    int rc = hci_send_cmd(fd, HCI_OP_LE_SET_ADV_ENABLE, &en, 1);
    if (rc == 0) wait_cmd_complete(fd, HCI_OP_LE_SET_ADV_ENABLE, NULL, NULL, 1000);
    return rc;
}

static void usage(const char* prog)
{
    fprintf(stderr,
        "Usage: %s -d <serial_dev> [-b baud] [-n name] [-t seconds]\n"
        "  -d  例: /dev/ttyUSB0, /dev/ttyS4\n"
        "  -b  ボーレート (既定 115200)\n"
        "  -n  広告に載せるデバイス名 (既定 ESP32-HCI)\n"
        "  -t  広告を続ける秒数 (既定 無限、Ctrl-C で停止)\n",
        prog);
}

int main(int argc, char** argv)
{
    const char* dev = NULL;
    int baud = 115200;
    const char* name = "ESP32-HCI";
    int duration = -1; // seconds; -1 = infinite

    int opt;
    while ((opt = getopt(argc, argv, "d:b:n:t:h")) != -1) {
        switch (opt) {
            case 'd': dev = optarg; break;
            case 'b': baud = atoi(optarg); break;
            case 'n': name = optarg; break;
            case 't': duration = atoi(optarg); break;
            case 'h': default: usage(argv[0]); return 1;
        }
    }
    if (!dev) { usage(argv[0]); return 1; }

    signal(SIGINT, on_sigint);
    signal(SIGTERM, on_sigint);

    int fd = open_serial(dev, baud);
    if (fd < 0) return 2;

    if (enable_advertising(fd, name) < 0) {
        fprintf(stderr, "Failed to enable advertising.\n");
        close(fd);
        return 3;
    }

    // 指定秒数だけイベントを読みながら待機
    int elapsed = 0;
    while (!g_stop) {
        uint8_t buf[260];
        int r = read_hci_event(fd, buf, sizeof(buf), 500 /*ms*/);
        (void)r; // ログは関数側が出力
        if (duration > 0) {
            elapsed += 0.5; // おおよそ
            if (elapsed >= duration) break;
        }
    }

    fprintf(stderr, "Stopping advertising...\n");
    disable_advertising(fd);
    close(fd);
    return 0;
}

5.アドバタイズスキャン

ESP32にアドバタイズをスキャンさせ、結果を表示させる。

5-1 コマンド

送信側

HCI Reset
LE Set Scan Parameters
LE Set Scan Enable

受信

HCI Advertising Report

5-2 実行

5-2-1 ビルド実行

gcc -O2 -Wall -o esp32_hci_ble_scan scan_with_name.c
./esp32_hci_ble_scan /dev/ttyUSB0

5-2-2 結果

アドバタイズを受信できるとMACアドレス、RSSI、サービス名が表示される。

ここでは、Arduino IDEのESP32のサンプルBLE_uartを書き込んだESP32のアドバタイズを拾っている結果

Event Type: 0x04, Addr Type: Public MAC Address: 84:F7:03:26:53:6A RSSI: -60 dBm Device Name: UART Service

5-3 コード

コード全文
scan_with_name.c
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <stdint.h>
#include <errno.h>

// HCI Packet Types
#define HCI_COMMAND_PKT   0x01
#define HCI_EVENT_PKT     0x04
#define EVT_LE_META_EVENT 0x3E
#define EVT_LE_ADVERTISING_REPORT 0x02

// Advertising Data Types
#define AD_TYPE_LOCAL_NAME_SHORT 0x08
#define AD_TYPE_LOCAL_NAME_COMPLETE 0x09
#define AD_TYPE_SERVICE_UUID_16_INCOMP 0x02
#define AD_TYPE_SERVICE_UUID_16_COMP   0x03

int debug = 0;

void usage(const char *progname) {
    printf("Usage: %s /dev/ttyUSB0\n", progname);
    exit(1);
}

// UART初期化
int init_uart(const char *device, int baudrate) {
    int fd = open(device, O_RDWR | O_NOCTTY);
    if (fd < 0) { perror("open"); return -1; }
    struct termios tio;
    memset(&tio, 0, sizeof(tio));
    cfsetospeed(&tio, baudrate);
    cfsetispeed(&tio, baudrate);
    tio.c_cflag = baudrate | CS8 | CLOCAL | CREAD;
    tio.c_iflag = IGNPAR;
    tio.c_oflag = 0;
    tio.c_lflag = 0;
    tio.c_cc[VTIME] = 1;
    tio.c_cc[VMIN] = 0;
    tcflush(fd, TCIFLUSH);
    tcsetattr(fd, TCSANOW, &tio);
    return fd;
}

void hexdump(const char *prefix, uint8_t *data, size_t len) {
    printf("%s (%zu bytes): ", prefix, len);
    for (size_t i=0; i<len; i++) printf("%02X ", data[i]);
    printf("\n");
}


// HCI Reset
void hci_reset(int fd) {
    uint8_t cmd_reset[] = { 0x01, 0x03, 0x0C, 0x00 }; // HCI_RESET
    write(fd, cmd_reset, sizeof(cmd_reset));
    usleep(200000); // 200ms待機
}

// HCI Advertising Report の解析関数
void parse_hci_adv_report(const uint8_t *data, size_t len) {
    size_t pos = 0;

    while (pos < len) {
        if (data[pos] != 0x04) { // HCI Event Packet
            if(debug)
            printf("Not an HCI Event packet\n");
            return;
        }
        pos++; // Skip Packet Type

        if (data[pos] != 0x3E) { // LE Meta Event
            if(debug)
            printf("Not a LE Meta Event\n");
            return;
        }
        pos++;

        uint8_t param_len = data[pos++];
        if (pos + param_len > len) {
            if(debug)
            printf("Invalid length\n");
            return;
        }

        uint8_t subevent = data[pos++];
        if (subevent != 0x02) { // LE Advertising Report
            if(debug)
            printf("Not an Advertising Report\n");
            return;
        }

        uint8_t num_reports = data[pos++];

        for (int r = 0; r < num_reports; r++) {
            uint8_t event_type = data[pos++];
            uint8_t addr_type  = data[pos++];

            uint8_t addr[6];
            for (int i = 0; i < 6; i++) {
                addr[i] = data[pos++];
            }

            uint8_t data_len = data[pos++];
            const uint8_t *adv_data = &data[pos];
            pos += data_len;

            // AD Structure の解析
            size_t adv_pos = 0;
            char name[32] = {0};
            while (adv_pos < data_len) {
                uint8_t field_len = adv_data[adv_pos++];
                if (field_len == 0) break;
                uint8_t ad_type = adv_data[adv_pos++];
                uint8_t ad_data_len = field_len - 1;

                if (ad_type == 0x09) { // Complete Local Name
                    if (ad_data_len < sizeof(name))
                        memcpy(name, &adv_data[adv_pos], ad_data_len);
                }

                adv_pos += ad_data_len;
            }

            int8_t rssi = (int8_t)data[pos++];

                printf("Event Type: 0x%02X, Addr Type: %s ",
                   event_type,
                   addr_type == 0 ? "Public" : "Random");
                printf("MAC Address: %02X:%02X:%02X:%02X:%02X:%02X ",
                   addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
                printf("RSSI: %d dBm ", rssi);
                if (name[0]) {
                    printf("Device Name: %s\n", name);
                }else{
                    printf("\n");

                }
                printf("---------------------------\n");
        }
    }
}

// メイン
int main(int argc, char *argv[]) {
    if (argc < 2) usage(argv[0]);

    int fd = init_uart(argv[1], B115200);
    if (fd < 0) return 1;

    hci_reset(fd);

    // LE Set Scan Parameters
    uint8_t scan_params[] = { 0x01,0x0B,0x20,0x07,0x01,0x10,0x00,0x10,0x00,0x00,0x00 };
    write(fd, scan_params, sizeof(scan_params));
    usleep(100000);

    // LE Set Scan Enable
    uint8_t scan_enable[] = {0x01,0x0C,0x20,0x02,0x01,0x00};
    write(fd, scan_enable, sizeof(scan_enable));

    printf("Scanning for BLE devices...\n");

    uint8_t buf[260];
    while (1) {
        int n = read(fd, buf, sizeof(buf));
        if (n > 0) {
            if(debug){
                hexdump("RX", buf, n);
            }
            parse_hci_adv_report(buf, n);
        } else {
            usleep(10000);
        }
    }

    close(fd);
    return 0;
}

6.結果

アドバタイズの送信と受信が、Linuxがわから操作できるようなコードを作成し、送信確認、受信の確認ができた。

1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?