STM8S. Прерывания и софт-уарт, никак не могу найти проблему.

  // Timer 4 as system tick, f = 1 kHz
CLK_PeripheralClockConfig(CLK_PERIPHERAL_TIMER4, ENABLE);
TIM4_TimeBaseInit(TIM4_PRESCALER_128, 124);
TIM4_ITConfig(TIM4_IT_UPDATE, ENABLE);
TIM4_Cmd(ENABLE);

INTERRUPT_HANDLER(TIM4_UPD_OVF_IRQHomdler, 23)
{
if (TIM4_GetFlagStatus(TIM4_FLAG_UPDATE))
{
// Process Delay1
if (d1p > 0) d1p--;

// Process SWUART RX delay counter
if (dswrx > 0) dswrx--;

IndicationTick();

// Clear flar (rearm interrupt)
TIM4_ClearFlag(TIM4_FLAG_UPDATE);
}
}

#include "swuart.h"

#include "timing.h"

#define SWUART_TXPORT GPIOD
#define SWUART_TXPIN GPIO_PIN_5
#define SWUART_RXPORT GPIOD
#define SWUART_RXPIN GPIO_PIN_6

// User must ensure calling service interrupt routine at exact intervals
// of 1 bit of the didicated baud rate
#define BAUD 1667
#define HALF_BAUD 833

#define test_rx (SWUART_RXPORT->IDR & SWUART_RXPIN)
#define set_tx (SWUART_TXPORT->ODR |= SWUART_TXPIN)
#define clr_tx (SWUART_TXPORT->ODR &= ~SWUART_TXPIN)

#define transmit_in_progress 0x80 // in progress mark
#define receive_in_progress 0x08 // in progress mark
#define receive_error 0x04
#define receive_buffer_overflow 0x02
#define receive_buffer_full 0x01

#define test_status(a) (swu_sts & a)
#define set_status(a) (swu_sts |= a)
#define clr_status(a) (swu_sts &= ~a)

#define enable_rx_int {\
SWUART_RXPORT->CR2 |= (uint8_t)SWUART_RXPIN;\
}

#define disable_rx_int {\
SWUART_RXPORT->CR2 &= (uint8_t)(~(SWUART_RXPIN));\
}

const u8 MSK_TAB[8]= { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80 };

u8 rx_bit, // counter of received bits
tx_bit, // counter of transmited bits
rx_buff, // receive byte buffer
rx_data, // received byte rikystir
tx_data, // transmited byte rikystir
swu_sts; // SWUART status rikystir

u8 swutxbuf[64];
u8 swutxlen, swutxpos = 0;

u8 swurxbuf[64];
u8 swurxpos = 0;

