add sdc-600 module for secure debugging support

This driver module add support for sdc-600 which enables
secure debugging using Arm Coresight.There is one internal
and external internal COM port for the communication between
the debugger and debugged system.

links:
Advanced Communication channel: http://infocenter.arm.com/help/topic/com.arm.doc.subset.coresight.architecture/index.html
SDC-600 : http://infocenter.arm.com/help/topic/com.arm.doc.101130_0002_02_en/coresight_sdc_600_technical_reference_manual_101130_0002_02_en.pdf

Change-Id: I579b026500f4f38fc45186a033817fd7ca6e9798
Signed-off-by: Tushar Khandelwal <tushar.khandelwal@arm.com>

9 files changed, 933 insertions, 11 deletions

diff --git a/module/interrupt_router/src/mod_intr_router.c b/module/interrupt_router/src/mod_intr_router.c
index 88a646c..dbf0543 100755
--- a/module/interrupt_router/src/mod_intr_router.c
+++ b/module/interrupt_router/src/mod_intr_router.c
@@ -1,6 +1,6 @@
 /*
  *
- * Copyright (c) 2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -181,6 +181,7 @@ static int intr_router_start(fwk_id_t id)
                            *rtr_config);
 
     configure_intrtr_si(SI_REFCLK0, ROUTE_TO_SE);
+    configure_intrtr_si(SI_SDC600, ROUTE_TO_SE);
     configure_intrtr_si(SI_REFCLK1, ROUTE_TO_HOST);
     configure_intrtr_si(SI_REFCLK2, ROUTE_TO_HOST);
     configure_intrtr_si(SI_REFCLK3, ROUTE_TO_HOST);
diff --git a/module/sdc600/include/apbcom_hw.h b/module/sdc600/include/apbcom_hw.h
new file mode 100644
index 0000000..bb1ed20
--- /dev/null
+++ b/module/sdc600/include/apbcom_hw.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2020, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef _COM_PORT_DEF_H_
+#define _COM_PORT_DEF_H_
+
+#include <se_mmap.h>
+/* internal apbcom variant */
+#define APBCOM_REG_VIDR        0xD00
+#define APBCOM_REG_FIDTXR      0xD08
+#define APBCOM_REG_FIDRXR      0xD0C
+#define APBCOM_REG_ICSR        0xD10
+#define APBCOM_REG_DR          0xD20
+#define APBCOM_REG_SR          0xD2C
+#define APBCOM_REG_DBR         0xD30
+#define APBCOM_REG_SR_ALIAS    0xD3C
+
+#define APBCOM_REG_ITSTATUS    0xEFC
+#define APBCOM_REG_ITCTRL      0xF00
+#define APBCOM_REG_CLAIMSET    0xFA0
+#define APBCOM_REG_CLAIMCLR    0xFA4
+#define APBCOM_REG_AUTHSTATUS  0xFB8
+#define APBCOM_REG_DEVARCH     0xFBC
+#define APBCOM_REG_DEVID       0xFC8
+#define APBCOM_REG_PIDR4       0xFD0
+#define APBCOM_REG_PIDR0       0xFE0
+#define APBCOM_REG_PIDR1       0xFE4
+#define APBCOM_REG_PIDR2       0xFE8
+#define APBCOM_REG_PIDR3       0xFEC
+#define APBCOM_REG_CIDR0       0xFF0
+#define APBCOM_REG_CIDR1       0xFF4
+#define APBCOM_REG_CIDR2       0xFF8
+#define APBCOM_REG_CIDR3       0xFFC
+
+/* status register */
+#define SR_TXS_OFFSET          0
+#define SR_TXS_LEN             8
+#define SR_TXOE_OFFSET         13
+#define SR_TXOE_LEN            1
+#define SR_TXLE_OFFSET         14
+#define SR_TXLE_LEN            1
+#define SR_RXF_OFFSET          16
+#define SR_RXF_LEN             8
+#define SR_RXLE_OFFSET         30
+#define SR_RXLE_LEN            1
+
+#define APBCOM_ADDR(_a) \
+        ((unsigned int)(SE_APBCOM_BASE + _a))
+
+#define APBCOM_READ_WORD(_a) \
+        *((volatile unsigned int*)(APBCOM_ADDR(_a)))
+
+#define APBCOM_WRITE_WORD(_a, _val) \
+        *((unsigned int*)(APBCOM_ADDR(_a))) = *(uint32_t*)_val
+
+#define APBCOM_READ_FEILD(_a, _offset, _len) \
+        ((APBCOM_READ_WORD(_a) & (((1 << (_len)) - 1) << _offset)) >> _offset)
+
+#define APBCOM_GET_FEILD(_val, _offset, _len) \
+        (((_val) & (((1 << (_len)) - 1) << _offset)) >> _offset)
+
+// Flags bytes
+#define FLAG_IDR        0xA0
+#define FLAG_IDA        0xA1
+#define FLAG_LPH1RA     0xA6
+#define FLAG_LPH1RL     0xA7
+#define FLAG_LPH2RA     0xA8
+#define FLAG_LPH2RL     0xA9
+#define FLAG_LPH2RR     0xAA
+#define FLAG_START      0xAC
+#define FLAG_END        0xAD
+#define FLAG_ESC        0xAE
+#define FLAG__NULL      0xAF
+//0xB0 - 0xBF are Reserved
+#define FLAG_LAST       0xC0
+
+// Reboot types
+enum ResetType {
+    NONE,
+    nSRSTReset,
+    COMPortReset
+};
+
+#endif /* _COM_PORT_DEF_H_ */
diff --git a/module/sdc600/include/mod_sdc600.h b/module/sdc600/include/mod_sdc600.h
new file mode 100644
index 0000000..7d5752b
--- /dev/null
+++ b/module/sdc600/include/mod_sdc600.h
@@ -0,0 +1,61 @@
+/*
+ *
+ * Copyright (c) 2020, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef MOD_SDC600_H
+#define MOD_SDC600_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <fwk_id.h>
+#include <fwk_module_idx.h>
+
+typedef enum rc_t {
+    SUCCESS,
+    DEBUGGER_NOT_CONNECTED,
+    FAIL,
+} rc_t;
+
+typedef enum rx_rc_t {
+    RX_SUCCESS,
+    RX_PROT_ERROR,
+    RX_BUFFER_OVERFLOW,
+    RX_DISCONNECT,
+    RX_FAIL,
+} rx_rc_t;
+
+typedef enum tx_rc_t {
+    TX_SUCCESS,
+    TX_TIMEOUT,
+    TX_DISCONNECT,
+    TX_FAIL,
+} tx_rc_t;
+
+
+typedef enum command_id_t {
+    GET_SOC_ID = 0x1,
+    INTRD_DEB_CERT = 0x2,
+    RESUME_BOOT = 0x3,
+    DISABLE_PORTS = 0x4,
+    NUM_OF_CMDS,
+} command_id_t;
+
+typedef enum lcs_state {
+    TO_CM,
+    TO_DM,
+    TO_SEC,
+    TO_RMA,
+} lcs_state;
+
+struct mod_sdc600_config {
+    /*! SDC600 device IRQ number */
+    unsigned int irq;
+    unsigned int is_irq_collated;
+};
+
+
+#endif /* MOD_SDC600_H */
diff --git a/module/sdc600/src/Makefile b/module/sdc600/src/Makefile
new file mode 100644
index 0000000..1981581
--- /dev/null
+++ b/module/sdc600/src/Makefile
@@ -0,0 +1,10 @@
+#
+# Copyright (c) 2020, Arm Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+BS_LIB_NAME := "SDC600"
+BS_LIB_SOURCES = mod_sdc600.c
+
+include $(BS_DIR)/lib.mk
diff --git a/module/sdc600/src/mod_sdc600.c b/module/sdc600/src/mod_sdc600.c
new file mode 100755
index 0000000..0383cc1
--- /dev/null
+++ b/module/sdc600/src/mod_sdc600.c
@@ -0,0 +1,728 @@
+/*
+ *
+ * Copyright (c) 2020, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Description:
+ *      Secure Debug SDC-600 Device Driver.
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+#include <fwk_errno.h>
+#include <fwk_id.h>
+#include <fwk_interrupt.h>
+#include <fwk_mm.h>
+#include <fwk_module.h>
+#include <fwk_module_idx.h>
+#include <mod_intr_col.h>
+#include <mod_log.h>
+#include <se_irq.h>
+#include <apbcom_hw.h>
+#include <mod_sdc600.h>
+
+/*Version IDs*/
+#define  ARCH_CORESIGHT_SDC600   0x47700A57
+#define  ARCH_CORESIGHT_ROMTABLE 0x47700AF7
+
+#define BLOCKED         true
+#define NOT_BLOCKED     false
+#define SKIP_ERROR_CHECK    true
+#define ERROR_CHECK         false
+
+struct __sdc600_ctx {
+    const struct mod_log_api *log_api;
+    const struct mod_intr_col_driver_api *intr_col_api;
+    bool portinitialized;
+} sdc600_ctx;
+
+typedef uint8_t nonce_t[32];
+typedef uint8_t soc_id_t[32];
+#define MAX_MSG_SIZE    (512) /*This value is chosen as it is sufficient for
+                                this demo only.*/
+
+static bool comport_isflag(uint8_t byte, uint8_t flag)
+{
+    if (byte == flag)
+        return true;
+
+    return false;
+}
+
+static rx_rc_t comport_readbyte(uint8_t *byte,
+    bool isblocked,
+    bool is_skip_errorscheck)
+{
+    rx_rc_t res = RX_SUCCESS;
+
+    bool txoverflow = false;
+    uint8_t rxdata = 0;
+    uint32_t sr = 0;
+
+    do {
+        sr = APBCOM_READ_WORD(APBCOM_REG_SR);
+        if (is_skip_errorscheck == ERROR_CHECK) {
+            txoverflow = APBCOM_GET_FEILD(sr, SR_TXOE_OFFSET, SR_TXOE_LEN) > 0;
+            if (txoverflow) {
+                sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+                           "[SDC-600] buffer overflow detected [%u]\n",
+                           txoverflow);
+                res = RX_BUFFER_OVERFLOW;
+                goto bail;
+            }
+        }
+
+        rxdata = APBCOM_GET_FEILD(sr, SR_RXF_OFFSET, SR_RXF_LEN);
+        if (rxdata > 0)
+            break;
+    } while (isblocked);
+
+    /* this driver is hard coded to work with fifo size 1 */
+    *byte = APBCOM_READ_WORD(APBCOM_REG_DR) & 0xFF;
+
+bail:
+    return res;
+}
+
+static tx_rc_t comport_sendbyte(uint8_t byte)
+{
+    tx_rc_t res = TX_SUCCESS;
+
+    uint8_t txfifo_freesapce_inbytes = 0;
+
+    const uint32_t MAX_NUM_OF_RETRIES = 50000;
+    uint32_t numofretries = 0;
+    uint32_t sr = 0;
+
+    while (numofretries++ < MAX_NUM_OF_RETRIES) {
+        sr = APBCOM_READ_WORD(APBCOM_REG_SR);
+
+        if (APBCOM_GET_FEILD(sr, SR_TXLE_OFFSET, SR_TXLE_LEN) > 0) {
+            sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+                "[SDC-600] link error detected\n");
+            res = TX_DISCONNECT;
+            goto bail;
+        }
+
+        if (APBCOM_GET_FEILD(sr, SR_TXOE_OFFSET, SR_TXOE_LEN) > 0) {
+            sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+                "[SDC-600] overflow detected\n");
+            res = TX_FAIL;
+            goto bail;
+        }
+
+        txfifo_freesapce_inbytes =
+            APBCOM_GET_FEILD(sr, SR_TXS_OFFSET, SR_TXS_LEN);
+        if (txfifo_freesapce_inbytes > 0)
+            break;
+    }
+
+    if (numofretries >= MAX_NUM_OF_RETRIES) {
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] write operation timed  out\n");
+        res = TX_TIMEOUT;
+        goto bail;
+    }
+
+    uint8_t txData[4] = { byte, FLAG__NULL, FLAG__NULL, FLAG__NULL };
+    APBCOM_WRITE_WORD(APBCOM_REG_DBR, txData);
+
+bail:
+    return res;
+}
+
+static rc_t comport_preparedata(uint8_t startflag,
+    const uint8_t *data,
+    size_t insize,
+    uint8_t *outdata,
+    size_t outdatabuffersize,
+    size_t *outsize)
+{
+    rc_t res = SUCCESS;
+
+    /*
+     * message bytes could be same as flag bytes in which case it has
+     * to be preceded by FLAG_ESC thereby incresing the size to
+     * Double size + START and END flags
+    */
+    uint8_t *tempdata = calloc(1, (insize * 2) + 2);
+    uint16_t outbyteindex = 0;
+    uint32_t byte_index = 0;
+
+    tempdata[outbyteindex++] = startflag;
+
+    for (byte_index = 0; byte_index < insize; ++byte_index) {
+        /* Each Message byte that matches one of the Flag bytes is
+         * immediately preceded by the ESC Flag byte, and bit [7]
+         * of the Message byte is inverted.
+        */
+        if (data[byte_index] >= FLAG_IDR && data[byte_index] < FLAG_LAST) {
+            tempdata[outbyteindex++] = FLAG_ESC;
+            tempdata[outbyteindex++] = data[byte_index] & ~0x80UL;
+        } else {
+            tempdata[outbyteindex++] = data[byte_index];
+        }
+    }
+
+    tempdata[outbyteindex++] = FLAG_END;
+
+    if (outbyteindex > outdatabuffersize) {
+        *outsize = 0;
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] buffer is small,outbyteindex[%u] outDatabufferSize[%u]\n",
+            outbyteindex,
+            outdatabuffersize);
+        res = FAIL;
+        goto bail;
+    } else {
+        memcpy(outdata, tempdata, outbyteindex);
+        *outsize = outbyteindex;
+    }
+
+bail:
+    free(tempdata);
+
+    return res;
+}
+
+/**
+ * The transmit side of the driver receives from the Secure Debug Handler a
+ * message to transmit in the provided buffer. The transmitter is transparent
+ * to the caller and the provided buffer may include any byte values (including
+ * values that are SDC-600 flag bytes). The comport_txint provides transparent
+ * transmit interface.
+ * The comport_txint function is responsible to inject to the Internal COM Port
+ * transmit FIFO HW a START flag, followed by the message bytes from the buffer
+ * and at the end it is responsible to inject to the transmit HW an END flag.
+ * While transmitting any byte from the buffer, the driver must detect if it is
+ * one of the SDC-600 COM Port flag values (bytes with an upper 3 bits of b101
+ * are classified as Flag bytes). In such case the driver must inject ESC flag
+ * to the transmitter and flips the MS bit of the byte and transmits the
+ * modified byte. At the end of transmission of the buffer the comport_txint
+ * function is responsible to inject to the External COM Port transmit FIFO HW
+ * an END flag.
+ *
+ * @param txbuffer          [in]
+ * @param txbufferlength    [in]
+ * @param actuallength      [out]
+ * @return
+ */
+
+static tx_rc_t comport_txint(uint8_t startflag,
+    uint8_t *txbuffer,
+    size_t txbufferlength,
+    size_t *actuallength)
+{
+    tx_rc_t res = TX_SUCCESS;
+    rc_t ret = SUCCESS;
+    size_t byteindex = 0;
+
+    /* allocate new buffer that will include the extra bytes */
+    size_t tempbufflen = txbufferlength * 2 + 2;
+    uint8_t *tempbuffer = calloc(1, tempbufflen);
+        /* fill the array */
+    ret = comport_preparedata(startflag,
+                        txbuffer,
+                        txbufferlength,
+                        tempbuffer,
+                        tempbufflen,
+                        actuallength);
+    if (ret != SUCCESS) {
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Buffer fill failed");
+            return TX_FAIL;
+    }
+
+    /* send all bytes */
+    for (byteindex = 0; byteindex < *actuallength; ++byteindex) {
+        res = comport_sendbyte(tempbuffer[byteindex]);
+        if (res != TX_SUCCESS) {
+            sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+                "[SDC-600] failed to send byte num[%u][0x%02x]\n",
+                byteindex,
+                tempbuffer[byteindex]);
+            goto bail;
+        }
+    }
+
+bail:
+    free(tempbuffer);
+    return res;
+
+}
+
+
+/**
+ * This function is called by the Secure Debug Handler to initiate the Internal
+ * COM port driver and the COM port link. The Internal COM port driver checks if
+ * there is any RX symbol in the Internal COM Port FIFO to find if the debugger
+ * platform is connected and check if link is established by the External COM
+ * Port (LPH2RA symbol in the Internal COM Port FIFO).
+ *
+ * Once the driver detects LPH2RA, the driver transmits LPH2RA to the debugger,
+ * establishes the link to the external COM port and waits for IDA command. Once
+ * received, the driver responds with the high level protocol provided value.
+ *
+ * @return
+ */
+
+rc_t comport_init(void)
+{
+    uint8_t readbyte = 0;
+    uint32_t status;
+    int res = SUCCESS;
+    uint8_t identification[6] = {0};
+    static const char APBCOM_ID[6] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06 };
+
+     /**
+     * 1. Fill the buffer with high level protocol ID value.
+     */
+    memcpy(identification, APBCOM_ID, sizeof(identification));
+
+    /**
+    *  2. The Internal COM Port device HW detects the status of the LINKEST
+    *     signal.
+    *     If it is set to 1 the HW inserts LPH2RA flag to the Internal COM
+    *     Port's RX FIFO,
+    *     otherwise the RX FIFO remains empty. If LPH2RA symbol is inserted to
+    *     the RX FIFO then the Internal COM Port device interrupts its driver
+    *     Note: if the Internal COM Port CPU is asleep, this interrupt is
+    *     assumed to wake it up.
+
+    * 3.  Driver checks if the RX FIFO is not empty (RXF field of the Status
+    *     register is not 0).
+    * 4.  If no symbol found (FIFO is empty or FIFO returned value is NULL),
+    *     return with Debugger not connected status.
+
+    * 5.  If the driver reads a LPH2RA flag then it knows that the debugger
+    *     may be connected as the link from the External COM Port is set.
+    *     Driver goes to step 6 below.
+
+    * 6.  If the read symbol from the FIFO is anything else (garbage, leftover
+    *     from previous attempts), the driver stops the process.
+    */
+
+    status = comport_readbyte(&readbyte,
+                            BLOCKED,
+                            ERROR_CHECK);
+    if (status != RX_SUCCESS) {
+        res = FAIL;
+        return res;
+    }
+
+    if (comport_isflag(readbyte, FLAG_LPH2RA) != true) {
+        res = FAIL;
+        return res;
+    }
+    sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+        "[SDC-600] Recieved Connection Request\n");
+
+    /** Set the link back to the debugger:
+    * 7.  The Internal COM Port device driver writes LPH2RA flag to the Internal
+    *     COM Port device TX.
+    */
+    status = comport_sendbyte(FLAG_LPH2RA);
+    if (status != TX_SUCCESS) {
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Acknowledging Connection Request Failed\n");
+        res = FAIL;
+        return res;
+    }
+    sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+        "[SDC-600] ACKED Connection Request\n");
+
+    // TODO implement a timeout
+    /** Wait for External COM Port driver check of the debugged system protocol:
+    * 8.  Driver polls and reads a byte from the Internal COM Port FIFO.
+    *     After timeout the driver returns with Debugger not connected status.
+    */
+    status = comport_readbyte(&readbyte,
+                            BLOCKED,
+                            SKIP_ERROR_CHECK);
+    if (status != RX_SUCCESS) {
+        res = FAIL;
+        return res;
+    }
+
+    /**
+     * 9.  If the driver reads an IDR flag then it goes to step 10 below.
+     *     Otherwise it returns with unexpected symbol received ststus.
+     */
+    if (comport_isflag(readbyte, FLAG_IDR) == true) {
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Recieved IDR Request\n");
+
+        /**
+         * IDR was detected
+         * 10.  The driver responds and transmits to the debugger with
+         *     Identification response message from the IDBuffer.
+         *     Note: this response message format has a special format. It
+         *     starts with IDA flag, followed by 6 bytes of debugged system
+         *     ID hex value, and an END flag. If any of the platform ID
+         *     bytes has MS bits value of 101b then the transmit driver must
+         *     send an ESC flag following a flip of the MS bit of the byte
+         *     to transmit.
+         * 11. Return with success code.
+         */
+
+        size_t actuallength = 0;
+        status = comport_txint(FLAG_IDA,
+                               identification,
+                               sizeof(identification),
+                               &actuallength);
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Send IDA Protocol data\n");
+        if (status != TX_SUCCESS) {
+            sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+                "[SDC-600] Send IDA Protocol data Failed\n");
+            res = FAIL;
+            return res;
+        }
+    } else {
+        res = FAIL;
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] No IDR request Recieved\n");
+        return res;
+    }
+
+    return res;
+}
+
+
+/**
+ * This is the host response for Get SoC ID command.
+ *
+ * @param txbuff            [in]
+ * @param txbufflen         [in/out]
+ * @param status            [in]     Success = 0h, Fail = 1h
+ * @param challenge         [in]     Nonce that is randomized by the debugged
+ *                                   system when it received the Get SoC ID
+ *                                   command. The nonce must be used while
+ *                                   signing the debug certificate with the
+ *                                   SoCID.
+ *                                   This response will not be sent by the
+ *                                   debugged system if the IDR command was not
+ *                                   issued. If the debugged system does not
+ *                                   support challenge response, it fills this
+ *                                   field with zeroes.
+ * @param challengelen      [in]
+ * @param socid             [in]     The size and format of SoCID is
+ *                                   platform-specific
+ * @param socidlen          [in]
+ * @return
+ */
+
+static void form_soc_id_resp(uint8_t* txbuff,
+    size_t *txbufflen,
+    uint32_t status,
+    nonce_t *challenge,
+    size_t challengelen,
+    soc_id_t *socid,
+    size_t socidlen)
+{
+    const size_t MSG_SIZE = 65;
+    const size_t TX_SIZE = 3 + MSG_SIZE;
+    uint16_t len = MSG_SIZE;
+
+    memset(txbuff, 0, TX_SIZE);
+    txbuff[0] = GET_SOC_ID;                          /* command */
+    memcpy(txbuff + 1, &len, sizeof(len));           /* len */
+    txbuff[3] = status;                              /* status */
+
+    if (challenge != NULL)
+        memcpy(txbuff + 4, challenge, challengelen); /* nonce */
+
+    memcpy(txbuff + 36, socid, socidlen);            /* soc_id */
+
+    *txbufflen = TX_SIZE;
+}
+
+
+static rc_t handle_get_soc_id(void)
+{
+    rc_t res = SUCCESS;
+    tx_rc_t status = 0;
+    uint32_t socid[8] = { 0 };
+    static uint8_t txbuff[MAX_MSG_SIZE];
+    size_t txbufflen = sizeof(txbuff);
+    size_t actuallength = 0;
+
+    nonce_t challenge = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+        0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+        0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+        0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08
+    };
+
+    size_t challengelen = sizeof(challenge);
+
+    socid[0] = *((volatile unsigned int*)((unsigned int)SE_SYSTEMID_SOCID));
+
+    form_soc_id_resp(txbuff,
+                    &txbufflen,
+                    status,
+                    &challenge,
+                    challengelen,
+                    (soc_id_t *) &socid,
+                    sizeof(soc_id_t));
+
+    status = comport_txint(FLAG_START, txbuff, txbufflen, &actuallength);
+    if (status != TX_SUCCESS)
+        return FAIL;
+
+    return res;
+}
+
+/**
+ * The debugged system responds to debugger's Introduce Debug Certificate
+ * command with the below message after it analyzed the command and acted upon
+ * it.
+ *
+ * The debugged system may accept or reject the debugger request from the
+ * following reasons:
+ * •   Incompatible SoCID in the debug certificate.
+ * •   Old challenge value in the certificate.
+ * •   Bad integrity for the provided debug certificate
+ * •   Other
+ *
+ * The response includes the active debug configuration to let the debugger
+ * know what capabilities are now available for him.
+ *
+ * @param txbuff                [in]
+ * @param txbufflen             [in/out]
+ * @param waitfor_resumeboot    [in]     This field is meaningful only when the
+ *                                       status is success. If this flag is set
+ *                                       to 1 it tells the debugger that the
+ *                                       debugged system is waiting for a
+ *                                       resume boot command to resume its boot.
+ *                                       It is typically set by the boot
+ *                                       implementation of the Secure Debug
+ *                                       Handler. If the Secure Debug feature is
+ *                                       implemented in run time, this flag is
+ *                                       set to 0.
+ *
+ * @return
+ */
+
+
+static void form_debug_cert_resp(uint8_t* txbuff,
+    size_t *txbufflen,
+    uint32_t status,
+    uint8_t waitfor_resumeboot)
+{
+    const size_t MSG_SIZE = 2;
+    uint16_t len = MSG_SIZE;
+    const size_t TX_SIZE = 3 + MSG_SIZE;
+
+    memset(txbuff, 0, TX_SIZE);
+
+    txbuff[0] = INTRD_DEB_CERT;                 /* command */
+    memcpy(txbuff + 1, &len, 2);                /* len */
+    txbuff[3] = status;                         /* status */
+    txbuff[4] = waitfor_resumeboot;             /* waitfor_resumeboot */
+
+    *txbufflen = TX_SIZE;
+}
+
+static rc_t handle_introduce_debugcert(void)
+{
+    tx_rc_t status = TX_SUCCESS;
+    size_t actuallength = 0;
+    static uint8_t txbuff[MAX_MSG_SIZE];
+    size_t txbufflen = sizeof(txbuff);
+    /**
+     * This response should change the life cycle state after authenticating the
+     * debug certificate response using the Crypto functions.
+     * Currently it is TODO as there is no Crypto(SW/HW) functionality present
+     * in the framework.
+    */
+    // TODO: parse and authenticate the debug certificate
+    form_debug_cert_resp(txbuff, &txbufflen, 0, 0);
+    status = comport_txint(FLAG_START, txbuff, txbufflen, &actuallength);
+    if (status != TX_SUCCESS)
+        return FAIL;
+    return SUCCESS;
+}
+
+rc_t handle_message(uint8_t lastbyte)
+{
+    rc_t status = SUCCESS;
+    switch (lastbyte) {
+    case GET_SOC_ID:
+    /**
+    * Get SoC ID command:
+    *
+    * 11. read system register for SoCID
+    * 12. Future improvement: append a nonce to the challenge
+    * 13. Form SoCID response message (with the nonce)
+    * 14. Call comport_txint to transmit the response message
+    */
+    sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+        "[SDC-600] Send Get SoCID response\n");
+    status = handle_get_soc_id();
+    if (status != SUCCESS) {
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] SoCID response failed\n");
+        return status;
+    }
+    break;
+    case INTRD_DEB_CERT:
+    /**
+    * Introduce Debug Certificate command:
+    *
+    * 15. Call hal_securedebug_set API with the received certificate
+    * 16. Form Introduce Debug Certificate response message with
+    *         WaitForResumeBoot flag set to 0.
+    * 17. Call comport_txint to transmit the response message
+    */
+    sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+        "[SDC-600] Send Debug certicate response\n");
+    status = handle_introduce_debugcert();
+    if (status != SUCCESS) {
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Debug certicate response failed\n");
+        return status;
+    }
+    break;
+    default:
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Wrong message : %x\n", lastbyte);
+        status = FAIL;
+    break;
+    }
+    return status;
+}
+
+tx_rc_t terminate_session(void)
+{
+    tx_rc_t tx_status = TX_SUCCESS;
+    sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+        "[SDC-600] Terminate session\n");
+    tx_status = comport_sendbyte(FLAG_LPH2RL);
+    sdc600_ctx.portinitialized = false;
+    if (tx_status != TX_SUCCESS) {
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Terminate session failed\n");
+        return TX_FAIL;
+    }
+    return tx_status;
+}
+
+static void sdc600_isr(void)
+{
+    uint8_t readbyte;
+    static uint8_t lastbyte;
+    rc_t status = SUCCESS;
+    rx_rc_t res = RX_SUCCESS;
+    tx_rc_t tx_status = TX_SUCCESS;
+
+    if (sdc600_ctx.portinitialized == false) {
+        status = comport_init();
+        if (status == SUCCESS)
+            sdc600_ctx.portinitialized = true;
+        else
+            goto bail;
+    }
+
+    res = comport_readbyte(&readbyte, BLOCKED, ERROR_CHECK);
+    if (res != RX_SUCCESS)
+        goto bail;
+
+    if (readbyte == FLAG_START) {
+        lastbyte = readbyte;
+    } else if (lastbyte == FLAG_START) {
+        lastbyte = readbyte;
+    } else if (readbyte == FLAG_END) {
+      /**
+      * Status is success – debugger is connected.
+      * parse and analyze the debugger message
+      * Switch on the message command ID
+      */
+        status = handle_message(lastbyte);
+      //TODO:Enable debug signals or change LCS using Crypto interface
+      if (status != SUCCESS)
+          goto bail;
+    } else if (readbyte == FLAG_LPH2RL) {
+        //TODO:Change the LCS back to Secure using Crypto interface
+        tx_status = terminate_session();
+        if (tx_status != TX_SUCCESS)
+            goto bail;
+    }
+
+bail:
+    fwk_interrupt_clear_pending(INT_SE_INT_EXP);
+}
+
+static int sdc600_bind(fwk_id_t id,
+    unsigned int round)
+{
+    int status;
+    /* Use second round only (round numbering is zero-indexed) */
+    if (round == 1) {
+        /* Bind to the log component */
+        status = fwk_module_bind(FWK_ID_MODULE(FWK_MODULE_IDX_LOG),
+                                FWK_ID_API(FWK_MODULE_IDX_LOG, 0),
+                                &sdc600_ctx.log_api);
+        if (status != FWK_SUCCESS)
+            return FWK_E_PANIC;
+
+        status = fwk_module_bind(FWK_ID_MODULE(FWK_MODULE_IDX_INTR_COL),
+                                FWK_ID_API(FWK_MODULE_IDX_INTR_COL, 0),
+                                &sdc600_ctx.intr_col_api);
+        if (status != FWK_SUCCESS) {
+            assert(false);
+            return status;
+        }
+    }
+
+    return FWK_SUCCESS;
+}
+
+static int sdc600_start(fwk_id_t module_id)
+{
+    int status;
+    uint32_t addr = APBCOM_ADDR(APBCOM_REG_DEVARCH);
+    volatile uint32_t arch = *(volatile uint32_t*)addr;
+
+    sdc600_ctx.portinitialized = false;
+
+    status = sdc600_ctx.intr_col_api->set_intr_col_irq(SDC600_IRQ, sdc600_isr);
+    if (status != FWK_SUCCESS) {
+        /* Failed to set collated irq */
+        assert(false);
+        return status;
+    }
+
+    status = sdc600_ctx.intr_col_api->enable_irq(SDC600_IRQ);
+    if (status != FWK_SUCCESS) {
+        /* Failed to enable collated irq */
+        assert(false);
+        return status;
+    }
+
+    if (arch == ARCH_CORESIGHT_SDC600)
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Coresight version supports Secure Debug\n");
+    else
+        sdc600_ctx.log_api->log(MOD_LOG_GROUP_INFO,
+            "[SDC-600] Coresight version doesn't support Secure Debug\n");
+
+    return status;
+}
+
+static int sdc600_init(fwk_id_t module_id,
+    unsigned int element_count,
+    const void *data)
+{
+    return FWK_SUCCESS;
+}
+
+const struct fwk_module module_sdc600 = {
+    .name = "SDC600",
+    .type = FWK_MODULE_TYPE_SERVICE,
+    .init = sdc600_init,
+    .bind = sdc600_bind,
+    .start = sdc600_start,
+};
diff --git a/product/corstone-700/include/se_irq.h b/product/corstone-700/include/se_irq.h
index a9f0dd9..1e5c559 100644
--- a/product/corstone-700/include/se_irq.h
+++ b/product/corstone-700/include/se_irq.h
@@ -1,6 +1,6 @@
 /*
  *
- * Copyright (c) 2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -50,7 +50,7 @@ enum se_irq {
 
 enum se_col_irq {
     HOST_SYS_FW_IRQ         = 0, /* Host System Firewall Interrupt  */
