1 #include <stdio.h> 2 #include <stdlib.h> 3 #include <string.h> 4 #include <console.h> 5 #include <com32.h> 6 #include <syslinux/loadfile.h> 7 #include "serial.h" 8 9 #define X86_INT_DB 1 10 #define X86_INT_BP 3 11 #define COM32_IDT ((void*)0x100000) 12 #define COM32_LOAD_ADDR ((void*)0x101000) 13 #define STACK_SIZE 0x1000 14 15 extern char _start[], _end[]; 16 17 struct reloc_info { 18 void *data; 19 size_t len; 20 uint32_t old_esp; 21 uint32_t reloc_base; 22 }; 23 24 static inline void error(const char *msg) 25 { 26 fputs(msg, stderr); 27 } 28 29 static inline uint32_t reloc_ptr(struct reloc_info *ri, void *ptr) 30 { 31 return ri->reloc_base + (uint32_t) ((char *)ptr - _start); 32 } 33 34 static void hijack_interrupt(int intn, uint32_t handler) 35 { 36 struct { 37 uint32_t lo; 38 uint32_t hi; 39 } *idt = COM32_IDT; 40 41 idt[intn].lo = (idt[intn].lo & 0xffff0000) | (handler & 0x0000ffff); 42 idt[intn].hi = (idt[intn].hi & 0x0000ffff) | (handler & 0xffff0000); 43 } 44 45 static void shift_cmdline(struct com32_sys_args *com32) 46 { 47 char *p; 48 49 /* Skip leading whitespace */ 50 for (p = com32->cs_cmdline; *p != '\0' && *p == ' '; p++) ; 51 52 /* Skip first word */ 53 for (; *p != '\0' && *p != ' '; p++) ; 54 55 /* Skip whitespace after first word */ 56 for (; *p != '\0' && *p == ' '; p++) ; 57 58 com32->cs_cmdline = p; 59 } 60 61 static __noreturn reloc_entry(struct reloc_info *ri) 62 { 63 extern char int_handler[]; 64 size_t stack_frame_size = sizeof(struct com32_sys_args) + 4; 65 struct com32_sys_args *com32; 66 uint32_t module_esp; 67 68 hijack_interrupt(X86_INT_DB, reloc_ptr(ri, int_handler)); 69 hijack_interrupt(X86_INT_BP, reloc_ptr(ri, int_handler)); 70 71 /* Copy module to load address */ 72 memcpy(COM32_LOAD_ADDR, ri->data, ri->len); 73 74 /* Copy stack frame onto module stack */ 75 module_esp = (ri->reloc_base - stack_frame_size) & ~15; 76 memcpy((void *)module_esp, (void *)ri->old_esp, stack_frame_size); 77 78 /* Fix up command line */ 79 com32 = (struct com32_sys_args *)(module_esp + 4); 80 shift_cmdline(com32); 81 82 /* Set up CPU state to run module and enter GDB */ 83 asm volatile ("movl %0, %%esp\n\t" 84 "pushf\n\t" 85 "pushl %%cs\n\t" 86 "pushl %1\n\t" 87 "jmp *%2\n\t"::"r" (module_esp), 88 "c"(COM32_LOAD_ADDR), "r"(reloc_ptr(ri, int_handler)) 89 ); 90 for (;;) ; /* shut the compiler up */ 91 } 92 93 static inline __noreturn reloc(void *ptr, size_t len) 94 { 95 extern uint32_t __entry_esp; 96 size_t total_size = _end - _start; 97 __noreturn(*entry_fn) (struct reloc_info *); 98 struct reloc_info ri; 99 uint32_t esp; 100 char *dest; 101 102 /* Calculate relocation address, preserve current stack */ 103 asm volatile ("movl %%esp, %0\n\t":"=m" (esp)); 104 dest = (char *)((esp - STACK_SIZE - total_size) & ~3); 105 106 /* Calculate entry point in relocated code */ 107 entry_fn = (void *)(dest + ((char *)reloc_entry - _start)); 108 109 /* Copy all sections to relocation address */ 110 printf("Relocating %d bytes from %p to %p\n", total_size, _start, dest); 111 memcpy(dest, _start, total_size); 112 113 /* Call into relocated code */ 114 ri.data = ptr; 115 ri.len = len; 116 ri.old_esp = __entry_esp; 117 ri.reloc_base = (uint32_t) dest; 118 entry_fn(&ri); 119 } 120 121 int main(int argc, char *argv[]) 122 { 123 void *data; 124 size_t data_len; 125 126 openconsole(&dev_null_r, &dev_stdcon_w); 127 128 if (argc < 2) { 129 error("Usage: gdbstub.c32 com32_file arguments...\n"); 130 return 1; 131 } 132 133 if (loadfile(argv[1], &data, &data_len)) { 134 error("Unable to load file\n"); 135 return 1; 136 } 137 138 serial_init(); 139 140 /* No more lib calls after this point */ 141 reloc(data, data_len); 142 } 143
