aboutsummaryrefslogtreecommitdiffstats
path: root/protocol/iwrap/main.c
blob: 3abdce8dfc90a6e3e471e3d064d5c4535d8e8248 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
/*
Copyright 2011 Jun Wako <wakojun@gmail.com>

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, either version 2 of the License, or
(at your option) any later version.

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/>.
*/
#include <stdint.h>
#include <avr/interrupt.h>
#include <avr/io.h>
//#include <avr/wdt.h>
#include "wd.h" // in order to use watchdog in interrupt mode
#include <avr/sleep.h>
#include <util/delay.h>
#include <avr/power.h>
#include "keyboard.h"
#include "matrix.h"
#include "host.h"
#include "action.h"
#include "iwrap.h"
#ifdef PROTOCOL_VUSB
#   include "vusb.h"
#   include "usbdrv.h"
#endif
#include "uart.h"
#include "suart.h"
#include "timer.h"
#include "debug.h"
#include "keycode.h"
#include "command.h"


static void sleep(uint8_t term);
static bool console(void);
static bool console_command(uint8_t c);
static uint8_t key2asc(uint8_t key);


/*
static void set_prr(void)
{
    power_adc_disable();
    power_spi_disable();
    power_twi_disable();
#ifndef TIMER_H
    //power_timer0_disable(); // used in timer.c
#endif
    power_timer1_disable();
    power_timer2_disable();
}
*/

/*
static void pullup_pins(void)
{
    // DDRs are set to 0(input) by default.
#ifdef PORTA
    PORTA = 0xFF;
#endif
    PORTB = 0xFF;
    PORTC = 0xFF;
    PORTD = 0xFF;
#ifdef PORTE
    PORTE = 0xFF;
#endif
#ifdef PORTE
    PORTF = 0xFF;
#endif
}
*/


#ifdef PROTOCOL_VUSB
static void disable_vusb(void)
{
    // disable interrupt & disconnect to prevent host from enumerating
    USB_INTR_ENABLE &= ~(1 << USB_INTR_ENABLE_BIT);
    usbDeviceDisconnect();
}

static void enable_vusb(void)
{
    USB_INTR_ENABLE |= (1 << USB_INTR_ENABLE_BIT);
    usbDeviceConnect();
}

static void init_vusb(void)
{
    uint8_t i = 0;

    usbInit();
    disable_vusb();
    /* fake USB disconnect for > 250 ms */
    while(--i){
        _delay_ms(1);
    }
    enable_vusb();
}
#endif

void change_driver(host_driver_t *driver)
{
    /*
    host_clear_keyboard_report();
    host_swap_keyboard_report();
    host_clear_keyboard_report();
    host_send_keyboard_report();
    */
    clear_keyboard();
    _delay_ms(1000);
    host_set_driver(driver);
}


static bool sleeping = false;
static bool insomniac = false;   // TODO: should be false for power saving
static uint16_t last_timer = 0;

int main(void)
{
    MCUSR = 0;
    clock_prescale_set(clock_div_1);
    WD_SET(WD_OFF);

    // power saving: the result is worse than nothing... why?
    //pullup_pins();
    //set_prr();

#ifdef PROTOCOL_VUSB
    disable_vusb();
#endif
    uart_init(115200);
    keyboard_init();
    print("\nSend BREAK for UART Console Commands.\n");

    // TODO: move to iWRAP/suart file
    print("suart init\n");
    // suart init
    // PC4: Tx Output IDLE(Hi)
    PORTC |= (1<<4);
    DDRC  |= (1<<4);
    // PC5: Rx Input(pull-up)
    PORTC |= (1<<5);
    DDRC  &= ~(1<<5);
    // suart receive interrut(PC5/PCINT13)
    PCMSK1 = 0b00100000;
    PCICR  = 0b00000010;

    host_set_driver(iwrap_driver());

    print("iwrap_init()\n");
    iwrap_init();
    iwrap_call();

    last_timer = timer_read();
    while (true) {
#ifdef PROTOCOL_VUSB
        if (host_get_driver() == vusb_driver())
            usbPoll();
#endif
        keyboard_task();
#ifdef PROTOCOL_VUSB
        if (host_get_driver() == vusb_driver())
            vusb_transfer_keyboard();
#endif
        // TODO: depricated
        if (matrix_is_modified() || console()) {
            last_timer = timer_read();
            sleeping = false;
        } else if (!sleeping && timer_elapsed(last_timer) > 4000) {
            sleeping = true;
            iwrap_check_connection();
        }

        // TODO: suspend.h
        if (host_get_driver() == iwrap_driver()) {
            if (sleeping && !insomniac) {
                _delay_ms(1);   // wait for UART to send
                iwrap_sleep();
                sleep(WDTO_60MS);
            }
        }
    }
}

static void sleep(uint8_t term)
{
    WD_SET(WD_IRQ, term);

    cli();
    set_sleep_mode(SLEEP_MODE_PWR_DOWN);
    sleep_enable();
    sleep_bod_disable();
    sei();
    sleep_cpu();
    sleep_disable();

    WD_SET(WD_OFF);
}

