/**HEADER********************************************************************
* 
* Copyright (c) 2010 Avnet Japan Co.,Ltd (NIKE Borad Project.)
* All Rights Reserved
*
*************************************************************************** 
*
* $FileName: sensor_adc_mcf522xx.c$
* $Version : 0.1.0.0$
* $Date    : Jan-29-2010$
*
* Comments:
*
*   This file contains the Sensor (ADC) driver CPU specific functions
*
*END************************************************************************/

#include <mqx.h>
#include <bsp.h>
#include <fio.h>
#include <fio_prv.h>
#include "io.h"
#include "../sensor.h"
#include "../sensor_prv.h"
#include "../sensor_conf.h"


/* ADC settings (../sensor.c) */
extern volatile SENSOR_ADC_PTR         pSensor_adc;
extern volatile SENSOR_ADC_CHANNEL_PTR pSensor_adc_ch[];

/* Peripherals pointer ( mcf522xx_adc.h) */
static VMCF522XX_ADC_STRUCT_PTR  mcf522xx_adc_ptr;
static VMCF522XX_PIT_STRUCT_PTR  mcf522xx_timer_ptr;

/* State values for timer */
static struct adc_t_struct {
    uint_32   time_to_next_adc; /* stores time needed to next AD conversion */
    uint_32   time_to_next_irq; /* stores time needed to next timer IRQ */
    uint_32   dispatch_time;    /* stores absolute time for dispatcher */
    boolean   dispatch_next;    /* informs if any data within channels_to_next_adc is valid */
} adc_t = {
    0,
    0,
    0,
    FALSE
};

/* State values for AD converter */
static struct adc_c_struct {
    SENSOR_ADC_HW_CHANNEL_MASK mask;   
} adc_c = {
    0
};

/* static variables storing information for deinitialization */
static void (_CODE_PTR_ old_isr_adc1)(pointer);
static void (_CODE_PTR_ old_isr_timer)(pointer);
static pointer          old_isr_adc1_param;
static pointer          old_isr_timer_param;


/* Declarations of IRQ handlers */
static void adc_setup_timer(uint_32 time);
static void adc_cpu_isr(pointer parameter);
static void adc_timer_cpu_isr(pointer parameter);
static uchar io_type_conv_bit( uchar type);

#define PORT_SENSOR_ACCELERATION_SELFT  __BIT(3)
#define PORT_SENSOR_ACCELERATION_0G_DET  __BIT(2)
#define PORT_SENSOR_ACCELERATION_GSEL  __BIT(1)

/* From i (timer constant), we get both prescaler and preset values
** time [usec] = 2^i / BSP_SYSTEM_CLOCK / 10^-6
** time [usec] = (1 << i) * 10^6 / BSP_SYSTEM_CLOCK
** time [usec] = (10^6 << i) / BSP_SYSTEM_CLOCK
** time [usec] = (0xF4240 << i) / BSP_SYSTEM_CLOCK
** time [usec] = (0x3D09 << (6 + i)) / BSP_SYSTEM_CLOCK
** time [usec] = ((0x3D09 << (6 + i + 12 + 32)) / BSP_SYSTEM_CLOCK) >> (12 + 32)
** time [usec] = ((0x3D09 << (6 + 12 + 32)) / BSP_SYSTEM_CLOCK) >> (12 + 32 - i)
*/
#define TIMER_CONST (uint_64)(0xF424000000000000 / (BSP_SYSTEM_CLOCK / 2))


/*FUNCTION*****************************************************************
* 
* Function Name    : _sensor_adc_hw_init
* Returned Value   : _mqx_int
* Comments         :
*    Performs CPU specific initialization for ADC driver
*
*END*********************************************************************/

