/**
 * @brief get message that matches the header in the queue
 *
 * NOTE it matches only daddr, saddr, did, sid, type
 * NOTE it only gets the first that matches the description
 */
Message *mq_get(mq_t *q, Message *m)
{
  HAS_CRITICAL_SECTION;
  Message *ret;

#ifdef SOS_USE_PREEMPTION
  if(q->head == NULL) return NULL;
  ENTER_CRITICAL_SECTION();

  // Search the queue
  ret = mq_real_get(&(q->head), m);
#else
  if(q->msg_cnt == 0) return NULL;
  ENTER_CRITICAL_SECTION();
	
  //! first search high priority queue
  ret = mq_real_get(&(q->hq_head), &(q->hq_tail), m);

  if(ret) {
	q->msg_cnt--;
	LEAVE_CRITICAL_SECTION();
	return ret;
  }
  //! search low priority queue
  ret = mq_real_get(&(q->lq_head), &(q->lq_tail), m);
  if(ret) {
	q->msg_cnt--;
  }
#endif
  LEAVE_CRITICAL_SECTION();
  return ret;
}

/*************************************************************************
 * will be called by post_net, etc functions to send message             *
 *************************************************************************/
void radio_msg_alloc(Message *msg)
{
	HAS_CRITICAL_SECTION;
	uint16_t sleeptime = 0;
	uint8_t resend_pack = 1;

	ENTER_CRITICAL_SECTION();

	if( Radio_Check_CCA() ) {
		incSeq();
		if(radio_msg_send(msg))
		{
			resend_pack = 0;
			msg_send_senddone(msg, 1, RADIO_PID);
		}
	}
	
	if(resend_pack)
	{
		if( getMsgNumOfQueue() < MAX_MSGS_IN_QUEUE )		//queue is full?
		{
			mq_enqueue(&vmac_pq, msg);
			ENTER_CRITICAL_SECTION(); // most probably mq_enqueue calls LEAVE_CRITICAL_SECTION somewhere!
		}
		else
		{
			msg_send_senddone(msg, 0, RADIO_PID);		//release the memory for the msg
			ENTER_CRITICAL_SECTION(); // most probably msg_send_senddone calls LEAVE_CRITICAL_SECTION somewhere!
		}
		sleeptime = MacBackoff_congestionBackoff(retry_count);
		ker_timer_restart(RADIO_PID, WAKEUP_TIMER_TID, sleeptime);	// setup backoff timer
	}

	LEAVE_CRITICAL_SECTION();
}

static BOOL FddpPushJobItem(FDD_JOB_ITEM *pJobItem)
{
	BOOL bResult = TRUE;

ENTER_CRITICAL_SECTION();
	{
		/* check up the remain space of Q */
		if(m_JobItemQ.cnt >= FDD_JOB_ITEM_Q_SIZE) {
			bResult = FALSE;
			goto $exit;
		}

		/* process */
		m_JobItemQ.cnt++;
		m_JobItemQ.queue[m_JobItemQ.tail].type					= pJobItem->type;
		m_JobItemQ.queue[m_JobItemQ.tail].sector				= pJobItem->sector;
		m_JobItemQ.queue[m_JobItemQ.tail].numbers_of_sectors	= pJobItem->numbers_of_sectors;
		m_JobItemQ.queue[m_JobItemQ.tail].pt_data				= pJobItem->pt_data;
		m_JobItemQ.queue[m_JobItemQ.tail].thread				= pJobItem->thread;
		m_JobItemQ.tail++;
		if(m_JobItemQ.tail >= FDD_JOB_ITEM_Q_SIZE)
			m_JobItemQ.tail = 0;
	}
$exit:
EXIT_CRITICAL_SECTION();
	return bResult;
}

int SafeRealloc(void **Memory_ptr, size_t NewBytes)
{
	void *New;
#ifdef WIN32
	ENTER_CRITICAL_SECTION(AllocCS);
#else
	LOCK_SPIN(AllocSpin);
#endif /* WIN32 */

	New = realloc(*Memory_ptr, NewBytes);

#ifdef WIN32
	LEAVE_CRITICAL_SECTION(AllocCS);
#else
	UNLOCK_SPIN(AllocSpin);
#endif /* WIN32 */

	if(New != NULL)
	{
		*Memory_ptr = New;
		return 0;
	} else {
		return -1;
	}
}

