ref: ca1b12064ff8d177c5642645b25b13aa192dfdc4
dir: /llt/utils.c/
#include <stdlib.h>
#include <string.h>
#include <stddef.h>
#include <alloca.h>
#include "dtypes.h"
#include "utils.h"
void memswap(char *a, char *b, size_t sz)
{
int8_t i8;
int32_t i32;
int32_t *a4, *b4;
if (sz < 4) {
while (sz--) {
i8 = *a;
*a++ = *b;
*b++ = i8;
}
}
else {
while (sz & 0x3) {
i8 = *a;
*a++ = *b;
*b++ = i8;
sz--;
}
a4 = (int32_t*)a;
b4 = (int32_t*)b;
sz >>= 2;
while (sz--) {
i32 = *a4;
*a4++ = *b4;
*b4++ = i32;
}
}
}
void memreverse(char *a, size_t n, size_t elsz)
{
int64_t i64, *pi64;
int32_t i32, *pi32;
int16_t i16, *pi16;
int8_t i8;
size_t i;
char *temp;
size_t eli, tot;
if (n==0 || elsz==0) return;
switch(elsz) {
case 16:
pi64 = (int64_t*)a;
for(i=0; i < n/2; i++) {
i64 = pi64[2*i];
pi64[2*i] = pi64[2*(n-i-1)];
pi64[2*(n-i-1)] = i64;
i64 = pi64[2*i+1];
pi64[2*i+1] = pi64[2*(n-i-1)+1];
pi64[2*(n-i-1)+1] = i64;
}
break;
case 8:
pi64 = (int64_t*)a;
for(i=0; i < n/2; i++) {
i64 = pi64[i];
pi64[i] = pi64[n-i-1];
pi64[n-i-1] = i64;
}
break;
case 4:
pi32 = (int32_t*)a;
for(i=0; i < n/2; i++) {
i32 = pi32[i];
pi32[i] = pi32[n-i-1];
pi32[n-i-1] = i32;
}
break;
case 2:
pi16 = (int16_t*)a;
for(i=0; i < n/2; i++) {
i16 = pi16[i];
pi16[i] = pi16[n-i-1];
pi16[n-i-1] = i16;
}
break;
case 1:
for(i=0; i < n/2; i++) {
i8 = a[i];
a[i] = a[n-i-1];
a[n-i-1] = i8;
}
break;
default:
tot = n*elsz;
if (elsz < 4097)
temp = alloca(elsz);
else
temp = malloc(elsz);
if (temp != NULL) {
for(i=0, eli=0; i < n/2; i++, eli+=elsz) {
memcpy(temp, &a[eli], elsz);
memcpy(&a[eli], &a[tot-eli-elsz], elsz);
memcpy(&a[tot-eli-elsz], temp, elsz);
}
if (elsz >= 4097)
free(temp);
}
break;
}
}
void memreverse_to(char *dest, char *a, size_t n, size_t elsz)
{
int64_t *pi64, *di64;
int32_t *pi32, *di32;
int16_t *pi16, *di16;
size_t i;
size_t eli, tot;
if (n==0 || elsz==0) return;
switch(elsz) {
case 16:
pi64 = (int64_t*)a;
di64 = (int64_t*)dest;
for(i=0; i < n/2; i++) {
di64[2*i] = pi64[2*(n-i-1)];
di64[2*(n-i-1)] = pi64[2*i];
di64[2*i+1] = pi64[2*(n-i-1)+1];
di64[2*(n-i-1)+1] = pi64[2*i+1];
}
if (n&0x1) {
di64[2*i] = pi64[2*i];
di64[2*i+1] = pi64[2*i+1];
}
break;
case 8:
pi64 = (int64_t*)a;
di64 = (int64_t*)dest;
for(i=0; i < n/2; i++) {
di64[i] = pi64[n-i-1];
di64[n-i-1] = pi64[i];
}
if (n&0x1)
di64[i] = pi64[i];
break;
case 4:
pi32 = (int32_t*)a;
di32 = (int32_t*)dest;
for(i=0; i < n/2; i++) {
di32[i] = pi32[n-i-1];
di32[n-i-1] = pi32[i];
}
if (n&0x1)
di32[i] = pi32[i];
break;
case 2:
pi16 = (int16_t*)a;
di16 = (int16_t*)dest;
for(i=0; i < n/2; i++) {
di16[i] = pi16[n-i-1];
di16[n-i-1] = pi16[i];
}
if (n&0x1)
di16[i] = pi16[i];
break;
case 1:
for(i=0; i < n/2; i++) {
dest[i] = a[n-i-1];
dest[n-i-1] = a[i];
}
if (n&0x1)
dest[i] = a[i];
break;
default:
tot = n*elsz;
for(i=0, eli=0; i < n/2; i++, eli+=elsz) {
memcpy(&dest[eli], &a[tot - eli - elsz], elsz);
memcpy(&dest[tot - eli - elsz], &a[eli], elsz);
}
if (n&0x1)
memcpy(&dest[eli], &a[eli], elsz);
break;
}
}
void bswap_buffer(byte_t *data, size_t sz, size_t npts)
{
size_t i, b;
byte_t *el;
byte_t temp;
if (sz <= 1)
return;
switch (sz) {
case 8:
for(i=0; i < npts; i++) {
((u_int64_t*)data)[i] = bswap_64(((u_int64_t*)data)[i]);
}
break;
case 4:
for(i=0; i < npts; i++) {
((u_int32_t*)data)[i] = bswap_32(((u_int32_t*)data)[i]);
}
break;
case 2:
for(i=0; i < npts; i++) {
((u_int16_t*)data)[i] = bswap_16(((u_int16_t*)data)[i]);
}
break;
default:
for(i=0; i < sz * npts; i += sz) {
el = data + i;
for(b=0; b < sz/2; b++) {
temp = el[b];
el[b] = el[sz-b-1];
el[sz-b-1] = temp;
}
}
}
}
void bswap(byte_t *s, size_t n)
{
unsigned int i;
char temp;
switch (n) {
case 8:
*(u_int64_t*)s = bswap_64(*(u_int64_t*)s); break;
case 4:
*(u_int32_t*)s = bswap_32(*(u_int32_t*)s); break;
case 2:
*(u_int16_t*)s = bswap_16(*(u_int16_t*)s); break;
case 1:
break;
default:
for(i=0; i < n/2; i++) {
temp = s[i];
s[i] = s[n-i-1];
s[n-i-1] = temp;
}
}
}
void bswap_to(byte_t *dest, byte_t *src, size_t n)
{
unsigned int i;
switch (n) {
case 8:
*(u_int64_t*)dest = bswap_64(*(u_int64_t*)src); break;
case 4:
*(u_int32_t*)dest = bswap_32(*(u_int32_t*)src); break;
case 2:
*(u_int16_t*)dest = bswap_16(*(u_int16_t*)src); break;
case 1:
break;
default:
for(i=0; i < n; i++) {
dest[i] = src[n-i-1];
}
}
}
#define ALIGNED_TO_ACTUAL(p) (((char*)p) - ((long*)p)[-1])
static void *aligned_ptr(char *ptr, size_t align_size)
{
char *ptr2, *aligned_ptr;
ptr2 = ptr + sizeof(long);
aligned_ptr = (char*)ALIGN(((uptrint_t)ptr2), align_size);
((long*)aligned_ptr)[-1] = (long)(aligned_ptr - ptr);
return aligned_ptr;
}
/* align_size has to be a power of two */
void *malloc_aligned(size_t size, size_t align_size)
{
char *ptr;
ptr = (char*)malloc(size + align_size-1 + sizeof(long));
if (ptr == NULL)
return NULL;
return aligned_ptr(ptr, align_size);
}
void free_aligned(void *ptr)
{
free(ALIGNED_TO_ACTUAL(ptr));
}
void *realloc_aligned(void *ptr, size_t size, size_t align_size)
{
char *pnew;
if (ptr != NULL)
ptr = ALIGNED_TO_ACTUAL(ptr);
pnew = realloc(ptr, size + align_size-1 + sizeof(long));
if (pnew == NULL)
return NULL;
return aligned_ptr(pnew, align_size);
}