1 /* $OpenBSD: kern_sensors.c,v 1.3 2005/11/10 08:32:56 dlg Exp $ */ 2 3 /* 4 * Copyright (c) 2005 David Gwynne <dlg@openbsd.org> 5 * 6 * Permission to use, copy, modify, and distribute this software for any 7 * purpose with or without fee is hereby granted, provided that the above 8 * copyright notice and this permission notice appear in all copies. 9 * 10 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 11 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 12 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 13 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 14 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 15 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 16 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 17 */ 18 19 #include <sys/param.h> 20 #include <sys/systm.h> 21 #include <sys/kernel.h> 22 #include <sys/malloc.h> 23 #include <sys/kthread.h> 24 #include <sys/queue.h> 25 #include <sys/types.h> 26 #include <sys/time.h> 27 28 #include <sys/sensors.h> 29 30 int nsensors = 0; 31 struct sensors_head sensors = SLIST_HEAD_INITIALIZER(&sensors); 32 33 struct sensor_task { 34 void *arg; 35 void (*func)(void *); 36 37 int period; 38 time_t nextrun; 39 volatile int running; 40 TAILQ_ENTRY(sensor_task) entry; 41 }; 42 43 void sensor_task_create(void *); 44 void sensor_task_thread(void *); 45 void sensor_task_schedule(struct sensor_task *); 46 47 TAILQ_HEAD(, sensor_task) tasklist = TAILQ_HEAD_INITIALIZER(tasklist); 48 49 int 50 sensor_task_register(void *arg, void (*func)(void *), int period) 51 { 52 struct sensor_task *st; 53 54 st = malloc(sizeof(struct sensor_task), M_DEVBUF, M_NOWAIT); 55 if (st == NULL) 56 return (1); 57 58 st->arg = arg; 59 st->func = func; 60 st->period = period; 61 62 st->running = 1; 63 64 if (TAILQ_EMPTY(&tasklist)) 65 kthread_create_deferred(sensor_task_create, NULL); 66 67 sensor_task_schedule(st); 68 wakeup(&tasklist); 69 70 return (0); 71 } 72 73 void 74 sensor_task_unregister(void *arg) 75 { 76 struct sensor_task *st, *nst; 77 78 nst = TAILQ_FIRST(&tasklist); 79 while ((st = nst) != NULL) { 80 nst = TAILQ_NEXT(st, entry); 81 82 if (st->arg == arg) 83 st->running = 0; 84 } 85 } 86 87 void 88 sensor_task_create(void *arg) 89 { 90 if (kthread_create(sensor_task_thread, NULL, NULL, "sensors") != 0) 91 panic("sensors kthread"); 92 } 93 94 void 95 sensor_task_thread(void *arg) 96 { 97 struct sensor_task *st, *nst; 98 time_t now; 99 100 while (!TAILQ_EMPTY(&tasklist)) { 101 nst = TAILQ_FIRST(&tasklist); 102 103 while ((nst = TAILQ_FIRST(&tasklist))->nextrun > 104 (now = time_uptime)) 105 tsleep(&tasklist, PWAIT, "timeout", 106 (nst->nextrun - now) * hz); 107 108 while ((st = nst) != NULL) { 109 nst = TAILQ_NEXT(st, entry); 110 111 if (st->nextrun > now) 112 break; 113 114 /* take it out while we work on it */ 115 TAILQ_REMOVE(&tasklist, st, entry); 116 117 if (!st->running) { 118 free(st, M_DEVBUF); 119 continue; 120 } 121 122 /* run the task */ 123 st->func(st->arg); 124 /* stick it back in the tasklist */ 125 sensor_task_schedule(st); 126 } 127 } 128 129 kthread_exit(0); 130 } 131 132 void 133 sensor_task_schedule(struct sensor_task *st) 134 { 135 struct sensor_task *cst, *nst; 136 137 st->nextrun = time_uptime + st->period; 138 139 nst = TAILQ_FIRST(&tasklist); 140 while ((cst = nst) != NULL) { 141 nst = TAILQ_NEXT(cst, entry); 142 143 if (cst->nextrun > st->nextrun) { 144 TAILQ_INSERT_BEFORE(cst, st, entry); 145 return; 146 } 147 } 148 149 /* must be an empty list, or at the end of the list */ 150 TAILQ_INSERT_TAIL(&tasklist, st, entry); 151 } 152 153