static BOOL FddpPopJobItem(FDD_JOB_ITEM *pJobItem)
{
	BOOL bResult = TRUE;

ENTER_CRITICAL_SECTION();
	{
		/* check up count */
		if(m_JobItemQ.cnt == 0) {
			bResult = FALSE;
			goto $exit;
		}

		/* process */
		m_JobItemQ.cnt--;
		pJobItem->type					= m_JobItemQ.queue[m_JobItemQ.head].type;
		pJobItem->sector				= m_JobItemQ.queue[m_JobItemQ.head].sector;
		pJobItem->numbers_of_sectors	= m_JobItemQ.queue[m_JobItemQ.head].numbers_of_sectors;
		pJobItem->pt_data				= m_JobItemQ.queue[m_JobItemQ.head].pt_data;
		pJobItem->thread				= m_JobItemQ.queue[m_JobItemQ.head].thread;
		m_JobItemQ.head++;
		if(m_JobItemQ.head >= FDD_JOB_ITEM_Q_SIZE)
			m_JobItemQ.head = 0;
	}
$exit:
EXIT_CRITICAL_SECTION();
	return bResult;
}

eMBErrorCode
eMBRTUReceive( UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHORT * pusLength )
{
    BOOL            xFrameReceived = FALSE;
    eMBErrorCode    eStatus = MB_ENOERR;

    ENTER_CRITICAL_SECTION();
    assert( usRcvBufferPos < MB_SER_PDU_SIZE_MAX );

    /* Length and CRC check */
    if( ( usRcvBufferPos >= MB_SER_PDU_SIZE_MIN )
        && ( usMBCRC16( ( UCHAR * ) ucRTUBuf, usRcvBufferPos ) == 0 ) )
    {
        /* Save the address field. All frames are passed to the upper layed
         * and the decision if a frame is used is done there.
         */
        *pucRcvAddress = ucRTUBuf[MB_SER_PDU_ADDR_OFF];

        /* Total length of Modbus-PDU is Modbus-Serial-Line-PDU minus
         * size of address field and CRC checksum.
         */
        *pusLength = ( USHORT )( usRcvBufferPos - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_CRC );

        /* Return the start of the Modbus PDU to the caller. */
        *pucFrame = ( UCHAR * ) & ucRTUBuf[MB_SER_PDU_PDU_OFF];
        xFrameReceived = TRUE;
    }
    else
    {
        eStatus = MB_EIO;
    }

    EXIT_CRITICAL_SECTION();
    return eStatus;
}

/*************************************************************************
 * eMBMasterRTUReceive
 * Функция приёма данных и проверка их целосности.
 *************************************************************************/
eMBErrorCode
eMBMasterRTUReceive( UCHAR * pucRcvAddress, UCHAR ** pucFrame, USHORT * pusLength )
{
    eMBErrorCode    eStatus = MB_ENOERR;

    ENTER_CRITICAL_SECTION(  );
    assert_param( usMasterRcvBufferPos < MB_SER_PDU_SIZE_MAX );

    // Проверка длинны пакета и его CRC
    if( ( usMasterRcvBufferPos >= MB_SER_PDU_SIZE_MIN )
        && ( usMBCRC16( ( UCHAR * ) ucMasterRTURcvBuf, usMasterRcvBufferPos ) == 0 ) )
    {
        // Save the address field. All frames are passed to the upper layed and the decision if a frame is used is done there.
        *pucRcvAddress = ucMasterRTURcvBuf[MB_SER_PDU_ADDR_OFF];

        // Общая длина данных(Modbus-PDU) без поля адреса и CRC.
        *pusLength = ( USHORT )( usMasterRcvBufferPos - MB_SER_PDU_PDU_OFF - MB_SER_PDU_SIZE_CRC );

        // Адрес начала данных(Modbus-PDU)
        *pucFrame = ( UCHAR * ) & ucMasterRTURcvBuf[MB_SER_PDU_PDU_OFF];
    }
    else
    {
        eStatus = MB_EIO;
    }

    EXIT_CRITICAL_SECTION(  );
    return eStatus;
}

/**
 * Timer initialization
 */
