Help with simple xgate code to test a binutils port I did. S12XDP512

[seank]

Hi I have created a binutils port to allow xgate assembly programming(http://github.com/seank/FreeScale-s12x-binutils).  I have verified the output of the test suite, what I'm trying to do now is create a simple test. 

My init code looks like this

void initXgate(){

    unsigned short *memBuffer = (unsigned short*)&TXBuffer;
    unsigned char i;

    /* write an entry in the vector table */
    for(i = 0x12; i; i--, memBuffer++){ /* first vector-table entry starts 0x24 after vector base address */
        *memBuffer = 0;
    }
    *memBuffer = (unsigned short)0x4800; /* Our first xgate thread starts here (s12x sees as E1x8800) */
    memBuffer++;
    *memBuffer = (unsigned short)&(PORTP); /* address of LEDs */
    //*memBuffer = (unsigned short)&(PORTS);

    eraseSector(0xE0,(unsigned short *)0x8800);
    writeSector(RPAGE, (unsigned short*)&TXBuffer, 0xE0, (unsigned short*)0x8800); /* xgate sees flash starting at paged address0xE0_0x8800 */

    // XGATE threads execute from flash
    // Set the XGVBR register to its start address in flash (page 0xE0 after 2K register space)
    XGVBR = (unsigned short )0x0800;

    // Enable XGate and XGate interrupts
    XGMCTL= (unsigned short)0x8181;

}

My software trigger code looks like this

    XGSWT = 0x0101;

Any my xgate code looks like this.

     NOP

     NOP

     ; need code to flip LED(s)

     RTS

Right now the CPU freezes unless I comment out my  XGSWT statement.  My s19 dump looks good.  My guess is that I'm missing some things. 

Any input would be greatly appreciated.

Thanks,

Sean

[kef]

Are you trying to trigger XGATE software trigger #0? I think it is so because you are triggering it with XGSWT = 0x0101;. But then you are setting up wrong XGATE vector table entry. Software trigger vector #0 number is 0x39. With XGVBR = 0x800, vector for software thread #0 should be at CPU banked address 0xE0_8800 + 0x39*4 = 0xE0_88E4 . I think your code is setting up vector number 0x12/2=9. This is reserved vector, not linked to any interrupt.

BTW. did you route xgate software thread #0 interrupt to XGATE? Codewarrior project wizard generated code for this is like this:

#ifndef ROUTE_INTERRUPT
 #define ROUTE_INTERRUPT(vec_adr, cfdata)               \
  INT_CFADDR= (vec_adr) & 0xF0;                         \
  INT_CFDATA_ARR[((vec_adr) & 0x0F) >> 1]= (cfdata)
#endif

foo()

{

   ROUTE_INTERRUPT(Vxst0, 0x81); // where Vxst0 is 0xFF72, CPU12 vector address for XGATE software thread #0

}

[seank]

Ah I see.  Let me revise my table. I'll repost when done.

THX!

[seank]

Awesome, it is no longer locking up, but I'm not sure if I got my test xgate ASM correct.

The following s12 C code will flash an LED on my dev board.

PORTP ^= 0xFF;

I tried to do this with xgate ASM but again may have missed a couple steps.

xgateThread0:
        NOP
        NOP
        LDW R2, R1, #0x00 ; get data at port-p
        COM R2, R2  ; flip the bits
        STW R2, R1, #0x00  ; write the bits to port-p
        RTS

Disassembly:

00E18800 0100 NOP       
00E18802 0100 NOP       
00E18804 4A20 LDW       R2, (R1,#0)
00E18806 120B COM       R2
00E18808 5A20 STW       R2, (R1,#0)
00E1880A 0200 RTS       

Again any input would be greatly appreciated.

[kef]

Is DDRP set up?

Assembler output seems being OK, bet PTP is 8 bits wide and you should use LDB and STB instructions instead of  LDW/STW.

You could use Codewarrior simulator to debug your S19 file. Lack of symbolic debugging is not big problem while code is not big.

BTW, does your GNU header file define datasheet specified PTP and PTS registers? It makes sense to have PORTP defined to ease porting of old code for HC12 to S12(X). But I think it doesn't make sense to use old port names in new code. I guess you won't need to port your new code for S12X to old HC12...

[seank]

DDRP = ONES; /* PWM pins (8) */ so 0xFF

I changed the opcodes as per your recommendation. It doesn't freeze up but the LED(s) don't flash.  Maybe my xgate asm technique is wrong?

Yes, I think the defs are good because I can get it to flash with s12x c code.

All I have is the Special Edition of CW will that suffice?

THX!!!!

[kef]

With your code you won't be able to see LEDs flashing. Since XGSWT flag is not reset in the thread, so software thread #0 will exit and reenter, exit and reenter .. so quickly that you may notice LED "flashing" only using scope.

Try making LED off in S12X code. If write to XGSWT=0x101 makes LED on, then XGATE thread is executing.

Maybe PORTP define is good, but datasheet doesn't define such register. IMO it doesn't make sense to rename registers defined in datasheets. Compatibility aliases are fine though.

Yes, you can use special edition to run your S19 file in simulator. If you didn't yet try hiwave simulator, then I guess you should create multicore S12XD project using project wizard. Run it and get a bit familiar with controls. Then you could load your S records file and debug. In debugger you may need to switch HCS12X FCS->Configure...->Mode to "user defined" and define all used RAM and FLASH areas. Are GNU S2 records banked or global? By default hiwave wants banked S2 records.

[seank]

Well here are my revisions.

void initXgate(){
    //TODO maybe build the vectortable pragmatically
    struct xgateIntVector {
        unsigned short threadAddress;
        unsigned short initialVariable;
    };

    struct xgateIntVector xgateIntVectorTable[] = {

            // Channel # = Vector address / 2
            /* channel 0..1E are not used, first used must match macro XGATE_VECTOR_OFFSET in xgate.h */
            {XGATE_ERROR_HANDLER, 0x00},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x01},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x02},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x03},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x04},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x05},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x06},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x07},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x08},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x09},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x0A},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x0B},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x0C},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x0D},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x0E},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x0F},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x10},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x11},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x12},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x13},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x14},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x15},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x16},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x17},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x18},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x19},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x1A},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x1B},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x1C},  // RESERVED
            {XGATE_ERROR_HANDLER, 0x1D},  // RESERVED NOT USED
            {XGATE_ERROR_HANDLER, 0x1E},  // Channel 1E - ATD1 compare
            {XGATE_ERROR_HANDLER, 0x1F},  // Channel 1F - ATD0 compare
            {XGATE_ERROR_HANDLER, 0x20},  // Channel 20 - TIM Pulse accumulator input edge
            {XGATE_ERROR_HANDLER, 0x21},  // Channel 21 - TIM Pulse accumulator A overflow
            {XGATE_ERROR_HANDLER, 0x22},  // Channel 22 - TIM overflow
            {XGATE_ERROR_HANDLER, 0x23},  // Channel 23 - TIM channel 7
            {XGATE_ERROR_HANDLER, 0x24},  // Channel 24 - TIM channel 6
            {XGATE_ERROR_HANDLER, 0x25},  // Channel 25 - TIM channel 5
            {XGATE_ERROR_HANDLER, 0x26},  // Channel 26 - TIM channel 4
            {XGATE_ERROR_HANDLER, 0x27},  // Channel 27 - TIM channel 3
            {XGATE_ERROR_HANDLER, 0x28},  // Channel 28 - TIM channel 2
            {XGATE_ERROR_HANDLER, 0x29},  // Channel 29 - TIM channel 1
            {XGATE_ERROR_HANDLER, 0x2A},  // Channel 2A - TIM channel 0
            {XGATE_ERROR_HANDLER, 0x2B},  // Channel 2B - SCI7
            {XGATE_ERROR_HANDLER, 0x2C},  // Channel 2C - Periodic Interrupt Timer 7
            {XGATE_ERROR_HANDLER, 0x2D},  // Channel 2D - Periodic Interrupt Timer 6
            {XGATE_ERROR_HANDLER, 0x2E},  // Channel 2E - Periodic Interrupt Timer 5
            {XGATE_ERROR_HANDLER, 0x2F},  // Channel 2F - Periodic Interrupt Timer 4
            {XGATE_ERROR_HANDLER, 0x30},  // Channel 30 - Reserved
            {XGATE_ERROR_HANDLER, 0x31},  // Channel 31 - Reserved
            {XGATE_ERROR_HANDLER, 0x32},  // Channel 32 - XGATE Software Trigger 7
            {XGATE_ERROR_HANDLER, 0x33},  // Channel 33 - XGATE Software Trigger 6
            {XGATE_ERROR_HANDLER, 0x34},  // Channel 34 - XGATE Software Trigger 5
            {XGATE_ERROR_HANDLER, 0x35},  // Channel 35 - XGATE Software Trigger 4
            {XGATE_ERROR_HANDLER, 0x36},  // Channel 36 - XGATE Software Trigger 3
            {XGATE_ERROR_HANDLER, 0x37},  // Channel 37 - XGATE Software Trigger 2
            {XGATE_ERROR_HANDLER, 0x38},  // Channel 38 - XGATE Software Trigger 1
            {(unsigned short)0x4800, (unsigned short)&PORTP}, // Channel 39 - XGATE Software Trigger 0
            //{(XGATE_Function)SoftwareTrigger0_XSR, (int)&STRCH0_Data}, // Channel 39 - XGATE Software Trigger 0
            {XGATE_ERROR_HANDLER, 0x3A},  // Channel 3A - Periodic Interrupt Timer 3
            {XGATE_ERROR_HANDLER, 0x3B},  // Channel 3B - Periodic Interrupt Timer 2
            {XGATE_ERROR_HANDLER, 0x3C},  // Channel 3C - Periodic Interrupt Timer 1
            {XGATE_ERROR_HANDLER, 0x3D},  // Channel 3D - Periodic Interrupt Timer 0
            //{(XGATE_Function)PIT_XSR, (int)&PITCH0_Data},              // Channel 3D - Periodic Interrupt Timer 0
            {XGATE_ERROR_HANDLER, 0x3E},  // Channel 3E - Reserved
            {XGATE_ERROR_HANDLER, 0x3F},  // Channel 3F - Autonomous Periodical interrupt API
            {XGATE_ERROR_HANDLER, 0x40},  // Channel 40 - Low Voltage interrupt LVI
            {XGATE_ERROR_HANDLER, 0x41},  // Channel 41 - IIC1 Bus
            {XGATE_ERROR_HANDLER, 0x42},  // Channel 42 - SCI5
            {XGATE_ERROR_HANDLER, 0x43},  // Channel 43 - SCI4
            {XGATE_ERROR_HANDLER, 0x44},  // Channel 44 - SCI3
            {XGATE_ERROR_HANDLER, 0x45},  // Channel 45 - SCI2
            {XGATE_ERROR_HANDLER, 0x46},  // Channel 46 - PWM Emergency Shutdown
            {XGATE_ERROR_HANDLER, 0x47},  // Channel 47 - Port P Interrupt
            {XGATE_ERROR_HANDLER, 0x48},  // Channel 48 - CAN4 transmit
            {XGATE_ERROR_HANDLER, 0x49},  // Channel 49 - CAN4 receive
            {XGATE_ERROR_HANDLER, 0x4A},  // Channel 4A - CAN4 errors
            {XGATE_ERROR_HANDLER, 0x4B},  // Channel 4B - CAN4 wake-up
            {XGATE_ERROR_HANDLER, 0x4C},  // Channel 4C - CAN3 transmit
            {XGATE_ERROR_HANDLER, 0x4D},  // Channel 4D - CAN3 receive
            {XGATE_ERROR_HANDLER, 0x4E},  // Channel 4E - CAN3 errors
            {XGATE_ERROR_HANDLER, 0x4F},  // Channel 4F - CAN3 wake-up
            {XGATE_ERROR_HANDLER, 0x50},  // Channel 50 - CAN2 transmit
            {XGATE_ERROR_HANDLER, 0x51},  // Channel 51 - CAN2 receive
            {XGATE_ERROR_HANDLER, 0x52},  // Channel 52 - CAN2 errors
            {XGATE_ERROR_HANDLER, 0x53},  // Channel 53 - CAN2 wake-up
            {XGATE_ERROR_HANDLER, 0x54},  // Channel 54 - CAN1 transmit
            {XGATE_ERROR_HANDLER, 0x55},  // Channel 55 - CAN1 receive
            {XGATE_ERROR_HANDLER, 0x56},  // Channel 56 - CAN1 errors
            {XGATE_ERROR_HANDLER, 0x57},  // Channel 57 - CAN1 wake-up
            {XGATE_ERROR_HANDLER, 0x58},  // Channel 58 - CAN0 transmit
            {XGATE_ERROR_HANDLER, 0x59},  // Channel 59 - CAN0 receive
            {XGATE_ERROR_HANDLER, 0x5A},  // Channel 5A - CAN0 errors
            {XGATE_ERROR_HANDLER, 0x5B},  // Channel 5B - CAN0 wake-up
            {XGATE_ERROR_HANDLER, 0x5C},  // Channel 5C - FLASH
            {XGATE_ERROR_HANDLER, 0x5D},  // Channel 5D - FLASH fault detect
            {XGATE_ERROR_HANDLER, 0x5E},  // Channel 5E - SPI2
            {XGATE_ERROR_HANDLER, 0x5F},  // Channel 5F - SPI1
            {XGATE_ERROR_HANDLER, 0x60},  // Channel 60 - IIC0 Bus
            {XGATE_ERROR_HANDLER, 0x61},  // Channel 61 - SCI6
            {XGATE_ERROR_HANDLER, 0x62},  // Channel 62 - CRG Self Clock Mode
            {XGATE_ERROR_HANDLER, 0x63},  // Channel 63 - CRG PLL lock
            {XGATE_ERROR_HANDLER, 0x64},  // Channel 64 - Pulse Accumulator B Overflow
            {XGATE_ERROR_HANDLER, 0x65},  // Channel 65 - Modulus Down Counter underflow
            {XGATE_ERROR_HANDLER, 0x66},  // Channel 66 - Port H
            {XGATE_ERROR_HANDLER, 0x67},  // Channel 67 - Port J
            {XGATE_ERROR_HANDLER, 0x68},  // Channel 68 - ATD1
            {XGATE_ERROR_HANDLER, 0x69},  // Channel 69 - ATD0
            {XGATE_ERROR_HANDLER, 0x6A},  // Channel 6A - SCI1
            {XGATE_ERROR_HANDLER, 0x6B},  // Channel 6B - SCI0
            {XGATE_ERROR_HANDLER, 0x6C},  // Channel 6C - SPI0
            {XGATE_ERROR_HANDLER, 0x6D},  // Channel 6D - ECT Pulse accumulator input edge
            {XGATE_ERROR_HANDLER, 0x6E},  // Channel 6E - ECT Pulse accumulator A overflow
            {XGATE_ERROR_HANDLER, 0x6F},  // Channel 6F - Enhanced Capture Timer overflow
            {XGATE_ERROR_HANDLER, 0x70},  // Channel 70 - Enhanced Capture Timer channel 7
            {XGATE_ERROR_HANDLER, 0x71},  // Channel 71 - Enhanced Capture Timer channel 6
            {XGATE_ERROR_HANDLER, 0x72},  // Channel 72 - Enhanced Capture Timer channel 5
            {XGATE_ERROR_HANDLER, 0x73},  // Channel 73 - Enhanced Capture Timer channel 4
            {XGATE_ERROR_HANDLER, 0x74},  // Channel 74 - Enhanced Capture Timer channel 3
            {XGATE_ERROR_HANDLER, 0x75},  // Channel 75 - Enhanced Capture Timer channel 2
            {XGATE_ERROR_HANDLER, 0x76},  // Channel 76 - Enhanced Capture Timer channel 1
            {XGATE_ERROR_HANDLER, 0x77},  // Channel 77 - Enhanced Capture Timer channel 0
            {XGATE_ERROR_HANDLER, 0x78}  // Channel 78 - Real Time Interrupt
    };

    //    eraseSector(0xE1,(unsigned short *)0x8800);

    memcpy((void*)&TXBuffer, (void*)&xgateIntVectorTable, (0x79 * sizeof(struct xgateIntVector)) );

    eraseSector(0xE0,(unsigned short *)0x8800);
    writeSector(RPAGE, (unsigned short*)&TXBuffer, 0xE0, (unsigned short*)0x8800); /* xgate sees flash starting at paged address0xE0_0x8800 */

    // XGATE threads execute from flash
    // Set the XGVBR register to its start address in flash (page 0xE0 after 2K register space)
    XGVBR = (unsigned short )0x0800;

    // Enable XGate and XGate interrupts
    XGMCTL= (unsigned short)0x8181;
}

