kernel/preempt.c

   1
   2 #ifdef CONFIG_PREEMPT_NOTIFIERS_COMPAT
   3
   4 #include <linux/sched.h>
   5 #include <linux/percpu.h>
   6
   7 static DEFINE_SPINLOCK(pn_lock);
   8 static LIST_HEAD(pn_list);
   9
  10 #define dprintk(fmt) do {                                               \
  11                 if (0)                                                  \
  12                         printk("%s (%d/%d): " fmt, __FUNCTION__,        \
  13                                current->pid, raw_smp_processor_id());   \
  14         } while (0)
  15
  16 static void preempt_enable_sched_out_notifiers(void)
  17 {
  18         asm volatile ("mov %0, %%db0" : : "r"(schedule));
  19         asm volatile ("mov %0, %%db7" : : "r"(0x701ul));
  20 #ifdef CONFIG_X86_64
  21         current->thread.debugreg7 = 0ul;
  22 #else
  23         current->thread.debugreg[7] = 0ul;
  24 #endif
  25 #ifdef TIF_DEBUG
  26         clear_tsk_thread_flag(current, TIF_DEBUG);
  27 #endif
  28 }
  29
  30 static void preempt_enable_sched_in_notifiers(void * addr)
  31 {
  32         asm volatile ("mov %0, %%db0" : : "r"(addr));
  33         asm volatile ("mov %0, %%db7" : : "r"(0x701ul));
  34 #ifdef CONFIG_X86_64
  35         current->thread.debugreg0 = (unsigned long) addr;
  36         current->thread.debugreg7 = 0x701ul;
  37 #else
  38         current->thread.debugreg[0] = (unsigned long) addr;
  39         current->thread.debugreg[7] = 0x701ul;
  40 #endif
  41 #ifdef TIF_DEBUG
  42         set_tsk_thread_flag(current, TIF_DEBUG);
  43 #endif
  44 }
  45
  46 void special_reload_dr7(void)
  47 {
  48         asm volatile ("mov %0, %%db7" : : "r"(0x701ul));
  49 }
  50 EXPORT_SYMBOL_GPL(special_reload_dr7);
  51
  52 static void __preempt_disable_notifiers(void)
  53 {
  54         asm volatile ("mov %0, %%db7" : : "r"(0ul));
  55 }
  56
  57 static void preempt_disable_notifiers(void)
  58 {
  59         __preempt_disable_notifiers();
  60 #ifdef CONFIG_X86_64
  61         current->thread.debugreg7 = 0ul;
  62 #else
  63         current->thread.debugreg[7] = 0ul;
  64 #endif
  65 #ifdef TIF_DEBUG
  66         clear_tsk_thread_flag(current, TIF_DEBUG);
  67 #endif
  68 }
  69
  70 static void fastcall  __attribute__((used)) preempt_notifier_trigger(void *** ip)
  71 {
  72         struct preempt_notifier *pn;
  73         int cpu = raw_smp_processor_id();
  74         int found = 0;
  75
  76         dprintk(" - in\n");
  77         //dump_stack();
  78         spin_lock(&pn_lock);
  79         list_for_each_entry(pn, &pn_list, link)
  80                 if (pn->tsk == current) {
  81                         found = 1;
  82                         break;
  83                 }
  84         spin_unlock(&pn_lock);
  85
  86         if (found) {
  87                 if ((void *) *ip != schedule) {
  88                         dprintk("sched_in\n");
  89                         preempt_enable_sched_out_notifiers();
  90
  91                         preempt_disable();
  92                         local_irq_enable();
  93                         pn->ops->sched_in(pn, cpu);
  94                         local_irq_disable();
  95                         preempt_enable_no_resched();
  96                 } else {
  97                         void * sched_in_addr;
  98                         dprintk("sched_out\n");
  99 #ifdef CONFIG_X86_64
 100                         sched_in_addr = **(ip+3);
 101 #else
 102                         /* no special debug stack switch on x86 */
 103                         sched_in_addr = (void *) *(ip+3);
 104 #endif
 105                         preempt_enable_sched_in_notifiers(sched_in_addr);
 106
 107                         preempt_disable();
 108                         local_irq_enable();
 109                         pn->ops->sched_out(pn, NULL);
 110                         local_irq_disable();
 111                         preempt_enable_no_resched();
 112                 }
 113         } else
 114                 __preempt_disable_notifiers();
 115         dprintk(" - out\n");
 116 }
 117
 118 unsigned long orig_int1_handler;
 119
 120 #ifdef CONFIG_X86_64
 121
 122 #define SAVE_REGS \
 123         "push %rax; push %rbx; push %rcx; push %rdx; " \
 124         "push %rsi; push %rdi; push %rbp; " \
 125         "push %r8;  push %r9;  push %r10; push %r11; " \
 126         "push %r12; push %r13; push %r14; push %r15"
 127
 128 #define RESTORE_REGS \
 129         "pop %r15; pop %r14; pop %r13; pop %r12; " \
 130         "pop %r11; pop %r10; pop %r9;  pop %r8; " \
 131         "pop %rbp; pop %rdi; pop %rsi; " \
 132         "pop %rdx; pop %rcx; pop %rbx; pop %rax "
 133
 134 #define TMP "%rax"
 135
 136 #else
 137
 138 #define SAVE_REGS "pusha"
 139 #define RESTORE_REGS "popa"
 140 #define TMP "%eax"
 141
 142 #endif
 143
 144 asm ("pn_int1_handler:  \n\t"
 145      "push "  TMP " \n\t"
 146      "mov %db6, " TMP " \n\t"
 147      "test $1, " TMP " \n\t"
 148      "pop "  TMP " \n\t"
 149      "jz .Lnotme \n\t"
 150      SAVE_REGS "\n\t"
 151 #ifdef CONFIG_X86_64
 152      "leaq 120(%rsp),%rdi\n\t"
 153 #else
 154      "leal 32(%esp),%eax\n\t"
 155 #endif
 156      "call preempt_notifier_trigger \n\t"
 157      RESTORE_REGS "\n\t"
 158 #ifdef CONFIG_X86_64
 159      "orq $0x10000, 16(%rsp) \n\t"
 160      "iretq \n\t"
 161 #else
 162      "orl $0x10000, 8(%esp) \n\t"
 163      "iret \n\t"
 164 #endif
 165      ".Lnotme: \n\t"
 166 #ifdef CONFIG_X86_64
 167      "jmpq *orig_int1_handler\n\t"
 168 #else
 169      "jmpl *orig_int1_handler\n\t"
 170 #endif
 171         );
 172
 173 void preempt_notifier_register(struct preempt_notifier *notifier)
 174 {
 175         unsigned long flags;
 176
 177         dprintk(" - in\n");
 178         spin_lock_irqsave(&pn_lock, flags);
 179         preempt_enable_sched_out_notifiers();
 180         notifier->tsk = current;
 181         list_add(&notifier->link, &pn_list);
 182         spin_unlock_irqrestore(&pn_lock, flags);
 183         dprintk(" - out\n");
 184 }
 185
 186 void preempt_notifier_unregister(struct preempt_notifier *notifier)
 187 {
 188         unsigned long flags;
 189
 190         dprintk(" - in\n");
 191         spin_lock_irqsave(&pn_lock, flags);
 192         list_del(&notifier->link);
 193         spin_unlock_irqrestore(&pn_lock, flags);
 194         preempt_disable_notifiers();
 195         dprintk(" - out\n");
 196 }
 197
 198 struct intr_gate {
 199         u16 offset0;
 200         u16 segment;
 201         u16 junk;
 202         u16 offset1;
 203 #ifdef CONFIG_X86_64
 204         u32 offset2;
 205         u32 blah;
 206 #endif
 207 } __attribute__((packed));
 208
 209 struct idt_desc {
 210         u16 limit;
 211         struct intr_gate *gates;
 212 } __attribute__((packed));
 213
 214 static struct intr_gate orig_int1_gate;
 215
 216 void pn_int1_handler(void);
 217
 218 void preempt_notifier_sys_init(void)
 219 {
 220         struct idt_desc idt_desc;
 221         struct intr_gate *int1_gate;
 222
 223         printk("kvm: emulating preempt notifiers;"
 224                " do not benchmark on this machine\n");
 225         dprintk("\n");
 226         asm ("sidt %0" : "=m"(idt_desc));
 227         int1_gate = &idt_desc.gates[1];
 228         orig_int1_gate = *int1_gate;
 229         orig_int1_handler = int1_gate->offset0
 230                 | ((u32)int1_gate->offset1 << 16);
 231 #ifdef CONFIG_X86_64
 232         orig_int1_handler |= (u64)int1_gate->offset2 << 32;
 233 #endif
 234         int1_gate->offset0 = (unsigned long)pn_int1_handler;
 235         int1_gate->offset1 = (unsigned long)pn_int1_handler >> 16;
 236 #ifdef CONFIG_X86_64
 237         int1_gate->offset2 = (unsigned long)pn_int1_handler >> 32;
 238 #endif
 239 }
 240
 241 static void do_disable(void *blah)
 242 {
 243 #ifdef TIF_DEBUG
 244         if (!test_tsk_thread_flag(current, TIF_DEBUG))
 245 #else
 246 #ifdef CONFIG_X86_64
 247         if (!current->thread.debugreg7)
 248 #else
 249         if (!current->thread.debugreg[7])
 250 #endif
 251 #endif
 252                 __preempt_disable_notifiers();
 253 }
 254
 255 void preempt_notifier_sys_exit(void)
 256 {
 257         struct idt_desc idt_desc;
 258
 259         dprintk("\n");
 260         on_each_cpu(do_disable, NULL, 1, 1);
 261         asm ("sidt %0" : "=m"(idt_desc));
 262         idt_desc.gates[1] = orig_int1_gate;
 263 }
 264
 265 #endif