Index: arch/amd64/amd64/cpu.c =================================================================== RCS file: /cvs/src/sys/arch/amd64/amd64/cpu.c,v retrieving revision 1.102 diff -u -p -r1.102 cpu.c --- arch/amd64/amd64/cpu.c 28 Jul 2016 21:57:57 -0000 1.102 +++ arch/amd64/amd64/cpu.c 2 Mar 2017 13:20:52 -0000 @@ -426,6 +426,7 @@ cpu_attach(struct device *parent, struct /* * Enable local apic */ + lapic_attach(ci); lapic_enable(); lapic_calibrate_timer(ci); #endif @@ -683,6 +684,7 @@ cpu_hatch(void *v) ci->ci_flags |= CPUF_PRESENT; + lapic_attach(ci); lapic_enable(); lapic_startclock(); Index: arch/amd64/amd64/lapic.c =================================================================== RCS file: /cvs/src/sys/arch/amd64/amd64/lapic.c,v retrieving revision 1.45 diff -u -p -r1.45 lapic.c --- arch/amd64/amd64/lapic.c 1 Nov 2016 01:13:19 -0000 1.45 +++ arch/amd64/amd64/lapic.c 2 Mar 2017 13:20:52 -0000 @@ -35,6 +35,8 @@ #include #include #include +#include +#include #include @@ -62,11 +64,24 @@ #include #endif -struct evcount clk_count; #ifdef MULTIPROCESSOR struct evcount ipi_count; #endif +struct lapic { + char lapic_clk_name[16]; + struct evcount lapic_clk_count; + struct task_heap lapic_tasks; + int lapic_now; + + struct task lapic_hardclock; + struct task lapic_statclock; + + struct intrframe *lapic_frame; +}; + +HEAP_PROTOTYPE(task_heap, task); + void lapic_delay(int); static u_int32_t lapic_gettick(void); void lapic_clockintr(void *, struct intrframe); @@ -224,10 +239,34 @@ lapic_map(paddr_t lapic_base) enable_intr(); } +int lapic_add(struct lapic *, struct task *, int); +void lapic_hardclock(void *); +void lapic_statclock(void *); + /* * enable local apic */ void +lapic_attach(struct cpu_info *ci) +{ + static u_int64_t clk_irq = 0; + struct lapic *l; + + l = malloc(sizeof(*l), M_DEVBUF, M_WAITOK); + + snprintf(l->lapic_clk_name, sizeof(l->lapic_clk_name), + "cpu%uclk", CPU_INFO_UNIT(ci)); + evcount_attach(&l->lapic_clk_count, l->lapic_clk_name, &clk_irq); + + HEAP_INIT(task_heap, &l->lapic_tasks); + task_set(&l->lapic_hardclock, lapic_hardclock, l); + task_set(&l->lapic_statclock, lapic_statclock, l); + l->lapic_now = 0; + + ci->ci_lapic = l; +} + +void lapic_enable(void) { lapic_writereg(LAPIC_SVR, LAPIC_SVR_ENABLE | LAPIC_SPURIOUS_VECTOR); @@ -330,7 +369,6 @@ lapic_set_lvt(void) void lapic_boot_init(paddr_t lapic_base) { - static u_int64_t clk_irq = 0; #ifdef MULTIPROCESSOR static u_int64_t ipi_irq = 0; #endif @@ -364,7 +402,6 @@ lapic_boot_init(paddr_t lapic_base) idt_vec_set(LAPIC_HYPERV_VECTOR, Xintr_hyperv_upcall); #endif - evcount_attach(&clk_count, "clock", &clk_irq); #ifdef MULTIPROCESSOR evcount_attach(&ipi_count, "ipi", &ipi_irq); #endif @@ -388,34 +425,119 @@ u_int32_t lapic_frac_usec_per_cycle; u_int64_t lapic_frac_cycle_per_usec; u_int32_t lapic_delaytab[26]; +void lapic_run(struct lapic *, int); + void lapic_clockintr(void *arg, struct intrframe frame) { struct cpu_info *ci = curcpu(); + struct lapic *l = ci->ci_lapic; + struct task *t; int floor; + int now; + + l->lapic_frame = &frame; + now = l->lapic_now + lapic_readreg(LAPIC_ICR_TIMER); floor = ci->ci_handled_intr_level; ci->ci_handled_intr_level = ci->ci_ilevel; - hardclock((struct clockframe *)&frame); + + t = HEAP_EXTRACT(task_heap, &l->lapic_tasks); + CLR(t->t_flags, TASK_ONQUEUE); + (*t->t_func)(t->t_arg); + ci->ci_handled_intr_level = floor; - clk_count.ec_count++; + if (lapic_readreg(LAPIC_CCR_TIMER) == 0) + lapic_run(l, now); + + ci->ci_lapic->lapic_clk_count.ec_count++; +} + +static inline int +lapic_usec(int usec) +{ + if (usec <= 25) + return (lapic_delaytab[usec]); + + return ((lapic_frac_cycle_per_usec * usec) >> 32); +} + +void +lapic_run(struct lapic *l, int now) +{ + struct task *t; + int diff; + + t = HEAP_FIRST(task_heap, &l->lapic_tasks); + if (t == NULL) + return; + + diff = t->t_deadline - now; + if (diff < 1) + diff = 1; + + lapic_writereg(LAPIC_ICR_TIMER, diff); + l->lapic_now = now; + lapic_writereg(LAPIC_LVTT, LAPIC_TIMER_VECTOR); +} + +int +lapic_add(struct lapic *l, struct task *t, int usec) +{ + int rv = 1; + int now; + + lapic_writereg(LAPIC_LVTT, LAPIC_LVTT_M|LAPIC_TIMER_VECTOR); + now = l->lapic_now; + now += lapic_readreg(LAPIC_ICR_TIMER) - lapic_readreg(LAPIC_CCR_TIMER); + + if (ISSET(t->t_flags, TASK_ONQUEUE)) { + HEAP_REMOVE(task_heap, &l->lapic_tasks, t); + rv = 0; + } else + SET(t->t_flags, TASK_ONQUEUE); + + t->t_deadline = now + lapic_usec(usec); + HEAP_INSERT(task_heap, &l->lapic_tasks, t); + + lapic_run(l, now); + + return (rv); +} + +void +lapic_hardclock(void *arg) +{ + struct lapic *l = arg; + + hardclock(l->lapic_frame); + + lapic_add(l, &l->lapic_hardclock, 1000000 / hz); +} + +void +lapic_statclock(void *arg) +{ + struct lapic *l = arg; + + statclock(l->lapic_frame); + + lapic_add(l, &l->lapic_statclock, 1000000 / stathz); } void lapic_startclock(void) { + struct cpu_info *ci = curcpu(); + struct lapic *l = ci->ci_lapic; + /* - * Start local apic countdown timer running, in repeated mode. - * - * Mask the clock interrupt and set mode, - * then set divisor, - * then unmask and set the vector. + * Start local apic countdown timer running; */ - lapic_writereg(LAPIC_LVTT, LAPIC_LVTT_TM|LAPIC_LVTT_M); lapic_writereg(LAPIC_DCR_TIMER, LAPIC_DCRT_DIV1); - lapic_writereg(LAPIC_ICR_TIMER, lapic_tval); - lapic_writereg(LAPIC_LVTT, LAPIC_LVTT_TM|LAPIC_TIMER_VECTOR); + lapic_add(l, &l->lapic_hardclock, 1000000 / hz); + lapic_add(l, &l->lapic_statclock, 1000000 / stathz); } void @@ -503,6 +625,8 @@ lapic_calibrate_timer(struct cpu_info *c ci->ci_dev->dv_xname, tmp / (1000 * 1000)); if (lapic_per_second != 0) { + stathz = 128; + /* * reprogram the apic timer to run in periodic mode. * XXX need to program timer on other cpu's, too. @@ -510,10 +634,7 @@ lapic_calibrate_timer(struct cpu_info *c lapic_tval = (lapic_per_second * 2) / hz; lapic_tval = (lapic_tval / 2) + (lapic_tval & 0x1); - lapic_writereg(LAPIC_LVTT, LAPIC_LVTT_TM | LAPIC_LVTT_M | - LAPIC_TIMER_VECTOR); - lapic_writereg(LAPIC_DCR_TIMER, LAPIC_DCRT_DIV1); - lapic_writereg(LAPIC_ICR_TIMER, lapic_tval); + lapic_startclock(); /* * Compute fixed-point ratios between cycles and @@ -539,7 +660,7 @@ lapic_calibrate_timer(struct cpu_info *c * Now that the timer's calibrated, use the apic timer routines * for all our timing needs.. */ - delay_func = lapic_delay; + //delay_func = lapic_delay; initclock_func = lapic_initclocks; } } Index: arch/amd64/include/cpu.h =================================================================== RCS file: /cvs/src/sys/arch/amd64/include/cpu.h,v retrieving revision 1.107 diff -u -p -r1.107 cpu.h --- arch/amd64/include/cpu.h 14 Dec 2016 10:30:59 -0000 1.107 +++ arch/amd64/include/cpu.h 2 Mar 2017 13:20:52 -0000 @@ -84,6 +84,8 @@ union vmm_cpu_cap { struct svm vcc_svm; }; +struct lapic; + struct x86_64_tss; struct cpu_info { struct device *ci_dev; @@ -117,6 +119,7 @@ struct cpu_info { int ci_mutex_level; #endif + struct lapic *ci_lapic; volatile u_int ci_flags; u_int32_t ci_ipis; Index: arch/amd64/include/i82489var.h =================================================================== RCS file: /cvs/src/sys/arch/amd64/include/i82489var.h,v retrieving revision 1.17 diff -u -p -r1.17 i82489var.h --- arch/amd64/include/i82489var.h 22 Jun 2016 01:12:38 -0000 1.17 +++ arch/amd64/include/i82489var.h 2 Mar 2017 13:20:53 -0000 @@ -119,6 +119,7 @@ struct cpu_info; extern void lapic_boot_init(paddr_t); extern void lapic_set_lvt(void); +extern void lapic_attach(struct cpu_info *); extern void lapic_enable(void); extern void lapic_disable(void); extern void lapic_calibrate_timer(struct cpu_info *ci); Index: kern/kern_task.c =================================================================== RCS file: /cvs/src/sys/kern/kern_task.c,v retrieving revision 1.19 diff -u -p -r1.19 kern_task.c --- kern/kern_task.c 14 Feb 2017 10:31:15 -0000 1.19 +++ kern/kern_task.c 2 Mar 2017 13:20:56 -0000 @@ -23,8 +23,6 @@ #include #include -#define TASK_ONQUEUE 1 - struct taskq { enum { TQ_S_CREATED, @@ -306,3 +304,11 @@ taskq_thread(void *xtq) kthread_exit(0); } + +static inline int +task_heap_cmp(const struct task *a, const struct task *b) +{ + return (a->t_deadline - b->t_deadline); +} + +HEAP_GENERATE(task_heap, task, _t_entry._t_heap, task_heap_cmp); Index: kern/subr_tree.c =================================================================== RCS file: /cvs/src/sys/kern/subr_tree.c,v retrieving revision 1.6 diff -u -p -r1.6 subr_tree.c --- kern/subr_tree.c 20 Sep 2016 01:11:27 -0000 1.6 +++ kern/subr_tree.c 2 Mar 2017 13:20:56 -0000 @@ -610,3 +610,181 @@ _rb_check(const struct rb_type *t, void (unsigned long)RBE_LEFT(rbe) == poison && (unsigned long)RBE_RIGHT(rbe) == poison); } + +static inline struct _heap_entry * +heap_n2e(const struct _heap_type *t, void *node) +{ + caddr_t addr = (caddr_t)node; + + return ((struct _heap_entry *)(addr + t->t_offset)); +} + +static inline void * +heap_e2n(const struct _heap_type *t, struct _heap_entry *he) +{ + caddr_t addr = (caddr_t)he; + + return ((void *)(addr - t->t_offset)); +} + +static struct _heap_entry * +_heap_merge(const struct _heap_type *t, + struct _heap_entry *he1, struct _heap_entry *he2) +{ + struct _heap_entry *hi, *lo; + struct _heap_entry *child; + + if (he1 == NULL) + return (he2); + if (he2 == NULL) + return (he1); + + if (t->t_compare(heap_e2n(t, he1), heap_e2n(t, he2)) >= 0) { + hi = he1; + lo = he2; + } else { + lo = he1; + hi = he2; + } + + child = lo->he_child; + + hi->he_left = lo; + hi->he_nextsibling = child; + if (child != NULL) + child->he_left = hi; + lo->he_child = hi; + lo->he_left = NULL; + lo->he_nextsibling = NULL; + + return (lo); +} + +static inline void +_heap_sibling_remove(struct _heap_entry *he) +{ + if (he->he_left == NULL) + return; + + if (he->he_left->he_child == he) { + if ((he->he_left->he_child = he->he_nextsibling) != NULL) + he->he_nextsibling->he_left = he->he_left; + } else { + if ((he->he_left->he_nextsibling = he->he_nextsibling) != NULL) + he->he_nextsibling->he_left = he->he_left; + } + + he->he_left = NULL; + he->he_nextsibling = NULL; +} + +static inline struct _heap_entry * +_heap_2pass_merge(const struct _heap_type *t, struct _heap_entry *root) +{ + struct _heap_entry *node, *next = NULL; + struct _heap_entry *tmp, *list = NULL; + + node = root->he_child; + if (node == NULL) + return (NULL); + + root->he_child = NULL; + + /* first pass */ + for (next = node->he_nextsibling; next != NULL; + next = (node != NULL ? node->he_nextsibling : NULL)) { + tmp = next->he_nextsibling; + node = _heap_merge(t, node, next); + + /* insert head */ + node->he_nextsibling = list; + list = node; + node = tmp; + } + + /* odd child case */ + if (node != NULL) { + node->he_nextsibling = list; + list = node; + } + + /* second pass */ + while (list->he_nextsibling != NULL) { + tmp = list->he_nextsibling->he_nextsibling; + list = _heap_merge(t, list, list->he_nextsibling); + list->he_nextsibling = tmp; + } + + list->he_left = NULL; + list->he_nextsibling = NULL; + + return (list); +} + +void +_heap_insert(const struct _heap_type *t, struct _heap *h, void *node) +{ + struct _heap_entry *he = heap_n2e(t, node); + + he->he_left = NULL; + he->he_child = NULL; + he->he_nextsibling = NULL; + + h->h_root = _heap_merge(t, h->h_root, he); +} + +void +_heap_remove(const struct _heap_type *t, struct _heap *h, void *node) +{ + struct _heap_entry *he = heap_n2e(t, node); + + if (he->he_left == NULL) { + _heap_extract(t, h); + return; + } + + _heap_sibling_remove(he); + h->h_root = _heap_merge(t, h->h_root, _heap_2pass_merge(t, he)); +} + +void * +_heap_first(const struct _heap_type *t, struct _heap *h) +{ + struct _heap_entry *first = h->h_root; + + if (first == NULL) + return (NULL); + + return (heap_e2n(t, first)); +} + +void * +_heap_extract(const struct _heap_type *t, struct _heap *h) +{ + struct _heap_entry *first = h->h_root; + + if (first == NULL) + return (NULL); + + h->h_root = _heap_2pass_merge(t, first); + + return (heap_e2n(t, first)); +} + +void * +_heap_cextract(const struct _heap_type *t, struct _heap *h, const void *key) +{ + struct _heap_entry *first = h->h_root; + void *node; + + if (first == NULL) + return (NULL); + + node = heap_e2n(t, first); + if (t->t_compare(node, key) > 0) + return (NULL); + + h->h_root = _heap_2pass_merge(t, first); + + return (node); +} Index: sys/task.h =================================================================== RCS file: /cvs/src/sys/sys/task.h,v retrieving revision 1.11 diff -u -p -r1.11 task.h --- sys/task.h 7 Jun 2016 07:53:33 -0000 1.11 +++ sys/task.h 2 Mar 2017 13:20:56 -0000 @@ -20,26 +20,45 @@ #define _SYS_TASK_H_ #include +#include struct taskq; struct task { - TAILQ_ENTRY(task) t_entry; + union { + TAILQ_ENTRY(task) _t_list; + HEAP_ENTRY(task) _t_heap; + } _t_entry; +#define t_entry _t_entry._t_list + void (*t_func)(void *); void *t_arg; + unsigned int t_flags; + int t_deadline; }; TAILQ_HEAD(task_list, task); +HEAP_HEAD(task_heap); + +#define TASK_ONQUEUE 2 /* task is on the todo queue */ +#define TASK_INITIALIZED 4 /* task is initialized */ #define TASKQ_MPSAFE (1 << 0) #define TASKQ_CANTSLEEP (1 << 1) -#define TASK_INITIALIZER(_f, _a) {{ NULL, NULL }, (_f), (_a), 0 } +#define task_pending(_t) ((_t)->t_flags & TASK_ONQUEUE) +#define task_initialized(_t) ((_t)->t_flags & TASK_INITIALIZED) #ifdef _KERNEL extern struct taskq *const systq; extern struct taskq *const systqmp; + +#define TASK_INITIALIZER(_f, _a) { \ + .t_func = (_f), \ + .t_arg = (_a), \ + .t_flags = TASK_INITIALIZED, \ +} struct taskq *taskq_create(const char *, unsigned int, int, unsigned int); void taskq_destroy(struct taskq *); Index: sys/tree.h =================================================================== RCS file: /cvs/src/sys/sys/tree.h,v retrieving revision 1.25 diff -u -p -r1.25 tree.h --- sys/tree.h 26 Sep 2016 08:08:51 -0000 1.25 +++ sys/tree.h 2 Mar 2017 13:20:56 -0000 @@ -984,4 +984,116 @@ RBT_GENERATE_INTERNAL(_name, _type, _fie #endif /* _KERNEL */ +struct _heap_type { + int (*t_compare)(const void *, const void *); + unsigned int t_offset; /* offset of heap_entry in type */ +}; + +struct _heap_entry { + struct _heap_entry *he_left; + struct _heap_entry *he_child; + struct _heap_entry *he_nextsibling; +}; +#define HEAP_ENTRY(_entry) struct _heap_entry + +struct _heap { + struct _heap_entry *h_root; +}; + +#define HEAP_HEAD(_name) \ +struct _name { \ + struct _heap heap; \ +} + +#ifdef _KERNEL + +static inline void +_heap_init(struct _heap *h) +{ + h->h_root = NULL; +} + +static inline int +_heap_empty(struct _heap *h) +{ + return (h->h_root == NULL); +} + +void _heap_insert(const struct _heap_type *, struct _heap *, void *); +void _heap_remove(const struct _heap_type *, struct _heap *, void *); +void *_heap_first(const struct _heap_type *, struct _heap *); +void *_heap_extract(const struct _heap_type *, struct _heap *); +void *_heap_cextract(const struct _heap_type *, struct _heap *, + const void *); + +#define HEAP_INITIALIZER(_head) { { NULL } } + +#define HEAP_PROTOTYPE(_name, _type) \ +extern const struct _heap_type *const _name##_HEAP_TYPE; \ + \ +static inline void \ +_name##_HEAP_INIT(struct _name *head) \ +{ \ + _heap_init(&head->heap); \ +} \ + \ +static inline void \ +_name##_HEAP_INSERT(struct _name *head, struct _type *elm) \ +{ \ + _heap_insert(_name##_HEAP_TYPE, &head->heap, elm); \ +} \ + \ +static inline void \ +_name##_HEAP_REMOVE(struct _name *head, struct _type *elm) \ +{ \ + _heap_remove(_name##_HEAP_TYPE, &head->heap, elm); \ +} \ + \ +static inline struct _type * \ +_name##_HEAP_FIRST(struct _name *head) \ +{ \ + return _heap_first(_name##_HEAP_TYPE, &head->heap); \ +} \ + \ +static inline struct _type * \ +_name##_HEAP_EXTRACT(struct _name *head) \ +{ \ + return _heap_extract(_name##_HEAP_TYPE, &head->heap); \ +} \ + \ +static inline struct _type * \ +_name##_HEAP_CEXTRACT(struct _name *head, const struct _type *key) \ +{ \ + return _heap_cextract(_name##_HEAP_TYPE, &head->heap, key); \ +} \ + \ +static inline int \ +_name##_HEAP_EMPTY(struct _name *head) \ +{ \ + return _heap_empty(&head->heap); \ +} + +#define HEAP_GENERATE(_name, _type, _field, _cmp) \ +static int \ +_name##_HEAP_COMPARE(const void *lptr, const void *rptr) \ +{ \ + const struct _type *l = lptr, *r = rptr; \ + return _cmp(l, r); \ +} \ +static const struct _heap_type _name##_HEAP_INFO = { \ + _name##_HEAP_COMPARE, \ + offsetof(struct _type, _field), \ +}; \ +const struct _heap_type *const _name##_HEAP_TYPE = &_name##_HEAP_INFO + +#define HEAP_INIT(_name, _h) _name##_HEAP_INIT((_h)) +#define HEAP_INSERT(_name, _h, _e) _name##_HEAP_INSERT((_h), (_e)) +#define HEAP_REMOVE(_name, _h, _e) _name##_HEAP_REMOVE((_h), (_e)) +#define HEAP_FIRST(_name, _h) _name##_HEAP_FIRST((_h)) +#define HEAP_EXTRACT(_name, _h) _name##_HEAP_EXTRACT((_h)) +#define HEAP_CEXTRACT(_name, _h, _k) _name##_HEAP_CEXTRACT((_h), (_k)) +#define HEAP_EMPTY(_name, _h) _name##_HEAP_EMPTY((_h)) + +#endif /* _KERNEL */ + #endif /* _SYS_TREE_H_ */