
#include "grifo_node0_if.h"

#include "grifo_fw_blob.h"

#include "grifo_fw_tag.h"

#include <stdint.h>
#include <string.h>

//#include "rtlog.h"

using namespace grifo_fw;

enum { node0_cfg_bloks=2};

struct Node0Cx
{
    unsigned int ch_mask;
    unsigned int ch_mask_ovr;
    unsigned int actual_mask;
    unsigned int rec_mask[node0_cfg_bloks];
    unsigned int rec_pending_mask;
    unsigned int rec_header[node0_cfg_bloks];
    unsigned int rec_pending_header;

    unsigned int dst[Node0::dispatcher_configuration_t::max_dispatcher_channles];
    unsigned int dstHdr;

    grifo_fw::Node0::dispatcher_configuration_t cfg[node0_cfg_bloks];

	unsigned int extra_counter;
};

struct node0_hdr_xtra_info_t
{
	unsigned int xmarker;
	unsigned int xcookie;
	unsigned int xcounter;
	unsigned int xsize;
};

static Node0Cx autoMonitorable_N0Cx;
#define cx autoMonitorable_N0Cx

class grifo_fw_node0_blob_t: public grifo_fw::grifo_fw_blob_typed_t<grifo_fw::Node0::dispatcher_configuration_t, GRIFO_FW_BLOB_ENCODE_NAME('N','0', 'C', '0')>
{
};

struct node0_header_t
{
    enum {data_size=4096/4};

    uint32_t header_type;
    uint32_t header_counter;
    uint32_t ch_mask;

    uint32_t data[data_size];
};

class grifo_fw_header_blob_t: public grifo_fw::grifo_fw_blob_typed_t<node0_header_t, GRIFO_FW_BLOB_ENCODE_NAME('H','D', 'R', '0')>
{
};

grifo_fw_node0_blob_t* grifo_fw_node0_blob;
grifo_fw_header_blob_t* grifo_fw_header_blob;


static TagGenerator headerTagGenerator;

void Node0::configureAsynchAcquisition(unsigned int samples_56Mhz)
{
#if 0
    Node0::dispatcher_configuration_t& cfg=grifo_fw_node0_blob->payload;
    cfg.cr.asynch_win_size=samples_56Mhz;
    cfg.cr.spare_high=0;
    cfg.cr.spare_low=0;
#endif
    for(int i=0; i<2; ++i)
    {
        dispatcher_configuration_t& cfg=cx.cfg[i];

        cfg.cr.asynch_win_size=samples_56Mhz;
        cfg.cr.synch=0;
    }
}

void Node0::configureSynchAcquisition(bool synch)
{
#if 0
    Node0::dispatcher_configuration_t& cfg=grifo_fw_node0_blob->payload;
    cfg.cr.synch=synch ? 1 : 0;
    cfg.cr.spare_high=0;
    cfg.cr.spare_low=0;
#endif
    for(int i=0; i<2; ++i)
    {
        dispatcher_configuration_t& cfg=cx.cfg[i];

        cfg.cr.synch=synch ? 1 : 0;
        cfg.ch_enable=cx.ch_mask;
        cfg.cr.spare_high=0;
        cfg.cr.spare_low=0;
    }

}

static Node0::dispatcher_configuration_t& cfgGet(unsigned int pingpong)
{
    pingpong= pingpong & 1;

    //unsigned int pp=pingpong & 1;
    //Node0::dispatcher_configuration_t& cfg=grifo_fw_node0_blob->payload;
    return cx.cfg[pingpong];
}


void Node0::initializeChannels(unsigned int dstId, const void* address, unsigned int doorbell)
{
    for(int i=0; i<2; ++i)
    {
        configureHeaderDestination(i, dstId, address, doorbell);
        for(int n=0; n<dispatcher_configuration_t::max_dispatcher_channles; ++n)
        {
            configureChannelDestination(i, n, dstId, address);
        }
    }
}

