OpenSSL C++でMAVLink Proxyのツールを自作

MAVLinkをネットワーク経由でリモートGCSと通信には、MAVProxyのpythonツールがありますが、SSL／TLSを対応していません。

MAVSecの論文がありますが、実現が難しい（Flight ControllerでMAVLinkプロトコルのPAYLOADを暗号化する処理を追加）、また実現ソースコードにはRC4アルゴリズムの暗号化バグがあります。

OpenSSL APIでMAVLink Proxyの簡単なツールを自作します。

環境

• Ubuntu 18.04.5
• cmake version 3.10.2
• OpenSSL 1.1.1
• QGroundControl 4.0.10
• Pixhawk PX4FMU 2.4.6
• PX4 Firmware 1.9.0dev
• WireShark 2.6.10

OpenSSL 証明書と秘密鍵の作成

テスト用self-signed certificate SSL証明書と秘密鍵を作成します。

openssl req -x509 -newkey rsa:4096 -keyout key.pem -out cert.pem -days 365

SSL／TLSでMAVLinkの通信の暗号化

GCS (QGroundControl) でMAVLink受信UDPポートの設定

OpenSSL MAVLink Proxyの簡単なサンプルコード

mavproxy_ssl_client.cpp

# include <fcntl.h>
# include <termios.h>
# include <netdb.h>
# include <openssl/ssl.h>
# include <sys/ioctl.h>

# define DEV_NAME       "/dev/ttyACM0"       // デバイスファイル名
# define BAUD_RATE      115200               // 通信ボーレート
# define BUFF_SIZE      16*1024              // 適当
# define PROXY_IP       "192.168.11.18"      // PROXY IP Address
# define PROXY_TCP_PORT 4433                 // PROXY TCP Port

int open_serial(const char *dev_name)
{
    int fd;
    fd = open(dev_name, O_RDWR);
    if(fd < 0){
        perror(dev_name);
        exit(EXIT_FAILURE);
    }

    return fd;
}

void init_serial(int fd, unsigned int baud_rate)
{
    struct termios tio;
    memset(&tio, 0, sizeof(tio));
    /*
     * CS8     : 8n1 (8 ビット，ノンパリティ，ストップビット 1)
     * CLOCAL  : ローカル接続，モデム制御なし
     * CREAD   : 受信文字(receiving characters)を有効
     */
    tio.c_cflag = CS8 | CLOCAL | CREAD;
    // キャラクタ間タイマは未使用
    tio.c_cc[VTIME] = 100;
    // ボーレートの設定
    cfsetispeed(&tio, baud_rate);
    cfsetospeed(&tio, baud_rate);
    // デバイスに設定
    tcsetattr(fd,TCSANOW,&tio);
}


