/*
 * b1553_ip.cpp
 *
 */

/*
 * About ZynQ Interrupt:
 *   PL (Programming Logic) can generate 16 interrupt, IRQF2P[15:0] with interrupt id from 61 to 68 and 84 to 91
 *    IRQF2P[0:2]  -> spi_status_0[31:39] -> IRQ ID# 63..61
 *    IRQF2P[7:3]  -> spi_status_1[4:0]   -> IRQ ID# 68..64
 *    IRQF2P[15:8] -> spi_status_1[27:20] -> IRQ ID# 84..91
 * The IP1553 shall map its interrupt to one of the IRQF2P, type "HIGH LEVEL". The interrupt will be active when Interrupt ACk (IACK) and
 * Interrupt Queue pointer (IQP) are different.
 *
 * Interrupt ISR shall traverse the list and updated the pointer one terminated:
 *   SAVED_IQP = IQP
 *   SAVED_IACK= IACK
 *   for(;SAVED_IACK!=SAVED_IACK; SAVED_IACK=(SAVED_IACK+1) mod QUEUE_DEPTH)
 *      process interrupt descriptor in slot SAVED_IACK
 *   IACK=SAVED_IACK;
 *   if (IACK!=SAVED_IQP) redo
 *
 *   Note that:
 *     * instead of using a SAVED_IQP, the IQP could be directly used. In this case the final check for "redo" should not be necessary
 *     * It could be a good idea to write an invalid descriptor after the interrupt processing. A check for invalid descriptor could be
 *       useful to verify if the processing and interrupt updating is correct.
 */

#include "b1553_driver.h"

#include "b1553_fw_types.h"

#include "bsp_platform.h"

#include "bsp_interrupts.h"

#include "rtlog.h"
#define LID	"AVB"

#include "eif_discrete_signals.h"

#include <string.h>

#ifndef __MINGW32__
//#define START_ENABLED
#endif

//#define ALL_SA_VALID 1

#define RT_ISR_PL_VECTOR	5
#define FW_ISR_VECTOR (RT_ISR_PL_VECTOR+61)

#define FW_BASE_ADDRESS (0x50000000)

#define FW_MEM_AREA		(FW_BASE_ADDRESS+0x00800000) //(0x40800000)
#define FW_CTRL_AREA	(FW_BASE_ADDRESS+0x00B00000) //(0x40B00000)
#define FW_CMD_AREA		(FW_BASE_ADDRESS+0x00A00000) //(0x40A00000)

#ifdef __MINGW32__
static unsigned int fw_hwsw_reg;
static unsigned int fw_hwsw_latch_reg;
#define FW_HWSW_REG	(&fw_hwsw_reg)
#define FW_HWSW_LATCH_REG	(&fw_hwsw_latch_reg)
#else
#define FW_HWSW_REG	0x5008000C
#define FW_HWSW_LATCH_REG	0x50080004
#endif


//Only for PC HOST: attach to CP 1553 Device
#ifdef __MINGW32__

extern void Pc1553DriverInitialize(unsigned int rtaddress);

extern void Pc1553DriverMsgDefine(unsigned int sa, int rxtx, unsigned int wc_min, unsigned int wc_max, unsigned int flags);

extern void Pc1553DriverStart();

extern void Pc1553DriverMsgWrite(unsigned int sa, int rxtx, const void* data, unsigned int wc);

extern int Pc1553DriverMsgRead(unsigned int sa, int rxtx, void* data, unsigned int wc);

static void Pc1553DriverMsgReadHelper(unsigned int sa, unsigned int rxtx)
{
	uint16_t buffer[32];

	if (rxtx)
		return;

	int wc=Pc1553DriverMsgRead(sa, 0, buffer, 32);
	if (wc>=0)
	{
		if (wc>0)
			b1553_drv_simulate_msg(0, sa, rxtx, wc, buffer);
	}
}

#else


static void Pc1553DriverMsgWrite(unsigned int , int , const void* , unsigned int )
{
}

static void Pc1553DriverMsgReadHelper(unsigned int, int)
{
}

void Pc1553DriverInitialize(unsigned int)
{
}

static void Pc1553DriverMsgDefine(unsigned int , int , unsigned int , unsigned int , unsigned int )
{
}

static void Pc1553DriverStart()
{
}

#endif

class b1553_critical_region
{
public:
	unsigned int sts;
	inline b1553_critical_region()
	{
		sts=bsp_global_interrupts_disable();
	};
	inline ~b1553_critical_region()
	{
		bsp_global_interrupts_restore(sts);
	}
};

//#define ILOCK() 	((void)0)
//#define IUNLOCK()	((void)0)

#ifndef START_ENABLED

static b1553_fw_memory_t b1553_drv_ip_memory_dummy __attribute__((aligned(4096)));

//b1553_fw_memory_t* b1553_drv_ip_memory_=&b1553_drv_ip_memory_dummy;
volatile b1553_fw_memory_t* b1553_drv_ip_memory_=&b1553_drv_ip_memory_dummy;

static b1553_fw_cmd_regs_t b1553_drv_ip_cmd_regs_dummy;

