////////////////////////////////////////////////////////////////////////
//
//  bios modules for Pico-2
//      version 2.4 (July 9, 2009)
//
//  (c) Hideki Kozima (xkozima@myu.ac.jp), subject to GPLv2
//
//  modules
//      SWT: address switches (SW1-5 = PE21-17)
//          void   SWT_init (void);
//          ushort SWT_get (void);
//      LED: indicators (LED1-5 = PE16-12)
//          void   LED_init (void);
//          void   LED_set (ushort dataW5);
//          void   LED_op  (ushort locW5, ushort dataW5);
//      PWM: PWM/DIR output (TIOC0A-D; PE11,9,7,5)
//          void   PWM_init (void);
//          void   PWM_set (uchar numB2, short valW);
//      RCS: RC servo output to CN6=RCS (PE10,8,6,4)
//          void   RCS_init (void);
//          void   RCS_set (uchar numB2, ushort valW);
//      ENC: encoder interface (ZEN2044F)
//          void   ENC_init (void);
//          int    ENC_get (uchar numB2);
//          void   ENC_set (uchar numB2, int count);
//      SEN: current sensor input (AN19-16)
//          void   SEN_init (void);
//          ushort SEN_get  (uchar numB2);
//          void   SEN_gets (ushort *sen0W, *sen1W, *sen2W, *sen3W);
//      LIO: logic I/O on CN7=LIO (PB5-2)
//          void   LIO_init (uchar dirB4);
//          void   LIO_set (uchar valB4);
//          ushort LIO_get (void);
//      LIN: logic input of CN8=ALINX (PF11-8) and CN9=ALINY (PF15-12)
//          void   LIN_init (void);
//          ushort LIN_get (void);
//      AIX: analog input of CN8=ALINX (PF11-8) 
//          void   AIX_init (void);
//          void   AIX_gets (ushort *val0W, ushort *val1W, 
//                           ushort *val2W, ushort *val3W );
//      AIY: analog input of CN8=ALINY (PF15-12) 
//          void   AIY_init (void);
//          void   AIY_gets (ushort *val0W, ushort *val1W, 
//                           ushort *val2W, ushort *val3W );
//      CMT: compare match timer (CMT0-1)
//          void  CMT0_init (uchar cselB2, ushort periodW, void (*action)());
//          void  CMT1_init (uchar cselB2, ushort periodW, void (*action)());
//          void  CMT0_quit (void);
//          void  CMT1_quit (void);
//      SCI: serial port (RS485)
//          void  SCI_init (uchar addr_mask, uchar addr, 
//                          uchar pack_len, void *interpreter());
//          void  SCI_transmit (uchar *packet);
//          void  SCI_quit  (void);
//      WDT: watch dog timer (WDT)
//          void  WDT_init (uchar cselB3);
//          void  WDT_reset ();
//          int   WDT_check ();

//
//  bitwise operation

#define  bit_op(var,mask,data)  (var=((var)&(~(mask)))|(data))

////////////////////////////////////////////////////////////////////////
//
//  SWT: address switches (SW1-5 = PE17-21)
//      void  SWT_init (void);
//          initialize SWT (PE21-17: in).
//      ushort  SWT_get (void);
//          read 5bits (0x00-0x1f) status of the switches.

void  SWT_init (void)
{
    //  PE21-17: I/O
    bit_op(PFC.PECRH.WORD,  0x0ffc, 0x0000);

    //  PE21-17: input
    bit_op(PFC.PEIORH.WORD, 0x003e, 0x0000);
}

ushort  SWT_get (void)
{
    //  get PE21-17 (= DRH[5:1])
    return (~(PE.DRH.WORD >> 1)) & 0x1f;
}

////////////////////////////////////////////////////////////////////////
//
//  LED: indicators (LED1-5 = PE16-12)
//      void   LED_init (void);
//          initialize LED (PE16-12: out).
//      void   LED_set (ushort  dataW5);
//          set 5bits data to LED port.
//      ushort LED_get (void);
//          get 5bits data from LED port
//      void   LED_op  (ushort locW5, ushort dataW5);
//          manipulate specific bit(s) of LED port.
//          (ex. LED_op(0x05, 0x05) puts on LED3 and LED0.)

void  LED_init (void)
{
    //  PE16-12: I/O
    bit_op(PFC.PECRH.WORD,  0x0003, 0x0000);
    bit_op(PFC.PECRL1.WORD, 0xff00, 0x0000);

    //  PE16-12: output
    bit_op(PFC.PEIORH.WORD, 0x0001, 0x0001);
    bit_op(PFC.PEIORL.WORD, 0xf000, 0xf000);
}

void  LED_set (ushort dataW5)
{
    ushort  dataH, dataL;

    //  set hi/lo
    dataH = (~dataW5 & 0x001f) >> 4;
    dataL = (~dataW5 & 0x001f) << 12;

    //  set PE16-12 (= DRH[0],DRL[15-12])
    bit_op(PE.DRH.WORD, 0x0001, dataH);
    bit_op(PE.DRL.WORD, 0xf000, dataL);
}

ushort LED_get (void)
{
    ushort  data;

    //  get PE16-12 (= DRH[0],DRL[15-12])
    data = (PE.DRH.WORD << 4) | (PE.DRL.WORD >> 12);

    //  return complement (lower 5bits)
    return (~data & 0x1f);
}

