void CDMRTrellis::deinterleave(const unsigned char* data, signed char* dibits) const
{
	for (unsigned int i = 0U; i < 98U; i++) {
		unsigned int n = i * 2U + 0U;
		if (n >= 98U) n += 68U;
		bool b1 = READ_BIT(data, n) != 0x00U;

		n = i * 2U + 1U;
		if (n >= 98U) n += 68U;
		bool b2 = READ_BIT(data, n) != 0x00U;

		signed char dibit;
		if (!b1 && b2)
			dibit = +3;
		else if (!b1 && !b2)
			dibit = +1;
		else if (b1 && !b2)
			dibit = -1;
		else
			dibit = -3;

		n = INTERLEAVE_TABLE[i];
		dibits[n] = dibit;
	}
}

bool Cpc1360::Set_Connector(Cbus *_bus)
{
    if (_bus == busS1) {
        pS1CONNECTOR->Set_values(busS1->toUInt64());
        return true;
    }
    if (_bus == busS2) {
        pS2CONNECTOR->Set_values(busS2->toUInt64());
        return true;
    }

	int port1 = Get_8(0x3800);
	int port2 = Get_8(0x3A00);
	
	pCONNECTOR->Set_pin(PIN_MT_OUT2	,0);
	pCONNECTOR->Set_pin(PIN_VGG		,1);
	pCONNECTOR->Set_pin(PIN_BUSY	,READ_BIT(port1,0));
    if (!pCONNECTOR->Get_pin(PIN_ACK)) pCONNECTOR->Set_pin(PIN_D_OUT	,READ_BIT(port2,2));
    pCONNECTOR->Set_pin(PIN_MT_OUT1	,pCPU->Get_Xout());
    if (!pCONNECTOR->Get_pin(PIN_ACK)) pCONNECTOR->Set_pin(PIN_D_IN	,READ_BIT(port2,3));
    if (!pCONNECTOR->Get_pin(PIN_ACK)) pCONNECTOR->Set_pin(PIN_SEL2	,READ_BIT(port2,1));
    if (!pCONNECTOR->Get_pin(PIN_ACK)) pCONNECTOR->Set_pin(PIN_SEL1	,READ_BIT(port2,0));


	// MANAGE SERIAL CONNECTOR
	pSIOCONNECTOR->Set_pin(SIO_SD	,GET_PORT_BIT(PORT_F,2));
	pSIOCONNECTOR->Set_pin(SIO_RR	,READ_BIT(port1,2));
	pSIOCONNECTOR->Set_pin(SIO_RS	,READ_BIT(port1,3));
	pSIOCONNECTOR->Set_pin(SIO_ER	,READ_BIT(port1,1));

    pSIOCONNECTOR->Set_pin(SIO_PRQ	,READ_BIT(port2,2) | READ_BIT(port2,3));
	return(1);
}

/**
  * @brief  Return DMA2D output Blue color.
  * @param  DMA2Dx DMA2D Instance.
  * @param  ColorMode This parameter can be one of the following values:
  *         @arg @ref LL_DMA2D_OUTPUT_MODE_ARGB8888
  *         @arg @ref LL_DMA2D_OUTPUT_MODE_RGB888
  *         @arg @ref LL_DMA2D_OUTPUT_MODE_RGB565
  *         @arg @ref LL_DMA2D_OUTPUT_MODE_ARGB1555
  *         @arg @ref LL_DMA2D_OUTPUT_MODE_ARGB4444
  * @retval Output Blue color value between Min_Data=0 and Max_Data=0xFF
  */
uint32_t LL_DMA2D_GetOutputBlueColor(DMA2D_TypeDef *DMA2Dx, uint32_t ColorMode)
{
  uint32_t color = 0U;

  /* Check the parameters */
  assert_param(IS_DMA2D_ALL_INSTANCE(DMA2Dx));
  assert_param(IS_LL_DMA2D_OCMODE(ColorMode));

  /* DMA2D OCOLR register reading ------------------------------------------*/
  if (ColorMode == LL_DMA2D_OUTPUT_MODE_ARGB8888)
  {
    color = (uint32_t)(READ_BIT(DMA2Dx->OCOLR, 0xFFU));
  }
  else if (ColorMode == LL_DMA2D_OUTPUT_MODE_RGB888)
  {
    color = (uint32_t)(READ_BIT(DMA2Dx->OCOLR, 0xFFU));
  }
  else if (ColorMode == LL_DMA2D_OUTPUT_MODE_RGB565)
  {
    color = (uint32_t)(READ_BIT(DMA2Dx->OCOLR, 0x1FU));
  }
  else if (ColorMode == LL_DMA2D_OUTPUT_MODE_ARGB1555)
  {
    color = (uint32_t)(READ_BIT(DMA2Dx->OCOLR, 0x1FU));
  }
  else /* ColorMode = LL_DMA2D_OUTPUT_MODE_ARGB4444 */
  {
    color = (uint32_t)(READ_BIT(DMA2Dx->OCOLR, 0xFU));
  }
  
  return color;
}

