monitor/mainloop.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation
   6  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This program is free software; you can redistribute it and/or modify
  10  *  it under the terms of the GNU General Public License as published by
  11  *  the Free Software Foundation; either version 2 of the License, or
  12  *  (at your option) any later version.
  13  *
  14  *  This program is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  *  GNU General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU General Public License
  20  *  along with this program; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <stdio.h>
  30 #include <errno.h>
  31 #include <unistd.h>
  32 #include <stdlib.h>
  33 #include <string.h>
  34 #include <signal.h>
  35 #include <sys/signalfd.h>
  36 #include <sys/timerfd.h>
  37 #include <sys/epoll.h>
  38
  39 #include "mainloop.h"
  40
  41 #define MAX_EPOLL_EVENTS 10
  42
  43 static int epoll_fd;
  44 static int epoll_terminate;
  45
  46 struct mainloop_data {
  47         int fd;
  48         uint32_t events;
  49         mainloop_event_func callback;
  50         mainloop_destroy_func destroy;
  51         void *user_data;
  52 };
  53
  54 #define MAX_MAINLOOP_ENTRIES 128
  55
  56 static struct mainloop_data *mainloop_list[MAX_MAINLOOP_ENTRIES];
  57
  58 struct timeout_data {
  59         int fd;
  60         mainloop_timeout_func callback;
  61         mainloop_destroy_func destroy;
  62         void *user_data;
  63 };
  64
  65 struct signal_data {
  66         int fd;
  67         sigset_t mask;
  68         mainloop_signal_func callback;
  69         mainloop_destroy_func destroy;
  70         void *user_data;
  71 };
  72
  73 static struct signal_data *signal_data;
  74
  75 void mainloop_init(void)
  76 {
  77         unsigned int i;
  78
  79         epoll_fd = epoll_create1(EPOLL_CLOEXEC);
  80
  81         for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++)
  82                 mainloop_list[i] = NULL;
  83
  84         epoll_terminate = 0;
  85 }
  86
  87 void mainloop_quit(void)
  88 {
  89         epoll_terminate = 1;
  90 }
  91
  92 static void signal_callback(int fd, uint32_t events, void *user_data)
  93 {
  94         struct signal_data *data = user_data;
  95         struct signalfd_siginfo si;
  96         ssize_t result;
  97
  98         if (events & (EPOLLERR | EPOLLHUP)) {
  99                 mainloop_quit();
 100                 return;
 101         }
 102
 103         result = read(fd, &si, sizeof(si));
 104         if (result != sizeof(si))
 105                 return;
 106
 107         if (data->callback)
 108                 data->callback(si.ssi_signo, data->user_data);
 109 }
 110
 111 int mainloop_run(void)
 112 {
 113         unsigned int i;
 114
 115         if (signal_data) {
 116                 if (sigprocmask(SIG_BLOCK, &signal_data->mask, NULL) < 0)
 117                         return 1;
 118
 119                 signal_data->fd = signalfd(-1, &signal_data->mask,
 120                                                 SFD_NONBLOCK | SFD_CLOEXEC);
 121                 if (signal_data->fd < 0)
 122                         return 1;
 123
 124                 if (mainloop_add_fd(signal_data->fd, EPOLLIN,
 125                                 signal_callback, signal_data, NULL) < 0) {
 126                         close(signal_data->fd);
 127                         return 1;
 128                 }
 129         }
 130
 131         while (!epoll_terminate) {
 132                 struct epoll_event events[MAX_EPOLL_EVENTS];
 133                 int n, nfds;
 134
 135                 nfds = epoll_wait(epoll_fd, events, MAX_EPOLL_EVENTS, -1);
 136                 if (nfds < 0)
 137                         continue;
 138
 139                 for (n = 0; n < nfds; n++) {
 140                         struct mainloop_data *data = events[n].data.ptr;
 141
 142                         data->callback(data->fd, events[n].events,
 143                                                         data->user_data);
 144                 }
 145         }
 146
 147         if (signal_data) {
 148                 mainloop_remove_fd(signal_data->fd);
 149                 close(signal_data->fd);
 150
 151                 if (signal_data->destroy)
 152                         signal_data->destroy(signal_data->user_data);
 153         }
 154
 155         for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++) {
 156                 struct mainloop_data *data = mainloop_list[i];
 157
 158                 mainloop_list[i] = NULL;
 159
 160                 if (data) {
 161                         epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL);
 162
 163                         if (data->destroy)
 164                                 data->destroy(data->user_data);
 165
 166                         free(data);
 167                 }
 168         }
 169
 170         close(epoll_fd);
 171         epoll_fd = 0;
 172
 173         return 0;
 174 }
 175
 176 int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback,
 177                                 void *user_data, mainloop_destroy_func destroy)
 178 {
 179         struct mainloop_data *data;
 180         struct epoll_event ev;
 181         int err;
 182
 183         if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1 || !callback)
 184                 return -EINVAL;
 185
 186         data = malloc(sizeof(*data));
 187         if (!data)
 188                 return -ENOMEM;
 189
 190         memset(data, 0, sizeof(*data));
 191         data->fd = fd;
 192         data->events = events;
 193         data->callback = callback;
 194         data->destroy = destroy;
 195         data->user_data = user_data;
 196
 197         memset(&ev, 0, sizeof(ev));
 198         ev.events = events;
 199         ev.data.ptr = data;
 200
 201         err = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, data->fd, &ev);
 202         if (err < 0) {
 203                 free(data);
 204                 return err;
 205         }
 206
 207         mainloop_list[fd] = data;
 208
 209         return 0;
 210 }
 211
 212 int mainloop_modify_fd(int fd, uint32_t events)
 213 {
 214         struct mainloop_data *data;
 215         struct epoll_event ev;
 216         int err;
 217
 218         if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1)
 219                 return -EINVAL;
 220
 221         data = mainloop_list[fd];
 222         if (!data)
 223                 return -ENXIO;
 224
 225         if (data->events == events)
 226                 return 0;
 227
 228         memset(&ev, 0, sizeof(ev));
 229         ev.events = events;
 230         ev.data.ptr = data;
 231
 232         err = epoll_ctl(epoll_fd, EPOLL_CTL_MOD, data->fd, &ev);
 233         if (err < 0)
 234                 return err;
 235
 236         data->events = events;
 237
 238         return 0;
 239 }
 240
 241 int mainloop_remove_fd(int fd)
 242 {
 243         struct mainloop_data *data;
 244         int err;
 245
 246         if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1)
 247                 return -EINVAL;
 248
 249         data = mainloop_list[fd];
 250         if (!data)
 251                 return -ENXIO;
 252
 253         mainloop_list[fd] = NULL;
 254
 255         err = epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL);
 256
 257         if (data->destroy)
 258                 data->destroy(data->user_data);
 259
 260         free(data);
 261
 262         return err;
 263 }
 264
 265 static void timeout_destroy(void *user_data)
 266 {
 267         struct timeout_data *data = user_data;
 268
 269         close(data->fd);
 270         data->fd = -1;
 271
 272         if (data->destroy)
 273                 data->destroy(data->user_data);
 274 }
 275
 276 static void timeout_callback(int fd, uint32_t events, void *user_data)
 277 {
 278         struct timeout_data *data = user_data;
 279         uint64_t expired;
 280         ssize_t result;
 281
 282         if (events & (EPOLLERR | EPOLLHUP))
 283                 return;
 284
 285         result = read(data->fd, &expired, sizeof(expired));
 286         if (result != sizeof(expired))
 287                 return;
 288
 289         if (data->callback)
 290                 data->callback(data->fd, data->user_data);
 291 }
 292
 293 static inline int timeout_set(int fd, unsigned int seconds)
 294 {
 295         struct itimerspec itimer;
 296
 297         memset(&itimer, 0, sizeof(itimer));
 298         itimer.it_interval.tv_sec = 0;
 299         itimer.it_interval.tv_nsec = 0;
 300         itimer.it_value.tv_sec = seconds;
 301         itimer.it_value.tv_nsec = 0;
 302
 303         return timerfd_settime(fd, 0, &itimer, NULL);
 304 }
 305
 306 int mainloop_add_timeout(unsigned int seconds, mainloop_timeout_func callback,
 307                                 void *user_data, mainloop_destroy_func destroy)
 308 {
 309         struct timeout_data *data;
 310
 311         if (!callback)
 312                 return -EINVAL;
 313
 314         data = malloc(sizeof(*data));
 315         if (!data)
 316                 return -ENOMEM;
 317
 318         memset(data, 0, sizeof(*data));
 319         data->callback = callback;
 320         data->destroy = destroy;
 321         data->user_data = user_data;
 322
 323         data->fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK | TFD_CLOEXEC);
 324         if (data->fd < 0) {
 325                 free(data);
 326                 return -EIO;
 327         }
 328
 329         if (seconds > 0) {
 330                 if (timeout_set(data->fd, seconds) < 0) {
 331                         close(data->fd);
 332                         free(data);
 333                         return -EIO;
 334                 }
 335         }
 336
 337         if (mainloop_add_fd(data->fd, EPOLLIN | EPOLLONESHOT,
 338                                 timeout_callback, data, timeout_destroy) < 0) {
 339                 close(data->fd);
 340                 free(data);
 341                 return -EIO;
 342         }
 343
 344         return data->fd;
 345 }
 346
 347 int mainloop_modify_timeout(int id, unsigned int seconds)
 348 {
 349         if (seconds > 0) {
 350                 if (timeout_set(id, seconds) < 0)
 351                         return -EIO;
 352         }
 353
 354         if (mainloop_modify_fd(id, EPOLLIN | EPOLLONESHOT) < 0)
 355                 return -EIO;
 356
 357         return 0;
 358 }
 359
 360 int mainloop_remove_timeout(int id)
 361 {
 362         return mainloop_remove_fd(id);
 363 }
 364
 365 int mainloop_set_signal(sigset_t *mask, mainloop_signal_func callback,
 366                                 void *user_data, mainloop_destroy_func destroy)
 367 {
 368         struct signal_data *data;
 369
 370         if (!mask || !callback)
 371                 return -EINVAL;
 372
 373         data = malloc(sizeof(*data));
 374         if (!data)
 375                 return -ENOMEM;
 376
 377         memset(data, 0, sizeof(*data));
 378         data->callback = callback;
 379         data->destroy = destroy;
 380         data->user_data = user_data;
 381
 382         data->fd = -1;
 383         memcpy(&data->mask, mask, sizeof(sigset_t));
 384
 385         free(signal_data);
 386         signal_data = data;
 387
 388         return 0;
 389 }