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 <cstdio>
#include <csignal>
#include <sys/socket.h>
#include <boost/thread/locks.hpp>
#include <endian.h>
 
#include "background_reader.h"
#include "fpga_com.h"
 
static void handler( int signal ) {
    //printf( "signal: %d\n", signal );
}
 
background_reader::background_reader(int s, size_t size)
        : m_stop(false),
        m_socket(s),
        m_max_size(size),
        m_pq_mem(0),
        m_pq_max_mem(0),
        m_pq_max_depth(0),
        N_THREADS(1),
        m_run_barrier(N_THREADS + 1),
        m_threads_joined(false)
        //m_native_handle(0)
{
 
}
 
background_reader::~background_reader()
{
    if( !m_threads_joined ) {
        printf( "WARNING: background_reader::~background_reader(): threads not joined!\n" );
    }
}
 
 
void background_reader::start() {
    /// m_thread.reset( new boost::thread( boost::bind(&background_reader::run, this)));
 
 
 
    printf( "starting %zd background reader threads\n", N_THREADS );
    //boost::barrier run_barrier( N_THREADS + 1 );
 
    while( m_thread_group.size() < N_THREADS ) {
        m_thread_group.create_thread(boost::bind(&background_reader::run, this ));
    }
 
    m_run_barrier.wait();
}
 
void background_reader::run()
{
    // BIG-UGLY-KLUDGE-WARNING!
    // we have to install a signal handler, in order to disable the auto-restart behaviour of the recv call...
    // This is necessary to make the recv call interuptable (= return -1 on interrupt). Otherwise the
    // bg threads would either wait forever, or we would need at least two syscalls per recv (any solution that involves select).
    //
    // the 'handler' does nothing, as we are only interested in interrupting the recv...
 
    struct sigaction sa;
    sa.sa_handler = handler;
    sigemptyset(&sa.sa_mask);
    sa.sa_flags = 0;
 
    sigaction( SIGUSR1, &sa, 0 );
 
    //m_native_handle = pthread_self();
 
    {
        lock_guard_t lock( m_nh_mtx );
        m_native_handles.push_back( pthread_self() );
 
    }
 
    const size_t MTU = 64 * 1024;
 
    std::vector<char>bufs(MTU);
    char *buf = bufs.data();
 
    //run_barrier->wait();
    //run_barrier = 0;
    m_run_barrier.wait();
 
    while ( !m_stop ) {
        ssize_t s = recv( m_socket, buf, MTU, 0 );
 
//         if( s < 1024 && s != -1 ) {
//             printf( "recved term packet %p\n", (void*)pthread_self() );
//         }
        //printf( "recv: %zd\n", s );
        if ( s == -1 && errno == EINTR ) {
             printf( "interrupted\n" );
 
        } else {
            lock_guard_t lock( m_pq_mtx );
            m_pq.push_back(std::string(buf, buf+s));
            m_pq_mem += s;
            m_pq_max_mem = std::max( m_pq_mem, m_pq_max_mem );
 
 
            m_pq_max_depth = std::max( m_pq.size(), m_pq_max_depth );
        }
//         printf( "notify\n" );
        m_can_read_condition.notify_one();
    }
 
    printf( "background reader thread exit\n" );
}
 
ssize_t background_reader::block_recv(void* buf, size_t size)
{
    boost::unique_lock<mutex_t> lock( m_pq_mtx );
 
    while ( ! m_pq.size() > 0 && !m_stop ) {
//         printf( "cond_wait\n" );
        m_can_read_condition.wait(lock);
    }
 
    if( m_pq.size() == 0 ) {
        return -1;
    }
 
    std::string &pbuf = m_pq.front();
 
    m_pq_mem -= pbuf.size();
 
 
    ssize_t s = std::min( size, pbuf.size() );
    std::copy( pbuf.begin(), pbuf.end(), (char *)buf );
    m_pq.pop_front();
 
    return s;
}
 
 
ssize_t background_reader::poll()
{
    lock_guard_t lock( m_pq_mtx );
    if( m_pq.size() > 0 ) {
        return m_pq.front().size();
    } else {
        return -1;
    }
}
 
 
void background_reader::interrupt() {
    //boost::thread::native_handle_type h = m_thread->native_handle();
   // pthread_kill( m_native_handle, SIGUSR1 );
 
    lock_guard_t lock( m_nh_mtx );
    for( std::vector<pthread_t>::iterator it = m_native_handles.begin(); it != m_native_handles.end(); ++it ) {
        pthread_kill( *it, SIGUSR1 );
    }
 
}
 