And I took your advice and tried to just flip the LED on since my xgatethread is by no means proper.

xgateThread0: ;load to E1 8800
        NOP
        NOP
        LDL R2,#0xFF
        LDH R2,#0xFF
;       LDB R2, R1, #0x00 ; get data at port-p
;       COM R2, R2  ; flip the bits
        STB R2, R1, #0x00  ; write the bits to port-p
        RTS

I'll see if I can fire up the sim, Im using banked paged mode.   I could only think of two possibilities.  1 you cant pass a HW address parameter into the vector table or 2 maybe the lack us using semaphores is hanging it up.  Thanks for all your input.  If you notice anything else let me know. 

[seank]

I found one error so far by creating a project in CW.

The first 8 vector entries arent even "blanks"

    //        {XGATE_ERROR_HANDLER, 0x00},  // RESERVED
    //        {XGATE_ERROR_HANDLER, 0x01},  // RESERVED
    //        {XGATE_ERROR_HANDLER, 0x02},  // RESERVED
    //        {XGATE_ERROR_HANDLER, 0x03},  // RESERVED
    //        {XGATE_ERROR_HANDLER, 0x04},  // RESERVED
    //        {XGATE_ERROR_HANDLER, 0x05},  // RESERVED
    //        {XGATE_ERROR_HANDLER, 0x06},  // RESERVED
    //        {XGATE_ERROR_HANDLER, 0x07},  // RESERVED
    //        {XGATE_ERROR_HANDLER, 0x08},  // RESERVED

