diff --git a/include/lib/stdlib/machine/endian.h b/include/lib/stdlib/machine/endian.h index a6a0cb7..57e33b1 100644 --- a/include/lib/stdlib/machine/endian.h +++ b/include/lib/stdlib/machine/endian.h @@ -29,6 +29,10 @@ * $NetBSD: endian.h,v 1.7 1999/08/21 05:53:51 simonb Exp $ * $FreeBSD$ */ +/* + * Portions copyright (c) 2017, ARM Limited and Contributors. + * All rights reserved. + */ #ifndef _MACHINE_ENDIAN_H_ #define _MACHINE_ENDIAN_H_ @@ -59,6 +63,45 @@ #define __htonl(x) (__bswap32(x)) #define __htons(x) (__bswap16(x)) +#ifdef AARCH32 +static __inline __uint64_t +__bswap64(__uint64_t _x) +{ + + return ((_x >> 56) | ((_x >> 40) & 0xff00) | ((_x >> 24) & 0xff0000) | + ((_x >> 8) & 0xff000000) | ((_x << 8) & ((__uint64_t)0xff << 32)) | + ((_x << 24) & ((__uint64_t)0xff << 40)) | + ((_x << 40) & ((__uint64_t)0xff << 48)) | ((_x << 56))); +} + +static __inline __uint32_t +__bswap32_var(__uint32_t v) +{ + __uint32_t t1; + + __asm __volatile("eor %1, %0, %0, ror #16\n" + "bic %1, %1, #0x00ff0000\n" + "mov %0, %0, ror #8\n" + "eor %0, %0, %1, lsr #8\n" + : "+r" (v), "=r" (t1)); + + return (v); +} + +static __inline __uint16_t +__bswap16_var(__uint16_t v) +{ + __uint32_t ret = v & 0xffff; + + __asm __volatile( + "mov %0, %0, ror #8\n" + "orr %0, %0, %0, lsr #16\n" + "bic %0, %0, %0, lsl #16" + : "+r" (ret)); + + return ((__uint16_t)ret); +} +#elif defined AARCH64 static __inline __uint64_t __bswap64(__uint64_t x) { @@ -91,6 +134,9 @@ return ((__uint16_t)ret); } +#else +#error "Only AArch32 or AArch64 supported" +#endif /* AARCH32 */ #ifdef __OPTIMIZE__