void  LED_op  (ushort locW5, ushort dataW5)
{
    ushort  data;

    //  get current LED status
    data = LED_get();

    //  manipulate specific bits
    bit_op(data, locW5, dataW5);

    //  output to LED port
    LED_set(data);
}

////////////////////////////////////////////////////////////////////////
//
//  PWM: PWM output (TIOC0A-D)
//      void  PWM_init (void);
//          initialize PWM (TIOC0A-D = PE0-3: PWM out).
//          (TGRA-D_0, PWM2 mode, clear with TGRA_1, 24KHz=0x0400*P.)
//      void  PWM_set (uchar numB2, short valW);
//          set duty ratio of PWM[numB2] to valW16,
//              where numB2: PWM axis number [0..3],
//                    valW : 16bit value -> abs -> shift to [0..0x03ff].

#define  PWM_CYCLE    0x0400                    //  41.666us (24KHz)
#define  PWM_SHIFT    6                         //  0xffff -> 0x03ff
#define  PWM_DEFAULT  0x0400                    //  never match (= 0%)

void  PWM_init (void)
{
    //  init DIR (PE11,9,7,5) as I/O out
    bit_op(PFC.PECRL1.WORD, 0x00cc, 0x0000);
    bit_op(PFC.PECRL2.WORD, 0xcc00, 0x0000);
    bit_op(PFC.PEIORL.WORD, 0x0aa0, 0x0aa0);

    //  wake up MTU from stand-by mode
    bit_op(MSTCR.MSTCR2.WORD, 0x2000, 0x0000);

    //  stop counting (for ch0,4)
    bit_op(MTU.TSTR.BYTE, 0x81, 0x00);

    //  select clear/edge/presc (for ch0)
    bit_op(MTU0.TCR.BYTE, 0xe0, 0x60);          //  sync clear
    bit_op(MTU0.TCR.BYTE, 0x18, 0x00);          //  rise edge
    bit_op(MTU0.TCR.BYTE, 0x07, 0x00);          //  at P (P=24.576MHz)

    //  TGRA-D as output cmp reg (0->1)
    MTU0.TIOR.WORD = 0x2222;

    //  set initial duty ratio
    MTU0.TCNT = 0x0000;
    MTU0.TGRA = PWM_DEFAULT;
    MTU0.TGRB = PWM_DEFAULT;
    MTU0.TGRC = PWM_DEFAULT;
    MTU0.TGRD = PWM_DEFAULT;

    //  set ch0 to PWM2 mode
    bit_op(MTU0.TMDR.BYTE, 0xcf, 0xc3);

    //  pin assignment PE0-3 -> TIOC0A-D
    bit_op(PFC.PECRL2.WORD, 0x00ff, 0x0055);
    bit_op(PFC.PEIORL.WORD, 0x000f, 0x000f);

    //  select clear/edge/presc (for ch4)
    bit_op(MTU34.TCR_4.BYTE, 0xe0, 0x20);       //  clear with TGRA_4
    bit_op(MTU34.TCR_4.BYTE, 0x18, 0x00);       //  rise edge
    bit_op(MTU34.TCR_4.BYTE, 0x07, 0x00);       //  P (P=24.576MHz)

    //  set PWM period (to clear all)
    MTU34.TCNT_4 = 0x0000;
    MTU34.TGRA_4 = PWM_CYCLE - 1;

    //  set ch4 to normal mode
    bit_op(MTU34.TMDR_4.BYTE, 0xcf, 0xc0);

    //  sync ch0,4
    bit_op(MTU.TSYR.BYTE, 0x81, 0x81);

    //  start ch0,4
    bit_op(MTU.TSTR.BYTE, 0x81, 0x81);
}

#define  PWM_dir(n)  (0x0020<<((n)*2))

void  PWM_set (uchar numB2, short valW)
{
    int     valLabs;
    ushort  dir, valWc;

    //  get absolute value & output direction
    dir = PWM_dir(numB2 & 0x03);
    if (valW == -32768) {
        //  avoid out-of-range of 15bits: abs(-32768)=0x8000
        valLabs = 0x00007fff;                   //  trim to 15bits
        bit_op(PE.DRL.WORD, dir, dir);          //  negative direction
    }
    else if (valW < 0) {
        valLabs = valW;
        valLabs = -valLabs;                     //  [0,0x00007fff]
        bit_op(PE.DRL.WORD, dir, dir);          //  negative direction
    }
    else {
        valLabs = valW;                         //  [0,0x00007fff]
        bit_op(PE.DRL.WORD, dir, 0);            //  positive direction
    }

    //  make complement and trim
    valWc = (0x00007fff - valLabs) >> (PWM_SHIFT - 1);

    //  when trying 0%
    if (valWc == PWM_CYCLE - 1)
        valWc = PWM_CYCLE;                      //  force 0% (never match)

    //  update duty-ratio
    switch (numB2 & 0x03) {
        case 0: MTU0.TGRA = valWc; break;
        case 1: MTU0.TGRB = valWc; break;
        case 2: MTU0.TGRC = valWc; break;
        case 3: MTU0.TGRD = valWc; break;
    }
}

