/* ======================================================================== *\
!
! *
! * This file is part of Stesy, the MAGIC Steering System
! * Software. It is distributed to you in the hope that it can be a useful
! * and timesaving tool in analysing Data of imaging Cerenkov telescopes.
! * It is distributed WITHOUT ANY WARRANTY.
! *
! * Permission to use, copy, modify and distribute this software and its
! * documentation for any purpose is hereby granted without fee,
! * provided that the above copyright notice appear in all copies and
! * that both that copyright notice and this permission notice appear
! * in supporting documentation. It is provided "as is" without express
! * or implied warranty.
! *
!
!
!   Author(s): Thomas Bretz <mailto:tbretz@uni-sw.gwdg.de>, 2001
!
!   Copyright: MAGIC Software Development, 2000-2001
!
!
\* ======================================================================== */

///////////////////////////////////////////////////////////////////////
//
// VmodIcan
//
// Class describing the interface to the Janz card in RawCan mode.
//
///////////////////////////////////////////////////////////////////////
#include "vmodican.h"

#include <iostream.h>      // cout
#include <iomanip.h>       // setw, setfill

#include <fcntl.h>         // O_RDONLY
#include <errno.h>         // errno
#include <unistd.h>        // read
#include <sys/time.h>      // gettimeofday
#include <sys/ioctl.h>     // ioctl

ClassImp(VmodIcan);

// --------------------------------------------------------------------------
//
// Prints a CAN Message.
//
void VmodIcan::PrintMsg(Message *m)
{
    cout << "Cmd=0x" << hex << (int)m->cmd << dec << " " << flush;
    cout << "len=" << (int)m->len << ":" << flush;

    cout << hex << flush;
    for (int i=0; i<m->len; i++)
        cout << " " << (int)m->data[i] << flush;
    cout << dec << endl;
}

// --------------------------------------------------------------------------
//
// Embedded ioctl-function from C-lib
//
int VmodIcan::Ioctl(int msg, void *arg)
{
    return fd<0 ? 1 : ioctl(fd, msg, (int)arg) >= 0;
}

// --------------------------------------------------------------------------
//
// Enables/Disables the termination.
//
void VmodIcan::SetTermination(int state)  /* 0 = off, 1 = on */
{
    /* -*-Func-*-
     *
     * SwitchCanTermination - Switch the CANbus termination resistor
     *
     * The VMOD-ICAN3 module allows the user to change the state of the CANbus
     * termination via software. This is done with this service.
     *
     * SERVICE: HwConf
     */

    Message msg;

    msg.cmd = M_HW_CONF;

    msg.len = 2;
    msg.data[0] = 0x00;
    msg.data[1] = (BYTE_t)state;

    while (!Send(&msg));

    lout << "- CAN Bus Termination set to " << (state?"on":"off") << endl;
}

// --------------------------------------------------------------------------
//
// Receiver Thread. Listener. Listens for incomming messages and processes
// these messages through the standard interface:
//  - therefor overload HandleCanMessge
//
void *VmodIcan::Thread()
{
    if (fd<0)
        return NULL;

    lout << "- Starting Receiver Loop." << endl;

    while (1)
    {
        //
        // Sleeps until a message arrives
        //
        unsigned char c;
        const int n = read(fd, &c, 1);

        if (n<0)
        {
            cerr << "Vmodican: read(" << dec << (int)fd << "," << (void*)&c;
            cerr << ",1) returned c=" << (int)c << " '" << strerror(errno);
            cerr << "' (errno = " << errno << ")" << endl;
            continue;
            //return (void *)1;
        }

        //
        // read the time for the message as soon as possible
        //
        timeval_t tv;
        gettimeofday(&tv, NULL);

        //
        // if n==0 something strange happened. Stop receiver(?)
        //
        if (n == 0)
        {
            cerr << "Vmodican: Panic read '" << strerror(errno) << "' ";
            cerr << "(errno=" << errno << ")" << endl;
            return (void *)1;
        }

        //
        // Check for what we received
        //
        switch (c)
        {
            //
            // Fast message (not used/working)
            //
        case FAST_QUEUE:
            cout << "--> Fast Queue:  " << flush;

            FastMessage fmsg;

            if (ReceiveFast(&fmsg) < 0)
                return (void *)1;

            cout << "Fast msg ID " <<
                (fmsg.data[0] << 3) + ((fmsg.data[1] >> 5) & 7) << ": " << flush;

            for(int i=0; i<16; i++)
                cout << (int)*(((unsigned char *)(&fmsg))+i) << " " << flush;

            cout << endl;
            continue;

        //
        // Plain Can Message to be processed
        //
        case PLAIN_QUEUE:

            Message msg;

            //
            // Read the message from the card and process it
            //
            if (Receive(&msg) < 0)
                return (void *)1;

            HandleMessage(&msg, &tv);
            continue;
        }

        cout << "Vmodican: read, Message c=" << (int)c << " unknown." << endl;
    }
    return NULL;
}

