setos

拙OS
Log | Files | Refs | LICENSE

utils.c (5646B)

      1 #include <stdarg.h> // TODO: implement stdarg.h by my self
      2 
      3 #include <uefi.h>
      4 #include <uefi_utils.h>
      5 
      6 CHAR16 *
      7 sprinth(UINT64 n, CHAR16 s[19])
      8 {
      9 	int i;
     10 	int d;
     11 	s[0] = L'0';
     12 	s[1] = L'x';
     13 	for (i = 0; i < 16; i++) {
     14 		d = (n >> (64 - (i + 1) * 4)) & 0xf;
     15 		if (d <= 9)
     16 			s[i + 2] = L'0' + d;
     17 		else
     18 			s[i + 2] = L'a' + d - 10;
     19 	}
     20 	s[19] = L'\0';
     21 	return s;
     22 }
     23 
     24 char *
     25 wstr2str(CHAR16 *s16, char *s8)
     26 {
     27 	char *s = s8;
     28 	for (;*s16;) {
     29 		*s8++ = (char) (*s16++ & 0xff);
     30 	}
     31 	*s8 = '\0';
     32 	return s;
     33 }
     34 
     35 int
     36 efi_printf(char *fmt, ...)
     37 {
     38 	va_list ap;
     39 	CHAR16 _buf[1024], *buf = _buf;
     40 	EFI_STATUS stat;
     41 
     42 	char               *s;
     43 	CHAR16             *w;
     44 	long long          d, e;
     45 	unsigned long long f, g, h;
     46 
     47 	va_start(ap, fmt);
     48 	for (;*fmt;) {
     49 		if (*fmt != '%') {
     50 			if (*fmt == '\n') {
     51 				*buf++ = (CHAR16) '\n';
     52 				*buf++ = (CHAR16) '\r';
     53 				fmt++;
     54 				continue;
     55 			} else {
     56 				*buf++ = (CHAR16) *fmt++;
     57 				continue;
     58 			}
     59 		}
     60 		fmt++;
     61 		switch (*fmt++) {
     62 		case '%':
     63 			*buf++ = L'%';
     64 			break;
     65 		case 's':
     66 			s = va_arg(ap, char *);
     67 			for (;*s;) {
     68 				*buf++ = (CHAR16) *s++;
     69 			}
     70 			break;
     71 		case 'w':
     72 			w = va_arg(ap, CHAR16 *);
     73 			for (;*w;) {
     74 				*buf++ = *w++;
     75 			}
     76 			break;
     77 		case 'd':
     78 			d = va_arg(ap, int);
     79 			if (d == 0) {
     80 				*buf++ = '0';
     81 				break;
     82 			} else if (d < 0) {
     83 				*buf++ = '-';
     84 				d = -d;
     85 			}
     86 			for (e = 1; e <= d; e *= 10) {
     87 				if (e > e * 10) { // integer too big;
     88 					return -1;
     89 				}
     90 			}
     91 			for (e /= 10; e >= 1; e /= 10) {
     92 				*buf++ = (CHAR16) ('0' + ((d / e) % 10));
     93 			}
     94 			break;
     95 		case 'x':
     96 			f = va_arg(ap, unsigned int);
     97 			if (f == 0) {
     98 				*buf++ = '0';
     99 				break;
    100 			}
    101 			for (g = 1; g <= f; g *= 0x10) {
    102 				if (g > g * 0x10) { // integer too big.
    103 					return -1;
    104 				}
    105 			}
    106 			for (g /= 0x10; g >= 1; g /= 0x10) {
    107 				h = ((f / g) % 0x10);
    108 				if (h < 0xa) {
    109 					*buf++ = (CHAR16) ('0' + h);
    110 				} else {
    111 					*buf++ = (CHAR16) ('a' + h - 0xa);
    112 				}
    113 			}
    114 			break;
    115 		}
    116 	}
    117 	va_end(ap);
    118 	*buf = L'\0';
    119 
    120 	stat = SystemTable->ConOut->OutputString(SystemTable->ConOut, _buf);
    121 	if (stat != EFI_SUCCESS) {
    122 		return -1;
    123 	}
    124 	return (int) (buf - _buf);
    125 }
    126 
    127 EFI_STATUS
    128 open_root(EFI_HANDLE ImageHandle, EFI_FILE_PROTOCOL **root)
    129 {
    130 	EFI_STATUS stat;
    131 	EFI_LOADED_IMAGE_PROTOCOL *li;
    132 	EFI_SIMPLE_FILE_SYSTEM_PROTOCOL *fs;
    133 
    134 	stat = SystemTable->BootServices->OpenProtocol(
    135 		ImageHandle, &(EFI_GUID)EFI_LOADED_IMAGE_PROTOCOL_GUID,
    136 		(VOID **) &li, ImageHandle, NULL, EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL
    137 		);
    138 	if (stat != EFI_SUCCESS) {
    139 		return stat;
    140 	}
    141 
    142 	stat = SystemTable->BootServices->OpenProtocol(
    143 		li->DeviceHandle, &(EFI_GUID)EFI_SIMPLE_FILE_SYSTEM_PROTOCOL_GUID,
    144 		(VOID **) &fs, ImageHandle, NULL, EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL
    145 		);
    146 	if (stat != EFI_SUCCESS) {
    147 		return stat;
    148 	}
    149 
    150 	stat = fs->OpenVolume(fs, root);
    151 	return stat;
    152 }
    153 
    154 EFI_STATUS
    155 open_gop(EFI_HANDLE ImageHandle, EFI_GRAPHICS_OUTPUT_PROTOCOL **gop)
    156 {
    157 	EFI_STATUS stat;
    158 	UINTN n = 0;
    159 	EFI_HANDLE *_gop;
    160 	stat = SystemTable->BootServices->LocateHandleBuffer(
    161 		ByProtocol, &(EFI_GUID)EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID, NULL,
    162 		&n, &_gop
    163 		);
    164 	if (stat != EFI_SUCCESS) {
    165 		efi_printf("locate handle buffer: %d\n", stat);
    166 		return stat;
    167 	}
    168 	stat = SystemTable->BootServices->OpenProtocol(
    169 		_gop[0], &(EFI_GUID)EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID,
    170 		(VOID **) gop, ImageHandle, NULL, EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL
    171 		);
    172 	if (stat != EFI_SUCCESS) {
    173 		efi_printf("open protocol: %d\n", stat);
    174 		return stat;
    175 	}
    176 	return EFI_SUCCESS;
    177 }
    178 
    179 EFI_STATUS
    180 read_ehdr(EFI_FILE_PROTOCOL *elf, ElfHdr *ehdr)
    181 {
    182 	EFI_STATUS stat;
    183 	UINTN ehdr_size = sizeof(ElfHdr);
    184 
    185 	stat = elf->SetPosition(elf, 0);
    186 	if (stat != EFI_SUCCESS) {
    187 		efi_printf("set position: %d\n", stat);
    188 		return stat;
    189 	}
    190 	stat = elf->Read(elf, &ehdr_size, (VOID *) ehdr);
    191 	if (stat != EFI_SUCCESS) {
    192 		efi_printf("read: %d\n", stat);
    193 		return stat;
    194 	}
    195 	// TODO: check file format.
    196 	// 	for now, it assumes that elf is 2's complement, 64-bit little endian,
    197 	// 	System V ABI...
    198 	if (ehdr->ident[0] != 0x7f || ehdr->ident[1] != 'E' ||
    199 		ehdr->ident[2] != 'L' || ehdr->ident[3] != 'F') {
    200 		efi_printf("not an elf file\n");
    201 		return EFI_INVALID_PARAMETER;
    202 	}
    203 
    204 	return EFI_SUCCESS;
    205 }
    206 
    207 EFI_STATUS
    208 read_phdr(EFI_FILE_PROTOCOL *elf, ElfHdr *ehdr, Phdr phdr[])
    209 {
    210 	EFI_STATUS stat;
    211 	UINTN      n;
    212 
    213 	stat = elf->SetPosition(elf, ehdr->phoff);
    214 	if (stat != EFI_SUCCESS) {
    215 		efi_printf("set position: %d\n", stat);
    216 		return stat;
    217 	}
    218 	// TODO: need overflow check??
    219 	n = ehdr->phentsize * ehdr->phnum;
    220 	stat = elf->Read(elf, &n, (VOID *) phdr);
    221 	if (stat != EFI_SUCCESS) {
    222 		efi_printf("read phdr: %d\n", stat);
    223 		return stat;
    224 	}
    225 	return EFI_SUCCESS;
    226 }
    227 
    228 EFI_STATUS
    229 load_elf(EFI_FILE_PROTOCOL *elf, ElfHdr *ehdr, Phdr phdr[])
    230 {
    231 	EFI_STATUS stat;
    232 	EFI_PHYSICAL_ADDRESS addr;
    233 	UINT64 size;
    234 	for (UINT16 i = 0; i < ehdr->phnum; i++) {
    235 		if (phdr[i].type != PT_LOAD) {
    236 			continue;
    237 		}
    238 		efi_printf("load_elf: allocate %d at %x\n",
    239 			phdr[i].memsz, phdr[i].vaddr
    240 			);
    241 		stat = SystemTable->BootServices->AllocatePages(
    242 			AllocateAddress, EfiLoaderData,
    243 			(phdr[i].memsz + 0xfff) / 0x1000, &phdr[i].vaddr
    244 			);
    245 		if (stat != EFI_SUCCESS) {
    246 			efi_printf("allocate pages: %d\n", stat);
    247 			return stat;
    248 		}
    249 		efi_printf("load_elf: phdr[i].vaddr: %x\n", phdr[i].vaddr);
    250 		stat = elf->SetPosition(elf, phdr[i].offset);
    251 		if (stat != EFI_SUCCESS) {
    252 			efi_printf("set position: %d\n", stat);
    253 			return stat;
    254 		}
    255 		addr = (EFI_PHYSICAL_ADDRESS) phdr[i].vaddr;
    256 		efi_printf("load_elf: read to %x\n", addr);
    257 		size = phdr[i].filesz;
    258 		stat = elf->Read(elf, &size, (VOID *)addr);
    259 		if (stat != EFI_SUCCESS) {
    260 			efi_printf("read to memory: %d\n", stat);
    261 			return stat;
    262 		}
    263 	}
    264 	return EFI_SUCCESS;
    265 }