volatile b1553_fw_cmd_regs_t* b1553_drv_ip_cmd_regs_=&b1553_drv_ip_cmd_regs_dummy;

static b1553_fw_ctrl_regs_t b1553_drv_ip_ctrl_regs_dummy;

volatile b1553_fw_ctrl_regs_t* b1553_drv_ip_ctrl_regs_=&b1553_drv_ip_ctrl_regs_dummy;

#else

//b1553_fw_memory_t* b1553_drv_ip_memory_=&b1553_drv_ip_memory_dummy;
volatile b1553_fw_memory_t* b1553_drv_ip_memory_=reinterpret_cast<b1553_fw_memory_t*>(FW_MEM_AREA);

volatile b1553_fw_cmd_regs_t* b1553_drv_ip_cmd_regs_=reinterpret_cast<b1553_fw_cmd_regs_t*>(FW_CMD_AREA);

volatile b1553_fw_ctrl_regs_t* b1553_drv_ip_ctrl_regs_=reinterpret_cast<b1553_fw_ctrl_regs_t*>(FW_CMD_AREA);

#endif


//#define FW_CTRL_AREA	((unsigned int)&b1553_drv_ip_ctrl_regs_dummy)
//#define FW_CMD_AREA	    ((unsigned int)&b1553_drv_ip_cmd_regs_dummy)
//#define FW_MEM_AREA	    ((unsigned int)&b1553_drv_ip_memory_dummy)
#define RTA_DEFAULT	20
static unsigned int sw_rt=RTA_DEFAULT;

static volatile unsigned int ip_lock;
static volatile int b1553_simulated_msg;

static inline uint32_t woffset(const volatile void* a)
{
	uint32_t offset=reinterpret_cast<uint32_t>(a)-(reinterpret_cast<uint32_t>(b1553_drv_ip_memory_));
	return offset;
}

static inline void* waddress(unsigned int a)
{
	void* m=reinterpret_cast<void*>(reinterpret_cast<uint32_t>(b1553_drv_ip_memory_)+a);
	return m;
}

volatile unsigned int b1553_fw_memory_size_;

static unsigned int check_align(volatile void* a, unsigned int align_requirement)
{
	uint32_t tmp=reinterpret_cast<uint32_t>(a);
	uint32_t align_mask=(0xFFFFFFFF>>(32-align_requirement));
	tmp=tmp & align_mask;
	return tmp;
}

static unsigned int rta_parity(unsigned int ta)
{
	unsigned int v=ta & 0x1F;
	unsigned int c=v^(v>>1);
	c=c^(c>>2);
	c=c^(c>>4);
	return c & 1 ? 0 : 1;
}

volatile unsigned int b1553_drv_ip_dummy__;

static void sys_thread_delay(unsigned int n) __attribute__((noinline));
static void sys_thread_delay(unsigned int n)
{
	for(unsigned int i=0; i<n*10; ++i)
		b1553_drv_ip_dummy__+=b1553_drv_ip_dummy__+i;
}


static void hwsw_hardwired_set(int sw_enable)
{
	if (!bsp_is_evm())
	{
		volatile unsigned int* const reg_cfg_2=(unsigned int*)(FW_HWSW_REG);
		volatile unsigned int* const reg_cfg_1=(unsigned int*)(FW_HWSW_LATCH_REG);
		volatile uint32_t* reg_1553_2 = &(b1553_drv_ip_ctrl_regs_->ticw);

        *reg_cfg_2&=(~1); //force bit 9 HWTHW of TERMINAL DIRECT CONTROL WORD to 0

        if (!sw_enable)
        {
        	(*reg_1553_2)|=(1<<9); //force bit 9 THW of TERMINAL INTERFACE CONTROL WORD to 1
        }
        else
        {
        	(*reg_1553_2)&=~(1<<9); //force bit 9 THW of TERMINAL INTERFACE CONTROL WORD to 1
        }

        (*reg_cfg_1)|=(1<<2); //active latch of hw address

		sys_thread_delay(20);

        //uint32_t value = *reg_1553_1;
        //uint32_t parity=(value>>10)&1;
        //uint32_t address=(value>>11)&0x1f;

        (*reg_cfg_1)&=~(1<<2); //active latch of hw address
	}
#if 0
		reg_cfg_1&=~(1<<2);	//disable latch

		for(int i=0; i<10; ++i)
			b1553_drv_ip_dummy__=reg_cfg_1;

		if (sw_enable)
		{
			reg_cfg_2|=1;
		}
		else
		{
			reg_cfg_2&=~1;
		}

		reg_cfg_1|=(1<<2);	//enable latch

		for(int i=0; i<10; ++i)
			b1553_drv_ip_dummy__=reg_cfg_1;

		reg_cfg_1|=(1<<2);	//enable latch
	}
#endif
}

