diff --git a/cdba-server.c b/cdba-server.c index 7a98352..e3267d8 100644 --- a/cdba-server.c +++ b/cdba-server.c @@ -4,8 +4,6 @@ * * SPDX-License-Identifier: BSD-3-Clause */ -#include -#include #include #include #include @@ -22,8 +20,8 @@ #include "device_parser.h" #include "fastboot.h" #include "list.h" +#include "watch.h" -static bool quit_invoked; static const char *username; struct device *selected_device; @@ -88,7 +86,7 @@ static void msg_select_board(const void *param) selected_device = device_open(param, username, &fastboot_ops); if (!selected_device) { fprintf(stderr, "failed to open %s\n", (const char *)param); - quit_invoked = true; + watch_quit(); } cdba_send(MSG_SELECT_BOARD); @@ -224,119 +222,14 @@ static int handle_stdin(int fd, void *buf) return 0; } -struct watch { - struct list_head node; - - int fd; - int (*cb)(int, void*); - void *data; -}; - -struct timer { - struct list_head node; - struct timeval tv; - - void (*cb)(void *); - void *data; -}; - -static struct list_head read_watches = LIST_INIT(read_watches); -static struct list_head timer_watches = LIST_INIT(timer_watches); - -void watch_add_readfd(int fd, int (*cb)(int, void*), void *data) -{ - struct watch *w; - - w = calloc(1, sizeof(*w)); - w->fd = fd; - w->cb = cb; - w->data = data; - - list_add(&read_watches, &w->node); -} - -void watch_timer_add(int timeout_ms, void (*cb)(void *), void *data) -{ - struct timeval tv_timeout; - struct timeval now; - struct timer *t; - - t = calloc(1, sizeof(*t)); - - gettimeofday(&now, NULL); - - tv_timeout.tv_sec = timeout_ms / 1000; - tv_timeout.tv_usec = (timeout_ms % 1000) * 1000; - - t->cb = cb; - t->data = data; - timeradd(&now, &tv_timeout, &t->tv); - - list_add(&timer_watches, &t->node); -} - -static struct timeval *watch_timer_next(void) -{ - static struct timeval timeout; - struct timeval now; - struct timer *next; - struct timer *t; - - if (list_empty(&timer_watches)) - return NULL; - - next = list_entry_first(&timer_watches, struct timer, node); - - list_for_each_entry(t, &timer_watches, node) { - if (timercmp(&t->tv, &next->tv, <)) - next = t; - } - - gettimeofday(&now, NULL); - timersub(&next->tv, &now, &timeout); - if (timeout.tv_sec < 0) { - timeout.tv_sec = 0; - timeout.tv_usec = 0; - } - - return &timeout; -} - -static void watch_timer_invoke(void) -{ - struct timeval now; - struct timer *tmp; - struct timer *t; - - gettimeofday(&now, NULL); - - list_for_each_entry_safe(t, tmp, &timer_watches, node) { - if (timercmp(&t->tv, &now, <)) { - t->cb(t->data); - - list_del(&t->node); - free(t); - } - } -} - static void sigpipe_handler(int signo) { - quit_invoked = true; -} - -void watch_quit(void) -{ - quit_invoked = true; + watch_quit(); } int main(int argc, char **argv) { - struct timeval *timeoutp; - struct watch *w; - fd_set rfds; int flags; - int nfds; int ret; signal(SIGPIPE, sigpipe_handler); @@ -365,40 +258,7 @@ int main(int argc, char **argv) flags = fcntl(STDIN_FILENO, F_GETFL, 0); fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK); - while (!quit_invoked) { - nfds = 0; - - list_for_each_entry(w, &read_watches, node) { - nfds = MAX(nfds, w->fd); - FD_SET(w->fd, &rfds); - } - - if (!FD_ISSET(STDIN_FILENO, &rfds)) { - fprintf(stderr, "rfds is trash!\n"); - goto done; - } - - timeoutp = watch_timer_next(); - ret = select(nfds + 1, &rfds, NULL, NULL, timeoutp); - if (ret < 0 && errno == EINTR) - continue; - else if (ret < 0) - break; - - watch_timer_invoke(); - - list_for_each_entry(w, &read_watches, node) { - if (FD_ISSET(w->fd, &rfds)) { - ret = w->cb(w->fd, w->data); - if (ret < 0) { - fprintf(stderr, "cb returned %d\n", ret); - goto done; - } - } - } - } - -done: + watch_run(); /* if we got here, stdin/out/err might be not accessible anymore */ ret = open("/dev/null", O_RDWR); diff --git a/cdba-server.h b/cdba-server.h index 0dd6f26..c8e00a4 100644 --- a/cdba-server.h +++ b/cdba-server.h @@ -6,12 +6,6 @@ #include "cdba.h" -void watch_add_readfd(int fd, int (*cb)(int, void*), void *data); -int watch_add_quit(int (*cb)(int, void*), void *data); -void watch_timer_add(int timeout_ms, void (*cb)(void *), void *data); -void watch_quit(void); -int watch_run(void); - int tty_open(const char *tty, struct termios *old); void cdba_send_buf(int type, size_t len, const void *buf); diff --git a/console.c b/console.c index 0b3abaf..434ad46 100644 --- a/console.c +++ b/console.c @@ -13,6 +13,7 @@ #include "cdba-server.h" #include "device.h" +#include "watch.h" struct console { int console_fd; diff --git a/device.c b/device.c index 7daffae..5baaf76 100644 --- a/device.c +++ b/device.c @@ -23,6 +23,7 @@ #include "list.h" #include "ppps.h" #include "status-cmd.h" +#include "watch.h" #define ARRAY_SIZE(x) ((sizeof(x)/sizeof((x)[0]))) diff --git a/drivers/cdb_assist.c b/drivers/cdb_assist.c index 8eaaf37..55b17dc 100644 --- a/drivers/cdb_assist.c +++ b/drivers/cdb_assist.c @@ -20,6 +20,7 @@ #include "cdba-server.h" #include "device.h" #include "status.h" +#include "watch.h" struct cdb_assist { char serial[9]; diff --git a/drivers/conmux.c b/drivers/conmux.c index a288c5c..552f245 100644 --- a/drivers/conmux.c +++ b/drivers/conmux.c @@ -18,6 +18,7 @@ #include "cdba-server.h" #include "device.h" +#include "watch.h" extern int h_errno; diff --git a/drivers/qcomlt_dbg.c b/drivers/qcomlt_dbg.c index bab0a43..6e24ce1 100644 --- a/drivers/qcomlt_dbg.c +++ b/drivers/qcomlt_dbg.c @@ -21,6 +21,7 @@ #include "cdba-server.h" #include "device.h" #include "status.h" +#include "watch.h" enum qcomlt_parse_state { STATE_, diff --git a/fastboot.c b/fastboot.c index cfb4bc7..32f08f8 100644 --- a/fastboot.c +++ b/fastboot.c @@ -22,6 +22,7 @@ #include "cdba-server.h" #include "fastboot.h" +#include "watch.h" #define MAX_USBFS_BULK_SIZE (16*1024) diff --git a/meson.build b/meson.build index 927a2b8..ef28570 100644 --- a/meson.build +++ b/meson.build @@ -88,7 +88,8 @@ server_srcs = ['cdba-server.c', 'console.c', 'ppps.c', 'status.c', - 'status-cmd.c'] + 'status-cmd.c', + 'watch.c'] build_server = true foreach d: server_deps diff --git a/status-cmd.c b/status-cmd.c index 7380be6..09998b8 100644 --- a/status-cmd.c +++ b/status-cmd.c @@ -17,6 +17,7 @@ #include "device.h" #include "status.h" #include "status-cmd.h" +#include "watch.h" static void launch_status_cmd(struct device *dev) { diff --git a/watch.c b/watch.c new file mode 100644 index 0000000..236911b --- /dev/null +++ b/watch.c @@ -0,0 +1,172 @@ +/* + * Copyright (c) 2016-2018, Linaro Ltd. + * All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cdba.h" +#include "list.h" +#include "watch.h" + +static bool quit_invoked; + +struct watch { + struct list_head node; + + int fd; + int (*cb)(int, void*); + void *data; +}; + +struct timer { + struct list_head node; + struct timeval tv; + + void (*cb)(void *); + void *data; +}; + +static struct list_head read_watches = LIST_INIT(read_watches); +static struct list_head timer_watches = LIST_INIT(timer_watches); + +void watch_add_readfd(int fd, int (*cb)(int, void*), void *data) +{ + struct watch *w; + + w = calloc(1, sizeof(*w)); + w->fd = fd; + w->cb = cb; + w->data = data; + + list_add(&read_watches, &w->node); +} + +void watch_timer_add(int timeout_ms, void (*cb)(void *), void *data) +{ + struct timeval tv_timeout; + struct timeval now; + struct timer *t; + + t = calloc(1, sizeof(*t)); + + gettimeofday(&now, NULL); + + tv_timeout.tv_sec = timeout_ms / 1000; + tv_timeout.tv_usec = (timeout_ms % 1000) * 1000; + + t->cb = cb; + t->data = data; + timeradd(&now, &tv_timeout, &t->tv); + + list_add(&timer_watches, &t->node); +} + +static struct timeval *watch_timer_next(void) +{ + static struct timeval timeout; + struct timeval now; + struct timer *next; + struct timer *t; + + if (list_empty(&timer_watches)) + return NULL; + + next = list_entry_first(&timer_watches, struct timer, node); + + list_for_each_entry(t, &timer_watches, node) { + if (timercmp(&t->tv, &next->tv, <)) + next = t; + } + + gettimeofday(&now, NULL); + timersub(&next->tv, &now, &timeout); + if (timeout.tv_sec < 0) { + timeout.tv_sec = 0; + timeout.tv_usec = 0; + } + + return &timeout; +} + +static void watch_timer_invoke(void) +{ + struct timeval now; + struct timer *tmp; + struct timer *t; + + gettimeofday(&now, NULL); + + list_for_each_entry_safe(t, tmp, &timer_watches, node) { + if (timercmp(&t->tv, &now, <)) { + t->cb(t->data); + + list_del(&t->node); + free(t); + } + } +} + +void watch_quit(void) +{ + quit_invoked = true; +} + +int watch_run(void) +{ + struct timeval *timeoutp; + struct watch *w; + fd_set rfds; + int nfds; + int ret; + + while (!quit_invoked) { + nfds = 0; + + list_for_each_entry(w, &read_watches, node) { + nfds = MAX(nfds, w->fd); + FD_SET(w->fd, &rfds); + } + + if (!FD_ISSET(STDIN_FILENO, &rfds)) { + fprintf(stderr, "rfds is trash!\n"); + return -EINVAL; + } + + timeoutp = watch_timer_next(); + ret = select(nfds + 1, &rfds, NULL, NULL, timeoutp); + if (ret < 0 && errno == EINTR) + continue; + else if (ret < 0) { + int err = errno; + fprintf(stderr, "select returned %s\n", strerror(err)); + return -err; + } + + watch_timer_invoke(); + + list_for_each_entry(w, &read_watches, node) { + if (FD_ISSET(w->fd, &rfds)) { + ret = w->cb(w->fd, w->data); + if (ret < 0) { + fprintf(stderr, "cb returned %d\n", ret); + return ret; + } + } + } + } + + return 0; +} diff --git a/watch.h b/watch.h new file mode 100644 index 0000000..22ae2bf --- /dev/null +++ b/watch.h @@ -0,0 +1,10 @@ +#ifndef __WATCH_H__ +#define __WATCH_H__ + +void watch_add_readfd(int fd, int (*cb)(int, void*), void *data); +int watch_add_quit(int (*cb)(int, void*), void *data); +void watch_timer_add(int timeout_ms, void (*cb)(void *), void *data); +void watch_quit(void); +int watch_run(void); + +#endif