_mqx_int _sensor_adc_hw_init( SENSOR_ADC_PTR pSensorAdc)
{ /* Body */

#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    VMCF5225_STRUCT_PTR mcf522xx_ptr;
//#elif MQX_CPU == 5223
//    VMCF5223_STRUCT_PTR mcf522xx_ptr;
//#elif MQX_CPU == 5222
//    VMCF5222_STRUCT_PTR mcf522xx_ptr;
#else
    #error This CPU is not supported here
#endif
    
    _mqx_uint        div_register;
    _mqx_int         i;
    uint_32          frq_system_kHz;
    uint_16          prescaler, preset;
    uint_32          result;

    /* Prepare pointers to peripherals */
    mcf522xx_ptr = _PSP_GET_IPSBAR();
    mcf522xx_timer_ptr = &mcf522xx_ptr->PIT[BSP_ADC_TIMER];
    mcf522xx_adc_ptr = _bsp_get_sensor_adc_base_address();

    if (pSensor_adc->g.resolution != SENSOR_ADC_RESOLUTION_12BIT){
        return IO_ERROR; /* cannot set ADC resolution */
    }

    /* Prepare ADC clocking to be as fast as possible */
    div_register = (BSP_SYSTEM_CLOCK - 1) / 20000000;
    if (div_register > 31){
        return IO_ERROR; /* cannot set ADC frequency */
    }

    /* store information about exact ADC clock frequency */
    pSensor_adc->g.adc_frq = (BSP_SYSTEM_CLOCK >> 2) / (div_register + 1);

    /* Prepare AD converter, but do not start it */
    mcf522xx_adc_ptr->CTRL2 = 0x0800 | div_register; /* Channel B can run in parallel with channel A */
    mcf522xx_adc_ptr->CTRL1 = 0x0800; /* Once mode, single conversion, all inputs single-ended */
    mcf522xx_adc_ptr->POWER &= 0xfff8; /* Power up ADC */

/* Install ISR for ADC */
#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    old_isr_adc1_param = _int_get_isr_data(BSP_ADC_INTERRUPT_VECTOR);
    old_isr_adc1 = _int_install_isr(BSP_ADC_INTERRUPT_VECTOR, adc_cpu_isr, NULL);
//#elif MQX_CPU == 5223
//    old_isr_adc1_param = _int_get_isr_data(MCF5223_INT_ADC_ADCA);
//    old_isr_adc1 = _int_install_isr(MCF5223_INT_ADC_ADCA, adc_cpu_isr, NULL);
//#elif MQX_CPU == 5222
//    old_isr_adc1_param = _int_get_isr_data(MCF5222_INT_ADC_ADCA);
//    old_isr_adc1 = _int_install_isr(MCF5222_INT_ADC_ADCA, adc_cpu_isr, NULL);
#else
    #error This CPU is not supported here
#endif

    mcf522xx_timer_ptr->PCSR = 0x0038;
    if (NULL == old_isr_adc1){
        return IO_ERROR; /* could not install new interrupt routine */  
    }
   
    old_isr_timer_param = _int_get_isr_data(BSP_ADC_TIMER_INTERRUPT_VECTOR);
    old_isr_timer = _int_install_isr(BSP_ADC_TIMER_INTERRUPT_VECTOR, adc_timer_cpu_isr, NULL);
    if (NULL == old_isr_timer) {
        _int_install_isr(BSP_ADC_INTERRUPT_VECTOR, old_isr_adc1, old_isr_adc1_param);
        return IO_ERROR; /* could not install new interrupt routine */
    }

    /* We have to timer scales:
    ** the first one is generic driver time (32 bit)
    ** the second one is low level driver time (31 bit) and presents directly preset + prescaler settings
    ** and the relation between them is:
    ** Eq.1: generic_time = lowlevel_time << shift
    **
    ** From above, following implies:
    ** 1) lowlevel_time (31 bit) cannot fit all the scale range of generic_time
    ** (and to solve that, generic_time wait will be achieved by sequential lowlevel_time periods
    ** 2) generic_time has the smallest resolution of "shift" bits. Shift should be found to be
    ** less or equal to 1usec.
    ** 3) all generic_time(s) from init structures must be recomputed to match Eq.1
    **/
    result = (uint_32)(TIMER_CONST >> (12 + 32 - 31));
    for (i = 16 + 15; i > 0; i--) { /* preset_value = 0xFFFF, preset = 0x10000, prescaler = 1 << n, i = n + (log2 (preset)) */
        if (result == 0) { /* we reached the lowest resolution (lower than 1 usec) */
            /* store the exponent of 2 between 2 timer scales */
            pSensor_adc->shift = i;
            break;
        }
        result = result >> 1;
    }


#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    /* Setup ISR levels for timer */
    _mcf5225_int_init(BSP_ADC_TIMER_INTERRUPT_VECTOR, BSP_ADC_TIMER_INTERRUPT_LEVEL, BSP_ADC_TIMER_INTERRUPT_SUBLEVEL, TRUE);
    /* Setup ISR levels for ADC */
    _mcf5225_int_init(BSP_ADC_INTERRUPT_VECTOR, BSP_ADC_INTERRUPT_LEVEL, BSP_ADC_INTERRUPT_SUBLEVEL, TRUE);
//#elif MQX_CPU == 5223
//    /* Setup ISR levels for timer */
//    _mcf5223_int_init(BSP_ADC_TIMER_INTERRUPT_VECTOR, BSP_ADC_TIMER_INTERRUPT_LEVEL, BSP_ADC_TIMER_INTERRUPT_SUBLEVEL, TRUE);
//    /* Setup ISR levels for ADC */
//    _mcf5223_int_init(BSP_ADC_INTERRUPT_VECTOR, BSP_ADC_INTERRUPT_LEVEL, BSP_ADC_INTERRUPT_SUBLEVEL, TRUE);
//#elif MQX_CPU == 5222
//    /* Setup ISR levels for timer */
//    _mcf5222_int_init(BSP_ADC_TIMER_INTERRUPT_VECTOR, BSP_ADC_TIMER_INTERRUPT_LEVEL, BSP_ADC_TIMER_INTERRUPT_SUBLEVEL, TRUE);
//    /* Setup ISR levels for ADC */
//    _mcf5222_int_init(BSP_ADC_INTERRUPT_VECTOR, BSP_ADC_INTERRUPT_LEVEL, BSP_ADC_INTERRUPT_SUBLEVEL, TRUE);
#else
        #error This CPU is not supported here
#endif
    

    return IO_OK;
} /* Endbody */

