usb_moded-worker.c 24.9 KB
Newer Older
1 2 3
/**
 * @file usb_moded-worker.c
 *
4
 * Copyright (C) 2013-2019 Jolla. All rights reserved.
5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
 *
 * @author: Philippe De Swert <philippe.deswert@jollamobile.com>
 * @author: Simo Piiroinen <simo.piiroinen@jollamobile.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the Lesser GNU General Public License
 * version 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the Lesser GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
 * 02110-1301 USA
 */

#include "usb_moded-worker.h"

#include "usb_moded-android.h"
#include "usb_moded-configfs.h"
28 29
#include "usb_moded-control.h"
#include "usb_moded-dyn-config.h"
30 31 32 33 34 35 36
#include "usb_moded-log.h"
#include "usb_moded-modes.h"
#include "usb_moded-modesetting.h"
#include "usb_moded-modules.h"

#include <sys/eventfd.h>

37 38 39 40 41
#include <pthread.h> // NOTRIM
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
42 43 44 45 46

/* ========================================================================= *
 * Prototypes
 * ========================================================================= */

47 48 49
/* ------------------------------------------------------------------------- *
 * WORKER
 * ------------------------------------------------------------------------- */
50

51 52 53 54 55 56 57 58 59 60 61 62
static bool        worker_thread_p                 (void);
bool               worker_bailing_out              (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);
63
const modedata_t  *worker_get_usb_mode_data        (void);
64
modedata_t        *worker_dup_usb_mode_data        (void);
65
void               worker_set_usb_mode_data        (const modedata_t *data);
66 67 68 69 70 71 72
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);
73
static void        worker_switch_to_mode           (const char *mode);
74 75 76 77 78 79 80 81 82 83 84
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);
85 86 87 88 89 90 91 92 93

/* ========================================================================= *
 * Data
 * ========================================================================= */

static pthread_t worker_thread_id = 0;

static pthread_mutex_t  worker_mutex = PTHREAD_MUTEX_INITIALIZER;

94 95 96 97 98 99 100 101 102 103 104 105 106
/** Flag for: Main thread has changed target mode worker should apply
 *
 * Worker should bailout from synchronous activities related to
 * ongoing activation of a usb mode.
 */
static volatile bool worker_bailout_requested = false;

/** Flag for: Worker thread is cleaning up after abandoning mode switch
 *
 * Asynchronous activities on mode cleanup should be executed without
 * bailing out.
 */
static volatile bool worker_bailout_handled = false;
107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128

#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)
{
129 130
    LOG_REGISTER_CONTEXT;

131 132 133 134 135 136
    return worker_thread_id && worker_thread_id == pthread_self();
}

bool
worker_bailing_out(void)
{
137 138
    LOG_REGISTER_CONTEXT;

139
    // ref: see common_msleep_()
140 141 142
    return (worker_thread_p() &&
            worker_bailout_requested &&
            !worker_bailout_handled);
143 144 145 146 147 148
}

/* ------------------------------------------------------------------------- *
 * MTP_DAEMON
 * ------------------------------------------------------------------------- */

149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168
/** Maximum time to wait for mtpd to start [ms]
 *
 * This needs to include time to start systemd unit
 * plus however long it might take for mtpd to scan
 * all files exposed over mtp. On a slow device with
 * lots of files it can easily take over 30 seconds,
 * especially during the 1st mtp connect after reboot.
 *
 * Use two minutes as some kind of worst case estimate.
 */
static unsigned worker_mtp_start_delay = 120 * 1000;

/** Maximum time to wait for mtpd to stop [ms]
 *
 * This is just regular service stop. Expected to
 * take max couple of seconds, but use someting
 * in the ballbark of systemd default i.e. 15 seconds
 */
static unsigned worker_mtp_stop_delay  =  15 * 1000;

169 170 171 172 173 174 175 176
/** Flag for: We have started mtp daemon
 *
 * If we have issued systemd unit start, we should also
 * issue systemd unit stop even if probing for mtpd
 * presense gives negative result.
 */
static bool worker_mtp_service_started = false;

177 178
static bool worker_mode_is_mtp_mode(const char *mode)
{
179 180
    LOG_REGISTER_CONTEXT;

181 182 183 184 185
    return mode && !strcmp(mode, "mtp_mode");
}

