Subversion Repositories pc_fpga_com

/*

 * Copyright (C) 2011 Simon A. Berger

 *

 *  This program is free software; you may redistribute it and/or modify its

 *  under the terms of the GNU Lesser General Public License as published by the Free

 *  Software Foundation; either version 2 of the License, or (at your option)

 *  any later version.

 *

 *  This program 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.

 */

#include <stdio.h>

#include <unistd.h>

#include <stdlib.h>

#include <string.h>

#include <assert.h>

#include "fpga_com.h"

#include <sys/socket.h>

#include <netinet/in.h>

#include <arpa/inet.h>

const size_t MAX_MTU = 64 * 1024;

static void die_perror( const char *call ) {

    perror( call );

    exit(-1);

}

void fpga_con_init( fpga_con_t *con, const void *daddr, int lport, int dport ) {

    /*set up local socket address*/

    memset( &con->l_sockaddr, 0, sizeof( con->l_sockaddr ));

    con->l_sockaddr.sin_family = AF_INET;

    con->l_sockaddr.sin_addr.s_addr = INADDR_ANY;

    con->l_sockaddr.sin_port = htons(lport);

    /*setup dest socket address*/

    memset( &con->d_sockaddr, 0, sizeof( con->d_sockaddr ));

    con->d_sockaddr.sin_family = AF_INET;

    con->d_sockaddr.sin_addr.s_addr = inet_addr( daddr );

    con->d_sockaddr.sin_port = htons(dport);

/*    create my socket*/

    con->s = socket( AF_INET, SOCK_DGRAM, IPPROTO_UDP );

    if( con->s < 0 ) {

        die_perror( "socket" );

    }

/*    bind it to local socket*/

    int r = bind( con->s, (struct sockaddr *)&con->l_sockaddr, sizeof(con->l_sockaddr));

    if( r < 0 ) {

        die_perror( "bind" );

    }

    con->mtu = 1500;

}

size_t fpga_con_get_mtu( fpga_con_t *con ) {

    return con->mtu;

}

void fpga_con_set_mtu( fpga_con_t *con, size_t mtu ) {

    if( mtu > MAX_MTU ) {

        printf( "fpga_con_set_mtu: mtu > MAX_MTU\n" );

        exit(-1);

    }

    con->mtu = mtu;

}

ssize_t fpga_con_send( fpga_con_t *con, const void *buf, size_t len ) {

    ssize_t rsend = sendto( con->s, buf, len, 0, (struct sockaddr*)&con->d_sockaddr, sizeof(con->d_sockaddr));

    if( rsend < 0 ) {

        die_perror( "sendto" );

    }

    return rsend;

}

/* send lut initialization packet sequence: rst, conf, empty*/

void fpga_con_send_init_packet( fpga_con_t *con ) {

    uint8_t buf[4];

    buf[0] = 255;

    fpga_con_send( con, buf, 1);

    buf[0] = 15;

    fpga_con_send( con, buf, 1);

    fpga_con_send( con, buf, 0);

}

/*blocking receive: this call will block until a packet is received*/

ssize_t fpga_con_block_recv( fpga_con_t *con, void *dbuf, size_t dsize ) {

    ssize_t rrecv = recv( con->s, dbuf, dsize, 0 );

    if( rrecv < 0 ) {

        die_perror( "recv" );

    }

    return rrecv;

}

static __inline size_t mymin( size_t a, size_t b ) {

    return a < b ? a : b;

}

//const static size_t MTU = 1500;

const static size_t PH_SIZE = 1;

// static __inline size_t pack( uint8_t *buf, uint8_t ht, size_t buf_size, void *src, size_t src_size ) {

//     assert( src_size + PH_SIZR <= buf_size );

//     

//     buf[0] = ht;

//     memcpy( buf + PH_SIZE, src, src_size );

//     

//     return src_size + PH_SIZE;

// }

// TODO; check how large the impact of doing the byte swappin on unaligned values really is

// there is no simple way around it (like in the receiving direction).

static __inline void swappy_16( void *dest, void *src, size_t n ) {

    uint16_t *idest = (uint16_t*)dest;

    uint16_t *isrc = (uint16_t*)src;

    for( size_t i = 0; i < n / 2; i++ ) {

        idest[i] = __bswap_16(isrc[i]);

    }

}

static __inline void swappy_32( void *dest, void *src, size_t n ) {

    uint32_t *idest = (uint32_t*)dest;

    uint32_t *isrc = (uint32_t*)src;

    for( size_t i = 0; i < n / 4; i++ ) {

        idest[i] = __bswap_32(isrc[i]);

    }

}

static __inline void swappy_64( void *dest, void *src, size_t n ) {

    uint64_t *idest = (uint64_t*)dest;

    uint64_t *isrc = (uint64_t*)src;

    for( size_t i = 0; i < n / 8; i++ ) {

        idest[i] = __bswap_64(isrc[i]);

    }

}

#define BS_NONE  (0)

#define BS_16  (1)

#define BS_32  (2)

#define BS_64  (3)

static __inline size_t pack_and_send( fpga_con_t *con, uint8_t *buf, size_t buf_size, uint8_t ht, void *src, size_t src_size, int swap ) {

    const size_t pack_size = src_size + PH_SIZE;

    assert( pack_size <= buf_size );

    buf[0] = ht;

    switch( swap ) {

    case BS_NONE:

        memcpy( buf + PH_SIZE, src, src_size );

        break;

    case BS_16:

        swappy_16( buf + PH_SIZE, src, src_size );

        break;

    case BS_32:

        swappy_32( buf + PH_SIZE, src, src_size );

        break;

    case BS_64:

        swappy_64( buf + PH_SIZE, src, src_size );

        break;

    default:

        assert(0);

    }

    fpga_con_send( con, buf, pack_size );

    return src_size + PH_SIZE;

}

