/******************************************************************************
*
* Freescale Semiconductor Inc.
* (c) Copyright 2011-2012 Freescale Semiconductor, Inc.
* ALL RIGHTS RESERVED.
*
***************************************************************************//*!
*
* @file common.c
*
* @author a13984
*
* @version 0.0.1
*
* @date Jul-15-2011
*
* @brief provide common system initialization routine,terminal I/O initialization,
*        and some common utilities. 
*
*******************************************************************************
*
* provide common system initialization routine,terminal I/O initialization,
*        and some common utilities
*
******************************************************************************/

#include "common.h"
#include "printf.h"

/******************************************************************************
* Global variables
******************************************************************************/
unsigned short global_pass_count = 0;
unsigned short global_fail_count = 0;


/*****************************************************************************//*!
+FUNCTION----------------------------------------------------------------
* @function name: print_sys_log
*
* @brief  print system reset sources
*        
* @param  none 
*
* @return none
*
* @ Pass/ Fail criteria: none
*****************************************************************************/

void print_sys_log(void) 
{
    uint8_t rst = 0;
   
    printf("\n\n--System Log BEGINS--\n\n");
    printf("\n SRS=0x%02x\n",SYS_SRS);
    
    if((SYS_SRS & SYS_SRS_POR_MASK) && (SYS_SRS & SYS_SRS_LVD_MASK)) 
    {
      rst = 1;
      printf("Power On Reset\n");
    }
    
    if(!(SYS_SRS & SYS_SRS_POR_MASK) && (SYS_SRS & SYS_SRS_LVD_MASK)) 
    {
      rst = 1;
      printf("LVD Reset\n");
    }
    
    if(SYS_SRS & SYS_SRS_WDOG_MASK)                 
    {
      rst = 1;
      printf("WDOG Reset\n");
    }
    
    if(SYS_SRS & SYS_SRS_ILAD_MASK)
    {
      rst = 1;
      printf("ILAD Reset\n");
    }
    
    if(SYS_SRS & SYS_SRS_ILOP_MASK)
    {
      rst = 1;
      printf("ILOP Reset\n");
    }
    
    if(SYS_SRS & SYS_SRS_PIN_MASK)
    {
      rst = 1;
      printf("Pin Reset\n");
    }
    if(SYS_SRS & SYS_SRS_LOC_MASK)
    {
      rst = 1;
      printf("Loss of Clock Reset\n");
    }    
    if(rst != 1)
    {
      printf("BDFR Reset\n");
    }
    
    printf("\n--System Log ENDS--\n\n");
  }


/*****************************************************************************//*!
+FUNCTION----------------------------------------------------------------
* @function name: end_test
*
* @brief  print test result (pass/fail counts) after end of test
*        
* @param  none 
*
* @return none
*
* @ Pass/ Fail criteria: none
*****************************************************************************/

void end_test(void){

  if(global_fail_count==0){
    printf("\n global_pass_count = 0x%02x%02x",(byte)(global_pass_count>>8),(byte)global_pass_count);
    printf("\n\n TEST PASSED");       
  } else{
    printf("\n global_pass_count = 0x%02x%02x",(byte)(global_pass_count>>8),(byte)global_pass_count);
    printf("\n global_fail_count = 0x%02x%02x",(byte)(global_fail_count>>8),(byte)global_fail_count);
    printf("\n\n TEST FAILED");
  }
  
  printf("\n\n TEST FINISHED");  
  
}


#pragma MESSAGE DISABLE C5703 /* INFORMATION C5703: w: parameter declared but not referenced */