void Node0::configureHeaderDestination(unsigned int pingpong, unsigned int dstId, const void* address, unsigned int doorbell)
{
    pingpong= pingpong & 1;

    dispatcher_configuration_t& cfg=cfgGet(pingpong);

    cfg.cr.synch=0;

    cfg.doorbell=doorbell|(doorbell<<16);
    cfg.header.dst_id=dstId;
    cfg.header.dst_address=reinterpret_cast<uint32_t>(address);
    cfg.hdr_doorbell[0]=0;
    cfg.hdr_doorbell[1]=0;
    cx.rec_header[pingpong]=0;
    cx.rec_mask[pingpong]=0;
    cx.dstHdr=dstId;
}

void Node0::configureChannelDestination(unsigned int pingpong, unsigned int ch, unsigned int dstId, const void* address)
{
    pingpong= pingpong & 1;

    dispatcher_configuration_t& cfg=cfgGet(pingpong);

    cfg.ch[ch].dst_id=dstId;
    cfg.ch[ch].dst_address=reinterpret_cast<uint32_t>(address);

    cx.dst[ch]=dstId;
}

void Node0::configureChannels(unsigned int pingpong, unsigned int dstId, const void* address, unsigned int ch_gap)
{
    const uint8_t* p=reinterpret_cast<const uint8_t*>(address);

    for(int i=0; i<dispatcher_configuration_t::first_det_capture_ch; ++i)
    {
       configureChannelDestination(pingpong, i, dstId, p);
       //if (i<dispatcher_configuration_t::num_of_hr_channels)
       //    configureChannelDestination(pingpong, i+dispatcher_configuration_t::first_hr_ch, dstId, p); //overlaps first low res ch
       p+=ch_gap;
    }
    for(int i=dispatcher_configuration_t::first_det_capture_ch; i<dispatcher_configuration_t::first_hr_ch; ++i)
    {
       configureChannelDestination(pingpong, i, dstId, p); //memory overlaps!
    }

    //HR Channel mapped to SUM, DAZ, DEL
    p=reinterpret_cast<const uint8_t*>(address);	//The first is the Guard
    configureChannelDestination(pingpong, dispatcher_configuration_t::first_hr_ch, dstId, &p[ch_gap*1]);	//SUM (DC1)
    configureChannelDestination(pingpong, dispatcher_configuration_t::first_hr_ch+1, dstId, &p[0]);	//DAZ (DC2)
    configureChannelDestination(pingpong, dispatcher_configuration_t::first_hr_ch+2, dstId, &p[ch_gap*2]);	//DAZ (DC3)
}

void Node0::configureCaptureChannel(unsigned int pingpong, unsigned int ch, unsigned int dstId, const void* address)
{
    configureChannelDestination(pingpong, ch, dstId, address);
}

void Node0::enableChannels(unsigned int mask)
{
    cx.ch_mask=0;
    for(unsigned int i=0; mask; ++i, mask>>=1)
    {
        if (mask & 1)
            cx.ch_mask|=(1<<i);
    }
    cx.actual_mask=cx.ch_mask_ovr ? cx.ch_mask_ovr : cx.ch_mask;
}

void Node0::enableChannel(unsigned int ch_num, bool enable)
{
    if (enable)
        cx.ch_mask|=(1<<ch_num);
    else
        cx.ch_mask&=~(1<<ch_num);

    cx.actual_mask=cx.ch_mask_ovr ? cx.ch_mask_ovr : cx.ch_mask;

    for(unsigned int i=0; i<node0_cfg_bloks; ++i)
    {
        dispatcher_configuration_t& cfg=cfgGet(i);
        cfg.ch_enable=cx.actual_mask; //|=cx.ch_mask;
    }
}

void Node0::headerReset()
{

}

void Node0::setRecMask(unsigned int m)
{
	cx.rec_pending_mask=m;
}

void Node0::setRecHeader(bool enable)
{
	cx.rec_pending_header=enable ? 1 : 0;
}