int create_socket(int type, int port, const char *ip)
{
    int sd;
    struct sockaddr_in addr;

    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr(ip);
    addr.sin_port = htons(port);

    sd = socket(AF_INET, type, 0);
    if (sd < 0) {
        perror("Unable to create socket");
        exit(EXIT_FAILURE);
    }
    if (connect(sd, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
        perror("Unable connect to server socket");
        exit(EXIT_FAILURE);
    }
    // set non-blocking socket
    int val = 1;
    ioctl(sd, FIONBIO, &val);

    return sd;
}

void init_openssl()
{
    SSL_load_error_strings();
    OpenSSL_add_ssl_algorithms();
}

void cleanup_openssl()
{
    EVP_cleanup();
}

SSL_CTX *create_ssl_context()
{
    const SSL_METHOD *method;
    SSL_CTX *ctx;

    method = SSLv23_client_method();
    ctx = SSL_CTX_new(method);
    if (!ctx) {
        perror("Unable to create SSL context");
        ERR_print_errors_fp(stderr);
        exit(EXIT_FAILURE);
    }

    return ctx;
}

SSL *connect_ssl(SSL_CTX *ctx, int socket)
{
    SSL *ssl;

    // create SSL stack endpoint
    ssl = SSL_new(ctx);
    // attach SSL stack to socket
    SSL_set_fd(ssl, socket);
    // initiate SSL handshake
    if (SSL_connect(ssl) < 0) {
        perror("Unable initiate SSL handshake");
//        exit(EXIT_FAILURE);
    }

    return ssl;
}

int main(int argc, char **argv)
{
    int fd_dev;
    // Pixhawk シリアルポートの開く
    fd_dev = open_serial(DEV_NAME);
    // Pixhawk シリアルポートの初期化
    init_serial(fd_dev, BAUD_RATE);
    printf("Serial connection to '%s', baud_rate %d\n", DEV_NAME, BAUD_RATE);

    int fd_proxy;
    SSL_CTX *ctx;
    SSL *ssl;
    // OpenSSLの初期化
    init_openssl();
    // SSL_CTXオブジェクトの生成
    ctx = create_ssl_context();
    printf("SSL context initialized\n");
    // PROXY TCPソケットの作成
    fd_proxy = create_socket(SOCK_STREAM, PROXY_TCP_PORT, PROXY_IP);
    printf("TCP connection to '%s', port %d\n", PROXY_IP, PROXY_TCP_PORT);
    // PROXY TCPソケットでSSLハンドシェイクの接続
    ssl = connect_ssl(ctx, fd_proxy);
    printf("SSL endpoint created & handshake completed\n");

    int len = 0;
    unsigned char buffer[BUFF_SIZE];
    while(1) {
        // PROXY SSLからMAVLinkを受信
        memset(&buffer, 0, sizeof(buffer));
        len = SSL_read(ssl, buffer, sizeof(buffer));
        if (len > 0) {
            // PixhawkにMAVLinkを送信
            write(fd_dev, buffer, len);
            // For debug
            for(int i = 0; i < len; i++){
                printf("%02X ",buffer[i]);
            }
            printf("\n\n");
        }
        // PixhawkからMAVLinkを受信
        memset(&buffer, 0, sizeof(buffer));
        len = read(fd_dev, buffer, sizeof(buffer));
        if(len == 0){
            continue;
        }
        if(len < 0){
            perror("Unable read serial data");
            exit(EXIT_FAILURE);
        }
        // PROXY SSLにMAVLinkを送信
        SSL_write(ssl, buffer, len);
    }

    SSL_shutdown(ssl);
    SSL_free (ssl);
    SSL_CTX_free (ctx);
    cleanup_openssl();
    close(fd_proxy);
    close(fd_dev);

    return EXIT_SUCCESS;
}

mavproxy_ssl_server.cpp

# include <unistd.h>
# include <string.h>
# include <arpa/inet.h>
# include <openssl/ssl.h>
# include <sys/ioctl.h>

# define BUFF_SIZE      16*1024              // 適当
# define PROXY_TCP_PORT 4433                 // GCS TCP Port
# define GCS_IP         "127.0.0.1"          // GCS IP Address
# define GCS_UDP_PORT   14540                // GCS UDP Port

int create_listen_socket(int port)
{
    int s;
    struct sockaddr_in addr;

    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);
    addr.sin_addr.s_addr = htonl(INADDR_ANY);

    s = socket(AF_INET, SOCK_STREAM, 0);
    if (s < 0) {
        perror("Unable to create listen socket");
        exit(EXIT_FAILURE);
    }

    if (bind(s, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
        perror("Unable to bind listen socket");
        exit(EXIT_FAILURE);
    }

    if (listen(s, 1) < 0) {
        perror("Unable to listen socket");
        exit(EXIT_FAILURE);
    }

    return s;
}

int create_udp_socket(const char *ip_address, int port)
{
    int s;
    struct sockaddr_in addr;

    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr(ip_address);
    addr.sin_port = htons(port);

    s = socket(AF_INET, SOCK_DGRAM, 0);
    if (s < 0) {
        perror("Unable to create udp socket");
        exit(EXIT_FAILURE);
    }

    if (connect(s, (struct sockaddr *)&addr, sizeof(struct sockaddr)) < 0) {
        printf("Unable to connect %s : %d", ip_address, port);
        perror("");
        exit(EXIT_FAILURE);
    }
    // set non blocking socket
    int val = 1;
    ioctl(s, FIONBIO, &val);

    return s;
}

void init_openssl()
{
    SSL_load_error_strings();
    OpenSSL_add_ssl_algorithms();
}

void cleanup_openssl()
{
    EVP_cleanup();
}

SSL_CTX *create_context()
{
    const SSL_METHOD *method;
    SSL_CTX *ctx;

    method = SSLv23_server_method();

    ctx = SSL_CTX_new(method);
    if (!ctx) {
        perror("Unable to create SSL context");
        exit(EXIT_FAILURE);
    }

    return ctx;
}

void configure_context(SSL_CTX *ctx)
{
    SSL_CTX_set_ecdh_auto(ctx, 1);

    /* Set the key and cert */
    if (SSL_CTX_use_certificate_file(ctx, "cert.pem", SSL_FILETYPE_PEM) <= 0) {
        exit(EXIT_FAILURE);
    }

    if (SSL_CTX_use_PrivateKey_file(ctx, "key.pem", SSL_FILETYPE_PEM) <= 0 ) {
        exit(EXIT_FAILURE);
    }
}

SSL *accept_ssl(SSL_CTX *ctx, int socket)
{
    SSL *ssl;

    // create SSL stack endpoint
    ssl = SSL_new(ctx);
    // attach SSL stack to socket
    SSL_set_fd(ssl, socket);
    // initiate SSL handshake
    if (SSL_accept(ssl) <= 0) {
        perror("Unable initiate SSL handshake");
        exit(EXIT_FAILURE);
    }

    return ssl;
}

int main(int argc, char **argv)
{
    int fd_gcs;
    // GCS UDPソケットの作成
    fd_gcs = create_udp_socket(GCS_IP, GCS_UDP_PORT);
    printf("UDP connection to '%s', port %d\n", GCS_IP, GCS_UDP_PORT);

    int sock;
    int client;
    SSL_CTX *ctx;
    SSL *ssl;
    // OpenSSLの初期化
    init_openssl();
    // SSL_CTXオブジェクトの生成
    ctx = create_context();
    printf("SSL context initialized\n");
    // SSL証明書と秘密鍵の登録
    configure_context(ctx);
    printf("SSL context configured\n");
    // TCPソケットの作成
    sock = create_listen_socket(PROXY_TCP_PORT);
    printf("proxy socket created.\n");
    // クライアントから接続待機
    struct sockaddr_in addr;
    uint len_addr = sizeof(addr);
    client = accept(sock, (struct sockaddr*)&addr, &len_addr);
    if (client < 0) {
        perror("Unable to accept socket");
        exit(EXIT_FAILURE);
    }
    printf("client socket accepted.\n");
    // SSLハンドシェイクの接続
    ssl = accept_ssl(ctx, client);
    printf("SSL endpoint created & handshake completed\n");

    int len = 0;
    unsigned char buffer[BUFF_SIZE];
    while(1) {
        // SSL からMAVLinkを受信
        memset(&buffer, 0, sizeof(buffer));
        len = SSL_read(ssl, buffer, sizeof(buffer));
        printf("read from ssl %d byte\n", len);
        if (len == 0) {
            sleep(1);
            continue;
        }
        if (len < 0) {
            perror("Unable read from ssl");
            break;
        }

        // GCS にMAVLinkを送信
        send(fd_gcs, buffer, len, 0);
        printf("send to gcs %d byte\n", len);

        // GCS からMAVLinkを受信
        memset(&buffer, 0, sizeof(buffer));
        len = recv(fd_gcs, buffer, sizeof(buffer), 0);
        printf("recv from gcs %d byte\n", len);

        // SSL にMAVLinkを送信
        if (len > 0) {
            SSL_write(ssl, buffer, len);
            printf("write to ssl %d byte\n", len);
            // For debug
            for (int i=0; i < len; i++) {
                printf("%02X ", buffer[i]);
            }
            printf("\n\n");
        }
    }

    SSL_shutdown(ssl);
    SSL_free(ssl);
    close(client);

    close(sock);
    SSL_CTX_free(ctx);
    cleanup_openssl();
}