#define _PUL_RA                  \
  }; asm PULH                  ; \
     asm PULX                  ; \
     asm {

#define _PUSH_REGS   \
  }; asm PSHA      ; \
     asm PSHX      ; \
     asm PSHH      ; \
     asm {

#pragma NO_EXIT
void _Search_Table_8_Addr(void) {
/* DESC:      Configure H:X with the jump address of the matching case statement
   IN:        A:X contains offset in jump table    TOS contains the return address(jump table address)
   OUT:       H:X contains the address of the matching case statement.
   WRITTEN:   A H:X SP */
/* ...
   JSR _Search_Table_8_Addr
   1 byte: Max (== Valuen)
   2 byte: Addr Default
   1 byte: Value0
   2 byte: Addr Label0
   1 byte: Value1
   2 byte: Addr Label1
   ...
   1 byte: Valuen
   2 byte: Addr Labeln

   All Values are sorted unsigned.
   All Addresses are absolute 16 bit.
*/
   asm {
                  PULH
                  PSHH
                  PSHX                ; // load low part of valuex on TOS
                  LDX   3,SP
                  TSTA                ; // If high part of valuex != 0
                  PULA                ; // load low part of valuex into A
                  BNE   found         ; // use default
                  CMP   0,X           ; // > max?
                  BHI   found         ; // if >max, branch to default

   search:        AIX   #3            ; // Skip (N & Default) entries
                  CMP   ,X            ; // Compare low part of valuex
                  BHI   search
                  BEQ   found

                  PULH                ; // use default
                  PULX                ; // Reload jump table address
                  BRA   end           ; // this may be optimized to a SKIP2 !
   found:
                  AIS   #2            ; // Update SP pointer
   end:
                  LDA   1,X           ; // Load address of matching case statement
                  LDX   2,X
                  PSHA
                  PULH
                  _SRET
                  JMP   ,X
   }
}

#pragma NO_EXIT
void _Search_Table_8_Offset(void) {
/* DESC:      Configure H:X with the jump address of the matching case statement
   IN:        A:X contains offset in jump table    TOS contains the return address(jump table address)
   OUT:       H:X contains the address of the matching case statement.
   WRITTEN:   A H:X SP */
/* ...
   JSR _Search_Table_8_Offset
   1 byte: Max (== Valuen)
   1 byte: Offset Default
   1 byte: Value0
   1 byte: Offset Label0
   1 byte: Value1
   1 byte: Offset Label1
   ...
   1 byte: Valuen
   1 byte: Offset Labeln

   All Values are sorted unsigned.
   All Addresses are absolute 8 bit relative from the next address.
     Example : 0x1234 DCB 0x12 points to address 0x1234+0x12+1 == 0x1247

*/
   asm {
                  PULH
                  PSHH
                  PSHX                ; // Save offset in jump table on TOS
                  LDX  3,SP
                  TSTA                ; // Compare high part
                  PULA
                  BNE   found         ; // use default if != 0
                  CMP   0,X           ; //  > max?
                  BHI   found         ; // if >max, branch to default
   search:
                  AIX   #2            ; // Skip (N & Default) entries
                  CMP   ,X            ; // Compare low part of valuex
                  BHI   search
                  BEQ   found
                  PULH
                  PULX
                  BRA   end           ; // this may be optimized to a SKIP2 !
   found:
                  AIS   #2            ; // Update SP pointer
   end:
                  TXA
                  ADD   1,X           ; // Load offset of matching case statement
                  TAX
                  PSHH
                  PULA
                  ADC   #0
                  PSHA
                  PULH
                  _SRET
                  JMP   2,X           ; //  2 : 1 (1,X), 1 : sizeof(value) 
   }
}

#pragma NO_EXIT
static void _IDIVMOD (char dummy_sgn, int j, int dummy, int i, ...) {
/**************************************************************************
    a := a % b,
    b := a / b,
------------------------------------
  algorithm: b == b1:b0, a == a1:a0
------------------------------------
  if (b1 == 0) {
    <simple algo>
  } else if (a < b) {
    <common shift algo (8 shifts suffice)>
  }
***************************************************************************/
  asm {
                TST     j:0
                BNE     _IDIVMOD_0    ; // b >= 256
                ; // {
                ; //    j < 256
                ; // }
                LDA     i:0
                LDX     j:1
                CLRH
                DIV
                STA     j:0
                LDA     i:1
                DIV
                STA     j:1
                CLR     i:0
                PSHH
                PULA
                STA     i:1
                RTS             ; // end
    _IDIVMOD_0: ; // {
                ; //    j >= 256
                ; // }
                CLRA
                PSHA
                ; //
#define r i:-8 /** the value pushed on the stack **/
                ; //
                LDX     #8
                CLC
                ;
    _IDIVMOD_1: ;
                ROL     i:1
                ROL     i:0
                ROL     r
                LDA     j:0
                CMP     r
                BHI     _IDIVMOD_3   ; // carry cleared !
                BNE     _IDIVMOD_2
                LDA     j:1
                CMP     i:0
                BHI     _IDIVMOD_3   ; // carry cleared !
    _IDIVMOD_2: ;
                LDA     i:0
                SUB     j:1
                STA     i:0
                LDA     r
                SBC     j:0
                STA     r
                SEC
                ;
    _IDIVMOD_3: ;
                DBNZX   _IDIVMOD_1
                ;
                LDA     i:1
                ROLA
                STA     j:1
                LDA     i:0
                STA     i:1
                PULA            ; // that is 'r'
                STA     i:0
                CLR     j:0
                ;
                RTS
  }
}

static void _SPLITSIGN (char dummy1, int j,  int dummy, int i, ...) {
  asm {
                TST     i:0
                BPL     _SPLIT_0
                COM     i:0
                NEG     i:1
                BNE     _SPLIT_0
                INC     i:0
    _SPLIT_0:   ;
                TST     j:0
                BPL     _SPLIT_1
                COM     j:0
                NEG     j:1
                BNE     _SPLIT_1
                INC     j:0
    _SPLIT_1:   ;
; //               RTS ! {A}, {HX+A+SR}
                JMP     _IDIVMOD  ! {}, {HX+A+SR}
  }
}

#if defined(__HCS08__)
#pragma NO_EXIT
#pragma NO_RETURN
void _IDIVU_STAR08(int i, ...) {
/* DESC:    16 bit unsigned division k = i / j
   IN:      TOS i, H:X j
   OUT:     H:X k
   WRITTEN: A,X,H */

  int j;
  asm {
                STHX j
                PSHA
                JSR      _IDIVMOD  ! {}, {HX+A+SR}
                PULA
                PULH
                PULX
                RTS ! {A + X}, {H + SR}
  }
}
#endif

#if defined(__HCS08__)
#pragma NO_EXIT
#pragma NO_RETURN
void _IMODU_STAR08(int i, ...) {
/* DESC:    16 bit unsigned modulus k = i % j
   IN:      TOS i, H:X j
   OUT:     H:X k
   WRITTEN: A,X,H */
  int j;
  asm {
                STHX     j
                PSHA
                JSR      _IDIVMOD ! {}, {HX+A+SR}
                PULA
                AIS      #2
                LDHX     3,SP
                RTS ! {A + X}, {H + SR}
  }
}
#endif

#pragma NO_EXIT
#pragma NO_RETURN
void _IDIVU_8(void) {
/* DESC:    16 by 8 bit unsigned division k = i / j, unsigned int i, char j 
   IN:      H:A i, X j
   OUT:     A k
   WRITTEN: A,X,H */
  asm {
                PSHA
                PSHH
                DIV                    ; // sets carry if overflow occured
                BCC      _IDIVU_8_0
#ifdef __HCS08__
                CLRH
                JSR      _IDIVU_STAR08
                TXA
                PSHH
                PULX
#else
                TXA
                CLRX
                JSR      _IDIVU
#endif
    _IDIVU_8_0:
                AIS      #2
                RTS
  }
}

#pragma NO_EXIT
#pragma NO_RETURN
void _IMODU_8(void) {
/* DESC:    16 by 8 bit unsigned modulus k = i % j, unsigned int i, char j 
   IN:      H:A i, X j
   OUT:     A k
   WRITTEN: A,X,H */
  asm {
                PSHA
                PSHH
                DIV                    ; // sets carry if overflow occured
                BCC      _IMODU_8_0
#ifdef __HCS08__
                CLRH
                JSR      _IMODU_STAR08
                TXA
                PSHH
                PULX
#else
                TXA
                CLRX
                JSR      _IMODU
#endif
                BRA      _IMODU_8_1
    _IMODU_8_0:
                PSHH
                PULA
    _IMODU_8_1:
                AIS      #2
                RTS
  }
}

#if !defined(__HCS08__)
#pragma NO_EXIT
#pragma NO_RETURN
void _IDIVS(int i, ...) {
/* DESC:    16 bit signed division k = i / j
   IN:      TOS i, X:A j
   OUT:     X:A k
   WRITTEN: A,X,H */
  int j;
  asm {
                STA j:1
                STX j
                LDA     i:0
                EOR     j:0
                PSHA
                JSR     _SPLITSIGN ! {}, {HX+A+SR}
; //              JSR     _IDIVMOD  ! {}, {HX+A+SR}
                PULA
                TSTA
                BPL     _IDIVS_0     ; // done
                COM     j:0
                NEG     j:1
                BNE     _IDIVS_0
                INC     j:0
     _IDIVS_0:    ;
                PULX
                PULA
                RTS ! {A + X}, {H + SR}
  }
}
#endif

#if defined(__HCS08__)
#pragma NO_EXIT
#pragma NO_RETURN
void _IDIVS_STAR08(int i, ...) {
/* DESC:    16 bit signed division k = i / j
   IN:      TOS i, H:X j
   OUT:     H:X k
   WRITTEN: A,X,H */
  int j;
  asm {
                STHX    j
                LDA     i:0
                EOR     j:0
                PSHA
                JSR     _SPLITSIGN ! {}, {HX+A+SR}
; //              JSR     _IDIVMOD  ! {}, {HX+A+SR}
                PULA
                TSTA
                BPL     _IDIVS_0     ; // done
                COM     j:0
                NEG     j:1
                BNE     _IDIVS_0
                INC     j:0
     _IDIVS_0:    ;
                PULH
                PULX
                RTS ! {A + X}, {H + SR}
  }
}
#endif

#if defined(__HCS08__)
#pragma NO_EXIT
#pragma NO_RETURN
void _IMUL_STAR08(int i, ...) {
/* DESC:    16 bit multiplication k = i * j
   IN:      TOS i, H:X j
   OUT:     H:X k
   WRITTEN: A,X,H */
  int j;
  asm {
                STHX     1,SP
                LDA      i:1
                LDX      j:1
                MUL
                PSHA
                PSHX
                LDA      i:1
                LDX      j:0
                MUL
                ADD      1,SP
                PSHA
                LDA      i:0
                LDX      j:1
                MUL
                ADD      1,SP
                STA      j:0
                AIS      #2
                PULA
                STA      j:1
                PULH
                PULX
                RTS  ! {A + X}, {H + SR}
  }
}
#endif

#if defined(__HCS08__)
#pragma NO_EXIT
void _CALL_STAR08(void) {
/* DESC:    calls a function (dest as pointer) with 2 register parameters (complex due to lack of registers!!!)
   IN:      H:X function pointer, TOS parameter assigned to X or H:X
   OUT:
   WRITTEN: A,X,H */

   asm {
     PSHX        ; // push function pointer
     PSHH
     PSHA        ; // push non pushed register param
     TSX
     LDA  6,X    ; // push pushed register params
     PSHA
     LDA  5,X
     PSHA
     LDA  4,X    ; // move ret@:lo
     STA  6,X
     LDA  3,X    ; // move ret@:hi
     STA  5,X
     LDA  2,X    ; // move function pointer:lo
     STA  4,X
     LDA  1,X    ; // move function pointer:hi
     STA  3,X
     PULH        ; // pull register params to their register
     PULX
     PULA
     AIS  #2     ; // free resulting gap. now the function pointer is TOS and the RTS calls the function.
     RTS  ! {A+X} , {H+SR}
   }
}
#endif

#pragma NO_EXIT
void _ICMP(int j, ...) {
/* DESC:     16 bit compare i <rel> j
   IN:       X:A i, TOS j
   OUT:
   WRITTEN:  A,H,X */

  asm {
                CPX     j:0
                BNE     _ICMP_0
                CMP     j:1
                BEQ     _ICMP_0
                CLRA
                BCS     _ICMP_1
                LDA     #2
  _ICMP_1:       ;
                CMP     #1
  _ICMP_0:       ; // flags are set for i - j
                _PUL_RA
                AIS     #2                      ; // free pushed parameter
                JMP     0,X ! {HX+SR}, {HX+A}
  }
}

#pragma NO_EXIT
void _Jump_Table_Addr(void) {
/* DESC:      Configure H:X with the jump address of the matching case statement
   IN:        A:X contains offset in jump table    TOS contains the return address(jump table address)
   OUT:       H:X contains the address of the matching case statement.
   WRITTEN:   A H:X SP */
/* ...
   JSR _Jump_Table_Addr
   2 byte: Addr Label0
   2 byte: Addr Label1
   ...
   2 byte: Addr Labeln

   All Addresses are absolute 16 bit.
*/
   asm {
                ASLX
                ROLA
                PSHA
                TXA
                ADD  3,SP  ; // add low(index) + low(retadr)
                TAX
                PULA
                ADC 1,SP   ; // add high(index) + high(retadr) + carry
                PSHA
                PULH
                AIS   #2    ; // Release stack
                LDA   ,X    ; // Load address of matching case statement
                LDX   1,X
                PSHA
                PULH
                _SRET
                JMP   ,X
   }
}

#pragma NO_EXIT
void _Jump_Table_Offset(void) {
/* DESC:      Configure H:X with the jump address of the matching case statement
   IN:        A:X contains offset in jump table    TOS contains the return address(jump table address)
   OUT:       H:X contains the address of the matching case statement.
   WRITTEN:   A H:X SP */
/* ...
   JSR _Jump_Table_Offset
   1 byte: Offset Label0
   1 byte: Offset Label1
   ...
   1 byte: Offset Labeln

   All Addresses are absolute 8 bit relative from the next address.
     Example : 0x1234 DCB 0x12 points to address 0x1234+0x12+1 == 0x1247.

*/
   asm {
                  PSHA                ; // save high of index
                  TXA
                  ADD   3,SP          ; // add low of return address to low of index
                  TAX
                  PULA                ; // restore high of index
                  ADC   1,SP          ; // add high of return address to high of index
                  PSHA                ; // move high to h register
                  PULH
                  AIS  #2             ; // free stack from return address
                  TXA
                  ADD   0,X           ; // A <- (H:X) 
                  TAX
                  PSHH                ; // H <- H + (1,SP)
                  PULA
                  ADC   #0
                  PSHA
                  PULH
                  _SRET
                  JMP  1,X            ; // 1: 1 for sizeof (offset)
   }
}

#pragma NO_EXIT
void _Jump_Table_Header_Addr(void) {
/* DESC:      Configure H:X with the jump address of the matching case statement
   IN:        A:X contains offset in jump table    TOS contains the return address(jump table address)
   OUT:       H:X contains the address of the matching case statement.
   WRITTEN:   A H:X SP */
/* ...
   JSR _Jump_Table_Header_Addr
   2 byte: N
   2 byte: Addr Default
   2 byte: Addr Label0
   2 byte: Addr Label1
   ...
   2 byte: Addr Labeln

   All Addresses are absolute 16 bit.

*/
   asm {
                  PULH                ; // load high of return address
                  PSHX                ; // save offset in jump table on TOS
                  PSHA
                  LDX   3,SP          ; // load low of return address
                  CMP   ,X            ; // compare high part

                  BHI   defaul
                  BNE   normalCase

                  LDA   1,X           ; // load low part of N
                  CMP   2,SP          ; // compare low part
                  BLS   defaul

   normalCase:    AIX   #2            ; // skip (N & Default) entries

                  ASL   2,SP          ; // offset scaling
                  ROL   1,SP

                  TXA                 ; // X <- X + (0,SP)
                  ADD   2,SP
                  TAX

                  PSHH                ; // H <- H + (1,SP)
                  PULA
                  ADC   1,SP
                  PSHA
                  PULH
   defaul:
                  AIS   #3            ; // Release the stack
                  LDA   2,X           ; // Load address of matching case statement
                  LDX   3,X
                  PSHA
                  PULH
                  _SRET
                  JMP   ,X
   }
}

#pragma NO_EXIT
void _Jump_Table_Header_Offset(void) {
/* DESC:      Configure H:X with the jump address of the matching case statement
   IN:        A:X contains offset in jump table    TOS contains the return address(jump table address)
   OUT:       H:X contains the address of the matching case statement.
   WRITTEN:   A H:X SP */
/* ...
   JSR _Jump_Table_Header_Offset
   1 byte: N
   1 byte: Offset Default
   1 byte: Offset Label0
   1 byte: Offset Label1
   ...
   1 byte: Offset Labeln


   All Addresses are absolute 8 bit relative from the next address.
     Example : 0x1234 DCB 0x12 points to address 0x1234+0x12+1 == 0x1247.

*/
   asm {
                  TSTA
                  TXA
                  PULH                ; // load return address
                  PULX
                  BNE   defaul        ; // if high(A:X) != 0, use default
                  CMP   0,X           ; // Compare low part
                  BHS   defaul        ; // if it contains less entries than table, goto nondefaul
                  AIX   #1            ; // skip default
                  PSHX
                  ADD   1,SP
                  PSHA
                  PSHH
                  PULA
                  ADC   #0
                  PSHA
                  PULH
                  PULX
                  PULA                ; // free stack
defaul:
                  TXA
                  ADD   1,X           ; // A <- (H:X) 
                  TAX
                  PSHH                ; // H <- H + (1,SP)
                  PULA
                  ADC   #0
                  PSHA
                  PULH
                  _SRET
                  JMP 2,X             ; // 2: 1 for number of labels, 1 sizeof (offset)
   }
}
