UpdatePTUDevCtrl/DCPlugin/src/sendWRcmd.cpp

#include <stdint.h>
#include <QString>
#include <QDebug>
#include "tslib.h"
#include "sendWRcmd.h"


void indat_PrnPwr(void);


void sendWRcmd_INI(void)
{

    sendWRcmd_clrCmdStack();
    sendWRcmd_clrCmd4Stack();
    sendFDcmd_clrStack();
    longFDcmd_clrStack();
}

// Command Stack for commands without parameters


static uint16_t nextAsynchsendCmd0[CMDSTACKDEPTH];
static uint8_t  nrOfCmdsInQueue;
/* convention: use simple (not rotating) FIFO Stack:
Example: nrOfCmdsInQueue=4 then
    nextAsynchsendCmd0[0]=cmd1      // was stored as first
    nextAsynchsendCmd0[1]=cmd2
    nextAsynchsendCmd0[2]=cmd3
    nextAsynchsendCmd0[3]=cmd4      // came in as last

    Send: [0] first, then move buffer 1 down:
    nextAsynchsendCmd0[0]=cmd2
    nextAsynchsendCmd0[1]=cmd3
    nextAsynchsendCmd0[2]=cmd4
    nextAsynchsendCmd0[3]=0;
    nrOfCmdsInQueue=3 now
*/

void sendWRcmd_clrCmdStack(void)
{
    uint8_t nn;
    for (nn=0; nn<CMDSTACKDEPTH; nn++)
        nextAsynchsendCmd0[nn]=0;
    nrOfCmdsInQueue=0;
}