////////////////////////////////////////////////////////////////////////
//
//  RCS: RC servo output to CN6=RCS (PE10,8,6,4)
//      void  RCS_init (void);
//          initialize PWM1 at T=10.667ms, 93.75Hz.
//          (PE10=TIOC3C, PE08=TIOC3A, PE06=TIOC2A, PE04=TIOC1A)
//      void  RCS_set (uchar numB2, ushort valW);
//          set duty ratio of RSC[numB2] to valW16,
//              where numB2 : RCS axis number [0..3],
//                    valW16: 16bit value [0..0xffff].
//                            (0x2400 gives 1.5ms = center)

#define  RCS_CYCLE    0x1000			//  10.667ms (4096)
#define  RCS_SHIFT    4                         //  16bit-log2(RCS_CYCLE)
#define  RCS_DEFAULT  0x0240                    //  1.500ms (576)

void  RCS_init (void)
{
    //  wake up MTU from stand-by mode
    bit_op(MSTCR.MSTCR2.WORD, 0x2000, 0x0000);

    //  stop counting (for ch1,2,3)
    bit_op(MTU.TSTR.BYTE, 0x46, 0x00);

    //  pin assignment PE10=TIOC3C,PE8=TIOC3A,PE6=TIOC2A,PE4=TIOC1A
    bit_op(PFC.PECRL1.WORD, 0x0033, 0x0011);
    bit_op(PFC.PECRL2.WORD, 0x3300, 0x1100);
    bit_op(PFC.PEIORL.WORD, 0x0550, 0x0550);

    //  select clear/edge/presc (for ch1)
    bit_op(MTU1.TCR.BYTE, 0xe0, 0x20);          //  TGRA clear
    bit_op(MTU1.TCR.BYTE, 0x18, 0x00);          //  rise edge
    bit_op(MTU1.TCR.BYTE, 0x07, 0x03);          //  P/64 (P=24.576MHz)

    //  select clear/edge/presc (for ch2)
    bit_op(MTU2.TCR.BYTE, 0xe0, 0x20);          //  TGRA clear
    bit_op(MTU2.TCR.BYTE, 0x18, 0x00);          //  rise edge
    bit_op(MTU2.TCR.BYTE, 0x07, 0x03);          //  P/64 (P=24.576MHz)

    //  select clear/edge/presc (for ch3)
    bit_op(MTU34.TCR_3.BYTE, 0xe0, 0x20);       //  TGRA clear
    bit_op(MTU34.TCR_3.BYTE, 0x18, 0x00);       //  rise edge
    bit_op(MTU34.TCR_3.BYTE, 0x07, 0x03);       //  P/64 (P=24.576MHz)

    //  ch1,2: TGRA=0/0, TGRB=0/1
    //  ch3  : TGRA=0/0, TGRB=0/1, TGRC=0/0, TGRD=0/1, 
    MTU1.TIOR.BYTE.H  = 0x21;
    MTU2.TIOR.BYTE.H  = 0x21;
    MTU34.TIOR_3.WORD = 0x2121;

    //  set PWM period and duty-ratio
    //      period: P/64/65536 -> 10.67ms (93.75Hz)
    //      duty 3520 -> 1.50ms (default)
    MTU1.TCNT    = 0x0000;
    MTU1.TGRA    = RCS_CYCLE -1;
    MTU1.TGRB    = RCS_CYCLE - RCS_DEFAULT;
    MTU2.TCNT    = 0x0000;
    MTU2.TGRA    = RCS_CYCLE -1;
    MTU2.TGRB    = RCS_CYCLE - RCS_DEFAULT;
    MTU34.TCNT_3 = 0x0000;
    MTU34.TGRA_3 = RCS_CYCLE -1;
    MTU34.TGRB_3 = RCS_CYCLE - RCS_DEFAULT;
    MTU34.TGRC_3 = RCS_CYCLE -1;
    MTU34.TGRD_3 = RCS_CYCLE - RCS_DEFAULT;

    //  set ch1,2,3 to PWM1 mode
    bit_op(MTU1.TMDR.BYTE, 0xcf, 0xc2);
    bit_op(MTU2.TMDR.BYTE, 0xcf, 0xc2);
    bit_op(MTU34.TMDR_3.BYTE, 0xcf, 0xc2);

    //  start ch1,2,3
    bit_op(MTU.TSTR.BYTE, 0x46, 0x46);
}

void  RCS_set (uchar numB2, ushort valW)
{
    ushort valWc;

    //  make complement and trim
    valWc = (0xffff - valW) >> RCS_SHIFT;

    //  when trying 0%
    if (valWc == RCS_CYCLE - 1)
        valWc = RCS_CYCLE;                      //  force 0% (never match)

    //  update duty-ratio
    switch (numB2 & 0x03) {
        case 0: MTU1.TGRB    = valWc; break;
        case 1: MTU2.TGRB    = valWc; break;
        case 2: MTU34.TGRB_3 = valWc; break;
        case 3: MTU34.TGRD_3 = valWc; break;
    }
}

