asm(" .ref _c_int00 ");
    asm(" .ref _c_irq ");
	asm(" .ref _c_fiq ");
//	asm(" .ref _c_undefined_instruction ");
//	asm(" .ref _c_swi ");
//	asm(" .ref _c_prefetch_abort ");
//	asm(" .ref _c_data_abort ");

//	asm(" .ref _c_spin ");
	asm(" .global _SWI_cmd");
	asm(" .global _SWI_arg1");
	asm(" .global _SWI_arg2");
    
asm("VECTORS: .sect \"vectors\" ");

asm("_vector:");    
	asm("            B   _board_dsp_init");   /*0x00000000  Reset*/
	asm("VEC_UNDEF:  B   VEC_UNDEF");   /*0x00000004  Undefined Instruction*/
	asm("            B   _trap ");      /*0x00000008  Software Interrupts*/
	asm("VEC_ABORTP: B   VEC_ABORTP");  /*0x0000000C  Abort (prefetch)*/
	asm("VEC_ABORTD: B   VEC_ABORTD");  /*0x00000010  Abort (data)*/
	asm("VEC_RESVD:  B   VEC_RESVD");   /*0x00000014  Reserved*/
	asm("            B   _c_irq ");     /*0x00000018  IRQ*/	
	asm("            B   _c_fiq ");     /*0x0000001C  FIQ*/
	
	asm("_trap:");
//	asm("            STR       R0, SAVER0");
//	asm("            STR       R1, SAVER1");
//	asm("            STR       R2, SAVER2");
//	asm("            STR       R3, SAVER3");
//	asm("            STR       R4, SAVER4");  
	asm("            MRS       R4, CPSR");  // Use R4 to store CPSR

    asm("            LDR       R0, SWI_CMD_ADDR");
    asm("            LDR       R0, [R0]");
    asm("            LDR       R1, SWI_ARG1_ADDR");
    asm("            LDR       R1, [R1]");
    asm("            LDR       R2, SWI_ARG2_ADDR");
    asm("            LDR       R2, [R2]");
    
    //  At this point R0-R4, SP are saved and:
    //      R0 = SWI_cmd
    //      R1 = SWI_arg1
    //      R2 = SWI_arg2;
    //      R3 = free
    //      R4 = CPSR
    
//    asm(" .long 0xDEFED0FE");
    asm("CMPSETSTK:  CMP       R0, #1");
    asm("            BNE       CMPSETCPSR");
    asm("            BIC       R3, R4, #0x1f");  // Clear the mode field
    asm("            ORR       R3, R3, R1");     // Set mode to ARG1
    asm("            MSR       CPSR, R3");       // Switch modes
    asm("            MOV       SP, R2");         // Set stack pointer to ARG2
    asm("            B         _trapret");
    asm("CMPSETCPSR: CMP       R0, #2");
    asm("            BNE       _trapret");
    asm("            MRS       R0, SPSR");
    asm("            BIC       R0, R0, R1");     // Clear bits spec'ed in ARG1
    asm("            ORR       R0, R0, R2");     // OR in the bits in ARG2
    asm("            MSR       SPSR, R0");
    asm("            B         _trapret");
    
    asm("_trapret");
    asm("    MSR       CPSR, R4");
//    asm("    LDR       R4, SAVER4");
//    asm("    LDR       R3, SAVER3");
//    asm("    LDR       R2, SAVER2");
//    asm("    LDR       R1, SAVER1");
//    asm("    LDR       R0, SAVER0");
    asm("    MOVS      PC, LR");

    asm("SAVESP:        .long 0x0");
    asm("SAVECPSR:      .long 0x0");
    asm("SAVER0:        .long 0x0");
    asm("SAVER1:        .long 0x0");
    asm("SAVER2:        .long 0x0");
    asm("SAVER3:        .long 0x0");
    asm("SAVER4:        .long 0x0");
    asm("SWI_CMD_ADDR:  .long _SWI_cmd");
    asm("SWI_ARG1_ADDR: .long _SWI_arg1");
    asm("SWI_ARG2_ADDR: .long _SWI_arg2");
    
 
    asm("_board_dsp_init:");
    asm(" ldr r0,CS0_addr");  // Set CS0
    asm(" ldr r1,CS0_value");
    asm(" str r1,[r0]");
    asm(" ldr r0,CS1_addr");  // Set CS1
    asm(" ldr r1,CS1_value");
    asm(" str r1,[r0]");
    asm(" ldr r0,CS2_addr");  // Set CS2
    asm(" ldr r1,CS2_value");
    asm(" str r1, [r0]");
    asm(" ldr r0,CS3_addr");  // Set CS3
    asm(" ldr r1,CS3_value");
    asm(" str r1, [r0]");
    asm(" ldr r0,CS4_addr");  // Set CS4
    asm(" ldr r1,CS4_value");
    asm(" str r1, [r0]");
    asm(" ldr r0,APIREG_addr");    // Set wait states for API RAM
    asm(" ldr r1,APIWS_value");
    asm(" str r1, [r0]");
    asm(" ldr r0,DSPboot0_addr");  // Set dummy loop at dsp boot
    asm(" ldr r1,DSPboot0_value");
    asm(" str r1, [r0]");
    asm(" ldr r0,DSPboot1_addr");  // Set NOPs at dsp boot
    asm(" ldr r1,DSPboot1_value");
    asm(" str r1, [r0]");        
    asm(" ldr r0,CLKMcntlreset_addr");  // Release DSP from reset
    asm(" ldr r1,CLKMcntlreset_value");
    asm(" str r1, [r0]");        
    asm(" B _c_int00");   // go to std C program setup before main
    asm("CS0_addr:");
    asm(" .long 0xffff2e00");
    asm("CS1_addr:");
    asm(" .long 0xffff2e04");
    asm("CS2_addr:");
    asm(" .long 0xffff2e08");    
    asm("CS3_addr:");
    asm(" .long 0xffff2e0c");    
    asm("CS4_addr:");
    asm(" .long 0xffff2e10");    
    asm("CS0_value");
    asm(" .long 0x4c3");  //little endian
    asm("CS1_value:");
    asm(" .long 0x4c3");  //little endian
    asm("CS2_value:");
    asm(" .long 0x4c3");  //little endian
    asm("CS3_value:");
    asm(" .long 0x4c3");  //little endian
    asm("CS4_value:");
    asm(" .long 0x4c3");  //little endian
    asm("APIREG_addr:");
    asm(" .long 0xffff2e14");  
    asm("APIWS_value:");
    asm(" .long 0x93");  // can be relaxed to 0x9f, if needed
    asm("DSPboot0_addr:");
    asm(" .long 0xffe03f00");  
    asm("DSPboot0_value:");
    asm(" .long 0xf073ff80");  
    asm("DSPboot1_addr:");
    asm(" .long 0xffe03f04");  
    asm("DSPboot1_value:");
    asm(" .long 0xf495f495");  
    asm("CLKMcntlreset_addr:");
    asm(" .long 0xffff2f10");  
    asm("CLKMcntlreset_value:");
    asm(" .long 0x00000000");