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
error(const char * msg)24 static inline void error(const char *msg)
25 {
26 fputs(msg, stderr);
27 }
28
reloc_ptr(struct reloc_info * ri,void * ptr)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
hijack_interrupt(int intn,uint32_t handler)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
shift_cmdline(struct com32_sys_args * com32)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
reloc_entry(struct reloc_info * ri)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
reloc(void * ptr,size_t len)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
main(int argc,char * argv[])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