int fpga_con_send_charv( fpga_con_t *con, char *buf, size_t n ) {

    const size_t blocksize = (con->mtu - PH_SIZE);

    uint8_t sb[MAX_MTU];

    size_t sent = 0;

    while( sent < n ) {

        const size_t to_copy = mymin( blocksize, n - sent );

        pack_and_send( con, sb, con->mtu, FPC_CODE_CHAR, (void*) &buf[sent], to_copy, BS_NONE );

        sent += to_copy;

    }

    return 1;

}

int fpga_con_send_shortv( fpga_con_t *con, int16_t *buf, size_t n ) {

    const size_t TSIZE = sizeof( short );

    const size_t blocksize = (con->mtu - PH_SIZE) / TSIZE;

    uint8_t sb[MAX_MTU];

    size_t sent = 0;

    while( sent < n ) {

        const size_t to_copy = mymin( blocksize, n - sent );

        //fpga_con_send( con, (void*) &buf[sent], to_copy * TSIZE );

        pack_and_send( con, sb, con->mtu, FPC_CODE_SHORT, (void*) &buf[sent], to_copy * TSIZE, BS_16 );

        sent += to_copy;

    }

    return 1;

}

int fpga_con_send_intv( fpga_con_t *con, int32_t *buf, size_t n ) {

    const size_t TSIZE = sizeof( int );

    const size_t blocksize = (con->mtu - PH_SIZE) / TSIZE;

    uint8_t sb[MAX_MTU];

    size_t sent = 0;

    while( sent < n ) {

        const size_t to_copy = mymin( blocksize, n - sent );

//         fpga_con_send( con, (void*) &buf[sent], to_copy * TSIZE );

        pack_and_send( con, sb, con->mtu, FPC_CODE_INT, (void*) &buf[sent], to_copy * TSIZE, BS_32 );

        sent += to_copy;

    }

    return 1;

}

int fpga_con_send_longv( fpga_con_t *con, int64_t *buf, size_t n ) {

    const size_t TSIZE = sizeof( int64_t );

    const size_t blocksize = (con->mtu - PH_SIZE) / TSIZE;

    uint8_t sb[MAX_MTU];

    size_t sent = 0;

    while( sent < n ) {

        const size_t to_copy = mymin( blocksize, n - sent );

//         fpga_con_send( con, (void*) &buf[sent], to_copy * TSIZE );

        pack_and_send( con, sb, con->mtu, FPC_CODE_LONG, (void*) &buf[sent], to_copy * TSIZE, BS_64 );

        sent += to_copy;

    }

    return 1;

}

int fpga_con_send_floatv( fpga_con_t *con, float *buf, size_t n ) {

    const size_t TSIZE = sizeof( int );

    const size_t blocksize = (con->mtu - PH_SIZE) / TSIZE;

    uint8_t sb[MAX_MTU];

    size_t sent = 0;

    while( sent < n ) {

        const size_t to_copy = mymin( blocksize, n - sent );

//         fpga_con_send( con, (void*) &buf[sent], to_copy * TSIZE );

        pack_and_send( con, sb, con->mtu, FPC_CODE_FLOAT, (void*) &buf[sent], to_copy * TSIZE, BS_32 );

        sent += to_copy;

    }

    return 1;

}

int fpga_con_send_doublev( fpga_con_t *con, double *buf, size_t n ) {

    const size_t TSIZE = sizeof( double );

    const size_t blocksize = (con->mtu - PH_SIZE) / TSIZE;

    uint8_t sb[MAX_MTU];

    size_t sent = 0;

    while( sent < n ) {

        const size_t to_copy = mymin( blocksize, n - sent );

//         fpga_con_send( con, (void*) &buf[sent], to_copy * TSIZE );

        pack_and_send( con, sb, con->mtu, FPC_CODE_DOUBLE, (void*) &buf[sent], to_copy * TSIZE, BS_64 );

        sent += to_copy;

    }

    return 1;

}

static void fpga_con_rpack( fpga_con_t *con, int code, int size ) {

    char buf[3] = { 120, 0, 0 };

    buf[0] += code;

    assert( size > 0 && size < 0xffff );

    unsigned short ssize = size;

    swappy_16( &buf[1], &ssize, 2);

    fpga_con_send(con, buf, 3);

}

void fpga_con_rpack_char( fpga_con_t *con, int size ) {

    fpga_con_rpack( con, FPC_CODE_CHAR, size );

}

void fpga_con_rpack_short( fpga_con_t *con, int size ) {

    fpga_con_rpack( con, FPC_CODE_SHORT, size );

}

void fpga_con_rpack_int( fpga_con_t *con, int size ) {

    fpga_con_rpack( con, FPC_CODE_INT, size);

}

void fpga_con_rpack_long( fpga_con_t *con, int size ) {

    fpga_con_rpack( con, FPC_CODE_LONG, size);

}

void fpga_con_rpack_float( fpga_con_t *con, int size ) {

    fpga_con_rpack( con, FPC_CODE_FLOAT, size );

}

void fpga_con_rpack_double( fpga_con_t *con, int size ) {

    fpga_con_rpack( con, FPC_CODE_DOUBLE, size );

}