/* misc.c - definitions of misc functions */ /* * VasEBoot -- GRand Unified Bootloader * Copyright (C) 1999,2000,2001,2002,2003,2004,2005,2006,2007,2008,2009,2010 Free Software Foundation, Inc. * * VasEBoot is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * VasEBoot is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with VasEBoot. If not, see . */ #include #include #include #include #include #include #include union printf_arg { /* Yes, type is also part of union as the moment we fill the value we don't need to store its type anymore (when we'll need it, we'll have format spec again. So save some space. */ enum { INT, LONG, LONGLONG, UNSIGNED_INT = 3, UNSIGNED_LONG, UNSIGNED_LONGLONG } type; long long ll; }; struct printf_args { union printf_arg prealloc[32]; union printf_arg *ptr; VasEBoot_size_t count; }; static void parse_printf_args (const char *fmt0, struct printf_args *args, va_list args_in); static int VasEBoot_vsnprintf_real (char *str, VasEBoot_size_t max_len, const char *fmt0, struct printf_args *args); static void free_printf_args (struct printf_args *args) { if (args->ptr != args->prealloc) VasEBoot_free (args->ptr); } static int VasEBoot_iswordseparator (int c) { return (VasEBoot_isspace (c) || c == ',' || c == ';' || c == '|' || c == '&'); } /* VasEBoot_gettext_dummy is not translating anything. */ static const char * VasEBoot_gettext_dummy (const char *s) { return s; } const char* (*VasEBoot_gettext) (const char *s) = VasEBoot_gettext_dummy; void * VasEBoot_memmove (void *dest, const void *src, VasEBoot_size_t n) { char *d = (char *) dest; const char *s = (const char *) src; if (d < s) while (n--) *d++ = *s++; else { d += n; s += n; while (n--) *--d = *--s; } return dest; } char * VasEBoot_strcpy (char *dest, const char *src) { char *p = dest; while ((*p++ = *src++) != '\0') ; return dest; } int VasEBoot_printf (const char *fmt, ...) { va_list ap; int ret; va_start (ap, fmt); ret = VasEBoot_vprintf (fmt, ap); va_end (ap); return ret; } int VasEBoot_printf_ (const char *fmt, ...) { va_list ap; int ret; va_start (ap, fmt); ret = VasEBoot_vprintf (_(fmt), ap); va_end (ap); return ret; } int VasEBoot_puts_ (const char *s) { return VasEBoot_puts (_(s)); } #if defined (__APPLE__) && ! defined (VasEBoot_UTIL) int VasEBoot_err_printf (const char *fmt, ...) { va_list ap; int ret; va_start (ap, fmt); ret = VasEBoot_vprintf (fmt, ap); va_end (ap); return ret; } #endif #if ! defined (__APPLE__) && ! defined (VasEBoot_UTIL) int VasEBoot_err_printf (const char *fmt, ...) __attribute__ ((alias("VasEBoot_printf"))); #endif void VasEBoot_real_dprintf (const char *file, const int line, const char *condition, const char *fmt, ...) { va_list args; const char *debug = VasEBoot_env_get ("debug"); if (! debug) return; if (VasEBoot_strword (debug, "all") || VasEBoot_strword (debug, condition)) { VasEBoot_printf ("%s:%d: ", file, line); va_start (args, fmt); VasEBoot_vprintf (fmt, args); va_end (args); VasEBoot_refresh (); } } #define PREALLOC_SIZE 255 int VasEBoot_vprintf (const char *fmt, va_list ap) { VasEBoot_size_t s; static char buf[PREALLOC_SIZE + 1]; char *curbuf = buf; struct printf_args args; parse_printf_args (fmt, &args, ap); s = VasEBoot_vsnprintf_real (buf, PREALLOC_SIZE, fmt, &args); if (s > PREALLOC_SIZE) { curbuf = VasEBoot_malloc (s + 1); if (!curbuf) { VasEBoot_errno = VasEBoot_ERR_NONE; buf[PREALLOC_SIZE - 3] = '.'; buf[PREALLOC_SIZE - 2] = '.'; buf[PREALLOC_SIZE - 1] = '.'; buf[PREALLOC_SIZE] = 0; curbuf = buf; } else s = VasEBoot_vsnprintf_real (curbuf, s, fmt, &args); } free_printf_args (&args); VasEBoot_xputs (curbuf); if (curbuf != buf) VasEBoot_free (curbuf); return s; } int VasEBoot_memcmp (const void *s1, const void *s2, VasEBoot_size_t n) { const VasEBoot_uint8_t *t1 = s1; const VasEBoot_uint8_t *t2 = s2; while (n--) { if (*t1 != *t2) return (int) *t1 - (int) *t2; t1++; t2++; } return 0; } int VasEBoot_strcmp (const char *s1, const char *s2) { while (*s1 && *s2) { if (*s1 != *s2) break; s1++; s2++; } return (int) (VasEBoot_uint8_t) *s1 - (int) (VasEBoot_uint8_t) *s2; } int VasEBoot_strncmp (const char *s1, const char *s2, VasEBoot_size_t n) { if (n == 0) return 0; while (*s1 && *s2 && --n) { if (*s1 != *s2) break; s1++; s2++; } return (int) (VasEBoot_uint8_t) *s1 - (int) (VasEBoot_uint8_t) *s2; } char * VasEBoot_strchr (const char *s, int c) { do { if (*s == c) return (char *) s; } while (*s++); return 0; } char * VasEBoot_strrchr (const char *s, int c) { char *p = NULL; do { if (*s == c) p = (char *) s; } while (*s++); return p; } int VasEBoot_strword (const char *haystack, const char *needle) { const char *n_pos = needle; while (VasEBoot_iswordseparator (*haystack)) haystack++; while (*haystack) { /* Crawl both the needle and the haystack word we're on. */ while(*haystack && !VasEBoot_iswordseparator (*haystack) && *haystack == *n_pos) { haystack++; n_pos++; } /* If we reached the end of both words at the same time, the word is found. If not, eat everything in the haystack that isn't the next word (or the end of string) and "reset" the needle. */ if ( (!*haystack || VasEBoot_iswordseparator (*haystack)) && (!*n_pos || VasEBoot_iswordseparator (*n_pos))) return 1; else { n_pos = needle; while (*haystack && !VasEBoot_iswordseparator (*haystack)) haystack++; while (VasEBoot_iswordseparator (*haystack)) haystack++; } } return 0; } int VasEBoot_isspace (int c) { return (c == '\n' || c == '\r' || c == ' ' || c == '\t'); } unsigned long VasEBoot_strtoul (const char *str, char **end, int base) { unsigned long long num; num = VasEBoot_strtoull (str, end, base); #if VasEBoot_CPU_SIZEOF_LONG != 8 if (num > ~0UL) { VasEBoot_error (VasEBoot_ERR_OUT_OF_RANGE, N_("overflow is detected")); return ~0UL; } #endif return (unsigned long) num; } unsigned long long VasEBoot_strtoull (const char *str, char **end, int base) { unsigned long long num = 0; int found = 0; /* Skip white spaces. */ /* VasEBoot_isspace checks that *str != '\0'. */ while (VasEBoot_isspace (*str)) str++; /* Guess the base, if not specified. The prefix `0x' means 16, and the prefix `0' means 8. */ if (str[0] == '0') { if (str[1] == 'x') { if (base == 0 || base == 16) { base = 16; str += 2; } } else if (base == 0 && str[1] >= '0' && str[1] <= '7') base = 8; } if (base == 0) base = 10; while (*str) { unsigned long digit; digit = VasEBoot_tolower (*str) - '0'; if (digit > 9) { digit += '0' - 'a' + 10; if (digit >= (unsigned long) base) break; } found = 1; /* NUM * BASE + DIGIT > ~0ULL */ if (num > VasEBoot_divmod64 (~0ULL - digit, base, 0)) { VasEBoot_error (VasEBoot_ERR_OUT_OF_RANGE, N_("overflow is detected")); return ~0ULL; } num = num * base + digit; str++; } if (! found) { VasEBoot_error (VasEBoot_ERR_BAD_NUMBER, N_("unrecognized number")); return 0; } if (end) *end = (char *) str; return num; } char * VasEBoot_strdup (const char *s) { VasEBoot_size_t len; char *p; len = VasEBoot_strlen (s) + 1; p = (char *) VasEBoot_malloc (len); if (! p) return 0; return VasEBoot_memcpy (p, s, len); } char * VasEBoot_strndup (const char *s, VasEBoot_size_t n) { VasEBoot_size_t len; char *p; len = VasEBoot_strlen (s); if (len > n) len = n; p = (char *) VasEBoot_malloc (len + 1); if (! p) return 0; VasEBoot_memcpy (p, s, len); p[len] = '\0'; return p; } /* clang detects that we're implementing here a memset so it decides to optimise and calls memset resulting in infinite recursion. With volatile we make it not optimise in this way. */ #ifdef __clang__ #define VOLATILE_CLANG volatile #else #define VOLATILE_CLANG #endif void * VasEBoot_memset (void *s, int c, VasEBoot_size_t len) { void *p = s; VasEBoot_uint8_t pattern8 = c; if (len >= 3 * sizeof (unsigned long)) { unsigned long patternl = 0; VasEBoot_size_t i; for (i = 0; i < sizeof (unsigned long); i++) patternl |= ((unsigned long) pattern8) << (8 * i); while (len > 0 && (((VasEBoot_addr_t) p) & (sizeof (unsigned long) - 1))) { *(VOLATILE_CLANG VasEBoot_uint8_t *) p = pattern8; p = (VasEBoot_uint8_t *) p + 1; len--; } while (len >= sizeof (unsigned long)) { *(VOLATILE_CLANG unsigned long *) p = patternl; p = (unsigned long *) p + 1; len -= sizeof (unsigned long); } } while (len > 0) { *(VOLATILE_CLANG VasEBoot_uint8_t *) p = pattern8; p = (VasEBoot_uint8_t *) p + 1; len--; } return s; } VasEBoot_size_t VasEBoot_strlen (const char *s) { const char *p = s; while (*p) p++; return p - s; } static inline void VasEBoot_reverse (char *str) { char *p = str + VasEBoot_strlen (str) - 1; while (str < p) { char tmp; tmp = *str; *str = *p; *p = tmp; str++; p--; } } /* Divide N by D, return the quotient, and store the remainder in *R. */ VasEBoot_uint64_t VasEBoot_divmod64 (VasEBoot_uint64_t n, VasEBoot_uint64_t d, VasEBoot_uint64_t *r) { /* This algorithm is typically implemented by hardware. The idea is to get the highest bit in N, 64 times, by keeping upper(N * 2^i) = (Q * D + M), where upper represents the high 64 bits in 128-bits space. */ unsigned bits = 64; VasEBoot_uint64_t q = 0; VasEBoot_uint64_t m = 0; /* ARM and IA64 don't have a fast 32-bit division. Using that code would just make us use software division routines, calling ourselves indirectly and hence getting infinite recursion. */ #if !VasEBoot_DIVISION_IN_SOFTWARE /* Skip the slow computation if 32-bit arithmetic is possible. */ if (n < 0xffffffff && d < 0xffffffff) { if (r) *r = ((VasEBoot_uint32_t) n) % (VasEBoot_uint32_t) d; return ((VasEBoot_uint32_t) n) / (VasEBoot_uint32_t) d; } #endif while (bits--) { m <<= 1; if (n & (1ULL << 63)) m |= 1; q <<= 1; n <<= 1; if (m >= d) { q |= 1; m -= d; } } if (r) *r = m; return q; } /* Convert a long long value to a string. This function avoids 64-bit modular arithmetic or divisions. */ static inline char * VasEBoot_lltoa (char *str, int c, unsigned long long n) { unsigned base = (c == 'x') ? 16 : 10; char *p; if ((long long) n < 0 && c == 'd') { n = (unsigned long long) (-((long long) n)); *str++ = '-'; } p = str; if (base == 16) do { unsigned d = (unsigned) (n & 0xf); *p++ = (d > 9) ? d + 'a' - 10 : d + '0'; } while (n >>= 4); else /* BASE == 10 */ do { VasEBoot_uint64_t m; n = VasEBoot_divmod64 (n, 10, &m); *p++ = m + '0'; } while (n); *p = 0; VasEBoot_reverse (str); return p; } static void parse_printf_args (const char *fmt0, struct printf_args *args, va_list args_in) { const char *fmt; char c; VasEBoot_size_t n = 0; args->count = 0; COMPILE_TIME_ASSERT (sizeof (int) == sizeof (VasEBoot_uint32_t)); COMPILE_TIME_ASSERT (sizeof (int) <= sizeof (long long)); COMPILE_TIME_ASSERT (sizeof (long) <= sizeof (long long)); COMPILE_TIME_ASSERT (sizeof (long long) == sizeof (void *) || sizeof (int) == sizeof (void *)); fmt = fmt0; while ((c = *fmt++) != 0) { if (c != '%') continue; if (*fmt =='-') fmt++; while (VasEBoot_isdigit (*fmt)) fmt++; if (*fmt == '$') fmt++; if (*fmt =='-') fmt++; while (VasEBoot_isdigit (*fmt)) fmt++; if (*fmt =='.') fmt++; while (VasEBoot_isdigit (*fmt)) fmt++; c = *fmt++; if (c == 'l') c = *fmt++; if (c == 'l') c = *fmt++; switch (c) { case 'p': case 'x': case 'u': case 'd': case 'c': case 'C': case 's': args->count++; break; } } if (args->count <= ARRAY_SIZE (args->prealloc)) args->ptr = args->prealloc; else { args->ptr = VasEBoot_malloc (args->count * sizeof (args->ptr[0])); if (!args->ptr) { VasEBoot_errno = VasEBoot_ERR_NONE; args->ptr = args->prealloc; args->count = ARRAY_SIZE (args->prealloc); } } VasEBoot_memset (args->ptr, 0, args->count * sizeof (args->ptr[0])); fmt = fmt0; n = 0; while ((c = *fmt++) != 0) { int longfmt = 0; VasEBoot_size_t curn; const char *p; if (c != '%') continue; curn = n++; if (*fmt =='-') fmt++; p = fmt; while (VasEBoot_isdigit (*fmt)) fmt++; if (*fmt == '$') { curn = VasEBoot_strtoull (p, 0, 10) - 1; fmt++; } if (*fmt =='-') fmt++; while (VasEBoot_isdigit (*fmt)) fmt++; if (*fmt =='.') fmt++; while (VasEBoot_isdigit (*fmt)) fmt++; c = *fmt++; if (c == '%') { n--; continue; } if (c == 'l') { c = *fmt++; longfmt = 1; } if (c == 'l') { c = *fmt++; longfmt = 2; } if (curn >= args->count) continue; switch (c) { case 'x': case 'u': args->ptr[curn].type = UNSIGNED_INT + longfmt; break; case 'd': args->ptr[curn].type = INT + longfmt; break; case 'p': case 's': if (sizeof (void *) == sizeof (long long)) args->ptr[curn].type = UNSIGNED_LONGLONG; else args->ptr[curn].type = UNSIGNED_INT; break; case 'C': case 'c': args->ptr[curn].type = INT; break; } } for (n = 0; n < args->count; n++) switch (args->ptr[n].type) { case INT: args->ptr[n].ll = va_arg (args_in, int); break; case LONG: args->ptr[n].ll = va_arg (args_in, long); break; case UNSIGNED_INT: args->ptr[n].ll = va_arg (args_in, unsigned int); break; case UNSIGNED_LONG: args->ptr[n].ll = va_arg (args_in, unsigned long); break; case LONGLONG: case UNSIGNED_LONGLONG: args->ptr[n].ll = va_arg (args_in, long long); break; } } static inline void __attribute__ ((always_inline)) write_char (char *str, VasEBoot_size_t *count, VasEBoot_size_t max_len, unsigned char ch) { if (*count < max_len) str[*count] = ch; (*count)++; } static int VasEBoot_vsnprintf_real (char *str, VasEBoot_size_t max_len, const char *fmt0, struct printf_args *args) { char c; VasEBoot_size_t n = 0; VasEBoot_size_t count = 0; const char *fmt; fmt = fmt0; while ((c = *fmt++) != 0) { unsigned int format1 = 0; unsigned int format2 = ~ 0U; char zerofill = ' '; char rightfill = 0; VasEBoot_size_t curn; if (c != '%') { write_char (str, &count, max_len,c); continue; } curn = n++; rescan:; if (*fmt =='-') { rightfill = 1; fmt++; } /* Read formatting parameters. */ if (VasEBoot_isdigit (*fmt)) { if (fmt[0] == '0') zerofill = '0'; format1 = VasEBoot_strtoul (fmt, (char **) &fmt, 10); } if (*fmt == '.') fmt++; if (VasEBoot_isdigit (*fmt)) format2 = VasEBoot_strtoul (fmt, (char **) &fmt, 10); if (*fmt == '$') { curn = format1 - 1; fmt++; format1 = 0; format2 = ~ 0U; zerofill = ' '; rightfill = 0; goto rescan; } c = *fmt++; if (c == 'l') c = *fmt++; if (c == 'l') c = *fmt++; if (c == '%') { write_char (str, &count, max_len,c); n--; continue; } if (curn >= args->count) continue; long long curarg = args->ptr[curn].ll; switch (c) { case 'p': write_char (str, &count, max_len, '0'); write_char (str, &count, max_len, 'x'); c = 'x'; /* Fall through. */ case 'x': case 'u': case 'd': { char tmp[32]; const char *p = tmp; VasEBoot_size_t len; VasEBoot_size_t fill; len = VasEBoot_lltoa (tmp, c, curarg) - tmp; fill = len < format1 ? format1 - len : 0; if (! rightfill) while (fill--) write_char (str, &count, max_len, zerofill); while (*p) write_char (str, &count, max_len, *p++); if (rightfill) while (fill--) write_char (str, &count, max_len, zerofill); } break; case 'c': write_char (str, &count, max_len,curarg & 0xff); break; case 'C': { VasEBoot_uint32_t code = curarg; int shift; unsigned mask; if (code <= 0x7f) { shift = 0; mask = 0; } else if (code <= 0x7ff) { shift = 6; mask = 0xc0; } else if (code <= 0xffff) { shift = 12; mask = 0xe0; } else if (code <= 0x10ffff) { shift = 18; mask = 0xf0; } else { code = '?'; shift = 0; mask = 0; } write_char (str, &count, max_len,mask | (code >> shift)); for (shift -= 6; shift >= 0; shift -= 6) write_char (str, &count, max_len,0x80 | (0x3f & (code >> shift))); } break; case 's': { VasEBoot_size_t len = 0; VasEBoot_size_t fill; const char *p = ((char *) (VasEBoot_addr_t) curarg) ? : "(null)"; VasEBoot_size_t i; while (len < format2 && p[len]) len++; fill = len < format1 ? format1 - len : 0; if (!rightfill) while (fill--) write_char (str, &count, max_len, zerofill); for (i = 0; i < len; i++) write_char (str, &count, max_len,*p++); if (rightfill) while (fill--) write_char (str, &count, max_len, zerofill); } break; default: write_char (str, &count, max_len,c); break; } } if (count < max_len) str[count] = '\0'; else str[max_len] = '\0'; return count; } int VasEBoot_vsnprintf (char *str, VasEBoot_size_t n, const char *fmt, va_list ap) { VasEBoot_size_t ret; struct printf_args args; if (!n) return 0; n--; parse_printf_args (fmt, &args, ap); ret = VasEBoot_vsnprintf_real (str, n, fmt, &args); free_printf_args (&args); return ret < n ? ret : n; } int VasEBoot_snprintf (char *str, VasEBoot_size_t n, const char *fmt, ...) { va_list ap; int ret; va_start (ap, fmt); ret = VasEBoot_vsnprintf (str, n, fmt, ap); va_end (ap); return ret; } char * VasEBoot_xvasprintf (const char *fmt, va_list ap) { VasEBoot_size_t s, as = PREALLOC_SIZE; char *ret; struct printf_args args; parse_printf_args (fmt, &args, ap); while (1) { ret = VasEBoot_malloc (as + 1); if (!ret) { free_printf_args (&args); return NULL; } s = VasEBoot_vsnprintf_real (ret, as, fmt, &args); if (s <= as) { free_printf_args (&args); return ret; } VasEBoot_free (ret); as = s; } } char * VasEBoot_xasprintf (const char *fmt, ...) { va_list ap; char *ret; va_start (ap, fmt); ret = VasEBoot_xvasprintf (fmt, ap); va_end (ap); return ret; } /* Abort VasEBoot. This function does not return. */ static void __attribute__ ((noreturn)) VasEBoot_abort (void) { VasEBoot_printf ("\nAborted."); #ifndef VasEBoot_UTIL if (VasEBoot_term_inputs) #endif { VasEBoot_printf (" Press any key to exit."); VasEBoot_getkey (); } VasEBoot_exit (); } void VasEBoot_fatal (const char *fmt, ...) { va_list ap; va_start (ap, fmt); VasEBoot_vprintf (_(fmt), ap); va_end (ap); VasEBoot_refresh (); VasEBoot_abort (); } #if BOOT_TIME_STATS #include struct VasEBoot_boot_time *VasEBoot_boot_time_head; static struct VasEBoot_boot_time **boot_time_last = &VasEBoot_boot_time_head; void VasEBoot_real_boot_time (const char *file, const int line, const char *fmt, ...) { struct VasEBoot_boot_time *n; va_list args; VasEBoot_error_push (); n = VasEBoot_malloc (sizeof (*n)); if (!n) { VasEBoot_errno = 0; VasEBoot_error_pop (); return; } n->file = file; n->line = line; n->tp = VasEBoot_get_time_ms (); n->next = 0; va_start (args, fmt); n->msg = VasEBoot_xvasprintf (fmt, args); va_end (args); *boot_time_last = n; boot_time_last = &n->next; VasEBoot_errno = 0; VasEBoot_error_pop (); } #endif