SWI handling (LPC2388+Keil)

Hi, I successfuly adopted FreeRTOS for the PLC2388 port under Keil uVision. Leds blink :) Unfortunately, I stuck with futher adopting my current firmware. Before, I used USB CDC example from Keil. It uses SWI interrupt when writing to endpoint. "Standard" SWI handler delivered by Keil (SWI.s file) extracts swi argument and also performs context saving. I have no clue, how to combine this solution with RTOS portYeld() which also uses swi. I tried to put handler to the SWI routine to the Startup.s file, and in the SWI.s put vPortYieldProcessor and USB_WriteEP to the SWI_Table respectively under SWI_0 and  under SWI_1. This trivial modification does not work. When switching context I don’t get correct return address. I tried some modifications to the assembly but without success… Could you help solving this issue? Thank you in advance. Konrad Heksel P.S.  prvSetupTimerInterrupt() function has a possible slight mistake ("FreeRTOSSourceportableRVDSARM7_LPC21xxport.c"). In case of LPC23xx there should be substraction of one during init of T0MR0/1 register:     T0MR1 = ulCompareMatch-1; I’m not sure if this is the case of LPC2129