static void initialize()
{
#if 1 //ndef  __MINGW32__
	rtlog_printf(LID, rtlog_info/*|rtlog_breaking_news*/, "Init: ctrl=0x%8.8X, cmd=0x%8.8X, m=0x%8.8X (%u)",
			reinterpret_cast<unsigned int>(b1553_drv_ip_ctrl_regs_),
			reinterpret_cast<unsigned int>(b1553_drv_ip_cmd_regs_),
			reinterpret_cast<unsigned int>(b1553_drv_ip_memory_),
			sizeof *b1553_drv_ip_memory_);

	if (b1553_drv_ip_memory_!=&b1553_drv_ip_memory_dummy)
	{
		unsigned int alignament=check_align(b1553_drv_ip_memory_->descriptors, 16);
		int align_severity=alignament ? (rtlog_breaking_news|rtlog_error) : rtlog_success;
		rtlog_printf(LID, align_severity, " descriptor alignment: 0x%8.8X", alignament);

		alignament=check_align(b1553_drv_ip_memory_->buffers, 8);
		align_severity=alignament ? (rtlog_breaking_news|rtlog_error) : rtlog_success;
		rtlog_printf(LID, align_severity, " buffer alignment: 0x%8.8X", alignament);

		alignament=check_align(b1553_drv_ip_memory_->iq, 14);
		align_severity=alignament ? (rtlog_breaking_news|rtlog_error) : rtlog_success;
		rtlog_printf(LID, align_severity, " iq history alignment: 0x%8.8X", alignament);

		alignament=check_align(b1553_drv_ip_memory_->thistory, 14);
		align_severity=alignament ? (rtlog_breaking_news|rtlog_error) : rtlog_success;
		rtlog_printf(LID, align_severity, " msg history alignment: 0x%8.8X", alignament);
	}
#endif

	b1553_drv_ip_cmd_regs_->stop=1;


	hwsw_hardwired_set(0);

	//Initialize memory
	//memset(&b1553_drv_ip_memory_dummy, 0, sizeof b1553_drv_ip_memory_dummy);
    //memset(&b1553_drv_ip_cmd_regs_dummy, 0, sizeof b1553_drv_ip_cmd_regs_dummy);

	//memset(const_cast<b1553_fw_memory_t*>(b1553_drv_ip_memory_), 0, sizeof *b1553_drv_ip_memory_);
	{
		memset((void*)&b1553_drv_ip_memory_->descriptors, 0, sizeof b1553_drv_ip_memory_->descriptors);
		memset((void*)&b1553_drv_ip_memory_->iq, 0, sizeof b1553_drv_ip_memory_->iq);
		memset((void*)&b1553_drv_ip_memory_->thistory, 0, sizeof b1553_drv_ip_memory_->thistory);
		//memset((void*)&b1553_drv_ip_memory_->descriptors_bc, 0, sizeof b1553_drv_ip_memory_->descriptors_bc);
	}

	b1553_fw_sadd_descriptor_t d;
	d.cw.raw=0;
	d.reserved=0;
	d.terminal_bufffer_pointer=woffset(&(b1553_drv_ip_memory_->buffers[0][0]));
	d.host_buffer_pointer=d.terminal_bufffer_pointer;

	d.cw.str.dsize=32;

#ifndef ALL_SA_VALID
	d.cw.str.esc=1; //enable size control
	d.cw.str.dsize=0; //size 0, that is invalid
	d.cw.str.ill=1;

	volatile b1553_fw_sadd_descriptor_t* p=&(b1553_drv_ip_memory_->descriptors[0][0]);
	for(int i=0; i<(b1553_fw_memory_t::desc_blocks*b1553_fw_memory_t::desc_num); ++i)
	{
		p[i].cw.raw=d.cw.raw;
		p[i].host_buffer_pointer=d.host_buffer_pointer;
		p[i].terminal_bufffer_pointer=d.terminal_bufffer_pointer;
		p[i].reserved=d.reserved;
	}
#endif

	for(int i=0; i<b1553_fw_memory_t::desc_blocks; ++i)
	{
		for(int j=0; j<32; ++j)
		{
			d.terminal_bufffer_pointer=woffset(&b1553_drv_ip_memory_->buffers[i][j][0]);
			d.host_buffer_pointer=woffset(&b1553_drv_ip_memory_->buffers[i][j][1]);

			b1553_drv_ip_memory_->descriptors[i][j].cw.raw=d.cw.raw;
			b1553_drv_ip_memory_->descriptors[i][j].host_buffer_pointer=d.host_buffer_pointer;
			b1553_drv_ip_memory_->descriptors[i][j].terminal_bufffer_pointer=d.terminal_bufffer_pointer;
			b1553_drv_ip_memory_->descriptors[i][j].reserved=d.reserved;
		}
	}

	//uint32_t tdcw=b1553_drv_ip_ctrl_regs_->tdcw;
	//unsigned int hw_forced=(tdcw & (1<<9)) ? 0 : 1;
	unsigned int hw_forced=sw_rt ? 0 :1 ;

	unsigned int used_sw_rt=sw_rt ? sw_rt : RTA_DEFAULT;

	//Initialize controls
	b1553_drv_ip_ctrl_regs_->tdcw=
			 (0<<0)	//Busy: disabled
			|(1<<6)	//TX DISABLE;
			|(1<<9) //HWTHW: terminal address by SW=1
			;

	unsigned int ta_parity=rta_parity(used_sw_rt);

	b1553_drv_ip_ctrl_regs_->ticw=
			 (0<<0)	//NO bc mode
			|(1<<1)	//RT mode
			|(1<<4) //History storage disable //|(0<<4) //History storage enable
			|(0<<6)	//watch dog: enabled
			|(1<<7) //no CBIT
			|(0<<8) // no self test
			|(hw_forced<<9) //Terminal address: SW=0
			|(ta_parity<<10)	//TA parity=1
			|(used_sw_rt<<11)   //TA 20
			;

	b1553_drv_ip_ctrl_regs_->iqp=woffset(&b1553_drv_ip_memory_->iq[0][0]);
	b1553_drv_ip_ctrl_regs_->iack=woffset(&b1553_drv_ip_memory_->iq[0][0]);

	b1553_drv_ip_ctrl_regs_->sbap=woffset(&b1553_drv_ip_memory_->descriptors[0][0]);
    b1553_drv_ip_ctrl_regs_->thp=woffset(&b1553_drv_ip_memory_->thistory[0][0]);

	b1553_drv_ip_ctrl_regs_->imw=0;
			 //(1<<0)		//Sync w DW
			//|(1<<8)		//Such w/o DW
			//;

	b1553_drv_ip_ctrl_regs_->imcw=	//Illegalize MC - all valid but:
			 (1<<11);	//Dynamic Bus Control

	b1553_fw_memory_size_=sizeof *b1553_drv_ip_memory_;

	unsigned int hwrt=(b1553_drv_ip_ctrl_regs_->tdcw>>11) & 0x1F;
	unsigned int swrt=(b1553_drv_ip_ctrl_regs_->ticw>>11) & 0x1F;

	rtlog_printf(LID, rtlog_success|rtlog_breaking_news, "AVB Initialize: RTA=%u - %s",
			hw_forced ? hwrt : swrt,
			hw_forced ? "HARD" : "SOFT");
}