void background_reader::join() {
   // m_thread->join();
   m_thread_group.join_all();
 
   printf( "pq max size (bytes): %zd\n", m_pq_max_mem );
   printf( "pq max depth (#packets): %zd\n", m_pq_max_depth );
 
   m_threads_joined = true;
}
 
ssize_t background_reader::purge()
{
    lock_guard_t lock( m_pq_mtx );
 
    ssize_t s = m_pq.size();
    m_pq.clear();
 
    return s;
}
 
 
 
// int mainx() {
//     fpga_con_t fc;
// 
//     fpga_con_init( &fc, "131.159.28.113", 12340, 12350 );
// 
// 
// 
//     background_reader bgr( fc.s, 1024 * 1024 * 10 );
// 
//     bgr.start();
// 
//     char buf[1024];
//     memset( buf, 0, 1024 );
//     
//     
//     const size_t rbuf_size = 10 * 1024;
//     char rbuf[rbuf_size];
// 
//     printf( "sleep\n" );
//   //  usleep( 2000000 );
//     printf( "close\n" );
//     //close( fc.s );
// 
//     //bgr.stop();
//     //bgr.interrupt();
//     //getchar();
// 
//    // bgr.join();
// 
//     printf( "joined\n" );
//     size_t n = 0;
//     for ( int i = 0; i < 10; i++ ) {
//         fpga_con_send( &fc, buf, 1024 );
//         printf( "sent\n" );
// 
// 
//         
//         while (true) {
//             size_t s = bgr.block_recv( rbuf, rbuf_size );
//             n++;
// //             printf( "recv: %zd\n", s );
//             if ( s < 1024 ) {
//                 break;
//             }
//         }
// 
//         printf( "recved: %zd\n", n );
//     }
//     
//     bgr.stop();
//     bgr.interrupt();
//     bgr.join();
//     printf( "bg reader joined. exit.\n" );
// }
 
 
 
static inline background_reader &get_bgr( fpga_bgr_t *cbgr ) {
    assert( cbgr != 0 );
    assert( cbgr->bgr_cpp != 0 );
    return *(static_cast<background_reader*>(cbgr->bgr_cpp));
}
 
void fpga_bgr_init( fpga_bgr_t *cbgr, int socket, size_t size ) {
    memset( cbgr, 0, sizeof( fpga_bgr_t ));
 
    background_reader *bgr = new background_reader(socket, size);
    cbgr->bgr_cpp = bgr;
}
 
void fpga_bgr_delete( fpga_bgr_t *cbgr ) {
 
    background_reader *bgr = static_cast<background_reader*>(cbgr->bgr_cpp);
    delete bgr;
 
}
 
