diff --git a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/PinNames.h b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/PinNames.h
index 8ce5fd984c..7cbff11502 100644
--- a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/PinNames.h
+++ b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/PinNames.h
@@ -30,201 +30,204 @@ typedef enum {
 #define PORT_SHIFT  5
 
 typedef enum {
-    // MPS2 EXP Pin Names
-        EXP0 = 0 ,
-        EXP1 = 4 ,
-        EXP2 = 2 ,
-        EXP3 = 3 ,
-        EXP4 = 1 ,
-        EXP5 = 15,
-        EXP6 = 5 ,
-        EXP7 = 6 ,
-        EXP8 = 7 ,
-        EXP9 = 8 ,
-        EXP10 =9 ,
-        EXP11 =13,
-        EXP12 =10,
-        EXP13 =11,
-        EXP14 =12,
-        EXP15 =14,
-        EXP16 =18,
-        EXP17 =19,
-        EXP18 =20,
-        EXP19 =21,
-        EXP20 =52,
-        EXP21 =53,
-        EXP22 =54,
-        EXP23 =55,
-        EXP24 =56,
-        EXP25 =57,
-
-        EXP26 =16,
-        EXP27 =25,
-        EXP28 =24,
-        EXP29 =31,
-        EXP30 =17,
-        EXP31 =23,
-        EXP32 =27,
-        EXP33 =30,
-        EXP34 =26,
-        EXP35 =28,
-        EXP36 =29,
-        EXP37 =58,
-        EXP38 =48,
-        EXP39 =49,
-        EXP40 =50,
-        EXP41 =22,
-        EXP42 =59,
-        EXP43 =60,
-        EXP44 =51,
-        EXP45 =61,
-        EXP46 =62,
-        EXP47 =63,
-        EXP48 =64,
-        EXP49 =65,
-        EXP50 =66,
-        EXP51 =67,
+    /* MPS2 EXP Pin Names */
+    EXP0  = 0,
+    EXP1  = 1,
+    EXP2  = 2,
+    EXP3  = 3,
+    EXP4  = 4,
+    EXP5  = 5,
+    EXP6  = 6,
+    EXP7  = 7,
+    EXP8  = 8,
+    EXP9  = 9,
+    EXP10 = 10,
+    EXP11 = 11,
+    EXP12 = 12,
+    EXP13 = 13,
+    EXP14 = 14,
+    EXP15 = 15,
+    EXP16 = 16,
+    EXP17 = 17,
+    EXP18 = 18,
+    EXP19 = 19,
+    EXP20 = 20,
+    EXP21 = 21,
+    EXP22 = 22,
+    EXP23 = 23,
+    EXP24 = 24,
+    EXP25 = 25,
+    EXP26 = 26,
+    EXP27 = 27,
+    EXP28 = 28,
+    EXP29 = 29,
+    EXP30 = 30,
+    EXP31 = 31,
+    EXP32 = 32,
+    EXP33 = 33,
+    EXP34 = 34,
+    EXP35 = 35,
+    EXP36 = 36,
+    EXP37 = 37,
+    EXP38 = 38,
+    EXP39 = 39,
+    EXP40 = 40,
+    EXP41 = 41,
+    EXP42 = 42,
+    EXP43 = 43,
+    EXP44 = 44,
+    EXP45 = 45,
+    EXP46 = 46,
+    EXP47 = 47,
+    EXP48 = 48,
+    EXP49 = 49,
+    EXP50 = 50,
+    EXP51 = 51,
 
 // Other mbed Pin Names
 
-        //LEDs on mps2
-        //user leds
-        USERLED1 = 100,
-        USERLED2 = 101,
-        //user switches
-        USERSW1  = 110,
-        USERSW2  = 111,
+    /* User leds */
+    USERLED1 = 100,
+    USERLED2 = 101,
+    /* User switches */
+    USERSW1  = 110,
+    USERSW2  = 111,
 
-        //mcc leds
-        LED1 = 200,
-        LED2 = 201,
-        LED3 = 202,
-        LED4 = 203,
-        LED5 = 204,
-        LED6 = 205,
-        LED7 = 206,
-        LED8 = 207,
+    /* MCC leds */
+    LED1 = 200,
+    LED2 = 201,
+    LED3 = 202,
+    LED4 = 203,
+    LED5 = 204,
+    LED6 = 205,
+    LED7 = 206,
+    LED8 = 207,
 
-        //MCC Switches
-        SW1 = 210,
-        SW2 = 211,
-        SW3 = 212,
-        SW4 = 213,
-        SW5 = 214,
-        SW6 = 215,
-        SW7 = 216,
-        SW8 = 217,
+    /* MCC Switches */
+    SW1 = 210,
+    SW2 = 211,
+    SW3 = 212,
+    SW4 = 213,
+    SW5 = 214,
+    SW6 = 215,
+    SW7 = 216,
+    SW8 = 217,
 
-        //MPS2 SPI header pins j21
-        SPI_MOSI = 300,
-        SPI_MISO = 301,
-        SPI_SCLK = 302,
-        SPI_SSEL = 303,
+    /* MPS2 SPI header pins J21 */
+    SPI_MOSI = 300,
+    SPI_MISO = 301,
+    SPI_SCLK = 302,
+    SPI_SSEL = 303,
 
-        //MPS2 CLCD SPI
-        CLCD_MOSI = 304,
-        CLCD_MISO = 305,
-        CLCD_SCLK = 306,
-        CLCD_SSEL = 307,
-        CLCD_RESET = 308,
-        CLCD_RS = 309,
-        CLCD_RD = 310,
-        CLCD_BL_CTRL = 311,
+    /* MPS2 CLCD SPI */
+    CLCD_MOSI = 304,
+    CLCD_MISO = 305,
+    CLCD_SCLK = 306,
+    CLCD_SSEL = 307,
+    CLCD_RESET = 308,
+    CLCD_RS = 309,
+    CLCD_RD = 310,
+    CLCD_BL_CTRL = 311,
 
-        //MPS2 shield 0 SPI
-        SHIELD_0_SPI_SCK = 320,
-        SHIELD_0_SPI_MOSI = 321,
-        SHIELD_0_SPI_MISO = 322,
-        SHIELD_0_SPI_nCS = 323,
+    /* MPS2 shield 0 SPI */
+    SHIELD_0_SPI_MOSI = EXP13,
+    SHIELD_0_SPI_MISO = EXP14,
+    SHIELD_0_SPI_SCK  = EXP11,
+    SHIELD_0_SPI_nCS  = EXP12,
 
-        //MPS2 shield 1 SPI
-        SHIELD_1_SPI_SCK = 331,
-        SHIELD_1_SPI_MOSI = 332,
-        SHIELD_1_SPI_MISO = 333,
-        SHIELD_1_SPI_nCS = 334,
+    /* MPS2 shield 1 SPI */
+    SHIELD_1_SPI_MOSI = EXP39,
+    SHIELD_1_SPI_MISO = EXP40,
+    SHIELD_1_SPI_SCK  = EXP44,
+    SHIELD_1_SPI_nCS  = EXP38,
 
-        //MPS2 shield ADC SPI
-        ADC_MOSI = 650,
-        ADC_MISO = 651,
-        ADC_SCLK = 652,
-        ADC_SSEL = 653,
+    /* MPS2 shield ADC SPI */
+    ADC_MOSI = EXP18,
+    ADC_MISO = EXP17,
+    ADC_SCLK = EXP19,
+    ADC_SSEL = EXP16,
 
-        //MPS2 Uart
-        USBTX  = 400,
-        USBRX  = 401,
-        XB_TX = 402,
-        XB_RX = 403,
-        SH0_TX = 404,
-        SH0_RX = 405,
-        SH1_TX = 406,
-        SH1_RX = 407,
-        MCC_TX = 408,
-        MCC_RX = 409,
+    /* MPS2 UART */
+    MCC_TX = 400,
+    MCC_RX = 401,
+    USBTX  = 402,
+    USBRX  = 403,
+    XB_TX  = EXP24,
+    XB_RX  = EXP23,
+    SH0_TX = EXP4,
+    SH0_RX = EXP0,
+    SH1_TX = EXP30,
+    SH1_RX = EXP26,
 
-        //MPS2 I2C touchscreen and audio
-        TSC_SDA = 500,
-        TSC_SCL = 501,
-        AUD_SDA = 502,
-        AUD_SCL = 503,
+    /* MPS2 I2C touchscreen and audio */
+    TSC_SDA = 500,
+    TSC_SCL = 501,
+    AUD_SDA = 502,
+    AUD_SCL = 503,
 
-        //MPS2 I2C for shield
-        SHIELD_0_SDA = 504,
-        SHIELD_0_SCL = 505,
-        SHIELD_1_SDA = 506,
-        SHIELD_1_SCL = 507,
+    /* MPS2 I2C for shield */
+    SHIELD_0_SDA = EXP15,
+    SHIELD_0_SCL = EXP5,
+    SHIELD_1_SDA = EXP41,
+    SHIELD_1_SCL = EXP31,
 
-        //MPS2 shield Analog pins
-        A0_0 = 600,
-        A0_1 = 601,
-        A0_2 = 602,
-        A0_3 = 603,
-        A0_4 = 604,
-        A0_5 = 605,
-        A1_0 = 606,
-        A1_1 = 607,
-        A1_2 = 608,
-        A1_3 = 609,
-        A1_4 = 610,
-        A1_5 = 611,
-        //MPS2 Shield Digital pins
-        D0_0  = EXP0,
-        D0_1  = EXP4,
-        D0_2  = EXP2,
-        D0_3  = EXP3,
-        D0_4  = EXP1,
-        D0_5  = EXP6,
-        D0_6  = EXP7,
-        D0_7  = EXP8,
-        D0_8  = EXP9,
-        D0_9  = EXP10,
-        D0_10 = EXP12,
-        D0_11 = EXP13,
-        D0_12 = EXP14,
-        D0_13 = EXP11,
-        D0_14 = EXP15,
-        D0_15 = EXP5,
+    /* MPS2 shield Analog pins */
+    A0_0 = 600,
+    A0_1 = 601,
+    A0_2 = 602,
+    A0_3 = 603,
+    A0_4 = 604,
+    A0_5 = 605,
+    A1_0 = 606,
+    A1_1 = 607,
+    A1_2 = 608,
+    A1_3 = 609,
+    A1_4 = 610,
+    A1_5 = 611,
+    /* MPS2 Shield Digital pins */
+    D0_0  = EXP0,
+    D0_1  = EXP4,
+    D0_2  = EXP2,
+    D0_3  = EXP3,
+    D0_4  = EXP1,
+    D0_5  = EXP6,
+    D0_6  = EXP7,
+    D0_7  = EXP8,
+    D0_8  = EXP9,
+    D0_9  = EXP10,
+    D0_10 = EXP12,
+    D0_11 = EXP13,
+    D0_12 = EXP14,
+    D0_13 = EXP11,
+    D0_14 = EXP15,
+    D0_15 = EXP5,
 
-        D1_0  = EXP26,
-        D1_1  = EXP30,
-        D1_2  = EXP28,
-        D1_3  = EXP29,
-        D1_4  = EXP27,
-        D1_5  = EXP32,
-        D1_6  = EXP33,
-        D1_7  = EXP34,
-        D1_8  = EXP35,
-        D1_9  = EXP36,
-        D1_10 = EXP38,
-        D1_11 = EXP39,
-        D1_12 = EXP40,
-        D1_13 = EXP44,
-        D1_14 = EXP41,
-        D1_15 = EXP31,
+    D1_0  = EXP26,
+    D1_1  = EXP30,
+    D1_2  = EXP28,
+    D1_3  = EXP29,
+    D1_4  = EXP27,
+    D1_5  = EXP32,
+    D1_6  = EXP33,
+    D1_7  = EXP34,
+    D1_8  = EXP35,
+    D1_9  = EXP36,
+    D1_10 = EXP38,
+    D1_11 = EXP39,
+    D1_12 = EXP40,
+    D1_13 = EXP44,
+    D1_14 = EXP41,
+    D1_15 = EXP31,
 
-        // Not connected
-        NC = (int)0xFFFFFFFF,
+    /* Not connected */
+    NC = (int)0xFFFFFFFF,
 } PinName;
 
+typedef enum {
+    ALTERNATE_FUNC = 0, /* The pin is used for alternative function */
+    GPIO_FUNC = 1       /* The pin is used for GPIO function */
+} PinFunction;
+
 typedef enum {
     PullUp = 2,
     PullDown = 1,
diff --git a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/gpio_api.c b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/gpio_api.c
index 64524dc326..11e4d44e1d 100644
--- a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/gpio_api.c
+++ b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/gpio_api.c
@@ -18,15 +18,7 @@
 #include "gpio_api.h"
 #include "pinmap.h"
 
-#define GPIO_NUM  4
-
-CMSDK_GPIO_TypeDef* GPIO_MAP[GPIO_NUM] = {
-    CMSDK_GPIO0,
-    CMSDK_GPIO1,
-    CMSDK_GPIO2,
-    CMSDK_GPIO3
-};
-
+#define GPIO_PIN_POS_MASK 0x0F /* pin % 16 */
 #define RESERVED_MISC_PIN 7
 
 /* \brief Gets the FPGA MISC (Miscellaneous control) bit position for the given
@@ -69,83 +61,96 @@ static uint8_t get_fpga_misc_pin_pos(PinName pin)
 
 uint32_t gpio_set(PinName pin)
 {
-    /* TODO */
-    return 1;
+    uint8_t pin_position;
+
+    if (pin >=EXP0 && pin <= EXP51) {
+        /* Set pin functinality as GPIO. pin_function asserts if pin == NC */
+        pin_function(pin, GPIO_FUNC);
+    } else {
+        /* Check if pin is a MISC pin */
+        pin_position = get_fpga_misc_pin_pos(pin);
+        if (pin_position != RESERVED_MISC_PIN) {
+            return (1 << pin_position);
+        }
+    }
+
+    /* Return pin mask */
+    return (1 << (pin & 0xFF));
 }
 
 void gpio_init(gpio_t *obj, PinName pin)
 {
-    uint8_t pin_position = 0;
+    uint8_t pin_position;
 
     if (pin == NC) {
        return;
     }
 
-    if(pin <= 15){
-        pin_position = pin;
+    obj->pin = pin;
+    obj->pin_number = pin;
+
+    if (pin <= EXP15) {
         obj->reg_data   = &CMSDK_GPIO0->DATAOUT;
         obj->reg_in     = &CMSDK_GPIO0->DATA;
         obj->reg_dir    = &CMSDK_GPIO0->OUTENABLESET;
         obj->reg_dirclr = &CMSDK_GPIO0->OUTENABLECLR;
-    } else if (pin >= 16 && pin <= 31) {
-        pin_position = (pin - 16);
+        /* Set pin function as a GPIO */
+        pin_function(pin, GPIO_FUNC);
+        pin_position = pin;
+    } else if (pin >= EXP16 && pin <= EXP31) {
         obj->reg_data   = &CMSDK_GPIO1->DATAOUT;
         obj->reg_in     = &CMSDK_GPIO1->DATA;
         obj->reg_dir    = &CMSDK_GPIO1->OUTENABLESET;
         obj->reg_dirclr = &CMSDK_GPIO1->OUTENABLECLR;
-    } else if (pin >= 32 && pin <= 47) {
-        pin_position = (pin - 32);
+        /* Set pin function as a GPIO */
+        pin_function(pin, GPIO_FUNC);
+        pin_position = (pin & GPIO_PIN_POS_MASK);
+    } else if (pin >= EXP32 && pin <= EXP47) {
         obj->reg_data   = &CMSDK_GPIO2->DATAOUT;
         obj->reg_in     = &CMSDK_GPIO2->DATA;
         obj->reg_dir    = &CMSDK_GPIO2->OUTENABLESET;
         obj->reg_dirclr = &CMSDK_GPIO2->OUTENABLECLR;
-    } else if (pin >= 48 && pin <= 51) {
-        pin_position = (pin - 48 );
+        /* Set pin function as a GPIO */
+        pin_function(pin, GPIO_FUNC);
+        pin_position = (pin & GPIO_PIN_POS_MASK);
+    } else if (pin >= EXP48 && pin <= EXP51) {
         obj->reg_data   = &CMSDK_GPIO3->DATAOUT;
         obj->reg_in     = &CMSDK_GPIO3->DATA;
         obj->reg_dir    = &CMSDK_GPIO3->OUTENABLESET;
         obj->reg_dirclr = &CMSDK_GPIO3->OUTENABLECLR;
+        /* Set pin function as a GPIO */
+        pin_function(pin, GPIO_FUNC);
+        pin_position = (pin & GPIO_PIN_POS_MASK);
     } else if (pin == 100 || pin == 101) {
         /* User LEDs */
-        pin_position    = (pin - 100);
+        pin_position = (pin - 100);
         obj->reg_data   = &MPS2_FPGAIO->LED;
         obj->reg_in     = &MPS2_FPGAIO->LED;
         obj->reg_dir    = NULL;
         obj->reg_dirclr = NULL;
     } else if (pin == 110 || pin == 111) {
         /* User buttons */
-        pin_position    = pin-110;
+        pin_position = (pin - 110);
         obj->reg_data   = &MPS2_FPGAIO->BUTTON;
         obj->reg_in     = &MPS2_FPGAIO->BUTTON;
         obj->reg_dir    = NULL;
         obj->reg_dirclr = NULL;
     } else if (pin >= 200 && pin <= 207) {
         /* MCC LEDs */
-        pin_position    = pin-200;
+        pin_position = (pin - 200);
         obj->reg_data   = &MPS2_SCC->LEDS;
         obj->reg_in     = &MPS2_SCC->LEDS;
         obj->reg_dir    = NULL;
         obj->reg_dirclr = NULL;
     } else if (pin >= 210 && pin <= 217) {
         /* MCC switches */
-        pin_position    = (pin - 210);
+        pin_position = (pin - 210);
         obj->reg_in     = &MPS2_SCC->SWITCHES;
         obj->reg_data   = NULL;
         obj->reg_dir    = NULL;
         obj->reg_dirclr = NULL;
-    } else if (pin == SHIELD_0_SPI_nCS) {
-        pin_position = 8;
-        GPIO_MAP[CMSDK_GPIO_SH0_CS_SPI_GPIO_NUM]->ALTFUNCSET |=
-                                (1 << CMSDK_GPIO_ALTFUNC_SH0_CS_SPI_SET);
-    } else if (pin == SHIELD_1_SPI_nCS) {
-        pin_position = 9;
-        GPIO_MAP[CMSDK_GPIO_SH1_CS_SPI_GPIO_NUM]->ALTFUNCSET |=
-                                (1 << CMSDK_GPIO_ALTFUNC_SH1_CS_SPI_SET);
-    } else if (pin == ADC_SSEL) {
-        pin_position = 7;
-        GPIO_MAP[CMSDK_GPIO_ADC_CS_SPI_GPIO_NUM]->ALTFUNCSET |=
-                                (1 << CMSDK_GPIO_ALTFUNC_ADC_CS_SPI_SET);
     } else {
+        /* Check if pin is a MISC pin */
         pin_position = get_fpga_misc_pin_pos(pin);
         if (pin_position != RESERVED_MISC_PIN) {
             obj->reg_data = &MPS2_FPGAIO->MISC;
@@ -154,9 +159,8 @@ void gpio_init(gpio_t *obj, PinName pin)
         }
     }
 
-    obj->pin  = pin;
-    obj->mask = (0x1 << pin_position);
-    obj->pin_number = pin;
+    /* Set pin mask */
+    obj->mask = (1 << pin_position);
 }
 
 void gpio_mode(gpio_t *obj, PinMode mode)
diff --git a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/pinmap.c b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/pinmap.c
index 136a71e96a..bb752f16be 100644
--- a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/pinmap.c
+++ b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/pinmap.c
@@ -17,12 +17,34 @@
 #include "pinmap.h"
 #include "mbed_error.h"
 
+#define GET_GPIO_PIN_POS(pin)  (pin & 0x0F)  /* pin % 16 */
+#define GET_GPIO_MAP_NUM(pin)  (pin >> 4)    /* pin / 16 */
+#define GPIO_NUM               4
+
+static CMSDK_GPIO_TypeDef* GPIO_MAP[GPIO_NUM] = {
+    CMSDK_GPIO0,
+    CMSDK_GPIO1,
+    CMSDK_GPIO2,
+    CMSDK_GPIO3
+};
 
 void pin_function(PinName pin, int function)
 {
+    CMSDK_GPIO_TypeDef* p_gpio_map = 0;
+
     MBED_ASSERT(pin != (PinName)NC);
 
-    /* TODO */
+    if (pin >= EXP0 && pin <= EXP51) {
+        if (function == ALTERNATE_FUNC) {
+            p_gpio_map = GPIO_MAP[GET_GPIO_MAP_NUM(pin)];
+            p_gpio_map->ALTFUNCSET = (1 << GET_GPIO_PIN_POS(pin));
+        } else if(function == GPIO_FUNC) {
+            p_gpio_map = GPIO_MAP[GET_GPIO_MAP_NUM(pin)];
+            p_gpio_map->ALTFUNCCLR = (1 << GET_GPIO_PIN_POS(pin));
+        } else {
+            error("Invalid pin_function value %d", function);
+        }
+    }
 }
 
 void pin_mode(PinName pin, PinMode mode)
diff --git a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/serial_api.c b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/serial_api.c
index 9c6494a145..828d162a11 100644
--- a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/serial_api.c
+++ b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/serial_api.c
@@ -32,25 +32,23 @@
 static const PinMap PinMap_UART_TX[] = {
     {MCC_TX  , UART_0, 0},
     {USBTX   , UART_1, 0},
-    {XB_TX   , UART_2, 0},
-    {SH0_TX  , UART_3, 0},
-    {SH1_TX  , UART_4, 0},
+    {SH0_TX  , UART_2, ALTERNATE_FUNC},
+    {SH1_TX  , UART_3, ALTERNATE_FUNC},
+    {XB_TX   , UART_4, ALTERNATE_FUNC},
     {NC      , NC    , 0}
 };
 
 static const PinMap PinMap_UART_RX[] = {
     {MCC_RX  , UART_0, 0},
     {USBRX   , UART_1, 0},
-    {XB_RX   , UART_2, 0},
-    {SH0_RX  , UART_3, 0},
-    {SH1_RX  , UART_4, 0},
+    {SH0_RX  , UART_2, ALTERNATE_FUNC},
+    {SH1_RX  , UART_3, ALTERNATE_FUNC},
+    {XB_RX   , UART_4, ALTERNATE_FUNC},
     {NC      , NC    , 0}
 };
 
 #define UART_NUM    5
 
-extern CMSDK_GPIO_TypeDef* GPIO_MAP[];
-
 static uart_irq_handler irq_handler;
 
 int stdio_uart_inited = 0;
@@ -66,6 +64,8 @@ static struct serial_global_data_s uart_data[UART_NUM];
 
 void serial_init(serial_t *obj, PinName tx, PinName rx)
 {
+    uint32_t uart_ctrl = 0;
+
     /* Determine the UART to use */
     UARTName uart_tx = (UARTName)pinmap_peripheral(tx, PinMap_UART_TX);
     UARTName uart_rx = (UARTName)pinmap_peripheral(rx, PinMap_UART_RX);
@@ -78,97 +78,48 @@ void serial_init(serial_t *obj, PinName tx, PinName rx)
 
     obj->uart = (CMSDK_UART_TypeDef *)uart;
 
+    if (tx != NC) {
+        uart_ctrl = 0x01;   /* TX enable */
+    }
+    if (rx != NC) {
+        uart_ctrl |= 0x02;  /* RX enable */
+    }
+
     switch (uart) {
         case UART_0:
-            /* Disable UART when changing configuration */
-            CMSDK_UART0->CTRL = 0;
-            if((int)tx != NC) {
-              CMSDK_UART0->CTRL |= 0x01;   /* TX enable */
-            }
-            if((int)rx != NC) {
-              CMSDK_UART0->CTRL |= 0x02;   /* RX enable */
-            }
+            CMSDK_UART0->CTRL = uart_ctrl;
+            obj->index = 0;
             break;
         case UART_1:
-            /* Disable UART when changing configuration */
-            CMSDK_UART1->CTRL = 0;
-            if((int)tx != NC) {
-              CMSDK_UART1->CTRL |= 0x01;   /* TX enable */
-            }
-            if((int)rx != NC) {
-              CMSDK_UART1->CTRL |= 0x02;   /* RX enable */
-            }
+            CMSDK_UART1->CTRL = uart_ctrl;
+            obj->index = 1;
             break;
         case UART_2:
-            /* Disable UART when changing configuration */
-            CMSDK_UART2->CTRL = 0x00;
-            if ((int)tx != NC) {
-                CMSDK_UART2->CTRL = 0x1;   /* TX enable */
-                GPIO_MAP[CMSDK_GPIO_SH0_UART2_TX_GPIO_NUM]->ALTFUNCSET |=
-                                   (1<<CMSDK_GPIO_ALTFUNC_SH0_UART2_TX_SET);
-            }
-            if ((int)rx != NC) {
-                CMSDK_UART2->CTRL |= 0x2; /* RX enable */
-                GPIO_MAP[CMSDK_GPIO_SH0_UART2_RX_GPIO_NUM]->ALTFUNCSET |=
-                                   (1<<CMSDK_GPIO_ALTFUNC_SH0_UART2_RX_SET);
-            }
+            CMSDK_UART2->CTRL = 0;
+            obj->index = 2;
+            pin_function(tx, ALTERNATE_FUNC);
+            pin_function(rx, ALTERNATE_FUNC);
+            CMSDK_UART2->CTRL = uart_ctrl;
             break;
         case UART_3:
-            /* Disable UART when changing configuration */
             CMSDK_UART3->CTRL = 0;
-            if ((int)tx != NC) {
-              CMSDK_UART3->CTRL = 0x1;   /* TX enable */
-              GPIO_MAP[CMSDK_GPIO_SH1_UART3_TX_GPIO_NUM]->ALTFUNCSET |=
-                                   (1<<CMSDK_GPIO_ALTFUNC_SH1_UART3_TX_SET);
-            }
-            if((int)rx != NC)
-            {
-              CMSDK_UART3->CTRL |= 0x2;  /* RX enable */
-              GPIO_MAP[CMSDK_GPIO_SH1_UART3_RX_GPIO_NUM]->ALTFUNCSET |=
-                                   (1<<CMSDK_GPIO_ALTFUNC_SH1_UART3_RX_SET);
-            }
+            obj->index = 3;
+            pin_function(tx, ALTERNATE_FUNC);
+            pin_function(rx, ALTERNATE_FUNC);
+            CMSDK_UART3->CTRL = uart_ctrl;
             break;
         case UART_4:
-            /* Disable UART when changing configuration */
-            CMSDK_UART4->CTRL = 0x00;
-            if ((int)uart_tx != NC) {
-                CMSDK_UART4->CTRL |= 0x01;  /* TX enable */
-                GPIO_MAP[CMSDK_GPIO_UART4_TX_GPIO_NUM]->ALTFUNCSET |=
-                                       (1<<CMSDK_GPIO_ALTFUNC_UART4_TX_SET);
-            }
-            if((int)uart_rx != NC) {
-                CMSDK_UART4->CTRL |= 0x02;  /* RX enable */
-                GPIO_MAP[CMSDK_GPIO_UART4_RX_GPIO_NUM]->ALTFUNCSET |=
-                                       (1<<CMSDK_GPIO_ALTFUNC_UART4_RX_SET);
-            }
+            CMSDK_UART4->CTRL = 0;
+            obj->index = 4;
+            pin_function(tx, ALTERNATE_FUNC);
+            pin_function(rx, ALTERNATE_FUNC);
+            CMSDK_UART4->CTRL = uart_ctrl;
             break;
     }
 
     /* Set default baud rate and format */
     serial_baud(obj, 9600);
 
-    /* Pinout the chosen uart */
-    pinmap_pinout(tx, PinMap_UART_TX);
-    pinmap_pinout(rx, PinMap_UART_RX);
-
-    switch (uart) {
-        case UART_0:
-            obj->index = 0;
-            break;
-        case UART_1:
-            obj->index = 1;
-            break;
-        case UART_2:
-            obj->index = 2;
-            break;
-        case UART_3:
-            obj->index = 3;
-            break;
-        case UART_4:
-            obj->index = 4;
-            break;
-    }
-
     /*
      *  The CMSDK APB UART doesn't have support for flow control.
      *  Ref. DDI0479C_cortex_m_system_design_kit_r1p0_trm.pdf
@@ -180,6 +131,9 @@ void serial_init(serial_t *obj, PinName tx, PinName rx)
         stdio_uart_inited = 1;
         memcpy(&stdio_uart, obj, sizeof(serial_t));
     }
+
+    /* Clear UART */
+    serial_clear(obj);
 }
 
 void serial_free(serial_t *obj)
diff --git a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/spi_api.c b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/spi_api.c
index f69419a409..b42079e3a8 100644
--- a/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/spi_api.c
+++ b/targets/TARGET_ARM_SSG/TARGET_CM3DS_MPS2/spi_api.c
@@ -29,49 +29,46 @@
 #define SPI_PL022_SSPCR0_SCR_MSK     (0xFFul<<SPI_PL022_SSPCR0_SCR_POS)
 
 static const PinMap PinMap_SPI_SCLK[] = {
-    {SPI_SCLK , SPI_0, 0},
-    {CLCD_SCLK , SPI_1, 0},
-    {ADC_SCLK , SPI_2, 0},
-    {SHIELD_0_SPI_SCK , SPI_3, 0},
-    {SHIELD_1_SPI_SCK , SPI_4, 0},
-    {NC   , NC   , 0}
+    {SPI_SCLK         , SPI_0, 0},
+    {CLCD_SCLK        , SPI_1, 0},
+    {ADC_SCLK         , SPI_2, ALTERNATE_FUNC},
+    {SHIELD_0_SPI_SCK , SPI_3, ALTERNATE_FUNC},
+    {SHIELD_1_SPI_SCK , SPI_4, ALTERNATE_FUNC},
+    {NC               , NC   , 0}
 };
 
 static const PinMap PinMap_SPI_MOSI[] = {
-    {SPI_MOSI, SPI_0, 0},
-    {CLCD_MOSI, SPI_1, 0},
-    {ADC_MOSI, SPI_2, 0},
-    {SHIELD_0_SPI_MOSI, SPI_3, 0},
-    {SHIELD_1_SPI_MOSI, SPI_4, 0},
-    {NC   , NC   , 0}
+    {SPI_MOSI         , SPI_0, 0},
+    {CLCD_MOSI        , SPI_1, 0},
+    {ADC_MOSI         , SPI_2, ALTERNATE_FUNC},
+    {SHIELD_0_SPI_MOSI, SPI_3, ALTERNATE_FUNC},
+    {SHIELD_1_SPI_MOSI, SPI_4, ALTERNATE_FUNC},
+    {NC               , NC   , 0}
 };
 
 static const PinMap PinMap_SPI_MISO[] = {
-    {SPI_MISO, SPI_0, 0},
-    {CLCD_MISO, SPI_1, 0},
-    {ADC_MISO, SPI_2, 0},
-    {SHIELD_0_SPI_MISO, SPI_3, 0},
-    {SHIELD_1_SPI_MISO, SPI_4, 0},
-    {NC   , NC   , 0}
+    {SPI_MISO         , SPI_0, 0},
+    {CLCD_MISO        , SPI_1, 0},
+    {ADC_MISO         , SPI_2, ALTERNATE_FUNC},
+    {SHIELD_0_SPI_MISO, SPI_3, ALTERNATE_FUNC},
+    {SHIELD_1_SPI_MISO, SPI_4, ALTERNATE_FUNC},
+    {NC               , NC   , 0}
 };
 
 static const PinMap PinMap_SPI_SSEL[] = {
-    {SPI_SSEL, SPI_0, 0},
-    {CLCD_SSEL, SPI_1, 0},
-    {ADC_SSEL, SPI_2, 0},
-    {SHIELD_0_SPI_nCS, SPI_3, 0},
-    {SHIELD_1_SPI_nCS, SPI_4, 0},
-    {NC   , NC   , 0}
+    {SPI_SSEL        , SPI_0, 0},
+    {CLCD_SSEL       , SPI_1, 0},
+    {ADC_SSEL        , SPI_2, ALTERNATE_FUNC},
+    {SHIELD_0_SPI_nCS, SPI_3, ALTERNATE_FUNC},
+    {SHIELD_1_SPI_nCS, SPI_4, ALTERNATE_FUNC},
+    {NC              , NC   , 0}
 };
 
-extern CMSDK_GPIO_TypeDef* GPIO_MAP[];
-
 static inline int ssp_disable(spi_t *obj);
 static inline int ssp_enable(spi_t *obj);
 
 void spi_init(spi_t *obj, PinName mosi, PinName miso, PinName sclk, PinName ssel)
 {
-    int altfunction[4];
     /* Determine the SPI to use */
     SPIName spi_mosi = (SPIName)pinmap_peripheral(mosi, PinMap_SPI_MOSI);
     SPIName spi_miso = (SPIName)pinmap_peripheral(miso, PinMap_SPI_MISO);
@@ -83,12 +80,11 @@ void spi_init(spi_t *obj, PinName mosi, PinName miso, PinName sclk, PinName ssel
     obj->spi = (MPS2_SSP_TypeDef*)pinmap_merge(spi_data, spi_cntl);
     if ((int)obj->spi == NC) {
         error("SPI pinout mapping failed");
-        return;
     }
 
     /* Enable power and clocking */
     switch ((int)obj->spi) {
-        case (int)SPI_0:
+        case SPI_0:
             obj->spi->CR1   = 0;
             obj->spi->CR0   = SSP_CR0_SCR_DFLT | SSP_CR0_FRF_MOT | SSP_CR0_DSS_8;
             obj->spi->CPSR  = SSP_CPSR_DFLT;
@@ -97,7 +93,7 @@ void spi_init(spi_t *obj, PinName mosi, PinName miso, PinName sclk, PinName ssel
             obj->spi->CR1   = SSP_CR1_SSE_Msk;
             obj->spi->ICR   = 0x3;
             break;
-        case (int)SPI_1: /* Configure SSP used for LCD */
+        case SPI_1: /* Configure SSP used for LCD */
             obj->spi->CR1   = 0;                /* Synchronous serial port disable  */
             obj->spi->DMACR = 0;                /* Disable FIFO DMA                 */
             obj->spi->IMSC  = 0;                /* Mask all FIFO/IRQ interrupts     */
@@ -112,7 +108,9 @@ void spi_init(spi_t *obj, PinName mosi, PinName miso, PinName sclk, PinName ssel
             obj->spi->CR1   = ((1ul <<  1) |    /* Synchronous serial port enable   */
                                (0ul <<  2) );   /* Device configured as master      */
             break;
-        case (int)SPI_2:
+        case SPI_2:  /* Shield ADC SPI */
+        case SPI_3:  /* Shield 0 SPI */
+        case SPI_4:  /* Shield 1 SPI */
             obj->spi->CR1   = 0;
             obj->spi->CR0   = SSP_CR0_SCR_DFLT | SSP_CR0_FRF_MOT | SSP_CR0_DSS_8;
             obj->spi->CPSR  = SSP_CPSR_DFLT;
@@ -120,79 +118,19 @@ void spi_init(spi_t *obj, PinName mosi, PinName miso, PinName sclk, PinName ssel
             obj->spi->DMACR = 0;
             obj->spi->CR1   = SSP_CR1_SSE_Msk;
             obj->spi->ICR   = 0x3;
-            break;
-        case (int)SPI_3:
-            obj->spi->CR1   = 0;
-            obj->spi->CR0   = SSP_CR0_SCR_DFLT | SSP_CR0_FRF_MOT | SSP_CR0_DSS_8;
-            obj->spi->CPSR  = SSP_CPSR_DFLT;
-            obj->spi->IMSC  = 0x8;
-            obj->spi->DMACR = 0;
-            obj->spi->CR1   = SSP_CR1_SSE_Msk;
-            obj->spi->ICR   = 0x3;
-            break;
-        case (int)SPI_4:
-            obj->spi->CR1   = 0;
-            obj->spi->CR0   = SSP_CR0_SCR_DFLT | SSP_CR0_FRF_MOT | SSP_CR0_DSS_8;
-            obj->spi->CPSR  = SSP_CPSR_DFLT;
-            obj->spi->IMSC  = 0x8;
-            obj->spi->DMACR = 0;
-            obj->spi->CR1   = SSP_CR1_SSE_Msk;
-            obj->spi->ICR   = 0x3;
-            break;
-    }
-
-    if (mosi != NC) {
-        altfunction[0] = 1;
-    } else {
-        altfunction[0] = 0;
-    }
-    if (miso != NC) {
-        altfunction[1] = 1;
-    } else {
-        altfunction[1] = 0;
-    }
-    if (sclk != NC) {
-        altfunction[2] = 1;
-    } else {
-        altfunction[2] = 0;
-    }
-    if (ssel != NC) {
-        altfunction[3] = 1;
-    } else {
-        altfunction[3] = 0;
-    }
-
-    /* Enable alt function */
-    switch ((int)obj->spi) {
-        case (int)SPI_2:    /* Shield ADC SPI */
-            GPIO_MAP[CMSDK_GPIO_ADC_MOSI_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[0]<<CMSDK_GPIO_ALTFUNC_ADC_MOSI_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_ADC_MISO_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[1]<<CMSDK_GPIO_ALTFUNC_ADC_MISO_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_ADC_SCK_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[2]<<CMSDK_GPIO_ALTFUNC_ADC_SCK_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_ADC_CS_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[3]<<CMSDK_GPIO_ALTFUNC_ADC_CS_SPI_SET);
-            break;
-        case (int)SPI_3:   /* Shield 0 SPI */
-            GPIO_MAP[CMSDK_GPIO_SH0_MOSI_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[0]<<CMSDK_GPIO_ALTFUNC_SH0_MOSI_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_SH0_MISO_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[1]<<CMSDK_GPIO_ALTFUNC_SH0_MISO_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_SH0_SCK_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[2]<<CMSDK_GPIO_ALTFUNC_SH0_SCK_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_SH0_CS_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[3]<<CMSDK_GPIO_ALTFUNC_SH0_CS_SPI_SET);
-            break;
-        case (int)SPI_4:   /* Shield 1 SPI */
-            GPIO_MAP[CMSDK_GPIO_SH1_MOSI_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[0]<<CMSDK_GPIO_ALTFUNC_SH1_MOSI_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_SH1_MISO_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[1]<<CMSDK_GPIO_ALTFUNC_SH1_MISO_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_SH1_SCK_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[2]<<CMSDK_GPIO_ALTFUNC_SH1_SCK_SPI_SET);
-            GPIO_MAP[CMSDK_GPIO_SH1_CS_SPI_GPIO_NUM]->ALTFUNCSET |=
-                        (altfunction[3]<<CMSDK_GPIO_ALTFUNC_SH1_CS_SPI_SET);
+            /* Set pin function as an alt-function */
+            if (mosi != NC) {
+                pin_function(mosi, ALTERNATE_FUNC);
+            }
+            if (miso != NC) {
+                pin_function(miso, ALTERNATE_FUNC);
+            }
+            if (sclk != NC) {
+                pin_function(sclk, ALTERNATE_FUNC);
+            }
+            if (ssel != NC) {
+                pin_function(ssel, ALTERNATE_FUNC);
+            }
             break;
     }
 
@@ -208,14 +146,6 @@ void spi_init(spi_t *obj, PinName mosi, PinName miso, PinName sclk, PinName ssel
 
     /* Enable the ssp channel */
     ssp_enable(obj);
-
-    /* pin out the spi pins */
-    pinmap_pinout(mosi, PinMap_SPI_MOSI);
-    pinmap_pinout(miso, PinMap_SPI_MISO);
-    pinmap_pinout(sclk, PinMap_SPI_SCLK);
-    if (ssel != NC) {
-        pinmap_pinout(ssel, PinMap_SPI_SSEL);
-    }
 }
 
 void spi_free(spi_t *obj)
@@ -250,7 +180,7 @@ void spi_format(spi_t *obj, int bits, int mode, int slave)
     tmp = obj->spi->CR1;
     tmp &= ~(0xD);
     tmp |= 0 << 0                  /* LBM - loop back mode - off */
-        | ((slave) ? 1 : 0) << 2   /* MS - master slave mode, 1 = slave */
+        | ((slave) ? 1 : 0) << 2   /* MS  - master slave mode, 1 = slave */
         | 0 << 3;                  /* SOD - slave output disable - na */
     obj->spi->CR1 = tmp;
 
@@ -260,12 +190,13 @@ void spi_format(spi_t *obj, int bits, int mode, int slave)
 void spi_frequency(spi_t *obj, int hz)
 {
     uint32_t clkps_dvsr, scr;
+    uint32_t sys_clk = SystemCoreClock;
 
     for(clkps_dvsr = SPI_PL022_MIN_SSPCPSR_VALUE;
         clkps_dvsr <= SPI_PL022_MAX_SSPCPSR_VALUE; clkps_dvsr += 2) {
 
         /* Calculate clock rate based on the new clock prescale divisor */
-        scr = (SystemCoreClock / (clkps_dvsr * hz)) - 1;
+        scr = (sys_clk / (clkps_dvsr * hz)) - 1;
 
         /* Checks if it can be supported by the divider */
         if (scr <= SPI_PL022_MAX_SRC_VALUE) {
@@ -278,7 +209,7 @@ void spi_frequency(spi_t *obj, int hz)
         }
     }
 
-    error("Couldn't setup requested SPI frequency");
+    error("Couldn't setup requested SPI frequency %dHz", hz);
 }
 
 static inline int ssp_disable(spi_t *obj)