static b1153_drv_synch_handler_t synch_handler;

static bsp_isr_extra_event_info_t avb_isr_infos={"synch", {0,0}};

static bsp_isr_extra_info_t avb_isr_extra_info={1, 0, 0, &avb_isr_infos};

static void process_iqueue(volatile b1553_fw_ctrl_regs_t* ctrl)
{
	static unsigned int irq_errors;
	static unsigned int irq_phantom;
	static unsigned int sync_counter;
	static unsigned int intr_counter;

	const unsigned int imaks_1024=0x3FF;

	unsigned int served=0;

	++intr_counter;

	uint32_t iack=ctrl->iack;

	if (ctrl->iqp==iack)
	{
		if (irq_phantom<4)
			++irq_phantom;
		else
			++avb_isr_extra_info.errors;

		if (irq_errors<500)
			++irq_errors;
		else
		{
			bsp_interrupt_enable(FW_ISR_VECTOR, 0);	//Protection: disable interrupt!
			rtlog_const_message("AVB", rtlog_error|rtlog_breaking_news, "INTR overload: inverted?");
		}
		return;
	}

	irq_errors=0;
	irq_phantom=0;

	while(ctrl->iqp!=iack) //b1553_drv_ip_ctrl_regs_->iack)
	{
		uint32_t iqp=ctrl->iqp;
		//iack=b1553_drv_ip_ctrl_regs_->iack;

		unsigned int start=(iack>>4) & imaks_1024;
		unsigned int stop=(iqp>>4) & imaks_1024;

		for(;start!=stop; start=(start+1) & imaks_1024)
		{
			//serve interrupt:
			uint32_t cw=b1553_drv_ip_memory_->iq[start][0];

			unsigned int is_not_mc=cw & (1<<15);
			unsigned int sa=cw & 0x0FF;
			unsigned int dw=0;
			unsigned int mc=0;
			if (!is_not_mc)
			{
				if (synch_handler)
				{
					++sync_counter;
					avb_isr_infos.data[0]=sync_counter;
					if (sa==31)	//TODO: WARNING - no way here to understand if it is a MC 17 or 0, so assume 0=SYNCH without, 31=SYNCH with!!!!
					{
						dw=(ctrl->last_synch_word) & 0x0FFFF;
						synch_handler(1, dw);
					}
					else
					{
						synch_handler(0, 0);
					}
				}
			}
			rtlog_printf(LID, rtlog_trace, "[%u:%u] INTR: %s:%u:%u (0x%4.4X)", intr_counter, sync_counter, is_not_mc ? "MSG" : "MC", sa, mc, dw);

			++avb_isr_infos.data[1];

			++served;
			if ((served>2))
			{
				ctrl->iack=iqp;
				return;
			}

		}

		iack=iqp;
	}

	ctrl->iack=iack;

	if (served==0)
		++avb_isr_extra_info.fakes;
}

static void isr(unsigned int /*vector*/)
{
	process_iqueue(b1553_drv_ip_ctrl_regs_);
}

static inline unsigned int safix(unsigned int sa)
{
	return sa;
}