////////////////////////////////////////////////////////////////////////
//
//  ENC: encoder interface (ZEN2044F)
//      void  ENC_init (void);
//          initialize the interface to ZEN2044F.
//              PA15 : C/D* (Commnand or Data)
//              PA14 : RD*  (Read strobe)
//              PA13 : WR*  (Write strobe)
//              PA12 : AD1  (address high-bit)
//              PA11 : AD0  (address low-bit)
//              PA7-0: command or data
//      int   ENC_get (uchar numB2);
//              get encoder count of axis numB2.
//      void  ENC_set (uchar numB2, int count);
//              set encoder count of axis numB2.

void  ENC_init (void)
{
    void  ENC_set(uchar, int);

    //  PA7-0: in
    bit_op(PFC.PACRL3.WORD, 0x00ff, 0x0000);
    bit_op(PFC.PACRL2.WORD, 0xffff, 0x0000);
    bit_op(PFC.PAIORL.WORD, 0x00ff, 0x0000);    //  PA7-0: in

    //  PA15-11: out
    //      PA15:C/D*=0, PA14:RD*=1, PA13:WR*=1, PA12-11:AD=00
    bit_op(PFC.PACRL3.WORD, 0xf800, 0x0000);
    bit_op(PFC.PACRL1.WORD, 0xffc0, 0x0000);
    bit_op(PA.DR.WORD, 0xf800, 0x6000);
    bit_op(PFC.PAIORL.WORD, 0xf800, 0xf800);    //  PA15-11: out

    //  set zero to encoders
    ENC_set(0, 0);
    ENC_set(1, 0);
    ENC_set(2, 0);
    ENC_set(3, 0);
}

int  ENC_get (uchar numB2)
{
    ushort  axis;
    uint    valLB, valMB, valHB;                //  24bits={H,M,L}
    int     result;

    //  latch data
    axis = (numB2 << 11);
    bit_op(PA.DR.WORD, 0xf8ff, 0xe010 | axis);  //  mark C + "ud-cnt/latch"
    PFC.PAIORL.BYTE.L = 0xff;                   //  set PA0-7 out
    bit_op(PA.DR.WORD, 0x6000, 0x4000);         //  mark WR*
    bit_op(PA.DR.WORD, 0xe000, 0x6000);         //  unmark WR* + C
    PFC.PAIORL.BYTE.L = 0x00;                   //  set PA0-7 in

    //  read data
    bit_op(PA.DR.WORD, 0x6000, 0x2000);         //  mark RD*
    valLB = PA.DR.BYTE.L;                       //  read low-byte
    bit_op(PA.DR.WORD, 0x6000, 0x6000);         //  unmark RD*
    bit_op(PA.DR.WORD, 0x6000, 0x2000);         //  mark RD*
    valMB = PA.DR.BYTE.L;                       //  read mid-byte
    bit_op(PA.DR.WORD, 0x6000, 0x6000);         //  unmark RD*
    bit_op(PA.DR.WORD, 0x6000, 0x2000);         //  mark RD*
    valHB = PA.DR.BYTE.L;                       //  read high-byte
    bit_op(PA.DR.WORD, 0x6000, 0x6000);         //  unmark RD*

    //  make up the result
    result = (valHB << 16) | (valMB << 8) | valLB;
    if (valHB & 0x80)
        result |= 0xff000000;

    return  result;
}

void  ENC_set (uchar numB2, int count)
{
    ushort  axis;

    //  set access pointer
    axis = (numB2 << 11);
    bit_op(PA.DR.WORD, 0xf8ff, 0xe008 | axis);  //  mark C + "preload"
    PFC.PAIORL.BYTE.L = 0xff;                   //  set PA0-7 out
    bit_op(PA.DR.WORD, 0x6000, 0x4000);         //  mark WR*
    bit_op(PA.DR.WORD, 0xe000, 0x6000);         //  unmark WR* + C

    //  write data
    PA.DR.BYTE.L = count & 0x000000ff;          //  set lower data
    bit_op(PA.DR.WORD, 0x6000, 0x4000);         //  mark WR*
    bit_op(PA.DR.WORD, 0x6000, 0x6000);         //  unmark WR*
    PA.DR.BYTE.L = (count & 0x0000ff00) >> 8;   //  set mid data
    bit_op(PA.DR.WORD, 0x6000, 0x4000);         //  mark WR*
    bit_op(PA.DR.WORD, 0x6000, 0x6000);         //  unmark WR*
    PA.DR.BYTE.L = (count & 0x00ff0000) >> 16;  //  set higher data
    bit_op(PA.DR.WORD, 0x6000, 0x4000);         //  mark WR*
    bit_op(PA.DR.WORD, 0x6000, 0x6000);         //  unmark WR*

    //  load (preload -> ud-cnt)
    bit_op(PA.DR.WORD, 0xf8ff, 0xe088 | axis);  //  mark C + "LD+preload"
    bit_op(PA.DR.WORD, 0x6000, 0x4000);         //  mark WR*
    bit_op(PA.DR.WORD, 0xe000, 0x6000);         //  unmark WR* + C

    //  calm down
    PFC.PAIORL.BYTE.L = 0x00;                   //  set PA0-7 in
}