// --------------------------------------------------------------------------
//
// Does a basic message processing and hadles the so called command (cmd)
// stamp of the message. This is some kind of message type which is send
// by the can interface
//
void VmodIcan::HandleMessage(Message *msg, timeval_t *tv)
{
    //
    // Decode message
    //
    const WORD_t desc  = msg->data[2]<<8 | msg->data[3];
    const BYTE_t rtr   = (desc>>4)&1;
    const BYTE_t len   = desc&0xf;
    const WORD_t cobid = desc>>5;

    switch (msg->cmd) // FROM mican.h
    {
    case M_MSG_LOST:
        cout << "VmodIcan reports: " << dec << (int)msg->data[0] << " msg(s) lost!" << endl;
        return;

    case M_BCAN_TX_con:  /* confirm (+/-) transmission */
        cout << "VmodIcan reports: CTXcon=0x35" << endl;
        cout << "This normally means, that the transmission of the following CAN frame failed:" << hex << endl;
        cout << "Descr: 0x" << cobid << dec;
        cout << "  Rtr: "   << (rtr?"Yes":"No");
        cout << "  Len: "   << (int)len << endl;
        return;

    case M_BCAN_EVENT_ind:
        cout << "VmodIcan reports: CEVTind=0x37: " << hex;
        switch (msg->data[0]) // error indicator
        {
        case 0x01:
            cout << "Error interrup occured" << endl;
            cout << "This means noisy network normally. Please check the bus termination." << endl;
            switch (msg->data[1]) // msg type (board depending)
            {
            case 2: // SJA1000
                cout << dec;
                cout << "ModeReg=" << (int)msg->data[2] << ", ";
                cout << "StatReg=" << (int)msg->data[3] << ", ";
                cout << "RxErrCnt=" << (int)msg->data[4] << ", ";
                cout << "TxErrCnt=" << (int)msg->data[5] << endl;
            }
            //FIXME? TerminateApp();
            return;
        case 0x02:
            cout << "Overrun interrup occured" << endl;
            return;
        case 0x04:
            cout << "Interrupts lost" << endl;
            return;
        case 0x08:
            cout << "Send queue full" << endl;
            return;
        case 0x10:
            cout << "CANbus bus-error" << endl;
            return;
        }
        return;
    case M_BCAN_RX_ind:
        //
        // Message is a message from the Can bus
        //
        HandleCanMessage(cobid, &msg->data[4], tv);
        return;
    }

    //
    // Nothing of the above happened
    //
    cout << hex;
    cout << "Cmd=0x"    << (int)msg->cmd << ":  ";
    cout << "Descr: 0x" << cobid << dec;
    cout << "  Rtr: "   << (rtr?"Yes":"No");
    cout << "  Len: "   << (int)len << endl;

    cout << "As Raw Data:" << hex << setfill('0');
    for (int i=0; i<msg->len; i++)
        cout << " " << setw(2) << (int)(msg->data[i]) << flush;
    cout << endl;
}