void timer_init(uint8_t interval, uint8_t scale)
{
    HAS_CRITICAL_SECTION;
    ENTER_CRITICAL_SECTION();
                                                                               
    scale &= 0x7;
    scale |= 0x8;
                                                                               
    cbi(TIMSK, TOIE0);
    cbi(TIMSK, OCIE0);
    //!< Disable TC0 interrupt
                                                                               
    /**
     *  set Timer/Counter0 to be asynchronous
     *  from the CPU clock with a second external
     *  clock(32,768kHz)driving it
     */
    sbi(ASSR, AS0);
    outp(scale, TCCR0);    //!< prescale the timer to be clock/128 to make it
                                                                               
    outp(0, TCNT0);
    outp(interval, OCR0);
    sbi(TIMSK, OCIE0);
    LEAVE_CRITICAL_SECTION();
}

/* ----------------------- Start implementation -----------------------------*/
void vMBPortSerialEnable( BOOL xRxEnable, BOOL xTxEnable )
{
	/* If xRXEnable enable serial receive interrupts. If xTxENable enable
	 * transmitter empty interrupts.
	 */
	ENTER_CRITICAL_SECTION(  );
	if(xRxEnable){
		//SciaRegs.SCICTL1.bit.RXENA = 1;
		SciaRegs.SCICTL2.bit.RXBKINTENA =1;
	}
	else {
		//SciaRegs.SCICTL1.bit.RXENA = 0;
		SciaRegs.SCICTL2.bit.RXBKINTENA =0;
	}

	if(xTxEnable){
		//SciaRegs.SCICTL1.bit.TXENA = 1;
		SciaRegs.SCICTL2.bit.TXINTENA =1;
		SciaRegs.SCICTL2.bit.TXEMPTY = 0;
		SciaRegs.SCICTL1.bit.SWRESET = 0;
		SciaRegs.SCICTL1.bit.SWRESET = 1;
		//IFR = 0x0000;
	}
	else{
		//SciaRegs.SCICTL1.bit.TXENA = 0;
		SciaRegs.SCICTL2.bit.TXINTENA =0;
	}
	EXIT_CRITICAL_SECTION(  );
}

/**
 * Write a block of data into flash (block size is 64 bytes)
 * Note: this routine has to be in the RAM.
 */
static void __attribute__ ((section(".data"))) flash_write_block( uint32_t addr, uint8_t* buf, uint16_t len )
{
	HAS_CRITICAL_SECTION;
	register uint16_t i;
	register uint16_t* d = (uint16_t*) ((uint16_t)addr);
	register uint16_t* b = (uint16_t*) buf;
	
	ENTER_CRITICAL_SECTION();
	while( FCTL3 & BUSY );
	
	FCTL2 = FWKEY + FSSEL1 + FN2;   // SMCLK / 5
	FCTL3 = FWKEY;                  // Clear LOCK
	FCTL1 = FWKEY + BLKWRT + WRT;   // Enable block write
	
	for( i = 0; i < 32; i++ ) {
		*d = *b;
		d++;
		b++;
		while( (FCTL3 & WAIT) == 0);
	}

	FCTL1 = FWKEY;                  // Clear WRT and BLKWRT
	
	while( FCTL3 & BUSY );          // Test Busy
	
	FCTL3 = FWKEY + LOCK;           // Set LOCK

	LEAVE_CRITICAL_SECTION();
}

/**
 * Send data over the spi
 */
int8_t ker_spi_send_data(
		uint8_t *msg,
		uint8_t msg_size,
		uint8_t calling_id) {
	HAS_CRITICAL_SECTION;

	if (s.state == SPI_SYS_IDLE) {
		return -EINVAL;
	}	
	if ((s.calling_mod_id != calling_id) || ((s.state != SPI_SYS_WAIT) && (s.state != SPI_SYS_DMA_WAIT))) {
		return -EBUSY;
	}

	// ensure calling app gave us a message
	if (NULL != msg) {
		s.usrBuf = s.bufPtr = msg;
	} else {
		return -EINVAL;
	}
	
	// need to assert CS pin
	if (s.flags & SPI_SYS_CS_HIGH_FLAG) {
		spi_cs_high(s.addr);
	} else {
		spi_cs_low(s.addr);
	}

	ENTER_CRITICAL_SECTION();
	s.len = msg_size;
	s.state = SPI_SYS_TX;
	LEAVE_CRITICAL_SECTION();
	UART_DBG(a, 0x22, s.calling_mod_id, 0x01, 0x02, SPI_PID);
	
  return spi_masterTxData(s.bufPtr, s.len, s.flags);
}