////////////////////////////////////////////////////////////////////////
//
//  SEN: current sensor input (AN19-16)
//      void   SEN_init (void);
//          init AD2 for AN[19-16]=PG03-00 (SEN3-0), 
//          and start the first A/D conversion.
//      ushort  SEN_get (uchar num);
//          read analog value from SENnum, 
//              where return=0x0000 for 0V (GND level),
//                           0xffff for 5V (Vcc level).
//              (usually, a steady offset (approx. 0x0300)
//               will be observed for 0V input.)
//      void   SEN_gets (ushort *sen0W, ushort *sen1W, 
//                       ushort *sen2W, ushort *sen3W );
//          read analog value from SEN3-0.
//              where sen?W=0x0000 for 0V (GND level),
//                          0xffff for 5V (Vcc level).
//              (usually, a steady offset (approx. 0x0300)
//               will be observed for 0V input.)

void  SEN_init (void)
{
    //  wake up AD2 from stand-by mode
    bit_op(MSTCR.MSTCR2.WORD, 0x0040, 0x0000);

    //  stop conversion (while setup)
    bit_op(AD.ADCR_2, 0x10, 0x00);

    //  setup control registers
    //      no-int, 4ch-scan, AN19-16
    //      P/8 (266/256T=24KHz), continuous-scan
    bit_op(AD.ADCSR_2, 0x77, 0x13);
    bit_op(AD.ADCR_2,  0xf8, 0x48);

    //  start first conversion (ADST<-1)
    bit_op(AD.ADCR_2,  0x10, 0x10);
}

ushort  SEN_get (uchar numB2)
{
    return  AD.ADDR[16 + (numB2 & 0x03)];
}

void  SEN_gets (ushort *sen0W, ushort *sen1W, 
                ushort *sen2W, ushort *sen3W )
{
    //  read data
    *sen0W = AD.ADDR[16];
    *sen1W = AD.ADDR[17];
    *sen2W = AD.ADDR[18];
    *sen3W = AD.ADDR[19];
}

////////////////////////////////////////////////////////////////////////
//
//  LIO: logic I/O on CN7=LIO (PB5-2)
//      void  LIO_init (uchar dirB4);
//          init PB[5-2] as logic I/O, and 
//          set direction as valB4[3-0] (0/1 for in/out).
//      void  LIO_set (uchar valB4);
//          for output pins, output the bit-pattern.
//      ushort  LIO_get (void);
//          for input pins, read the bit-pattern at the pins.
//          (for output pins, output levels appear in the result.)

void  LIO_init (uchar dirB4)
{
    //  PB5-2 as logic I/O
    bit_op(PFC.PBCR1.WORD, 0x3c00, 0x0000);
    bit_op(PFC.PBCR2.WORD, 0x0ff0, 0x0000);

    //  direction of PB5-2 (0/1 for in/out)
    bit_op(PFC.PBIOR.WORD, 0x003c, (dirB4 & 0x0f) << 2);
}

void  LIO_set (uchar valB4)
{
    PB.DR.WORD = (valB4 & 0x0f) << 2;
}

ushort  LIO_get (void)
{
    return PB.DR.WORD >> 2;
}

////////////////////////////////////////////////////////////////////////
//
//  LIN: logic input of CN8=ALINX (PF11-8) and CN9=ALINY (PF15-12)
//      void  LIN_init (void);
//          init CN8/9 as logic input.
//      ushort  LIN_get (void);
//          read the 8-bit-pattern of the pins.

void  LIN_init (void)
{
    //  nothing to do with LIN (PF8-15)
}

ushort  LIN_get (void)
{
    return PF.DR.BYTE.H;
}

////////////////////////////////////////////////////////////////////////
//
//  AIX: analog input of CN8(ALINX) = AN11-8
//      void  AIX_init (void);
//          init AD0 for AN[11-8] (ALINX4-0), 
//          and start the first A/D conversion.
//      void  AIX_gets (ushort *val0W, ushort *val1W, 
//                      ushort *val2W, ushort *val3W );
//          read analog value from AN[11-8] (ALINX4-0), 
//          and start the next A/D conversion.
//              where val?W=0x0000 for 0V (GND level),
//                          0xffff for 5V (Vcc level).

void  AIX_init (void)
{
    //  wake up AD2 from stand-by mode
    bit_op(MSTCR.MSTCR2.WORD, 0x0010, 0x0000);

    //  stop conversion (while setup)
    bit_op(AD.ADCR_0, 0x10, 0x00);

    //  setup control registers
    //      no-int, 4ch-scan, AN11-8
    //      P/8 (266/256T=24KHz), 1cycle-scan
    bit_op(AD.ADCSR_0, 0x77, 0x17);
    bit_op(AD.ADCR_0,  0xf8, 0x40);

    //  start first conversion
    bit_op(AD.ADCR_0,  0x10, 0x10);
}

void  AIX_gets (ushort *val0W, ushort *val1W, 
                ushort *val2W, ushort *val3W )
{
    //  wait for ADF (end flag)
    while ((AD.ADCSR_0 & 0x80) == 0);
    bit_op(AD.ADCSR_0, 0x80, 0x00);             //  clear ADF (end flag)

    //  read data from AN[11-8]
    *val0W = AD.ADDR[8];
    *val1W = AD.ADDR[9];
    *val2W = AD.ADDR[10];
    *val3W = AD.ADDR[11];

    //  start next conversion
    bit_op(AD.ADCR_0,  0x10, 0x10);
}