static bool worker_is_mtpd_running(void)
{
186 187
    LOG_REGISTER_CONTEXT;

188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212
    /* 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;
}

213 214 215
static bool
worker_mtpd_running_p(void *aptr)
{
216 217
    LOG_REGISTER_CONTEXT;

218 219 220 221 222 223 224
    (void)aptr;
    return worker_is_mtpd_running();
}

static bool
worker_mtpd_stopped_p(void *aptr)
{
225 226
    LOG_REGISTER_CONTEXT;

227 228 229 230
    (void)aptr;
    return !worker_is_mtpd_running();
}

231 232 233
static bool
worker_stop_mtpd(void)
{
234 235
    LOG_REGISTER_CONTEXT;

236
    bool ack = false;
237

238
    if( !worker_mtp_service_started && worker_mtpd_stopped_p(0) ) {
239
        log_debug("mtp daemon is not running");
240
        goto SUCCESS;
241 242 243 244 245
    }

    int rc = common_system("systemctl-user stop buteo-mtp.service");
    if( rc != 0 ) {
        log_warning("failed to stop mtp daemon; exit code = %d", rc);
246
        goto FAILURE;
247 248
    }

249 250 251
    /* Have succesfully stopped mtp service */
    worker_mtp_service_started = false;

252 253 254 255
    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;
    }
256

257
    log_debug("mtp daemon has stopped");
258

259 260
SUCCESS:
    ack = true;
261

262
FAILURE:
263 264 265 266 267 268
    return ack;
}

static bool
worker_start_mtpd(void)
{
269 270
    LOG_REGISTER_CONTEXT;

271
    bool ack = false;
272

273 274 275
    if( worker_mtpd_running_p(0) ) {
        log_debug("mtp daemon is running");
        goto SUCCESS;
276 277
    }

278 279 280
    /* Have attempted to start mtp service */
    worker_mtp_service_started = true;

281 282 283
    int rc = common_system("systemctl-user start buteo-mtp.service");
    if( rc != 0 ) {
        log_warning("failed to start mtp daemon; exit code = %d", rc);
284
        goto FAILURE;
285 286
    }

287 288 289 290
    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;
    }
291

292
    log_debug("mtp daemon has started");
293

294 295
SUCCESS:
    ack = true;
296

297
FAILURE:
298 299 300 301 302
    return ack;
}

static bool worker_switch_to_charging(void)
{
303 304
    LOG_REGISTER_CONTEXT;

305 306 307 308 309 310 311 312 313
    bool ack = true;

    if( android_set_charging_mode() )
        goto SUCCESS;

    if( configfs_set_charging_mode() )
        goto SUCCESS;

    if( modules_in_use() ) {
314
        if( worker_set_kernel_module(MODULE_MASS_STORAGE) )
315
            goto SUCCESS;
316
        worker_set_kernel_module(MODULE_NONE);
317 318 319 320 321 322 323 324 325 326 327 328 329 330
    }

    log_err("switch to charging mode failed");

    ack = false;
SUCCESS:
    return ack;
}

/* ------------------------------------------------------------------------- *
 * KERNEL_MODULE
 * ------------------------------------------------------------------------- */

/** The module name for the specific mode */
331
static char *worker_kernel_module = NULL;
332 333 334 335 336 337

/** get the supposedly loaded module
 *
 * @return The name of the loaded module
 *
 */
338
const char * worker_get_kernel_module(void)
339
{
340 341
    LOG_REGISTER_CONTEXT;

342
    return worker_kernel_module ?: MODULE_NONE;
343 344 345 346 347 348 349
}

/** set the loaded module
 *
 * @param module The module name for the requested mode
 *
 */
350
bool worker_set_kernel_module(const char *module)
351
{
352 353
    LOG_REGISTER_CONTEXT;

354 355 356 357 358
    bool ack = false;

    if( !module )
        module = MODULE_NONE;

359
    const char *current = worker_get_kernel_module();
360 361 362 363 364 365 366 367 368

    log_debug("current module: %s -> %s", current, module);

    if( !g_strcmp0(current, module) )
        goto SUCCESS;

    if( modules_unload_module(current) != 0 )
        goto EXIT;

369
    free(worker_kernel_module), worker_kernel_module = 0;
370 371 372 373 374

    if( modules_load_module(module) != 0 )
        goto EXIT;

    if( g_strcmp0(module, MODULE_NONE) )
375
        worker_kernel_module = strdup(module);
376 377 378 379 380 381 382

SUCCESS:
    ack = true;
EXIT:
    return ack;
}

