usb_moded-worker.c

Go to the documentation of this file.

 
#include "usb_moded-worker.h"
 
#include "usb_moded.h"
#include "usb_moded-android.h"
#include "usb_moded-configfs.h"
#include "usb_moded-control.h"
#include "usb_moded-log.h"
#include "usb_moded-modes.h"
#include "usb_moded-modesetting.h"
#include "usb_moded-modules.h"
#include "usb_moded-appsync.h"
 
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/eventfd.h>
 
#include <pthread.h> // NOTRIM
#include <unistd.h>
#include <pwd.h>
 
/* ========================================================================= *
 * Types
 * ========================================================================= */

typedef enum {
    DEVSTATE_UNKNOWN,
    DEVSTATE_UNMOUNTED,
    DEVSTATE_MOUNTED,
} devstate_t;
 
static const char * const devstate_name[] = {
  [DEVSTATE_UNKNOWN]   = "unknown",
  [DEVSTATE_UNMOUNTED] = "unmounted",
  [DEVSTATE_MOUNTED]   = "mounted",
};
 
/* ========================================================================= *
 * Prototypes
 * ========================================================================= */
 
/* ------------------------------------------------------------------------- *
 * WORKER
 * ------------------------------------------------------------------------- */
 
static bool        worker_thread_p                 (void);
bool               worker_bailing_out              (void);
static devstate_t  worker_get_mtp_device_state     (void);
static void        worker_unmount_mtp_device       (void);
static bool        worker_mount_mtp_device         (void);
static bool        worker_mode_is_mtp_mode         (const char *mode);
static bool        worker_is_mtpd_running          (void);
static bool        worker_mtpd_running_p           (void *aptr);
static bool        worker_mtpd_stopped_p           (void *aptr);
static bool        worker_stop_mtpd                (void);
static bool        worker_start_mtpd               (void);
static bool        worker_switch_to_charging       (void);
const char        *worker_get_kernel_module        (void);
bool               worker_set_kernel_module        (const char *module);
void               worker_clear_kernel_module      (void);
const modedata_t  *worker_get_usb_mode_data        (void);
modedata_t        *worker_dup_usb_mode_data        (void);
void               worker_set_usb_mode_data        (const modedata_t *data);
static const char *worker_get_activated_mode_locked(void);
static bool        worker_set_activated_mode_locked(const char *mode);
static const char *worker_get_requested_mode_locked(void);
static bool        worker_set_requested_mode_locked(const char *mode);
void               worker_request_hardware_mode    (const char *mode);
void               worker_clear_hardware_mode      (void);
static void        worker_execute                  (void);
static void        worker_switch_to_mode           (const char *mode);
static guint       worker_add_iowatch              (int fd, bool close_on_unref, GIOCondition cnd, GIOFunc io_cb, gpointer aptr);
static void       *worker_thread_cb                (void *aptr);
static gboolean    worker_notify_cb                (GIOChannel *chn, GIOCondition cnd, gpointer data);
static bool        worker_start_thread             (void);
static void        worker_stop_thread              (void);
static void        worker_delete_eventfd           (void);
static bool        worker_create_eventfd           (void);
bool               worker_init                     (void);
void               worker_quit                     (void);
void               worker_wakeup                   (void);
static void        worker_notify                   (void);
 
/* ========================================================================= *
 * Data
 * ========================================================================= */
 
static pthread_t worker_thread_id = 0;
 
static pthread_mutex_t  worker_mutex = PTHREAD_MUTEX_INITIALIZER;

static volatile bool worker_bailout_requested = false;

static volatile bool worker_bailout_handled = false;
 
#define WORKER_LOCKED_ENTER do {\
    if( pthread_mutex_lock(&worker_mutex) != 0 ) { \
        log_crit("WORKER LOCK FAILED");\
        _exit(EXIT_FAILURE);\
    }\
}while(0)
 
#define WORKER_LOCKED_LEAVE do {\
    if( pthread_mutex_unlock(&worker_mutex) != 0 ) { \
        log_crit("WORKER UNLOCK FAILED");\
        _exit(EXIT_FAILURE);\
    }\
}while(0)
 