void fpga_bgr_start( fpga_bgr_t *cbgr) {
    background_reader &bgr = get_bgr(cbgr);
    bgr.start();
}
ssize_t fpga_bgr_block_recv( fpga_bgr_t *cbgr, void *buf, size_t size ) {
    background_reader &bgr = get_bgr(cbgr);
 
    return bgr.block_recv(buf, size);
}
ssize_t fpga_bgr_poll( fpga_bgr_t *cbgr ) {
    background_reader &bgr = get_bgr(cbgr);
 
    return bgr.poll();
 
}
void fpga_bgr_stop_interrupt_join( fpga_bgr_t *cbgr) {
    background_reader &bgr = get_bgr(cbgr);
 
    bgr.stop();
    bgr.interrupt();
    bgr.join();
}
 
 
// template <typename T>
// inline static T swap_endian( T &v ) {
//     
//     T vo = v;
//     uint8_t *vb = (uint8_t*) &vo;
//     
//     for( size_t i = 0; i < sizeof(T) / 2; i++ ) {
//         std::swap(vb[i], vb[sizeof(T) - i - 1]);
//         
//     }
//     return vo;
// }
 
 
template<typename T,size_t N>
struct swap_endian {
    inline T operator()( T &v ) {
 
        T vo = v;
        uint8_t *vb = (uint8_t*) &vo;
 
        for( size_t i = 0; i < sizeof(T) / 2; i++ ) {
            std::swap(vb[i], vb[sizeof(T) - i - 1]);
        }
        return vo;
    }
};
 
 
template<typename T>
struct swap_endian<T,2> {
    inline T operator()( T &v ) {
        uint16_t t = __bswap_16(*((uint16_t*)&v));
 
        T* r = (T*)&t; // we don't want to dereference a type-punned pointer, do we?
        return *r;
 
    }
};
 
 
template<typename T>
struct swap_endian<T,4> {
    inline T operator()( T &v ) {
        uint32_t t = __bswap_32(*((uint32_t*)&v));
 
        T* r = (T*)&t;
        return *r;
    }
};
 
template<typename T>
struct swap_endian<T,8> {
    inline T operator()( T &v ) {
        uint64_t t = __bswap_64(*((uint64_t*)&v));
 
        T* r = (T*)&t;
        return *r;
    }
};
 
 
 
 
 
template<typename T>
struct swappy_au {
    const static size_t N = sizeof(T);    
 
 
    swap_endian<T,N> swap;
    inline void operator()( void * dest, void * src, size_t n ) {
        std::copy( (char *)src, ((char *)src) + n * N, (char *)dest );
        //std::memcpy( dest, src, n * N );
 
        T * ptr = (T *)dest;
        T * end = ptr + n;
 
        // do the endian swapping in place in the aligned buffer
        while( ptr < end ) {
            *ptr = swap( *ptr );
            ptr++;
        }
 
    }
 
};
 
template<typename T>
struct swappy_aa {
    const static size_t N = sizeof(T);    
    swap_endian<T,N> swap;
    inline void operator()( void * dest, void * src, size_t n ) {
 
 
        T * sptr = (T *)src;
        T * ptr = (T *)dest;
        T * end = ptr + n;
 
        // copy and swap on the fly, assuming that both buffers are aligned
        while( ptr < end ) {
            *ptr = swap( *sptr );
            ptr++;
            sptr++;
        }
 
    }
 
};
 
 
template<typename T>
struct xerox_plain {
    const static size_t N = sizeof(T);    
    inline void operator()( void * dest, void * src, size_t n ) {
        std::copy( (char*)src, ((char*)src) + n * N, (char*)dest ); // using char* here to prevent std;:copy form making any assumptions about th alignment of src/dest
        //std::copy( (T*)src, ((T*)src) + n, (T*)dest );
    }
};
 
 
// template <typename T>
// inline T passthrough( T &v ) {
//  
//     return v;
// }
// 
 