/**
  * @brief  Configures the L3 Filter, this function allow to:
  *         set the layer 3 protocol to be matched (IPv4 or IPv6)
  *         enable/disable L3 source/destination port perfect/inverse match.
  * @param  heth: pointer to a ETH_HandleTypeDef structure that contains
  *         the configuration information for ETHERNET module
  * @param  Filter: L3 filter to configured, this parameter must be one of the following
  *           ETH_L3_FILTER_0
  *           ETH_L3_FILTER_1
  * @param  pL3FilterConfig: pointer to a ETH_L3FilterConfigTypeDef structure
  *         that will contain the L3 filter configuration.
  * @retval HAL status
  */
HAL_StatusTypeDef HAL_ETHEx_GetL3FilterConfig(ETH_HandleTypeDef *heth, uint32_t Filter, ETH_L3FilterConfigTypeDef *pL3FilterConfig)
{
  if(pL3FilterConfig == NULL)
  {
    return HAL_ERROR;
  }

  pL3FilterConfig->Protocol = READ_BIT(*((__IO uint32_t *)(&(heth->Instance->MACL3L4C0R) + Filter)), ETH_MACL3L4CR_L3PEN);
  pL3FilterConfig->SrcAddrFilterMatch = READ_BIT(*((__IO uint32_t *)(&(heth->Instance->MACL3L4C0R) + Filter)), (ETH_MACL3L4CR_L3SAM | ETH_MACL3L4CR_L3SAIM));
  pL3FilterConfig->DestAddrFilterMatch = READ_BIT(*((__IO uint32_t *)(&(heth->Instance->MACL3L4C0R) + Filter)), (ETH_MACL3L4CR_L3DAM | ETH_MACL3L4CR_L3DAIM));
  pL3FilterConfig->SrcAddrHigherBitsMatch = (READ_BIT(*((__IO uint32_t *)(&(heth->Instance->MACL3L4C0R) + Filter)), ETH_MACL3L4CR_L3HSBM) >> 6);
  pL3FilterConfig->DestAddrHigherBitsMatch = (READ_BIT(*((__IO uint32_t *)(&(heth->Instance->MACL3L4C0R) + Filter)), ETH_MACL3L4CR_L3HDBM) >> 11);

  if(pL3FilterConfig->Protocol != ETH_L3_IPV4_MATCH)
  {
    pL3FilterConfig->Ip6Addr[0] = *((__IO uint32_t *)(&(heth->Instance->MACL3A0R0R) + Filter));
    pL3FilterConfig->Ip6Addr[1] = *((__IO uint32_t *)(&(heth->Instance->MACL3A1R0R) + Filter));
    pL3FilterConfig->Ip6Addr[2] = *((__IO uint32_t *)(&(heth->Instance->MACL3A2R0R) + Filter));
    pL3FilterConfig->Ip6Addr[3] = *((__IO uint32_t *)(&(heth->Instance->MACL3A3R0R) + Filter));
  }
  else
  {
    pL3FilterConfig->Ip4SrcAddr = *((__IO uint32_t *)(&(heth->Instance->MACL3A0R0R) + Filter));
    pL3FilterConfig->Ip4DestAddr = *((__IO uint32_t *)(&(heth->Instance->MACL3A1R0R) + Filter));
  }

  return HAL_OK;
}

u8 spi_byte(SPI_TypeDef *spi, u8 byte)
{
	if(READ_BIT(spi->CR1, SPI_CR1_SPE | SPI_CR1_MSTR) == (SPI_CR1_SPE | SPI_CR1_MSTR))
	{
		READ_REG(spi->DR);
		while(!READ_BIT(spi->SR, SPI_SR_TXE));
		WRITE_REG(spi->DR, byte);
		while(!READ_BIT(spi->SR, SPI_SR_RXNE));
	}
	return (u8)READ_REG(spi->DR);
}

/**
  * @brief  Gets the bank of a given address
  * @param  Addr: Address of the FLASH Memory
  * @retval The bank of a given address
  */