/* ========================================================================= *
 * Functions
 * ========================================================================= */
 
static bool
worker_thread_p(void)
{
    LOG_REGISTER_CONTEXT;
 
    return worker_thread_id && worker_thread_id == pthread_self();
}
 
bool
worker_bailing_out(void)
{
    LOG_REGISTER_CONTEXT;
 
    // ref: see common_msleep_()
    return (worker_thread_p() &&
            worker_bailout_requested &&
            !worker_bailout_handled);
}
 
/* ------------------------------------------------------------------------- *
 * MTP_DEVICE
 * ------------------------------------------------------------------------- */

static devstate_t
worker_get_mtp_device_state(void)
{
    LOG_REGISTER_CONTEXT;
 
    devstate_t state = DEVSTATE_UNKNOWN;
 
    if( access("/dev/mtp/ep0", F_OK) == 0 )
        state = DEVSTATE_MOUNTED;
    else if( errno == ENOENT )
        state = DEVSTATE_UNMOUNTED;
    else
        log_warning("/dev/mtp/ep0: %m");
 
    log_debug("mtp device state = %s", devstate_name[state]);
    return state;
}

static void
worker_unmount_mtp_device(void)
{
    LOG_REGISTER_CONTEXT;
 
    if( worker_get_mtp_device_state() != DEVSTATE_UNMOUNTED ) {
        log_debug("unmounting mtp device");
        common_system("/bin/umount /dev/mtp");
    }
}

static bool
worker_mount_mtp_device(void)
{
    LOG_REGISTER_CONTEXT;
 
    bool mounted = false;
 
    /* Fail if control endpoint is already present */
    if( worker_get_mtp_device_state() != DEVSTATE_UNMOUNTED ) {
        log_err("mtp device already mounted");
        goto EXIT;
    }
 
    /* Ensure that device directory exists */
    if( mkdir("/dev/mtp", 0755) == -1 && errno != EEXIST ) {
        log_err("failed to create /dev/mtp directory: %m");
        goto EXIT;
    }
 
    /* Probe currently active user for uid/gid info. In case these
     * can't be obtained, use values for default user as fallback. */
    gid_t gid = 100000;
    uid_t uid = usbmoded_get_current_user();
    if( uid == UID_UNKNOWN )
        uid = 100000;
 
    struct passwd *pw = getpwuid(uid);
    if( pw )
        gid = pw->pw_gid;
 
    /* Attempt to mount mtp device using root uid and primary
     * gid of the current user.
     */
    char cmd[256];
    snprintf(cmd, sizeof cmd,
             "/bin/mount -o mode=0770,uid=0,gid=%u -t functionfs mtp /dev/mtp",
             (unsigned)gid);
 
    log_debug("mounting mtp device");
    if( common_system(cmd) != 0 )
        goto EXIT;
 
    /* Check that control endpoint is present */
    if( worker_get_mtp_device_state() != DEVSTATE_MOUNTED ) {
        log_err("mtp control not mounted");
        goto EXIT;
    }
 
    mounted = true;
 
EXIT:
    return mounted;
}
 
/* ------------------------------------------------------------------------- *
 * MTP_DAEMON
 * ------------------------------------------------------------------------- */

static unsigned worker_mtp_start_delay = 120 * 1000;

static unsigned worker_mtp_stop_delay  =  15 * 1000;

static bool worker_mtp_service_started = false;
 
static bool worker_mode_is_mtp_mode(const char *mode)
{
    LOG_REGISTER_CONTEXT;
 
    return mode && !strcmp(mode, "mtp_mode");
}
 
static bool worker_is_mtpd_running(void)
{
    LOG_REGISTER_CONTEXT;
 
    /* ep0 becomes available when /dev/mtp is mounted.
     *
     * ep1, ep2, ep3 exist while mtp daemon is running,
     * has ep0 opened and has written config data to it.
     */
    static const char * const lut[] = {
        "/dev/mtp/ep0",
        "/dev/mtp/ep1",
        "/dev/mtp/ep2",
        "/dev/mtp/ep3",
        0
    };
 
    bool ack = true;
 
    for( size_t i = 0; lut[i]; ++i ) {
        if( access(lut[i], F_OK) == -1 ) {
            ack = false;
            break;
        }
    }
 
    return ack;
}
 