////////////////////////////////////////////////////////////////////////
//
//  AIY: analog input of CN9(ALINY) = AN15-12
//      void  AIY_init (void);
//          init AD1 for AN[15-12] (ALINY4-0), 
//          and start the first A/D conversion.
//      void  AIY_gets (ushort *val0W, ushort *val1W, 
//                      ushort *val2W, ushort *val3W );
//          read analog value from AN[11-8] (ALINX4-0), 
//          and start the next A/D conversion.
//              where val?W=0x0000 for 0V (GND level),
//                          0xffff for 5V (Vcc level).

void  AIY_init (void)
{
    //  wake up AD1 from stand-by mode
    bit_op(MSTCR.MSTCR2.WORD, 0x0020, 0x0000);

    //  stop conversion (while setup)
    bit_op(AD.ADCR_1, 0x10, 0x00);

    //  setup control registers
    //      no-int, 4ch-scan, AN15-12
    //      P/8 (266/256T=24KHz), 1cycle-scan
    bit_op(AD.ADCSR_1, 0x77, 0x17);
    bit_op(AD.ADCR_1,  0xf8, 0x40);

    //  start first conversion
    bit_op(AD.ADCR_1,  0x10, 0x10);
}

void  AIY_gets (ushort *val0W, ushort *val1W, 
                ushort *val2W, ushort *val3W )
{
    //  wait for ADF (end flag)
    while ((AD.ADCSR_1 & 0x80) == 0);
    bit_op(AD.ADCSR_1, 0x80, 0x00);             //  clear ADF (end flag)

    //  read data from AN[11-8]
    *val0W = AD.ADDR[12];
    *val1W = AD.ADDR[13];
    *val2W = AD.ADDR[14];
    *val3W = AD.ADDR[15];

    //  start next conversion
    bit_op(AD.ADCR_1,  0x10, 0x10);
}

////////////////////////////////////////////////////////////////////////
//
//  CMT: compare match timer
//      void  CMT0_init (uchar cselB2, ushort periodW, void (*action)());
//      void  CMT1_init (uchar cselB2, ushort periodW, void (*action)());
//          initialize CMT0 or CMT1, where
//              cselB2 : clock prescaler
//                       00 for C=P/8,    01 for C=P/32
//                       10 for C=P/128,  11 for C=P/512
//              periodW: period (C /(period+1) Hz)
//              action : a function called by CMT0/1 interrupt
//                       ex. void  f_tick (void) {
//                               do_something();
//                           }
//          ex. CMT0_init(0x03, 47999, f_tick);
//              C=P/512, C/48000 -> 1Hz
//      void  CMT0_quit (void);
//      void  CMT1_quit (void);
//          disable CMT interruption

void  CMT0_interrupt (void);
void  CMT1_interrupt (void);

void   (*CMT0_action)();                        //  user's action
void   (*CMT1_action)();                        //  user's action

void  CMT0_init (uchar cselB2, ushort periodW, void (*action)())
{
    ushort  start_bit, vec_num;

    //  wakeu up CMT from stand-by mode
    bit_op(MSTCR.MSTCR2.WORD, 0x1000, 0x0000);

    //  stop counting
    bit_op(CMT.CMSTR.WORD,  0x0001, 0x0000);

    //  set period
    bit_op(CMT0.CMCSR.WORD, 0x0003, cselB2);
    CMT0.CMCOR = periodW;

    //  setup interrupt CMI0
    definterrupt(144, (void *) CMT0_interrupt);
    CMT0_action = action;
    bit_op(CMT0.CMCSR.WORD, 0x0040, 0x0040);
    bit_op(INTC.IPRG.WORD,  0x00f0, 0x0070);

    //  start counting
    bit_op(CMT.CMSTR.WORD,  0x0001, 0x0001);
}

void  CMT1_init (uchar cselB2, ushort periodW, void (*action)())
{
    ushort  start_bit, vec_num;

    //  wake up CMT from stand-by mode
    bit_op(MSTCR.MSTCR2.WORD, 0x1000, 0x0000);

    //  stop counting
    bit_op(CMT.CMSTR.WORD,  0x0002, 0x0000);

    //  set period
    bit_op(CMT1.CMCSR.WORD, 0x0003, cselB2);
    CMT1.CMCOR = periodW;

    //  setup interrupt CMI1
    definterrupt(148, (void *) CMT1_interrupt);
    CMT1_action = action;
    bit_op(CMT1.CMCSR.WORD, 0x0040, 0x0040);
    bit_op(INTC.IPRG.WORD,  0x000f, 0x0007);

    //  start counting
    bit_op(CMT.CMSTR.WORD,  0x0002, 0x0002);
}

#pragma interrupt
void  CMT0_interrupt (void)
{
    //  clear interrupt flag for CMT0
    bit_op(CMT0.CMCSR.WORD, 0x0080, 0x0000);

    //  call the action function
    (*CMT0_action)();
}

#pragma interrupt
void  CMT1_interrupt (void)
{
    //  clear interrupt flag for CMT0
    bit_op(CMT1.CMCSR.WORD, 0x0080, 0x0000);

    //  call the action function
    (*CMT1_action)();
}

void  CMT0_quit (void)
{
    //  stop counting
    bit_op(CMT.CMSTR.WORD,  0x0001, 0x0000);
}

void  CMT1_quit (void)
{
    //  stop counting
    bit_op(CMT.CMSTR.WORD,  0x0002, 0x0000);
}