[kef]

Since first 8 XGATE vectors on S12XD/XA don't have even associated thread flags (these vectors are never used), CW created project saves few bytes of codespace by starting vector table from vector 9 and setting up XGVBR with XGATE address of vector minus 8*4. See Setup XGATE routine and XGATE_VECTOR_OFFSET define in CW xgate.h. You should either uncommend first 8 vectors back or adjust XGVBR.

I don't know, memory dumps look right. So you are writing XGSWT=0x101; and nothing happens? Maybe attach you S19 file then. Of course if would ease simulation if you could initialize XGATE vector table at compile time, without the calls to flash program/erase routines. Simulator doesn't support simulation of flash programming.

[seank]

Thx again I def owe you one.  I uncommented out those 8 lines for verbosity rather than give it an offset.  The problem is my loader (hc12mem) cant seem to erase that deep into flash so I have to do it from the main code as you stated.  The first s19 is the s12, beware it is rather large as it is part of the FreeEMS project.  The second is the tiny xgate thread. 

Right when I write x0101 to the int vector I get nothing.  Ah I see a mistake, in-order for s12 to be able to write to the correct vector I need to flip the page first.

  PPAGE = 0xE0;
  XGSWT = 0x0101;
 PPAGE = savedPage;

But I still got nothing........

[kef]