static bool
worker_mtpd_running_p(void *aptr)
{
    LOG_REGISTER_CONTEXT;
 
    (void)aptr;
    return worker_is_mtpd_running();
}
 
static bool
worker_mtpd_stopped_p(void *aptr)
{
    LOG_REGISTER_CONTEXT;
 
    (void)aptr;
    return !worker_is_mtpd_running();
}
 
static bool
worker_stop_mtpd(void)
{
    LOG_REGISTER_CONTEXT;
 
    bool ack = false;
 
    if( !worker_mtp_service_started && worker_mtpd_stopped_p(0) ) {
        log_debug("mtp daemon is not running");
        goto SUCCESS;
    }
 
    int rc = common_system("systemctl-user stop buteo-mtp.service");
    if( rc != 0 ) {
        log_warning("failed to stop mtp daemon; exit code = %d", rc);
        goto FAILURE;
    }
 
    /* Have succesfully stopped mtp service */
    worker_mtp_service_started = false;
 
    if( common_wait(worker_mtp_stop_delay, worker_mtpd_stopped_p, 0) != WAIT_READY ) {
        log_warning("failed to stop mtp daemon; giving up");
        goto FAILURE;
    }
 
    log_debug("mtp daemon has stopped");
 
SUCCESS:
    ack = true;
 
FAILURE:
    return ack;
}
 
static bool
worker_start_mtpd(void)
{
    LOG_REGISTER_CONTEXT;
 
    bool ack = false;
 
    if( worker_mtpd_running_p(0) ) {
        log_debug("mtp daemon is running");
        goto SUCCESS;
    }
 
    /* Have attempted to start mtp service */
    worker_mtp_service_started = true;
 
    int rc = common_system("systemctl-user start buteo-mtp.service");
    if( rc != 0 ) {
        log_warning("failed to start mtp daemon; exit code = %d", rc);
        goto FAILURE;
    }
 
    if( common_wait(worker_mtp_start_delay, worker_mtpd_running_p, 0) != WAIT_READY ) {
        log_warning("failed to start mtp daemon; giving up");
        goto FAILURE;
    }
 
    log_debug("mtp daemon has started");
 
SUCCESS:
    ack = true;
 
FAILURE:
    return ack;
}
 
static bool worker_switch_to_charging(void)
{
    LOG_REGISTER_CONTEXT;
 
    bool ack = true;
 
    if( android_set_charging_mode() )
        goto SUCCESS;
 
    if( configfs_set_charging_mode() )
        goto SUCCESS;
 
    if( modules_in_use() ) {
        if( worker_set_kernel_module(MODULE_MASS_STORAGE) )
            goto SUCCESS;
        worker_set_kernel_module(MODULE_NONE);
    }
 
    log_err("switch to charging mode failed");
 
    ack = false;
SUCCESS:
    return ack;
}
 
/* ------------------------------------------------------------------------- *
 * KERNEL_MODULE
 * ------------------------------------------------------------------------- */

static char *worker_kernel_module = NULL;

const char * worker_get_kernel_module(void)
{
    LOG_REGISTER_CONTEXT;
 
    return worker_kernel_module ?: MODULE_NONE;
}

bool worker_set_kernel_module(const char *module)
{
    LOG_REGISTER_CONTEXT;
 
    bool ack = false;
 
    if( !module )
        module = MODULE_NONE;
 
    const char *current = worker_get_kernel_module();
 
    log_debug("current module: %s -> %s", current, module);
 
    if( !g_strcmp0(current, module) )
        goto SUCCESS;
 
    if( modules_unload_module(current) != 0 )
        goto EXIT;
 
    free(worker_kernel_module), worker_kernel_module = 0;
 
    if( modules_load_module(module) != 0 )
        goto EXIT;
 
    if( g_strcmp0(module, MODULE_NONE) )
        worker_kernel_module = strdup(module);
 
SUCCESS:
    ack = true;
EXIT:
    return ack;
}
 
void worker_clear_kernel_module(void)
{
    LOG_REGISTER_CONTEXT;
 
    free(worker_kernel_module), worker_kernel_module = 0;
}
 
