Kevin O'Connor | 9521e26 | 2008-07-04 13:04:29 -0400 | [diff] [blame] | 1 | // Misc utility functions. |
| 2 | // |
| 3 | // Copyright (C) 2008 Kevin O'Connor <kevin@koconnor.net> |
| 4 | // |
| 5 | // This file may be distributed under the terms of the GNU GPLv3 license. |
| 6 | |
Kevin O'Connor | a4d3576 | 2008-03-08 15:43:03 -0500 | [diff] [blame] | 7 | #include "util.h" // usleep |
Kevin O'Connor | 9521e26 | 2008-07-04 13:04:29 -0400 | [diff] [blame] | 8 | #include "bregs.h" // struct bregs |
| 9 | #include "config.h" // SEG_BIOS |
| 10 | #include "farptr.h" // GET_FARPTR |
Kevin O'Connor | 7f34309 | 2009-01-01 18:31:11 -0500 | [diff] [blame] | 11 | #include "biosvar.h" // get_ebda_seg |
Kevin O'Connor | 9521e26 | 2008-07-04 13:04:29 -0400 | [diff] [blame] | 12 | |
| 13 | // Call a function with a specified register state. Note that on |
| 14 | // return, the interrupt enable/disable flag may be altered. |
| 15 | inline void |
| 16 | call16(struct bregs *callregs) |
| 17 | { |
| 18 | asm volatile( |
Kevin O'Connor | 7ab798f | 2008-07-13 11:08:36 -0400 | [diff] [blame] | 19 | #if MODE16 == 1 |
Kevin O'Connor | 9521e26 | 2008-07-04 13:04:29 -0400 | [diff] [blame] | 20 | "calll __call16\n" |
| 21 | #else |
| 22 | "calll __call16_from32\n" |
| 23 | #endif |
| 24 | : "+a" (callregs), "+m" (*callregs) |
| 25 | : |
Kevin O'Connor | 7f34309 | 2009-01-01 18:31:11 -0500 | [diff] [blame] | 26 | : "ebx", "ecx", "edx", "esi", "edi", "ebp", "cc", "memory"); |
Kevin O'Connor | 9521e26 | 2008-07-04 13:04:29 -0400 | [diff] [blame] | 27 | } |
| 28 | |
| 29 | inline void |
Kevin O'Connor | 6e5b4a4 | 2008-12-06 13:03:52 -0500 | [diff] [blame] | 30 | call16big(struct bregs *callregs) |
| 31 | { |
Kevin O'Connor | 7f34309 | 2009-01-01 18:31:11 -0500 | [diff] [blame] | 32 | extern void __force_link_error__call16big_only_in_32bit_mode(); |
| 33 | if (MODE16) |
| 34 | __force_link_error__call16big_only_in_32bit_mode(); |
| 35 | |
Kevin O'Connor | 6e5b4a4 | 2008-12-06 13:03:52 -0500 | [diff] [blame] | 36 | asm volatile( |
| 37 | "calll __call16big_from32\n" |
| 38 | : "+a" (callregs), "+m" (*callregs) |
| 39 | : |
Kevin O'Connor | 7f34309 | 2009-01-01 18:31:11 -0500 | [diff] [blame] | 40 | : "ebx", "ecx", "edx", "esi", "edi", "ebp", "cc", "memory"); |
Kevin O'Connor | 6e5b4a4 | 2008-12-06 13:03:52 -0500 | [diff] [blame] | 41 | } |
| 42 | |
| 43 | inline void |
Kevin O'Connor | 9521e26 | 2008-07-04 13:04:29 -0400 | [diff] [blame] | 44 | __call16_int(struct bregs *callregs, u16 offset) |
| 45 | { |
| 46 | callregs->cs = SEG_BIOS; |
| 47 | callregs->ip = offset; |
| 48 | call16(callregs); |
| 49 | } |
Kevin O'Connor | a4d3576 | 2008-03-08 15:43:03 -0500 | [diff] [blame] | 50 | |
Kevin O'Connor | a83ff55 | 2009-01-01 21:00:59 -0500 | [diff] [blame^] | 51 | inline void |
| 52 | call16_simpint(int nr, u32 *eax, u32 *flags) |
| 53 | { |
| 54 | extern void __force_link_error__call16_simpint_only_in_16bit_mode(); |
| 55 | if (!MODE16) |
| 56 | __force_link_error__call16_simpint_only_in_16bit_mode(); |
| 57 | |
| 58 | asm volatile( |
| 59 | "stc\n" |
| 60 | "int %2\n" |
| 61 | "pushfl\n" |
| 62 | "popl %1\n" |
| 63 | "cld\n" |
| 64 | "cli\n" |
| 65 | : "+a"(*eax), "=r"(*flags) |
| 66 | : "i"(nr) |
| 67 | : "cc", "memory"); |
| 68 | } |
| 69 | |
Kevin O'Connor | 7f34309 | 2009-01-01 18:31:11 -0500 | [diff] [blame] | 70 | // Switch to the extra stack in ebda and call a function. |
| 71 | inline u32 |
| 72 | stack_hop(u32 eax, u32 edx, u32 ecx, void *func) |
| 73 | { |
| 74 | extern void __force_link_error__stack_hop_only_in_16bit_mode(); |
| 75 | if (!MODE16) |
| 76 | __force_link_error__stack_hop_only_in_16bit_mode(); |
| 77 | |
| 78 | u32 ebda_seg = get_ebda_seg(); |
| 79 | u32 tmp; |
| 80 | asm volatile( |
| 81 | // Backup current %ss value. |
| 82 | "movl %%ss, %4\n" |
| 83 | // Copy ebda seg to %ss and %ds |
| 84 | "movl %3, %%ss\n" |
| 85 | "movl %3, %%ds\n" |
| 86 | // Backup %esp and set it to new value |
| 87 | "movl %%esp, %3\n" |
| 88 | "movl %5, %%esp\n" |
| 89 | // Call func |
| 90 | "calll %6\n" |
| 91 | // Restore segments and stack |
| 92 | "movl %3, %%esp\n" |
| 93 | "movl %4, %%ss\n" |
| 94 | "movl %4, %%ds\n" |
| 95 | : "+a" (eax), "+d" (edx), "+c" (ecx), "+r" (ebda_seg), "=r" (tmp) |
| 96 | : "i" (EBDA_OFFSET_TOP_STACK), "m" (*(u8*)func) |
| 97 | : "cc", "memory"); |
| 98 | return eax; |
| 99 | } |
| 100 | |
Kevin O'Connor | 2e7ab8b | 2008-03-29 14:29:35 -0400 | [diff] [blame] | 101 | // Sum the bytes in the specified area. |
| 102 | u8 |
| 103 | checksum(u8 *far_data, u32 len) |
| 104 | { |
| 105 | u32 i; |
| 106 | u8 sum = 0; |
| 107 | for (i=0; i<len; i++) |
| 108 | sum += GET_FARPTR(far_data[i]); |
| 109 | return sum; |
| 110 | } |
| 111 | |
Kevin O'Connor | 5e4235f | 2008-04-12 09:00:04 -0400 | [diff] [blame] | 112 | void * |
Kevin O'Connor | 567e4e3 | 2008-04-05 11:37:51 -0400 | [diff] [blame] | 113 | memset(void *s, int c, size_t n) |
| 114 | { |
| 115 | while (n) |
| 116 | ((char *)s)[--n] = c; |
Kevin O'Connor | 5e4235f | 2008-04-12 09:00:04 -0400 | [diff] [blame] | 117 | return s; |
Kevin O'Connor | 567e4e3 | 2008-04-05 11:37:51 -0400 | [diff] [blame] | 118 | } |
| 119 | |
| 120 | void * |
Kevin O'Connor | 18b927e | 2008-08-29 21:14:36 -0400 | [diff] [blame] | 121 | memcpy_far(void *far_d1, const void *far_s1, size_t len) |
Kevin O'Connor | 567e4e3 | 2008-04-05 11:37:51 -0400 | [diff] [blame] | 122 | { |
Kevin O'Connor | e0113c9 | 2008-04-05 15:51:12 -0400 | [diff] [blame] | 123 | u8 *d = far_d1; |
| 124 | u8 *s = (u8*)far_s1; |
Kevin O'Connor | 567e4e3 | 2008-04-05 11:37:51 -0400 | [diff] [blame] | 125 | |
Kevin O'Connor | e0113c9 | 2008-04-05 15:51:12 -0400 | [diff] [blame] | 126 | while (len--) { |
| 127 | SET_FARPTR(*d, GET_FARPTR(*s)); |
| 128 | d++; |
| 129 | s++; |
| 130 | } |
Kevin O'Connor | 567e4e3 | 2008-04-05 11:37:51 -0400 | [diff] [blame] | 131 | |
Kevin O'Connor | e0113c9 | 2008-04-05 15:51:12 -0400 | [diff] [blame] | 132 | return far_d1; |
Kevin O'Connor | 567e4e3 | 2008-04-05 11:37:51 -0400 | [diff] [blame] | 133 | } |
| 134 | |
Kevin O'Connor | c781293 | 2008-06-08 23:08:12 -0400 | [diff] [blame] | 135 | void * |
Kevin O'Connor | 18b927e | 2008-08-29 21:14:36 -0400 | [diff] [blame] | 136 | memcpy(void *d1, const void *s1, size_t len) |
| 137 | { |
| 138 | u8 *d = (u8*)d1, *s = (u8*)s1; |
| 139 | while (len--) |
| 140 | *d++ = *s++; |
| 141 | return d1; |
| 142 | } |
| 143 | |
| 144 | void * |
Kevin O'Connor | c781293 | 2008-06-08 23:08:12 -0400 | [diff] [blame] | 145 | memmove(void *d, const void *s, size_t len) |
| 146 | { |
| 147 | if (s >= d) |
| 148 | return memcpy(d, s, len); |
| 149 | |
| 150 | d += len-1; |
| 151 | s += len-1; |
| 152 | while (len--) { |
| 153 | *(char*)d = *(char*)s; |
| 154 | d--; |
| 155 | s--; |
| 156 | } |
| 157 | |
| 158 | return d; |
| 159 | } |