static void init_msg(unsigned int sa, unsigned int rxtx, unsigned int wc, unsigned int db, unsigned int intr)
{
	sa=safix(sa);

	b1553_fw_sadd_control_word_union_t cw;
	cw.raw=b1553_drv_ip_memory_->descriptors[rxtx][sa].cw.raw;
	cw.str.dsize=wc;
	cw.str.ill=0;
	cw.str.edb=db ? 1 : 0;
	cw.str.esc=1;//Enable size control
	cw.str.eei=0;  //error interrupt
	cw.str.eeti=intr ? 1 : 0;

	b1553_drv_ip_memory_->descriptors[rxtx][sa].cw.raw=cw.raw;

	Pc1553DriverMsgDefine(sa, rxtx, 0, wc, intr);
}

static unsigned int rx_msg_update(unsigned int sa, uint16_t* data, unsigned int wc, bool no_update=false)
{
	sa=safix(sa);

	Pc1553DriverMsgReadHelper(sa, 0);
#if 1
	//WARNING: does not use double buffer disable:
	// * It does not work if the execution time is greater then the message frequency
	// * It does not work if during execution, two message on with SA arrives
	if (wc==0)
	{
		wc=b1553_drv_ip_memory_->descriptors[0][sa].cw.str.dsize;
	}

	volatile b1553_fw_buffer_t* b=reinterpret_cast<b1553_fw_buffer_t*>(waddress(b1553_drv_ip_memory_->descriptors[0][sa].host_buffer_pointer));
	memcpy(data, const_cast<const uint16_t*>(b->data), wc*2);

	//Note that the temporary is used to try to force only one read or write cycle to the fpga shared memory
	uint32_t id_raw_id=b->identify.raw;
	b1553_fw_buffer_t::identify_union_t id;
	id.raw=id_raw_id;

	bool updated=id.id.wc!=0; //b->identify.id.wc!=0;

	if (updated && (!no_update))
	{
		id.id.wc=0;
		b->identify.raw=id.raw; //b->identify.id.wc=0;	//mark as read
	}

	if (updated)
	{
		if (b1553_simulated_msg==0)
		{
			b1553_simulated_msg=-1;
			b1553_drv_enable_interrupts(0);
		}
	}

	return updated;
#else
	//Use double buffer disable to guarantee mutual exclusion with FPGA updated.
	//WARNING: it seems the FPGA IP does not keep pending host_buffer_pointer updated, so fi a message arrives, it will be lost
	b1553_fw_sadd_control_word_union_t tmp;
	tmp.raw=b1553_drv_ip_memory_->descriptors[0][sa].cw.raw;

	uint32_t cw=tmp.raw; //b1553_drv_ip_memory_->descriptors[0][sa].cw.raw;

	tmp.str.edb=0;

	b1553_drv_ip_memory_->descriptors[0][sa].cw.raw=tmp.raw; //b1553_drv_ip_memory_->descriptors[0][sa].cw.str.edb=0;	//Disable double buffer

	volatile b1553_fw_buffer_t* b=reinterpret_cast<b1553_fw_buffer_t*>(waddress(b1553_drv_ip_memory_->descriptors[0][sa].host_buffer_pointer));

	bool updated=b->identify.id.wc!=0;

	if (wc==0)
		wc=b1553_drv_ip_memory_->descriptors[0][sa].cw.str.dsize;

	if (updated)
	{
		if (b1553_simulated_msg==0)
		{
			b1553_simulated_msg=-1;
			b1553_drv_enable_interrupts(0);
		}
	}

	memcpy(data, const_cast<const uint16_t*>(b->data), wc*2);

	if (!no_update)
		b->identify.id.wc=0;	//mark as read

	b1553_drv_ip_memory_->descriptors[0][sa].cw.raw=cw;	//Re-enable double buffer

	return updated;
#endif
}

static void tx_msg_update(unsigned int sa, const uint16_t* data, unsigned int wc)
{
	sa=safix(sa);

	b1553_fw_buffer_t* b=reinterpret_cast<b1553_fw_buffer_t*>(waddress(b1553_drv_ip_memory_->descriptors[1][sa].host_buffer_pointer));

	if (wc==0)
		wc=b1553_drv_ip_memory_->descriptors[1][sa].cw.str.dsize;

	memcpy(b->data, data, wc*2);

	{//Swap TX buffer
		b1553_critical_region critical_region; //ILOCK();
		uint32_t tmp=b1553_drv_ip_memory_->descriptors[1][sa].terminal_bufffer_pointer;
		b1553_drv_ip_memory_->descriptors[1][sa].terminal_bufffer_pointer=b1553_drv_ip_memory_->descriptors[1][sa].host_buffer_pointer;
		b1553_drv_ip_memory_->descriptors[1][sa].host_buffer_pointer=tmp;
		//IUNLOCK();
	}
}


//External I/F
static unsigned int b1553_drv_rt_default=0;

void b1553_drv_set_default_rt_address(unsigned int swrt)
{
	b1553_drv_rt_default=swrt;
}