/* ------------------------------------------------------------------------- *
 * MODE_DATA
 * ------------------------------------------------------------------------- */

static modedata_t *worker_mode_data = NULL;

const modedata_t *worker_get_usb_mode_data(void)
{
    LOG_REGISTER_CONTEXT;
 
    return worker_mode_data;
}

modedata_t *worker_dup_usb_mode_data(void)
{
    LOG_REGISTER_CONTEXT;
 
    WORKER_LOCKED_ENTER;
 
    modedata_t *modedata = modedata_copy(worker_mode_data);
 
    WORKER_LOCKED_LEAVE;
 
    return modedata;;
}

void worker_set_usb_mode_data(const modedata_t *data)
{
    LOG_REGISTER_CONTEXT;
 
    WORKER_LOCKED_ENTER;
 
    modedata_free(worker_mode_data),
        worker_mode_data = modedata_copy(data);
 
    WORKER_LOCKED_LEAVE;
}
 
/* ------------------------------------------------------------------------- *
 * HARDWARE_MODE
 * ------------------------------------------------------------------------- */
 
/* The hardware mode name
 *
 * How the usb hardware has been configured.
 *
 * For example internal_mode=MODE_ASK gets
 * mapped to hardware_mode=MODE_CHARGING */
static gchar *worker_requested_mode = NULL;
 
static gchar *worker_activated_mode = NULL;
 
static const char *
worker_get_activated_mode_locked(void)
{
    LOG_REGISTER_CONTEXT;
 
    return worker_activated_mode ?: MODE_UNDEFINED;
}
 
static bool
worker_set_activated_mode_locked(const char *mode)
{
    LOG_REGISTER_CONTEXT;
 
    bool changed = false;
    const char *prev = worker_get_activated_mode_locked();
 
    if( !g_strcmp0(prev, mode) )
        goto EXIT;
 
    log_debug("activated_mode: %s -> %s", prev, mode);
    g_free(worker_activated_mode),
        worker_activated_mode =  g_strdup(mode);
    changed = true;
 
EXIT:
    return changed;
}
 
static const char *
worker_get_requested_mode_locked(void)
{
    LOG_REGISTER_CONTEXT;
 
    return worker_requested_mode ?: MODE_UNDEFINED;
}
 
static bool
worker_set_requested_mode_locked(const char *mode)
{
    LOG_REGISTER_CONTEXT;
 
    bool changed = false;
    const char *prev = worker_get_requested_mode_locked();
 
    if( !g_strcmp0(prev, mode) )
        goto EXIT;
 
    log_debug("requested_mode: %s -> %s", prev, mode);
    g_free(worker_requested_mode),
        worker_requested_mode =  g_strdup(mode);
    changed = true;
 
EXIT:
    return changed;
}
 
void worker_request_hardware_mode(const char *mode)
{
    LOG_REGISTER_CONTEXT;
 
    WORKER_LOCKED_ENTER;
 
    if( !worker_set_requested_mode_locked(mode) )
        goto EXIT;
 
    worker_wakeup();
 
EXIT:
    WORKER_LOCKED_LEAVE;
    return;
}
 
void worker_clear_hardware_mode(void)
{
    LOG_REGISTER_CONTEXT;
 
    WORKER_LOCKED_ENTER;
    g_free(worker_requested_mode), worker_requested_mode = 0;
    WORKER_LOCKED_LEAVE;
}
 
static void
worker_execute(void)
{
    LOG_REGISTER_CONTEXT;
 
    WORKER_LOCKED_ENTER;
 
    const char *activated = worker_get_activated_mode_locked();
    const char *requested = worker_get_requested_mode_locked();
    const char *activate  = common_map_mode_to_hardware(requested);
 
    log_debug("activated = %s", activated);
    log_debug("requested = %s", requested);
    log_debug("activate = %s",   activate);
 
    bool changed = g_strcmp0(activated, activate) != 0;
    gchar *mode  = g_strdup(activate);
 
    WORKER_LOCKED_LEAVE;
 
    if( changed )
        worker_switch_to_mode(mode);
    else
        worker_notify();
 
    g_free(mode);
 
    return;
}
 