after freeems...s19 completed writing vector table, it started looping doing something but XGSWT is never written. Could you make sure you set up interrupt routing and write 0x101 to XGSWT?

test.s19 loads properly at address 0x4800 in XGATE address space. Do you load it separately?

So I loaded two S19 files, switched xgswt#0 interrupt routing to XGATE by hand and wrote 0x101 to XGSWT by hand. XGATE jumped properly to your code in test.s19. No problems.

Unfortunately it is not easy to write to xgate registers in hiwave. Only word write to XGSWT is accepted and it seems debugger command  ww $398 $101  writes not one word at a time but two bytes, of course that doesn't work. I had to write opcodes LDY #0x101  STY 0x398 by hand.

[seank]

I had XGSWT defined as the address of my vector thread entry.  It is now fixed and the generated asm looks like this.

.LSM19:
        movw    #257,920

I'll have a look at interrupt routing.  Maybe my concept of it is broken or absent.  I thought all you had to do is write the thread info to the vector table after allocating it and write to XGSWT to fire it.

Yes right now I don't have the Makefile setup to do the linking and generate 1 s19, at least not yet anyway.

[kef]

By default interrupt controller passes interrupts to CPU12X. Almost any given interrupt can be switching to be handler by XGATE. XGATE vector table only defines thread start addresses (initial PC) and initial R1 register settings for every thread.