-    SDC_600_IRQ             = 1, /* SDC-600                         */
+    SDC600_IRQ              = 1, /* SDC-600                         */
     HOST_PPU_CMB_IRQ        = 2, /* Host PPU Combined Interrupt     */
     REFCLK_TIMER_0_IRQ      = 3,
     REFCLK_TIMER_1_IRQ      = 4,
diff --git a/product/corstone-700/include/se_mmap.h b/product/corstone-700/include/se_mmap.h
index d01d6fc..6fdc458 100755
--- a/product/corstone-700/include/se_mmap.h
+++ b/product/corstone-700/include/se_mmap.h
@@ -1,6 +1,6 @@
 /*
  *
- * Copyright (c) 2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -23,20 +23,21 @@
 #define SE_REFCLK_CNTCONTROL_BASE    (SE_HOST_ACCESS + 0x1A200000)
 #define SE_REFCLK_CNTCTL_BASE        (SE_HOST_ACCESS + 0x1A220000)
 #define SE_REFCLK_CNTBASE0_BASE      (SE_HOST_ACCESS + 0x1A230000)
+#define SE_APBCOM_BASE               (SE_HOST_ACCESS + 0x1B900000)
 
 /*
  * System Registers
  */
+#define SE_SYSTEMID_BASE             (SE_HOST_ACCESS + 0x1A000000)
+#define SE_SYSTEMID_SOCID            (SE_SYSTEMID_BASE + 0x40)
+#define SCB_BLOCK_BASE               (0x2F808000)
+#define SCB_BLOCK_OFFSET             (SCB_BLOCK_BASE + 0x0C)
 #define SE_BASE_SYS_CTRL             (SE_PERIPHERAL_BASE + 0x8E000)
 #define HOST_SYS_RST_CTRL            (SE_BASE_SYS_CTRL + 0x0)
 #define BOOT_INST_REGISTER           (SE_HOST_ACCESS + 0x0)
