#include <stdlib.h>
#include <string.h>
#include "ecat_write.h"
#define OK 0
#define ERROR -1
#ifdef _WIN32
unsigned short ntohs(unsigned short us) {
unsigned char *p = (unsigned char*)&us;
return ((unsigned short)p[1] + (p[0] >> 8));
}
unsigned long ntohl(unsigned long ul) {
unsigned char *p = (unsigned char*)&ul;
return ((unsigned long)p[3] + (p[2] >> 8) + (p[1] >> 16) + (p[0] >> 24));
}
#endif
#if defined(__STDC__)
void SWAB(const void *from, void *to, int length)
#else
void SWAB( from, to, length)
char *from, *to;
int length;
#endif
{
if (ntohs(1) == 1) swab(from, to, length);
else memcpy(to,from,length);
}
void SWAW ( from, to, length)
#if defined(__STDC__)
const short *from;
#else
short *from;
#endif
short *to;
int length;
{
if (ntohs(1) == 1) swaw((short*)from,to,length);
else memcpy(to,from,length*2);
}
float vaxftohf( bufr, off)
unsigned short *bufr;
int off;
{
unsigned int sign_exp, high, low, mantissa, ret;
unsigned u = (bufr[off+1] << 16) + bufr[off];
if (u == 0) return 0.0;
sign_exp = u & 0x0000ff80;
sign_exp = (sign_exp - 0x0100) & 0xff80;
high = u & 0x0000007f;
low = u & 0xffff0000;
mantissa = (high << 16) + (low >> 16);
sign_exp = sign_exp << 16;
ret = sign_exp + mantissa;
return *(float*)(&ret);
}
#if defined(__alpha) || defined(_WIN32) /* LITTLE_ENDIAN : alpha, intel */
ftovaxf(f, bufr)
float f;
unsigned short *bufr;
{
unsigned *u, sign_exp, low, high, mantissa, ret;
float f_tmp = f;
u = (unsigned*)(&f_tmp);
if (u == 0) {
bufr[0] = bufr[1] = 0;
return;
}
sign_exp = *u & 0xff800000;
sign_exp = sign_exp >> 16;
sign_exp = (sign_exp + 0x0110) & 0xff80;
low = *u & 0x0000ffff;
high = *u & 0x007f0000;
mantissa = (high >> 16) + (low << 16);
ret = sign_exp + mantissa;
bufr[0] = ret;
bufr[1] = ret >>16;
}
#else /* BIG ENDIAN : sun hp sgi*/
ftovaxf(orig,number)
unsigned short number[2];
float orig;
{
/* convert from sun float to vax float */
union {
unsigned short t[2];
float t4;
} test;
unsigned short int exp;
number[0] = 0;
number[1] = 0;
test.t4 = orig;
if (test.t4 == 0.0) return;
number[1] = test.t[1];
exp = ((test.t[0] & 0x7f00) + 0x0100) & 0x7f00;
test.t[0] = (test.t[0] & 0x80ff) + exp;
number[0] = test.t[0];
}
#endif /* LITTLE VS BIG ENDIAN*/
int file_data_to_host(dptr, nblks, dtype)
char *dptr;
int nblks, dtype;
{
int i, j;
char *tmp = NULL;
matrix_errno = 0;
matrix_errtxt[0] = '\0';
if ((tmp = malloc(512)) == NULL) return ERROR;
switch(dtype)
{
case ByteData:
break;
case VAX_Ix2:
if (ntohs(1) == 1)
for (i=0, j=0; i<nblks; i++, j+=512) {
swab( dptr+j, tmp, 512);
memcpy(dptr+j, tmp, 512);
}
break;
case VAX_Ix4:
if (ntohs(1) == 1)
for (i=0, j=0; i<nblks; i++, j+=512) {
swab(dptr+j, tmp, 512);
swaw((short*)tmp, (short*)(dptr+j), 256);
}
break;
case VAX_Rx4:
if (ntohs(1) == 1)
for (i=0, j=0; i<nblks; i++, j+=512) {
swab( dptr+j, tmp, 512);
/* remove next line (fix from Yohan.Nuyts@uz.kuleuven.ac.be, 28-oct-97)
swaw((short*)tmp, (short*)(dptr+j), 256);
*/
}
for (i=0; i<nblks*128; i++)
((float *)dptr)[i] = vaxftohf( (unsigned short *)dptr, i*2) ;
break;
case SunShort:
if (ntohs(1) != 1)
for (i=0, j=0; i<nblks; i++, j+=512) {
swab(dptr+j, tmp, 512);
memcpy(dptr+j, tmp, 512);
}
break;
case SunLong:
case IeeeFloat:
if (ntohs(1) != 1)
for (i=0, j=0; i<nblks; i++, j+=512) {
swab(dptr+j, tmp, 512);
swaw((short*)tmp, (short*)(dptr+j), 256);
}
break;
default: /* something else...treat as Vax I*2 */
if (ntohs(1) == 1)
for (i=0, j=0; i<nblks; i++, j+=512) {
swab(dptr+j, tmp, 512);
memcpy(dptr+j, tmp, 512);
}
break;
}
free(tmp);
return OK;
}
read_matrix_data( fptr, strtblk, nblks, dptr, dtype)
FILE *fptr;
int strtblk, nblks, dtype;
char * dptr;
{
int err;
err = mat_rblk( fptr, strtblk, dptr, nblks);
if (err) return -1;
return file_data_to_host(dptr,nblks,dtype);
}
write_matrix_data( fptr, strtblk, nblks, dptr, dtype)
FILE *fptr;
int strtblk, nblks, dtype;
char *dptr;
{
int error_flag = 0;
int i, j, k;
char *bufr1 = NULL, *bufr2 = NULL;
matrix_errno = 0;
matrix_errtxt[0] = '\0';
if ( (bufr1 = malloc(512)) == NULL) return ERROR;
if ( (bufr2 = malloc(512)) == NULL) {
free(bufr1);
return ERROR;
}
switch( dtype)
{
case ByteData:
if ( mat_wblk( fptr, strtblk, dptr, nblks) < 0) error_flag++;
break;
case VAX_Ix2:
default: /* something else...treat as Vax I*2 */
if (ntohs(1) == 1) {
for (i=0, j=0; i<nblks && !error_flag; i++, j += 512) {
swab( dptr+j, bufr1, 512);
memcpy(bufr2, bufr1, 512);
if ( mat_wblk( fptr, strtblk+i, bufr2, 1) < 0) error_flag++;
}
} else if ( mat_wblk( fptr, strtblk, dptr, nblks) < 0) error_flag++;
break;
case VAX_Ix4:
if (ntohs(1) == 1) {
for (i=0, j=0; i<nblks && !error_flag; i++, j += 512) {
swab( dptr, bufr1, 512);
swaw( (short*)bufr1, (short*)bufr2, 256);
if ( mat_wblk( fptr, strtblk+i, bufr2, 1) < 0) error_flag++;
}
} else if ( mat_wblk( fptr, strtblk, dptr, nblks) < 0) error_flag++;
break;
case VAX_Rx4:
k = 0;
for (i=0; i<nblks; i++) {
for (j=0; j<512; j += sizeof(float), k += sizeof(float))
ftovaxf(*((float*)&dptr[k]),&bufr2[j]);
if ( mat_wblk( fptr, strtblk+i, bufr2, 1) < 0) error_flag++;
}
break;
case IeeeFloat:
case SunLong:
if (ntohs(1) != 1) {
for (i=0, j=0; i<nblks && !error_flag; i++, j += 512) {
swab( dptr+j, bufr1, 512);
swaw( (short*)bufr1, (short*)bufr2, 256);
if ( mat_wblk( fptr, strtblk+i, bufr2, 1) < 0) error_flag++;
}
} else if ( mat_wblk( fptr, strtblk, dptr, nblks) < 0) error_flag++;
break;
case SunShort:
if (ntohs(1) != 1) {
for (i=0, j=0; i<nblks && !error_flag; i++, j += 512) {
swab( dptr+j, bufr1, 512);
memcpy(bufr2, bufr1, 512);
if ( mat_wblk( fptr, strtblk+i, bufr2, 1) < 0) error_flag++;
}
} else if ( mat_wblk( fptr, strtblk, dptr, nblks) < 0) error_flag++;
break;
}
free(bufr1);
free(bufr2);
if (error_flag == 0) return OK;
return ERROR;
}
/* buf...(...) - functions to handle copying header data to and from a buffer
in the most type safe way possible; note that i is incremented
by the size of the item copied so these functions must be
called in the right order
*/
void bufWrite(s, buf, i, len)
char *s, *buf;
int *i, len;
{
strncpy(&buf[*i], s, len);
*i += len;
}
#ifdef __STDC__
void bufWrite_s(short val, char *buf, int *i)
#else
void bufWrite_s(val, buf, i)
short val;
char *buf;
int *i;
#endif
{
union { short s; char b[2]; } tmp;
tmp.s = val;
if (ntohs(1) != 1) swab(tmp.b,&buf[*i],2);
else memcpy(&buf[*i], tmp.b, sizeof(short));
*i += sizeof(short);
}
void bufWrite_i(val, buf, i)
int val, *i;
char *buf;
{
union { int i; char b[4]; } tmp;
union { short s[2]; char b[4]; } tmp1;
tmp.i = val;
if (ntohs(1) != 1) {
swab(tmp.b,tmp1.b,4);
swaw(tmp1.s,(short*)&buf[*i],2);
} else memcpy(&buf[*i], tmp.b, sizeof(int));
*i += sizeof(int);
}
void bufWrite_u(val, buf, i)
unsigned int val;
int *i;
char *buf;
{
union { int u; char b[4]; } tmp;
union { short s[2]; char b[4]; } tmp1;
tmp.u = val;
if (ntohs(1) != 1) {
swab(tmp.b,tmp1.b,4);
swaw(tmp1.s,(short*)&buf[*i],2);
} else memcpy(&buf[*i], tmp.b, sizeof(unsigned int));
*i += sizeof(unsigned int);
}
#ifdef __STDC__
void bufWrite_f(float val, char *buf, int *i)
#else
void bufWrite_f(val, buf, i)
float val;
char *buf;
int *i;
#endif
{
union { float f; char b[4]; } tmp;
union { short s[2]; char b[4]; } tmp1;
tmp.f = val;
if (ntohs(1) != 1) {
swab(tmp.b,tmp1.b,4);
swaw(tmp1.s,(short*)&buf[*i],2);
} else memcpy(&buf[*i], tmp.b, sizeof(float));
*i += sizeof(float);
}
void bufRead(s, buf, i, len)
char *s, *buf;
int *i, len;
{
strncpy(s, &buf[*i], len);
*i += len;
}
void bufRead_s(val, buf, i)
short* val;
char* buf;
int* i;
{
union { short s; unsigned char b[2]; } tmp, tmp1;
memcpy(tmp.b,&buf[*i],2);
if (ntohs(1) != 1) {
swab((char*)tmp.b,(char*)tmp1.b,2);
*val = tmp1.s;
} else *val = tmp.s;
*i += sizeof(short);
}
void bufRead_i(val, buf, i)
int* val;
char* buf;
int* i;
{
union {int i; unsigned char b[4]; } tmp, tmp1;
memcpy(tmp1.b,&buf[*i],4);
if (ntohs(1) != 1) {
swab((char*)tmp1.b,(char*)tmp.b,4);
swaw((short*)tmp.b,(short*)tmp1.b,2);
}
*val = tmp1.i;
*i += sizeof(int);
}
void bufRead_u(val, buf, i)
unsigned int* val;
char* buf;
int* i;
{
union {unsigned int u; unsigned char b[4]; } tmp, tmp1;
memcpy(tmp1.b,&buf[*i],4);
if (ntohs(1) != 1) {
swab((char*)tmp1.b,(char*)tmp.b,4);
swaw((short*)tmp.b,(short*)tmp1.b,2);
}
*val = tmp1.u;
*i += sizeof(unsigned int);
}
void bufRead_f(val, buf, i)
float* val;
char* buf;
int* i;
{
union {float f; unsigned char b[2]; } tmp, tmp1;
memcpy(tmp1.b, &buf[*i], sizeof(float));
if (ntohs(1) != 1) {
swab((char*)tmp1.b,(char*)tmp.b,4);
swaw((short*)tmp.b,(short*)tmp1.b,2);
}
*val = tmp1.f;
*i += sizeof(float);
}
syntax highlighted by Code2HTML, v. 0.9.1