int b1553_drv_initialize()
{
	bool is_evm=bsp_is_evm();

	if (!is_evm)
	{
		if ((b1553_drv_rt_default>0) && (b1553_drv_rt_default<31))
		{
			sw_rt=b1553_drv_rt_default;
			b1553_drv_enable_sw_rt_address(1);
		}
		else
		{
			sw_rt=0;
			b1553_drv_enable_sw_rt_address(0);
		}

		//initialize();
		//b1553_drv_remap(0, 0, 0, 1);
		b1553_drv_remap(0, 0, 0, 0);
	}
	else
		initialize();


	Pc1553DriverInitialize(b1553_drv_rt_default);

	return 0;
}

void b1553_drv_ssflag_watchdog_enable(int enabled)
{
	uint32_t tmp=b1553_drv_ip_ctrl_regs_->ticw;
	if (enabled==0)
		tmp|=(1<<6);
	else
		tmp&= ~(1<<6);
	b1553_drv_ip_ctrl_regs_->ticw=tmp;
}

void b1553_drv_ssflag_sw_enable(int enabled)
{
	uint32_t tmp=b1553_drv_ip_ctrl_regs_->tdcw;
	if (enabled==0)
		tmp|=(1<<1);
	else
		tmp&= ~(1<<1);
	b1553_drv_ip_ctrl_regs_->tdcw=tmp;
}


#if 0
#define NO_INTR
#if defined(__MINGW32__) || defined(NO_INTR)

static void install_isr(void)
{
	return;
}

#else

#include "bsp.h"

static void install_isr(void)
{
#ifdef FW_ISR_VECTOR
	bsp_set_isr("AVB", isr);
#else
	rtlog_printf(LID, rtlog_error|rtlog_breaking_news, "1553 ISR: undefined vector number!");
#endif
	return;
}

#endif
#endif

static void install_isr(void)
{
#ifdef FW_ISR_VECTOR
	bsp_set_isr("AVB", FW_ISR_VECTOR, isr);
    bsp_set_isr_extra_info(FW_ISR_VECTOR, &avb_isr_extra_info);

#else
	rtlog_printf(LID, rtlog_error|rtlog_breaking_news, "1553 ISR: undefined vector number!");
#endif
	return;
}

int b1553_drv_start()
{
	b1553_drv_ip_cmd_regs_->stop=1;

	uint32_t tdcw=b1553_drv_ip_ctrl_regs_->tdcw;
	tdcw=tdcw & (~(1<<6));	//TX ENABLE

	b1553_drv_ip_ctrl_regs_->tdcw=tdcw;

	b1553_drv_ip_ctrl_regs_->imw=
			(1<<0)		//Sync w DW
			|(1<<8)		//Such w/o DW
			|(1<<31) ;

	b1553_drv_ip_cmd_regs_->start=1;
	b1553_drv_ip_cmd_regs_->restart_timetag=1;

	//install_isr();

	unsigned int hwrt=(b1553_drv_ip_ctrl_regs_->tdcw>>11) & 0x1F;
	unsigned int swrt=(b1553_drv_ip_ctrl_regs_->ticw>>11) & 0x1F;

	//b1553_drv_enable_interrupts(0);
	bsp_interrupt_enable(FW_ISR_VECTOR, 0);

	rtlog_printf(LID, rtlog_success|rtlog_breaking_news, "AVB Started. HWRT=%u, SWRT=%u", hwrt, swrt);

	GrifoEifDiscreteSignal::instance().setFcrRdy(true);

	return 0;
}


int b1553_drv_ready()
{
	Pc1553DriverStart();
	return 0;
}

void b1553_drv_disable_interrupts()
{
	b1553_drv_ip_ctrl_regs_->imw=0;
}

void b1553_drv_enable_interrupts(int mode)
{
	install_isr();

	b1553_drv_ip_ctrl_regs_->imw=
			 (1<<0)		//Sync w DW
			|(1<<8)		//Such w/o DW
			|(1<<31);
			 ;
#ifdef FW_ISR_VECTOR
    if (mode==0)
    	bsp_interrupt_set_level_mode(FW_ISR_VECTOR, 1);
    else
    {
    	bsp_interrupt_set_level_mode(FW_ISR_VECTOR, 0);
    }
    bsp_interrupt_enable(FW_ISR_VECTOR, 1);
#endif
}

int b1553_drv_stop()
{
	b1553_drv_ip_cmd_regs_->stop=1;
#ifdef FW_ISR_VECTOR
	bsp_interrupt_enable(FW_ISR_VECTOR, 0);
#endif
	return 0;
}

int b1553_drv_remap(unsigned int ctrl_area, unsigned int cmd_area, unsigned int mem_area, unsigned int flags)
{
	if (ctrl_area==0)
		ctrl_area=FW_CTRL_AREA;
	if (cmd_area==0)
		cmd_area=FW_CMD_AREA;
	if (mem_area==0)
		mem_area=FW_MEM_AREA;

	ip_lock=1;

	b1553_drv_ip_memory_=reinterpret_cast<b1553_fw_memory_t*>(mem_area);
	b1553_drv_ip_cmd_regs_=reinterpret_cast<b1553_fw_cmd_regs_t*>(cmd_area);
	b1553_drv_ip_ctrl_regs_=reinterpret_cast<b1553_fw_ctrl_regs_t*>(ctrl_area);

	//int res=b1553_drv_initialize();
	initialize();

	if (flags & 1)
		memcpy(const_cast<b1553_fw_memory_t*>(b1553_drv_ip_memory_), &b1553_drv_ip_memory_dummy, sizeof *b1553_drv_ip_memory_);

	if (flags & 2)
		b1553_drv_start();

	ip_lock=0;
	return 0;
}