////////////////////////////////////////////////////////////////////////
//
//  SCI: serial port (RS485)
//      void  SCI_init (uchar addr, uchar paclen, void *interpreter());
//          initialize SCI3 (PA8:RxD, PA9:TxD) as 38400bps 1-7-M-1
//          *interpreter is a pointer to packet interpreting function
//              ex. void  packet_interpreter (uchar *packet);
//          where packet is an array of paclen uchars.
//                addr is bit[6:2] (5bits) of packet[0]
//      void  SCI_transmit (uchar *packet);
//          transmit the packet, where packet is an array of paclen
//          uchars.
//
//  Serial data format:
//      packet[0]       packet[1]  ...  packet[paclen-2]  packet[paclen-1]
//        bit[7]=1        bit[7]=0        bit[7]=0          bit[7]=0   
//        bit[6:2]=Addr   bit[6:0]=data   bit[6:0]=data     bit[6:0]=data
//        bit[1:0]=data

#define  ADDR_ALL  0x1f
uchar  Addr;                                    //  my address (5bits)

uchar  Rnum,                                    //  num bytes received
       Tnum;                                    //  num bytes transmitted

#define  PACLEN_MAX  8                          //  max packet length
uchar  Rdata[PACLEN_MAX],                       //  packet received 
       Tdata[PACLEN_MAX];                       //  packet to transmit
uchar  Paclen;

void   SCI_int_ERI (void);
void   SCI_int_RXI (void);
void   SCI_int_TXI (void);
void   SCI_int_TEI (void);

void   (*SCI_interpreter)();                    //  receiver function

void  SCI_init (uchar addr, uchar paclen, void *interpreter())
{
    int  i;

    //  assign my address (7bits)
    Addr = addr;

    //  store Paclen (packet length)
    Paclen = paclen;

    //  clear Rnum/Tnum
    Rnum = 0;                                   //  no data received
    Tnum = 0;                                   //  no data transmitted

    //  wake up SCI3 (from stand-by mode)
    bit_op(MSTCR.MSTCR1.WORD, 0x0008, 0x0000);

    //  disable SCI3 while setup
    bit_op(SCI3.SCR.BYTE,  0xfc, 0x00);         //  TE/RE/... are all off

    //  select clock source
    bit_op(SCI3.SCR.BYTE,  0x03, 0x00);         //  use internal clock

    //  setup data format (1-7-M-1)
    bit_op(SCI3.SMR.BYTE,  0xff, 0x44);         //  async, 1-7-M-1, P/1
    bit_op(SCI3.SDCR.BYTE, 0x08, 0x00);         //  LSB-first (as usual)

    //  set baud 38400 (n:N=0:19)
    //      P=24576000, B=P/(32*(N+1)) -> B=38400, N=19
    SCI3.BRR = 19;

    //  wait for the 1bit period
    //      49152000/38400 = 1280T
    //          fastest code:  Lable  DT  Rn      (1)
    //                                BT  Lable   (3)
    //      1280/4 = 320
    for (i = 0; i < 320; i++);

    //  setup interrupt hander
    //      ERI3 172 (error = ORER|FER|PER)
    //      RXI3 173 (received data = RDRF)
    //      TXI3 174 (transmitter ready = TDRE)
    //      TEI3 175 (transmission completed)
    //      SCI_interpreter (packet interpreter)
    definterrupt(172, (void *) SCI_int_ERI);
    definterrupt(173, (void *) SCI_int_RXI);
    definterrupt(174, (void *) SCI_int_TXI);
    definterrupt(175, (void *) SCI_int_TEI);
    SCI_interpreter = (void *) interpreter;

    //  enable interrupt (TIE/RIE) and multi-processor mode
    bit_op(INTC.IPRI.WORD,  0x0f00, 0x0700);
    bit_op(SCI3.SCR.BYTE, 0xcc, 0xc8);

    //  pin re-assignment
    //      PA08: RxD
    //      PA09: TxD
    //      PA10: TE (transmitter enable for the RS485 driver)
    bit_op(PFC.PACRL1.WORD, 0x003f, 0x0005);
    bit_op(PFC.PACRL3.WORD, 0x0700, 0x0300);
    bit_op(PFC.PAIORL.WORD, 0x0400, 0x0400);    //  PA10 out for TE
    bit_op(PA.DR.WORD,      0x0400, 0x0000);    //  driver disable

    //  enable transmitter and receiver
    bit_op(SCI3.SCR.BYTE, 0x30, 0x30);
}

#pragma interrupt
void  SCI_int_ERI (void)
{
    //  clear error flags (RDRF, ORER, FER, PER)
    bit_op(SCI3.SSR.BYTE, 0x78, 0x00);

    //  on error, ignore so-far-received data
    //  sleep until next address-mark
    Rnum = 0;
    bit_op(SCI3.SCR.BYTE, 0x08, 0x08);
}