// --------------------------------------------------------------------------
//
//  This is can_recv from the Janz software.
//
//  /* can_recv - receive a message from standard interface
//   *
//   * This function reads a whole message from the standard host interface of
//   * a VMOD-ICAN.
//   * The module is selected by the module number <fd>.
//   *
//   * The structure <pm> is filled with the received message.
//   *
//   * RETURNS:
//   * The function returns the number of message received, or -1 when the
//   * system call failed.
//   * The return value therefore 0 determines, that no message was available to
//   * be read: can_recv() does not block in such a case and therefore
//   * can be used to poll a module for incoming messages.
//   */
//
int VmodIcan::Receive(Message *pm) /* receive buffer */
{

    struct dpm_rw_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_READ_MBOX, &arg))
        return -1;

    return arg.rval;
}

// --------------------------------------------------------------------------
//
// This is can_recv_fast from the Janz software
//
//  /* can_recv_fast - receive a message from layer2 interface
//   *
//   * This function reads a FastMessage from the layer2 fast message
//   * interface of a VMOD-ICAN.
//   * The module is selected by the file descriptor <fd>.
//   *
//   * The structure <pm> is filled with the received message.
//   *
//   * RETURNS:
//   * The function returns -1 when the * system call failed.
//   * The return value therefore 0 determines, that no message was available to
//   * be read: can_recv_fast() does not block in such a case and therefore
//   * can be used to poll a module for incoming messages.
//   */
//
int VmodIcan::ReceiveFast(FastMessage *pm)
{
    struct dpm_write_fast_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_READ_FAST_MBOX, &arg))
        return -1;

    return arg.rval;
}

// --------------------------------------------------------------------------
//
// This is IcWriteBtrBCAN from the Janz software
//
//  /* IcWriteBtrBCAN - Set bit timing parameters
//   *
//   * Set bit timing parameters in CAN controller. May only be used if
//   * CAN controller is in bus-off state. <btr> stores the bus-timing
//   * parameters as required by the 82C200 controller. See the description
//   * of the CBTRreq-service for possible values.
//   *
//   * BTR1 is stored in the upper byte of <btr> and BTR0 in the lower. Examples
//   * are:
//   * .CS
//   *     Baudrate   btr       Macro
//   *       1Mbit    0x2300    BTR_1MB
//   *     500kBit    0x1c00    BTR_500KB
//   *     250kBit    0x1c01    BTR_250KB
//   *     125kBit    0x1c03    BTR_125KB
//   *     100kBit    0x34c7    BTR_100KB
//   *      50kBit    0x34cf    BTR_50KB
//   *      20kBit    0x7fcf    BTR_20KB
//   * .CE
//   *
//   * SERVICE: CBTRreq
//   *
//   * NOTE:
//   * Raw ICANOS version of the firmware only.
//   */
//
void VmodIcan::SetBaudRate(int rate)
{
    Message msg;  /* buffer for module messages */

    int rateid;

    switch (rate)
    {
    case 1000:
        rateid=BTR_1MB;
        break;
    case  500:
        rateid=BTR_500KB;
        break;
    case  250:
        rateid=BTR_250KB;
        break;
    case  125:
        rateid=BTR_125KB;
        break;
    case  100:
        rateid=BTR_100KB;
        break;
    case   50:
        rateid=BTR_50KB;
        break;
    case   20:
        rateid=BTR_20KB;
        break;

    default:
        cout << "Error: Wrong bit rate specified" << endl;
        return;
    }

    msg.cmd = M_BCAN_SET_BTR_req;

    msg.len = 4;

    msg.data[2] = word_to_lsb(rateid);
    msg.data[3] = word_to_msb(rateid);

    while (!Send(&msg));      /* transmitt to module */

    lout << "- Baudrate set to " << rate << "kbps" << endl;
}

// --------------------------------------------------------------------------
//
// This is IcBusOnBCAN from the Janz software
//
//  /* IcBusOnBCAN - switch CANbus controller to bus-on state
//   *
//   * Switch CAN controller bus-on. You will need to use this
//   * function explicitly after you have connected yourself
//   * to the module with can_open() (or ican_open() under DOS/WINDOWS).
//   * This is because the module comes up in the bus-off state.
//   *
//   * SERVICE: CONreq
//   *
//   * NOTE:
//   * Raw ICANOS version of the firmware only.
//   */
//
void VmodIcan::EnableCanBusConnection()
{
    Message msg;                  /* buffer for module messages */

    msg.cmd = M_BCAN_BUSON_req;
    msg.len = 0;

    while (!Send(&msg));

    lout << "- Controller connected to bus" << endl;
}