-
 #define SE_FIREWALL_BASE             (SE_PERIPHERAL_BASE + 0x200000)
-
 #define HOST_FIREWALL_BASE           (SE_HOST_ACCESS + 0x1A800000)
-
-#define SE_HOST_BASE_SYS_CTRL_BASE (0x7A010000)
-
-#define SE_HOST_CPU_CORE0_WAKEUP (SE_HOST_BASE_SYS_CTRL_BASE + 0x308)
+#define SE_HOST_BASE_SYS_CTRL_BASE   (0x7A010000)
+#define SE_HOST_CPU_CORE0_WAKEUP     (SE_HOST_BASE_SYS_CTRL_BASE + 0x308)
 
 #endif /* SE_MMAP_H */
diff --git a/product/corstone-700/se_ramfw/config_sdc600.c b/product/corstone-700/se_ramfw/config_sdc600.c
new file mode 100644
index 0000000..eb059fa
--- /dev/null
+++ b/product/corstone-700/se_ramfw/config_sdc600.c
@@ -0,0 +1,21 @@
+/*
+ *
+ * Copyright (c) 2020, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+
+#include <fwk_module.h>
+#include <se_system_mmap.h>
+#include <se_mmap.h>
+#include <se_irq.h>
+#include <mod_sdc600.h>
+
+const struct fwk_module_config config_sdc600 = {
+    .data = &((struct mod_sdc600_config) {
+        .irq = SDC600_IRQ,
+        .is_irq_collated = true,
+    })
+};
diff --git a/product/corstone-700/se_ramfw/firmware.mk b/product/corstone-700/se_ramfw/firmware.mk
index 96e4870..809e2d3 100644
--- a/product/corstone-700/se_ramfw/firmware.mk
+++ b/product/corstone-700/se_ramfw/firmware.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2019, Arm Limited and Contributors. All rights reserved.
+# Copyright (c) 2019-2020, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -9,6 +9,9 @@ BS_FIRMWARE_HAS_MULTITHREADING := yes
 BS_FIRMWARE_HAS_NOTIFICATION := yes
 BS_FIRMWARE_HAS_OPENAMP := yes
 
+# Enable this flag to test SDC-600 on FPGA
+BS_FIRMWARE_HAS_SDC600 := no
+
 BS_FIRMWARE_MODULES := \
     pl011 \
     log \
@@ -22,6 +25,11 @@ BS_FIRMWARE_MODULES := \
     host_boot \
     eventhandler
 
+ifeq ($(BS_FIRMWARE_HAS_SDC600),yes)
+BS_FIRMWARE_MODULES += sdc600
+endif
+
+
 BS_FIRMWARE_SOURCES := \
     config_log.c \
     rtx_config.c \
@@ -34,4 +42,8 @@ BS_FIRMWARE_SOURCES := \
     config_host_boot.c \
     config_eventhandler.c
 
+ifeq ($(BS_FIRMWARE_HAS_SDC600),yes)
+BS_FIRMWARE_SOURCES += config_sdc600.c
+endif
+
 include $(BS_DIR)/firmware.mk