template <typename T,class CopyF>
static bool fpga_bgr_recv_genv( fpga_bgr_t *cbgr, T *buf, size_t n, char ht ) {  
    background_reader &bgr = get_bgr(cbgr);
 
 
 
    T *buf_end = buf + n;
    T *ptr = buf;
 
    const size_t MPU = 9000;
    uint8_t rbuf[MPU];
 
    CopyF xerox;
 
    while( ptr < buf_end ) {
 
 
        ssize_t raw_size = bgr.block_recv(rbuf, MPU);
//         printf( "ptr: %p %zd %d %d %d %d\n", ptr, size, rbuf[0], rbuf[1], rbuf[2], rbuf[3] );
        assert( raw_size > 1 );
 
        if( rbuf[0] != ht ) {
            printf( "drop wrong packet type: %d %d\n", rbuf[0], ht );
        }
 
        ssize_t size = raw_size - 1;
        uint8_t *rptr = rbuf + 1;
 
        ssize_t ne = size / sizeof(T);
        ssize_t left = buf_end - buf;
 
        ssize_t to_copy = std::min( ne, left );
        xerox( ptr, rptr, to_copy );
 
 
        ptr += to_copy;
    }
 
    return true;
}
 
 
int fpga_bgr_recv_charv( fpga_bgr_t *bgr, char *buf, size_t n ) {
    return fpga_bgr_recv_genv<char,xerox_plain<char> >( bgr, buf, n, FPC_CODE_CHAR );
}
 
int fpga_bgr_recv_shortv( fpga_bgr_t *bgr, int16_t *buf, size_t n ) {
    return fpga_bgr_recv_genv<int16_t,swappy_au<int16_t> >( bgr, buf, n, FPC_CODE_SHORT );
}
 
int fpga_bgr_recv_intv( fpga_bgr_t *bgr, int32_t *buf, size_t n ) {
    return fpga_bgr_recv_genv<int32_t,swappy_au<int32_t> >( bgr, buf, n, FPC_CODE_INT );
}
 
int fpga_bgr_recv_longv( fpga_bgr_t *bgr, int64_t *buf, size_t n ) {
    return fpga_bgr_recv_genv<int64_t,swappy_au<int64_t> >( bgr, buf, n, FPC_CODE_LONG );
}
 
int fpga_bgr_recv_floatv( fpga_bgr_t *bgr, float *buf, size_t n ) {
    return fpga_bgr_recv_genv<float,swappy_au<float> >( bgr, buf, n, FPC_CODE_FLOAT );
}
 
int fpga_bgr_recv_doublev( fpga_bgr_t *bgr, double *buf, size_t n ) {
    //return fpga_bgr_recv_genv<double,xerox_plain<double,8> >( bgr, buf, n );
    return fpga_bgr_recv_genv<double,swappy_au<double> >( bgr, buf, n, FPC_CODE_DOUBLE );
}
 
#if 0
 
 
int main() {
    fpga_con_t fc;
 
    fpga_con_init( &fc, "131.159.28.113", 12340, 12350 );
 
    fpga_bgr_t bgr;
 
    fpga_bgr_init( &bgr, fc.s, 1024 * 1024 * 10 );
 
    fpga_bgr_start( &bgr );
 
    char buf[1024];
    memset( buf, 0, 1024 );
    int ibuf[1000];
    std::fill( ibuf, ibuf + 1000, 666 );
 
 
    const size_t rbuf_size = 10 * 1024;
    char rbuf[rbuf_size];
 
//     printf( "sleep\n" );
  //  usleep( 2000000 );
//     printf( "close\n" );
    //close( fc.s );
 
    //bgr.stop();
    //bgr.interrupt();
    //getchar();
 
   // bgr.join();
 
//     printf( "joined\n" );
    size_t n = 0;
    for ( int i = 0; i < 1; i++ ) {
        //fpga_con_send( &fc, buf, 1024 );
        fpga_con_send_intv( &fc, ibuf, 1000 );
        printf( "sent\n" );
 
        double iv[1000];
        bool succ = fpga_bgr_recv_doublev( &bgr, iv, 1000 );
 
        for( int i = 0; i < 1000; i++ ) {
           printf( "%.2f ", iv[i] );
            if( i % 20 == 19 ) {
                printf( "\n" );
            }
        }
        printf( "\n" );
 
        printf( "recved: %d\n", succ );
    }
 
    fpga_bgr_stop_interrupt_join( &bgr );
    printf( "bg reader joined. exit.\n" );
 
    fpga_bgr_delete( &bgr );
}
#endif