static bool console(void)
{
        // Send to Bluetoot module WT12
        static bool breaked = false;
        if (!uart_available())
            return false;
        else {
            uint8_t c;
            c = uart_getchar();
            uart_putchar(c);
            switch (c) {
                case 0x00: // BREAK signal
                    if (!breaked) {
                        print("break(? for help): ");
                        breaked = true;
                    }
                    break;
                case '\r':
                    uart_putchar('\n');
                    iwrap_buf_send();
                    break;
                case '\b':
                    iwrap_buf_del();
                    break;
                default:
                    if (breaked) {
                        print("\n");
                        console_command(c);
                        breaked = false;
                    } else {
                        iwrap_buf_add(c);
                    }
                    break;
            }
            return true;
        }
}

bool command_extra(uint8_t code)
{
    return console_command(key2asc(code));
}

static bool console_command(uint8_t c)
{
    switch (c) {
        case 'h':
        case '?':
            print("\nCommands for Bluetooth(WT12/iWRAP):\n");
            print("r: reset. software reset by watchdog\n");
            print("i: insomniac. prevent KB from sleeping\n");
            print("c: iwrap_call. CALL for BT connection.\n");
#ifdef PROTOCOL_VUSB
            print("u: USB mode. switch to USB.\n");
            print("w: BT mode. switch to Bluetooth.\n");
#endif
            print("k: kill first connection.\n");
            print("Del: unpair first pairing.\n");
            print("\n");
            return 0;
        case 'r':
            print("reset\n");
            WD_AVR_RESET();
            return 1;
        case 'i':
            insomniac = !insomniac;
            if (insomniac)
                print("insomniac\n");
            else
                print("not insomniac\n");
            return 1;
        case 'c':
            print("iwrap_call()\n");
            iwrap_call();
            return 1;
#ifdef PROTOCOL_VUSB
        case 'u':
            print("USB mode\n");
            init_vusb();
            change_driver(vusb_driver());
            //iwrap_kill();
            //iwrap_sleep();
            // disable suart receive interrut(PC5/PCINT13)
            PCMSK1 &= ~(0b00100000);
            PCICR  &= ~(0b00000010);
            return 1;
        case 'w':
            print("iWRAP mode\n");
            change_driver(iwrap_driver());
            disable_vusb();
            // enable suart receive interrut(PC5/PCINT13)
            PCMSK1 |= 0b00100000;
            PCICR  |= 0b00000010;
            return 1;
#endif
        case 'k':
            print("kill\n");
            iwrap_kill();
            return 1;
        case 0x7F:  // DELETE
            print("unpair\n");
            iwrap_unpair();
            return 1;
    }
    return 0;
}