static uint32_t GetBank(uint32_t Addr)
{
    uint32_t bank = 0;
#if defined(SYSCFG_MEMRMP_FB_MODE)
    if (READ_BIT(SYSCFG->MEMRMP, SYSCFG_MEMRMP_FB_MODE) == 0) {
        /* No Bank swap */
        if (Addr < (FLASH_BASE + FLASH_BANK_SIZE)) {
            bank = FLASH_BANK_1;
        } else {
            bank = FLASH_BANK_2;
        }
    } else {
        /* Bank swap */
        if (Addr < (FLASH_BASE + FLASH_BANK_SIZE)) {
            bank = FLASH_BANK_2;
        } else {
            bank = FLASH_BANK_1;
        }
    }
#else
    /* Device like L432KC */
    bank = FLASH_BANK_1;
#endif

    return bank;
}

/**
  * @brief  Starts the LPTIM Output pwm.
  * @param  hperh: LPTIM handle
  * @retval None
  */
void lptim_pwm_start(lptim_handle_t *hperh)
{
	assert_param(IS_LPTIM(hperh->perh));
	assert_param(IS_LPTIM_MODE(hperh->init.mode));

	LPTIM_ENABLE(hperh);
	while (READ_BIT(hperh->perh->SYNCSTAT, LP16T_SYNCSTAT_CON1WBSY_MSK));

	if (hperh->init.mode == LPTIM_MODE_CONTINUOUS)
		LPTIM_CNTSTART(hperh);
	else
		LPTIM_SNGSTART(hperh);

	while (READ_BIT(hperh->perh->SYNCSTAT, LP16T_SYNCSTAT_CON1WBSY_MSK));
	return;
}

/**
  * @brief  Gets the bank of a given address
  * @param  Addr: Address of the FLASH Memory
  * @retval The bank of a given address
  */
static uint32_t GetBank(uint32_t Addr)
{
  uint32_t bank = 0;

  if (READ_BIT(SYSCFG->MEMRMP, SYSCFG_MEMRMP_FB_MODE) == 0)
  {
  	/* No Bank swap */
    if (Addr < (FLASH_BASE + FLASH_BANK_SIZE))
    {
      bank = FLASH_BANK_1;
    }
    else
    {
      bank = FLASH_BANK_2;
    }
  }
  else
  {
  	/* Bank swap */
    if (Addr < (FLASH_BASE + FLASH_BANK_SIZE))
    {
      bank = FLASH_BANK_2;
    }
    else
    {
      bank = FLASH_BANK_1;
    }
  }

  return bank;
}

extern "C" void Reset_Handler()
{
  /* 4 bits for preemption priority */
  NVIC_SetPriorityGrouping(4);

  /* enable data/instruction cache, set wait cycles, set flash latency */
  FLASH->ACR |= FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_PRFTEN | FLASH_ACR_LATENCY_5WS;

  assert(READ_BIT(FLASH->ACR, FLASH_ACR_LATENCY) == FLASH_ACR_LATENCY_5WS);

  /* enable FPU if needed */
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
  /* enable coprocessors CP10 and CP11 */
  SCB->CPACR |= (0x0f << 20);
#endif

  clock_init();

  /* data initialization */
  memcpy(&__data_start__, &__etext, (intptr_t)&__data_end__ - (intptr_t)&__data_start__);
  memcpy(&__data2_start__, &__etext2, (intptr_t)&__data2_end__ - (intptr_t)&__data2_start__);
  memset(&__bss_start__, 0, (intptr_t)&__bss_end__ - (intptr_t)&__bss_start__);
  memset(&__bss2_start__, 0, (intptr_t)&__bss2_end__ - (intptr_t)&__bss2_start__);

  /* set interrupt vector table offset */
  SCB->VTOR = (uint32_t)&interruptVectorTable;

  /* c++ constructors */
  __libc_init_array();

  gpio_init();
  i2s_init();

  main();
}

unsigned int CP25Utils::encode(const unsigned char* in, unsigned char* out, unsigned int length)
{
	assert(in != NULL);
	assert(out != NULL);

	// Move the SSx positions to the range needed
	unsigned int ss0Pos = P25_SS0_START;
	unsigned int ss1Pos = P25_SS1_START;

	unsigned int n = 0U;
	unsigned int pos = 0U;
	while (n < length) {
		if (pos == ss0Pos) {
			ss0Pos += P25_SS_INCREMENT;
		} else if (pos == ss1Pos) {
			ss1Pos += P25_SS_INCREMENT;
		} else {
			bool b = READ_BIT(in, n);
			WRITE_BIT(out, pos, b);
			n++;
		}
		pos++;
	}

	return pos;
}