To reroute XGATE software thread0 from default CPU12X to XGATE you should have something like this in your code:

#ifndef ROUTE_INTERRUPT
 #define ROUTE_INTERRUPT(vec_adr, cfdata)               \
  INT_CFADDR= (vec_adr) & 0xF0;                         \
  INT_CFDATA_ARR[((vec_adr) & 0x0F) >> 1]= (cfdata)
#endif

foo()

{

   // reroute Vxst0 to XGATE

   ROUTE_INTERRUPT(Vxst0, 0x81); // where Vxst0 is 0xFF72, CPU12X vector address for XGATE software thread #0

}

Sorry but I won't try your new file. Old one was lacking only ROUTE_INTERRUPT and a write to XGSWT.(though maybe I just missed it).

[seank]

It WORKS!  The LED lit, I tried to rewrite the xgate code so the LED would cycle but it just stays lit. 

/* route interrupt to xgate */
    INT_CFADDR = (0x72 & 0xF0);
    INT_CFDATA0 = 0x01;
    INT_CFDATA1 = 0x81;

Disassembly:

00E18800 F398 LDL       R3, #152
00E18802 FB03 LDH       R3, #3
00E18804 F400 LDL       R4, #0
00E18806 FC01 LDH       R4, #1
00E18808 5460 STB       R4, (R3,#0) ;disable interrupts
00E1880A 0100 NOP
00E1880C 0100 NOP
00E1880E 4220 LDB       R2, (R1,#0)
00E18810 120B COM       R2
00E18812 5220 STB       R2, (R1,#0)
00E18814 0200 RTS

When I'm done with my example code, do you mind if I put a thank you reference in there with your name?

THX

[kef · Accepted solution]

00E18800 F398 LDL       R3, #152 // address of XGSWT to R3
00E18802 FB03 LDH       R3, #3     //
00E18804 F400 LDL       R4, #0      // 0x100 to R4
00E18806 FC01 LDH       R4, #1    //
00E18808 5460 STB       R4, (R3,#0) ;disable interrupts

Should be STW here to write 0x100 word to XGSWT and clear software thread #0 request. It is not "disable interrupts". Other interrupts (threads) have no relation to XGSWT.

Regards,

Edward Karpicz

[seank]

http://github.com/seank/freeems-vanilla/blob/master/src/xgate/assembly/xgate.s

[seank]

Seems maybe my clear code should look like this.

xgateThread0:

        LDL  R2, #0x80     ;load R2 with immediate value of 0x80 (rtif bit)
        LDL  R3, #0x37     ;load R3 with CRGFLG register address
        STB  R2, R3, #0x00 ;store R2 to CRGFLG register to clear rti flag
        NOP
        NOP
        LDB R2, R1, #0x00 ; get data at port-p
        COM R2, R2  ; flip the bits
        STB R2, R1, #0x00  ; write the bits to port-p
        RTS