General I2C refactoring and environment settings cleanup

These are the patches for WP252006.
There will be a follow-up patched for WP254081.

Environment settings

    * board_id bootargs parameter from board_id environment var
      set in board_late_init().
    * decreased bootdelay to 1 second
    * added check for keypress on bootdelay==0
    * removed preboot command and introduced "run cmdfile" instead
    * changed rootfstype from ext2 to ext3

    General I2C refactoring

    * include/configs/u8500.h:
    	Enabled i2c command line commands.
    	Changed environment settings to use board_id as set in
    	board_late_init().
    	Added I2C address and configuration defines.

    * board/st/u8500/Makefile: Replaced i2c.o with u8500_i2c.o.

    * board/st/u8500/gpio.c, gpio.h: Added I2C busses to altfunc_table.

    * board/st/u8500/init_mmc.c: removed config_extended_gpio() and therefore I2C
    	references. GPIOE settings are now in board_late_init().

    * board/st/u8500/u8500.c:
    	Removed I2C/SD gpio settings from addr,value array.
    	board_late_init():
              Determine and set board_id environment variable
              0: mop500, 1: href500
              Above boards have different GPIO expander chips which we can
              distinguish by the chip id (1 and 3).
              The board_id environment variable is needed for the Linux bootargs.

Signed-off-by: Michael Brandt <Michael.Brandt@stericsson.com>
Change-Id: I4c2cab28c1cb74692e0c72b2daa422e97787ff8d
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/167
Reviewed-by: Michael BRANDT <michael.brandt@stericsson.com>
Reviewed-by: Jonas ABERG <jonas.aberg@stericsson.com>
Tested-by: Jonas ABERG <jonas.aberg@stericsson.com>

7 files changed, 686 insertions, 3172 deletions

diff --git a/board/st/u8500/Makefile b/board/st/u8500/Makefile
index 632a49161..b5dfc7bdd 100755..100644
--- a/board/st/u8500/Makefile
+++ b/board/st/u8500/Makefile
@@ -26,7 +26,7 @@ include $(TOPDIR)/config.mk
 CFLAGS += -D__RELEASE -D__STN_8500
 LIB	= $(obj)lib$(BOARD).a
 
-COBJS	:= u8500.o flash.o gpio.o i2c.o mmc.o mmc_utils.o init_mmc.o emmc.o
+COBJS	:= u8500.o flash.o gpio.o u8500_i2c.o mmc.o mmc_utils.o init_mmc.o emmc.o
 SOBJS	:= core2.o
 
 SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