/*FUNCTION*****************************************************************
* 
* Function Name    : _sensor_port_hw_init
* Returned Value   : _mqx_int
* Comments         :
*    Performs CPU specific deinitialization for ADC driver
*
*END*********************************************************************/
_mqx_int _sensor_port_hw_init(SENSOR_ADC_PTR pSensorAdc)
{

#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    VMCF5225_GPIO_STRUCT_PTR    gpio_ptr;
#else
    #error This CPU is not supported here
#endif

    gpio_ptr = _bsp_get_gpio_base_address();

    /* GPIO Function */
    gpio_ptr->PUBPAR &= 0x0003;

    /* Out/In Put SET */
    GPIO_P_CLR(UB, PORT_SENSOR_ACCELERATION_SELFT);
    GPIO_DDR_OUT(UB, PORT_SENSOR_ACCELERATION_SELFT);

    GPIO_P_CLR(UB, PORT_SENSOR_ACCELERATION_GSEL);
    GPIO_DDR_OUT(UB, PORT_SENSOR_ACCELERATION_GSEL);

    GPIO_DDR_IN(UB, PORT_SENSOR_ACCELERATION_0G_DET);

    return IO_OK;
}



/*FUNCTION*****************************************************************
* 
* Function Name    : _sensor_adc_hw_deinit
* Returned Value   : _mqx_int
* Comments         :
*    Performs CPU specific deinitialization for ADC driver
*
*END*********************************************************************/