#pragma interrupt
void  SCI_int_RXI (void)
{
    uchar  data, addr5;

    //  read data from RDR and clear RDRF
    data = SCI3.RDR;
    bit_op(SCI3.SSR.BYTE, 0x40, 0x00);

    //  address or body
    if (SCI3.SSR.BYTE & 0x02) {
        //  the data has address-mark (MPB==1)

        //  check the address
        addr5 = data >> 2;
        if (addr5 == Addr || addr5 == ADDR_ALL)  {
            //  HIT (to me or boardcast)
            //  entering listening mode
            Rdata[0] = data | 0x80;
            Rnum = 1;
        }
        else {
            //  IGNORE (not to me)
            //  sleep until next address-mark (MPIE<-1)
            Rnum = 0;
            bit_op(SCI3.SCR.BYTE, 0x08, 0x08);
        }
    }
    else {
        //  the data is packet-body (MPB==0)

        //  store the packet-body
        Rdata[Rnum++] = data;

        //  check packet length
        if (Rnum == Paclen) {
            //  reached packet end

            //  interpret the packet
            (*SCI_interpreter)(Rdata);
            //  sleep until next address-mark (MPIE<-1)
            Rnum = 0;
            bit_op(SCI3.SCR.BYTE, 0x08, 0x08);
        }
    }
}

#pragma interrupt
void  SCI_int_TXI (void)
{
    //  if not in sending, then return.
    if (Tnum == 0) return;

    //  send one byte and clear TDRE and MPBT
    SCI3.TDR = Tdata[Tnum];
    bit_op(SCI3.SSR.BYTE, 0x81, 0x00);

    //  update Tnum
    Tnum++;

    //  if the final byte is being sent, clear Tnum 
    //  and disable TXI and enable TEI
    if (Tnum == Paclen) {
        Tnum = 0;
        bit_op(SCI3.SCR.BYTE, 0x84, 0x04);
    }
}

#pragma interrupt
void  SCI_int_TEI (void)
{
    //  disable TEI
    bit_op(SCI3.SCR.BYTE, 0x04, 0x00);

    //  disable the RS485 driver (TE10<-0)
    bit_op(PA.DR.WORD, 0x0400, 0x0000);         //  driver disable
}

void  SCI_transmit (uchar *packet)
{
    int  i;

    //  wait for Tnum==0
    while (Tnum);

    //  double check TDRE==1
    while ((SCI3.SSR.BYTE & 0x80) == 0);

    //  store tdata to Tdata
    for (i = 0; i < Paclen; i++)
        Tdata[i] = packet[i] & 0x7f;

    //  enable TXI
    bit_op(SCI3.SCR.BYTE, 0x80, 0x80);

    //  enable the RS485 driver (TE10<-1)
    bit_op(PA.DR.WORD, 0x0400, 0x0400);         //  driver enable

    //  send the first byte with MPB
    SCI3.TDR = Tdata[0];
    bit_op(SCI3.SSR.BYTE, 0x81, 0x01);
    Tnum = 1;
}

void  SCI_quit (void)
{
    //  disable SCI3
    bit_op(SCI3.SCR.BYTE,  0xfc, 0x00);         //  TE/RE/... are all off

    //  sleep down SCI3 (into stand-by mode)
    bit_op(MSTCR.MSTCR1.WORD, 0x0008, 0x0008);
}

////////////////////////////////////////////////////////////////////////
//
//  WDT: watch dog timer (WDT)
//      void  WDT_init (uchar cselB3);
//          initializes WDT (watch dog timer) that could reset the CPU
//          when the counter TCNT overflows.  Not to get the CPU reset,
//          the user program has to call WDT_clear() before TCNT exceeds
//          0xff.  cselB3 selects the clock for counting up TCNT --- see
//          the table below for details.
//      void  WDT_reset ();
//          clears TCNT of WDT (watch dog timer).  The user program has
//          to call WDT_clear before TCNT overflows.
//      int   WDT_check ();
//          checks if the system has just rebooted from WDT reset.  If
//          this is the case, WOVF of WDT.RSTCSR has been set after
//          reboot, so WDT_check clears WOVF and returns non-zero value;
//          otherwise, it returns zero.
//
//  Time to overflow for pico2 (clock 49.152MHz):
//      csel = 0 :  0.0104msec (96000.0Hz)
//             1 :  0.3333msec (3000.00Hz)
//             2 :  0.6667msec (1500.00Hz)
//             3 :  1.3333msec (750.000Hz)
//             4 :  2.6667msec (375.000Hz)
//             5 :  5.3333msec (187.500Hz)
//             6 : 21.3333msec (46.8750Hz)
//             7 : 42.6667msec (23.4375Hz)

void  WDT_init (uchar cselB3)
{
    ushort  tcsr;

    tcsr = 0xa558 | (cselB3 & 0x07);

    //  set WT=1,TME=0,CKS2-0 to TCSR
    WDT.TCSR.W.WORD = tcsr;

    //  set RSTE=1, RSTS=0 to RSTCSR
    WDT.RSTCSR.W.WORD = 0x5a5f;

    //  set TME=1 (start counting)
    WDT.TCSR.W.WORD = tcsr | 0x20;
}

void  WDT_reset ()
{
    //  clear TCNT
    WDT.TCNT.W.WORD = 0x5a00;
}

int  WDT_check ()
{
    int  wovf;

    //  check WOVF
    wovf = WDT.RSTCSR.R.BYTE & 0x80;

    //  clear WOVF
    if (wovf)
        WDT.RSTCSR.W.WORD = 0xa500;

    //  return wovf
    return wovf;
}

////////////////////////////////////////////////////////////////////////