// convert keycode into ascii charactor
static uint8_t key2asc(uint8_t key)
{
    switch (key) {
        case KC_A: return 'a';
        case KC_B: return 'b';
        case KC_C: return 'c';
        case KC_D: return 'd';
        case KC_E: return 'e';
        case KC_F: return 'f';
        case KC_G: return 'g';
        case KC_H: return 'h';
        case KC_I: return 'i';
        case KC_J: return 'j';
        case KC_K: return 'k';
        case KC_L: return 'l';
        case KC_M: return 'm';
        case KC_N: return 'n';
        case KC_O: return 'o';
        case KC_P: return 'p';
        case KC_Q: return 'q';
        case KC_R: return 'r';
        case KC_S: return 's';
        case KC_T: return 't';
        case KC_U: return 'u';
        case KC_V: return 'v';
        case KC_W: return 'w';
        case KC_X: return 'x';
        case KC_Y: return 'y';
        case KC_Z: return 'z';
        case KC_1: return '1';
        case KC_2: return '2';
        case KC_3: return '3';
        case KC_4: return '4';
        case KC_5: return '5';
        case KC_6: return '6';
        case KC_7: return '7';
        case KC_8: return '8';
        case KC_9: return '9';
        case KC_0: return '0';
        case KC_ENTER: return '\n';
        case KC_ESCAPE: return 0x1B;
        case KC_BSPACE: return '\b';
        case KC_TAB: return '\t';
        case KC_SPACE: return ' ';
        case KC_MINUS: return '-';
        case KC_EQUAL: return '=';
        case KC_LBRACKET: return '[';
        case KC_RBRACKET: return ']';
        case KC_BSLASH: return '\\';
        case KC_NONUS_HASH: return '\\';
        case KC_SCOLON: return ';';
        case KC_QUOTE: return '\'';
        case KC_GRAVE: return '`';
        case KC_COMMA: return ',';
        case KC_DOT: return '.';
        case KC_SLASH: return '/';
        default: return 0x00;
    }
}
cessor error", "alignment check", "machine check", "simd error" }; watchdog_on = 0; show_registers(regs); if ( trapnr == TRAP_page_fault ) { __asm__ __volatile__ ("mov %%cr2,%0" : "=r" (cr2) : ); printk("Faulting linear address might be %0lx %lx\n", cr2, cr2); } printk("************************************\n"); printk("CPU%d FATAL TRAP %d (%s), ERROR_CODE %04x%s.\n", cpu, trapnr, trapstr[trapnr], regs->error_code, (regs->eflags & X86_EFLAGS_IF) ? "" : ", IN INTERRUPT CONTEXT"); printk("System shutting down -- need manual reset.\n"); printk("************************************\n"); (void)debugger_trap_fatal(trapnr, regs); /* Lock up the console to prevent spurious output from other CPUs. */ console_force_lock(); /* Wait for manual reset. */ for ( ; ; ) __asm__ __volatile__ ( "hlt" ); } static inline int do_trap(int trapnr, char *str, struct xen_regs *regs, int use_error_code) { struct exec_domain *ed = current; struct trap_bounce *tb = &ed->arch.trap_bounce; trap_info_t *ti; unsigned long fixup; DEBUGGER_trap_entry(trapnr, regs); if ( !GUEST_MODE(regs) ) goto xen_fault; #ifndef NDEBUG if ( (ed->arch.traps[trapnr].address == 0) && (ed->domain->id == 0) ) goto xen_fault; #endif ti = current->arch.traps + trapnr; tb->flags = TBF_EXCEPTION; tb->cs = ti->cs; tb->eip = ti->address; if ( use_error_code ) { tb->flags |= TBF_EXCEPTION_ERRCODE; tb->error_code = regs->error_code; } if ( TI_GET_IF(ti) ) ed->vcpu_info->evtchn_upcall_mask = 1; return 0; xen_fault: if ( likely((fixup = search_exception_table(regs->eip)) != 0) ) { DPRINTK("Trap %d: %p -> %p\n", trapnr, regs->eip, fixup); regs->eip = fixup; return 0; } DEBUGGER_trap_fatal(trapnr, regs); show_registers(regs); panic("CPU%d FATAL TRAP: vector = %d (%s)\n" "[error_code=%04x]\n", smp_processor_id(), trapnr, str, regs->error_code); return 0; } #define DO_ERROR_NOCODE(trapnr, str, name) \ asmlinkage int do_##name(struct xen_regs *regs) \ { \ return do_trap(trapnr, str, regs, 0); \ } #define DO_ERROR(trapnr, str, name) \ asmlinkage int do_##name(struct xen_regs *regs) \ { \ return do_trap(trapnr, str, regs, 1); \ } DO_ERROR_NOCODE( 0, "divide error", divide_error) DO_ERROR_NOCODE( 4, "overflow", overflow) DO_ERROR_NOCODE( 5, "bounds", bounds) DO_ERROR_NOCODE( 6, "invalid operand", invalid_op) DO_ERROR_NOCODE( 9, "coprocessor segment overrun", coprocessor_segment_overrun) DO_ERROR(10, "invalid TSS", invalid_TSS) DO_ERROR(11, "segment not present", segment_not_present) DO_ERROR(12, "stack segment", stack_segment) DO_ERROR_NOCODE(16, "fpu error", coprocessor_error) DO_ERROR(17, "alignment check", alignment_check) DO_ERROR_NOCODE(19, "simd error", simd_coprocessor_error) asmlinkage int do_int3(struct xen_regs *regs) { struct exec_domain *ed = current; struct trap_bounce *tb = &ed->arch.trap_bounce; trap_info_t *ti; DEBUGGER_trap_entry(TRAP_int3, regs); if ( !GUEST_MODE(regs) ) { DEBUGGER_trap_fatal(TRAP_int3, regs); show_registers(regs); panic("CPU%d FATAL TRAP: vector = 3 (Int3)\n", smp_processor_id()); } ti = current->arch.traps + 3; tb->flags = TBF_EXCEPTION; tb->cs = ti->cs; tb->eip = ti->address; if ( TI_GET_IF(ti) ) ed->vcpu_info->evtchn_upcall_mask = 1; return 0; } asmlinkage void do_machine_check(struct xen_regs *regs) { fatal_trap(TRAP_machine_check, regs); } void propagate_page_fault(unsigned long addr, u16 error_code) { trap_info_t *ti; struct exec_domain *ed = current; struct trap_bounce *tb = &ed->arch.trap_bounce; ti = ed->arch.traps + 14; tb->flags = TBF_EXCEPTION | TBF_EXCEPTION_ERRCODE | TBF_EXCEPTION_CR2; tb->cr2 = addr; tb->error_code = error_code; tb->cs = ti->cs; tb->eip = ti->address; if ( TI_GET_IF(ti) ) ed->vcpu_info->evtchn_upcall_mask = 1; ed->arch.guest_cr2 = addr; } asmlinkage int do_page_fault(struct xen_regs *regs) { unsigned long off, addr, fixup; struct exec_domain *ed = current; struct domain *d = ed->domain; extern int map_ldt_shadow_page(unsigned int); int cpu = ed->processor; int ret; __asm__ __volatile__ ("mov %%cr2,%0" : "=r" (addr) : ); DEBUGGER_trap_entry(TRAP_page_fault, regs); perfc_incrc(page_faults); if ( likely(VM_ASSIST(d, VMASST_TYPE_writable_pagetables)) ) { LOCK_BIGLOCK(d); if ( unlikely(ptwr_info[cpu].ptinfo[PTWR_PT_ACTIVE].l1va) && unlikely((addr >> L2_PAGETABLE_SHIFT) == ptwr_info[cpu].ptinfo[PTWR_PT_ACTIVE].l2_idx) ) { ptwr_flush(PTWR_PT_ACTIVE); UNLOCK_BIGLOCK(d); return EXCRET_fault_fixed; } if ( (addr < PAGE_OFFSET) && ((regs->error_code & 3) == 3) && /* write-protection fault */ ptwr_do_page_fault(addr) ) { if ( unlikely(shadow_mode_enabled(d)) ) (void)shadow_fault(addr, regs); UNLOCK_BIGLOCK(d); return EXCRET_fault_fixed; } UNLOCK_BIGLOCK(d); } if ( unlikely(shadow_mode_enabled(d)) && (addr < PAGE_OFFSET) && shadow_fault(addr, regs) ) return EXCRET_fault_fixed; if ( unlikely(addr >= LDT_VIRT_START(ed)) && (addr < (LDT_VIRT_START(ed) + (ed->arch.ldt_ents*LDT_ENTRY_SIZE))) ) { /* * Copy a mapping from the guest's LDT, if it is valid. Otherwise we * send the fault up to the guest OS to be handled. */ LOCK_BIGLOCK(d); off = addr - LDT_VIRT_START(ed); addr = ed->arch.ldt_base + off; ret = map_ldt_shadow_page(off >> PAGE_SHIFT); UNLOCK_BIGLOCK(d); if ( likely(ret) ) return EXCRET_fault_fixed; /* successfully copied the mapping */ } if ( !GUEST_MODE(regs) ) goto xen_fault; #ifndef NDEBUG if ( (ed->arch.traps[TRAP_page_fault].address == 0) && (d->id == 0) ) goto xen_fault; #endif propagate_page_fault(addr, regs->error_code); return 0; xen_fault: if ( likely((fixup = search_exception_table(regs->eip)) != 0) ) { perfc_incrc(copy_user_faults); if ( !shadow_mode_enabled(d) ) DPRINTK("Page fault: %p -> %p\n", regs->eip, fixup); regs->eip = fixup; return 0; } DEBUGGER_trap_fatal(TRAP_page_fault, regs); show_registers(regs); show_page_walk(addr); panic("CPU%d FATAL PAGE FAULT\n" "[error_code=%04x]\n" "Faulting linear address might be %p\n", smp_processor_id(), regs->error_code, addr); return 0; } long do_fpu_taskswitch(int set) { struct exec_domain *ed = current; if ( set ) { set_bit(EDF_GUEST_STTS, &ed->ed_flags); stts(); } else { clear_bit(EDF_GUEST_STTS, &ed->ed_flags); if ( test_bit(EDF_USEDFPU, &ed->ed_flags) ) clts(); } return 0; } static inline int user_io_okay( unsigned int port, unsigned int bytes, struct exec_domain *ed, struct xen_regs *regs) { if ( ed->arch.iopl < (KERNEL_MODE(ed, regs) ? 1 : 3) ) return 0; return 1; } #define insn_fetch(_type, _size, _ptr) \ ({ unsigned long _x; \ if ( get_user(_x, (_type *)eip) ) \ goto read_fault; \ eip += _size; (_type)_x; }) static int emulate_privileged_op(struct xen_regs *regs) { struct exec_domain *ed = current; unsigned long *reg, eip = regs->eip; u8 opcode, modrm_reg = 0, rep_prefix = 0; unsigned int port, i, op_bytes = 4, data; /* Legacy prefixes. */ for ( i = 0; i < 8; i++ ) { switch ( opcode = insn_fetch(u8, 1, eip) ) { case 0x66: /* operand-size override */ op_bytes ^= 6; /* switch between 2/4 bytes */ break; case 0x67: /* address-size override */ case 0x2e: /* CS override */ case 0x3e: /* DS override */ case 0x26: /* ES override */ case 0x64: /* FS override */ case 0x65: /* GS override */ case 0x36: /* SS override */ case 0xf0: /* LOCK */ case 0xf2: /* REPNE/REPNZ */ break; case 0xf3: /* REP/REPE/REPZ */ rep_prefix = 1; break; default: goto done_prefixes; } } done_prefixes: #ifdef __x86_64__ /* REX prefix. */ if ( (opcode & 0xf0) == 0x40 ) { modrm_reg = (opcode & 4) << 1; /* REX.R */ /* REX.W, REX.B and REX.X do not need to be decoded. */ opcode = insn_fetch(u8, 1, eip); } #endif /* Input/Output String instructions. */ if ( (opcode >= 0x6c) && (opcode <= 0x6f) ) { if ( rep_prefix && (regs->ecx == 0) ) goto done; continue_io_string: switch ( opcode ) { case 0x6c: /* INSB */ op_bytes = 1; case 0x6d: /* INSW/INSL */ if ( !user_io_okay((u16)regs->edx, op_bytes, ed, regs) ) goto fail; switch ( op_bytes ) { case 1: data = (u8)inb((u16)regs->edx); if ( put_user((u8)data, (u8 *)regs->edi) ) goto write_fault; break; case 2: data = (u16)inw((u16)regs->edx); if ( put_user((u16)data, (u16 *)regs->edi) ) goto write_fault; break; case 4: data = (u32)inl((u16)regs->edx); if ( put_user((u32)data, (u32 *)regs->edi) ) goto write_fault; break; } regs->edi += (regs->eflags & EF_DF) ? -op_bytes : op_bytes; break; case 0x6e: /* OUTSB */ op_bytes = 1; case 0x6f: /* OUTSW/OUTSL */ if ( !user_io_okay((u16)regs->edx, op_bytes, ed, regs) ) goto fail; switch ( op_bytes ) { case 1: if ( get_user(data, (u8 *)regs->esi) ) goto read_fault; outb((u8)data, (u16)regs->edx); break; case 2: if ( get_user(data, (u16 *)regs->esi) ) goto read_fault; outw((u16)data, (u16)regs->edx); break; case 4: if ( get_user(data, (u32 *)regs->esi) ) goto read_fault; outl((u32)data, (u16)regs->edx); break; } regs->esi += (regs->eflags & EF_DF) ? -op_bytes : op_bytes; break; } if ( rep_prefix && (--regs->ecx != 0) ) { if ( !hypercall_preempt_check() ) goto continue_io_string; eip = regs->eip; } goto done; } /* I/O Port and Interrupt Flag instructions. */ switch ( opcode ) { case 0xe4: /* IN imm8,%al */ op_bytes = 1; case 0xe5: /* IN imm8,%eax */ port = insn_fetch(u8, 1, eip); exec_in: if ( !user_io_okay(port, op_bytes, ed, regs) ) goto fail; switch ( op_bytes ) { case 1: regs->eax &= ~0xffUL; regs->eax |= (u8)inb(port); break; case 2: regs->eax &= ~0xffffUL; regs->eax |= (u16)inw(port); break; case 4: regs->eax = (u32)inl(port); break; } goto done; case 0xec: /* IN %dx,%al */ op_bytes = 1; case 0xed: /* IN %dx,%eax */ port = (u16)regs->edx; goto exec_in; case 0xe6: /* OUT %al,imm8 */ op_bytes = 1; case 0xe7: /* OUT %eax,imm8 */ port = insn_fetch(u8, 1, eip); exec_out: if ( !user_io_okay(port, op_bytes, ed, regs) ) goto fail; switch ( op_bytes ) { case 1: outb((u8)regs->eax, port); break; case 2: outw((u16)regs->eax, port); break; case 4: outl((u32)regs->eax, port); break; } goto done; case 0xee: /* OUT %al,%dx */ op_bytes = 1; case 0xef: /* OUT %eax,%dx */ port = (u16)regs->edx; goto exec_out; case 0xfa: /* CLI */ case 0xfb: /* STI */ if ( ed->arch.iopl < (KERNEL_MODE(ed, regs) ? 1 : 3) ) goto fail; /* * This is just too dangerous to allow, in my opinion. Consider if the * caller then tries to reenable interrupts using POPF: we can't trap * that and we'll end up with hard-to-debug lockups. Fast & loose will * do for us. :-) */ /*ed->vcpu_info->evtchn_upcall_mask = (opcode == 0xfa);*/ goto done; case 0x0f: /* Two-byte opcode */ break; default: goto fail; } /* Remaining instructions only emulated from guest kernel. */ if ( !KERNEL_MODE(ed, regs) ) goto fail; /* Privileged (ring 0) instructions. */ opcode = insn_fetch(u8, 1, eip); switch ( opcode ) { case 0x06: /* CLTS */ (void)do_fpu_taskswitch(0); break; case 0x09: /* WBINVD */ /* Ignore the instruction if unprivileged. */ if ( !IS_CAPABLE_PHYSDEV(ed->domain) ) DPRINTK("Non-physdev domain attempted WBINVD.\n"); else wbinvd(); break; case 0x20: /* MOV CR?,<reg> */ opcode = insn_fetch(u8, 1, eip); if ( (opcode & 0xc0) != 0xc0 ) goto fail; modrm_reg |= opcode & 7; reg = decode_register(modrm_reg, regs, 0); switch ( (opcode >> 3) & 7 ) { case 0: /* Read CR0 */ *reg = (read_cr0() & ~X86_CR0_TS) | (test_bit(EDF_GUEST_STTS, &ed->ed_flags) ? X86_CR0_TS : 0); break; case 2: /* Read CR2 */ *reg = ed->arch.guest_cr2; break; case 3: /* Read CR3 */ *reg = pagetable_val(ed->arch.guest_table); break; default: goto fail; } break; case 0x22: /* MOV <reg>,CR? */ opcode = insn_fetch(u8, 1, eip); if ( (opcode & 0xc0) != 0xc0 ) goto fail; modrm_reg |= opcode & 7; reg = decode_register(modrm_reg, regs, 0); switch ( (opcode >> 3) & 7 ) { case 0: /* Write CR0 */ (void)do_fpu_taskswitch(!!(*reg & X86_CR0_TS)); break; case 2: /* Write CR2 */ ed->arch.guest_cr2 = *reg; break; case 3: /* Write CR3 */ LOCK_BIGLOCK(ed->domain); (void)new_guest_cr3(*reg); UNLOCK_BIGLOCK(ed->domain); break; default: goto fail; } break; case 0x30: /* WRMSR */ /* Ignore the instruction if unprivileged. */ if ( !IS_PRIV(ed->domain) ) DPRINTK("Non-priv domain attempted WRMSR(%p,%08lx,%08lx).\n", regs->ecx, (long)regs->eax, (long)regs->edx); else if ( wrmsr_user(regs->ecx, regs->eax, regs->edx) ) goto fail; break; case 0x32: /* RDMSR */ if ( !IS_PRIV(ed->domain) ) DPRINTK("Non-priv domain attempted RDMSR(%p,%08lx,%08lx).\n", regs->ecx, (long)regs->eax, (long)regs->edx); /* Everyone can read the MSR space. */ if ( rdmsr_user(regs->ecx, regs->eax, regs->edx) ) goto fail; break; default: goto fail; } done: regs->eip = eip; return EXCRET_fault_fixed; fail: return 0; read_fault: propagate_page_fault(eip, 4); /* user mode, read fault */ return EXCRET_fault_fixed; write_fault: propagate_page_fault(eip, 6); /* user mode, write fault */ return EXCRET_fault_fixed; } asmlinkage int do_general_protection(struct xen_regs *regs) { struct exec_domain *ed = current; struct trap_bounce *tb = &ed->arch.trap_bounce; trap_info_t *ti; unsigned long fixup; DEBUGGER_trap_entry(TRAP_gp_fault, regs); if ( regs->error_code & 1 ) goto hardware_gp; if ( !GUEST_MODE(regs) ) goto gp_in_kernel; /* * Cunning trick to allow arbitrary "INT n" handling. * * We set DPL == 0 on all vectors in the IDT. This prevents any INT <n> * instruction from trapping to the appropriate vector, when that might not * be expected by Xen or the guest OS. For example, that entry might be for * a fault handler (unlike traps, faults don't increment EIP), or might * expect an error code on the stack (which a software trap never * provides), or might be a hardware interrupt handler that doesn't like * being called spuriously. * * Instead, a GPF occurs with the faulting IDT vector in the error code. * Bit 1 is set to indicate that an IDT entry caused the fault. Bit 0 is * clear to indicate that it's a software fault, not hardware. * * NOTE: Vectors 3 and 4 are dealt with from their own handler. This is * okay because they can only be triggered by an explicit DPL-checked * instruction. The DPL specified by the guest OS for these vectors is NOT * CHECKED!! */ if ( (regs->error_code & 3) == 2 ) { /* This fault must be due to <INT n> instruction. */ ti = current->arch.traps + (regs->error_code>>3); if ( PERMIT_SOFTINT(TI_GET_DPL(ti), ed, regs) ) { tb->flags = TBF_EXCEPTION; regs->eip += 2; goto finish_propagation; } } /* Emulate some simple privileged and I/O instructions. */ if ( (regs->error_code == 0) && emulate_privileged_op(regs) ) return 0; #if defined(__i386__) if ( VM_ASSIST(ed->domain, VMASST_TYPE_4gb_segments) && (regs->error_code == 0) && gpf_emulate_4gb(regs) ) return 0; #endif #ifndef NDEBUG if ( (ed->arch.traps[TRAP_gp_fault].address == 0) && (ed->domain->id == 0) ) goto gp_in_kernel; #endif /* Pass on GPF as is. */ ti = current->arch.traps + 13; tb->flags = TBF_EXCEPTION | TBF_EXCEPTION_ERRCODE; tb->error_code = regs->error_code; finish_propagation: tb->cs = ti->cs; tb->eip = ti->address; if ( TI_GET_IF(ti) ) ed->vcpu_info->evtchn_upcall_mask = 1; return 0; gp_in_kernel: if ( likely((fixup = search_exception_table(regs->eip)) != 0) ) { DPRINTK("GPF (%04x): %p -> %p\n", regs->error_code, regs->eip, fixup); regs->eip = fixup; return 0; } DEBUGGER_trap_fatal(TRAP_gp_fault, regs); hardware_gp: show_registers(regs); panic("CPU%d GENERAL PROTECTION FAULT\n[error_code=%04x]\n", smp_processor_id(), regs->error_code); return 0; } unsigned long nmi_softirq_reason; static void nmi_softirq(void) { if ( dom0 == NULL ) return; if ( test_and_clear_bit(0, &nmi_softirq_reason) ) send_guest_virq(dom0->exec_domain[0], VIRQ_PARITY_ERR); if ( test_and_clear_bit(1, &nmi_softirq_reason) ) send_guest_virq(dom0->exec_domain[0], VIRQ_IO_ERR); } asmlinkage void mem_parity_error(struct xen_regs *regs) { /* Clear and disable the parity-error line. */ outb((inb(0x61)&15)|4,0x61); switch ( opt_nmi[0] ) { case 'd': /* 'dom0' */ set_bit(0, &nmi_softirq_reason); raise_softirq(NMI_SOFTIRQ); case 'i': /* 'ignore' */ break; default: /* 'fatal' */ console_force_unlock(); printk("\n\nNMI - MEMORY ERROR\n"); fatal_trap(TRAP_nmi, regs); } } asmlinkage void io_check_error(struct xen_regs *regs) { /* Clear and disable the I/O-error line. */ outb((inb(0x61)&15)|8,0x61); switch ( opt_nmi[0] ) { case 'd': /* 'dom0' */ set_bit(0, &nmi_softirq_reason); raise_softirq(NMI_SOFTIRQ); case 'i': /* 'ignore' */ break; default: /* 'fatal' */ console_force_unlock(); printk("\n\nNMI - I/O ERROR\n"); fatal_trap(TRAP_nmi, regs); } } static void unknown_nmi_error(unsigned char reason) { printk("Uhhuh. NMI received for unknown reason %02x.\n", reason); printk("Dazed and confused, but trying to continue\n"); printk("Do you have a strange power saving mode enabled?\n"); } asmlinkage void do_nmi(struct xen_regs *regs, unsigned long reason) { ++nmi_count(smp_processor_id()); if ( nmi_watchdog ) nmi_watchdog_tick(regs); if ( reason & 0x80 ) mem_parity_error(regs); else if ( reason & 0x40 ) io_check_error(regs); else if ( !nmi_watchdog ) unknown_nmi_error((unsigned char)(reason&0xff)); } asmlinkage int math_state_restore(struct xen_regs *regs) { /* Prevent recursion. */ clts(); if ( !test_bit(EDF_USEDFPU, &current->ed_flags) ) { if ( test_bit(EDF_DONEFPUINIT, &current->ed_flags) ) restore_fpu(current); else init_fpu(); set_bit(EDF_USEDFPU, &current->ed_flags); /* so we fnsave on switch_to() */ } if ( test_and_clear_bit(EDF_GUEST_STTS, &current->ed_flags) ) { struct trap_bounce *tb = &current->arch.trap_bounce; tb->flags = TBF_EXCEPTION; tb->cs = current->arch.traps[7].cs; tb->eip = current->arch.traps[7].address; } return EXCRET_fault_fixed; } asmlinkage int do_debug(struct xen_regs *regs) { unsigned long condition; struct exec_domain *d = current; struct trap_bounce *tb = &d->arch.trap_bounce; DEBUGGER_trap_entry(TRAP_debug, regs); __asm__ __volatile__("mov %%db6,%0" : "=r" (condition)); /* Mask out spurious debug traps due to lazy DR7 setting */ if ( (condition & (DR_TRAP0|DR_TRAP1|DR_TRAP2|DR_TRAP3)) && (d->arch.debugreg[7] == 0) ) { __asm__("mov %0,%%db7" : : "r" (0UL)); goto out; } if ( !GUEST_MODE(regs) ) { /* Clear TF just for absolute sanity. */ regs->eflags &= ~EF_TF; /* * We ignore watchpoints when they trigger within Xen. This may happen * when a buffer is passed to us which previously had a watchpoint set * on it. No need to bump EIP; the only faulting trap is an instruction * breakpoint, which can't happen to us. */ goto out; } /* Save debug status register where guest OS can peek at it */ d->arch.debugreg[6] = condition; tb->flags = TBF_EXCEPTION; tb->cs = d->arch.traps[1].cs; tb->eip = d->arch.traps[1].address; out: return EXCRET_not_a_fault; } asmlinkage int do_spurious_interrupt_bug(struct xen_regs *regs) { return EXCRET_not_a_fault; } void set_intr_gate(unsigned int n, void *addr) { #ifdef __i386__ int i; /* Keep secondary tables in sync with IRQ updates. */ for ( i = 1; i < NR_CPUS; i++ ) if ( idt_tables[i] != NULL ) _set_gate(&idt_tables[i][n], 14, 0, addr); #endif _set_gate(&idt_table[n], 14, 0, addr); } void set_system_gate(unsigned int n, void *addr) { _set_gate(idt_table+n,14,3,addr); } void set_task_gate(unsigned int n, unsigned int sel) { idt_table[n].a = sel << 16; idt_table[n].b = 0x8500; } void set_tss_desc(unsigned int n, void *addr) { _set_tssldt_desc( gdt_table + __TSS(n), (unsigned long)addr, offsetof(struct tss_struct, __cacheline_filler) - 1, 9); } void __init trap_init(void) { extern void percpu_traps_init(void); extern void cpu_init(void); /* * Note that interrupt gates are always used, rather than trap gates. We * must have interrupts disabled until DS/ES/FS/GS are saved because the * first activation must have the "bad" value(s) for these registers and * we may lose them if another activation is installed before they are * saved. The page-fault handler also needs interrupts disabled until %cr2 * has been read and saved on the stack. */ set_intr_gate(TRAP_divide_error,&divide_error); set_intr_gate(TRAP_debug,&debug); set_intr_gate(TRAP_nmi,&nmi); set_system_gate(TRAP_int3,&int3); /* usable from all privileges */ set_system_gate(TRAP_overflow,&overflow); /* usable from all privileges */ set_intr_gate(TRAP_bounds,&bounds); set_intr_gate(TRAP_invalid_op,&invalid_op); set_intr_gate(TRAP_no_device,&device_not_available); set_intr_gate(TRAP_copro_seg,&coprocessor_segment_overrun); set_intr_gate(TRAP_invalid_tss,&invalid_TSS); set_intr_gate(TRAP_no_segment,&segment_not_present); set_intr_gate(TRAP_stack_error,&stack_segment); set_intr_gate(TRAP_gp_fault,&general_protection); set_intr_gate(TRAP_page_fault,&page_fault); set_intr_gate(TRAP_spurious_int,&spurious_interrupt_bug); set_intr_gate(TRAP_copro_error,&coprocessor_error); set_intr_gate(TRAP_alignment_check,&alignment_check); set_intr_gate(TRAP_machine_check,&machine_check); set_intr_gate(TRAP_simd_error,&simd_coprocessor_error); percpu_traps_init(); cpu_init(); open_softirq(NMI_SOFTIRQ, nmi_softirq); } long do_set_trap_table(trap_info_t *traps) { trap_info_t cur; trap_info_t *dst = current->arch.traps; long rc = 0; LOCK_BIGLOCK(current->domain); for ( ; ; ) { if ( hypercall_preempt_check() ) { rc = hypercall1_create_continuation( __HYPERVISOR_set_trap_table, traps); break; } if ( copy_from_user(&cur, traps, sizeof(cur)) ) { rc = -EFAULT; break; } if ( cur.address == 0 ) break; if ( !VALID_CODESEL(cur.cs) ) { rc = -EPERM; break; } memcpy(dst+cur.vector, &cur, sizeof(cur)); traps++; } UNLOCK_BIGLOCK(current->domain); return rc; } #if defined(__i386__) #define DB_VALID_ADDR(_a) \ ((_a) <= (PAGE_OFFSET - 4)) #elif defined(__x86_64__) #define DB_VALID_ADDR(_a) \ ((_a) >= HYPERVISOR_VIRT_END) || ((_a) <= (HYPERVISOR_VIRT_START-8)) #endif long set_debugreg(struct exec_domain *p, int reg, unsigned long value) { int i; switch ( reg ) { case 0: if ( !DB_VALID_ADDR(value) ) return -EPERM; if ( p == current ) __asm__ ( "mov %0, %%db0" : : "r" (value) ); break; case 1: if ( !DB_VALID_ADDR(value) ) return -EPERM; if ( p == current ) __asm__ ( "mov %0, %%db1" : : "r" (value) ); break; case 2: if ( !DB_VALID_ADDR(value) ) return -EPERM; if ( p == current ) __asm__ ( "mov %0, %%db2" : : "r" (value) ); break; case 3: if ( !DB_VALID_ADDR(value) ) return -EPERM; if ( p == current ) __asm__ ( "mov %0, %%db3" : : "r" (value) ); break; case 6: /* * DR6: Bits 4-11,16-31 reserved (set to 1). * Bit 12 reserved (set to 0). */ value &= 0xffffefff; /* reserved bits => 0 */ value |= 0xffff0ff0; /* reserved bits => 1 */ if ( p == current ) __asm__ ( "mov %0, %%db6" : : "r" (value) ); break; case 7: /* * DR7: Bit 10 reserved (set to 1). * Bits 11-12,14-15 reserved (set to 0). * Privileged bits: * GD (bit 13): must be 0. * R/Wn (bits 16-17,20-21,24-25,28-29): mustn't be 10. * LENn (bits 18-19,22-23,26-27,30-31): mustn't be 10. */ /* DR7 == 0 => debugging disabled for this domain. */ if ( value != 0 ) { value &= 0xffff27ff; /* reserved bits => 0 */ value |= 0x00000400; /* reserved bits => 1 */ if ( (value & (1<<13)) != 0 ) return -EPERM; for ( i = 0; i < 16; i += 2 ) if ( ((value >> (i+16)) & 3) == 2 ) return -EPERM; } if ( p == current ) __asm__ ( "mov %0, %%db7" : : "r" (value) ); break; default: return -EINVAL; } p->arch.debugreg[reg] = value; return 0; } long do_set_debugreg(int reg, unsigned long value) { return set_debugreg(current, reg, value); } unsigned long do_get_debugreg(int reg) { if ( (reg < 0) || (reg > 7) ) return -EINVAL; return current->arch.debugreg[reg]; } /* * Local variables: * mode: C * c-set-style: "BSD" * c-basic-offset: 4 * tab-width: 4 * indent-tabs-mode: nil * End: */