383
void worker_clear_kernel_module(void)
384
{
385 386
    LOG_REGISTER_CONTEXT;

387
    free(worker_kernel_module), worker_kernel_module = 0;
388 389 390 391 392 393 394
}

/* ------------------------------------------------------------------------- *
 * MODE_DATA
 * ------------------------------------------------------------------------- */

/** Contains the mode data */
395
static modedata_t *worker_mode_data = NULL;
396 397 398

/** get the usb mode data
 *
399
 * Note: This function should be called only from the worker thread.
400
 *
401
 * @return a pointer to the usb mode data
402
 */
403
const modedata_t *worker_get_usb_mode_data(void)
404
{
405 406
    LOG_REGISTER_CONTEXT;

407
    return worker_mode_data;
408 409
}

410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428
/** get clone of the usb mode data
 *
 * Caller must release the returned object via #modedata_free().
 *
 * @return a pointer to the usb 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;;
}

429
/** set the modedata_t data
430
 *
431
 * Note: This function should be called only from the worker thread,
432
 *
433
 * @param data mode_list_element pointer
434
 */
435
void worker_set_usb_mode_data(const modedata_t *data)
436
{
437 438
    LOG_REGISTER_CONTEXT;

439 440
    WORKER_LOCKED_ENTER;

441 442
    modedata_free(worker_mode_data),
        worker_mode_data = modedata_copy(data);
443 444

    WORKER_LOCKED_LEAVE;
445 446 447 448 449 450 451 452 453 454 455 456
}

/* ------------------------------------------------------------------------- *
 * 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 */
457
static gchar *worker_requested_mode = NULL;
458

459
static gchar *worker_activated_mode = NULL;
460 461 462 463

static const char *
worker_get_activated_mode_locked(void)
{
464 465
    LOG_REGISTER_CONTEXT;

466 467 468 469 470 471
    return worker_activated_mode ?: MODE_UNDEFINED;
}

static bool
worker_set_activated_mode_locked(const char *mode)
{
472 473
    LOG_REGISTER_CONTEXT;

474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491
    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)
{
492 493
    LOG_REGISTER_CONTEXT;

494 495 496 497 498 499
    return worker_requested_mode ?: MODE_UNDEFINED;
}

static bool
worker_set_requested_mode_locked(const char *mode)
{
500 501
    LOG_REGISTER_CONTEXT;

502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518
    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)
{
519 520
    LOG_REGISTER_CONTEXT;

521 522 523 524 525 526 527 528 529 530 531 532 533 534
    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)
{
535 536
    LOG_REGISTER_CONTEXT;

537 538 539 540 541 542 543 544
    WORKER_LOCKED_ENTER;
    g_free(worker_requested_mode), worker_requested_mode = 0;
    WORKER_LOCKED_LEAVE;
}

static void
worker_execute(void)
{
545 546
    LOG_REGISTER_CONTEXT;

547 548 549 550 551 552 553 554 555 556 557
    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;
558
    gchar *mode  = g_strdup(activate);
559 560 561 562

    WORKER_LOCKED_LEAVE;

    if( changed )
563
        worker_switch_to_mode(mode);
564 565 566
    else
        worker_notify();

567 568
    g_free(mode);

569 570 571 572 573 574 575
    return;
}

/* ------------------------------------------------------------------------- *
 * MODE_SWITCH
 * ------------------------------------------------------------------------- */

576
static void
577 578
worker_switch_to_mode(const char *mode)
{
579 580
    LOG_REGISTER_CONTEXT;

581 582 583 584 585 586
    const char *override = 0;

    /* set return to 1 to be sure to error out if no matching mode is found either */

    log_debug("Cleaning up previous mode");

587 588 589 590
    /* Either mtp daemon is not needed, or it must be *started* in
     * correct phase of gadget configuration when entering mtp mode.
     */
    worker_stop_mtpd();
591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614

    if( worker_get_usb_mode_data() ) {
        modesetting_leave_dynamic_mode();
        worker_set_usb_mode_data(NULL);
    }

    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;
    }