int b1553_drv_set_rt_sw_address(unsigned int rt)
{
	sw_rt=rt;
	unsigned int ta_parity=rta_parity(sw_rt);
	uint32_t tmp=b1553_drv_ip_ctrl_regs_->ticw;
	tmp&=~((1<<10)|(0x1F<<11));

	tmp|=
			(ta_parity<<10)
			|(sw_rt<<11);
	b1553_drv_ip_ctrl_regs_->ticw=tmp;
	return 0;
}

int b1553_drv_enable_sw_rt_address(int enable)
{
	b1553_drv_ip_cmd_regs_->stop=1;

	hwsw_hardwired_set(enable);

#if 0
	uint32_t tmp=b1553_drv_ip_ctrl_regs_->ticw;
	tmp&=~(1<<9);
	if (!enable)
		tmp|=(1<<9);

	if (enable)
	{
	}

	b1553_drv_ip_ctrl_regs_->ticw=tmp;
#endif

	b1553_drv_ip_cmd_regs_->start=1;

	return 0;
}

int b1553_drv_get_rta(unsigned int *sw_hdr, unsigned int* hw_hdr)
{
	uint32_t ticw=b1553_drv_ip_ctrl_regs_->ticw;
	uint32_t tdcw=b1553_drv_ip_ctrl_regs_->tdcw;

	unsigned int hwrta=(tdcw>>11) & (0x01F);
	unsigned int hwparity=(tdcw>>10) & (0x01);
	unsigned int swrta=(ticw>>11) & (0x01F);
	unsigned int swparity=(ticw>>10) & (0x01);

	unsigned int thw=(ticw>>9) & 0x1;
	unsigned int hwthw=(tdcw>>9) & 0x1;

	int rasel=0;
	if (hwthw==0)
	{
		if (thw==1)
			rasel=1;	//HW
		else
			rasel=2; //SW
	}
	else if (thw==0)
		rasel=3; //only SW
	else
		rasel=-1; //invalid;

	*sw_hdr=(swrta<<1)|swparity;
	*hw_hdr=(hwrta<<1)|hwparity;
	return rasel;
}

int b1553_drv_msg_configure(unsigned int sa, int rxtx, unsigned int /*wc_min*/, unsigned int wc_max, unsigned int flags)
{
	unsigned int intr=flags & 1;

	init_msg(sa, rxtx, wc_max, intr ? 0 : 1, intr);
	return 0;
}

int b1553_drv_msg_dbg_all_valid(void)
{
	for(unsigned int sa=0; sa<=31; ++sa)
	{
		init_msg(sa, 0, 32, 0, 0);
		init_msg(sa, 1, 32, 0, 0);
	}
	return 0;
}

int b1553_drv_msg_update(unsigned int sa, int rxtx, const void* data, unsigned int wc)
{
	if (ip_lock)
		return 1;

	if (rxtx)
		tx_msg_update(sa, (const uint16_t*)data, wc);

	Pc1553DriverMsgWrite(sa, rxtx, data, wc);

	return 0;
}

int b1553_drv_msg_get(unsigned int sa, int rxtx, void* data, unsigned int wc)
{
	if (ip_lock)
		return 1;

	if (rxtx==0)
	{
		unsigned int updated=rx_msg_update(sa, (uint16_t*)data, wc);
		if (updated)
			return wc;
	}
	return 0;
}

int b1553_drv_msg_get_noupdate(unsigned int sa, int rxtx, void* data, unsigned int wc)
{
	//if (ip_lock)
	//	return 1;

	if (rxtx==0)
	{
		rx_msg_update(sa, (uint16_t*)data, wc, true);
		return wc;
	}
	return 0;
}

static void simulate_isr()
{
	if (ip_lock)
		return;

    if (b1553_drv_ip_ctrl_regs_dummy.iqp!=b1553_drv_ip_ctrl_regs_dummy.iack) isr(0);
		process_iqueue(&b1553_drv_ip_ctrl_regs_dummy);
	//if (b1553_drv_ip_ctrl_regs_->iqp!=b1553_drv_ip_ctrl_regs_->iack) isr(0);
}

volatile unsigned int b1553_drv_simulate_b8_;