// *************************************************************************************************
// @fn          WritePATable
// @brief       Write data to power table
// @param       unsigned char value             Value to write
// @return      none
// *************************************************************************************************
void WritePATable(unsigned char value)
{
   unsigned char readbackPATableValue = 0;
   u16 int_state;

   ENTER_CRITICAL_SECTION(int_state);

   while (readbackPATableValue != value)
   {
      while (!(RF1AIFCTL1 & RFINSTRIFG)) ;
      RF1AINSTRW = 0x7E00 + value;      // PA Table write (burst)

      while (!(RF1AIFCTL1 & RFINSTRIFG)) ;
      RF1AINSTRB = RF_SNOP;     // reset pointer

      while (!(RF1AIFCTL1 & RFINSTRIFG)) ;
      RF1AINSTRB = 0xFE;        // PA Table read (burst)

      while (!(RF1AIFCTL1 & RFDINIFG)) ;
      RF1ADINB = 0x00;          //dummy write

      while (!(RF1AIFCTL1 & RFDOUTIFG)) ;
      readbackPATableValue = RF1ADOUT0B;

      while (!(RF1AIFCTL1 & RFINSTRIFG)) ;
      RF1AINSTRB = RF_SNOP;
   }

   EXIT_CRITICAL_SECTION(int_state);
}

void protocolSendPacket(uint8 *buf, uint8 len)
{
    uint16	checksum;

    if ( (len!=PDU_SIZE) || (g_serialPortRecvState!=STATE_RX_IDLE) )
    {
    	return ;
    }

    ENTER_CRITICAL_SECTION();

	g_serialPortBufSendCnt = len;
	g_serialPortSendPtr = g_serialPortBuf;

	checksum = computeChecksum(buf, len-2);
	buf[CSM_OFF] = (uint8)(checksum & 0xFF);
    buf[CSM_OFF+1] = (uint8)(checksum >> 8);

    g_serialPortSendState = STATE_TX_XMIT;
    serialPortEnable(FALSE, TRUE);

    EXIT_CRITICAL_SECTION();

    return ;
}

/*************************************************************************
 * eMBMasterRTUSend
 * отправка пакета в MODBUS
 *************************************************************************/
