void TIM6_Init( void ) // Timer for delayus() loop
{
    /* Timer TIM6 reset*/
   RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM6, ENABLE);
   RCC_APB1PeriphResetCmd(RCC_APB1Periph_TIM6, DISABLE);

  /* Enable TIM6 clock */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
  TIM6->PSC = (uint16_t) (SystemCoreClock / 1000000) - 1;   // Timer clock prescaler

#ifdef DEBUG
  DBGMCU->CR |= DBGMCU_CR_DBG_TIM6_STOP;
#endif
}

void delayus( const uint16_t us )
{
   TIM6->ARR = us;              // set auto-reload register
   TIM6->SR = 0x0000;        // clear status register (contains only UIF bit)
   TIM6->CR1 = (TIM_CR1_CEN | TIM_CR1_OPM);       // start timer in one pulse mode
   while(!(TIM6->SR));   // wait until UIF bit is set
}

void delayms( uint16_t ms )
{
  uint16_t fullCycles = ms / 65;
  uint8_t remainder = ms % 65;

  for ( ;fullCycles ; fullCycles--)
  {
    delayus(65000);
  }

  if (remainder)  delayus(remainder * 1000);
}