/* ------------------------------------------------------------------------- *
 * MODE_SWITCH
 * ------------------------------------------------------------------------- */
 
static void
worker_switch_to_mode(const char *mode)
{
    LOG_REGISTER_CONTEXT;
 
    const char *override = 0;
    modedata_t *data     = 0;
 
    /* set return to 1 to be sure to error out if no matching mode is found either */
 
    log_debug("Cleaning up previous mode");
 
    /* Either mtp daemon is not needed, or it must be *started* in
     * correct phase of gadget configuration when entering mtp mode.
     *
     * Similarly, unmount mtp device to make sure sure it gets mounted
     * with appropriate uid/gid values when it is actually needed.
     */
    worker_stop_mtpd();
    worker_unmount_mtp_device();
 
    if( worker_get_usb_mode_data() ) {
        modesetting_leave_dynamic_mode();
        worker_set_usb_mode_data(NULL);
    }
 
    /* Mode specific applications have been stopped and we can
     * take updated appsync configuration in use.
     */
    appsync_switch_configuration();
 
    log_debug("Setting %s\n", mode);
 
    /* Mode mapping should mean we only see MODE_CHARGING here, but just
     * in case redirect fixed charging related things to charging ... */
 
    if( !strcmp(mode, MODE_CHARGING) ||
        !strcmp(mode, MODE_CHARGING_FALLBACK) ||
        !strcmp(mode, MODE_CHARGER) ||
        !strcmp(mode, MODE_UNDEFINED) ||
        !strcmp(mode, MODE_ASK)) {
        goto CHARGE;
    }
 
    if( !usbmoded_can_export() ) {
        log_warning("Policy does not allow mode: %s", mode);
        goto FAILED;
    }
 
    if( (data = usbmoded_dup_modedata(mode)) ) {
        log_debug("Matching mode %s found.\n", mode);
 
        /* set data before calling any of the dynamic mode functions
         * as they will use the worker_get_usb_mode_data function */
        worker_set_usb_mode_data(data);
 
        /* When dealing with configfs, we can't enable UDC without
         * already having mtpd running */
        if( worker_mode_is_mtp_mode(mode) && configfs_in_use() ) {
            if( !worker_mount_mtp_device() )
                goto FAILED;
            if( !worker_start_mtpd() )
                goto FAILED;
        }
 
        if( !worker_set_kernel_module(data->mode_module) )
            goto FAILED;
 
        if( !modesetting_enter_dynamic_mode() )
            goto FAILED;
 
        /* When dealing with android usb, it must be enabled before
         * we can start mtpd. Assumption is that the same applies
         * when using kernel modules. */
        if( worker_mode_is_mtp_mode(mode) && !configfs_in_use() ) {
            if( !worker_mount_mtp_device() )
                goto FAILED;
            if( !worker_start_mtpd() )
                goto FAILED;
        }
 
        goto SUCCESS;
    }
 
    log_warning("Matching mode %s was not found.", mode);
 
FAILED:
    worker_bailout_handled = true;
 
    /* Undo any changes we might have might have already done */
    if( worker_get_usb_mode_data() ) {
        log_debug("Cleaning up failed mode switch");
        worker_stop_mtpd();
        modesetting_leave_dynamic_mode();
        worker_set_usb_mode_data(NULL);
    }
 
    /* From usb configuration point of view MODE_UNDEFINED and
     * MODE_CHARGING are the same, but for the purposes of exposing
     * a sane state over D-Bus we need to differentiate between
     * "failure to set mode" and "aborting mode setting due to cable
     * disconnect" by inspecting whether target mode has been
     * switched to undefined.
     */
    WORKER_LOCKED_ENTER;
    const char *requested = worker_get_requested_mode_locked();
    if( !g_strcmp0(requested, MODE_UNDEFINED) )
        override = MODE_UNDEFINED;
    else
        override = MODE_CHARGING;
    WORKER_LOCKED_LEAVE;
    log_warning("mode setting failed, try %s", override);
 
CHARGE:
    if( worker_switch_to_charging() )
        goto SUCCESS;
 
    log_crit("failed to activate charging, all bets are off");
 
    /* FIXME: double check this error path */
 
    /* If we get here then usb_module loading failed,
     * no mode matched, and charging setup failed too.
     */
 
    override = MODE_UNDEFINED;
    log_warning("mode setting failed, fallback to %s", override);
    worker_set_kernel_module(MODULE_NONE);
 
SUCCESS:
 
    WORKER_LOCKED_ENTER;
    if( override ) {
        worker_set_requested_mode_locked(override);
        override = common_map_mode_to_hardware(override);
        worker_set_activated_mode_locked(override);
    }
    else {
        worker_set_activated_mode_locked(mode);
    }
    WORKER_LOCKED_LEAVE;
 
    worker_notify();
 
    modedata_free(data);
 
    return;
}
 