// --------------------------------------------------------------------------
//
// This is ican2_init_fast_canfrom the Janz software
//
//  /* ican2_init_fast_can - initialize fast can access for VMOD-ICAN
//   *
//   * By this function, the user may initialize and enable the fast
//   * host interface (layer2 access) for a VMOD-ICAN module.
//   *
//   * The calling application can request <rbuffers> buffer elements in the queue
//   * that sends data to the host and <wbuffers> buffer elements for the queue
//   * that transports data to the module.
//   *
//   * NOTE:
//   * Notice that the message filtering on the VMOD-ICAN has to be
//   * set correctly, so that messages can be received through the fast
//   * interface.
//   *
//   * CAVE AT:
//   * The <rbuffers> and wbuffers> have no special limit, but the general
//   * resources of the DPM must not be exceeded.
//   * For the calculation you need to assume, that 16 buffers in one of the fast
//   * interface queues take the same DPM space as 1 buffer in the standard
//   * host interface.
//   *
//   * The user must use only one of the functions, either
//   * ican2_init_fast_can or ican2_init_fast_can_prio
//   *
//   * RETURNS:
//   * Zero if the operation performed successfully, or less than zero on error.
//   */
//
int VmodIcan::EnableFastCan(int rbuffers, int wbuffers)
{
    struct dpm_fast_can_desc hdp;

    hdp.tohost_len   = rbuffers;
    hdp.fromhost_len = wbuffers;

    if (!Ioctl(DPM_INIT_FAST_CAN, &hdp))
        return -1;

    lout << "- Fast Host Interface Enabled" << endl;

    return 0;
}

// --------------------------------------------------------------------------
//
// This is IcWriteEwlBCAN from the Janz software
//
//  /* IcWriteEwlBCAN - Set error warning limit
//   *
//   * Set error warning limit in CAN controller. If this limit is passed, the
//   * user will get a CEVTind message stating an error interrupt. This type
//   * of message will also occur if the both error counter again fall below
//   * this limit.
//   *
//   * RESTRICTIONS:
//   * Will only take effect if CAN controller is in bus-off state. Requires
//   * an SJA1000 CANbus controller, and will be no-op for 82C200.
//   *
//   * SERVICE: CBCONFreq
//   *
//   * NOTE:
//   * Raw ICANOS version of the firmware only.
//   */
//
void VmodIcan::DisableCanBusConnection()
{
    lout << "- Disconnect VmodIcan module from Bus!" << endl;

    Message msg;                  /* buffer for module messages */

    msg.cmd = M_BCAN_BUSOFF_req;
    msg.len = 0;

    while (!Send(&msg));

    lout << "- VmodIcan disconnected." << endl;
}

// --------------------------------------------------------------------------
//
// This is can_close from the Janz software
//
//  /* can_close - close connection to a VMOD-ICAN module
//   *
//   * The function can be used to close a connection to a VMOD-ICAN
//   * that has been established by a can_open() call.
//   * The module has to be selected by the file descriptor <fd> which was
//   * obtained when you did the can_open() call.
//   *
//   * When you call can_close, all the resources that were used by the driver
//   * for communication are freed.
//   *
//   * The VMOD-ICAN module under question will be reseted, to make sure that
//   * the communication with the host will stop. That means especially that
//   * no further interrupt will occur and that the module will not longer be
//   * active on the CANbus.
//   *
//   * RETURNS: N/A
//   */
//
void VmodIcan::Close()
{
    lout << "- Closing device VmodIcan #" << (int)fd << endl;

    Message msg; /* disconnect message */

    msg.cmd = M_DISCONNECT;
    msg.len = 0;

    while (!Send(&msg));

    close(fd);

    lout << "- Device closed." << endl;
}