diff --git a/board/st/u8500/gpio.c b/board/st/u8500/gpio.c
index 4062d022b..52c585be6 100755..100644
--- a/board/st/u8500/gpio.c
+++ b/board/st/u8500/gpio.c
@@ -29,6 +29,14 @@ static	struct gpio_register *addr_gpio_register[GPIO_BLOCKS_COUNT];
 int sz_altfun_tbl;
 
 struct gpio_altfun_data altfun_table[] = {
+    {.altfun = GPIO_ALT_I2C_0,.start = 147,.end = 148,.cont = 0,.type =
+        GPIO_ALTF_A,},
+    {.altfun = GPIO_ALT_I2C_1,.start = 16,.end = 17,.cont = 0,.type =
+        GPIO_ALTF_B,},
+    {.altfun = GPIO_ALT_I2C_2,.start = 10,.end = 11,.cont = 0,.type =
+        GPIO_ALTF_B,},
+    {.altfun = GPIO_ALT_I2C_3,.start = 229,.end = 230,.cont = 0,.type =
+        GPIO_ALTF_C,},
     {.altfun = GPIO_ALT_UART_0_MODEM,.start = 0,.end = 3,.cont = 1,.type =
         GPIO_ALTF_A,},
     {.altfun = GPIO_ALT_UART_0_MODEM,.start = 33,.end = 36,.cont = 0,.type =
@@ -181,7 +189,7 @@ gpio_error gpio_altfunction(gpio_alt_function alt_func,
             }
             if (!error)
                 continue;
-            nmdk_error
+            printf
                 ("GPIO %d configuration failure (nmdk_error:%d)",
                  j, error);
             error = GPIO_INVALID_PARAMETER;
diff --git a/board/st/u8500/gpio.h b/board/st/u8500/gpio.h
index 37908ad37..5277fa4b4 100755..100644
--- a/board/st/u8500/gpio.h
+++ b/board/st/u8500/gpio.h
@@ -376,6 +376,8 @@ typedef enum {
 	GPIO_ALT_UART_2,
 	GPIO_ALT_I2C_0,
 	GPIO_ALT_I2C_1,
+	GPIO_ALT_I2C_2,
+	GPIO_ALT_I2C_3,
 	GPIO_ALT_MSP_0,
 	GPIO_ALT_MSP_1,
 	GPIO_ALT_MSP_2,
diff --git a/board/st/u8500/i2c.c b/board/st/u8500/i2c.c
deleted file mode 100755
index 9c23c29db..000000000
--- a/board/st/u8500/i2c.c
+++ /dev/null
@@ -1,3029 +0,0 @@
-/*
- * (C) Copyright 2009
- * STEricsson, <www.stericsson.com>
- *
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
- */
-
-#include "i2c.h"
-
-/*---------------------------------------------------------------------------
- * defines
- *---------------------------------------------------------------------------*/
-/* 8500 has 4 I2C controllers */
-t_i2c_registers *gp_i2c_registers[4];
-
-#define I2C_ENDAD_COUNTER       500000
-#define I2C_INT_ENDED_COUNTER   5
-#define I2C_BTF_COUNTER         5
-#define I2C_BTF_COUNTER_POLLING 10
-#define I2C_FIFO_FLUSH_COUNTER  500
-
-/*-----------------------------------------------------------------------------
-Global variables
------------------------------------------------------------------------------*/
-
-/* 8500 has 4 I2C Controllers */
- volatile t_i2c_system_context   g_i2c_system_context[4];    
-
-#undef I2C_DEBUG
-#ifdef I2C_DEBUG
-#define info(fmt,args...)	printf("I2C: "fmt, ##args)
-#else
-#define info(fmt,args...)	(void) 0;
-#endif
-
-
-/*-----------------------------------------------------------------------------
-	Configuration functions
------------------------------------------------------------------------------*/
-/****************************************************************************/
-/* NAME			:	I2C_Init												*/
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	Initialize the given I2C controller by specifying the   */
-/*                  base logical address.                                   */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                t_logical_address : The controller's logical address      */
-/*     InOut    :  None                                                     */
-/* 			:  None                                                	    */
-/*                                                                          */
-/* RETURN		: t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are invalid    */
-/*                I2C_UNSUPPORTED_HW    if peripheral ids are not matched   */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_Init(t_i2c_device_id id, t_logical_address address)
-{
-    t_i2c_registers *p_i2c_registers;
-
-    /* 
-	Check if the controller id is valid.
-	*/
-	
-
-    if (((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id)  ) || (NULL == address))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-    
-        
-
-    p_i2c_registers = (t_i2c_registers *) address;
-
-
-    if
-    (
-        (I2C_P_ID_0 != p_i2c_registers->periph_id_0)
-    ||  (I2C_P_ID_1 != p_i2c_registers->periph_id_1)
-    ||  (I2C_P_ID_2 != p_i2c_registers->periph_id_2)
-    ||  (I2C_P_ID_3 != p_i2c_registers->periph_id_3)
-    ||  (I2C_CELL_ID_0 != p_i2c_registers->cell_id_0)
-    ||  (I2C_CELL_ID_1 != p_i2c_registers->cell_id_1)
-    ||  (I2C_CELL_ID_2 != p_i2c_registers->cell_id_2)
-    ||  (I2C_CELL_ID_3 != p_i2c_registers->cell_id_3)
-    )
-    {
-        return(I2C_UNSUPPORTED_HW);
-    }
-
-
-    /*
-	Initialize the right structure and save the base address.
-	*/
-    g_i2c_system_context[id].base_address = address;
-    g_i2c_system_context[id].freq_scl = 0;
-    g_i2c_system_context[id].freq_input = 0;
-    g_i2c_system_context[id].mode = I2C_FREQ_MODE_STANDARD;
-    g_i2c_system_context[id].own_address = 0;
-    g_i2c_system_context[id].enabled = FALSE;
-    g_i2c_system_context[id].slave_address = 0;
-    g_i2c_system_context[id].status = I2C_STATUS_SLAVE_MODE;
-    g_i2c_system_context[id].data = 0;
-    g_i2c_system_context[id].databuffer = NULL;
-    g_i2c_system_context[id].count_data = 0;
-    g_i2c_system_context[id].register_index = 0;
-    g_i2c_system_context[id].operation = (t_i2c_operation) I2C_NO_OPERATION;
-    g_i2c_system_context[id].active_event = I2C_NO_EVENT;
-    g_i2c_system_context[id].transfer_data = 0;
-    g_i2c_system_context[id].multi_operation = FALSE;
-
-    /*   g_i2c_system_context[id].i2c_device_context... to be initialized*/
-    g_i2c_system_context[id].digital_filter_control = I2C_DIGITAL_FILTERS_OFF;
-    g_i2c_system_context[id].dma_sync_logic_control = I2C_DISABLE;
-    g_i2c_system_context[id].start_byte_procedure = I2C_DISABLE;
-    g_i2c_system_context[id].slave_data_setup_time = 0; /* TBD */
-    g_i2c_system_context[id].high_speed_master_code = 0;
-    g_i2c_system_context[id].bus_control_mode = I2C_BUS_SLAVE_MODE;
-    g_i2c_system_context[id].i2c_loopback_mode = I2C_DISABLE;
-    g_i2c_system_context[id].general_call_mode_handling = I2C_NO_GENERAL_CALL_HANDLING;
-
-    g_i2c_system_context[id].index_transfer_mode = I2C_TRANSFER_MODE_POLLING;
-    g_i2c_system_context[id].data_transfer_mode = I2C_TRANSFER_MODE_POLLING;
-    g_i2c_system_context[id].i2c_transmit_interrupt_threshold = 1;
-    g_i2c_system_context[id].i2c_receive_interrupt_threshold = 1;
-    g_i2c_system_context[id].transmit_burst_length = 0;
-    g_i2c_system_context[id].receive_burst_length = 0;
-    g_i2c_system_context[id].index_format = I2C_NO_INDEX;
-    g_i2c_system_context[id].current_bus_config = I2C_CURRENT_BUS_SLAVE_TRANSMITTER;
-    g_i2c_system_context[id].std                = FALSE;
-
-    
-    /*Disable the interrupts */
-    I2C_WRITE_REG(p_i2c_registers->imscr,I2C_CLEAR);
-    /* Disable the controller */
-    I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    return(I2C_OK);
-}
-
-
-/****************************************************************************/
-/* NAME			:	I2C_SetDeviceConfiguration                  			*/
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	Configure the given I2C controller, by clearing         */
-/*                   registers, setting the input clock. The controller and */
-/*                   interrupts are disabled after this routine             */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                t_i2c_device_config : pointer to the structer containg    */
-/*                                      the configuration                   */
-/*     InOut    :  None                                                     */
-/* 			:  None                                                	    */
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not invalid*/
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_SetDeviceConfiguration(t_i2c_device_id id, t_i2c_device_config *p_device_config)
-{
-    t_i2c_error     error_status = I2C_OK;
-    t_i2c_registers *p_i2c_registers;
-
-
-    /* Check if parameters are valid.*/
-    
-    if (NULL == p_device_config || ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id)))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-  
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    /* Disable the controller.*/
-    I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    /* Save the value.*/
-    g_i2c_system_context[id].enabled = FALSE;
-
-    /* Now save the input parameters.*/
-    g_i2c_system_context[id].digital_filter_control = p_device_config->i2c_digital_filter_control;
-    g_i2c_system_context[id].slave_data_setup_time = p_device_config->slave_data_setup_time;
-    g_i2c_system_context[id].dma_sync_logic_control = p_device_config->i2c_dma_sync_logic_control;
-    g_i2c_system_context[id].start_byte_procedure = p_device_config->i2c_start_byte_procedure;
-    g_i2c_system_context[id].high_speed_master_code = p_device_config->i2c_high_speed_master_code;
-    g_i2c_system_context[id].freq_input = p_device_config->input_frequency;
-    g_i2c_system_context[id].own_address = p_device_config->controller_i2c_address;
-
-    /* Clear registers.*/
-    I2C_WRITE_REG(p_i2c_registers->cr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->scr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->hsmcr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->tftr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->rftr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->dmar, I2C_CLEAR);
-
-    /* Set own address.*/
-    error_status = i2cp_SetOwnAddress(id, (u16) g_i2c_system_context[id].own_address);
-    if (I2C_OK != error_status)
-    {
-        return(error_status);
-    }
-
-    /* set the  digital filter   */
-    I2C_WRITE_FIELD
-    (
-        p_i2c_registers->cr,
-        I2C_CR_FON,
-        I2C_CR_SHIFT_FON,
-        (u32) g_i2c_system_context[id].digital_filter_control
-    );
-
-    /* Set the DMA sync logic */
-    I2C_WRITE_FIELD
-    (
-        p_i2c_registers->cr,
-        I2C_CR_DMA_SLE,
-        I2C_CR_SHIFT_DMA_SLE,
-        (u32) g_i2c_system_context[id].dma_sync_logic_control
-    );
-
-    /* Set the Slave Data Set up Time */
-    I2C_WRITE_FIELD
-    (
-        p_i2c_registers->scr,
-        I2C_SCR_DATA_SETUP_TIME,
-        I2C_SCR_SHIFT_DATA_SETUP_TIME,
-        g_i2c_system_context[id].slave_data_setup_time
-    );
-
-    /* Disable generation of interrupts.*/
-    I2C_DisableIRQSrc((t_i2c_irq_src_id) (((u32) id << (u32) I2CID_SHIFT) | (u32) I2C_IRQ_SRC_ALL)); /* 
-	                                            Single Interrupt source matches
-	                                            the sequence of device ids they
-	                                            are used interchangeably.
-	                                            */
-
-    /* Enable the I2C Controller */
-    I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    return(error_status);
-
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_SetTransferConfiguration                  			*/
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	Configure the given I2C controller for transfer mode,   */
-/*                  baud rate, general call handling and bus access mode.   */
-/*                  Additionally fifo levels, loopback control and DMA      */
-/*                  burst length are also configured.           */
-/*                  This routine enable the I2C controller                  */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                t_i2c_transfer_config :pointer to the structure containing */
-/*                                      the configuration                   */
-/*     InOut    :  None                                                     */
-/* 			:  None                                                	    */
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*                I2C_UNSUPPORTED_FEATURE if required index and data        */
-/*                  transfer modes are not supported.                       */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-
-/****************************************************************************/
- t_i2c_error I2C_SetTransferConfiguration(t_i2c_device_id id, t_i2c_transfer_config *p_transfer_config)
-{
-    t_i2c_error     error_status = I2C_OK;
-    t_i2c_registers *p_i2c_registers;
-
-    /* Check if parameters are valid.*/
-  
-    if (NULL == p_transfer_config || ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id)))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-
-    /*
-    Error handling for unsopported features according to the platform
-    */
-    if
-    (
-        (
-            I2C_TRANSFER_MODE_POLLING != p_transfer_config->index_transfer_mode
-        &&  I2C_TRANSFER_MODE_POLLING == p_transfer_config->data_transfer_mode
-        )
-    ||  (    (I2C_TRANSFER_MODE_INTERRUPT == p_transfer_config->index_transfer_mode) 
-          && (I2C_TRANSFER_MODE_DMA == p_transfer_config->data_transfer_mode)
-        )
-        
-    ||  (I2C_TRANSFER_MODE_DMA == p_transfer_config->index_transfer_mode)
-    )
-    {
-        return(I2C_UNSUPPORTED_FEATURE);
-    }
-
-    /* 
-        Clear all the existing state of the controller by clearing PE bit
-    */
-    I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    /* Now save the input parameters.*/
-    g_i2c_system_context[id].i2c_loopback_mode = p_transfer_config->i2c_loopback_mode;
-    g_i2c_system_context[id].general_call_mode_handling = p_transfer_config->i2c_slave_general_call_mode;
-    g_i2c_system_context[id].index_transfer_mode = p_transfer_config->index_transfer_mode;
-
-    /*Index transfer mode is still relevant even if I2C_NO_INDEX is 
-      used since then this mode is used for the address transmission */
-      
-    g_i2c_system_context[id].data_transfer_mode = p_transfer_config->data_transfer_mode;
-    g_i2c_system_context[id].i2c_transmit_interrupt_threshold = p_transfer_config->i2c_transmit_interrupt_threshold;
-    g_i2c_system_context[id].i2c_receive_interrupt_threshold = p_transfer_config->i2c_receive_interrupt_threshold;
-    g_i2c_system_context[id].transmit_burst_length = p_transfer_config->transmit_burst_length;
-    g_i2c_system_context[id].receive_burst_length = p_transfer_config->receive_burst_length;
-    g_i2c_system_context[id].freq_scl = p_transfer_config->i2c_transfer_frequency;
-    g_i2c_system_context[id].bus_control_mode = p_transfer_config->bus_control_mode;
-
-    g_i2c_system_context[id].multi_operation = FALSE;
-    g_i2c_system_context[id].register_index = 0;    /* The index of the slave's registers*/
-    g_i2c_system_context[id].index_format = I2C_NO_INDEX;
-
-
-    /* Set the SCL bus clock frequency. -> transfer frequency*/
-    error_status = i2cp_SetBusClock(id, g_i2c_system_context[id].freq_scl, g_i2c_system_context[id].freq_input);
-    if (I2C_OK != error_status)
-    {
-        return(error_status);
-    }
-
-    /*Set the loop back mode */
-    I2C_WRITE_FIELD
-    (
-        p_i2c_registers->cr,
-        I2C_CR_LM,
-        I2C_CR_SHIFT_LM,
-        (u32) g_i2c_system_context[id].i2c_loopback_mode
-    );
-
-    /*  Enable the general call handing in the controller*/
-    /*  Only possible general call handing in this controller is 
-	    in the software mode.
-	*/
-    if (I2C_HARDWARE_GENERAL_CALL_HANDLING == g_i2c_system_context[id].general_call_mode_handling)
-    {
-        I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_SGCM);
-    }
-    else
-    {
-        I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_SGCM);
-    }
-
-    /* Disable the Tx DMA */
-    I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_DMA_TX_EN);
-
-    /* Disable the Rx DMA */
-    I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_DMA_RX_EN);
-
-    /* configure the Tx DMA burst size */
-    if (g_i2c_system_context[id].transmit_burst_length >= 1)
-    {
-        /* set the DMA Tx request mode to Burst */
-        I2C_SET_BIT(p_i2c_registers->dmar, I2C_DMA_BURST_TX);
-
-        /* Set the Destination Burst Size */
-        I2C_WRITE_FIELD
-        (
-            p_i2c_registers->dmar,
-            I2C_DMA_DBSIZE_TX,
-            I2C_DMA_SHIFT_DBSIZE_TX,
-            g_i2c_system_context[id].transmit_burst_length
-        );
-    }
-    else
-    {
-        /* Set the DMA Tx Request mode to Single */
-        I2C_CLR_BIT(p_i2c_registers->dmar, I2C_DMA_BURST_TX);
-    }
-
-    /* configure the Rx DMA burst size */
-    if (g_i2c_system_context[id].receive_burst_length >= 1)
-    {
-        /* set the DMA Rx request mode to Burst */
-        I2C_SET_BIT(p_i2c_registers->dmar, I2C_DMA_BURST_RX);
-
-        /* Set the source burst size */
-        I2C_WRITE_FIELD
-        (
-            p_i2c_registers->dmar,
-            I2C_DMA_SBSIZE_RX,
-            I2C_DMA_SHIFT_SBSIZE_RX,
-            g_i2c_system_context[id].receive_burst_length
-        );
-    }
-    else
-    {
-        /* Set the DMA Rx Request mode to Single */
-        I2C_CLR_BIT(p_i2c_registers->dmar, I2C_DMA_BURST_RX);
-    }
-
-    /* Set the Bus control mode */
-    I2C_WRITE_FIELD
-    (
-        p_i2c_registers->cr,
-        I2C_CR_OM,
-        I2C_CR_SHIFT_OM,
-        (u32) g_i2c_system_context[id].bus_control_mode
-    );
-
-    /* Set the Transmit Fifo threshold value */
-    p_i2c_registers->tftr = g_i2c_system_context[id].i2c_transmit_interrupt_threshold;
-
-    /* Set the Receive Fifo Threshold value */
-    p_i2c_registers->rftr = g_i2c_system_context[id].i2c_receive_interrupt_threshold;
-
-    /*Disable the interrupts if index transfer mode is polling */
-    if (I2C_TRANSFER_MODE_POLLING == g_i2c_system_context[id].index_transfer_mode)
-    {
-        I2C_DisableIRQSrc((t_i2c_irq_src_id) (((u32) id << (u32) I2CID_SHIFT) | (u32) I2C_IRQ_SRC_ALL));
-    }
-
-    /* Now restore CR register with the values it has before it was disabled.*/
-    /*Enable the I2C Controller and set the enable status in the global structure */
-    I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
-    g_i2c_system_context[id].enabled = TRUE;
-
-    return(error_status);
-
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_SetTransferMode                  			        */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	Set the transfer modes for the index and data transfer  */
-/*                   on the given I2C controller                            */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                t_i2c_transfer_mode : Index transfer mode                 */
-/*                t_i2c_transfer_mode : data  transger mode                 */
-/*     InOut    :  None                                                     */
-/* 			:  None                                                	    */
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*                I2C_UNSUPPORTED_FEATURE if required index and data        */
-/*                  transfer modes are not supported.                       */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_SetTransferMode
-(
-    t_i2c_device_id      id,
-    t_i2c_transfer_mode  index_transfer_mode,
-    t_i2c_transfer_mode  data_transfer_mode
-)
-{
-    info(
-        "Id is %d, index  and data transfer modes are %lx and %lx",
-        id,
-        (u32) index_transfer_mode,
-        (u32) data_transfer_mode
-    );
-
-    if
-    (
-        (I2C_TRANSFER_MODE_POLLING != index_transfer_mode && I2C_TRANSFER_MODE_POLLING == data_transfer_mode)
-    ||  (    (I2C_TRANSFER_MODE_INTERRUPT == index_transfer_mode) 
-          && (I2C_TRANSFER_MODE_DMA == data_transfer_mode)
-        )
-
-    ||  (I2C_TRANSFER_MODE_DMA == index_transfer_mode)
-    )
-    {
-        return(I2C_UNSUPPORTED_FEATURE);
-    }
-
-
-    /* Check if parameters are valid.*/
-
-    if ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-
-    g_i2c_system_context[id].index_transfer_mode = index_transfer_mode;
-
-    /*Index transfer mode is still relevant even if I2C_NO_INDEX is 
-      used since then this mode is used for the addres transmission */
-      
-    g_i2c_system_context[id].data_transfer_mode = data_transfer_mode;
-
-    /*Disable the interrupts if index tranfer mode is polling  */
-    if (I2C_TRANSFER_MODE_POLLING == index_transfer_mode)
-    {
-
-        I2C_DisableIRQSrc((t_i2c_irq_src_id) (((u32) id << (u32) I2CID_SHIFT) | (u32) I2C_IRQ_SRC_ALL));
-
-    }
-
-    return(I2C_OK);
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_SetBusControlMode                  			        */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	Set the bus control mode for the data transfer on the   */
-/*                given I2C controller.                                     */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                t_i2c_bus_control_mode : The mode in which I2C bus        */
-/*                                         is accessed                      */
-/*     InOut    :  None                                                     */
-/* 			:  None                                                	    */
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-
-/****************************************************************************/
- t_i2c_error I2C_SetBusControlMode(t_i2c_device_id id, t_i2c_bus_control_mode bus_control_mode)
-{
-
-
-    t_i2c_registers *p_i2c_registers;
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-
-    /* Check if parameters are valid.*/
-
-    if ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-
-    g_i2c_system_context[id].bus_control_mode = bus_control_mode;
-
-    /* Disable the I2C controller before configuring */
-    I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    /* Set the Bus control mode */
-    I2C_WRITE_FIELD
-    (
-        p_i2c_registers->cr,
-        I2C_CR_OM,
-        I2C_CR_SHIFT_OM,
-        (u32) g_i2c_system_context[id].bus_control_mode
-    );
-
-    /* Enable the I2C controller */
-    I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    return(I2C_OK);
-}
-
-/*-----------------------------------------------------------------------------
-	    Configuration functions
------------------------------------------------------------------------------*/
-/****************************************************************************/
-/* NAME			:	I2C_FlushFifo                  			                */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	Flush the transmit or receive FIFO                      */ 
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                t_i2c_fifo :        FIFO to be flused                     */
-/*     InOut    :  None                                                     */
-/* 			:  None                                                	    */
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*               = I2C_INVALID_PARAMETER  - if input id is wrong            */
-/*               = I2C_HW_FAILED          - if FIFO flush bit is not reset  */
-/*                            itself after setting. This could be happen if */
-/*                            i2c clock frequency is not set                */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-
-/****************************************************************************/
- t_i2c_error I2C_FlushFifo(t_i2c_device_id id, t_i2c_fifo fifo)
-{
-
-    u32        loop_counter;
-    t_i2c_registers *p_i2c_registers;
-
-    /* Check parameters valid */
-    if ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    switch (fifo)
-    {
-        case I2C_TRANSMIT_FIFO:
-            I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_FTX);   /* Flush the Tx Fifo */
-
-            /*Wait till for the Tx Flush bit to reset */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->cr, I2C_CR_FTX, I2C_CR_SHIFT_FTX)
-            &&  loop_counter < I2C_FIFO_FLUSH_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_FIFO_FLUSH_COUNTER)
-            {
-                return(I2C_HW_FAILED);
-            }
-            break;
-
-        case I2C_RECEIVE_FIFO:
-            I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_FRX);   /* Flush the Rx Fifo */
-
-            /* Wait till Rx flush bit to reset */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->cr, I2C_CR_FRX, I2C_CR_SHIFT_FRX)
-            &&  loop_counter < I2C_FIFO_FLUSH_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_FIFO_FLUSH_COUNTER)
-            {
-                return(I2C_HW_FAILED);
-            }
-            break;
-    }
-
-    return(I2C_OK);
-
-}
-
-
-
-
-
-/****************************************************************************/
-/* NAME			:	I2C_Enable                  		                    */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	:Enable the given I2C controller.                           */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*     InOut    :  None                                                     */
-/* 			:  None														*/
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_Enable(t_i2c_device_id id)
-{
-    t_i2c_registers *p_i2c_registers;
-
-    /* Check if parameters are valid.*/
-    if ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-  
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    g_i2c_system_context[id].enabled = TRUE;
-    return(I2C_OK);
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_Disable                  		                    */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	:Disble the given I2C controller.                           */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to disabled       		*/
-/*     InOut    :  None                                                     */
-/* 			:  None														*/
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_Disable(t_i2c_device_id id)
-{
-    t_i2c_registers *p_i2c_registers;
-
-    /* Check if parameters are valid.*/
-    if ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-    
-    I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    g_i2c_system_context[id].enabled = FALSE;
-    return(I2C_OK);
-
-}
-
-
-
-/****************************************************************************/
-/* NAME			:	I2C_WriteSingleData                  		            */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	:This routine is used to write a single data byte to        */
-/*                a receiver. Writing can be done to a slave device by      */
-/*               using the indexed modes.                                   */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                u16   : The address of the slave to be accessed      */
-/*                u16   : The index of the register on the receiver    */
-/*                             to which data is written                     */
-/*                t_unit16   :The format of the index on receiver side      */
-/*                u8 : The data byte to be written to the slave device */
-/*     InOut    :  None                                                     */
-/* 			:  None														*/
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*                I2C_SLAVE_ADDRESS_NOT_VALID  If requested slave address   */
-/*                                      is not valid                        */
-/*                I2C_CONTROLLER_BUSY    if I2C controller is busy          */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_WriteSingleData
-(
-    t_i2c_device_id      id,
-    u16             slave_address,
-    t_i2c_index_format   index_format,
-    u16             index_value,
-    u8              data
-)
-{
-
-/*
-Steps:
-    - Check Mode
-        - Polling
-        - Interrupt
-        - DMA
-*/
-
-    volatile u32   mcr = 0;
-    t_i2c_error         error_status = I2C_OK;
-    t_i2c_registers     *p_i2c_registers;
-    info(
-        "Id is %d, Address is %x, Index format is %d and value is %d, Data is %d",
-        id,
-        slave_address,
-        index_format,
-        index_value,
-        data
-    );
-
-    /* Check if parameters are valid.*/
-    if ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-
-    if (!i2cp_AddressIsValid(slave_address))
-    {
-        return(I2C_SLAVE_ADDRESS_NOT_VALID);
-    }
-
-    /* Index transfers are only valid in case the Bus Control Mode is not slave*/
-    if ((I2C_BUS_MASTER_MODE != g_i2c_system_context[id].bus_control_mode) && (I2C_NO_INDEX != index_format))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    /* Check if not busy.*/
-    if ((g_i2c_system_context[id].operation != I2C_NO_OPERATION))
-    {
-        return(I2C_CONTROLLER_BUSY);
-    }
-
-    /* Save parameters.*/
-    g_i2c_system_context[id].slave_address = slave_address;
-    g_i2c_system_context[id].status = I2C_STATUS_SLAVE_MODE;
-    g_i2c_system_context[id].register_index = index_value;
-    g_i2c_system_context[id].index_format = index_format;
-    g_i2c_system_context[id].data = data;
-    g_i2c_system_context[id].databuffer = NULL;
-    g_i2c_system_context[id].count_data = 1;
-    g_i2c_system_context[id].operation = I2C_WRITE;
-    g_i2c_system_context[id].active_event = I2C_NO_EVENT;
-    g_i2c_system_context[id].transfer_data = 0;
-    g_i2c_system_context[id].multi_operation = FALSE;
-
-    /* Disable all the interrupts to remove previously garbage interrupts */
-    switch(id)
-    {
-    	case I2C0 :
-    	     I2C_DisableIRQSrc(I2C0_IRQ_SRC_ALL);
-    	     break;
-    	case I2C1 :
-    	     I2C_DisableIRQSrc(I2C1_IRQ_SRC_ALL);
-    	     break;    	
-    	case I2C2:
-    	     I2C_DisableIRQSrc(I2C2_IRQ_SRC_ALL);    	
-    	     break;
-    	case I2C3:
-    	     I2C_DisableIRQSrc(I2C3_IRQ_SRC_ALL);    	     
-    	     break;
-    	default:
-    	     break;
-    }
-/*    I2C_DisableIRQSrc((I2C0 == id) ? I2C0_IRQ_SRC_ALL : I2C1_IRQ_SRC_ALL);*/
-
-    /* Check if I2C controller is Master */
-    if (I2C_BUS_MASTER_MODE == g_i2c_system_context[id].bus_control_mode)
-    {
-        /* Master control configuration  */
-
-        /* Set the Master write operation */
-        I2C_CLR_BIT(mcr, I2C_MCR_OP);
-
-        /*  start byte procedure configuration */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_SB, I2C_MCR_SHIFT_SB, (u32) g_i2c_system_context[id].start_byte_procedure);
-
-        /* Check the General call handling */
-        if (g_i2c_system_context[id].general_call_mode_handling != I2C_NO_GENERAL_CALL_HANDLING)
-        {
-            /* The Transaction is intiated by a general call command */
-            I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 0);
-        }
-        else
-        {
-            /* Check if Slave address is 10 bit */
-            if (g_i2c_system_context[id].slave_address < 1024 && g_i2c_system_context[id].slave_address > 127)
-            {
-                /* Set the Address mode to 10 bit */
-                I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 2);
-            }
-            else
-            {
-                /* Set the Address mode to 7 bit */
-                I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 1);
-            }
-        }
-
-        /* Store the HS master code */
-        if (I2C_FREQ_MODE_HIGH_SPEED == g_i2c_system_context[id].mode)
-        {
-            p_i2c_registers->hsmcr = g_i2c_system_context[id].high_speed_master_code;
-        }
-
-        /* Store  the Slave addres in the Master control register */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_A10, I2C_MCR_SHIFT_A10, slave_address);
-
-        /* Configure the STOP condition*/
-        /* Current transaction is terminated by STOP condition */
-        I2C_SET_BIT(mcr, I2C_MCR_STOP);
-
-        /* Configuring the Frame length */
-        switch (index_format)
-        {
-            case I2C_NO_INDEX:
-                I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 1);
-                break;
-
-            case I2C_BYTE_INDEX:
-                I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 2);
-                break;
-
-            case I2C_HALF_WORD_LITTLE_ENDIAN:
-            case I2C_HALF_WORD_BIG_ENDIAN:
-                I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 3);
-                break;
-
-            default:
-                break;
-        }
-
-        /*Write the MCR register  */
-        p_i2c_registers->mcr = mcr;
-
-    }
-
-    switch (g_i2c_system_context[id].index_transfer_mode)
-    {
-        case I2C_TRANSFER_MODE_POLLING:
-            /*  	
-          Index Transfer
-          */
-            if (I2C_BUS_SLAVE_MODE == g_i2c_system_context[id].bus_control_mode)
-            {
-                error_status = i2cp_SlaveIndexReceive(id);
-                if (I2C_OK != error_status)
-                {
-                    return(error_status);
-                }
-            }
-            else
-            {
-                error_status = i2cp_MasterIndexTransmit(id);
-                if (I2C_OK != error_status)
-                {
-                    return(error_status);
-                }
-            }
-
-            /*
-          Data Transfer
-          */
-            switch (g_i2c_system_context[id].data_transfer_mode)
-            {
-                case I2C_TRANSFER_MODE_POLLING:
-                    error_status = i2cp_TransmitDataPolling(id, (u8 *) &g_i2c_system_context[id].data);
-                    if (I2C_OK != error_status)
-                    {
-                        return(error_status);
-                    }
-
-                    /* Stop Signal to be sent/received for transfer completion*/
-                    break;
-
-                case I2C_TRANSFER_MODE_INTERRUPT:
-                case I2C_TRANSFER_MODE_DMA:
-                default:
-                    break;
-            }
-            break;
-
-        case I2C_TRANSFER_MODE_INTERRUPT:
-        case I2C_TRANSFER_MODE_DMA:
-        default:
-            return(I2C_INVALID_PARAMETER);
-    }
-
-    return(error_status);
-
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_WriteMultipleData                  		            */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	:This routine is used to write a multiple data byte to      */
-/*                a receiver. Writing can be done to a slave device by      */
-/*               using the indexed modes.                                   */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                u16   : The address of the slave to be accessed      */
-/*                u16   : The index of the register on the receiver    */
-/*                             to which data is written                     */
-/*                t_unit16   :The format of the index on receiver side      */
-/*                u8*   : The data buffer to be written to the         */
-/*                           slave device                                   */
-/*                t_unit32   : no of bytes to be transfered                 */
-/*     InOut    :  None                                                     */
-/* 			:  None														*/
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*                I2C_SLAVE_ADDRESS_NOT_VALID  If requested slave address   */
-/*                                      is not valid                        */
-/*                I2C_CONTROLLER_BUSY    if I2C controller is busy          */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_WriteMultipleData
-(
-    t_i2c_device_id      id,
-    u16             slave_address,
-    t_i2c_index_format   index_format,
-    u16             index_value,
-    u8              *p_data,
-    u32             count
-)
-{
-
-/*
-Steps:
-    - Check Mode
-        - Polling
-        - Interrupt
-        - DMA
-*/
-    volatile u32   mcr = 0;
-    t_i2c_error         error_status = I2C_OK;
-    t_i2c_registers     *p_i2c_registers;
-    info(
-        "Id is %d, Address is %x, Index format is %d and value is %d, Data count is %d and @ is %p",
-        id,
-        slave_address,
-        index_format,
-        index_value,
-        count,
-        (void *) p_data
-    );
-
-    /* Check if parameters are valid.*/
-    if (((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id)) || (NULL == p_data))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-
-    if (!i2cp_AddressIsValid(slave_address))
-    {
-        return(I2C_SLAVE_ADDRESS_NOT_VALID);
-    }
-
-    /* Index transfers are only valid in case the Bus Control Mode is not slave*/
-    if ((I2C_BUS_MASTER_MODE != g_i2c_system_context[id].bus_control_mode) && (I2C_NO_INDEX != index_format))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    /* Check if not busy.*/
-    if ((g_i2c_system_context[id].operation != I2C_NO_OPERATION))
-    {
-        return(I2C_CONTROLLER_BUSY);
-    }
-
-    /* Save parameters.*/
-    g_i2c_system_context[id].slave_address = slave_address;
-    g_i2c_system_context[id].status = I2C_STATUS_SLAVE_MODE;
-    g_i2c_system_context[id].register_index = index_value;
-    g_i2c_system_context[id].index_format = index_format;
-    g_i2c_system_context[id].data = 0;
-    g_i2c_system_context[id].databuffer = p_data;
-    g_i2c_system_context[id].count_data = count;
-    g_i2c_system_context[id].operation = I2C_WRITE;
-    g_i2c_system_context[id].active_event = I2C_NO_EVENT;
-    g_i2c_system_context[id].transfer_data = 0;
-    g_i2c_system_context[id].multi_operation = TRUE;
-
-    /* Disable all the interrupts to remove previously garbage interrupts */
-    switch(id)
-    {
-    	case I2C0 :
-    	     I2C_DisableIRQSrc(I2C0_IRQ_SRC_ALL);
-    	     break;
-    	case I2C1 :
-    	     I2C_DisableIRQSrc(I2C1_IRQ_SRC_ALL);
-    	     break;    	
-    	case I2C2:
-    	     I2C_DisableIRQSrc(I2C2_IRQ_SRC_ALL);    	
-    	     break;
-    	case I2C3:
-    	     I2C_DisableIRQSrc(I2C3_IRQ_SRC_ALL);    	     
-    	     break;
-    	default:
-    	     break;
-    }
-    /*I2C_DisableIRQSrc((I2C0 == id) ? I2C0_IRQ_SRC_ALL : I2C1_IRQ_SRC_ALL);*/
-
-    /* Check if I2C controller is Master */
-    if (I2C_BUS_MASTER_MODE == g_i2c_system_context[id].bus_control_mode)
-    {
-        /* Master control configuration  */
-
-        /* Set the Master write operation */
-        I2C_CLR_BIT(mcr, I2C_MCR_OP);
-
-        /*  start byte procedure configuration */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_SB, I2C_MCR_SHIFT_SB, (u32) g_i2c_system_context[id].start_byte_procedure);
-
-        /* Check the General call handling */
-        if (g_i2c_system_context[id].general_call_mode_handling != I2C_NO_GENERAL_CALL_HANDLING)
-        {
-            /* The Transaction is intiated by a general call command */
-            I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 0);
-        }
-        else
-        {
-            /* Check if Slave address is 10 bit */
-            if (g_i2c_system_context[id].slave_address < 1024 && g_i2c_system_context[id].slave_address > 127)
-            {
-                /* Set the Address mode to 10 bit */
-                I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 2);
-            }
-            else
-            {
-                /* Set the Address mode to 7 bit */
-                I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 1);
-            }
-        }
-
-        /* Store the HS master code */
-        if (I2C_FREQ_MODE_HIGH_SPEED == g_i2c_system_context[id].mode)
-        {
-            p_i2c_registers->hsmcr = g_i2c_system_context[id].high_speed_master_code;
-        }
-
-        /* Store  the Slave addres in the Master control register */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_A10, I2C_MCR_SHIFT_A10, slave_address);
-
-        /* Configure the STOP condition*/
-        /* Current transaction is terminated by STOP condition */
-        I2C_SET_BIT(mcr, I2C_MCR_STOP);
-
-        /* Configuring the Frame length */
-        switch (index_format)
-        {
-            case I2C_NO_INDEX:
-                I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, count);
-                break;
-
-            case I2C_BYTE_INDEX:
-                I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, (count + 1));
-                break;
-
-            case I2C_HALF_WORD_LITTLE_ENDIAN:
-            case I2C_HALF_WORD_BIG_ENDIAN:
-                I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, (count + 2));
-                break;
-
-            default:
-                break;
-        }
-
-        p_i2c_registers->mcr = mcr;
-    }
-
-    switch (g_i2c_system_context[id].index_transfer_mode)
-    {
-        case I2C_TRANSFER_MODE_POLLING:
-            /*  	
-         Index Transfer
-         */
-            if (I2C_BUS_SLAVE_MODE == g_i2c_system_context[id].bus_control_mode)
-            {
-                error_status = i2cp_SlaveIndexReceive(id);
-                if (I2C_OK != error_status)
-                {
-                    return(error_status);
-                }
-            }
-            else
-            {
-                error_status = i2cp_MasterIndexTransmit(id);
-                if (I2C_OK != error_status)
-                {
-                    return(error_status);
-                }
-            }
-
-            /*
-         Data Transfer
-         */
-            switch (g_i2c_system_context[id].data_transfer_mode)
-            {
-                case I2C_TRANSFER_MODE_POLLING:
-                    error_status = i2cp_TransmitDataPolling(id, g_i2c_system_context[id].databuffer);
-                    if (I2C_OK != error_status)
-                    {
-                        return(error_status);
-                    }
-                    break;
-
-                case I2C_TRANSFER_MODE_INTERRUPT:
-                case I2C_TRANSFER_MODE_DMA:
-                default:
-                    break;
-            }
-            break;
-
-        case I2C_TRANSFER_MODE_INTERRUPT:
-        case I2C_TRANSFER_MODE_DMA:
-        default:
-            return(I2C_INVALID_PARAMETER);
-    }
-
-    return(error_status);
-
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_ReadSingleData                  		            */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	:This routine is used to read a single  data byte from      */
-/*                a transmitter. Read can be done from  a slave device by   */
-/*               using the indexed modes.                                   */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                u16   : The address of the slave to be accessed      */
-/*                u16   : The index of the register on the transmitter */
-/*                              from which data is read                     */
-/*                t_unit16   :The format of the index on tranmitter side    */
-/*                u8    : The data  to be read from the tranmitter     */
-/*                t_unit32   : no of bytes to be transfered                 */
-/*     InOut    :  None                                                     */
-/* 			:  None														*/
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*                I2C_SLAVE_ADDRESS_NOT_VALID  If requested slave address   */
-/*                                      is not valid                        */
-/*                I2C_CONTROLLER_BUSY    if I2C controller is busy          */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_ReadSingleData
-(
-    t_i2c_device_id      id,
-    u16             slave_address,
-    t_i2c_index_format   index_format,
-    u16             index_value,
-    u8              *p_data
-)
-{
-
-/*
-Steps:
-    - Check Mode
-        - Polling
-        - Interrupt
-        - DMA
-*/
-    volatile u32   mcr = 0;
-    t_i2c_error         error_status = I2C_OK;
-    t_i2c_registers     *p_i2c_registers;
-    info(
-        "Id is %d, Address is %x, Index format is %d and value is %d, Data count is %d and @ is %p",
-        id,
-        slave_address,
-        index_format,
-        index_value,
-        (void *) p_data
-    );
-
-    /* Check if parameters are valid.*/
-    if (((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id)) || (NULL == p_data))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-    if (!i2cp_AddressIsValid(slave_address))
-    {
-        return(I2C_SLAVE_ADDRESS_NOT_VALID);
-    }
-
-    /* Index transfers are only valid in case the Bus Control Mode is not slave*/
-    if ((I2C_BUS_MASTER_MODE != g_i2c_system_context[id].bus_control_mode) && (I2C_NO_INDEX != index_format))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    /* Check if not busy.*/
-    if ((g_i2c_system_context[id].operation != I2C_NO_OPERATION))
-    {
-        return(I2C_CONTROLLER_BUSY);
-    }
-
-    /* Save parameters.*/
-    g_i2c_system_context[id].slave_address = slave_address;
-    g_i2c_system_context[id].status = I2C_STATUS_SLAVE_MODE;
-    g_i2c_system_context[id].register_index = index_value;
-    g_i2c_system_context[id].index_format = index_format;
-    g_i2c_system_context[id].data = 0;
-    g_i2c_system_context[id].databuffer = p_data;
-    g_i2c_system_context[id].count_data = 1;
-    g_i2c_system_context[id].operation = I2C_READ;
-    g_i2c_system_context[id].active_event = I2C_NO_EVENT;
-    g_i2c_system_context[id].transfer_data = 0;
-    g_i2c_system_context[id].multi_operation = FALSE;
-
-    /* Disable all the interrupts to remove previously garbage interrupts */
-    switch(id)
-    {
-    	case I2C0 :
-    	     I2C_DisableIRQSrc(I2C0_IRQ_SRC_ALL);
-    	     break;
-    	case I2C1 :
-    	     I2C_DisableIRQSrc(I2C1_IRQ_SRC_ALL);
-    	     break;    	
-    	case I2C2:
-    	     I2C_DisableIRQSrc(I2C2_IRQ_SRC_ALL);    	
-    	     break;
-    	case I2C3:
-    	     I2C_DisableIRQSrc(I2C3_IRQ_SRC_ALL);    	     
-    	     break;
-    	default:
-    	     break;
-    }
-    
-    /*I2C_DisableIRQSrc((id == I2C0) ? I2C0_IRQ_SRC_ALL : I2C1_IRQ_SRC_ALL);*/
-
-    /* Check if I2C controller is Master */
-    if (I2C_BUS_MASTER_MODE == g_i2c_system_context[id].bus_control_mode)
-    {
-
-        /*  start byte procedure configuration */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_SB, I2C_MCR_SHIFT_SB, (u32) g_i2c_system_context[id].start_byte_procedure);
-
-        /* Check the General call handling */
-        if (g_i2c_system_context[id].general_call_mode_handling != I2C_NO_GENERAL_CALL_HANDLING)
-        {
-            /* The Transaction is intiated by a general call command */
-            I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 0);
-        }
-        else
-        {
-            /* Check if Slave address is 10 bit */
-            if (g_i2c_system_context[id].slave_address < 1024 && g_i2c_system_context[id].slave_address > 127)
-            {
-                /* Set the Address mode to 10 bit */
-                I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 2);
-            }
-            else
-            {
-                /* Set the Address mode to 7 bit */
-                I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 1);
-            }
-        }
-
-        /* Store the HS master code */
-        if (I2C_FREQ_MODE_HIGH_SPEED == g_i2c_system_context[id].mode)
-        {
-            p_i2c_registers->hsmcr = g_i2c_system_context[id].high_speed_master_code;
-        }
-
-        /* Store  the Slave addres in the Master control register */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_A10, I2C_MCR_SHIFT_A10, slave_address);
-
-        if
-        (
-            (g_i2c_system_context[id].slave_address < 1024 && g_i2c_system_context[id].slave_address > 127)
-        &&  (I2C_NO_INDEX == index_format)
-        )
-        {
-            /* Set the Master write operation */
-            I2C_CLR_BIT(mcr, I2C_MCR_OP);
-
-            I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 0);
-
-            /* Current transaction is not terminated by STOP condition,
-                 a repeated start operation will be fallowed */
-            I2C_CLR_BIT(mcr, I2C_MCR_STOP);
-
-            /*Write MCR register   */
-            p_i2c_registers->mcr = mcr;
-
-            /* Enable the I2C controller */
-            I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
-        }
-        else
-        {
-            /* Master control configuration  */
-            if (I2C_NO_INDEX != index_format)
-            {
-                /* Set the Master write operation */
-                I2C_CLR_BIT(mcr, I2C_MCR_OP);
-            }
-            else
-            {
-                /* Set the Master read operation */
-                I2C_SET_BIT(mcr, I2C_MCR_OP);
-            }
-
-            /* Configuring the Frame length */
-            switch (index_format)
-            {
-                case I2C_NO_INDEX:
-                    I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 1);
-
-                    /* Current transaction is terminated by STOP condition */
-                    I2C_SET_BIT(mcr, I2C_MCR_STOP);
-                    break;
-
-                case I2C_BYTE_INDEX:
-                    I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 1);
-
-                    /* Current transaction is not terminated by STOP condition,
-	                 a repeated start operation will be fallowed */
-                    I2C_CLR_BIT(mcr, I2C_MCR_STOP);
-                    break;
-
-                case I2C_HALF_WORD_LITTLE_ENDIAN:
-                case I2C_HALF_WORD_BIG_ENDIAN:
-                    I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 2);
-
-                    /* Current transaction is not terminated by STOP condition,
-	                a repeated start operation will be fallowed */
-                    I2C_CLR_BIT(mcr, I2C_MCR_STOP);
-                    break;
-
-                default:
-                    break;
-            }
-
-            /*Write MCR register   */
-            p_i2c_registers->mcr = mcr;
-
-        }
-    }
-
-    switch (g_i2c_system_context[id].index_transfer_mode)
-    {
-        case I2C_TRANSFER_MODE_POLLING:
-            /*  	
-         Index Transfer
-         */
-            if (I2C_BUS_SLAVE_MODE == g_i2c_system_context[id].bus_control_mode)
-            {
-                error_status = i2cp_SlaveIndexReceive(id);
-                if (I2C_OK != error_status)
-                {
-                    return(error_status);
-                }
-            }
-            else
-            {
-                error_status = i2cp_MasterIndexTransmit(id);
-                if (I2C_OK != error_status)
-                {
-                    return(error_status);
-                }
-            }
-
-            /*
-         Data Transfer
-         */
-            switch (g_i2c_system_context[id].data_transfer_mode)
-            {
-                case I2C_TRANSFER_MODE_POLLING:
-                    error_status = i2cp_ReceiveDataPolling(id, g_i2c_system_context[id].databuffer);
-                    if (I2C_OK != error_status)
-                    {
-                        return(error_status);
-                    }
-
-                    /* Stop Signal to be sent/received for transfer completion*/
-                    break;
-
-                case I2C_TRANSFER_MODE_INTERRUPT:
-                case I2C_TRANSFER_MODE_DMA:
-                default:
-                    break;
-            }
-            break;
-
-        case I2C_TRANSFER_MODE_INTERRUPT:
-        case I2C_TRANSFER_MODE_DMA:
-        default:
-            return(I2C_INVALID_PARAMETER);
-    }
-
-    return(error_status);
-
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_ReadMultipleData                  		            */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	:This routine is used to read a multiple  data byte from    */
-/*                a transmitter. Read can be done from  a slave device by   */
-/*               using the indexed modes.                                   */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be initialized		*/
-/*                u16   : The address of the slave to be accessed      */
-/*                u16   : The index of the register on the transmitter */
-/*                              from which data is read                     */
-/*                t_unit16   :The format of the index on tranmitter side    */
-/*                u8*   : The data buffer to be written to the         */
-/*                           slave device                                   */
-/*                t_unit32   : no of bytes to be transfered                 */
-/*     InOut    :  None                                                     */
-/* 			:  None														*/
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*                I2C_SLAVE_ADDRESS_NOT_VALID  If requested slave address   */
-/*                                      is not valid                        */
-/*                I2C_CONTROLLER_BUSY    if I2C controller is busy          */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_ReadMultipleData
-(
-    t_i2c_device_id      id,
-    u16             slave_address,
-    t_i2c_index_format   index_format,
-    u16             index_value,
-    u8              *p_data,
-    u32             count
-)
-{
-
-/*
-Steps:
-    - Check Mode
-        - Polling
-        - Interrupt
-        - DMA
-*/
-    volatile u32   mcr = 0;
-    t_i2c_error         error_status = I2C_OK;
-    t_i2c_registers     *p_i2c_registers;
-    info(
-        "Id is %d, Address is %x, Index format is %d and value is %d, Data count is %d and @ is %p",
-        id,
-        slave_address,
-        index_format,
-        index_value,
-        count,
-        (void *) p_data
-    );
-
-    /* Check if parameters are valid.*/
-    if (((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id) ) || (NULL == p_data))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-    
-
-    if (!i2cp_AddressIsValid(slave_address))
-    {
-        return(I2C_SLAVE_ADDRESS_NOT_VALID);
-    }
-
-    /* Index transfers are only valid in case the Bus Control Mode is not slave*/
-    if ((I2C_BUS_MASTER_MODE != g_i2c_system_context[id].bus_control_mode) && (I2C_NO_INDEX != index_format))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    /* Check if not busy.*/
-    if ((g_i2c_system_context[id].operation != I2C_NO_OPERATION))
-    {
-        return(I2C_CONTROLLER_BUSY);
-    }
-
-    /* Save parameters.*/
-    g_i2c_system_context[id].slave_address = slave_address;
-    g_i2c_system_context[id].status = I2C_STATUS_SLAVE_MODE;
-    g_i2c_system_context[id].register_index = index_value;
-    g_i2c_system_context[id].index_format = index_format;
-    g_i2c_system_context[id].data = 0;
-    g_i2c_system_context[id].databuffer = p_data;
-    g_i2c_system_context[id].count_data = count;
-    g_i2c_system_context[id].operation = I2C_READ;
-    g_i2c_system_context[id].active_event = I2C_NO_EVENT;
-    g_i2c_system_context[id].transfer_data = 0;
-    g_i2c_system_context[id].multi_operation = TRUE;
-
-    /* Disable all the interrupts to remove previously garbage interrupts */
-    switch(id)
-    {
-    	case I2C0 :
-    	     I2C_DisableIRQSrc(I2C0_IRQ_SRC_ALL);
-    	     break;
-    	case I2C1 :
-    	     I2C_DisableIRQSrc(I2C1_IRQ_SRC_ALL);
-    	     break;    	
-    	case I2C2:
-    	     I2C_DisableIRQSrc(I2C2_IRQ_SRC_ALL);    	
-    	     break;
-    	case I2C3:
-    	     I2C_DisableIRQSrc(I2C3_IRQ_SRC_ALL);    	     
-    	     break;
-    	default:
-    	     break;
-    }
-/*    I2C_DisableIRQSrc((I2C0 == id) ? I2C0_IRQ_SRC_ALL : I2C1_IRQ_SRC_ALL);*/
-
-    /* Check if I2C controller is Master */
-    if (I2C_BUS_MASTER_MODE == g_i2c_system_context[id].bus_control_mode)
-    {
-
-        /*  start byte procedure configuration */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_SB, I2C_MCR_SHIFT_SB, (u32) g_i2c_system_context[id].start_byte_procedure);
-
-        /* Check the General call handling */
-        if (g_i2c_system_context[id].general_call_mode_handling != I2C_NO_GENERAL_CALL_HANDLING)
-        {
-            /* The Transaction is intiated by a general call command */
-            I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 0);
-        }
-        else
-        {
-            /* Check if Slave address is 10 bit */
-            if (g_i2c_system_context[id].slave_address < 1024 && g_i2c_system_context[id].slave_address > 127)
-            {
-                /* Set the Address mode to 10 bit */
-                I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 2);
-            }
-            else
-            {
-                /* Set the Address mode to 7 bit */
-                I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 1);
-            }
-        }
-
-        /* Store the HS master code */
-        if (I2C_FREQ_MODE_HIGH_SPEED == g_i2c_system_context[id].mode)
-        {
-            p_i2c_registers->hsmcr = g_i2c_system_context[id].high_speed_master_code;
-        }
-
-        /* Store  the Slave addres in the Master control register */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_A10, I2C_MCR_SHIFT_A10, slave_address);
-
-        if
-        (
-            (g_i2c_system_context[id].slave_address < 1024 && g_i2c_system_context[id].slave_address > 127)
-        &&  (I2C_NO_INDEX == index_format)
-        )
-        {
-            /* Set the Master write operation */
-            I2C_CLR_BIT(mcr, I2C_MCR_OP);
-
-            I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 0);
-
-            /* Current transaction is not terminated by STOP condition,
-                 a repeated start operation will be fallowed */
-            I2C_CLR_BIT(mcr, I2C_MCR_STOP);
-
-            /*Write MCR register   */
-            p_i2c_registers->mcr = mcr;
-
-            /* Enable the I2C controller */
-            I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
-        }
-        else
-        {
-            /* Master control configuration  */
-            if (I2C_NO_INDEX != index_format)
-            {
-                /* Set the Master write operation */
-                I2C_CLR_BIT(mcr, I2C_MCR_OP);
-            }
-            else
-            {
-                /* Set the Master read operation */
-                I2C_SET_BIT(mcr, I2C_MCR_OP);
-            }
-
-            /* Configuring the Frame length */
-            switch (index_format)
-            {
-                case I2C_NO_INDEX:
-                    I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, count);
-
-                    /* Current transaction is terminated by STOP condition */
-                    I2C_SET_BIT(mcr, I2C_MCR_STOP);
-                    break;
-
-                case I2C_BYTE_INDEX:
-                    I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 1);
-
-                    /* Current transaction is not terminated by STOP condition,
-	                 a repeated start operation will be fallowed */
-                    I2C_CLR_BIT(mcr, I2C_MCR_STOP);
-                    break;
-
-                case I2C_HALF_WORD_LITTLE_ENDIAN:
-                case I2C_HALF_WORD_BIG_ENDIAN:
-                    I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 2);
-
-                    /* Current transaction is not terminated by STOP condition,
-	                a repeated start operation will be fallowed */
-                    I2C_CLR_BIT(mcr, I2C_MCR_STOP);
-                    break;
-
-                default:
-                    break;
-            }
-
-            /*Write MCR register   */
-            p_i2c_registers->mcr = mcr;
-
-        }
-    }
-
-    switch (g_i2c_system_context[id].index_transfer_mode)
-    {
-        case I2C_TRANSFER_MODE_POLLING:
-            /*  	
-          Index Transfer
-          */
-            if (I2C_BUS_SLAVE_MODE == g_i2c_system_context[id].bus_control_mode)
-            {
-                error_status = i2cp_SlaveIndexReceive(id);
-                if (I2C_OK != error_status)
-                {
-                    return(error_status);
-                }
-            }
-            else
-            {
-                error_status = i2cp_MasterIndexTransmit(id);
-                if (I2C_OK != error_status)
-                {
-                    return(error_status);
-                }
-            }
-
-            /*
-          Data Transfer
-          */
-            switch (g_i2c_system_context[id].data_transfer_mode)
-            {
-                case I2C_TRANSFER_MODE_POLLING:
-                    error_status = i2cp_ReceiveDataPolling(id, g_i2c_system_context[id].databuffer);
-                    if (I2C_OK != error_status)
-                    {
-                        return(error_status);
-                    }
-                    break;
-
-                case I2C_TRANSFER_MODE_INTERRUPT:
-                case I2C_TRANSFER_MODE_DMA:
-                default:
-                    break;
-            }
-            break;
-
-        case I2C_TRANSFER_MODE_INTERRUPT:
-        case I2C_TRANSFER_MODE_DMA:
-        default:
-            return(I2C_INVALID_PARAMETER);
-    }
-
-    return(error_status);
-
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_Cancel                  			                */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	This routine is used to cancel the current transfer     */
-/*                  operation, if any.                                      */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller to be canceled  		*/
-/*     InOut    :  None                                                     */
-/* 			: t_i2c_active_event: It will contain the result of         */
-/*                                    the operation                         */
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*                I2C_OK                 if it is ok                        */
-/*                I2C_INVALID_PARAMETER  if input parameters are not valid  */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_i2c_error I2C_Cancel(t_i2c_device_id id, t_i2c_active_event *event)   /*Only IT mode*/
-{
-    /* Check if parameters are valid.*/
-    if ((NULL == event) || ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id)))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-    if (I2C_NO_EVENT == g_i2c_system_context[id].active_event)
-    {
-        event->type = I2C_NO_EVENT;
-        event->transfer_data = 0;
-        event->id = id;
-    }
-    else
-    {
-        event->type = I2C_CANCEL_EVENT;
-        event->transfer_data = g_i2c_system_context[id].transfer_data;
-        event->id = id;
-        g_i2c_system_context[id].active_event = I2C_CANCEL_EVENT;
-    }
-
-    i2cp_Abort(id);
-
-    /*Set the I2C operation to No operation  */
-    g_i2c_system_context[id].operation = (t_i2c_operation) I2C_NO_OPERATION;
-
-    return(I2C_OK);
-}
-
-
-
-/****************************************************************************/
-/* NAME			:	I2C_IsEventActive                  		                */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	:This routine is used to determine if the given event       */
-/*                is still active.                                          */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_active_event: the Event to be checked      		    */
-/*     InOut    : None                                                      */
-/* 			: None                                                      */
-/*                                                                          */
-/* RETURN		:	t_bool  :   TRUE or FALSE               	            */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
- t_bool I2C_IsEventActive(t_i2c_active_event *event)
-{
-    if (event->type == g_i2c_system_context[event->id].active_event)
-    {
-        return(TRUE);
-    }
-    else
-    {
-        return(FALSE);
-    }
-}
-
-
-/****************************************************************************/
-/* NAME			:	I2C_Reset                           		            */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	:Reset the I2C Registers for given I2C controller           */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The ID of the controller for reset    */
-/*     InOut    :  None                                                     */
-/* 			:  None														*/
-/*                                                                          */
-/* RETURN		:	t_i2c_error  									    	*/
-/*                  return  I2C_INVALID_PARAMETER  if id is not correct     */
-/*                  else    I2C_OK                                          */
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-/****************************************************************************/
-t_i2c_error I2C_Reset(t_i2c_device_id id)
-{
-    t_i2c_registers *p_i2c_registers;
-
-    /* Check if parameters are valid.*/
-    if ((I2C0 != id) && (I2C1 != id) && (I2C2 != id) && (I2C3 != id))
-    {
-        return(I2C_INVALID_PARAMETER);
-    }
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-
-    /* Clear registers.*/
-    I2C_WRITE_REG(p_i2c_registers->cr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->scr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->hsmcr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->mcr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->tftr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->rftr, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->dmar, I2C_CLEAR);
-    I2C_WRITE_REG(p_i2c_registers->icr, 0x31F0008);
-    I2C_WRITE_REG(p_i2c_registers->imscr,I2C_CLEAR);
-
-    I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_FTX);       /* Flush the Tx Fifo */
-    I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_FRX);       /* Flush the Rx Fifo */
-
-    /*
-	Initialize the right structure to default state 
-	*/
-    g_i2c_system_context[id].freq_scl = 0;
-    g_i2c_system_context[id].freq_input = 0;
-    g_i2c_system_context[id].mode = I2C_FREQ_MODE_STANDARD;
-    g_i2c_system_context[id].own_address = 0;
-    g_i2c_system_context[id].enabled = FALSE;
-    g_i2c_system_context[id].slave_address = 0;
-    g_i2c_system_context[id].status = I2C_STATUS_SLAVE_MODE;
-    g_i2c_system_context[id].data = 0;
-    g_i2c_system_context[id].databuffer = NULL;
-    g_i2c_system_context[id].count_data = 0;
-    g_i2c_system_context[id].register_index = 0;
-    g_i2c_system_context[id].operation = (t_i2c_operation) I2C_NO_OPERATION;
-    g_i2c_system_context[id].active_event = I2C_NO_EVENT;
-    g_i2c_system_context[id].transfer_data = 0;
-    g_i2c_system_context[id].multi_operation = FALSE;
-
-    /*   g_i2c_system_context[id].i2c_device_context... to be initialized*/
-    g_i2c_system_context[id].digital_filter_control = I2C_DIGITAL_FILTERS_OFF;
-    g_i2c_system_context[id].dma_sync_logic_control = I2C_DISABLE;
-    g_i2c_system_context[id].start_byte_procedure = I2C_DISABLE;
-    g_i2c_system_context[id].slave_data_setup_time = 0; /* TBD */
-    g_i2c_system_context[id].high_speed_master_code = 0;
-    g_i2c_system_context[id].bus_control_mode = I2C_BUS_SLAVE_MODE;
-    g_i2c_system_context[id].i2c_loopback_mode = I2C_DISABLE;
-    g_i2c_system_context[id].general_call_mode_handling = I2C_NO_GENERAL_CALL_HANDLING;
-
-    g_i2c_system_context[id].index_transfer_mode = I2C_TRANSFER_MODE_POLLING;
-    g_i2c_system_context[id].data_transfer_mode = I2C_TRANSFER_MODE_POLLING;
-    g_i2c_system_context[id].i2c_transmit_interrupt_threshold = 1;
-    g_i2c_system_context[id].i2c_receive_interrupt_threshold = 1;
-    g_i2c_system_context[id].transmit_burst_length = 0;
-    g_i2c_system_context[id].receive_burst_length = 0;
-    g_i2c_system_context[id].index_format = I2C_NO_INDEX;
-    g_i2c_system_context[id].current_bus_config = I2C_CURRENT_BUS_SLAVE_TRANSMITTER;
-    g_i2c_system_context[id].std =FALSE;
-
-    return(I2C_OK);
-}
-
-/*-----------------------------------------------------------------------------
-		Private functions
------------------------------------------------------------------------------*/
-
-/*-----------------------------------------------------------------------------
-Name		: i2cp_SetOwnAddress
-Description	: Set the I2C bus address of the controller.
-In			: t_i2c_registers* p_i2c_registers	: pointer to the controller's 
-											  registers.
-			  u32 address				: the slave address of the 
-			  								  controller.
-InOut		: None
-Out			: None
-Return value: Always ok / I2C_SLAVE_ADDRESS_NOT_VALID
-Type		: Private
-Comments	: 
-
-
-	In all cases, the bits are not cleared when the interface is disabled 
-	(PE = 0b).
------------------------------------------------------------------------------*/
- t_i2c_error i2cp_SetOwnAddress(t_i2c_device_id id, u16 address)
-{
-    t_i2c_registers *p_i2c_registers;
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    /* CM: check if the given address is valid ???*/
-    if (!i2cp_AddressIsValid(address))
-    {
-        return(I2C_SLAVE_ADDRESS_NOT_VALID);
-    }
-
-    if (address < 1024 && address > 127)
-    {
-        /* Set Slave address mode to 10 bit addressing mode */
-        I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_SAM);
-        I2C_WRITE_FIELD(p_i2c_registers->scr, I2C_SCR_ADDR, I2C_SCR_SHIFT_ADDR, address);
-    }
-    else
-    {
-        /* Set the slave address mode to 7 bit addressing mode */
-        I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_SAM);
-        I2C_WRITE_FIELD(p_i2c_registers->scr, I2C_SCR_ADDR, I2C_SCR_SHIFT_ADDR, address);
-    }
-
-    return(I2C_OK);
-}
-
-/*-----------------------------------------------------------------------------
-Name		: i2cp_SetBusClock
-Description	: Set the I2C bus clock for the given controller.
-In			: t_i2c_registers* p_i2c_registers	: pointer to the controller's 
-											  registers.
-			  u32 freq_scl					: the I2C bus frequency freq_scl (Hz).
-			  u32 freq_input					: the input clock frequency (Hz).
-InOut		: None
-Out			: None
-Return value: I2C_OK						: no error.
-			  I2C_INVALID_PARAMETER			: wrong id parameter.
-			  I2C_freq_scl_NOT_SUPPORTED		: freq_scl is not supported.
-Type		: Private
-Comments	: The freq_input parameter is only necessary to calculate the I2C bus 
-			  frequency and is not used for other purposes.
-			  It is not necessary to save the freq_scl as it has been already 
-			  saved by I2C_Config().
-	
------------------------------------------------------------------------------*/
-t_i2c_error i2cp_SetBusClock(t_i2c_device_id id, u32 freq_scl, u32 freq_input)
-{
-
-
-    /* To be defined */
-    u32        value;
-    t_i2c_registers *p_i2c_registers;
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    /* Standard mode */
-    if (freq_scl <= (u32) I2C_MAX_STANDARD_SCL)
-    {
-        value = (u32) (freq_input / (freq_scl * 2));
-
-        /*Set the Standard mode in the control register  */
-        I2C_WRITE_FIELD(p_i2c_registers->cr, I2C_CR_SM, I2C_CR_SHIFT_SM, 0x0);
-
-        /* set the Baud rate counter 2 value  */
-        I2C_WRITE_FIELD(p_i2c_registers->brcr, I2C_BRCR_BRCNT2, I2C_BRCR_SHIFT_BRCNT2, value);
-
-        /* Make ensure that BRCNT value set to be zero */
-        I2C_WRITE_FIELD(p_i2c_registers->brcr, I2C_BRCR_BRCNT1, I2C_BRCR_SHIFT_BRCNT1, 0);
-
-        /*Update the Frequency mode in the global strcture */
-        g_i2c_system_context[id].mode = I2C_FREQ_MODE_STANDARD;
-    }
-    else    /* Fast Mode */
-    if (freq_scl <= (u32) I2C_MAX_FAST_SCL)
-    {
-        value = (u32) (freq_input / ((freq_scl * 3) / 2));
-
-        /*Set the Fast mode in the control register  */
-        I2C_WRITE_FIELD(p_i2c_registers->cr, I2C_CR_SM, I2C_CR_SHIFT_SM, 0x1);
-
-        /* set the Baud rate counter 2 value  */
-        I2C_WRITE_FIELD(p_i2c_registers->brcr, I2C_BRCR_BRCNT2, I2C_BRCR_SHIFT_BRCNT2, value);
-
-        /* Make ensure that BRCNT value set to be zero */
-        I2C_WRITE_FIELD(p_i2c_registers->brcr, I2C_BRCR_BRCNT1, I2C_BRCR_SHIFT_BRCNT1, 0);
-
-        /*Update the Frequency mode in the global strcture */
-        g_i2c_system_context[id].mode = I2C_FREQ_MODE_FAST;
-    }
-    else    /* High Speed Mode  */
-    if (freq_scl <= (u32) I2C_MAX_HIGH_SPEED_SCL)
-    {
-        value = (u32) (freq_input / ((freq_scl * 3) / 2));
-
-        /*Set the High speed mode in the control register  */
-        I2C_WRITE_FIELD(p_i2c_registers->cr, I2C_CR_SM, I2C_CR_SHIFT_SM, 0x2);
-
-        /* set the Baud rate counter 1 value  */
-        I2C_WRITE_FIELD(p_i2c_registers->brcr, I2C_BRCR_BRCNT1, I2C_BRCR_SHIFT_BRCNT1, value);
-
-        /*Update the Frequency mode in the global strcture */
-        g_i2c_system_context[id].mode = I2C_FREQ_MODE_HIGH_SPEED;
-    }
-    else
-    {
-        return(I2C_LINE_FREQ_NOT_SUPPORTED);
-    }
-
-    return(I2C_OK);
-
-}
-
-/*-----------------------------------------------------------------------------
-Name		: i2cp_AddressIsValid
-Description	: Check if the given address is valid.
-In			: u16 address	: the slave address to be checked.
-InOut		: None
-Out			: None
-Return value: TRUE				: address is valid.
-			  FALSE				: address is not valid.
-Type		: Private
-Comments	: Note that the least-significant bit of the address parameter 
-			  is not relevant for the addressing of the slave device, for 
-			  example 0xE2 and 0xE3 will address the same slave device.
-
-
-  Reserved addresses:
-	SLAVE ADDRESS	R/W BIT		RANGE		DESCRIPTION
-	0000 000		0			0			General call address
-	0000 000		1			1			START byte(1)
-	0000 001		X			2-3			CBUS address(2)
-	0000 010		X			4-5			Reserved for different bus format(3)
-	0000 011		X			6-7			Reserved for future purposes
-	0000 1XX		X			8-15		Hs-mode master code
-	
-	1111 1XX		X			248-255		Reserved for future purposes
-	1111 0XX		X			240-247		10-bit slave addressing
-
-  Note that with 7-bit address:
-	0000xxxx and 1111xxxx are reserved.  
------------------------------------------------------------------------------*/
-t_bool i2cp_AddressIsValid(u16 address)
-{
-    /* Check if more than 10 bits are needed.*/
-    if (address > 1023)
-    {
-        return(FALSE);
-    }
-
-    /* 7-bit address. LSB is not considered.*/
-    /*Address 0x4 is enabled to support the ST Pepper pot camera  */
-    if (address < 128 && !(address == 0 || address == 0x4))
-    {
-        if ((address < 8) || (address > 119))
-        {
-            return(FALSE);
-        }
-    }
-
-    /* CM: add here the 10-bit check.*/
-    return(TRUE);
-}
-
-
-/*-----------------------------------------------------------------------------
-Name		: i2cp_Abort
-Description	: Abort the current transfer operation of the given controller.
-In			: t_i2c_device_id id	: the controller to be aborted.
-InOut		: None
-Out			: None
-Return		: I2C_OK		: always no error.
-Type		: Private
-Comments	: This is called when an unexpected event happens (internal error). 
------------------------------------------------------------------------------*/
-void i2cp_Abort(t_i2c_device_id id)
-{
-
-    t_i2c_registers *p_i2c_registers;
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-    /*Disable the interrupts  */
-    I2C_WRITE_REG(p_i2c_registers->imscr,I2C_CLEAR);
-
-    /*Disable the Controller  */
-    I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    /*Enable the controller */
-    I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
-
-    /*Set the I2C operation to No operation  */
-    g_i2c_system_context[id].operation = (t_i2c_operation) I2C_NO_OPERATION;
-}
-
-
-
-/*-----------------------------------------------------------------------------
-Name		: i2cp_SlaveIndexReceive
-Description	: 
-In			: t_i2c_id              : I2C Controller id 
-
-InOut		: None
-Out			: t_i2c_error          error status
-Return value: I2C_OK						: no error
-			  I2C_INVALID_PARAMETER			: wrong id parameter.
-
-Type		: Private
-Comments	: This  function  perform the operations, when 
-              I2C controller addressed as a slave 
------------------------------------------------------------------------------*/
-t_i2c_error i2cp_SlaveIndexReceive(t_i2c_device_id id)
-{
-
-    u32        loop_counter = 0;
-    t_i2c_registers *p_i2c_registers;
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    if (I2C_WRITE == g_i2c_system_context[id].operation)
-    {
-        /* SLAVE TRANSMITTER  */
-        /* Waiting for the Read from slave request */
-        loop_counter = 0;
-        while
-        (
-            (!I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_RFSR, I2C_INT_SHIFT_RFSR))
-        &&  loop_counter < I2C_ENDAD_COUNTER
-        )
-        {
-            loop_counter++;
-        };
-        if (loop_counter >= I2C_ENDAD_COUNTER)
-        {
-            i2cp_Abort(id);
-            return(I2C_ADDRESS_MATCH_FAILED);
-        }
-
-        /* Acknowledge the Read from slave request */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_RFSR);
-
-        /* Read from slave request recieved */
-        /* Flush the Tx Fifo */
-        I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_FTX);
-
-        /*Wait till for the Tx Flush bit to reset */
-        loop_counter = 0;
-        while
-        (
-            I2C_READ_FIELD(p_i2c_registers->cr, I2C_CR_FTX, I2C_CR_SHIFT_FTX)
-        &&  loop_counter < I2C_FIFO_FLUSH_COUNTER
-        )
-        {
-            loop_counter++;
-        };
-        if (loop_counter >= I2C_FIFO_FLUSH_COUNTER)
-        {
-            return(I2C_HW_FAILED);
-        }
-
-        /* update the status */
-        g_i2c_system_context[id].status = I2C_STATUS_SLAVE_TRANSMITTER_MODE;
-        g_i2c_system_context[id].active_event = I2C_READ_FROM_SLAVE_REQUEST_EVENT;
-        g_i2c_system_context[id].current_bus_config = I2C_CURRENT_BUS_SLAVE_TRANSMITTER;
-
-    }
-    else
-    {
-        /* SLAVE RECEIVER  */
-        /* Waiting for the Write to slave request */
-        loop_counter = 0;
-        while
-        (
-            (!I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_WTSR, I2C_INT_SHIFT_WTSR))
-        &&  loop_counter < I2C_ENDAD_COUNTER
-        )
-        {
-            loop_counter++;
-        };
-        if (loop_counter >= I2C_ENDAD_COUNTER)
-        {
-            i2cp_Abort(id);
-            return(I2C_ADDRESS_MATCH_FAILED);
-        }
-
-        /* Acknowledge the Write to slave request */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_WTSR);
-
-        /* update the status */
-        g_i2c_system_context[id].status = I2C_STATUS_SLAVE_TRANSMITTER_MODE;
-        g_i2c_system_context[id].active_event = I2C_WRITE_TO_SLAVE_REQUEST_EVENT;
-        g_i2c_system_context[id].current_bus_config = I2C_CURRENT_BUS_SLAVE_RECEIVER;
-
-    }
-
-    /* Update the status of the I2C controller  */
-    if (I2C_READ == g_i2c_system_context[id].operation)
-    {
-        g_i2c_system_context[id].status = I2C_STATUS_SLAVE_RECEIVER_MODE;
-    }
-    else
-    {
-        g_i2c_system_context[id].status = I2C_STATUS_SLAVE_TRANSMITTER_MODE;
-    }
-
-    return(I2C_OK);
-}
-
-/*-----------------------------------------------------------------------------
-Name		: i2cp_TransmitDataPolling
-Description	: Transmit the data in the polling mode 
-In			: t_i2c_id              : I2C Controller id 
-
-InOut		: None
-Out			: t_i2c_error          error status
-Return value: I2C_OK						: no error
-			  I2C_INVALID_PARAMETER			: wrong id parameter.
-
-Type		: Private
-Comments	: 
------------------------------------------------------------------------------*/
-t_i2c_error i2cp_TransmitDataPolling(t_i2c_device_id id, volatile u8 *p_data)
-{
-
-    u32        loop_counter = 0;
-    t_i2c_registers *p_i2c_registers;
-    t_i2c_error     error_status;
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    if (I2C_BUS_SLAVE_MODE == g_i2c_system_context[id].bus_control_mode)
-    {
-        /* Slave tranmitter */
-        while (g_i2c_system_context[id].count_data != 0)
-        {
-            /* Check for Tx Fifo not full */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_TXFF, I2C_INT_SHIFT_TXFF)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_ENDAD_COUNTER)
-            {
-                error_status = i2cp_GetAbortCause(id);
-                if (error_status != I2C_OK)
-                {
-                    return(error_status);
-                }
-                else
-                {
-                    return(I2C_TRANSMIT_FIFO_FULL);
-                }
-            }
-
-            p_i2c_registers->tfr = *p_data;
-
-            g_i2c_system_context[id].transfer_data++;
-            g_i2c_system_context[id].count_data--;
-            p_data++;
-            g_i2c_system_context[id].active_event = I2C_DATA_TX_EVENT;
-        }
-
-        /* End of Data transfer */
-        /* Check for the Slave tranaction done */
-        loop_counter = 0;
-        while
-        (
-            !I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_STD, I2C_INT_SHIFT_STD)
-        &&  loop_counter < I2C_ENDAD_COUNTER
-        )
-        {
-            loop_counter++;
-        };
-        if (loop_counter >= I2C_ENDAD_COUNTER)
-        {
-            error_status = i2cp_GetAbortCause(id);
-            if (error_status != I2C_OK)
-            {
-                return(error_status);
-            }
-            else
-            {
-                i2cp_Abort(id);
-            }
-
-            return(I2C_INTERNAL_ERROR);
-        }
-
-        /* Slave Transaction has been done */
-        /* Acknowledge the Slave Transaction done */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_STD);
-
-        g_i2c_system_context[id].active_event = I2C_TRANSFER_OK_EVENT;
-        return(I2C_OK);
-    }
-    else
-    {
-        /* Master Transmitter */
-        while (g_i2c_system_context[id].count_data != 0)
-        {
-            /* Check for Tx Fifo not full */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_TXFF, I2C_INT_SHIFT_TXFF)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_ENDAD_COUNTER)
-            {
-                error_status = i2cp_GetAbortCause(id);
-                if (error_status != I2C_OK)
-                {
-                    return(error_status);
-                }
-                else
-                {
-                    i2cp_Abort(id);
-                }
-
-                return(I2C_TRANSMIT_FIFO_FULL);
-            }
-
-            p_i2c_registers->tfr = *p_data;
-
-            g_i2c_system_context[id].transfer_data++;
-            g_i2c_system_context[id].count_data--;
-            p_data++;
-            g_i2c_system_context[id].active_event = I2C_DATA_TX_EVENT;
-        }
-
-        /* End of Data transfer */
-        
-        loop_counter = 0;
-        /* Check whether the Stop bit has been programmed or not */
-        if(I2C_READ_FIELD(p_i2c_registers->mcr, I2C_MCR_STOP, I2C_MCR_SHIFT_STOP))
-        {
-            /* Check for the Master transaction Done */
-            while
-            (
-                !I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_MTD, I2C_INT_SHIFT_MTD)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-        }
-        else
-        {
-        	/* Check for the Master transaction Done Without Stop */
-            while
-            (
-                !I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_MTDWS, I2C_INT_SHIFT_MTDWS)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-        }
-        
-        
-        if (loop_counter >= I2C_ENDAD_COUNTER)
-        {
-            error_status = i2cp_GetAbortCause(id);
-            if (error_status != I2C_OK)
-            {
-                i2cp_Abort(id);
-                return(error_status);
-            }
-            else
-            {
-                i2cp_Abort(id);
-            }
-
-            return(I2C_INTERNAL_ERROR);
-        }
-        
-        
-        /* Master Transaction has been done */
-        /* Acknowledge the Master Transaction Done */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTD);
-        
-        /* Master Transaction Without Stop has been done */
-        /* Acknowledge the Master Transaction Done Without Stop */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTDWS);
-        
-        
-        g_i2c_system_context[id].active_event = I2C_TRANSFER_OK_EVENT;
-
-        g_i2c_system_context[id].operation = I2C_NO_OPERATION;
-        return(I2C_OK);
-    }
-
-}
-
-/*-----------------------------------------------------------------------------
-Name		: i2cp_ReceiveDataPolling
-Description	: Receiving the data in polling mode
-In			: t_i2c_id              : I2C Controller id 
-
-InOut		: None
-Out			: t_i2c_error          error status
-Return value: I2C_OK						: no error
-			  I2C_WRONG_PARAMETER			: wrong id parameter.
-
-Type		: Private
-Comments	: 
------------------------------------------------------------------------------*/
-t_i2c_error i2cp_ReceiveDataPolling(t_i2c_device_id id, u8 *p_data)
-{
-
-    u32        loop_counter = 0;
-    t_i2c_error     error_status;
-    t_i2c_registers *p_i2c_registers;
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    if (I2C_BUS_SLAVE_MODE == g_i2c_system_context[id].bus_control_mode)
-    {
-        /* Slave Receiver */
-        while (g_i2c_system_context[id].count_data != 0)
-        {
-            /* Wait for the Rx Fifo  empty */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_RXFE, I2C_INT_SHIFT_RXFE)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_ENDAD_COUNTER)
-            {
-                error_status = i2cp_GetAbortCause(id);
-                if (error_status != I2C_OK)
-                {
-                    return(error_status);
-                }
-                else
-                {
-                    i2cp_Abort(id);
-                }
-
-                return(I2C_RECEIVE_FIFO_EMPTY);
-            }
-
-            /* Read the data byte from Rx Fifo */
-            *p_data = (u8) p_i2c_registers->rfr;
-
-            g_i2c_system_context[id].transfer_data++;
-            g_i2c_system_context[id].count_data--;
-            p_data++;
-            g_i2c_system_context[id].active_event = I2C_DATA_RX_EVENT;
-        }   /* Data Reception has been completed */
-
-        /* Check for the slave transaction done */
-        loop_counter = 0;
-        while
-        (
-            !I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_STD, I2C_INT_SHIFT_STD)
-        &&  loop_counter < I2C_ENDAD_COUNTER
-        )
-        {
-            loop_counter++;
-        };
-        if (loop_counter >= I2C_ENDAD_COUNTER)
-        {
-            error_status = i2cp_GetAbortCause(id);
-            if (error_status != I2C_OK)
-            {
-                return(error_status);
-            }
-            else
-            {
-                i2cp_Abort(id);
-            }
-
-            return(I2C_INTERNAL_ERROR);
-        }
-
-        /* Slave Transaction has been done */
-        /* Acknowledge the Slave Transaction Done */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_STD);
-        g_i2c_system_context[id].active_event = I2C_TRANSFER_OK_EVENT;
-        g_i2c_system_context[id].operation = I2C_NO_OPERATION;
-
-        return(I2C_OK);
-    }
-    else
-    {
-        /* Master Receiver */
-        while (g_i2c_system_context[id].count_data != 0)
-        {
-            /* Wait for the Rx Fifo  empty */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_RXFE, I2C_INT_SHIFT_RXFE)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_ENDAD_COUNTER)
-            {
-                error_status = i2cp_GetAbortCause(id);
-                if (error_status != I2C_OK)
-                {
-                    return(error_status);
-                }
-                else
-                {
-                    i2cp_Abort(id);
-                }
-
-                return(I2C_RECEIVE_FIFO_EMPTY);
-            }
-
-            /* Read the data byte from Rx Fifo */
-            *p_data = (u8) p_i2c_registers->rfr;
-
-            g_i2c_system_context[id].transfer_data++;
-            g_i2c_system_context[id].count_data--;
-            p_data++;
-            g_i2c_system_context[id].active_event = I2C_DATA_RX_EVENT;
-        }   /* Data reception  has been completed */
-
-        loop_counter = 0;
-        /* Check whether the Stop bit has been programmed or not */
-        if(I2C_READ_FIELD(p_i2c_registers->mcr, I2C_MCR_STOP, I2C_MCR_SHIFT_STOP))
-        {
-            /* Check for the Master transaction Done */
-            while
-            (
-                !I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_MTD, I2C_INT_SHIFT_MTD)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-        }
-        else
-        {
-        	/* Check for the Master transaction Done Without Stop */
-            while
-            (
-                !I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_MTDWS, I2C_INT_SHIFT_MTDWS)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-        }
-        if (loop_counter >= I2C_ENDAD_COUNTER)
-        {
-            error_status = i2cp_GetAbortCause(id);
-            if (error_status != I2C_OK)
-            {
-                return(error_status);
-            }
-            else
-            {
-                i2cp_Abort(id);
-            }
-
-            return(I2C_INTERNAL_ERROR);
-        }
-
-        /* Master Transaction has been done */
-        /* Acknowledge the Master Transaction Done */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTD);
-        
-        /* Master Transaction Without Stop has been done */
-        /* Acknowledge the Master Transaction Done Without Stop */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTDWS);
-
-        g_i2c_system_context[id].active_event = I2C_TRANSFER_OK_EVENT;
-        g_i2c_system_context[id].operation = I2C_NO_OPERATION;
-
-    }
-
-    return(I2C_OK);
-}
-
-/*-----------------------------------------------------------------------------
-Name		: i2cp_MasterIndexTransmit
-Description	: Transmits the index to slave 
-In			: t_i2c_id              : I2C Controller id 
-
-InOut		: None
-Out			: t_i2c_error          error status
-Return value: I2C_OK						: no error
-			  I2C_WRONG_PARAMETER			: wrong id parameter.
-
-Type		: Private
-Comments	: 
------------------------------------------------------------------------------*/
-t_i2c_error i2cp_MasterIndexTransmit(t_i2c_device_id id)
-{
-
-    volatile u32   mcr = 0;
-    u32            loop_counter = 0;
-    t_i2c_error         error_status = I2C_OK;
-    t_i2c_registers     *p_i2c_registers;
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    switch (g_i2c_system_context[id].index_format)
-    {
-        case I2C_NO_INDEX:
-            if (g_i2c_system_context[id].slave_address <= 127)
-            {  
-               return(I2C_OK);
-            }
-            
-            break;
-
-        case I2C_BYTE_INDEX:
-            /* Checking for the Tx fifo not full */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_TXFF, I2C_INT_SHIFT_TXFF)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_ENDAD_COUNTER)
-            {
-                error_status = i2cp_GetAbortCause(id);
-                if (error_status != I2C_OK)
-                {
-                    return(error_status);
-                }
-                else
-                {
-                    i2cp_Abort(id);
-                }
-
-                return(I2C_TRANSMIT_FIFO_FULL);
-            }
-
-            p_i2c_registers->tfr = (0xFF & g_i2c_system_context[id].register_index);
-
-            g_i2c_system_context[id].active_event = I2C_INDEX_TX_EVENT;
-            g_i2c_system_context[id].index_format = I2C_NO_INDEX;
-
-            break;
-
-        case I2C_HALF_WORD_LITTLE_ENDIAN:
-            /* Checking for the Tx Fifo not full  */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_TXFF, I2C_INT_SHIFT_TXFF)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_ENDAD_COUNTER)
-            {
-                error_status = i2cp_GetAbortCause(id);
-                if (error_status != I2C_OK)
-                {
-                    return(error_status);
-                }
-                else
-                {
-                    i2cp_Abort(id);
-                }
-
-                return(I2C_TRANSMIT_FIFO_FULL);
-            }
-
-            p_i2c_registers->tfr = (0xFF & (u32) g_i2c_system_context[id].register_index);
-
-            p_i2c_registers->tfr = (g_i2c_system_context[id].register_index >> 8);
-
-            g_i2c_system_context[id].index_format = I2C_NO_INDEX;
-            g_i2c_system_context[id].active_event = I2C_INDEX_TX_EVENT;
-            break;
-
-        case I2C_HALF_WORD_BIG_ENDIAN:
-            /* Cheking for the Tx Fifo full */
-            loop_counter = 0;
-            while
-            (
-                I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_TXFF, I2C_INT_SHIFT_TXFF)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-            if (loop_counter >= I2C_ENDAD_COUNTER)
-            {
-                error_status = i2cp_GetAbortCause(id);
-                if (error_status != I2C_OK)
-                {
-                    return(error_status);
-                }
-                else
-                {
-                    i2cp_Abort(id);
-                }
-
-                return(I2C_TRANSMIT_FIFO_FULL);
-            }
-
-            p_i2c_registers->tfr = (g_i2c_system_context[id].register_index >> 8);
-
-            p_i2c_registers->tfr = (0xFF & g_i2c_system_context[id].register_index);
-
-            g_i2c_system_context[id].index_format = I2C_NO_INDEX;
-            g_i2c_system_context[id].active_event = I2C_INDEX_TX_EVENT;
-            break;
-
-        default:
-            break;
-    }
-
-    if (g_i2c_system_context[id].operation == I2C_READ)
-    {
-        loop_counter = 0;
-        /* Check whether the Stop bit has been programmed or not */
-        if(I2C_READ_FIELD(p_i2c_registers->mcr, I2C_MCR_STOP, I2C_MCR_SHIFT_STOP))
-        {
-            /* Check for the Master transaction Done */
-            while
-            (
-                !I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_MTD, I2C_INT_SHIFT_MTD)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-        }
-        else
-        {
-        	/* Check for the Master transaction Done Without Stop */
-            while
-            (
-                !I2C_READ_FIELD(p_i2c_registers->risr, I2C_INT_MTDWS, I2C_INT_SHIFT_MTDWS)
-            &&  loop_counter < I2C_ENDAD_COUNTER
-            )
-            {
-                loop_counter++;
-            };
-        }
-        
-        if (loop_counter >= I2C_ENDAD_COUNTER)
-        {
-            error_status = i2cp_GetAbortCause(id);
-            if (error_status != I2C_OK)
-            {
-                return(error_status);
-            }
-            else
-            {
-                i2cp_Abort(id);
-            }
-
-            return(I2C_INTERNAL_ERROR);
-        }
-
-        /* Master Transaction has been done */
-        /* Acknowledge the Master Transaction Done */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTD);
-        
-        /* Master Transaction Without Stop has been done */
-        /* Acknowledge the Master Transaction Done Without Stop */
-        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTDWS);
-
-        /* Master control configuration for read operation  */
-        I2C_SET_BIT(mcr, I2C_MCR_OP);
-
-        /* Configure the STOP condition*/
-        I2C_SET_BIT(mcr, I2C_MCR_STOP);
-
-        /* Configuring the Frame length */
-        I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, g_i2c_system_context[id].count_data);
-        
-        I2C_WRITE_FIELD(p_i2c_registers->mcr,I2C_MCR_LENGTH_STOP_OP,I2C_MCR_SHIFT_LENGTH_STOP_OP,mcr);        
-
-    }
-
-    /* added to remove the warning unused variable */
-    error_status = error_status;
-
-    /* Update the status of the I2C controller  */
-    if (I2C_READ == g_i2c_system_context[id].operation)
-    {
-        g_i2c_system_context[id].status = I2C_STATUS_MASTER_RECEIVER_MODE;
-    }
-    else
-    {
-        g_i2c_system_context[id].status = I2C_STATUS_MASTER_TRANSMITTER_MODE;
-    }
-
-    return(I2C_OK);
-}
-
- 
-/* Private function valid for HS controller */
-
-
-/****************************************************************************/
-/* NAME			:	i2cp_GetAbortCause                  		  	        */
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	Get the abort Cause                                     */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		 	: t_i2c_device_id 	: The controller that aborted   		*/
-/*     InOut    :  None                                                     */
-/* 			: None                                                     */
-/*                                                                          */
-/* RETURN		:	t_i2c_error										    	*/
-/*--------------------------------------------------------------------------*/
-/* Type              :                                                */
-/* REENTRANCY 	     :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES :														*/
-
-/****************************************************************************/
-t_i2c_error i2cp_GetAbortCause(t_i2c_device_id id)
-{
-    u8         abort_cause;
-    t_i2c_error     error_status;
-
-    t_i2c_registers *p_i2c_registers;
-
-    p_i2c_registers = (t_i2c_registers *) g_i2c_system_context[id].base_address;
-
-    if (I2C_READ_FIELD(p_i2c_registers->sr, I2C_SR_STATUS, I2C_SR_SHIFT_STATUS) == 3)
-    {
-        abort_cause = (u8) I2C_READ_FIELD(p_i2c_registers->sr, I2C_SR_CAUSE, I2C_SR_SHIFT_CAUSE);
-
-        switch (abort_cause)
-        {
-            case 0:
-                error_status = I2C_ACK_FAIL_ON_ADDRESS;
-                break;
-
-            case 1:
-                error_status = I2C_ACK_FAIL_ON_DATA;
-                break;
-
-            case 2:
-                error_status = I2C_ACK_IN_HS_MODE;
-                break;
-
-            case 3:
-                error_status = I2C_ARBITRATION_LOST;
-                break;
-
-            case 4:
-                error_status = I2C_BUS_ERROR_DETECTED_START;
-                break;
-
-            case 5:
-                error_status = I2C_BUS_ERROR_DETECTED_STOP;
-                break;
-
-            case 6:
-                error_status = I2C_OVERFLOW;
-                break;
-
-            default:
-                error_status = I2C_INTERNAL_ERROR;
-                break;
-        }
-
-        return(error_status);
-    }
-
-    return(I2C_OK);
-}
-
-
-/****************************************************************************/
-/* NAME			:	I2C_SetBaseAddress										*/
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	This routine initializes I2C register base address.     */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		IN  	:	id          : I2C controller id                         */
-/*                    i2c_base_address	:	I2C registers base address		*/
-/* 		OUT 	:	None                                                	*/
-/*                                                                          */
-/* RETURN		:	None											    	*/
-/*--------------------------------------------------------------------------*/
-/* REENTRANCY 	    :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES:														*/
-/*			1)		Global variable gp_registers (register base address)	*/
-/*					is being modified										*/
-
-/****************************************************************************/
-void I2C_SetBaseAddress(t_i2c_device_id id, t_logical_address address)
-{
-    /* Initializing the I2C controller base address */
-    gp_i2c_registers[id] = (t_i2c_registers *) address;
-}
-
-/****************************************************************************/
-/* NAME			:	I2C_DisableIRQSrc										*/
-/*--------------------------------------------------------------------------*/
-/* DESCRIPTION	: 	Disable the given I2C controller to generate interrupts. */
-/*                                                                          */
-/* PARAMETERS	:                                                           */
-/* 		IN  	: t_i2c_irq_src_id id	: the IRQ source to be disabled.	*/
-/* 		OUT 	:	None                                                	*/
-/*                                                                          */
-/* RETURN		:	None											    	*/
-/*--------------------------------------------------------------------------*/
-/* REENTRANCY 	    :	Non Re-entrant                                      */
-/* REENTRANCY ISSUES:														*/
-
-/****************************************************************************/
-void I2C_DisableIRQSrc(t_i2c_irq_src_id irq_id)
-{
-
-    gp_i2c_registers[GETDEVICE((u32)irq_id)]->imscr &= ~((u32)I2C_IRQ_SRC_ALL & (u32)irq_id);
-
-}
diff --git a/board/st/u8500/init_mmc.c b/board/st/u8500/init_mmc.c
index 5ee8190af..b9c22e0bc 100644
--- a/board/st/u8500/init_mmc.c
+++ b/board/st/u8500/init_mmc.c
@@ -26,19 +26,12 @@
 #include "init_mmc.h"
 #include "gpio.h"
 #include "mmc_utils.h"