/* ------------------------------------------------------------------------- *
 * WORKER_THREAD
 * ------------------------------------------------------------------------- */

static int              worker_req_evfd  = -1;

static int              worker_rsp_evfd  = -1;

static guint            worker_rsp_wid   = 0;
 
static guint
worker_add_iowatch(int fd, bool close_on_unref,
               GIOCondition cnd, GIOFunc io_cb, gpointer aptr)
{
    LOG_REGISTER_CONTEXT;
 
    guint         wid = 0;
    GIOChannel   *chn = 0;
 
    if( !(chn = g_io_channel_unix_new(fd)) )
        goto cleanup;
 
    g_io_channel_set_close_on_unref(chn, close_on_unref);
 
    cnd |= G_IO_ERR | G_IO_HUP | G_IO_NVAL;
 
    if( !(wid = g_io_add_watch(chn, cnd, io_cb, aptr)) )
        goto cleanup;
 
cleanup:
    if( chn != 0 ) g_io_channel_unref(chn);
 
    return wid;
 
}
 
static void *worker_thread_cb(void *aptr)
{
    LOG_REGISTER_CONTEXT;
 
    (void)aptr;
 
    /* Async cancellation, but disabled */
    pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, 0);
    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, 0);
 
    /* Leave INT/TERM signal processing up to the main thread */
    sigset_t ss;
    sigemptyset(&ss);
    sigaddset(&ss, SIGINT);
    sigaddset(&ss, SIGTERM);
    pthread_sigmask(SIG_BLOCK, &ss, 0);
 
    /* Loop until explicitly canceled */
    for( ;; ) {
        /* Async cancellation point at wait() */
        pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, 0);
        uint64_t cnt = 0;
        int rc = read(worker_req_evfd, &cnt, sizeof cnt);
        pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, 0);
 
        if( rc == -1 ) {
            if( errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK )
                continue;
            log_err("read: %m");
            goto EXIT;
        }
 
        if( rc != sizeof cnt )
            continue;
 
        if( cnt > 0 ) {
            worker_bailout_requested = false;
            worker_bailout_handled = false;
            worker_execute();
        }
 
    }
EXIT:
    return 0;
}
 
static gboolean
worker_notify_cb(GIOChannel *chn, GIOCondition cnd, gpointer data)
{
    LOG_REGISTER_CONTEXT;
 
    (void)data;
 
    gboolean keep_going = FALSE;
 
    if( !worker_rsp_wid )
        goto cleanup_nak;
 
    int fd = g_io_channel_unix_get_fd(chn);
 
    if( fd < 0 )
        goto cleanup_nak;
 
    if( cnd & ~G_IO_IN )
        goto cleanup_nak;
 
    if( !(cnd & G_IO_IN) )
        goto cleanup_ack;
 
    uint64_t cnt = 0;
 
    int rc = read(fd, &cnt, sizeof cnt);
 
    if( rc == 0 ) {
        log_err("unexpected eof");
        goto cleanup_nak;
    }
 
    if( rc == -1 ) {
        if( errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK )
            goto cleanup_ack;
 
        log_err("read error: %m");
        goto cleanup_nak;
    }
 
    if( rc != sizeof cnt )
        goto cleanup_nak;
 
    {
        WORKER_LOCKED_ENTER;
        const char *mode = worker_get_requested_mode_locked();
        gchar *work = g_strdup(mode);
        WORKER_LOCKED_LEAVE;
 
        control_mode_switched(work);
        g_free(work);
    }
 
cleanup_ack:
    keep_going = TRUE;
 
cleanup_nak:
 
    if( !keep_going ) {
        worker_rsp_wid = 0;
        log_crit("worker notifications disabled");
    }
 
    return keep_going;
}
 