// --------------------------------------------------------------------------
//
// Enable the fifo of the Janz card
// Allow VMOD to send messages through the fifo
//
int VmodIcan::EnableFifo()
{
    Message msg; /* connect message */

    msg.cmd = M_CONNECT_INTR;
    msg.len = 0;

    while (!Send(&msg));

    lout << "- Fifo enabled" << endl;

    return TRUE;
}

// --------------------------------------------------------------------------
//
// Reset the module
//
int VmodIcan::Reset()
{
    const int rc = Ioctl(DPM_RESET, 0);

    lout << "- Reset done." << endl;

    return rc;
}

// --------------------------------------------------------------------------
//
// This is can_open from the Janz software
//
//  /* can_open - open VMOD-ICAN device
//   *
//   * With this function call you open a VMOD-ICAN plugged
//   * into a MODULbus carrier board for use. The module is
//   * reseted and then initialized for communication to the host.
//   *
//   * A specific module is selected by it's device name (e.g. "/dev/dpm_01").
//   */
//
int VmodIcan::Open(const char *devname)          /* pathname of device */
{
    fd = open(devname, O_RDONLY, 0);

    if (fd < 0)
    {
        lout << "Error: Opening device '" << devname << "' (rc=" << fd << ")" << endl;
        lout << strerror(errno) << endl;
        return FALSE;
    }

    lout << "- Device " << devname << " #" << fd << " open." << endl;

    return TRUE;
}

// --------------------------------------------------------------------------
//
// This is ican2_select_hostif from the Janz software
//
//  /* ican2_select_hostif - switch standard host interface to new style mode
//   *
//   * The routine ican2_select_hostif() can be used to switch a module from
//   * the standard host interface to the new style mode. The module is selected
//   * by the module number <fd>.
//   *
//   * The calling application can request <rbuffers> buffer for the communication
//   * queue that sends data to the host and <wbuffers> buffer for the reverse
//   * communication direction (normal priority queue). By this function the hi- and
//   * low-prioritized message-queues which sends data to the module are initialized
//   * to a length of 1.
//   *
//   * NOTE:
//   * To notify the module of the new situation, the driver sends
//   * a M_NEWHOSTIF message to the module. This is the last message to be
//   * transfered through the old style host interface. Immediately after
//   * sending this message, the library is switched to the new style mode.
//   * Any messages that are sent by the module in this time gap, may be lost.
//   * It is therefore not recommended to use this function when you wait
//   * for messages from the module.
//   *
//   * The selection of the new host interface is not reversible. It will stay
//   * until the next reset for the module occurs. This will probably occur
//   * when you use can_close().
//   *
//   * HINTS:
//   * When the new style mode is active, no more internal message buffering
//   * on the module exists. That is whenever the module tries to send something
//   * and cannot because the queue is full, this message will be dropped.
//   * Thereby, when enabling the new style host interface you should create
//   * enough buffers for the queue that sends to the host, to prevent the
//   * loss of messages. If you loose messages, however you will be indicated
//   * of that event by a MSGLOST messages (which will not be lost!).
//   *
//   * CAVE AT:
//   * The parameters <rbuffers>, <wbuffers>, <wbuffers_hi> and <wbuffers_low>
//   * must be greater than 0, less than 128, and the total sum must not
//   * exceed 236. These parameters aren't checked by the driver!
//   */
//
int VmodIcan::StdHost2NewStyle(int rbuffers, int wbuffers,
                               int wbuffers_hi, int wbuffers_low)
{
    struct dpm_new_hostif_desc_prio hdp;

    hdp.tohost_len       = rbuffers;
    hdp.fromhost_len     = wbuffers;
    hdp.fromhost_hi_len  = wbuffers_hi;
    hdp.fromhost_low_len = wbuffers_low;

    Ioctl(DPM_INIT_NEW_HOSTIF_PRIO, &hdp);

    lout << "- New style host interface enabled" << endl;

    return 0;
}