-#include "i2c.h"
-
-#define HREF_BOARD_ID   ('1')
-#define MOP500_BOARD_ID ('0')
-char  Bootargs_buf[512];
 
 #ifdef CONFIG_CMD_FAT
 #include <part.h>
 #include <fat.h>
 #endif
 
-#define LOOP(x) {int i;for(i=0;i<1000;i++);}
-
 #define MMC_CARD_NUM	1
 #define EMMC_CARD_NUM	4
 enum {
@@ -133,130 +126,6 @@ int mmc_hw_init(void)
     return 1;
 }
 
-#define I2C_SCL_FREQ            100000          /* I2C bus clock frequency.*/
-#define I2C_INPUT_FREQ          48000000        /* Input clock frequency.*/
-#define I2C0_SLAVE_ADDRESS	(0x84 >>1)		/*GPIO expander slave address*/
-#define REG_CHIP_ID_INDEX	0x80
-#define HS_MASTER_CODE 0x01  		/* High speed Master code */
-#define TX_FIFO_THRESHOLD 0x4 /* The threshold below or equal to which the transmit FIFO generates interrupt */
-#define RX_FIFO_THRESHOLD 0x4 /* The threshold above or equal to which the receive FIFO generates interrupt */
-#define BURST_LENGTH 0 /* The burst length used in the DMA operation */
-#define SLAVE_SETUP_TIME 14 /* Slave data setup time */
-
-
-static void config_extended_gpio(void)
-{
-  t_i2c_error    error_status;
-  t_i2c_device_config  i2c_device_config;
-  t_i2c_transfer_config i2c_transfer_config;
-  t_i2c_error error_i2c;
-  u8 read_data = 0;
-  u8 dataArr[2]={0x06,0x06};
-  char board_id = HREF_BOARD_ID;
-
-  I2C_SetBaseAddress(I2C0, CFG_I2C0_BASE);
-  error_i2c = I2C_Init(I2C0, CFG_I2C0_BASE);
- 
-  i2c_device_config.controller_i2c_address = 0;
-  i2c_device_config.input_frequency = I2C_INPUT_FREQ;
-  i2c_device_config.i2c_digital_filter_control = I2C_DIGITAL_FILTERS_OFF;
-  i2c_device_config.i2c_dma_sync_logic_control = I2C_DISABLE;
-  i2c_device_config.i2c_start_byte_procedure = I2C_DISABLE;
-
-  i2c_transfer_config.i2c_transfer_frequency = I2C_SCL_FREQ;
-  i2c_transfer_config.bus_control_mode = I2C_BUS_MASTER_MODE;
- 
-  i2c_transfer_config.i2c_transmit_interrupt_threshold = TX_FIFO_THRESHOLD;
-  i2c_transfer_config.i2c_receive_interrupt_threshold = RX_FIFO_THRESHOLD;
-  i2c_transfer_config.transmit_burst_length = BURST_LENGTH;
-  i2c_transfer_config.receive_burst_length = BURST_LENGTH;
-  i2c_transfer_config.i2c_loopback_mode = I2C_DISABLE;
-  i2c_transfer_config.index_transfer_mode = I2C_TRANSFER_MODE_POLLING;
-  i2c_transfer_config.data_transfer_mode = I2C_TRANSFER_MODE_POLLING;
-  i2c_transfer_config.bus_control_mode = I2C_BUS_MASTER_MODE;
-  i2c_transfer_config.i2c_slave_general_call_mode = I2C_NO_GENERAL_CALL_HANDLING;
- 
-  i2c_device_config.slave_data_setup_time = SLAVE_SETUP_TIME;
- 
-  error_status = I2C_SetDeviceConfiguration(I2C0, &i2c_device_config);
-  if (I2C_OK != error_status)
-  {
-  printf("\n Error in I2C_SetDeviceConfiguration; err = %d", error_status);
-   return;
-  }
- 
-   error_status = I2C_SetTransferConfiguration(I2C0, &i2c_transfer_config);
-  if (I2C_OK != error_status)
-  {
-	printf("\n Error in I2C_SetTransferConfiguration; err = %d", error_status);
-	return;
-  }
-	LOOP(10); 
-	error_status = I2C_ReadSingleData (I2C0, I2C0_SLAVE_ADDRESS, I2C_BYTE_INDEX, 0x80, &read_data);
-	LOOP(1);
-	printf("\nGPIO expander Chip ID %x\n", read_data);
-	
-	if(error_status){
-		printf("\n Error in I2C_ReadSingleData = %d", error_status);
-		return;
-	}
-	if (read_data == 0x01) /*If chip is = 0x1, the platform is MOP500, so config STMPE*/
-	{
-		printf("\nMOP500 platform\n");
-		//config_stmpe();
-	        error_status = I2C_WriteSingleData(I2C0, I2C0_SLAVE_ADDRESS, I2C_BYTE_INDEX, 0x89, 0x0C);
-		LOOP(5);
-		error_status = I2C_WriteSingleData(I2C0, I2C0_SLAVE_ADDRESS, I2C_BYTE_INDEX, 0x83, 0x0C);
-		LOOP(5);
-		board_id = MOP500_BOARD_ID;
-	    }
-	else if(read_data==0x03) /* If chip is = 0x3,the platform is HREF, so config Toshiba controller*/
-		{
-		printf("\nHREF platform\n");	
-		//following code is for HREF 
-		//set the direction of the GPIO KPY9 and KPY10
-		error_status = I2C_WriteSingleData(I2C0, I2C0_SLAVE_ADDRESS, I2C_BYTE_INDEX, 0xC8, 0x06);
-		LOOP(5);
-		dataArr[0]= 0x06;
-		dataArr[1]= 0x06;
-		
- 		error_status = I2C_WriteMultipleData(I2C0, I2C0_SLAVE_ADDRESS, I2C_BYTE_INDEX, 0xC4, dataArr,2);
-		LOOP(5);
-		board_id = HREF_BOARD_ID;
- 		if(error_status)
- 			printf("Error in I2C_WriteMultipleData error = %d",error_status);
-		
- 	}
-	else
-		printf("\nunknown platform: chip ID = %x\n", read_data);
-
-	/* Now modify bootargs to save the board_id, required for automatic platform detection */
-	char * bootargs = getenv("bootargs");
-	if(sizeof(Bootargs_buf) < strlen(bootargs)) {
-		printf("ERROR: Insufficient temp buffer, bootargs not modified");
-		return;
-	}
-	strcpy(Bootargs_buf, bootargs);
-	bootargs = strstr (Bootargs_buf, "board_id=");
-	if(bootargs){
-	/*board_id parameter already present , modify correct value*/
-		bootargs[9] = board_id;
-	}
-	else {
-	 /*board_id parameter not present , append board_id with proper value*/
-		strcat(Bootargs_buf, " board_id=1 ");
-		/*point to the last character of string*/
-		bootargs = Bootargs_buf + strlen(Bootargs_buf) -2;
-		*bootargs = board_id;
-	}
-	/*Now save the new bootargs*/
-	setenv("bootargs", Bootargs_buf);
-	saveenv();
-	//printf("Bootargs after platform detection:\n%s\n", getenv("bootargs"));
-	return;
-}
-
-
 /*
  * mmc_legacy_init - called from commandline mmc init <dev>
  *
@@ -267,7 +136,7 @@ int mmc_legacy_init(int dev)
 {
 
 	if (dev == DEV_EMMC) {
-		printf("EMMC init\n");
+		debug("EMMC init\n");
 		/* XXX: emmc_init() does write the MBR (called pib)! */
 		emmc_init(EMMC_CARD_NUM);
 		emmc_dev.if_type = IF_TYPE_MMC;
