diff options
-rw-r--r-- | semi_loader.c | 536 | ||||
-rw-r--r-- | semi_loader.h | 99 | ||||
-rw-r--r-- | semihosting.c | 143 | ||||
-rw-r--r-- | semihosting.h | 50 | ||||
-rw-r--r-- | string.c | 137 |
5 files changed, 965 insertions, 0 deletions
diff --git a/semi_loader.c b/semi_loader.c new file mode 100644 index 0000000..9039445 --- /dev/null +++ b/semi_loader.c @@ -0,0 +1,536 @@ +/* + * Copyright (c) 2012 Linaro Limited + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of Linaro Limited nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + */ + +#include <string.h> +#include "libfdt.h" +#include "semihosting.h" +#include "semi_loader.h" + +static void _print_info(char const **strings) +{ + char const *string; + + semi_write0("[bootwrapper] "); + while((string = *strings++)) + semi_write0(string); +} + +#define info(strings...) do { \ + char const *__info_strings[] = { strings, NULL }; \ + \ + _print_info(__info_strings); \ +} while(0) + +#define warn(strings...) info("WARNING: ", strings) +#define error(strings...) info("ERROR: ", strings) + +#define fatal(strings...) do { \ + error(strings); \ + semi_fatal("[bootwrapper] BOOT FAILED\n"); \ +} while(0) + +#define CMDLINE_KERNEL "--kernel" +#define CMDLINE_INITRD "--initrd" +#define CMDLINE_NOINITRD "--no-initrd" +#define CMDLINE_DTB "--dtb" +#define CMDLINE_FDT "--fdt" /* deprecated */ +#define CMDLINE_REST "-- " + +static void _usage_fatal(void) +{ + info("Usage: [" CMDLINE_KERNEL " <kernel filename>] [" + CMDLINE_NOINITRD "|" CMDLINE_INITRD " <initrd filename>] [" + CMDLINE_DTB " <dtb filename>] [" + CMDLINE_REST "<kernel boot arguments>]\n"); + fatal("Incorrect bootwrapper command-line.\n"); +} + +#define usage_fatal(strings...) do { \ + error(strings); \ + _usage_fatal(); \ +} while(0) + +static void atag_append(void **dest, unsigned tag, void const *data, unsigned size) +{ + char *d = *dest; + unsigned padded_size = ALIGN_INT(size, 4) + 8; + struct atag_header header = { + padded_size >> 2, + tag + }; + + if(tag == ATAG_NONE) + header.size = 0; + + memcpy(d, &header, sizeof header); + memcpy(d + 8, data, size); + + if(padded_size > size + 8) + memset(d + 8 + size, 0, padded_size - (size + 8)); + + *dest = d + padded_size; +} + +static int _fdt_make_node(void *fdt, int parentoffset, const char *name) +{ + int e; + + e = fdt_subnode_offset(fdt, parentoffset, name); + if(e != -FDT_ERR_NOTFOUND) + return e; + + return fdt_add_subnode(fdt, parentoffset, name); +} + +static void update_fdt(void **dest, struct loader_info *info) +{ + int e; + int _chosen; + void *fdt; + uint32_t const *p; + + if(!info->fdt_start) + return; + + fdt = ALIGN(*dest, 4); + if((e = fdt_open_into((void *)info->fdt_start, fdt, FDT_SIZE_MAX)) < 0) + goto libfdt_error; + + /* + * Sanity-check address sizes, since addresses and sizes which do + * not take up exactly 4 bytes are not supported. + */ + { + if(!(p = fdt_getprop(fdt, 0, "#address-cells", &e))) + goto libfdt_error; + else if(e != 4 || fdt32_to_cpu(*p) != 1) + goto size_error; + + if(!(p = fdt_getprop(fdt, 0, "#size-cells", &e))) + goto libfdt_error; + else if(e != 4 || fdt32_to_cpu(*p) != 1) + goto size_error; + } + + /* + * Add a memory node, but only if there isn't one already. If + * there is already a memory node with non-zero size, it was put + * in the DT on purpose and should take precedence over our + * guesses. Otherwise, make a memory node with the appropriate + * parameters. + */ + { + int offset, depth = 0; + int _memory; + uint32_t reg[2]; + + for(offset = fdt_next_node(fdt, 0, &depth); offset >= 0; + offset = fdt_next_node(fdt, offset, &depth)) { + char const *name; + + if(depth != 1) + continue; + + name = fdt_get_name(fdt, offset, (void *)0); + if(!strcmp(name, "memory") || + !strncmp(name, "memory@", 7)) { + p = fdt_getprop(fdt, offset, "reg", &e); + if(e < 0) + goto libfdt_error; + + if(fdt32_to_cpu(p[1]) != 0) + goto no_add_memory; + } + } + + if((e = _fdt_make_node(fdt, 0, "memory")) < 0) + goto libfdt_error; + _memory = e; + + reg[0] = cpu_to_fdt32(PHYS_OFFSET); + reg[1] = cpu_to_fdt32(PHYS_SIZE); + if((e = fdt_setprop(fdt, _memory, "reg", ®, sizeof reg)) < 0) + goto libfdt_error; + + if((e = fdt_setprop_string(fdt, _memory, "device_type", + "memory")) < 0) + goto libfdt_error; + } + +no_add_memory: + /* populate the "chosen" node */ + + if((e = _fdt_make_node(fdt, 0, "chosen")) < 0) + goto libfdt_error; + _chosen = e; + + e = fdt_setprop_string(fdt, _chosen, "bootargs", + (char const *)info->cmdline_start); + if(e < 0) + goto libfdt_error; + + if(info->initrd_start) { + uint32_t initrd_end = info->initrd_start + info->initrd_size; + + if((e = fdt_setprop_cell(fdt, _chosen, "linux,initrd-start", + info->initrd_start)) < 0) + goto libfdt_error; + + if((e = fdt_setprop_cell(fdt, _chosen, "linux,initrd-end", + initrd_end)) < 0) + goto libfdt_error; + } + + /* success */ + + /* clean up */ + fdt_pack(fdt); + info->fdt_start = (unsigned)fdt; + info->fdt_size = fdt_totalsize(fdt); + info("FDT updated.\n"); + + return; + +libfdt_error: + fatal("libfdt: ", fdt_strerror(e), ", while updating device tree\n"); + +size_error: + fatal("Unexpected/invalid #address-cells/#size-cells in device tree\n"); +} + +static int is_space(char c) +{ + return c == ' '; +} + +static void skip_space(char **s) +{ + char *t = *s; + + for(t = *s; is_space(*t); t++); + *s = t; +} + +static void find_space(char **s) +{ + char *t = *s; + + for(t = *s; *t && !is_space(*t); t++); + *s = t; +} + +static int match_word(char **s, char const *string) +{ + unsigned l; + + l = strlen(string); + if(strncmp(*s, string, l)) + return 0; + + *s += l; + skip_space(s); + return 1; +} + +/* + * Match an option with a mandatory argument. + * On success, a pointer to the argument is returned, with leading and + * trailing whitespace stripped. + */ +static char *match_option(char **s, char const *option_string) +{ + char *arg; + + if(!match_word(s, option_string)) + return (void *)0; + + if(!**s) + usage_fatal("Option requires as argument: ", + option_string, "\n"); + + /* otherwise, *s now points to the argument */ + arg = *s; + + find_space(s); /* find the end of the argument */ + if(**s) { + *(*s)++ = '\0'; /* null-terminate if necessary */ + skip_space(s); /* skip any remaining space */ + } + + return arg; +} + +static void load_file_essential(void **dest, char const *filename, + unsigned *size, char const *failmsg) +{ + if(semi_load_file(dest, size, filename)) + fatal(failmsg, ": \"", filename, "\"\n"); +} + +/* Move the kernel if necessary, based on the image type: */ +static void correct_kernel_location(struct loader_info *info) +{ + char *const text_start = (char *)(PHYS_OFFSET + TEXT_OFFSET); + char *const text_end = text_start + info->kernel_size; + char *const uImage_payload = text_start + UIMAGE_HEADER_SIZE; + unsigned long *const zImage_magic_p = (unsigned long *)( + uImage_payload + ZIMAGE_MAGIC_OFFSET); + + /* + * If the image is not a uImage, then it is a raw Image or zImage, + * and no action is necessary: + */ + if(info->kernel_size <= UIMAGE_HEADER_SIZE) + return; + + if(memcmp(text_start, uImage_magic, sizeof uImage_magic)) + return; + + warn("Ignoring uImage meta-data\n"); + + /* + * If the uImage payload is a zImage, the position-independent + * nature of the zImage header means that no relocation is + * needed. Instead, just enter at the start of the loaded + * zImage header: + */ + if(text_end >= (char *)&zImage_magic_p[1] + && *zImage_magic_p == ZIMAGE_MAGIC) { + info->kernel_entry += UIMAGE_HEADER_SIZE; + return; + } + + /* + * Otherwise, move the payload to replace the uImage header, and + * leave the entry point unmodified. + */ + memmove(text_start, uImage_payload, + info->kernel_size - UIMAGE_HEADER_SIZE); +} + +static char semi_cmdline[SEMI_CMDLINE_MAX]; + +static char *kernel_arg = (void *)0; +static char *initrd_arg = (void *)0; +static char *fdt_arg = (void *)0; +static char *dtb_arg = (void *)0; +static char *cmdline_arg = (void *)0; +static char *noinitrd_arg = (void *)0; + +static const struct { + char const *option_string; + char **argp; + enum { OPT_ARG, OPT_BOOL, OPT_REST } action; +} options[] = { + { CMDLINE_KERNEL, &kernel_arg, OPT_ARG }, + { CMDLINE_INITRD, &initrd_arg, OPT_ARG }, + { CMDLINE_NOINITRD, &noinitrd_arg, OPT_BOOL }, + { CMDLINE_FDT, &fdt_arg, OPT_ARG }, + { CMDLINE_DTB, &dtb_arg, OPT_ARG }, + { CMDLINE_REST, &cmdline_arg, OPT_REST }, +}; + +void load_kernel(struct loader_info *info) +{ + unsigned i; + char *cmdline = semi_cmdline; + int cmdline_length; + void *phys = (char *)(PHYS_OFFSET + TEXT_OFFSET); + void *atagp = (char *)(PHYS_OFFSET + ATAGS_OFFSET); + + union { + struct atag_core core; + struct atag_mem mem; + struct atag_initrd2 initrd; + } atag; + + /* Fetch the command line: */ + + if(semi_get_cmdline(semi_cmdline, sizeof semi_cmdline, + &cmdline_length) || + cmdline_length >= sizeof semi_cmdline) { + warn("Failed to get semihosting command line, using built-in defaults\n"); + cmdline_length = 0; + } + cmdline[cmdline_length] = '\0'; + + /* Parse the arguments (if any): */ + + skip_space(&cmdline); + + while(*cmdline) { + for(i = 0; i < sizeof options / sizeof *options; i++) { + char *arg; + + switch(options[i].action) { + case OPT_BOOL: + if(!match_word(&cmdline, + options[i].option_string)) + continue; + + *options[i].argp = cmdline; /* non-NULL */ + goto next_arg; + + case OPT_REST: + if(!match_word(&cmdline, + options[i].option_string)) + continue; + + *options[i].argp = cmdline; + goto args_done; + + case OPT_ARG: + arg = match_option(&cmdline, + options[i].option_string); + if(!arg) + continue; + + if(*options[i].argp) + usage_fatal("Duplicate option ", + options[i].option_string); + + /* otherwise, option was parsed successfully: */ + *options[i].argp = arg; + goto next_arg; + } + } /* for(i) */ + + /* Failed to match any expected option: */ + usage_fatal("Invalid option(s): ", cmdline); + + next_arg: ; + } /* while(*cmdline) */ + +args_done: + if(initrd_arg && noinitrd_arg) + usage_fatal("Option --initrd conflicts with --no-initrd.\n"); + + if(fdt_arg) { + warn("--fdt is deprecated. Please use --dtb instead.\n"); + + if(dtb_arg) + usage_fatal("--fdt conflicts with --dtb.\n"); + else + dtb_arg = fdt_arg; + } + + /* + * Now, proceed to load images and set up ATAGs. + * For simplicity, ATAGs are generated even if there is a DTB + */ + + /* built-in FDT not supported, for now */ + info->fdt_start = info->fdt_size = 0; + + info->atags_start = (unsigned)atagp; + + memset(&atag.core, 0, sizeof atag.core); + atag_append(&atagp, ATAG_CORE, &atag.core, sizeof atag.core); + + /* create the essential ATAGs */ + + atag.mem.start = PHYS_OFFSET; + atag.mem.size = PHYS_SIZE; + atag_append(&atagp, ATAG_MEM, &atag.mem, sizeof atag.mem); + + /* load the kernel */ + + info->kernel_entry = (unsigned)phys; + + if(kernel_arg) { + load_file_essential(&phys, kernel_arg, &info->kernel_size, + "Failed to load kernel image"); + info("Loaded kernel: ", kernel_arg, "\n"); + } else if(info->kernel_size) { + info("Using built-in kernel\n"); + phys += info->kernel_size; + } else + usage_fatal("Expected " CMDLINE_KERNEL "\n"); + + /* move the kernel to the correct place, if necessary */ + + correct_kernel_location(info); + + phys = (char *)(PHYS_OFFSET + INITRD_OFFSET); + + /* load the initrd */ + + atag.initrd.size = 0; + + if(initrd_arg) { + unsigned start = (unsigned)phys; + + load_file_essential(&phys, initrd_arg, NULL, + "Failed to load initrd image"); + info("Loaded initrd: ", initrd_arg, "\n"); + + info->initrd_start = atag.initrd.start = start; + info->initrd_size = atag.initrd.size = (unsigned)phys - start; + } else if(info->initrd_size) { + if(noinitrd_arg) { + info->initrd_size = 0; + info("Built-in initrd discarded, as requested\n"); + } else { + info("Using built-in initrd\n"); + + atag.initrd.start = info->initrd_start; + atag.initrd.size = info->initrd_size; + } + } else + info->initrd_size = 0; + + if(atag.initrd.size) + atag_append(&atagp, ATAG_INITRD2, &atag.initrd, sizeof atag.initrd); + + /* load the FDT, if specified */ + + if(fdt_arg) { + phys = ALIGN(phys, 4); + info->fdt_start = (unsigned)phys; + + load_file_essential(&phys, fdt_arg, NULL, + "Failed to load device tree blob"); + info("Loaded FDT: ", fdt_arg, "\n"); + + info->fdt_size = (unsigned)phys - info->fdt_start; + } + + /* + * The FDT will get modified to reflect bootargs, initrd and memory + * configuration later. + */ + + /* set the command line */ + + if(cmdline_arg) { + info->cmdline_start = (unsigned)cmdline_arg; + info->cmdline_size = strlen(cmdline_arg) + 1; + } else if(info->cmdline_size) + info("Using built-in kernel bootargs\n"); + + /* cmdline_size is presumed to include a NUL terminator: */ + if(info->cmdline_size) { + atag_append(&atagp, ATAG_CMDLINE, + (char *)info->cmdline_start, info->cmdline_size); + info("Kernel bootargs: ", (char *)info->cmdline_start, "\n"); + } + + atag_append(&atagp, ATAG_NONE, 0, 0); + + update_fdt(&phys, info); +} diff --git a/semi_loader.h b/semi_loader.h new file mode 100644 index 0000000..8a1a602 --- /dev/null +++ b/semi_loader.h @@ -0,0 +1,99 @@ +/* + * Copyright (c) 2012 Linaro Limited + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of Linaro Limited nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + */ + +#ifndef SEMI_LOADER_H +#define SEMI_LOADER_H + +#define ATAG_NONE 0x00000000 +#define ATAG_CORE 0x54410001 +#define ATAG_MEM 0x54410002 +#define ATAG_INITRD2 0x54420005 +#define ATAG_CMDLINE 0x54410009 + +struct atag_header { + unsigned size; + unsigned tag; +}; + +struct atag_core { + unsigned flags; + unsigned pagesize; + unsigned rootdev; +}; + +struct atag_mem { + unsigned size; + unsigned start; +}; + +struct atag_initrd2 { + unsigned start; + unsigned size; +}; + +static const char uImage_magic[] = { + 0x27, 0x05, 0x19, 0x56 +}; +#define UIMAGE_HEADER_SIZE 0x40 + +#define ZIMAGE_MAGIC_OFFSET 36 +#define ZIMAGE_MAGIC 0x016f2818UL + +#define PHYS_OFFSET 0x80000000 +#define PHYS_SIZE 0x80000000 /* can limit on kernel cmdline if necessary */ +#define ATAGS_OFFSET 0x100 +#define TEXT_OFFSET 0x8000 +#define INITRD_OFFSET 0xD00000 /* qemu uses the same random offset */ + +#define FDT_SIZE_MAX 0x10000 /* maximum size allowed for device tree blob */ + +#define SEMI_CMDLINE_MAX 0x1000 /* maximum semihosting command line length */ + +/* + * Align number <n> or pointer <p> up to the next boundary of size <size>. + * <size> must be a power of two. + */ +#define ALIGN_INT(n, size) (((n) + ((size) - 1)) & ~((size) - 1)) +#define ALIGN(p, size) ((void *)ALIGN_INT((unsigned)(p), size)) + +struct loader_info { + unsigned kernel_size; /* nonzero indicates preloaded kernel size */ + unsigned initrd_start; /* start of preloaded initrd, if any */ + unsigned initrd_size; + unsigned cmdline_start; /* start of cmdline buffer */ + unsigned cmdline_size; + + /* The remaining fields are set by the loader: */ + + /* There could be a built-in FDT, but currently that it not supported */ + unsigned fdt_start; /* start of device tree blob, if any */ + unsigned fdt_size; + + unsigned atags_start; + unsigned kernel_entry; /* kernel entry point */ +}; + +void load_kernel(struct loader_info *info); + +static void boot_kernel(struct loader_info *info, + unsigned r0, unsigned r1, unsigned r2, unsigned r3) { + ((void (*)(unsigned, unsigned, unsigned, unsigned))info->kernel_entry)( + r0, r1, r2, r3); +} + +#endif /* ! SEMI_LOADER_H */ diff --git a/semihosting.c b/semihosting.c new file mode 100644 index 0000000..b5e1c9b --- /dev/null +++ b/semihosting.c @@ -0,0 +1,143 @@ +/* + * Copyright (c) 2012 Linaro Limited + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of Linaro Limited nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + */ + +#include <string.h> +#include "semihosting.h" + +int semi_open(char const *filename, int mode) +{ + struct { + char const *filename; + int mode; + int filename_length; + } args; + + args.filename = filename; + args.mode = mode; + args.filename_length = strlen(filename); + + return __semi_call(SYS_OPEN, &args); +} + +int semi_close(int fd) +{ + return __semi_call(SYS_CLOSE, &fd); +} + +int semi_write0(char const *string) +{ + return __semi_call(SYS_WRITE0, string); +} + +int semi_read(int fd, char *buffer, int length) +{ + struct { + int fd; + char *buffer; + int length; + } args; + + args.fd = fd; + args.buffer = buffer; + args.length = length; + + return __semi_call(SYS_READ, &args); +} + +int semi_flen(int fd) +{ + return __semi_call(SYS_FLEN, &fd); +} + +int semi_get_cmdline(char *buffer, int size, int *length) +{ + int result; + struct { + char *buffer; + int size; + } args; + + args.buffer = buffer; + args.size = size; + + result = __semi_call(SYS_GET_CMDLINE, &args); + if(result) + return result; + + if(length) + *length = args.size; + + return 0; +} + +int semi_reportexc(int reason) +{ + return __semi_call(SYS_REPORTEXC, (void *)reason); +} + +void semi_exit(void) +{ + semi_reportexc(REPORTEXC_REASON_APP_EXIT); + while(1); /* should not be reached */ +} + +void semi_fatal(char const *message) +{ + semi_write0(message); + semi_exit(); +} + +int semi_load_file(void **dest, unsigned *size, char const *filename) +{ + int result = -1; /* fail by default */ + int fd = -1; + int filesize; + + fd = semi_open(filename, OPEN_RDONLY); + if(fd == -1) { + semi_write0("Cannot open file: "); + goto out; + } + + filesize = semi_flen(fd); + if(filesize == -1) { + semi_write0("Cannot get file size for: "); + goto out; + } + + if(semi_read(fd, *dest, filesize)) { + semi_write0("Could not read: "); + goto out; + } + + result = 0; /* success */ + *dest = (char *)*dest + filesize; + +out: + if(fd != -1) + semi_close(fd); + + if(result) { /* print context for the error message */ + semi_write0(filename); + semi_write0("\n"); + } else + if(size) + *size = filesize; + + return result; +} diff --git a/semihosting.h b/semihosting.h new file mode 100644 index 0000000..40817c2 --- /dev/null +++ b/semihosting.h @@ -0,0 +1,50 @@ +/* + * Copyright (c) 2012 Linaro Limited + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of Linaro Limited nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + */ + +#ifndef SEMIHOSTING_H +#define SEMIHOSTING_H + +#define SYS_OPEN 1 +#define OPEN_RDONLY 1 +#define SYS_CLOSE 2 +#define SYS_WRITE0 4 +#define SYS_READ 6 +#define SYS_FLEN 0x0C +#define SYS_GET_CMDLINE 0x15 +#define SYS_REPORTEXC 0x18 +#define REPORTEXC_REASON_APP_EXIT 0x20026 +#define SEMIHOSTING_SVC 0x123456 /* SVC comment field for semihosting */ + +#ifndef __ASSEMBLER__ + +int __semi_call(int id, ...); +int semi_open(char const *filename, int mode); +int semi_close(int fd); +int semi_write0(char const *string); +int semi_read(int fd, char *buffer, int length); +int semi_flen(int fd); +int semi_get_cmdline(char *buffer, int size, int *length); +int semi_reportexc(int reason); +void semi_fatal(char const *message); +void semi_exit(void); +/* semi_load_file: *dest is advanced to point to the end of the loaded data */ +int semi_load_file(void **dest, unsigned *size, char const *filename); + +#endif /* ! __ASSEMBLER__ */ + +#endif /* ! SEMIHOSTING_H */ diff --git a/string.c b/string.c new file mode 100644 index 0000000..a807baa --- /dev/null +++ b/string.c @@ -0,0 +1,137 @@ +#include <string.h> + +static void *__memmove_down(void *__dest, __const void *__src, size_t __n) +{ + unsigned char *d = (unsigned char *)__dest, *s = (unsigned char *)__src; + + while (__n--) + *d++ = *s++; + + return __dest; +} + +static void *__memmove_up(void *__dest, __const void *__src, size_t __n) +{ + unsigned char *d = (unsigned char *)__dest + __n - 1, *s = (unsigned char *)__src + __n - 1; + + while (__n--) + *d-- = *s--; + + return __dest; +} + +void *(memcpy)(void *__dest, __const void *__src, size_t __n) +{ + return __memmove_down(__dest, __src, __n); +} + +void *(memmove)(void *__dest, __const void *__src, size_t __n) +{ + if(__dest > __src) + return __memmove_up(__dest, __src, __n); + else + return __memmove_down(__dest, __src, __n); +} + +void *(memchr)(void const *s, int c, size_t n) +{ + unsigned char const *_s = (unsigned char const *)s; + + while(n && *_s != c) { + ++_s; + --n; + } + + if(n) + return (void *)_s; /* the C library casts const away */ + else + return (void *)0; +} + +size_t (strlen)(const char *s) +{ + const char *sc = s; + + while (*sc != '\0') + sc++; + return sc - s; +} + +void *(memset)(void *s, int c, size_t count) +{ + char *xs = s; + while (count--) + *xs++ = c; + return s; +} + +int (memcmp)(void const *p1, void const *p2, size_t n) +{ + unsigned char const *_p1 = p1; + unsigned char const *_p2 = p2; + + while(n--) { + if(*_p1 < *_p2) + return -1; + else if(*_p1 > *_p2) + return 1; + + ++_p1; + ++_p2; + } + + return 0; +} + +int (strcmp)(char const *s1, char const *s2) +{ + while(*s1 && *s2) { + if(*s1 < *s2) + return -1; + else if(*s1 > *s2) + return 1; + + ++s1; + ++s2; + } + + if(!*s1 && !*s2) + return 0; + else if(!*s1) + return -1; + else + return 1; +} + +int (strncmp)(char const *s1, char const *s2, size_t n) +{ + while(*s1 && *s2 && n--) { + if(*s1 < *s2) + return -1; + else if(*s1 > *s2) + return 1; + + ++s1; + ++s2; + } + + if(n == 0 || (!*s1 && !*s2)) + return 0; + else if(!*s1) + return -1; + else + return 1; +} + +char *(strchr)(char const *s, int c) +{ + unsigned char const *_s = (unsigned char const *)s; + + while(*_s && *_s != c) + ++_s; + + if(*_s) + return (char *)_s; /* the C library casts const away */ + else + return (char *)0; +} |