615 616
    const modedata_t *data = usbmoded_get_modedata(mode);
    if( data ) {
617 618 619 620 621 622
        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);

623 624 625
        /* When dealing with configfs, we can't enable UDC without
         * already having mtpd running */
        if( worker_mode_is_mtp_mode(mode) && configfs_in_use() ) {
626 627 628 629
            if( !worker_start_mtpd() )
                goto FAILED;
        }

630
        if( !worker_set_kernel_module(data->mode_module) )
631 632 633 634 635
            goto FAILED;

        if( !modesetting_enter_dynamic_mode() )
            goto FAILED;

636 637 638 639 640 641 642 643
        /* 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_start_mtpd() )
                goto FAILED;
        }

644 645 646 647 648 649
        goto SUCCESS;
    }

    log_warning("Matching mode %s was not found.", mode);

FAILED:
650 651 652 653 654
    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");
655
        worker_stop_mtpd();
656 657 658 659
        modesetting_leave_dynamic_mode();
        worker_set_usb_mode_data(NULL);
    }

660 661 662 663 664 665 666 667 668 669 670 671 672 673
    /* 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;
674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689
    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);
690
    worker_set_kernel_module(MODULE_NONE);
691 692 693 694 695 696

SUCCESS:

    WORKER_LOCKED_ENTER;
    if( override ) {
        worker_set_requested_mode_locked(override);
697 698
        override = common_map_mode_to_hardware(override);
        worker_set_activated_mode_locked(override);
699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726
    }
    else {
        worker_set_activated_mode_locked(mode);
    }
    WORKER_LOCKED_LEAVE;

    worker_notify();

    return;
}

/* ------------------------------------------------------------------------- *
 * WORKER_THREAD
 * ------------------------------------------------------------------------- */

/** eventfd descriptor for waking up worker thread after adding new jobs */
static int              worker_req_evfd  = -1;

/** eventfd descriptor for waking up main thread after executing jobs */
static int              worker_rsp_evfd  = -1;

/** I/O watch identifier for worker_rsp_evfd */
static guint            worker_rsp_wid   = 0;

static guint
worker_add_iowatch(int fd, bool close_on_unref,
               GIOCondition cnd, GIOFunc io_cb, gpointer aptr)
{
727 728
    LOG_REGISTER_CONTEXT;

729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750
    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)
{
751 752
    LOG_REGISTER_CONTEXT;

753 754
    (void)aptr;

755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784
    /* 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 ) {
785 786
            worker_bailout_requested = false;
            worker_bailout_handled = false;
787 788 789 790 791 792 793 794 795 796 797
            worker_execute();
        }

    }
EXIT:
    return 0;
}

static gboolean
worker_notify_cb(GIOChannel *chn, GIOCondition cnd, gpointer data)
{
798 799
    LOG_REGISTER_CONTEXT;

800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863
    (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)
{
864 865
    LOG_REGISTER_CONTEXT;

866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882
    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)
{
883 884
    LOG_REGISTER_CONTEXT;

885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920
    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)
{
921 922
    LOG_REGISTER_CONTEXT;

923 924 925 926 927 928 929 930 931 932 933 934 935
    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)
{
936 937
    LOG_REGISTER_CONTEXT;

938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964
    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)
{
965 966
    LOG_REGISTER_CONTEXT;

967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986
    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)
{
987 988
    LOG_REGISTER_CONTEXT;

989 990
    worker_stop_thread();
    worker_delete_eventfd();
991 992 993

    /* Worker thread is stopped and resources can be released. */
    worker_set_usb_mode_data(0);
994 995 996 997 998
}

void
worker_wakeup(void)
{
999 1000
    LOG_REGISTER_CONTEXT;

1001
    worker_bailout_requested = true;
1002 1003 1004 1005 1006 1007 1008 1009 1010 1011

    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)
{
1012 1013
    LOG_REGISTER_CONTEXT;

1014 1015 1016 1017 1018
    uint64_t cnt = 1;
    if( write(worker_rsp_evfd, &cnt, sizeof cnt) == -1 ) {
        log_err("failed to signal handled: %m");
    }
}