void Node0::dispatcherUpdate(unsigned int pingpong)
{
    if (grifo_fw_node0_blob)
    {
        unsigned int index=pingpong & 1;

        dispatcher_configuration_t& cfg=cfgGet(pingpong);

        cx.actual_mask=cx.ch_mask_ovr ? cx.ch_mask_ovr : cx.ch_mask;

        cfg.ch_enable=cx.actual_mask; //cfg.ch_enable=cx.ch_mask;

    	//WARNING: be sure to not use rec node_id when destinaiton is ZynQ
    	//TODO: remove hard-coded condition and find a way to make it configurable
    	//Note that recording control via override of Node0 Cfg DOES not work after an IBIT
    	if (cfg.ch[0].dst_id==0x56)
    	{
        	for(int i=0; i<16; ++i)
        	{
        		cfg.ch[i].dst_id=cfg.ch[i].dst_id & (~0x80);
        	}
    		cx.rec_mask[index]=0;
        	cx.rec_header[index]=0;
    		cfg.header.dst_id&=(~0x80);
    	}
    	else
    	{
            if (cx.rec_header[index]!=cx.rec_pending_header)
            {
            	cx.rec_header[index]=cx.rec_pending_header;
            	if (cx.rec_header[index])
            		cfg.header.dst_id|=0x80;
            	else
            		cfg.header.dst_id&=(~0x80);
            }

			if (cx.rec_mask[index]!=cx.rec_pending_mask)
			{
				cx.rec_mask[index]=cx.rec_pending_mask;
				for(int i=0; i<16; ++i)
				{
					if (cx.rec_mask[index] & (1<<i))
						cfg.ch[i].dst_id=cfg.ch[i].dst_id|0x80;
					else
						cfg.ch[i].dst_id=cfg.ch[i].dst_id & (~0x80);
				}
			}
    	}

        //WARNING: never use rec node_id for header doorbell
    	cfg.hdr_doorbell[0]=cfg.header.dst_id&=(~0x80);
    	cfg.hdr_doorbell[1]=cfg.hdr_doorbell[0];

        grifo_fw_node0_blob->header.autoFinalize(pingpong);
        grifo_fw_node0_blob->payload=cfg;
        //memset(grifo_fw_node0_blob->payload.spare, 0, sizeof grifo_fw_node0_blob->payload.spare);
    }
}

int Node0::headerSet(const void* data, unsigned int size)
{
    grifo_fw_header_blob->payload.header_type=GRIFO_FW_BLOB_ENCODE_NAME('E', 'V', 'O', '0');

    grifo_fw_header_blob->payload.header_counter=headerTagGenerator.getNextTag();
    grifo_fw_header_blob->payload.ch_mask=autoMonitorable_N0Cx.actual_mask; //ch_mask;

    if (size>sizeof grifo_fw_header_blob->payload.data)
        size=sizeof grifo_fw_header_blob->payload.data;

    const uint32_t* p=reinterpret_cast<const uint32_t*>(data);

    //@#AC 24/08/22 copy only required data instead of max data (preventing also walking on maybe uninitialized memory)
    //for(unsigned int i=0; i<(size/4)/*node0_header_t::data_size*/; ++i)
    //{
    //    grifo_fw_header_blob->payload.data[i]=p[i];
    //}
    memcpy(&grifo_fw_header_blob->payload.data[0], p, size);

    unsigned int wsize=(size+3)>>2;//round up to 32 bits word
    grifo_fw_header_blob->header.work_size=wsize;

    grifo_fw_header_blob->header.autoFinalize(headerTagGenerator.getCurrentArea());

    return grifo_fw_header_blob->header.work_size;
}


int Node0::headerAdd(const char* extra_cookie, const void* data, unsigned int size)
{
	unsigned int xcookie=(extra_cookie[3]<<24)|(extra_cookie[2]<<16)|(extra_cookie[1]<<8)|(extra_cookie[0]);
	return headerAdd(xcookie, data, size);
}