static bool
worker_start_thread(void)
{
    LOG_REGISTER_CONTEXT;
 
    bool ack = false;
    int err = pthread_create(&worker_thread_id, 0, worker_thread_cb, 0);
    if( err ) {
        worker_thread_id = 0;
        log_err("failed to start worker thread");
    }
    else {
        ack = true;
        log_debug("worker thread started");
    }
 
    return ack;
}
 
static void
worker_stop_thread(void)
{
    LOG_REGISTER_CONTEXT;
 
    if( !worker_thread_id )
        goto EXIT;
 
    log_debug("stopping worker thread");
    int err = pthread_cancel(worker_thread_id);
    if( err ) {
        log_err("failed to cancel worker thread");
    }
    else {
        log_debug("waiting for worker thread to exit ...");
        void *ret = 0;
        struct timespec tmo = { 0, 0};
        clock_gettime(CLOCK_REALTIME, &tmo);
        tmo.tv_sec += 3;
        err = pthread_timedjoin_np(worker_thread_id, &ret, &tmo);
        if( err ) {
            log_err("worker thread did not exit");
        }
        else {
            log_debug("worker thread terminated");
            worker_thread_id = 0;
        }
    }
 
    if( worker_thread_id ) {
        /* Orderly exit is not safe, just die */
        _exit(EXIT_FAILURE);
    }
 
EXIT:
    return;
}
 
static void
worker_delete_eventfd(void)
{
    LOG_REGISTER_CONTEXT;
 
    if( worker_req_evfd != -1 )
        close(worker_req_evfd), worker_req_evfd = -1;
 
    if( worker_rsp_wid )
        g_source_remove(worker_rsp_wid), worker_rsp_wid = 0;
 
    if( worker_rsp_evfd != -1 )
        close(worker_rsp_evfd), worker_req_evfd = -1;
}
 
static bool
worker_create_eventfd(void)
{
    LOG_REGISTER_CONTEXT;
 
    bool ack = false;
 
    /* Setup notify pipeline */
 
    if( (worker_rsp_evfd = eventfd(0, EFD_CLOEXEC | EFD_NONBLOCK)) == -1 )
        goto EXIT;
 
    worker_rsp_wid = worker_add_iowatch(worker_rsp_evfd, false, G_IO_IN,
                                worker_notify_cb, 0);
    if( !worker_rsp_wid )
        goto EXIT;
 
    /* Setup request pipeline */
 
    if( (worker_req_evfd = eventfd(0, EFD_CLOEXEC)) == -1 )
        goto EXIT;
 
    ack = true;
 
EXIT:
 
    return ack;
}
 
bool
worker_init(void)
{
    LOG_REGISTER_CONTEXT;
 
    bool ack = false;
 
    if( !worker_create_eventfd() )
        goto EXIT;
 
    if( !worker_start_thread() )
        goto EXIT;
 
    ack = true;
 
EXIT:
    if( !ack )
        worker_quit();
 
    return ack;
}
 
void
worker_quit(void)
{
    LOG_REGISTER_CONTEXT;
 
    worker_stop_thread();
    worker_delete_eventfd();
 
    /* Worker thread is stopped and resources can be released. */
    worker_set_usb_mode_data(0);
}
 
void
worker_wakeup(void)
{
    LOG_REGISTER_CONTEXT;
 
    worker_bailout_requested = true;
 
    uint64_t cnt = 1;
    if( write(worker_req_evfd, &cnt, sizeof cnt) == -1 ) {
        log_err("failed to signal requested: %m");
    }
}
 
static void
worker_notify(void)
{
    LOG_REGISTER_CONTEXT;
 
    uint64_t cnt = 1;
    if( write(worker_rsp_evfd, &cnt, sizeof cnt) == -1 ) {
        log_err("failed to signal handled: %m");
    }
}