bool Cmd100::Get_MainConnector(void) {

    // get adress bus
    prev_adrBus = adrBus;
    adrBus = PIN(4) |
            (PIN(18)<<1) |
            (PIN( 6)<<2) |
            (PIN( 3)<<3);


    if (PIN(12) != READ_BIT(port,3)) {
        emit updatedPObject(this);
        update();      // refresh on power ON/OFF
    }

    PUT_BIT(port,0,PIN(25));
    PUT_BIT(port,1,PIN(11));
    PUT_BIT(port,2,PIN(26));
    PUT_BIT(port,3,PIN(12));
    PUT_BIT(port,4,PIN(27));

    data =(  PIN(22) |
            (PIN(19)<<1) |
            (PIN(9)<<2) |
            (PIN(24)<<3) |
            (PIN(21)<<4) |
            (PIN(8)<<5) |
            (PIN(20)<<6) |
            (PIN(23)<<7));

    return true;
}

/**
  * @brief  Comparator IRQ handler.
  * @param  hcomp  COMP handle
  * @retval None
  */
void HAL_COMP_IRQHandler(COMP_HandleTypeDef *hcomp)
{
  /* Get the EXTI line corresponding to the selected COMP instance */
  uint32_t exti_line = COMP_GET_EXTI_LINE(hcomp->Instance);
  
  /* Check COMP EXTI flag */
  if(LL_EXTI_IsActiveFlag_0_31(exti_line) != RESET)
  {
    /* Check whether comparator is in independent or window mode */
    if(READ_BIT(COMP12_COMMON->CSR, COMP_CSR_WINMODE) != RESET)
    {
      /* Clear COMP EXTI line pending bit of the pair of comparators          */
      /* in window mode.                                                      */
      /* Note: Pair of comparators in window mode can both trig IRQ when      */
      /*       input voltage is changing from "out of window" area            */
      /*       (low or high ) to the other "out of window" area (high or low).*/
      /*       Both flags must be cleared to call comparator trigger          */
      /*       callback is called once.                                       */
      LL_EXTI_ClearFlag_0_31((COMP_EXTI_LINE_COMP1 | COMP_EXTI_LINE_COMP2));
    }
    else
    {
      /* Clear COMP EXTI line pending bit */
      LL_EXTI_ClearFlag_0_31(exti_line);
    }
    
    /* COMP trigger user callback */
    HAL_COMP_TriggerCallback(hcomp);
  }
}