int Node0::headerAdd(unsigned int extra_cookie, const void* data, unsigned int size)
{
	unsigned int pos=grifo_fw_header_blob->header.work_size;
	if (pos>=node0_header_t::data_size)
	{
		//rtlog_printf("N0", rtlog_warning|rtlog_breaking_news, "Header too big: %u", pos);
		return -1;
	}
	unsigned int wsize=((size+sizeof(node0_hdr_xtra_info_t))+3)>>2;
	if ((pos+wsize)>=node0_header_t::data_size)
	{
		//rtlog_printf("N0", rtlog_warning|rtlog_breaking_news, "Header too big: %u + %u", pos, wsize);
		return -2;
	}

	node0_hdr_xtra_info_t* p=reinterpret_cast<node0_hdr_xtra_info_t*>(&(grifo_fw_header_blob->payload.data[pos]));
	p->xmarker=0x20545845;//"EXT "
	p->xcookie=extra_cookie;
	p->xcounter=cx.extra_counter++;
	p->xsize=size;
	memcpy(&p[1], data, size);

	grifo_fw_header_blob->header.work_size+=wsize;

	return grifo_fw_header_blob->header.work_size;
}

int Node0::headerSetExtra(const void* data, unsigned int size, unsigned int extra_cookie, const void* extra, unsigned int extra_size)
{
	headerSet(data, size);
	return headerAdd(extra_cookie, extra, extra_size);
#if 0
	static unsigned int extra_counter;
	struct hdr_xtra_info_t__ {
		unsigned int xmarker;
		unsigned int xcookie;
		unsigned int xcounter;
		unsigned int xsize;
	} *p=0;

	if (size<((sizeof grifo_fw_header_blob->payload.data)+sizeof(*p)))
    {
		p=reinterpret_cast<hdr_xtra_info_t__*>(&(grifo_fw_header_blob->payload.data[size/4]));
		p->xmarker=0x45585458;//"EXTX"
		p->xcookie=extra_cookie;
		p->xcounter=extra_counter++;
		p->xsize=extra_size;
		unsigned int free_space=(sizeof grifo_fw_header_blob->payload.data)-(size+sizeof(*p));
		unsigned int xsize=(extra_size<free_space) ? extra_size : free_space;
		memcpy(&p[1], extra, xsize);
    }
	headerSet(data, size);
#endif
}

void Node0::headerClear()
{
	memset(grifo_fw_header_blob->payload.data, 0, (unsigned int) sizeof grifo_fw_header_blob->payload.data);
}

void Node0::generateHeaderTestPattern()
{
    static unsigned int test_pattern_counter;
    grifo_fw_header_blob->payload.header_type=0xFFFFFFFFU;  //an invalid header
    grifo_fw_header_blob->payload.header_counter=headerTagGenerator.getNextTag();
    grifo_fw_header_blob->payload.ch_mask=cx.actual_mask; //ch_mask;

    unsigned int test_data=test_pattern_counter<<16;

    for(unsigned int i=0; i<node0_header_t::data_size; ++i)
    {
        grifo_fw_header_blob->payload.data[i]=test_data|(i);
    }
    ++test_pattern_counter;
}

namespace grifo_fw
{
    void dispatcher_initialize_raw_default_blob(void* m)
    {
        grifo_fw_node0_blob=reinterpret_cast<grifo_fw_node0_blob_t*>(m);
        if (grifo_fw_node0_blob)
        {
            grifo_fw_node0_blob->initialize();
            grifo_fw_node0_blob->clear();

            memset((void*)&grifo_fw_node0_blob->payload, 0, (unsigned int)(sizeof grifo_fw_node0_blob->payload));
        }
    }

    void header_initialize_raw_default_blob(void* m)
    {
        grifo_fw_header_blob=reinterpret_cast<grifo_fw_header_blob_t*>(m);
        if (grifo_fw_header_blob)
        {
            grifo_fw_header_blob->initialize();
            grifo_fw_header_blob->clear();

            memset((void*)&grifo_fw_header_blob->payload, 0, (unsigned int)(sizeof grifo_fw_header_blob->payload));
        }
    }

}

const grifo_fw::Node0::dispatcher_configuration_t* Node0::node0RegisterImage()
{
    return &grifo_fw_node0_blob->payload;
}