bool sendWRcmd_setSendCommand0(uint16_t nextCmd)
{
    // write Command to memory, wait for transport
    if (nrOfCmdsInQueue>=CMDSTACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    nextAsynchsendCmd0[nrOfCmdsInQueue++]=nextCmd;
    //qDebug() << "PI cmd queued:"<< nextCmd << ", saved, pp=" << nrOfCmdsInQueue;
    return true;                // ok, will be sent
}

uint16_t sendWRcmd_getSendCommand0(void)
{
    uint16_t nxtAsynchCmd;
    uint8_t nn, ll;
    if (nrOfCmdsInQueue==0 || nrOfCmdsInQueue>CMDSTACKDEPTH)
        return 0;   // error
    nxtAsynchCmd=nextAsynchsendCmd0[0];

    // move Puffer down by one element
    if (CMDSTACKDEPTH>0)
        ll=CMDSTACKDEPTH-1;
    else
        ll=0;
    for (nn=0; nn<ll; nn++)
        nextAsynchsendCmd0[nn]=nextAsynchsendCmd0[nn+1];

    if (nrOfCmdsInQueue>0)
        nrOfCmdsInQueue--;
    //qDebug() << "PI cmd queued:"<< nxtAsynchCmd << ", restored, pp now =" << nrOfCmdsInQueue;
    return nxtAsynchCmd;
}

//---------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------
// Command Stack for commands with 4 parameters



static uint16_t nextAsynchsendCmd4[CMD4STACKDEPTH];
static uint8_t  nextCmd4para1[CMD4STACKDEPTH];
static uint8_t  nextCmd4para2[CMD4STACKDEPTH];
static uint8_t  nextCmd4para3[CMD4STACKDEPTH];
static uint8_t  nextCmd4para4[CMD4STACKDEPTH];
static uint8_t  nrOfCmds4InQueue;
/* convention: use simple (not rotating) FIFO Stack:
Example: nrOfCmdsInQueue=4 then
    nextAsynchsendCmd0[0]=cmd1      // was stored as first
    nextAsynchsendCmd0[1]=cmd2
    nextAsynchsendCmd0[2]=cmd3
    nextAsynchsendCmd0[3]=cmd4      // came in as last

    Send: [0] first, then move buffer 1 down:
    nextAsynchsendCmd0[0]=cmd2
    nextAsynchsendCmd0[1]=cmd3
    nextAsynchsendCmd0[2]=cmd4
    nextAsynchsendCmd0[3]=0;
    nrOfCmdsInQueue=3 now
*/


void sendWRcmd_clrCmd4Stack(void)
{
    uint8_t nn;
    for (nn=0; nn<CMD4STACKDEPTH; nn++)
    {
        nextAsynchsendCmd4[nn]=0;
        nextCmd4para1[nn]=0;
        nextCmd4para2[nn]=0;
        nextCmd4para3[nn]=0;
        nextCmd4para4[nn]=0;
    }
    nrOfCmds4InQueue=0;
}

bool sendWRcmd_setSendCommand4(uint16_t nextCmd, uint8_t dat1, uint8_t dat2, uint8_t dat3, uint8_t dat4)
{
    // write Command to memory, wait for transport
    if (nrOfCmds4InQueue>=CMD4STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    nextAsynchsendCmd4[nrOfCmds4InQueue]=nextCmd;
    nextCmd4para1[nrOfCmds4InQueue]=dat1;
    nextCmd4para2[nrOfCmds4InQueue]=dat2;
    nextCmd4para3[nrOfCmds4InQueue]=dat3;
    nextCmd4para4[nrOfCmds4InQueue]=dat4;
    //qDebug() << "data with 4 data byte saved, pp=" << nrOfCmds4InQueue;
    //qDebug() << " dat1=" << nextCmd4para1[nrOfCmds4InQueue] << " dat2=" << nextCmd4para2[nrOfCmds4InQueue]
    //             << " dat3=" << nextCmd4para3[nrOfCmds4InQueue]  << " dat4=" << nextCmd4para4[nrOfCmds4InQueue];
    nrOfCmds4InQueue++;
    return true;                // ok, will be sent
}

uint16_t sendWRcmd_getSendCommand4(uint8_t *dat1, uint8_t *dat2, uint8_t *dat3, uint8_t *dat4)
{
    uint16_t nxtAsynchCmd;
    uint8_t nn, ll;

    if (nrOfCmds4InQueue==0 || nrOfCmds4InQueue>CMD4STACKDEPTH)
        return 0;   // error
    nxtAsynchCmd=nextAsynchsendCmd4[0];
    *dat1=nextCmd4para1[0];
    *dat2=nextCmd4para2[0];
    *dat3=nextCmd4para3[0];
    *dat4=nextCmd4para4[0];
    //qDebug() << "cmd4  restored to send from [0];  pp=" << nrOfCmds4InQueue;
    //qDebug() << " data1: " << nextCmd4para1[0] << " data2: " << nextCmd4para2[0] <<
    //            " data3: " << nextCmd4para3[0] << " data4: " << nextCmd4para4[0];

    // move Puffer down by one element
        if (CMD4STACKDEPTH>0)
        ll=CMD4STACKDEPTH-1;
    else
        ll=0;
    for (nn=0; nn<ll; nn++)
    {
        nextAsynchsendCmd4[nn]=nextAsynchsendCmd4[nn+1];
        nextCmd4para1[nn]=nextCmd4para1[nn+1];
        nextCmd4para2[nn]=nextCmd4para2[nn+1];
        nextCmd4para3[nn]=nextCmd4para3[nn+1];
        nextCmd4para4[nn]=nextCmd4para4[nn+1];
    }
    if (nrOfCmds4InQueue>0)
        nrOfCmds4InQueue--;
    //qDebug() << "cmd4  after push down: pp=" << nrOfCmds4InQueue;
    return nxtAsynchCmd;
}





static uint16_t  nextAsynchsendCmd8[CMD8STACKDEPTH];
static uint8_t   nextCmd8para1[CMD8STACKDEPTH];
static uint8_t   nextCmd8para2[CMD8STACKDEPTH];
static uint16_t  nextCmd8para3[CMD8STACKDEPTH];
static uint32_t  nextCmd8para4[CMD8STACKDEPTH];
static uint8_t   nrOfCmds8InQueue;

void sendWRcmd_clrCmd8Stack(void)
{
    uint8_t nn;
    for (nn=0; nn<CMD8STACKDEPTH; nn++)
    {
        nextAsynchsendCmd8[nn]=0;
        nextCmd8para1[nn]=0;
        nextCmd8para2[nn]=0;
        nextCmd8para3[nn]=0;
        nextCmd8para4[nn]=0;
    }
    nrOfCmds8InQueue=0;
}

bool sendWRcmd_setSendCommand8(uint16_t nextCmd, uint8_t dat1, uint8_t dat2, uint16_t dat3, uint32_t dat4)
{
    // write Command to memory, wait for transport
    if (nrOfCmds8InQueue>=CMD8STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    nextAsynchsendCmd8[nrOfCmds8InQueue]=nextCmd;
    nextCmd8para1[nrOfCmds8InQueue]=dat1;
    nextCmd8para2[nrOfCmds8InQueue]=dat2;
    nextCmd8para3[nrOfCmds8InQueue]=dat3;
    nextCmd8para4[nrOfCmds8InQueue]=dat4;
    nrOfCmds8InQueue++;
    return true;                // ok, will be sent
}

uint16_t sendWRcmd_getSendCommand8(uint8_t *dat1, uint8_t *dat2, uint16_t *dat3, uint32_t *dat4)
{
    uint16_t nxtAsynchCmd;
    uint8_t nn, ll;

    if (nrOfCmds8InQueue==0 || nrOfCmds8InQueue>CMD8STACKDEPTH)
        return 0;   // error
    nxtAsynchCmd=nextAsynchsendCmd8[0];
    *dat1=nextCmd8para1[0];
    *dat2=nextCmd8para2[0];
    *dat3=nextCmd8para3[0];
    *dat4=nextCmd8para4[0];

    // move buffer down by one element
    if (CMD8STACKDEPTH>0)
        ll=CMD8STACKDEPTH-1;
    else
        ll=0;
    for (nn=0; nn<ll; nn++)
    {
        nextAsynchsendCmd8[nn]=nextAsynchsendCmd8[nn+1];
        nextCmd8para1[nn]=nextCmd8para1[nn+1];
        nextCmd8para2[nn]=nextCmd8para2[nn+1];
        nextCmd8para3[nn]=nextCmd8para3[nn+1];
        nextCmd8para4[nn]=nextCmd8para4[nn+1];
    }
    if (nrOfCmds8InQueue>0)
        nrOfCmds8InQueue--;
    return nxtAsynchCmd;
}







static uint8_t sendAsynchDataBuf[160]; // no stack, only ONE buffer
static uint8_t sendAsyDatLen;

bool sendWRcmd_setSendBlock160(uint8_t leng, uint8_t *buf)
{
    //qDebug()  << "pi epi: storing send data";
    if (leng>160) leng=160;
    sendAsyDatLen=leng;
    tslib_strclr(sendAsynchDataBuf, 0, 160);
    for (uint8_t nn=0; nn<leng; nn++)
        sendAsynchDataBuf[nn]=buf[nn];
    return true;                // ok, will be sent
}

uint8_t sendWRcmd_getSendBlock160(uint8_t *leng, uint8_t *buf)
{
    //qDebug()  << "pi gpi: restoring send data";
    *leng=sendAsyDatLen;
    for (uint8_t nn=0; nn<sendAsyDatLen; nn++)
        buf[nn]=sendAsynchDataBuf[nn];
    sendAsyDatLen=0;
    //tslib_strclr(sendAsynchDataBuf, 0, 64);
    return *leng;
}


// ------------------------------------------------------------------------------------
// MDB Sendind Data  are store here for next transport to DC (Device Controller)
// Transport to Slave runs every 100ms, answer from mdb-slave (e.g. coin changer) comes rigth
// with next slave answer

// start with: SENDDIRCMD_EXCHGMDB,
    // send crude data from here to DC, DC to mdb slaves, mdb answer, return here within 50ms


static uint8_t Sdata_mdbSendBuffer[64];
static uint8_t Sdata_mdbSendLen;

uint8_t epi_store64ByteSendData(uint8_t length, uint8_t *buf)
{
    // HWapi writes data to be forwarded to DC and further to mdb-device
    for (uint8_t nn=0; nn<length; nn++)
        Sdata_mdbSendBuffer[nn]=buf[nn];
    Sdata_mdbSendLen=length;
    return 0;
}

uint8_t gpi_restore64ByteSendData(uint8_t *length, uint8_t *buf)
{
    // datif reads data to forward to dc
    for (uint8_t nn=0; nn<Sdata_mdbSendLen; nn++)
        buf[nn]=Sdata_mdbSendBuffer[nn];
    *length=Sdata_mdbSendLen;
    Sdata_mdbSendLen=0;
    return 0;
}






//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
//---------------------------------------- Printer Text Fifo -------------------------
static uint8_t prnDataParameters[4];
static uint8_t prnDataBufferUser;

void epi_storeUserOfSendingTextBuffer(uint8_t user, uint8_t para1, uint8_t para2, uint8_t para3, uint8_t para4 )
{
    // user=1: Text-Print is using this buffer
    //      2: QR-code-Printer is using this buffer
    prnDataBufferUser=user;
    prnDataParameters[0]=para1;
    prnDataParameters[1]=para2;
    prnDataParameters[2]=para3;
    prnDataParameters[3]=para4;

//    qDebug() << "new user stored: " << user;

}

uint8_t gpi_getUserOfSendingTextBuffer(uint8_t *para1, uint8_t *para2, uint8_t *para3, uint8_t *para4)
{
    // user=1: Text-Print is using this buffer
    //      2: QR-code-Printer is using this buffer
    //qDebug() << "returning user "<< prnDataBufferUser;

    *para1=prnDataParameters[0];
    *para2=prnDataParameters[1];
    *para3=prnDataParameters[2];
    *para4=prnDataParameters[3];
    return prnDataBufferUser;

}


    // Sending Text Fifo
// ONE printer doc consists of 20 x 64 byte
// #define MAXNROF_PRNBYTES   64
// #define MAXNROF_PRNBLOCKS  20
static char Sdata_PRN_TEXT[MAXNROF_PRNBLOCKS][MAXNROF_PRNBYTES];
static uint8_t pPrnDataBuff;        // points to next PRINTER_BLOCK


//static uint8_t pPrnDataBuff;        // points to next waiting printer text
//  defined above, needed if more then one text is stored (before sent)
// every block will be sent after 100ms, if 8 blocks are stored within this 100ms
// then pointer goes up to 8. Important: FIFO!!!!!!!!

void epi_resetPrinterStack(void)
{
    pPrnDataBuff=0;
}

uint8_t epi_storePrnText(char *buf, uint8_t leng)
{
    // store text from Gui in next higher free memory 0....9

    uint16_t len;
    uint8_t  pp, nn;

    pp=pPrnDataBuff;    // next free memory block with 64byte each
    if (pp>=MAXNROF_PRNBLOCKS)
        return 1;   // not possible, no free mem

    //len=tslib_strlen(buf);    // kennt keine Binärzeichen!!!!!!
    len=leng;
    if (len>MAXNROF_PRNBYTES)
        len=MAXNROF_PRNBYTES;

    tslib_strclr(Sdata_PRN_TEXT[pp], 0, MAXNROF_PRNBYTES);

    for (nn=0; nn<len; nn++)
        Sdata_PRN_TEXT[pp][nn]=buf[nn];     // copy new text into buffer

    if (pPrnDataBuff<MAXNROF_PRNBLOCKS)
        pPrnDataBuff++;                     // inc pointer if end not yet reached
    return 0;       // OK
}



uint8_t gpi_restorePrnText(uint8_t *retbuf)
{
    // read printer text and send to slave, size of retbuf == 64

    // always read from [0] because this is the oldest (Fifo)
    // then move all text lines down by one and dec pointer

    uint8_t  nn, pp=pPrnDataBuff;

    if (pp==0)    // next free memory block with 64byte each
        return 1;           // no text in buffer

    // example: pp=5: then buffers [0...4] are occupied

    for (nn=0; nn<MAXNROF_PRNBYTES; nn++)
        retbuf[nn] = uint8_t (Sdata_PRN_TEXT[0][nn]);     // restore oldest text

    // now copy textline [1] to [0], then
    // copy textline [2] to [1], then
    // copy textline [3] to [2] .... upto [pp-1] to [pp-2]
    // hint: copying from 9....0 would delete all strings!!!!!!

    for (nn=0; nn<(pp-1); nn++)
        tslib_strcpy(Sdata_PRN_TEXT[nn+1], Sdata_PRN_TEXT[nn], MAXNROF_PRNBYTES);
    if (pPrnDataBuff>0)
        pPrnDataBuff--;
    pp=pPrnDataBuff;
    // example: pp=4: then buffers [0...3] are still occupied, pp=0: all buffers empty

    // now clear highest copyed line (which got free now)
    tslib_strclr(Sdata_PRN_TEXT[pp], 0, MAXNROF_PRNBYTES);

    // optionally: clear all remaining higher lines:
    for (nn=(pp+1); nn<MAXNROF_PRNBLOCKS; nn++)
         tslib_strclr(Sdata_PRN_TEXT[nn], 0, MAXNROF_PRNBYTES);

    return 0;
}

uint8_t gpi_chk4remainingText(void)
{
    // retval: 0: no more textline left (to send)  >0: nr of 64byte-blocks
    return (pPrnDataBuff);
}










// ---------------------------------------------------------------------------------
//   11.4.23 neu, Kommando direkt an "FastDevice"-protokoll senden, nicht mehr umsetzen
// ---------------------------------------------------------------------------------


// short command, 4 data bytes
static uint8_t nextFDwrCmd[FDCMD_STACKDEPTH];
static uint8_t nextFDrdCmd[FDCMD_STACKDEPTH];
static uint8_t nextFDblkNr[FDCMD_STACKDEPTH];

static uint8_t  nextFDpara1[FDCMD_STACKDEPTH];
static uint8_t  nextFDpara2[FDCMD_STACKDEPTH];
static uint8_t  nextFDpara3[FDCMD_STACKDEPTH];
static uint8_t  nextFDpara4[FDCMD_STACKDEPTH];
static uint8_t  p_nextFDcmdsInQueue;
/* convention: use simple (not rotating) FIFO Stack:
Example: nrOfCmdsInQueue=4 then
    nextAsynchsendCmd0[0]=cmd1      // was stored as first
    nextAsynchsendCmd0[1]=cmd2
    nextAsynchsendCmd0[2]=cmd3
    nextAsynchsendCmd0[3]=cmd4      // came in as last

    Send: [0] first, then move buffer 1 down:
    nextAsynchsendCmd0[0]=cmd2
    nextAsynchsendCmd0[1]=cmd3
    nextAsynchsendCmd0[2]=cmd4
    nextAsynchsendCmd0[3]=0;
    nrOfCmdsInQueue=3 now
*/


void sendFDcmd_clrStack(void)
{
    uint8_t nn;
    for (nn=0; nn<FDCMD_STACKDEPTH; nn++)
    {
        nextFDwrCmd[nn]=0;
        nextFDrdCmd[nn]=0;
        nextFDblkNr[nn]=0;
        nextFDpara1[nn]=0;
        nextFDpara2[nn]=0;
        nextFDpara3[nn]=0;
        nextFDpara4[nn]=0;
    }
    p_nextFDcmdsInQueue=0;
}

bool sendFDcmd_set(uint8_t nextWrCmd, uint8_t nextRdCmd, uint8_t blockNum, uint8_t dat1, uint8_t dat2, uint8_t dat3, uint8_t dat4)
{
    // write Command to memory, wait for transport
    if (p_nextFDcmdsInQueue>=FDCMD_STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    nextFDwrCmd[p_nextFDcmdsInQueue]=nextWrCmd;
    nextFDrdCmd[p_nextFDcmdsInQueue]=nextRdCmd;
    nextFDblkNr[p_nextFDcmdsInQueue]=blockNum;
    nextFDpara1[p_nextFDcmdsInQueue]=dat1;
    nextFDpara2[p_nextFDcmdsInQueue]=dat2;
    nextFDpara3[p_nextFDcmdsInQueue]=dat3;
    nextFDpara4[p_nextFDcmdsInQueue]=dat4;
    //qDebug() << "data with 4 data byte saved, pp=" << nrOfCmds4InQueue;
    //qDebug() << " dat1=" << nextCmd4para1[nrOfCmds4InQueue] << " dat2=" << nextCmd4para2[nrOfCmds4InQueue]
    //             << " dat3=" << nextCmd4para3[nrOfCmds4InQueue]  << " dat4=" << nextCmd4para4[nrOfCmds4InQueue];
    p_nextFDcmdsInQueue++;
    return true;                // ok, will be sent
}

bool sendFDcmd_get(uint8_t *nextWrCmd, uint8_t *nextRdCmd, uint8_t *blockNum, uint8_t *dat1, uint8_t *dat2, uint8_t *dat3, uint8_t *dat4)
{
    uint8_t nn, ll;

    if (p_nextFDcmdsInQueue==0 || p_nextFDcmdsInQueue>FDCMD_STACKDEPTH)
        return false;           // not possible

    *nextWrCmd=nextFDwrCmd[0];
    *nextRdCmd=nextFDrdCmd[0];
    *blockNum=nextFDblkNr[0];
    *dat1=nextFDpara1[0];
    *dat2=nextFDpara2[0];
    *dat3=nextFDpara3[0];
    *dat4=nextFDpara4[0];
    //qDebug() << "cmd4  restored to send from [0];  pp=" << nrOfCmds4InQueue;
    //qDebug() << " data1: " << nextCmd4para1[0] << " data2: " << nextCmd4para2[0] <<
    //            " data3: " << nextCmd4para3[0] << " data4: " << nextCmd4para4[0];

    // move Puffer down by one element
    if (FDCMD_STACKDEPTH>0)
        ll=FDCMD_STACKDEPTH-1;
    else
        ll=0;
    for (nn=0; nn<ll; nn++)
    {
        nextFDwrCmd[nn]=nextFDwrCmd[nn+1];
        nextFDrdCmd[nn]=nextFDrdCmd[nn+1];
        nextFDblkNr[nn]=nextFDblkNr[nn+1];
        nextFDpara1[nn]=nextFDpara1[nn+1];
        nextFDpara2[nn]=nextFDpara2[nn+1];
        nextFDpara3[nn]=nextFDpara3[nn+1];
        nextFDpara4[nn]=nextFDpara4[nn+1];
    }
    if (p_nextFDcmdsInQueue>0)
        p_nextFDcmdsInQueue--;
    //qDebug() << "cmd4  after push down: pp=" << nrOfCmds4InQueue;
    return true;                // ok, will be sent
}

uint8_t check4FDshortCmd(void)
{
    // returns number of waiting command, max FDCMD_STACKDEPTH
    return p_nextFDcmdsInQueue;
}


uint8_t check4freeFDshortCmd(void)
{
    // returns number of free places in short-command stack
    return FDCMD_STACKDEPTH - p_nextFDcmdsInQueue;
}


// long command, 64 data bytes
static uint8_t longFDwrCmd[FDLONG_STACKDEPTH];
static uint8_t longFDrdCmd[FDLONG_STACKDEPTH];
static uint8_t longFDblkNr[FDLONG_STACKDEPTH];
static uint8_t longFDlength[FDLONG_STACKDEPTH];

static uint8_t  longFDpara[FDLONG_STACKDEPTH][64];
static uint8_t  p_longFDcmdsInQueue;


void longFDcmd_clrStack(void)
{
    uint8_t nn, mm;
    for (nn=0; nn<FDLONG_STACKDEPTH; nn++)
    {
        longFDwrCmd[nn]=0;
        longFDrdCmd[nn]=0;
        longFDblkNr[nn]=0;
        longFDlength[nn]=0;
        for (mm=0; mm<64; mm++)
            longFDpara[nn][mm]=0;
    }
    p_longFDcmdsInQueue=0;
}


bool longFDcmd_set(uint8_t nextWrCmd, uint8_t nextRdCmd, uint8_t blockNum, uint8_t length, uint8_t *data)
{
    // write Command to memory, wait for transport
    // data buffer size always 64! data[64], padded with 0
    uint8_t nn;
    if (p_longFDcmdsInQueue>=FDLONG_STACKDEPTH)
    {
        qDebug() << "cannot save cmd because stack is full";
        return false;           // not possible
    }
    longFDwrCmd[p_longFDcmdsInQueue]=nextWrCmd;
    longFDrdCmd[p_longFDcmdsInQueue]=nextRdCmd;
    longFDblkNr[p_longFDcmdsInQueue]=blockNum;
    longFDlength[p_longFDcmdsInQueue]=length;
    for (nn=0; nn<64; nn++)
        longFDpara[p_longFDcmdsInQueue][nn]=data[nn];

    p_longFDcmdsInQueue++;
    return true;                // ok, will be sent
}

bool longFDcmd_get(uint8_t *nextWrCmd, uint8_t *nextRdCmd, uint8_t *blockNum, uint8_t *length, uint8_t *data)
{
    uint8_t nn, mm, ll;

    if (p_longFDcmdsInQueue==0 || p_longFDcmdsInQueue>FDLONG_STACKDEPTH)
        return false;           // not possible

    *nextWrCmd= longFDwrCmd[0];
    *nextRdCmd= longFDrdCmd[0];
    *blockNum = longFDblkNr[0];
    *length   = longFDlength[0];
    for (mm=0; mm<64; mm++)
        data[mm] = longFDpara[0][mm];

    // move Puffer down by one element
    if (FDLONG_STACKDEPTH>0)
        ll=FDLONG_STACKDEPTH-1;
    else
        ll=0;
    for (nn=0; nn<ll; nn++)
    {
        longFDwrCmd[nn]  = longFDwrCmd[nn+1];
        longFDrdCmd[nn]  = longFDrdCmd[nn+1];
        longFDblkNr[nn]  = longFDblkNr[nn+1];
        longFDlength[nn] = longFDlength[nn+1];

        for (mm=0; mm<64; mm++)
            longFDpara[nn][mm] = longFDpara[nn+1][mm];

    }
    if (p_longFDcmdsInQueue>0)
        p_longFDcmdsInQueue--;

    return true;                // ok, will be sent
}

uint8_t check4FDlongCmd(void)
{
    // returns number of waiting command
    return p_longFDcmdsInQueue;
}

uint8_t check4freeFDlongCmd(void)
{
    // returns number of free places in command stack
    return FDLONG_STACKDEPTH - p_longFDcmdsInQueue;
}