// --------------------------------------------------------------------------
//
// This is can_send_hi from the Janz software
//
//  /* can_send_hi - send message to standard host interface (high priority)
//   *
//   * This function performs the same action as can_send(), except it will
//   * append message <pm> to the highest priority queue of the standard
//   * host interface.
//   *
//   * NOTE:
//   * Notice that the prioritized issue of the message take effect on the new style
//   * mode of the standard host interface only.
//   *
//   * RETURNS:
//   * The function returns the number of message send, or -1 when the system
//   * call failed.
//   * The return value 0 determines that no message could be send,
//   * probably because there was no space in the targeted queue. can_send_hi()
//   * does not block or retry in such a case, so you need to loop explicitly
//   * until the message is send.
//   */
//
int VmodIcan::SendHi(Message *pm)
{
    struct dpm_rw_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_WRITE_MBOX_HI, &arg))
        return FALSE;

    return arg.rval;
}

// --------------------------------------------------------------------------
//
// This is can_send_low from the Janz software
//
//  /* can_send_low - send message to standard host interface (low priority)
//   *
//   * This function performs the same action as can_send(), except it will
//   * append message <pm> to the lowest priority queue of the standard
//   * host interface.
//   *
//   * NOTE:
//   * Notice that the prioritized issue of the message take effect on the new
//   * style mode of the standard host interface only.
//   *
//   * RETURNS:
//   * The function returns the number of message send, or -1 when the system
//   * call failed.
//   * The return value 0 determines that no message could be send,
//   * probably because there was no space in the targeted queue. can_send_low()
//   * does not block or retry in such a case, so you need to loop explicitly
//   * until the message is send.
//   *
//   */
int VmodIcan::SendLo(Message *pm)
{
    struct dpm_rw_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_WRITE_MBOX_LOW, &arg))
        return FALSE;

    return arg.rval;
}

// --------------------------------------------------------------------------
//
// This is can_send from the Janz software
//
//  /* can_send - send message to standard host interface (mid priority)
//   *
//   * This function sends a complete message to the standard host interface of
//   * a VMOD-ICAN.
//   *
//   * The message <pm> will be queued to the middle prioritized of the three
//   * queues.
//   *
//   * RETURNS:
//   * The function returns the number of message send, or -1 when the system
//   * call failed.
//   * The return value 0 determines that no message could be send,
//   * probably because there was no space in the targeted queue. can_send()
//   * does not block or retry in such a case, so you need to loop explicitly
//   * until the message is send.
//   */
//
int VmodIcan::Send(Message *pm) /* file descriptor, message to send */
{
    struct dpm_rw_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_WRITE_MBOX, &arg))
        return FALSE;

    return arg.rval;
}

// --------------------------------------------------------------------------
//
// This is can_fast_send from the Janz software
//
//  /* can_fast_send - send message to fast interface
//   *
//   * This function sends a message to the fast host interface (layer-2
//   * interface) of a VMOD-ICAN. The module is selected by the module number
//   * <fd>.
//   * The message to be send will be given in the structure <pm>.
//   *
//   * The fast host interface needs to be established before can_fast_send()
//   * can be used successfully.
//   *
//   * RETURNS:
//   * The function returns 1 if can_fast_send() completed successful.
//   * Otherwise the return value 0 determines that the message could not be send,
//   * probably because there was no space in the DPM. The function
//   * does not block or retry in such a case, so you need to loop explicitly
//   * until the message is send.
//   * The function returns -1 when the system-call itself failed.
//   */
//
int VmodIcan::Send(FastMessage *pm) /* file descriptor, message to send */
{
    struct dpm_write_fast_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_WRITE_FAST_CAN, &arg))
        return FALSE;

    lout << "done." << endl;

    return arg.rval;
}

// --------------------------------------------------------------------------
//
// This is IcSetAfil from the Janz software
//
//  /*
//   * IcSetAfil - Set software acceptance filter mask
//   *
//   * Set software acceptance filtering.
//   *
//   * SERVICE: SetAfilMask
//   */
//
void VmodIcan::DisableAllCobIds()
{
    Message msg;

    msg.cmd = M_SET_AFIL;
    msg.len = 5;

    msg.data[0]  = word_to_lsb(0);
    msg.data[1]  = word_to_msb(0);
    msg.data[2]  = word_to_lsb(0x7ff); // 11 bit Cob-Ids
    msg.data[3]  = word_to_msb(0x7ff); // 11 bit Cob-Ids
    msg.data[4]  = 0;

    while (!Send(&msg));

    lout << "- All CobIds disabled." << endl;
}