eMBErrorCode
eMBMasterRTUSend( UCHAR ucSlaveAddress, const UCHAR * pucFrame, USHORT usLength )
{
    eMBErrorCode    eStatus = MB_ENOERR;
    USHORT          usCRC16;

    if ( ucSlaveAddress > MB_MASTER_TOTAL_SLAVE_NUM ) return MB_EINVAL;

    ENTER_CRITICAL_SECTION(  );

    // Если приемник в режиме ожидания.
    if( eRcvState == STATE_M_RX_IDLE )
    {
        // Первый байт  до PDU это slave address.
        pucMasterSndBufferCur = ( UCHAR * ) pucFrame - 1;
        usMasterSndBufferCount = 1;

        // Добавляем к пакету SlaveAddress
        pucMasterSndBufferCur[MB_SER_PDU_ADDR_OFF] = ucSlaveAddress;
        usMasterSndBufferCount += usLength;

        // считаем CRC16
        usCRC16 = usMBCRC16( ( UCHAR * ) pucMasterSndBufferCur, usMasterSndBufferCount );
        ucMasterRTUSndBuf[usMasterSndBufferCount++] = ( UCHAR )( usCRC16 & 0xFF );
        ucMasterRTUSndBuf[usMasterSndBufferCount++] = ( UCHAR )( usCRC16 >> 8 );

        // Режим предачи.
        eSndState = STATE_M_TX_XMIT;
        xMBMasterPortSerialPutBUF(( CHAR *)pucMasterSndBufferCur,usMasterSndBufferCount);						// передадим в порт весь блок сразу
        xFrameIsBroadcast = ( ucMasterRTUSndBuf[MB_SER_PDU_ADDR_OFF] == MB_ADDRESS_BROADCAST ) ? TRUE : FALSE;	// определяем широковещательный ли ?

    }

/**
 -----------------------------------------------------------------------------------------------------------------------
 eMBRTUStop
 -----------------------------------------------------------------------------------------------------------------------
*   Event Handler for GPI module
*
* 	@date       			DEC/02/2013
* 	@author                         FW_DEV_2
* 	@pre                            None
* 	@return	 			None
************************************************************************************************************************
*/
void eMBRTUStop( void )
{
    ENTER_CRITICAL_SECTION(  );
    vMBPortSerialEnable( FALSE, FALSE );
    vMBPortTimersDisable(  );
    EXIT_CRITICAL_SECTION(  );
}

void CMainFrame::OnCommand(UINT /*wNotifyCode*/, int wID, HWND /*hwndCtl*/)
{
	ENTER_CRITICAL_SECTION(&m_csThreadRefreshStatus);

	// Commands which do not change the tree are sent to the object directly.
	CTreeItem itemSelected = m_view.GetSelectedItem();

	if ( itemSelected.IsNull() )
	{
		LEAVE_CRITICAL_SECTION(&m_csThreadRefreshStatus);
		return;
	}

	CDiskObjectPtr obj;
	const CObjectUIHandler *phandler;
	obj = m_mapObject[m_view.GetItemData(itemSelected)];
	ATLASSERT( obj.get() != NULL );
	phandler = CObjectUIHandler::GetUIHandler( obj );
	phandler->OnCommand( obj, wID );
	
	m_view.UpdateDiskObject( obj );

	OnSelChanged(NULL);

	LEAVE_CRITICAL_SECTION(&m_csThreadRefreshStatus);
}

// mouse_up_count() returns the number of times button n has gone from down to up
// since the last call
//
// parameters:  n - button of mouse (see #define's in mouse.h)
//
int mouse_up_count(int n)
{
	int tmp = 0;
	if ( !mouse_inited ) return 0;

	if ( (n < LOWEST_MOUSE_BUTTON) || (n > HIGHEST_MOUSE_BUTTON)) return 0;

	ENTER_CRITICAL_SECTION( mouse_lock );

	switch (n) {
		case MOUSE_LEFT_BUTTON:
			tmp = mouse_left_up;
			mouse_left_up = 0;
			break;

		case MOUSE_RIGHT_BUTTON:
			tmp = mouse_right_up;
			mouse_right_up = 0;
			break;

		case MOUSE_MIDDLE_BUTTON:
			tmp = mouse_middle_up;
			mouse_middle_up = 0;
			break;

		default:
			Assert(0);	// can't happen
			break;
	} // end switch

	LEAVE_CRITICAL_SECTION( mouse_lock );	

	return tmp;
}

sp_pid_t sp_create( task_t task, uint8_t priority )
{
  sp_pid_t i;
  HAS_CRITICAL_SECTION;
  
  ENTER_CRITICAL_SECTION();
  // Search for empty slot
  for( i = 0; i < SPK_MAX_TASKS; i++ ) {
    // Found an empty slot
    if( spk_tasks[i].task == NULL ) {
      spk_tasks[i].lc       = 0;
      // The pid field is not being used
      spk_tasks[i].status   = TASK_WAITING;
      spk_tasks[i].priority = priority;
      spk_tasks[i].errno    = 0;
      spk_tasks[i].task     = task;
      spk_tasks[i].wait_obj = NULL;
      spk_tasks[i].next     = NULL;
      LEAVE_CRITICAL_SECTION();
      sp_signal( i );
      return i;
    }
  }
  // All slots taken
  LEAVE_CRITICAL_SECTION();
  SP_EXCEPTION();
  return 0;
}

static void uxos_update_sysclock()
{

	uint16_t temp_overflow_count;
	ENTER_CRITICAL_SECTION();
	temp_overflow_count=uxos_get_overflow();
	EXIT_CRITICAL_SECTION();
	if (temp_overflow_count>0)
	{
		ENTER_CRITICAL_SECTION();
		uxos_clear_overflow();
		EXIT_CRITICAL_SECTION();
		uxos_timer_update(temp_overflow_count);
	} 
		
}

/** 
 * @brief timer hardware routine
 */
void timer_hardware_init(uint8_t interval, uint8_t scale){
	HAS_CRITICAL_SECTION;
	ENTER_CRITICAL_SECTION();

	scale &= 0x7;
	scale |= (1<<WGM1); // reset on match
	

	TIMSK &= ((unsigned char)~(1 << (TOIE0)));
	TIMSK &= ((unsigned char)~(1 << (OCIE0)));
	//!< Disable TC0 interrupt

	/** 
	 *  set Timer/Counter0 to be asynchronous 
	 *  from the CPU clock with a second external 
	 *  clock(32,768kHz)driving it
	 */
	ASSR |= (1 << (AS0)); //!< us external oscillator
	TCCR0 = scale;
	TCNT0 = 0;
	OCR0 = interval;
	//TIMSK |= (1 << (OCIE0)); replaced by the line below
	timer_enable_interrupt();
	LEAVE_CRITICAL_SECTION();

	timer_init();
}