[15/40] xenner: kernel: lapic code

diff --git a/pc-bios/xenner/xenner-lapic.c b/pc-bios/xenner/xenner-lapic.c
new file mode 100644
index 0000000..af08c20
--- /dev/null
+++ b/pc-bios/xenner/xenner-lapic.c
@@ -0,0 +1,622 @@ 
+/*
+ *  Copyright (C) Red Hat 2007
+ *  Copyright (C) Novell Inc. 2010
+ *
+ *  Author(s): Gerd Hoffmann <kraxel@redhat.com>
+ *             Alexander Graf <agraf@suse.de>
+ *
+ *  Xenner lapic handling
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; under version 2 of the License.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * interrupt handling is here:
+ *  - ioapic control
+ *  - lapic control
+ *  - event channel management
+ *  - irq handler
+ */
+
+#include "xenner.h"
+
+#include "cpufeature.h"
+#include "msr-index.h"
+#include "apicdef.h"
+
+/* --------------------------------------------------------------------- */
+
+static void      *ioapic_mmio;
+static uint32_t  ioapic_pins;
+
+static uint32_t ioapic_read(int reg)
+{
+    volatile uint32_t *sel = (ioapic_mmio + IOAPIC_REG_SELECT);
+    volatile uint32_t *win = (ioapic_mmio + IOAPIC_REG_WINDOW);
+    *sel = reg;
+    return *win;
+}
+
+static void ioapic_write(int reg, uint32_t val)
+{
+    volatile uint32_t *sel = (ioapic_mmio + IOAPIC_REG_SELECT);
+    volatile uint32_t *win = (ioapic_mmio + IOAPIC_REG_WINDOW);
+    *sel = reg;
+    *win = val;
+}
+
+static void ioapic_write_irq_entry(int pin, struct IO_APIC_route_entry e)
+{
+    union route_entry_union {
+        struct { uint32_t w1, w2; };
+        struct IO_APIC_route_entry entry;
+    } eu;
+
+    eu.entry = e;
+    ioapic_write(0x11 + 2*pin, eu.w2);
+    ioapic_write(0x10 + 2*pin, eu.w1);
+}
+
+static void ioapic_route_irq(int pin, int vector, int cpu_id)
+{
+    struct IO_APIC_route_entry entry;
+
+    memset(&entry,0,sizeof(entry));
+    entry.vector = vector;
+    entry.dest = cpu_id;
+
+    ioapic_write_irq_entry(pin, entry);
+}
+
+static void ioapic_unroute_irq(int pin)
+{
+    struct IO_APIC_route_entry entry;
+
+    memset(&entry,0,sizeof(entry));
+    ioapic_write_irq_entry(pin, entry);
+}
+
+static void ioapic_init(struct xen_cpu *cpu)
+{
+    ureg_t base = IOAPIC_DEFAULT_BASE_ADDRESS;
+    uint32_t ver, id;
+
+    ioapic_mmio = fixmap_page(cpu, base);
+    id = ioapic_read(IOAPIC_REG_APIC_ID);
+    ver = ioapic_read(IOAPIC_REG_VERSION);
+    ioapic_pins = ((ver >> 16) & 0xff) + 1;
+    printk(1, "%s: base %" PRIxREG ", mapped to %p, id %d, version %d, pins %d\n",
+           __FUNCTION__, base, ioapic_mmio,
+           (id >> 24) & 0x0f, ver & 0xff, ioapic_pins);
+    if (!ver) {
+        panic("oops: ioapic version register is zero", NULL);
+    }
+
+    /* PICs: mask all irqs */
+    outb(0x10, 0x20);
+}
+
+/* --------------------------------------------------------------------- */
+
+static uint32_t lapic_read(struct xen_cpu *cpu, int reg)
+{
+    volatile uint32_t *ptr = (cpu->lapic + reg);
+    return *ptr;
+}
+
+static void lapic_write(struct xen_cpu *cpu, int reg, uint32_t val)
+{
+    volatile uint32_t *ptr = (cpu->lapic + reg);
+    *ptr = val;
+}
+
+void lapic_eoi(struct xen_cpu *cpu)
+{
+    lapic_write(cpu, APIC_EOI, 0);
+}
+
+void lapic_timer(struct xen_cpu *cpu)
+{
+    uint64_t systime;
+    uint32_t lvt;
+    uint32_t count;
+    uint32_t div;
+    int64_t nsecs;
+
+    systime = cpu->v.vcpu_info->time.system_time;
+    if (cpu->oneshot) {
+        nsecs = cpu->oneshot - systime;
+        if (nsecs < 10000) {
+            nsecs = 10000;
+        }
+    } else {
+        nsecs = cpu->periodic;
+    }
+
+    /* cap the max timer time - if we return too fast the guest will be nice
+       with us and just retrigger again. */
+    if (nsecs > 0x80000000) {
+        nsecs = 0x80000000;
+    }
+
+    printk(3, "%s/%d: periodic %" PRId64 ", oneshot %" PRId64
+           ", systime %" PRId64 ", nsecs %" PRId64 "\n", __FUNCTION__,
+           cpu->id, cpu->periodic, cpu->oneshot, systime, nsecs);
+
+    printk(3, "%s/%d: periodic %" PRIx64 ", oneshot %" PRIx64
+           ", systime %" PRIx64 ", nsecs %" PRIx64 "\n", __FUNCTION__,
+           cpu->id, cpu->periodic, cpu->oneshot, systime, nsecs);
+
+    lvt = cpu->virq_to_vector[VIRQ_TIMER];
+    if (!cpu->oneshot) {
+        lvt |= (1 << 17);
+    }
+
+    div = APIC_TDR_DIV_1;
+    count = nsecs;  /* kvm virtual apic has 1 ns ticks */
+    if (count != nsecs) {
+        /* count overflow, get some more bits */
+        div = APIC_TDR_DIV_128;
+        count = nsecs / 128;
+        if (count != nsecs / 128) {
+            /* Hmm, still overflows ... */
+            printk(0, "%s: nsecs 0x%" PRIx64 ", nsecs/128 0x%" PRIx64 ", count 0x%x\n",
+                   __FUNCTION__, nsecs, nsecs / 128, count);
+            panic("lapic timer count overflow", NULL);
+        }
+    }
+
+    lapic_write(cpu, APIC_LVTT,  lvt);
+    lapic_write(cpu, APIC_TDCR,  div);
+    lapic_write(cpu, APIC_TMICT, count);
+}
+
+static void lapic_ipi_send(struct xen_cpu *cpu, int dest,
+                           int vector, uint32_t flags)
+{
+    uint32_t icr2 = SET_APIC_DEST_FIELD(dest);
+    uint32_t icr  = vector | flags;
+
+    if (lapic_read(cpu, APIC_ICR) & APIC_ICR_BUSY) {
+        printk(0, "%s: busy ...\n", __FUNCTION__);
+        while (lapic_read(cpu, APIC_ICR) & APIC_ICR_BUSY)
+            /* busy wait */;
+        printk(0, "%s: ... ok\n", __FUNCTION__);
+    }
+
+    lapic_write(cpu, APIC_ICR2, icr2);
+    lapic_write(cpu, APIC_ICR,  icr);
+}
+
+void lapic_ipi_boot(struct xen_cpu *cpu, struct xen_cpu *ap)
+{
+    int addr = EMU_PA(sipi);
+
+    emudev_set(EMUDEV_CONF_NEXT_SECONDARY_VCPU, 0, ap->id);
+    printk(0, "%s/%d: send init ...\n", __FUNCTION__, ap->id);
+    lapic_ipi_send(cpu, ap->id, 0, APIC_DM_INIT | APIC_INT_ASSERT);
+    printk(0, "%s/%d: send sipi @ %x ...\n", __FUNCTION__, ap->id, addr);
+    lapic_ipi_send(cpu, ap->id, addr >> PAGE_SHIFT, APIC_DM_STARTUP | APIC_INT_ASSERT);
+}
+
+void lapic_ipi_flush_tlb(struct xen_cpu *cpu)
+{
+    lapic_ipi_send(cpu, 0, VECTOR_FLUSH_TLB,
+                   APIC_DEST_ALLBUT | APIC_DM_FIXED | APIC_INT_ASSERT);
+}
+
+static int lapic_init(struct xen_cpu *cpu)
+{
+    struct kvm_cpuid_entry entry;
+    uint32_t ax, dx, ver, id, spiv;
+    ureg_t base;
+
+    entry.function = 0x00000001;
+    real_cpuid(&entry);
+    if (!(entry.edx & (1 << (X86_FEATURE_APIC % 32)))) {
+        printk(1, "%s: no lapic present\n", __FUNCTION__);
+        return 0;
+    }
+
+    rdmsr(MSR_IA32_APICBASE, &ax, &dx);
+    base = (uint64_t)dx << 32 | (ax & PAGE_MASK);
+    cpu->lapic = fixmap_page(cpu, base);
+    id   = lapic_read(cpu, APIC_ID);
+    ver  = lapic_read(cpu, APIC_LVR);
+    spiv = lapic_read(cpu, APIC_SPIV);
+    printk(1, "%s: base %" PRIxREG ", mapped to %p, id %d, version %d, maxlvt %d%s%s%s\n",
+           __FUNCTION__, base, cpu->lapic,
+           GET_APIC_ID(id),
+           GET_APIC_VERSION(ver),
+           GET_APIC_MAXLVT(ver),
+           ax & 0x00000100 ? ", bootcpu"    : "",
+           ax & 0x00000800 ? ", hw-enabled" : "",
+           spiv & APIC_SPIV_APIC_ENABLED ? ", sw-enabled" : "");
+    if (!ver) {
+        panic("oops: lapic version register is zero", NULL);
+    }
+
+    lapic_write(cpu, APIC_SPIV, spiv | APIC_SPIV_APIC_ENABLED);
+
+    if (ax & 0x00000100) {
+        /* boot cpu */
+        return 1;
+    }
+
+    return 2;
+}
+
+/* --------------------------------------------------------------------- */
+
+#define NUM_VEC 256
+
+static struct vector {
+    enum {
+        VECTYPE_UNDEFINED = 0,
+        VECTYPE_INTERDOMAIN,
+        VECTYPE_VIRQ,
+        VECTYPE_IPI,
+    }               type;
+    int             vec;
+    int             pin;
+    int             evtchn;
+    int             virq;
+    struct xen_cpu  *cpu;
+    char            *desc;
+} vectors[NUM_VEC];
+
+static struct vector *evtchn_to_vec[NUM_VEC];
+static struct vector *pin_to_vec[NUM_VEC];
+
+static void evtchn_route_print(int level, struct vector *vector)
+{
+    static const char *tname[] = {
+        [ VECTYPE_UNDEFINED ]   = "???",
+        [ VECTYPE_INTERDOMAIN ] = "ext",
+        [ VECTYPE_VIRQ ]        = "virq",
+        [ VECTYPE_IPI ]         = "ipi",
+    };
+    char *name = vminfo.enames + vector->evtchn * XEN_ENAME_LEN;
+    char linfo[64], sinfo[20];
+
+    switch (vector->type) {
+    case VECTYPE_INTERDOMAIN:
+        snprintf(linfo, sizeof(linfo), "vcpu %d, io-apic pin %d, %s",
+                 vector->cpu->id, vector->pin, vector->desc);
+        snprintf(sinfo, sizeof(sinfo), "%s", vector->desc);
+        break;
+    case VECTYPE_VIRQ:
+        snprintf(linfo, sizeof(linfo), "vcpu %d, virq %d, %s",
+                 vector->cpu->id, vector->virq, vector->desc);
+        snprintf(sinfo, sizeof(sinfo), "virq%d (%s)",
+                 vector->virq, vector->desc);
+        break;
+    case VECTYPE_IPI:
+        snprintf(linfo, sizeof(linfo), "vcpu %d", vector->cpu->id);
+        snprintf(sinfo, sizeof(sinfo), "ipi");
+        break;
+    default:
+        snprintf(linfo, sizeof(linfo), "FIXME");
+        snprintf(sinfo, sizeof(sinfo), "FIXME");
+        break;
+    }
+    printk(1, "irq route: vec %d = evtchn %d, type %s, %s\n",
+           vector->vec, vector->evtchn, tname[vector->type], linfo);
+    snprintf(name, XEN_ENAME_LEN, "#%d/%d %s",
+             vector->evtchn, vector->cpu->id, sinfo);
+}
+
+static struct vector *evtchn_route_add(int type, int port)
+{
+    struct vector *vector;
+    int vec = VECTOR_EVTCHN_START;
+
+    while (vectors[vec].type != VECTYPE_UNDEFINED) {
+        vec++;
+    }
+    vector = vectors + vec;
+
+    evtchn_to_vec[port] = vector;
+    vector->type   = type;
+    vector->vec    = vec;
+    vector->evtchn = port;
+    return vector;
+}
+
+int evtchn_route_interdomain(struct xen_cpu *cpu, int port, char *desc)
+{
+    struct vector *vector;
+    int pin;
+
+    vector = evtchn_to_vec[port];
+    if (vector) {
+        /* re-route to other vcpu */
+        if (vector->type != VECTYPE_INTERDOMAIN) {
+            return -1;
+        }
+    } else {
+        /* new evtchn */
+        vector = evtchn_route_add(VECTYPE_INTERDOMAIN, port);
+        vector->desc = desc ? desc : "other";
+        for (pin = 1; pin_to_vec[pin]; pin++) {
+        }
+        vector->pin = pin;
+        pin_to_vec[pin] = vector;
+        emudev_set(EMUDEV_CONF_EVTCHN_TO_PIN, vector->evtchn, vector->pin);
+    }
+    vector->cpu = cpu;
+    ioapic_route_irq(vector->pin, vector->vec, vector->cpu->id);
+    evtchn_route_print(1, vector);
+    return 0;
+}
+
+int evtchn_route_virq(struct xen_cpu *cpu, int virq, int port, char *desc)
+{
+    struct vector *vector;
+
+    vector = evtchn_route_add(VECTYPE_VIRQ, port);
+    vector->virq = virq;
+    vector->cpu  = cpu;
+    vector->desc = desc ? desc : "other";
+    cpu->virq_to_vector[virq] = vector->vec;
+    evtchn_route_print(1, vector);
+    return 0;
+}
+
+int evtchn_route_ipi(struct xen_cpu *cpu, int port)
+{
+    struct vector *vector;
+
+    vector = evtchn_route_add(VECTYPE_IPI, port);
+    vector->cpu = cpu;
+    evtchn_route_print(1, vector);
+    return 0;
+}
+
+int evtchn_send(struct xen_cpu *cpu, int port)
+{
+    struct vector *vector;
+
+    if (port >= NUM_VEC) {
+        printk(0, "%s: oops: port %d is out of range\n", __FUNCTION__, port);
+        return 0;
+    }
+
+    vector = evtchn_to_vec[port];
+    if (!vector) {
+        printk(0, "%s: oops: vector for port %d is NULL\n", __FUNCTION__, port);
+        return 0;
+    }
+    switch (vector->type) {
+    case VECTYPE_VIRQ:
+        /* should not happen */
+        printk(0, "%s: port %d, virq (Huh? -- FIXME)\n", __FUNCTION__, port);
+        return 1;
+    case VECTYPE_IPI:
+        /* handled internally */
+        printk(2, "%s: port %d, ipi\n", __FUNCTION__, port);
+        lapic_ipi_send(cpu, vector->cpu->id, vector->vec,
+                       APIC_DM_FIXED | APIC_INT_ASSERT);
+        return 1;
+    default:
+        /* handled by xenner */
+        printk(3, "%s: port %d, external\n", __FUNCTION__, port);
+        return 0;
+    }
+}
+
+void evtchn_unmask(struct xen_cpu *cpu, int port)
+{
+    struct vector *vector = evtchn_to_vec[port];
+    int resent = 0;
+
+    if (!vector) {
+        printk(0, "%s: oops: vector for port %d is NULL\n", __FUNCTION__, port);
+        return;
+    }
+
+    clear_bit(port, shared_info.evtchn_mask);
+    if (test_and_clear_bit(port, shared_info.evtchn_pending)) {
+        lapic_ipi_send(cpu, vector->cpu->id, vector->vec,
+                       APIC_DM_FIXED | APIC_INT_ASSERT);
+        resent = 1;
+    }
+    printk(2, "%s: port %d%s\n", __FUNCTION__, port,
+           resent ? ", resent" : "");
+}
+
+void evtchn_close(struct xen_cpu *cpu, int port)
+{
+    struct vector *vector = evtchn_to_vec[port];
+    char *name;
+
+    if (!vector) {
+        printk(0, "%s: oops: vector for port %d is NULL\n", __FUNCTION__, port);
+        return;
+    }
+
+    switch (vector->type) {
+    case VECTYPE_INTERDOMAIN:
+        ioapic_unroute_irq(vector->pin);
+        pin_to_vec[vector->pin] = NULL;
+        break;
+    case VECTYPE_VIRQ:
+        if (vector->virq == VIRQ_TIMER) {
+            cpu->oneshot  = 0;
+            cpu->periodic = XEN_DEFAULT_PERIOD;
+            lapic_timer(cpu);
+            return;
+        }
+        break;
+    default:
+        /* nothing -- make gcc happy */
+        break;
+    }
+    printk(1, "irq route: vec %d = evtchn %d, closing\n",
+           vector->vec, vector->evtchn);
+    name = vminfo.enames + vector->evtchn * XEN_ENAME_LEN;
+    snprintf(name, XEN_ENAME_LEN, "#%d (closed)", vector->evtchn);
+
+    memset(vector, 0, sizeof(*vector));
+    evtchn_to_vec[port] = NULL;
+    emudev_cmd(EMUDEV_CMD_EVTCHN_CLOSE, port);
+}
+
+int evtchn_alloc(int vcpu_id)
+{
+    ureg_t port;
+
+    emudev_cmd(EMUDEV_CMD_EVTCHN_ALLOC, vcpu_id);
+    port = emudev_get(EMUDEV_CONF_COMMAND_RESULT, vcpu_id);
+    return port;
+}
+
+static int evtchn_route_init(struct xen_cpu *cpu)
+{
+    uint64_t evtchn_store;
+    uint64_t evtchn_console;
+
+    evtchn_store = emudev_get(EMUDEV_CONF_EVTCH_XENSTORE, 0);
+    evtchn_console = emudev_get(EMUDEV_CONF_EVTCH_CONSOLE, 0);
+
+    evtchn_route_interdomain(cpu, evtchn_store, "xenstore");
+    evtchn_route_interdomain(cpu, evtchn_console, "console");
+
+    cpu->timerport = evtchn_alloc(cpu->id);
+    evtchn_route_virq(cpu, VIRQ_TIMER, cpu->timerport, "timer");
+    lapic_timer(cpu);
+
+    return 0;
+}
+
+/* --------------------------------------------------------------------- */
+
+static void evtchn_raise_event(struct xen_cpu *cpu, int port)
+{
+    int word = port / (sizeof(intptr_t)*8);
+
+    if (test_and_set_bit(port, shared_info.evtchn_pending) ||
+        test_bit(port, shared_info.evtchn_mask) ||
+        test_and_set_bit(word, &cpu->v.vcpu_info->evtchn_pending_sel)) {
+        return;
+    }
+    cpu->v.vcpu_info->evtchn_upcall_pending = 1;
+}
+
+int evtchn_pending(struct xen_cpu *cpu)
+{
+    if (!cpu->v.vcpu_info->evtchn_upcall_pending ||
+        !guest_irq_flag(cpu)) {
+        return 0;
+    }
+    return 1;
+}
+
+static void evtchn_forward(struct xen_cpu *cpu, struct regs *regs)
+{
+    vminfo.faults[XEN_FAULT_EVENT_CALLBACK]++;
+    bounce_trap(cpu, regs, -1, CALLBACKTYPE_event);
+#ifdef CONFIG_64BIT
+    /* return via iretq please */
+    regs->error = HCALL_IRET;
+#endif
+}
+
+void evtchn_try_forward(struct xen_cpu *cpu, struct regs *regs)
+{
+    if (context_is_emu(regs)) {
+        return;
+    }
+
+    if (!evtchn_pending(cpu)) {
+
+#if 0 /* deadlock detector */
+        static int masked;
+        uint8_t *instr = (void*)regs->rip;
+
+        if (cpu->v.vcpu_info->evtchn_upcall_pending &&
+            !guest_irq_flag(cpu)) {
+            masked++;
+            if (masked > 10000) {
+                printk(0, "%s: deadlocked? injecting BUG() for trace\n", __FUNCTION__);
+                instr[0] = 0x0f; /* ud2a -- BUG() */
+                instr[1] = 0x0b;
+                instr[2] = 0xcd; /* int 255 */
+                instr[3] = 0xff;
+                masked = 0;
+            }
+        } else {
+            masked = 0;
+        }
+#endif
+
+        return;
+    }
+
+    evtchn_forward(cpu, regs);
+}
+
+/* --------------------------------------------------------------------- */
+
+asmlinkage void do_irq(struct regs *regs)
+{
+    struct xen_cpu *cpu = get_cpu();
+    struct vector *vector = vectors + regs->trapno;
+
+    printk(3, "%s: irq vector %d\n", __FUNCTION__, vector->vec);
+
+    lapic_eoi(cpu);
+    switch (vector->type) {
+    case VECTYPE_UNDEFINED:
+        printk(0, "%s: unhandled irq (vector %d)\n", __FUNCTION__, (int)regs->trapno);
+        panic("unknown irq", regs);
+        break;
+    case VECTYPE_VIRQ:
+        if (vector->virq == VIRQ_TIMER) {
+            if (cpu->oneshot) {
+                cpu->oneshot = 0;
+                lapic_timer(cpu);
+            }
+            pv_clock_update(0);
+        }
+        /* fall through */
+    default:
+        vminfo.events[vector->evtchn]++;
+        evtchn_raise_event(cpu, vector->evtchn);
+        evtchn_try_forward(cpu, regs);
+        break;
+    }
+
+    if (context_is_emu(regs)) {
+        uint8_t *ins = (void*)regs->rip;
+        if (ins[0] == 0xf4) {
+            printk(0, "%s: WARN: rip %" PRIxREG " points to hlt\n",
+                   __FUNCTION__, regs->rip);
+        }
+    }
+}
+
+int irq_init(struct xen_cpu *cpu)
+{
+    int rc;
+
+    rc = lapic_init(cpu);
+    if (rc == 0) {
+        return 0;
+    } else if (rc == 1) {
+        /* boot cpu */
+        ioapic_init(cpu);
+        evtchn_route_init(cpu);
+    }
+    return rc;
+}