// --------------------------------------------------------------------------
//
// This is IcSetAfil from the Janz software
//
//  /*
//   * IcSetAfil - Set software acceptance filter mask
//   *
//   * Set software acceptance filtering.
//   *
//   * SERVICE: SetAfilMask
//   */
//
void VmodIcan::EnableCobId(WORD_t cobid, int flag)
{
    Message msg;

    msg.cmd = M_SET_AFIL;
    msg.len = 3;

    msg.data[0]  = word_to_lsb(cobid);
    msg.data[1]  = word_to_msb(cobid);
    msg.data[2]  = (BYTE_t)(flag?3:0);

    while (!Send(&msg));

    lout << "- CobId 0x" << hex << setfill('0') << setw(3) << cobid << " enabled." << endl;
}

// --------------------------------------------------------------------------
//
// This is IcSendReqBCAN from the Janz software
//
//  /*
//   * IcSendReqBCAN - Send a CANbus message
//   *
//   * Issue request to send a CAN message. <Spec> controls whether to send with
//   * or without spec/confirmation.
//   * .CS
//   *    spec     action
//   *      0      send only
//   *      1      send with confirmation to the host.
//   *      2      send and echo message to the host.
//   *      3      send and generate both echo and confirmation.
//   * .CE
//   *
//   * SERVICE: CTXreq, CTXCreq, CTXEreq, CTXCEreq
//   *
//   * NOTE:
//   * Raw ICANOS version of the firmware only.
//   */
//
void VmodIcan::SendCanFrame(WORD_t cobid, BYTE_t m[8], BYTE_t rtr)
{
    const WORD_t desc = MsgDescr(cobid, 8, rtr);

    Message msg;

    msg.cmd = M_BCAN_TX_req;

    msg.len = 12;
    msg.data[0]  = 0;
    msg.data[1]  = 0;
    msg.data[2]  = word_to_msb(desc);
    msg.data[3]  = word_to_lsb(desc);

    memcpy(&msg.data[4], m, 8);

    while (!Send(&msg));
}

// --------------------------------------------------------------------------
//
// Constructor. Sets logging.
//  Set the receiving thread to priority -10 and detached.
//
//  Open the device.
//  reset the device
//  Enable the fifo buffers
//  Set the baud rate to the given rate
//  Disable passthrough of all cobids (all canbus messages)
//  and switch the can bus communication on
//
VmodIcan::VmodIcan(const char *dev, const int baud, MLog &out) : Log(out), MThread(false, -10)//: CanDriver(dev, baud)
{
    //
    // Set priority of receiving thread and detach the receiving thread
    //
    Detach();

    if (!Open(dev))
    {
        cout << "Cannot open device '" << dev << "'... exit." << endl;
        cout << strerror(errno) << endl;
//        exit(1);                       // open module
    }

    Reset();
    EnableFifo();                      // connect to host (in interrupt mode)
    SetBaudRate(baud);                 // set baud rate
    DisableAllCobIds();
    EnableCanBusConnection();          // connect to bus
    /*
     StdHost2NewStyle(50, 50, 50, 50);  // set host style
     EnableFastCan(50, 50);
     SetTermination(0);
     */

    lout << "- VmodIcan initialized." << endl;

}

// --------------------------------------------------------------------------
//
// Destructor. Stopt the receiver, disables the bus connection and
// close the device
//
VmodIcan::~VmodIcan()
{
    lout << "- Stopping VmodIcan module." << endl;
    Stop();
    DisableCanBusConnection();
    Close();
    lout << "- VmodIcan stopped." << endl;
}

// --------------------------------------------------------------------------
//
// Sends a can frame with the given cobid and the given eight bytes
// through the can network
//
void VmodIcan::SendCanFrame(WORD_t cobid,
                            BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
                            BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
{
    BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
    SendCanFrame(cobid, msg);
}