@@ -284,12 +153,8 @@ int mmc_legacy_init(int dev)
 		emmc_dev.block_read = emmc_block_read;
 		return 0;
 	} else if (dev == DEV_MMC) {
-		printf("MMC init\n");
-		/* config extended GPIO pins for Level shifter and
-		 * SDMMC_ENABLE */
-		config_extended_gpio();
-		init_mmc();
-		return 0;
+		debug("MMC init\n");
+		return init_mmc();
 	}
 
 	printf("mmc_legacy_init: unsupported device# %d\n", dev);
@@ -321,6 +186,7 @@ static int init_mmc(void)
 
 	if (mmc_hw_init() != 0) {
 		printf("mmc_init: hw init failed\n");
+		return -1;
 	}
 	mmc_dev.if_type = IF_TYPE_MMC;
 	mmc_dev.part_type = PART_TYPE_DOS;
diff --git a/board/st/u8500/u8500.c b/board/st/u8500/u8500.c
index 5be3f5149..83e899434 100644
--- a/board/st/u8500/u8500.c
+++ b/board/st/u8500/u8500.c
@@ -22,6 +22,8 @@
 */
 
 #include <config.h>
+#include <common.h>
+#include <i2c.h>
 #include <asm/types.h>
 #include <asm/io.h>
 #include <asm/errno.h>
