1 /* $OpenBSD: kern_sensors.c,v 1.6 2006/01/19 17:08:40 grange 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 _sensors_count = 0; 31 struct sensors_head _sensors_list = SLIST_HEAD_INITIALIZER(&_sensors_list); 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 void 50 sensor_add(struct sensor *sens) 51 { 52 struct sensor *v, *nv; 53 int s; 54 55 s = splhigh(); 56 if (_sensors_count == 0) { 57 sens->num = 0; 58 SLIST_INSERT_HEAD(&_sensors_list, sens, list); 59 } else { 60 for (v = SLIST_FIRST(&_sensors_list); 61 (nv = SLIST_NEXT(v, list)) != NULL; v = nv) 62 if (nv->num - v->num > 1) 63 break; 64 sens->num = v->num + 1; 65 SLIST_INSERT_AFTER(v, sens, list); 66 } 67 _sensors_count++; 68 splx(s); 69 } 70 71 void 72 sensor_del(struct sensor *sens) 73 { 74 int s; 75 76 s = splhigh(); 77 _sensors_count--; 78 SLIST_REMOVE(&_sensors_list, sens, sensor, list); 79 splx(s); 80 } 81 82 int 83 sensor_task_register(void *arg, void (*func)(void *), int period) 84 { 85 struct sensor_task *st; 86 87 st = malloc(sizeof(struct sensor_task), M_DEVBUF, M_NOWAIT); 88 if (st == NULL) 89 return (1); 90 91 st->arg = arg; 92 st->func = func; 93 st->period = period; 94 95 st->running = 1; 96 97 if (TAILQ_EMPTY(&tasklist)) 98 kthread_create_deferred(sensor_task_create, NULL); 99 100 sensor_task_schedule(st); 101 wakeup(&tasklist); 102 103 return (0); 104 } 105 106 void 107 sensor_task_unregister(void *arg) 108 { 109 struct sensor_task *st; 110 111 TAILQ_FOREACH(st, &tasklist, entry) { 112 if (st->arg == arg) 113 st->running = 0; 114 } 115 } 116 117 void 118 sensor_task_create(void *arg) 119 { 120 if (kthread_create(sensor_task_thread, NULL, NULL, "sensors") != 0) 121 panic("sensors kthread"); 122 } 123 124 void 125 sensor_task_thread(void *arg) 126 { 127 struct sensor_task *st, *nst; 128 time_t now; 129 130 while (!TAILQ_EMPTY(&tasklist)) { 131 while ((nst = TAILQ_FIRST(&tasklist))->nextrun > 132 (now = time_uptime)) 133 tsleep(&tasklist, PWAIT, "timeout", 134 (nst->nextrun - now) * hz); 135 136 while ((st = nst) != NULL) { 137 nst = TAILQ_NEXT(st, entry); 138 139 if (st->nextrun > now) 140 break; 141 142 /* take it out while we work on it */ 143 TAILQ_REMOVE(&tasklist, st, entry); 144 145 if (!st->running) { 146 free(st, M_DEVBUF); 147 continue; 148 } 149 150 /* run the task */ 151 st->func(st->arg); 152 /* stick it back in the tasklist */ 153 sensor_task_schedule(st); 154 } 155 } 156 157 kthread_exit(0); 158 } 159 160 void 161 sensor_task_schedule(struct sensor_task *st) 162 { 163 struct sensor_task *cst; 164 165 st->nextrun = time_uptime + st->period; 166 167 TAILQ_FOREACH(cst, &tasklist, entry) { 168 if (cst->nextrun > st->nextrun) { 169 TAILQ_INSERT_BEFORE(cst, st, entry); 170 return; 171 } 172 } 173 174 /* must be an empty list, or at the end of the list */ 175 TAILQ_INSERT_TAIL(&tasklist, st, entry); 176 } 177 178