1 /* $OpenBSD: kern_sensors.c,v 1.7 2006/01/28 09:53:37 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 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 struct sensor * 83 sensor_get(int num) 84 { 85 struct sensor *s; 86 87 SLIST_FOREACH(s, &sensors_list, list) { 88 if (s->num == num) 89 return (s); 90 } 91 92 return (NULL); 93 } 94 95 int 96 sensor_task_register(void *arg, void (*func)(void *), int period) 97 { 98 struct sensor_task *st; 99 100 st = malloc(sizeof(struct sensor_task), M_DEVBUF, M_NOWAIT); 101 if (st == NULL) 102 return (1); 103 104 st->arg = arg; 105 st->func = func; 106 st->period = period; 107 108 st->running = 1; 109 110 if (TAILQ_EMPTY(&tasklist)) 111 kthread_create_deferred(sensor_task_create, NULL); 112 113 sensor_task_schedule(st); 114 wakeup(&tasklist); 115 116 return (0); 117 } 118 119 void 120 sensor_task_unregister(void *arg) 121 { 122 struct sensor_task *st; 123 124 TAILQ_FOREACH(st, &tasklist, entry) { 125 if (st->arg == arg) 126 st->running = 0; 127 } 128 } 129 130 void 131 sensor_task_create(void *arg) 132 { 133 if (kthread_create(sensor_task_thread, NULL, NULL, "sensors") != 0) 134 panic("sensors kthread"); 135 } 136 137 void 138 sensor_task_thread(void *arg) 139 { 140 struct sensor_task *st, *nst; 141 time_t now; 142 143 while (!TAILQ_EMPTY(&tasklist)) { 144 while ((nst = TAILQ_FIRST(&tasklist))->nextrun > 145 (now = time_uptime)) 146 tsleep(&tasklist, PWAIT, "timeout", 147 (nst->nextrun - now) * hz); 148 149 while ((st = nst) != NULL) { 150 nst = TAILQ_NEXT(st, entry); 151 152 if (st->nextrun > now) 153 break; 154 155 /* take it out while we work on it */ 156 TAILQ_REMOVE(&tasklist, st, entry); 157 158 if (!st->running) { 159 free(st, M_DEVBUF); 160 continue; 161 } 162 163 /* run the task */ 164 st->func(st->arg); 165 /* stick it back in the tasklist */ 166 sensor_task_schedule(st); 167 } 168 } 169 170 kthread_exit(0); 171 } 172 173 void 174 sensor_task_schedule(struct sensor_task *st) 175 { 176 struct sensor_task *cst; 177 178 st->nextrun = time_uptime + st->period; 179 180 TAILQ_FOREACH(cst, &tasklist, entry) { 181 if (cst->nextrun > st->nextrun) { 182 TAILQ_INSERT_BEFORE(cst, st, entry); 183 return; 184 } 185 } 186 187 /* must be an empty list, or at the end of the list */ 188 TAILQ_INSERT_TAIL(&tasklist, st, entry); 189 } 190 191