monitor/mainloop.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2014  Intel Corporation
   6  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This library is free software; you can redistribute it and/or
  10  *  modify it under the terms of the GNU Lesser General Public
  11  *  License as published by the Free Software Foundation; either
  12  *  version 2.1 of the License, or (at your option) any later version.
  13  *
  14  *  This library 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 GNU
  17  *  Lesser General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU Lesser General Public
  20  *  License along with this library; 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         memset(&ev, 0, sizeof(ev));
 226         ev.events = events;
 227         ev.data.ptr = data;
 228
 229         err = epoll_ctl(epoll_fd, EPOLL_CTL_MOD, data->fd, &ev);
 230         if (err < 0)
 231                 return err;
 232
 233         data->events = events;
 234
 235         return 0;
 236 }
 237
 238 int mainloop_remove_fd(int fd)
 239 {
 240         struct mainloop_data *data;
 241         int err;
 242
 243         if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1)
 244                 return -EINVAL;
 245
 246         data = mainloop_list[fd];
 247         if (!data)
 248                 return -ENXIO;
 249
 250         mainloop_list[fd] = NULL;
 251
 252         err = epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL);
 253
 254         if (data->destroy)
 255                 data->destroy(data->user_data);
 256
 257         free(data);
 258
 259         return err;
 260 }
 261
 262 static void timeout_destroy(void *user_data)
 263 {
 264         struct timeout_data *data = user_data;
 265
 266         close(data->fd);
 267         data->fd = -1;
 268
 269         if (data->destroy)
 270                 data->destroy(data->user_data);
 271 }
 272
 273 static void timeout_callback(int fd, uint32_t events, void *user_data)
 274 {
 275         struct timeout_data *data = user_data;
 276         uint64_t expired;
 277         ssize_t result;
 278
 279         if (events & (EPOLLERR | EPOLLHUP))
 280                 return;
 281
 282         result = read(data->fd, &expired, sizeof(expired));
 283         if (result != sizeof(expired))
 284                 return;
 285
 286         if (data->callback)
 287                 data->callback(data->fd, data->user_data);
 288 }
 289
 290 static inline int timeout_set(int fd, unsigned int msec)
 291 {
 292         struct itimerspec itimer;
 293         unsigned int sec = msec / 1000;
 294
 295         memset(&itimer, 0, sizeof(itimer));
 296         itimer.it_interval.tv_sec = 0;
 297         itimer.it_interval.tv_nsec = 0;
 298         itimer.it_value.tv_sec = sec;
 299         itimer.it_value.tv_nsec = (msec - (sec * 1000)) * 1000;
 300
 301         return timerfd_settime(fd, 0, &itimer, NULL);
 302 }
 303
 304 int mainloop_add_timeout(unsigned int msec, mainloop_timeout_func callback,
 305                                 void *user_data, mainloop_destroy_func destroy)
 306 {
 307         struct timeout_data *data;
 308
 309         if (!callback)
 310                 return -EINVAL;
 311
 312         data = malloc(sizeof(*data));
 313         if (!data)
 314                 return -ENOMEM;
 315
 316         memset(data, 0, sizeof(*data));
 317         data->callback = callback;
 318         data->destroy = destroy;
 319         data->user_data = user_data;
 320
 321         data->fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK | TFD_CLOEXEC);
 322         if (data->fd < 0) {
 323                 free(data);
 324                 return -EIO;
 325         }
 326
 327         if (msec > 0) {
 328                 if (timeout_set(data->fd, msec) < 0) {
 329                         close(data->fd);
 330                         free(data);
 331                         return -EIO;
 332                 }
 333         }
 334
 335         if (mainloop_add_fd(data->fd, EPOLLIN | EPOLLONESHOT,
 336                                 timeout_callback, data, timeout_destroy) < 0) {
 337                 close(data->fd);
 338                 free(data);
 339                 return -EIO;
 340         }
 341
 342         return data->fd;
 343 }
 344
 345 int mainloop_modify_timeout(int id, unsigned int msec)
 346 {
 347         if (msec > 0) {
 348                 if (timeout_set(id, msec) < 0)
 349                         return -EIO;
 350         }
 351
 352         if (mainloop_modify_fd(id, EPOLLIN | EPOLLONESHOT) < 0)
 353                 return -EIO;
 354
 355         return 0;
 356 }
 357
 358 int mainloop_remove_timeout(int id)
 359 {
 360         return mainloop_remove_fd(id);
 361 }
 362
 363 int mainloop_set_signal(sigset_t *mask, mainloop_signal_func callback,
 364                                 void *user_data, mainloop_destroy_func destroy)
 365 {
 366         struct signal_data *data;
 367
 368         if (!mask || !callback)
 369                 return -EINVAL;
 370
 371         data = malloc(sizeof(*data));
 372         if (!data)
 373                 return -ENOMEM;
 374
 375         memset(data, 0, sizeof(*data));
 376         data->callback = callback;
 377         data->destroy = destroy;
 378         data->user_data = user_data;
 379
 380         data->fd = -1;
 381         memcpy(&data->mask, mask, sizeof(sigset_t));
 382
 383         free(signal_data);
 384         signal_data = data;
 385
 386         return 0;
 387 }