_mqx_int _sensor_adc_hw_deinit( void) {
    mcf522xx_timer_ptr->PCSR &= 0x007e; /* stop timer */
    if (old_isr_timer != NULL){
        _int_install_isr(BSP_ADC_TIMER_INTERRUPT_VECTOR, old_isr_timer, old_isr_timer_param);
    }
    return IO_OK;
}

/*FUNCTION*****************************************************************
* 
* Function Name    : _sensor_adc_hw_channel_init
* Returned Value   : int_32
* Comments         :
*    Returns result of HW channel initialization
*
*END*********************************************************************/

_mqx_int _sensor_adc_hw_channel_init
   (
    /* [IN] number of channel on which to perform hardware initialization */
    _mqx_uint         nr
   )
{ /* Body */
    _mqx_uint i, j = 0;

    /* we need to recompute timers (cross-multiplication):
    ** source (init value)                                                dest (working value)
    ** ---------------------------------------------------------------------------------------
    ** ((0x3D09 << (6 + 12 + 32)) / BSP_SYSTEM_CLOCK) >> (12 + 32 - i)    2^i
    ** ((0x3D09 << (6 + 12 + 32)) / BSP_SYSTEM_CLOCK) >> (12 + 32 - 31)   2^31
    ** input [usec]                                                       output
    **
    ** output = input * (TIMER_CONST >> (12 + 32 - 31)) * 2^31 = 
    */
    pSensor_adc_ch[nr]->g.period = (((uint_64) pSensor_adc_ch[nr]->g.period << 31) / (TIMER_CONST >> (12 + 32 - 31))) >> pSensor_adc->shift; /* note: we are losing bits for timer setting */;
    pSensor_adc_ch[nr]->g.offset = (((uint_64) pSensor_adc_ch[nr]->g.offset << 31) / (TIMER_CONST >> (12 + 32 - 31))) >> pSensor_adc->shift; /* note: we are losing bits for timer setting */;

    if (pSensor_adc_ch[nr]->g.source > SENSOR_ADC_SOURCE_AN6_AN7){
        return IO_ERROR; /* ADC_SOURCE_ constant does not match any source signal for this CPU */
    }

    for (i = 0; i < SENSOR_ADC_MAX_CHANNELS; i++)
        if ((i != nr) && (pSensor_adc_ch[i] != NULL)) {
            if (pSensor_adc_ch[i]->g.source == pSensor_adc_ch[nr]->g.source){
                return IO_ERROR; /* channel already used */
            }

            switch (pSensor_adc_ch[i]->g.source) {
                case SENSOR_ADC_SOURCE_AN0:
                    if (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN0_AN1){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN1:
                    if (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN0_AN1){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN2:
                    if (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN2_AN3){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN3:
                    if (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN2_AN3){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN4:
                    if (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN4_AN5){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN5:
                    if (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN4_AN5){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN6:
                    if (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN6_AN7){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN7:
                    if (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN6_AN7){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN0_AN1:
                    if ((pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN0) || (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN1)){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN2_AN3:
                    if ((pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN2) || (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN3)){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN4_AN5:
                    if ((pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN4) || (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN5)){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                case SENSOR_ADC_SOURCE_AN6_AN7:
                    if ((pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN6) || (pSensor_adc_ch[nr]->g.source == SENSOR_ADC_SOURCE_AN7)){
                        return IO_ERROR; /* channel already used */
                    }
                    break;
                default:
                    break;
            }
            j++;
        }
     
     /* @Pending */
    /* configure GPIO and MUX for ADC */
    return _bsp_sensor_adc_io_init(pSensor_adc_ch[nr]->g.source);
} /* Endbody */

/*FUNCTION*****************************************************************
* 
* Function Name    : _sensor_adc_hw_channel_deinit
* Returned Value   : _mqx_int
* Comments         :
*    Performs CPU specific deinitialization for ADC channel
*
*END*********************************************************************/

_mqx_int _sensor_adc_hw_channel_deinit(SENSOR_ADC_CHANNEL_PTR pChannel) {
    return IO_OK;
}

/*FUNCTION*****************************************************************
* 
* Function Name    : _sesnor_adc_hw_set_wait
* Returned Value   : void
* Comments         : sets the timer to elapse (to call dispatcher) in 'time'
*
*END**********************************************************************/

void _sensor_adc_hw_set_wait(uint_32 time) {
    /* dispatcher inform us to wait time since now */
    adc_t.time_to_next_adc = time;
    adc_setup_timer(time);
}

/*FUNCTION*****************************************************************
* 
* Function Name    : _sensor_adc_hw_get_time
* Returned Value   : uint_32 - actual absolute time
* Comments         : 
*
*END**********************************************************************/

uint_32 _sensor_adc_hw_get_time( void) {
    uint_32 elapsed;

    elapsed = adc_t.time_to_next_irq * (mcf522xx_timer_ptr->PMR - mcf522xx_timer_ptr->PCNTR) / (mcf522xx_timer_ptr->PMR + 1);
    return adc_t.dispatch_time + elapsed;
}

/*FUNCTION*****************************************************************
* 
* Function Name    : adc_setup_timer
* Returned Value   : void
* Comments         : sets HW timer to be able to measure time effectivelly
*
*END**********************************************************************/

static void adc_setup_timer(uint_32 time) {
    uint_32  j;
    _mqx_int i;
    uint_16  preset, prescaler;

    /* search for next IRQ time */
    j = (time << pSensor_adc->shift) & 0x7FFFFFFF;
    if (time != (j >> pSensor_adc->shift)) { /* only 31 bits for timer setting */
        /* Till now, we need to have all AD conversions done, because we are going to modify ADC
        ** interrupt internal structures.
        */
        preset = 0xFFFF;  /* use maximum time for preset */
        prescaler = 15 << 8; /* use maximum prescaler  */
        adc_t.time_to_next_irq = 0x80000000 >> pSensor_adc->shift;
        adc_t.dispatch_next = FALSE;
    }
    else {
        /* compute time needed for next ADC: searching for first 1 */
        for (i = 0; i < 15; i++, j <<= 1) {
            if (j & 0x40000000) {
                preset = 0xFFFF; /* set maximum preset */
                prescaler = (14 - i) << 8; /* set prescaler */
                adc_t.time_to_next_irq = 0x40000000 >> (i + pSensor_adc->shift);
                if (j == 0x40000000)
                    adc_t.dispatch_next = TRUE;
                else
                    adc_t.dispatch_next = FALSE;
                goto Set_Timer;
            }
        }
        preset = j >> i; /* set preset */
        if (preset){
            preset--;
        }
        prescaler = 0 << 8; /* set prescaler */
        adc_t.time_to_next_irq = j >> (15 + pSensor_adc->shift);
        adc_t.dispatch_next = TRUE;
    }

Set_Timer:
    /* setup timer constants that correspond to time_to_next_irq */
    mcf522xx_timer_ptr->PCSR &= 0x007e; /* stop timer */
    mcf522xx_timer_ptr->PCNTR = mcf522xx_timer_ptr->PMR = preset;
    mcf522xx_timer_ptr->PCSR |= prescaler | 0x0021; /* starts timer */
}

/*FUNCTION*****************************************************************
* 
* Function Name    : _sensor_adc_hw_run
* Returned Value   : void
* Comments         : starts timer
*
*END*********************************************************************/

_mqx_int _sensor_adc_hw_run( void) {
    return 0;
}

/*FUNCTION*****************************************************************
* 
* Function Name    : _adc_hw_stop
* Returned Value   : void
* Comments         : stops timer
*
*END*********************************************************************/

_mqx_int _sensor_adc_hw_stop( void) {
    return adc_t.dispatch_time = 0;
}

/*FUNCTION*****************************************************************
* 
* Function Name    : adc_timer_cpu_isr
* Returned Value   : void
* Comments         : PIT (= timer) interrupt for ADC driver planning
*
*END*********************************************************************/

static void adc_timer_cpu_isr
    (
        /* [IN] the address of the device specific information */
        pointer parameter
    )
{
    _mqx_int i, j;
    uint_16  prescaler;
    uint_16  preset;
    boolean  performed_adc;

    adc_t.time_to_next_adc -= adc_t.time_to_next_irq; /* update remaining time to ADC */
    adc_t.dispatch_time += adc_t.time_to_next_irq; /* update absolute time */
    
    if (adc_t.dispatch_next) {
        mcf522xx_timer_ptr->PCSR &= 0xfffe; /* stop timer */
//        _sensor_adc_scheduler(adc_t.dispatch_time);
        _sensor_intterrupt_adc_scheduler(adc_t.dispatch_time);
    }
    else{
        adc_setup_timer(adc_t.time_to_next_adc);
    }
}


/*FUNCTION****************************************************************
* 
* Function Name    : _sensor_adc_hw_measure
* Returned Value   : void
* Comments         : Prepares and runs ADC to measure data from channel
*
*END*********************************************************************/

void _sensor_adc_hw_measure(SENSOR_ADC_HW_CHANNEL_MASK mask) 
{
    _mqx_int i, j;

    mcf522xx_adc_ptr->ADSDIS = 0x00ff; /* presume all disabled */
    mcf522xx_adc_ptr->ADLST1 = mcf522xx_adc_ptr->ADLST2 = 0;
    for (i = 0, j = 0; i < SENSOR_ADC_SOURCE_MAX + 1; i++) {
        if ((1 << i) & mask) {
            if (j < 4){
                mcf522xx_adc_ptr->ADLST1 |= i << (4 * j++);
            } else{
                mcf522xx_adc_ptr->ADLST2 |= i << (4 * (j++ - 4));
            }
            
            mcf522xx_adc_ptr->ADSDIS = mcf522xx_adc_ptr->ADSDIS << 1;
        }
    }
    mcf522xx_adc_ptr->CTRL1 |= 0x2000; /* start AD conversion */ 
    adc_c.mask = mask;
}

/*FUNCTION*****************************************************************
* 
* Function Name    : adc_cpu_isr
* Returned Value   : void
* Comments         : IRQ handler for completed AD conversion(s)
*    In adc_c.channels_to_next_adc we have list of channels that
*    have been sampled and whole we need to handle the results.
*    We will copy results from AD converter to channels buffers.
*    Since AD converter can hold 8 samples a time, we check if any
*    other channel needs to be sampled and if yes, prepare next AD
*    conversion.
*
*END*********************************************************************/

static void adc_cpu_isr
    (
        /* [IN] the address of the device specific information */
        pointer parameter
    )
{ /* Body */
    _mqx_int    i, j;
    uint_16_ptr results = (pointer) &mcf522xx_adc_ptr->ADRSLT[0]; /* prepare pointer to peripheral results */
    mcf522xx_adc_ptr->ADSTAT |= 0x1800; /* clear interrupt flags */
   
    /* perform copies from ADC */
    for (i = 0; ((mcf522xx_adc_ptr->ADSDIS & 0x0001) == 0x0000) && (i < 8); i++, mcf522xx_adc_ptr->ADSDIS >>= 1) {
        if (i < 4) {
            j = mcf522xx_adc_ptr->ADLST1 & 0x000f;
            mcf522xx_adc_ptr->ADLST1 >>= 4;
        }
        else {
            j = mcf522xx_adc_ptr->ADLST2 & 0x000f;
            mcf522xx_adc_ptr->ADLST2 >>= 4;
        }
        pSensor_adc->results[j] = results[i] >> 3;
    }
    _sensor_adc_conv_end(pSensor_adc->results, adc_c.mask);
} /* Endbody */

/*FUNCTION****************************************************************
* 
* Function Name    : _bsp_get_sensor_adc_base_address
* Returned Value   : pointer to base of ADC registers
* Comments         :
*    This function returns base address of ADC related register space.
*
*END*********************************************************************/

pointer _bsp_get_sensor_adc_base_address
(
    void
)
{

#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    return (pointer)(&((VMCF5225_STRUCT_PTR)_PSP_GET_IPSBAR())->ADC);
//#elif MQX_CPU == 5222
//    return (pointer)(&((VMCF5222_STRUCT_PTR)_PSP_GET_IPSBAR())->ADC);
//#elif MQX_CPU == 5223
//    return (pointer)(&((VMCF5223_STRUCT_PTR)_PSP_GET_IPSBAR())->ADC);
#else
    #error This CPU is not supported here
    return NULL;
#endif
}

_mqx_int _sensor_port_set( uchar type )
{

#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    VMCF5225_GPIO_STRUCT_PTR    gpio_ptr;
#else
    #error This CPU is not supported here
#endif
    uchar bit;
    _mqx_int result = IO_OK;

    gpio_ptr = _bsp_get_gpio_base_address();

    bit = io_type_conv_bit(type);

    if( (type & SENSOR_IO_TYPE_MASK) == SENSOR_IO_BLK_GROUP_UB){
      /* Out/In Put SET */
      GPIO_P_SET(UB, bit);
      GPIO_DDR_OUT(UB, bit);
    } else {
      result = IO_ERROR;
    }
    return result;
}

_mqx_int _sensor_port_clr( uchar type)
{

#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    VMCF5225_GPIO_STRUCT_PTR    gpio_ptr;
#else
    #error This CPU is not supported here
#endif
    uchar bit;
    _mqx_int result = IO_OK;

    gpio_ptr = _bsp_get_gpio_base_address();
    bit = io_type_conv_bit(type);

    if( (type & SENSOR_IO_TYPE_MASK) == SENSOR_IO_BLK_GROUP_UB){
      /* Out/In Put SET */
      GPIO_P_CLR(UB, bit);
      GPIO_DDR_OUT(UB, bit);
    } else {
      result = IO_ERROR;
    }
    return result;
}

_mqx_int _sensor_port_in( uchar type)
{

#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    VMCF5225_GPIO_STRUCT_PTR    gpio_ptr;
#else
    #error This CPU is not supported here
#endif
    uchar bit;
    _mqx_int result = IO_OK;

    gpio_ptr = _bsp_get_gpio_base_address();
    bit = io_type_conv_bit(type);

    if( (type & SENSOR_IO_TYPE_MASK) == SENSOR_IO_BLK_GROUP_UB){
      GPIO_DDR_IN(UB, bit);
    } else {
      result = IO_ERROR;
    }
    return result;
}

_mqx_int _sensor_port_get( uchar type, uchar_ptr pData)
{
#if PSP_MQX_CPU_IS_MCF5225X
//#if MQX_CPU == 5225
    VMCF5225_GPIO_STRUCT_PTR    gpio_ptr;
#else
    #error This CPU is not supported here
#endif
    _mqx_int result = IO_OK;

    gpio_ptr = _bsp_get_gpio_base_address();

    if( (type & SENSOR_IO_TYPE_MASK) == SENSOR_IO_BLK_GROUP_UB){
      *pData = gpio_ptr->PORTUB;
    } else {
      result = IO_ERROR;
    }
    return result;
}

static uchar io_type_conv_bit( uchar type)
{
  uchar bit = 0;

  switch(type){
  case SENSOR_IO_G_SEL:
    bit = PORT_SENSOR_ACCELERATION_GSEL;
    break;
  case SENSOR_IO_0G_DET:
    bit = PORT_SENSOR_ACCELERATION_0G_DET;
    break;

  case SENSOR_IO_SELTEST:
    bit = PORT_SENSOR_ACCELERATION_SELFT;
    break;

  default:
    break;
  }

  return bit;
}

/* EOF */