int b1553_drv_simulate_msg(uint16_t /*cwraw_*/, unsigned int sa, int rxtx, unsigned int wc, void* data)
{
	if (ip_lock)
		return 1;

	if (b1553_simulated_msg>=0)
		b1553_simulated_msg=1;

	b1553_fw_sadd_control_word_union_t cw;
	cw.raw=b1553_drv_ip_memory_->descriptors[rxtx][sa].cw.raw;
	if (cw.str.eeti || (sa==0))
	{

		uint32_t iqp=b1553_drv_ip_ctrl_regs_dummy.iqp;
		unsigned int index=(iqp & 0x03FF0)>>4;

		//unsigned int cw=17;
		uint16_t cwraw=sa|(rxtx<<13);
		if (!((sa==0) || (sa==31)))
			cwraw|=(1<<15);

		b1553_drv_ip_memory_->iq[index][0]=cwraw;
		index=(index+1) & 0x3FF;
		iqp=(iqp & (~(0x03FFF)));
		iqp=iqp|index<<4;
		b1553_drv_ip_ctrl_regs_dummy.iqp=iqp; //index<<4;

		simulate_isr();
	}

#if 0
	if ((sa==0) || (sa==31))
	{
		if (rxtx==0)
		{
			if (wc==17)	//Synch with data word
			{
				uint16_t* d=reinterpret_cast<uint16_t*>(data);
				b1553_drv_ip_ctrl_regs_->last_synch_word=d[0];

				uint32_t iqp=b1553_drv_ip_ctrl_regs_->iqp;
				unsigned int index=(iqp & 0x03FF0)>>4;

				unsigned int cw=17;
				b1553_drv_ip_memory_->iq[index][0]=cw;
				index=(index+1) & 0x3FF;
				iqp=(iqp & (~(0x03FFF)));
				iqp=iqp|index<<4;
				b1553_drv_ip_ctrl_regs_->iqp=iqp; //index<<4;

				simulate_isr();
			}
		}
		return 0;
	}
#endif

	if (rxtx)
	{
		sa=safix(sa);

		if (sa==18)
		{
			++b1553_drv_simulate_b8_;
		}
		unsigned int wc_=b1553_drv_ip_memory_->descriptors[1][sa].cw.str.dsize;
		if (wc_==0)
			return 1;
		if (wc>wc_)
			wc=wc_;
		{
			b1553_critical_region critical_region; //ILOCK();
			b1553_fw_buffer_t* b=reinterpret_cast<b1553_fw_buffer_t*>(waddress(b1553_drv_ip_memory_->descriptors[1][sa].terminal_bufffer_pointer));
			memcpy(data, b->data, wc*2); //memcpy(b->data, data, wc*2);
		}
		return 0;
	}
	else
	{
		sa=safix(sa);

		volatile b1553_fw_buffer_t* b=reinterpret_cast<volatile b1553_fw_buffer_t*>(waddress(b1553_drv_ip_memory_->descriptors[0][sa].terminal_bufffer_pointer));

		unsigned int wc_=b1553_drv_ip_memory_->descriptors[0][sa].cw.str.dsize;
		if (wc_==0)
			return 1;
		if (wc>wc_)
			wc=wc_;

		memcpy((void*)b->data, data, wc*2);
		b->identify.id.wc=wc;
		++b->identify.id.timetag;
		b->identify.id.sadd=sa;

		if (b1553_drv_ip_memory_->descriptors[0][sa].cw.str.edb)
		{
			b1553_critical_region critical_region; //ILOCK();
			uint32_t tmp=b1553_drv_ip_memory_->descriptors[0][sa].terminal_bufffer_pointer;
			b1553_drv_ip_memory_->descriptors[0][sa].terminal_bufffer_pointer=b1553_drv_ip_memory_->descriptors[0][sa].host_buffer_pointer;
			b1553_drv_ip_memory_->descriptors[0][sa].host_buffer_pointer=tmp;
		}

		return 0;
	}

}

int b1553_drv_set_synch_handler(b1153_drv_synch_handler_t handler)
{
	synch_handler=handler;
	return 0;
}

int b1553_drv_set_status_flag(unsigned int bit, int sts_flag)
{
	uint32_t mask=1<<bit;
	uint32_t tdcw=b1553_drv_ip_ctrl_regs_->tdcw;
	if (sts_flag<0)
	{

	}
	else
	{
		if (sts_flag>0)
			tdcw|=mask;
		else if (sts_flag==0)
			tdcw&=~mask;
		b1553_drv_ip_ctrl_regs_->tdcw=tdcw;
	}
	uint32_t hw_val=(b1553_drv_ip_ctrl_regs_->tdcw>>27);
	uint32_t sts=(hw_val|tdcw) & mask;
	return sts ? 1 : 0;
}

int b1553_drv_set_busy(int busy)
{
	return b1553_drv_set_status_flag(b1553_drv_busy_flag, busy);
}

int b1553_drv_set_terminal_flag(int ts)
{
	return b1553_drv_set_status_flag(b1553_drv_terminal_flag, ts);
}

int b1553_drv_set_subsystem_flag(int ss)
{
	return b1553_drv_set_status_flag(b1553_drv_subsystem_flag, ss);
}

unsigned int b1553_drv_get_bite_word(void)
{
	return b1553_drv_ip_ctrl_regs_->bit_word & 0x0FFFF;
}

void b1553_drv_set_bite_word(unsigned int bw)
{
	b1553_drv_ip_ctrl_regs_->bit_word=(bw & 0x0FFFF);
}

