A small USV for ALIX boards with i2c support
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

310 lines
7.9KB

  1. /***************************************************************************
  2. * Copyright (C) 07/2007 by Olaf Rempel *
  3. * razzor@kopf-tisch.de *
  4. * *
  5. * This program is free software; you can redistribute it and/or modify *
  6. * it under the terms of the GNU General Public License as published by *
  7. * the Free Software Foundation; version 2 of the License *
  8. * *
  9. * This program is distributed in the hope that it will be useful, *
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of *
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
  12. * GNU General Public License for more details. *
  13. * *
  14. * You should have received a copy of the GNU General Public License *
  15. * along with this program; if not, write to the *
  16. * Free Software Foundation, Inc., *
  17. * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
  18. ***************************************************************************/
  19. #include <stdlib.h>
  20. #include <unistd.h>
  21. #include <string.h>
  22. #include <sys/time.h>
  23. #include <sys/types.h>
  24. #include "list.h"
  25. #include "logging.h"
  26. #include "event.h"
  27. static LIST_HEAD(event_fd_list);
  28. static LIST_HEAD(event_timeout_list);
  29. static int leave_loop;
  30. struct event_fd {
  31. struct list_head list;
  32. unsigned int flags;
  33. int fd;
  34. int (*read_cb)(int fd, void *privdata);
  35. int (*write_cb)(int fd, void *privdata);
  36. void *read_priv;
  37. void *write_priv;
  38. };
  39. struct event_timeout {
  40. struct list_head list;
  41. unsigned int flags;
  42. struct timeval intervall;
  43. struct timeval nextrun;
  44. int (*callback)(void *privdata);
  45. void *privdata;
  46. };
  47. struct event_fd * event_add_fd(
  48. struct event_fd *entry,
  49. int fd,
  50. int type,
  51. int (*callback)(int fd, void *privdata),
  52. void *privdata)
  53. {
  54. /* check valid filediskriptor */
  55. if (fd < 0 || fd > FD_SETSIZE) {
  56. log_print(LOG_ERROR, "event_add_fd(): invalid fd");
  57. return NULL;
  58. }
  59. /* check valid type (read/write) */
  60. if (!(type & FD_TYPES)) {
  61. log_print(LOG_ERROR, "event_add_fd(): invalid type");
  62. return NULL;
  63. }
  64. /* create new entry */
  65. if (entry == NULL) {
  66. entry = malloc(sizeof(struct event_fd));
  67. if (entry == NULL) {
  68. log_print(LOG_ERROR, "event_add_fd(): out of memory");
  69. return NULL;
  70. }
  71. memset(entry, 0, sizeof(struct event_fd));
  72. entry->flags |= EVENT_NEW;
  73. entry->fd = fd;
  74. /* put it on the list */
  75. list_add_tail(&entry->list, &event_fd_list);
  76. }
  77. if (type & FD_READ) {
  78. entry->flags = (callback != NULL) ? (entry->flags | FD_READ | EVENT_NEW) : (entry->flags & ~FD_READ);
  79. entry->read_cb = callback;
  80. entry->read_priv = privdata;
  81. } else if (type & FD_WRITE) {
  82. entry->flags = (callback != NULL) ? (entry->flags | FD_WRITE | EVENT_NEW) : (entry->flags & ~FD_WRITE);
  83. entry->write_cb = callback;
  84. entry->write_priv = privdata;
  85. }
  86. return entry;
  87. }
  88. int event_get_fd(struct event_fd *entry)
  89. {
  90. return (entry != NULL) ? entry->fd: -1;
  91. }
  92. void event_remove_fd(struct event_fd *entry)
  93. {
  94. /* mark the event as deleted -> remove in select() loop */
  95. entry->flags |= EVENT_DELETE;
  96. }
  97. static void add_timeval(struct timeval *ret, struct timeval *a, struct timeval *b)
  98. {
  99. ret->tv_usec = a->tv_usec + b->tv_usec;
  100. ret->tv_sec = a->tv_sec + b->tv_sec;
  101. if (ret->tv_usec >= 1000000) {
  102. ret->tv_usec -= 1000000;
  103. ret->tv_sec++;
  104. }
  105. }
  106. static void sub_timeval(struct timeval *ret, struct timeval *a, struct timeval *b)
  107. {
  108. ret->tv_usec = a->tv_usec - b->tv_usec;
  109. ret->tv_sec = a->tv_sec - b->tv_sec;
  110. if (ret->tv_usec < 0) {
  111. ret->tv_usec += 1000000;
  112. ret->tv_sec--;
  113. }
  114. }
  115. static int cmp_timeval(struct timeval *a, struct timeval *b)
  116. {
  117. if (a->tv_sec > b->tv_sec)
  118. return -1;
  119. if (a->tv_sec < b->tv_sec)
  120. return 1;
  121. if (a->tv_usec > b->tv_usec)
  122. return -1;
  123. if (a->tv_usec < b->tv_usec)
  124. return 1;
  125. return 0;
  126. }
  127. static void schedule_nextrun(struct event_timeout *entry, struct timeval *now)
  128. {
  129. add_timeval(&entry->nextrun, now, &entry->intervall);
  130. struct event_timeout *search;
  131. list_for_each_entry(search, &event_timeout_list, list) {
  132. if (search->nextrun.tv_sec > entry->nextrun.tv_sec) {
  133. list_add_tail(&entry->list, &search->list);
  134. return;
  135. } else if (search->nextrun.tv_sec == entry->nextrun.tv_sec &&
  136. search->nextrun.tv_usec > entry->nextrun.tv_usec) {
  137. list_add_tail(&entry->list, &search->list);
  138. return;
  139. }
  140. }
  141. list_add_tail(&entry->list, &event_timeout_list);
  142. }
  143. struct event_timeout * event_add_timeout(
  144. struct timeval *timeout,
  145. int (*callback)(void *privdata),
  146. void *privdata)
  147. {
  148. struct event_timeout *entry;
  149. entry = malloc(sizeof(struct event_timeout));
  150. if (entry == NULL) {
  151. log_print(LOG_ERROR, "event_add_timeout(): out of memory");
  152. return NULL;
  153. }
  154. entry->flags = 0;
  155. memcpy(&entry->intervall, timeout, sizeof(entry->intervall));
  156. entry->callback = callback;
  157. entry->privdata = privdata;
  158. struct timeval now;
  159. gettimeofday(&now, NULL);
  160. schedule_nextrun(entry, &now);
  161. return entry;
  162. }
  163. void event_remove_timeout(struct event_timeout *entry)
  164. {
  165. /* mark the event as deleted -> remove in select() loop */
  166. entry->flags |= EVENT_DELETE;
  167. }
  168. void event_loop_break(void)
  169. {
  170. leave_loop = 1;
  171. }
  172. int event_loop(void)
  173. {
  174. fd_set *fdsets = malloc(sizeof(fd_set) * 2);
  175. if (fdsets == NULL) {
  176. log_print(LOG_ERROR, "event_loop(): out of memory");
  177. return -1;
  178. }
  179. leave_loop = 0;
  180. while (!leave_loop) {
  181. struct timeval timeout, *timeout_p = NULL;
  182. if (!list_empty(&event_timeout_list)) {
  183. struct timeval now;
  184. gettimeofday(&now, NULL);
  185. struct event_timeout *entry, *tmp;
  186. list_for_each_entry_safe(entry, tmp, &event_timeout_list, list) {
  187. if (entry->flags & EVENT_DELETE) {
  188. list_del(&entry->list);
  189. free(entry);
  190. continue;
  191. }
  192. /* first timeout not elapsed, exit search (since list is sorted) */
  193. if (cmp_timeval(&entry->nextrun, &now) == -1)
  194. break;
  195. /* remove event from list */
  196. list_del(&entry->list);
  197. /* execute callback, when callback returns 0 -> schedule event again */
  198. if (entry->callback(entry->privdata)) {
  199. free(entry);
  200. } else {
  201. schedule_nextrun(entry, &now);
  202. }
  203. }
  204. if (!list_empty(&event_timeout_list)) {
  205. entry = list_entry(event_timeout_list.next, typeof(*entry), list);
  206. /* calc select() timeout */
  207. sub_timeval(&timeout, &entry->nextrun, &now);
  208. timeout_p = &timeout;
  209. }
  210. }
  211. fd_set *readfds = NULL, *writefds = NULL;
  212. struct event_fd *entry, *tmp;
  213. int maxfd = -1;
  214. list_for_each_entry_safe(entry, tmp, &event_fd_list, list) {
  215. entry->flags &= ~EVENT_NEW;
  216. if (entry->flags & EVENT_DELETE) {
  217. list_del(&entry->list);
  218. free(entry);
  219. continue;
  220. }
  221. if (entry->flags & FD_READ) {
  222. if (readfds == NULL) {
  223. readfds = &fdsets[0];
  224. FD_ZERO(readfds);
  225. }
  226. FD_SET(entry->fd, readfds);
  227. }
  228. if (entry->flags & FD_WRITE) {
  229. if (writefds == NULL) {
  230. writefds = &fdsets[1];
  231. FD_ZERO(writefds);
  232. }
  233. FD_SET(entry->fd, writefds);
  234. }
  235. maxfd = (entry->fd > maxfd) ? entry->fd : maxfd;
  236. }
  237. int i = select(maxfd +1, readfds, writefds, NULL, timeout_p);
  238. if (i <= 0) {
  239. /* On error, -1 is returned, and errno is set
  240. * appropriately; the sets and timeout become
  241. * undefined, so do not rely on their contents
  242. * after an error.
  243. */
  244. continue;
  245. }
  246. list_for_each_entry(entry, &event_fd_list, list) {
  247. if (((entry->flags & (FD_READ | EVENT_NEW)) == FD_READ) && FD_ISSET(entry->fd, readfds))
  248. if (entry->read_cb(entry->fd, entry->read_priv) != 0)
  249. entry->flags |= EVENT_DELETE;
  250. if (((entry->flags & (FD_WRITE | EVENT_NEW)) == FD_WRITE) && FD_ISSET(entry->fd, writefds))
  251. if (entry->write_cb(entry->fd, entry->write_priv) != 0)
  252. entry->flags |= EVENT_DELETE;
  253. }
  254. }
  255. free(fdsets);
  256. return 0;
  257. }