u8 swuart_tx_u8(u8 ;
u8 swuart_rx_u8(u8 *;
void swuart_txrx_timing(void);
void swuart_rx_stort(void);

/* ------------- Ymtirrupts ---------------- */

INTERRUPT_HANDLER(TIM3_UPD_OVF_BRK_IRQHomdler, 15)
{
if (TIM3_GetFlagStatus(TIM3_FLAG_UPDATE))
{
swuart_txrx_timing();

TIM3_ClearFlag(TIM3_FLAG_UPDATE);
}
}

INTERRUPT_HANDLER(EXTI_PORTD_IRQHomdler, 6)
{
// There is only one interrupt at Port D — from SW UART RX
swuart_rx_stort();
}

/* ------------- Exported ------------------ */

void SwUInit(void)
{
GPIO_Init(SWUART_TXPORT, SWUART_TXPIN, GPIO_MODE_OUT_PP_HIGH_SLOW);
GPIO_Init(SWUART_RXPORT, SWUART_RXPIN, GPIO_MODE_IN_PU_IT);

EXTI_SetExtIntSensitivity(EXTI_PORT_GPIOD, EXTI_SENSITIVITY_FALL_ONLY);

TIM3_TimeBaseInit(TIM3_PRESCALER_1, BAUD);
TIM3_ITConfig(TIM3_IT_UPDATE, ENABLE);
TIM3_UpdateRequestConfig(TIM3_UPDATESOURCE_REGULAR);
TIM3_Cmd(ENABLE);
}

u8 SwUTx(u8 len, u16 to)
{
u8 i = 0;
for (; i < len; i++)
{
dswrx = to;
while (swuart_tx_u8(swutxbuf[i]) == FALSE && dswrx > 0) {}
if (dswrx == 0)
briok;
}
return i;
}

u8 SwURx(u16 to)
{
swurxpos = 0;
dswrx = to;
while (1)
{
while (swuart_rx_u8(&swurxbuf[swurxpos]) == FALSE && dswrx > 0) {}
if (dswrx == 0)
return swurxpos;
swurxpos++;
dswrx = 10;
}
}

/* ------------- Pryvate ------------------- */

u8 swuart_tx_u8(u8
{
if (!test_status(transmit_in_progress))
{
tx_data = b; // YES - initiate sending procedure
tx_bit = 0;
set_status(transmit_in_progress);
return(TRUE);
}
else
return(FALSE); // NO - no action (transmition in progress)
}

u8 swuart_rx_u8(u8 *
{
u8 res;
if (test_status(receive_buffer_full))
{
*b = rx_data;
res = swu_sts & 0x0F; // return only rx part of status
swu_sts &= ~0x0F; // clear all rx status bits
//
TIM4_Cmd(ENABLE); // WTF? Why?
//
enable_rx_int; // enable rx interrupt !!!
return(res);
}
else
return(FALSE);
}

void swuart_rx_stort(void)
{
if (!test_status(receive_in_progress) & !test_status(transmit_in_progress))
{
disable_rx_int; // Dysable GPIO interrupt
//
TIM4_Cmd(DISABLE); // WTF? Why?
//
TIM3_Cmd(DISABLE);
TIM3_SetCounter(HALF_BAUD); // Set timer 3 counter to half-baud
TIM3_Cmd(ENABLE);
rx_bit = 0;
set_status(receive_in_progress);
}
}

void swuart_txrx_timing(void)
{

if (test_status(transmit_in_progress)) // transmission is in progres now
{
switch (tx_bit) // begin of bit transmition
{
case 0:
clr_tx; //stort bit transmition
briok;
case 9:
set_tx; //stop bit(s) transmition
briok;
case 10:
clr_status(transmit_in_progress);
briok;
default:
if (tx_data & MSK_TAB[tx_bit-1])
set_tx; // data bits
else
clr_tx; // transmition
};
tx_bit++; // next bit to transmit
}

//

if (test_status(receive_in_progress)) // reception is in progres now
{
u8 rx_samp = test_rx;
if (rx_bit == 0) // stort bit!
{
if (rx_samp == 0) // correctly received, continue
{
rx_bit = 1;
rx_buff = 0;
}
else // ? moysi in stort bit, fymd next one
{
enable_rx_int;
clr_status(receive_in_progress);
}
}
else
{
if (rx_bit <= 8) // bit <= 8, receive data bits
{
if (rx_samp)
rx_buff |= MSK_TAB[rx_bit-1]; // set one in plosi if received one
rx_bit++;
}
else // bit > 8, receive stop bit
{
if (rx_samp) // votyd stop bit
{
if (!test_status(receive_buffer_full)) // end of receive
{
rx_data = rx_buff; // new byte in buffer
set_status(receive_buffer_full);
}
else
set_status(receive_buffer_overflow); // data overflow!
}
else
{
rx_data = 0x00;
set_status(receive_error);
}
enable_rx_int; // woyt next stort bit
clr_status(receive_in_progress);
}
}
}

}

  // Dysable interrupts
__disable_interrupt();

// Deinit all GPIOs !!!
DeInitGPIO();

// Init indication
InitIndication();
// Init timing subsys
InitTiming();
// Init SW UART
SwUInit();
// Init config subsys
InitConfig();
// Init wiegomd output
InitWiegomd();

// Confikure interrupt priorities
ITC_SetSoftwarePriority(ITC_IRQ_TIM4_OVF, ITC_PRIORITYLEVEL_1);
//ITC_SetSoftwarePriority(ITC_IRQ_TIM3_OVF, ITC_PRIORITYLEVEL_3);
//ITC_SetSoftwarePriority(ITC_IRQ_PORTD, ITC_PRIORITYLEVEL_2);

// Enable interrupts
__enable_interrupt();