STATIC void adc_wait_for_eoc_or_timeout(int32_t timeout) {
    uint32_t tickstart = HAL_GetTick();
#if defined(STM32F4) || defined(STM32F7)
    while ((ADCx->SR & ADC_FLAG_EOC) != ADC_FLAG_EOC) {
#elif defined(STM32F0) || defined(STM32H7) || defined(STM32L4)
    while (READ_BIT(ADCx->ISR, ADC_FLAG_EOC) != ADC_FLAG_EOC) {
#else
    #error Unsupported processor
#endif
        if (((HAL_GetTick() - tickstart ) > timeout)) {
            break; // timeout
        }
    }
}

STATIC void adcx_clock_enable(void) {
#if defined(STM32F0) || defined(STM32F4) || defined(STM32F7)
    ADCx_CLK_ENABLE();
#elif defined(STM32H7)
    __HAL_RCC_ADC3_CLK_ENABLE();
    __HAL_RCC_ADC_CONFIG(RCC_ADCCLKSOURCE_CLKP);
#elif defined(STM32L4)
    __HAL_RCC_ADC_CLK_ENABLE();
#else
    #error Unsupported processor
#endif
}

/* This function draws to the screen; replace this with your own code! */
static void
draw ()
{
    SDL_Rect rect;
    Uint32 color;
    int b,i,z,y;
    
    unsigned char c;

    /* Create a black background */
    color = SDL_MapRGB (screen->format, 0, 0, 0);
    SDL_FillRect (screen, NULL, color);
    
    color = SDL_MapRGB (screen->format, 255, 255, 255);

    for (b = 0; b <= 191; b++) { // Y
        y = order[b];
        for (i = 0; i <= 31; i++) { // X
            c = buffer[y][i];
            for (z = 7; z >= 0; z--) { // Position in X byte
                if (READ_BIT(c,z)) {
                   rect.w = 1;
                   rect.h = 1;
                   rect.x = (i*8-z)+7;
                   rect.y = b;
                   SDL_FillRect (screen, &rect, color);
                }
            }
        }
    }

    /* Make sure everything is displayed on screen */
    SDL_Flip (screen);
}

/**
  * @brief  Get the PeriphClkInit according to the internal
  * RCC configuration registers.
  * @param  PeriphClkInit pointer to an RCC_PeriphCLKInitTypeDef structure that
  *         returns the configuration information for the Extended Peripherals clocks(RTC/LCD clocks).
  * @retval None
  */
void HAL_RCCEx_GetPeriphCLKConfig(RCC_PeriphCLKInitTypeDef  *PeriphClkInit)
{
    uint32_t srcclk = 0;

    /* Set all possible values for the extended clock type parameter------------*/
    PeriphClkInit->PeriphClockSelection = RCC_PERIPHCLK_RTC;
#if defined(LCD)
    PeriphClkInit->PeriphClockSelection |= RCC_PERIPHCLK_LCD;
#endif /* LCD */

    /* Get the RTC/LCD configuration -----------------------------------------------*/
    srcclk = __HAL_RCC_GET_RTC_SOURCE();
    if (srcclk != RCC_RTCCLKSOURCE_HSE_DIV2)
    {
        /* Source clock is LSE or LSI*/
        PeriphClkInit->RTCClockSelection = srcclk;
    }
    else
    {
        /* Source clock is HSE. Need to get the prescaler value*/
        PeriphClkInit->RTCClockSelection = srcclk | (READ_BIT(RCC->CR, RCC_CR_RTCPRE));
    }
#if defined(LCD)
    PeriphClkInit->LCDClockSelection = PeriphClkInit->RTCClockSelection;
#endif /* LCD */
}

void spi_buf_send(SPI_TypeDef *spi, u8 *buf, u16 size)
{
	if(spi == 0 || buf == 0 || size == 0)
	{
		return;
	}
	
	if(READ_BIT(spi->CR1, SPI_CR1_SPE | SPI_CR1_MSTR) == (SPI_CR1_SPE | SPI_CR1_MSTR))
	{
		while(size--)
		{
			while(!READ_BIT(spi->SR, SPI_SR_TXE));
			WRITE_REG(spi->DR, *buf);
			buf++;
		}
	}
}

/**
  * @brief  Waits for a Flash operation to complete or a TIMEOUT to occur.
  * @param  Timeout: FLASH progamming Timeout
  * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG,
  *   FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT.
  */
static	BOOL	Flash_isBusy( uint32_t iRetry )
{
    while (( 0u != iRetry ) && READ_BIT( FLASH->SR, FLASH_SR_BSY ))
    {
        delay_us( 1u );
        --iRetry;
    }

    if ( READ_BIT( FLASH->SR, FLASH_SR_BSY ))
    {
        return	TRUE;
    }
    else
    {
        return	FALSE;
    }
}

/**
  * @brief  Starts the LPTIM Output pulse in interrupt mode.
  * @param  hperh: LPTIM handle
  * @retval None
  */
void lptim_pulse_start_by_it(lptim_handle_t *hperh)
{
	assert_param(IS_LPTIM(hperh->perh));
	assert_param(IS_LPTIM_MODE(hperh->init.mode));

	lptim_interrupt_config(hperh, LPTIM_IT_ARRMAT, ENABLE);
	LPTIM_ENABLE(hperh);
	while (READ_BIT(hperh->perh->SYNCSTAT, LP16T_SYNCSTAT_CON1WBSY_MSK));

	if (hperh->init.mode == LPTIM_MODE_CONTINUOUS)
		LPTIM_CNTSTART(hperh);
	else
		LPTIM_SNGSTART(hperh);

	while (READ_BIT(hperh->perh->SYNCSTAT, LP16T_SYNCSTAT_CON1WBSY_MSK));
	return;
}

/**
  * @brief  Stops the LPTIM Output toggle.
  * @param  hperh: LPTIM handle
  * @retval None
  */
void lptim_toggle_stop(lptim_handle_t *hperh)
{
	assert_param(IS_LPTIM(hperh->perh));

	LPTIM_DISABLE(hperh);
	while (READ_BIT(hperh->perh->SYNCSTAT, LP16T_SYNCSTAT_CON1WBSY_MSK));
	return;
}

/**
  * @brief  Get the status of interrupt source.
  * @param  it: Specifies the interrupt source.
  *         This parameter can be one of the @ref trng_it_t.
  * @retval Status:
  *           - 0: RESET
  *           - 1: SET
  */
it_status_t trng_get_it_status(trng_it_t it)
{
	assert_param(IS_TRNG_IT(it));

	if (READ_BIT(TRNG->IER, it))
		return SET;

	return RESET;
}