@@ -67,6 +69,8 @@
 
 #define PRCM_TCR			(PRCMU_BASE + 0x1C8)
 
+int board_id;	/* set in board_late_init() */
+
 /* PLLs for clock management registers */
 enum {
 	GATED = 0,
@@ -189,8 +193,6 @@ unsigned int addr_vall_arr[] = {
 0x8011F008, 0x00001CFF, // Clocks for HSI  TODO Enable reqd only
 0x8000F000, 0x00007FFF, // Clocks for I2C  TODO Enable reqd only
 0x8000F008, 0x00007FFF, // Clocks for I2C  TODO Enable reqd only
-0x8000E120, 0x003C0000, // GPIO for I2C/SD
-0x8000E124, 0x00000000, // GPIO for I2C/SD
 0x80157020, 0x00000150, // I2C 48MHz clock
 0x8012F000, 0x00007FFF, // Clocks for SD  TODO Enable reqd only
 0x8012F008, 0x00007FFF, // Clocks for SD  TODO Enable reqd only
@@ -214,11 +216,57 @@ unsigned int addr_vall_arr[] = {
 };
 
 #ifdef BOARD_LATE_INIT
+/*
+ * called after all initialisation were done, but before the generic
+ * mmc_initialize().
+ */
 int board_late_init(void)
 {
+	uchar byte;
+#ifdef CONFIG_MMC
+	uchar byte_array[] = {0x06, 0x06};
+#endif
+
+	/*
+	 * Determine and set board_id environment variable
+	 * 0: mop500, 1: href500
+	 * Above boards have different GPIO expander chips which we can
+	 * distinguish by the chip id.
+	 *
+	 * The board_id environment variable is needed for the Linux bootargs.
+	 */
+	(void) i2c_set_bus_num(0);
+	(void) i2c_read(CONFIG_SYS_I2C_GPIOE_ADDR, 0x80, 1, &byte, 1);
+	if (byte == 0x01) {
+		board_id = 0;
+		setenv("board_id", "0");
+	} else {
+		board_id = 1;
+		setenv("board_id", "1");
+	}
+#ifdef CONFIG_MMC
+	/*
+	 * config extended GPIO pins for level shifter and
+	 * SDMMC_ENABLE
+	 */
+	if (board_id == 0) {
+		/* MOP500 */
+		byte = 0x0c;
+		(void) i2c_write(CONFIG_SYS_I2C_GPIOE_ADDR, 0x89, 1, &byte, 1);
+		(void) i2c_write(CONFIG_SYS_I2C_GPIOE_ADDR, 0x83, 1, &byte, 1);
+	} else {
+		/* HREF */
+		/* set the direction of GPIO KPY9 and KPY10 */
+		byte = 0x06;
+		(void) i2c_write(CONFIG_SYS_I2C_GPIOE_ADDR, 0xC8, 1, &byte, 1);
+		/* must be a multibyte access */
+		(void) i2c_write(CONFIG_SYS_I2C_GPIOE_ADDR, 0xC4, 1,
+						&byte_array[0], 2);
+	}
+#endif /* CONFIG_MMC */
 	return (0);
 }
-#endif
+#endif /* BOARD_LATE_INIT */
 
 static void init_regs(void)
 {
diff --git a/board/st/u8500/u8500_i2c.c b/board/st/u8500/u8500_i2c.c
new file mode 100644
index 000000000..fe1b7ae56
--- /dev/null
+++ b/board/st/u8500/u8500_i2c.c
@@ -0,0 +1,619 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2009
+ *
+ * Basic U-Boot I2C interface for STn8500/DB8500
+ * Author: Michael Brandt <Michael.Brandt@stericsson.com>
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/*
+ * Only 7-bit I2C device address are supported.
+ *
+ * TODO:
+ *	- eliminate "timeout" loop counters and replace with real
+ *        timeouts based on timer ticks.
+ */
+
+#include <common.h>
+#include <i2c.h>
+
+/* later: #include <asm/arch/i2c.h> */
+#include "i2c.h"
+#include "gpio.h"
+#include <asm/io.h>
+
+typedef enum {
+	I2C_NACK_ADDR,
+	I2C_NACK_DATA,
+	I2C_ACK_MCODE,
+	I2C_ARB_LOST,
+	I2C_BERR_START,
+	I2C_BERR_STOP,
+	I2C_OVFL
+} i2c_error_t;
+
+#define I2C_ENDAD_COUNTER       500000		/* I2C bus "timeout" */
+#define I2C_FIFO_FLUSH_COUNTER	50000		/* flush "timeout" */
+#define I2C_SCL_FREQ            100000          /* I2C bus clock frequency.*/
+#define I2C_INPUT_FREQ          48000000        /* Input clock frequency.*/
+#define TX_FIFO_THRESHOLD	0x4
+#define RX_FIFO_THRESHOLD	0x4
+#define SLAVE_SETUP_TIME 14 /* Slave data setup time, 250ns for 48MHz i2c_clk */
+
+static unsigned int bus_initialized[CONFIG_SYS_I2C_BUS_MAX];
+static unsigned int i2c_bus_num = 0;
+static unsigned int i2c_bus_speed[] = {
+	CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SPEED,
+	CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SPEED
+};
+static t_i2c_registers *i2c_dev[] = {
+	(t_i2c_registers *)CONFIG_SYS_I2C0_BASE,
+	(t_i2c_registers *)CONFIG_SYS_I2C1_BASE,
+	(t_i2c_registers *)CONFIG_SYS_I2C2_BASE,
+	(t_i2c_registers *)CONFIG_SYS_I2C3_BASE,
+};
+
+static struct {
+	gpio_alt_function altfunc;
+	char *dev_name;
+} i2c_gpio_altfunc[] = {
+	{GPIO_ALT_I2C_0, "i2c0"},
+	{GPIO_ALT_I2C_1, "i2c1"},
+	{GPIO_ALT_I2C_2, "i2c2"},
+	{GPIO_ALT_I2C_3, "i2c3"},
+};
+
+static int __i2c_set_bus_speed(unsigned int speed)
+{
+	u32 value;
+	t_i2c_registers *p_i2c_registers;
+
+	p_i2c_registers = i2c_dev[i2c_bus_num];
+
+	/* Select standard (100 kbps) speed mode */
+        I2C_WRITE_FIELD(p_i2c_registers->cr, I2C_CR_SM, I2C_CR_SHIFT_SM, 0x0);
+
+        /*
+	 * Set the Baud Rate Counter 2 value
+	 * Baud rate (standard) = fi2cclk / ( (BRCNT2 x 2) + Foncycle )
+	 * Foncycle = 0 (no digital filtering)
+	 */
+        value = (u32) (I2C_INPUT_FREQ / (speed * 2));
+        I2C_WRITE_FIELD(p_i2c_registers->brcr, I2C_BRCR_BRCNT2,
+			I2C_BRCR_SHIFT_BRCNT2, value);
+
+        /* ensure that BRCNT value is zero */
+        I2C_WRITE_FIELD(p_i2c_registers->brcr, I2C_BRCR_BRCNT1,
+			I2C_BRCR_SHIFT_BRCNT1, 0);
+
+	return I2C_INPUT_FREQ/(value * 2);
+}
+
+/*
+ * i2c_init - initialize the i2c bus
+ *
+ *	speed: bus speed (in HZ)
+ *	slaveaddr: address of device in slave mode
+ *
+ *	Slave mode is not implemented.
+ */
+void i2c_init(int speed, int slaveaddr)
+{
+	t_i2c_registers *p_i2c_registers;
+
+	debug("i2c_init bus %d, speed %d\n", i2c_bus_num, speed);
+
+	(void) gpio_altfuncenable(i2c_gpio_altfunc[i2c_bus_num].altfunc,
+			i2c_gpio_altfunc[i2c_bus_num].dev_name);
+
+	p_i2c_registers = i2c_dev[i2c_bus_num];
+
+	/* Disable the controller */
+	I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
+
+	/* Clear registers */
+	I2C_WRITE_REG(p_i2c_registers->cr, 0);
+	I2C_WRITE_REG(p_i2c_registers->scr, 0);
+	I2C_WRITE_REG(p_i2c_registers->hsmcr, 0);
+	I2C_WRITE_REG(p_i2c_registers->tftr, 0);
+	I2C_WRITE_REG(p_i2c_registers->rftr, 0);
+	I2C_WRITE_REG(p_i2c_registers->dmar, 0);
+
+	/* No digital filter */
+	I2C_WRITE_FIELD(p_i2c_registers->cr, I2C_CR_FON, I2C_CR_SHIFT_FON,
+			I2C_DIGITAL_FILTERS_OFF);
+
+	i2c_bus_speed[i2c_bus_num] = __i2c_set_bus_speed(speed);
+
+        /*
+	 * Set our own address.
+	 * Set slave address mode to 7 bit addressing mode
+	 */
+        I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_SAM);
+        I2C_WRITE_FIELD(p_i2c_registers->scr, I2C_SCR_ADDR, I2C_SCR_SHIFT_ADDR,
+			slaveaddr);
+	/* Slave Data Set up Time */
+	I2C_WRITE_FIELD(p_i2c_registers->scr, I2C_SCR_DATA_SETUP_TIME,
+			I2C_SCR_SHIFT_DATA_SETUP_TIME, SLAVE_SETUP_TIME);
+
+	/* Set the DMA sync logic */
+	I2C_WRITE_FIELD(p_i2c_registers->cr, I2C_CR_DMA_SLE,
+			I2C_CR_SHIFT_DMA_SLE, I2C_DISABLE);
+
+	/* Disable interrupts */
+	I2C_WRITE_REG(p_i2c_registers->imscr, 0);
+
+	/* Configure bus master mode */
+	I2C_WRITE_FIELD(p_i2c_registers->cr, I2C_CR_OM, I2C_CR_SHIFT_OM,
+							I2C_BUS_MASTER_MODE);
+	/* Set FIFO threshold values */
+	writel(TX_FIFO_THRESHOLD, &p_i2c_registers->tftr);
+	writel(RX_FIFO_THRESHOLD, &p_i2c_registers->rftr);
+
+	/* Enable the I2C Controller */
+	I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
+
+	bus_initialized[i2c_bus_num] = 1;
+}
+
+
+/*
+ * loop_till_bit_clear - polls on a bit till it clears
+ * ioreg: register where you want to check status
+ * mask: bit mask for the bit you wish to check
+ * end_counter: upper limit to the counter when you stop checking
+ */
+static int loop_till_bit_clear(void *io_reg, u32 mask, int end_counter)
+{
+	int loop = 0;
+	while (1) {
+		if ((readl(io_reg) & mask) == 0x0UL)
+			return 0;
+		loop++;
+		if (loop == end_counter)
+			return 1;
+	}
+}
+
+/*
+ * loop_till_bit_set - polls on a bit till it is set.
+ * ioreg: register where you want to check status
+ * mask: bit mask for the bit you wish to check
+ * end_counter: upper limit to the counter when you stop checking
+ *
+ */
+static int loop_till_bit_set(void * io_reg, u32 mask, int end_counter)
+{
+	int loop = 0;
+	while (1) {
+		if ((readl(io_reg) & mask) != 0x0UL)
+			return 0;
+		loop++;
+		if (loop == end_counter)
+			return 1;
+	}
+}
+
+/*
+ * flush_fifo - flush the I2C TX and RX FIFOs
+ */
+static void flush_fifo(t_i2c_registers *p_i2c_registers)
+{
+	int counter = I2C_FIFO_FLUSH_COUNTER;
+
+	/* Flush Tx FIFO */
+	I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_FTX);
+	/* Flush Rx FIFO */
+	I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_FRX);
+	while (counter--) {
+		if (!(readl(&p_i2c_registers->cr) & (I2C_CR_FTX | I2C_CR_FRX)))
+			break;
+	}
+	return;
+}
+
+#ifdef DEBUG
+static inline void print_abort_reason(t_i2c_registers *p_i2c_registers)
+{
+	i2c_error_t cause;
+
+	printf("abort: risr %08x, sr %08x\n", p_i2c_registers->risr,
+			p_i2c_registers->sr);
+	cause = (i2c_error_t) I2C_READ_FIELD(p_i2c_registers->sr, I2C_SR_CAUSE,
+			I2C_SR_SHIFT_CAUSE);
+	switch (cause) {
+	case I2C_NACK_ADDR:
+		printf("No Ack received after Slave Address xmission\n");
+		break;
+	case I2C_NACK_DATA:
+		printf("Valid for MASTER_WRITE: No Ack received"
+				"during data phase\n");
+		break;
+	case I2C_ACK_MCODE:
+		printf("Master recv ack after xmission of master code"
+				"in hs mode\n");
+		break;
+	case I2C_ARB_LOST:
+		printf("Master Lost arbitration\n");
+		break;
+	case I2C_BERR_START:
+		printf("Slave restarts\n");
+		break;
+	case I2C_BERR_STOP:
+		printf("Slave reset\n");
+		break;
+	case I2C_OVFL:
+		printf("Overflow\n");
+		break;
+	default:
+		printf("Unknown error type\n");
+	}
+}
+#endif
+
+/*
+ * i2c_abort - called when a I2C transaction failed
+ */
+static void i2c_abort(t_i2c_registers *p_i2c_registers)
+{
+#ifdef DEBUG
+	print_abort_reason(p_i2c_registers);
+#endif
+	/* flush fifo */
+	flush_fifo(p_i2c_registers);
+
+        /* Acknowledge the Master Transaction Done */
+        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTD);
+
+        /* Acknowledge the Master Transaction Done Without Stop */
+        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTDWS);
+
+	/* disable controller */
+	I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
+
+	/* delay 10 milliseconds */
+	udelay(10*1000);
+
+	/* enable controller */
+	I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
+}
+
+/*
+ * write addr, alias index, to I2C bus.
+ */
+static int i2c_write_addr(t_i2c_registers *p_i2c_registers, uint addr, int alen)
+{
+	while (alen--) {
+		/* Wait until the Tx Fifo is not full */
+		if (loop_till_bit_clear((void* )&p_i2c_registers->risr,
+					I2C_INT_TXFF, I2C_ENDAD_COUNTER))
+		{
+			i2c_abort(p_i2c_registers);
+			return -1;
+		}
+
+		/* MSB first */
+		writeb((addr >> (alen * 8)) & 0xff, &p_i2c_registers->tfr);
+	}
+
+	return 0;
+}
+
+/*
+ * Internal simplified read function:
+ *   i2c_registers:	Pointer to I2C registers for current bus
+ *   chip:    I2C chip address, range 0..127
+ *   addr:    Memory (register) address within the chip
+ *   alen:    Number of bytes to use for addr (typically 1, 2 for larger
+ *              memories, 0 for register type devices with only one
+ *              register)
+ *   value:   Where to put the data
+ *
+ *   Returns: 0 on success, not 0 on failure
+ */
+static int i2c_read_byte(t_i2c_registers *p_i2c_registers, uchar chip,
+		uint addr, int alen, uchar *value)
+{
+	volatile u32   mcr = 0;
+
+	/* Set the address mode to 7 bit */
+	I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 1);
+
+        /* Store the slave address in the master control register */
+        I2C_WRITE_FIELD(mcr, I2C_MCR_A7, I2C_MCR_SHIFT_A7, chip);
+
+	if (alen != 0) {
+		/* Master write operation */
+		I2C_CLR_BIT(mcr, I2C_MCR_OP);
+
+		/* Configure the Frame length to one byte */
+		I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 1);
+
+		/* Repeated start, no stop */
+		I2C_CLR_BIT(mcr, I2C_MCR_STOP);
+
+		/* Write Master Control Register */
+		writel(mcr, &p_i2c_registers->mcr);
+
+		/* send addr/index */
+		if (i2c_write_addr(p_i2c_registers, addr, alen) != 0)
+			return -1;
+
+		/* Check for the Master Transaction Done Without Stop */
+		if (loop_till_bit_set((void *)&p_i2c_registers->risr,
+					I2C_INT_MTDWS, I2C_ENDAD_COUNTER)) {
+			return -1;
+		}
+
+		/* Master Transaction Without Stop has been done */
+		/* Acknowledge the Master Transaction Done Without Stop */
+		I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTDWS);
+	}
+
+	/* Master control configuration for read operation  */
+	I2C_SET_BIT(mcr, I2C_MCR_OP);
+
+	/* Configure the STOP condition, we read only one byte */
+	I2C_SET_BIT(mcr, I2C_MCR_STOP);
+
+	/* Set the frame length to one byte, we support only 1 byte
+	 * reads */
+	I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 1);
+
+	I2C_WRITE_FIELD(p_i2c_registers->mcr, I2C_MCR_LENGTH_STOP_OP,
+			I2C_MCR_SHIFT_LENGTH_STOP_OP, mcr);
+
+	/*
+	 * receive_data_polling
+	 */
+
+	/* Wait until the Rx FIFO is not empty */
+	if (loop_till_bit_clear((void* )&p_i2c_registers->risr, I2C_INT_RXFE,
+			I2C_ENDAD_COUNTER))
+	{
+		return -1;
+	}
+
+	/* Read the data byte from Rx FIFO */
+	*value = readb(&p_i2c_registers->rfr);
+
+	/* Wait until the work is done */
+	if (loop_till_bit_set((void *)&p_i2c_registers->risr, I2C_INT_MTD,
+				I2C_ENDAD_COUNTER)) {
+		return -1;
+	}
+
+        /* Acknowledge the Master Transaction Done */
+        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTD);
+
+        /* If MTD is set, Master Transaction Done Without Stop is set too */
+        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTDWS);
+
+	return 0;
+}
+
+/*
+ * Internal simplified write function:
+ *   i2c_registers:	Pointer to I2C registers for current bus
+ *   chip:    I2C chip address, range 0..127
+ *   addr:    Memory (register) address within the chip
+ *   alen:    Number of bytes to use for addr (typically 1, 2 for larger
+ *              memories, 0 for register type devices with only one
+ *              register)
+ *   data:    Where to read the data
+ *   len:     How many bytes to write
+ *
+ *   Returns: 0 on success, not 0 on failure
+ */
+static int __i2c_write(t_i2c_registers *p_i2c_registers, u8 chip, uint addr,
+		int alen, u8 *data, int len)
+{
+	int i;
+	volatile u32 mcr = 0;
+
+	/* Set the address mode to 7 bit */
+	I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 1);
+
+        /* Store the slave address in the master control register */
+        I2C_WRITE_FIELD(mcr, I2C_MCR_A7, I2C_MCR_SHIFT_A7, chip);
+
+	/* Write operation */
+	I2C_CLR_BIT(mcr, I2C_MCR_OP);
+
+        /* Current transaction is terminated by STOP condition */
+        I2C_SET_BIT(mcr, I2C_MCR_STOP);
+
+	/* Frame length: addr byte + len */
+	/* XXX: fix I2C_WRITE_FIELD macro: use () around value parameter */
+	I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH,
+			(alen + len));
+
+	/* Write MCR register */
+	writel(mcr, &p_i2c_registers->mcr);
+
+	if (i2c_write_addr(p_i2c_registers, addr, alen) != 0)
+		return -1;
+
+	for (i = 0; i < len; i++) {
+
+		/* Wait until the Tx FIFO is not full */
+		if (loop_till_bit_clear((void *)&p_i2c_registers->risr,
+					I2C_INT_TXFF, I2C_ENDAD_COUNTER)) {
+			return -1;
+		}
+
+		/* it is a 32 bit register with upper 24 reserved R/O */
+		writeb(data[i], &p_i2c_registers->tfr);
+	}
+
+	/* Check for Master Transaction Done */
+	if (loop_till_bit_set((void *)&p_i2c_registers->risr, I2C_INT_MTD,
+				I2C_ENDAD_COUNTER)) {
+		printf("i2c_write_byte error2: risr %08x\n",
+				p_i2c_registers->risr);
+		return -1;
+	}
+
+        /* Acknowledge Master Transaction Done */
+        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTD);
+
+        /* Acknowledge Master Transaction Done Without Stop */
+        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTDWS);
+
+	return 0;
+}
+
+/*
+ * Probe the given I2C chip address. Returns 0 if a chip responded,
+ * not 0 on failure.
+ */
+int i2c_probe(uchar chip)
+{
+	volatile u32   mcr = 0;
+	t_i2c_registers     *p_i2c_registers;
+
+	if (chip == CONFIG_SYS_I2C_SLAVE)
+		return 1;
+
+	p_i2c_registers = i2c_dev[i2c_bus_num];
+
+	/* Set the address mode to 7 bit */
+	I2C_WRITE_FIELD(mcr, I2C_MCR_AM, I2C_MCR_SHIFT_AM, 1);
+
+        /* Store the slave address in the master control register */
+        I2C_WRITE_FIELD(mcr, I2C_MCR_A10, I2C_MCR_SHIFT_A7, chip);
+
+	/* Read operation */
+	I2C_SET_BIT(mcr, I2C_MCR_OP);
+
+	/* Set the frame length to one byte */
+	I2C_WRITE_FIELD(mcr, I2C_MCR_LENGTH, I2C_MCR_SHIFT_LENGTH, 1);
+
+	/* Current transaction is terminated by STOP condition */
+	I2C_SET_BIT(mcr, I2C_MCR_STOP);
+
+	/* Write MCR register */
+	writel(mcr, &p_i2c_registers->mcr);
+
+	/* Wait until the Rx Fifo is not empty */
+	if (loop_till_bit_clear((void* )&p_i2c_registers->risr, I2C_INT_RXFE,
+			I2C_ENDAD_COUNTER))
+	{
+		i2c_abort(p_i2c_registers);
+		return -1;
+	}
+
+	flush_fifo(p_i2c_registers);
+
+        /* Master Transaction has been done */
+        /* Acknowledge the Master Transaction Done */
+        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTD);
+
+        /* Master Transaction Without Stop has been done */
+        /* Acknowledge the Master Transaction Done Without Stop */
+        I2C_SET_BIT(p_i2c_registers->icr, I2C_INT_MTDWS);
+
+	return 0;
+}
+
+/*
+ * Read/Write interface:
+ *   chip:    I2C chip address, range 0..127
+ *   addr:    Memory (register) address within the chip
+ *   alen:    Number of bytes to use for addr (typically 1, 2 for larger
+ *              memories, 0 for register type devices with only one
+ *              register)
+ *   buffer:  Where to read/write the data
+ *   len:     How many bytes to read/write
+ *
+ *   Returns: 0 on success, not 0 on failure
+ */
+int i2c_read(uchar chip, uint addr, int alen, uchar *buffer, int len)
+{
+	int i;
+	int rc;
+	t_i2c_registers *p_i2c_registers;
+
+	if (alen > 1) {
+		debug("I2C read: addr len %d not supported\n", alen);
+		return 1;
+	}
+
+	p_i2c_registers = i2c_dev[i2c_bus_num];
+
+	for (i = 0; i < len; i++) {
+		rc = i2c_read_byte(p_i2c_registers, chip, addr + i, alen,
+				&buffer[i]);
+		if (rc != 0) {
+			debug("I2C read: I/O error: %d\n", rc);
+			i2c_abort(p_i2c_registers);
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+int i2c_write(uchar chip, uint addr, int alen, uchar *buffer, int len)
+{
+	int rc;
+	t_i2c_registers *p_i2c_registers;
+	p_i2c_registers = i2c_dev[i2c_bus_num];
+
+	rc = __i2c_write(p_i2c_registers, chip, addr, alen, buffer,
+			len);
+	if (rc != 0) {
+		debug("I2C write: I/O error\n");
+		i2c_abort(p_i2c_registers);
+		return rc;
+	}
+	return 0;
+}
+
+int i2c_set_bus_num(unsigned int bus)
+{
+	if (bus > ARRAY_SIZE(i2c_dev) - 1) {
+		return -1;
+	}
+
+	i2c_bus_num = bus;
+
+	if(!bus_initialized[i2c_bus_num])
+		i2c_init(CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE);
+
+	return 0;
+}
+
+int i2c_set_bus_speed(unsigned int speed)
+{
+	t_i2c_registers *p_i2c_registers;
+
+	if (speed > I2C_MAX_STANDARD_SCL) {
+		debug("i2c_set_bus_speed: only up to %d supported\n",
+				I2C_MAX_STANDARD_SCL);
+		return -1;
+	}
+
+	p_i2c_registers = i2c_dev[i2c_bus_num];
+
+	/* Disable the controller */
+	I2C_CLR_BIT(p_i2c_registers->cr, I2C_CR_PE);
+
+	i2c_bus_speed[i2c_bus_num] = __i2c_set_bus_speed(speed);
+
+	/* Enable the controller */
+	I2C_SET_BIT(p_i2c_registers->cr, I2C_CR_PE);
+
+	return 0;
+}
+
+unsigned int i2c_get_bus_num(void)
+{
+	return i2c_bus_num;
+}
+
+unsigned int i2c_get_bus_speed(void)
+{
+	return i2c_bus_speed[i2c_bus_num];
+}