From d53da4de4b03f8ba21355edec5e658cf694de6c1 Mon Sep 17 00:00:00 2001
From: Valerio Setti <vsetti@baylibre.com>
Date: Tue, 13 Jun 2023 16:24:18 +0200
Subject: [PATCH 41/41] rswitch: initial support (WIP)

Signed-off-by: Valerio Setti <vsetti@baylibre.com>
---
 .../cortex-a/armv8/spider/ethernet/eth.oil    |   1 +
 .../cortex-a/armv8/spider/ethernet/eth_gptp.c |  35 +
 .../cortex-a/armv8/spider/ethernet/eth_gptp.h |  14 +
 .../cortex-a/armv8/spider/ethernet/main.c     |  23 +-
 .../cortex-a/armv8/spider/ethernet/pfcmap.h   |  60 ++
 .../cortex-a/armv8/spider/ethernet/rswitch.c  | 829 +++++++++++++++++-
 .../cortex-a/armv8/spider/ethernet/rswitch.h  |   1 +
 .../armv8/spider/ethernet/rswitch_regs.h      | 659 ++++++++++++++
 .../cortex-a/armv8/spider/ethernet/utils.c    |  25 +
 .../cortex-a/armv8/spider/ethernet/utils.h    |  23 +
 10 files changed, 1652 insertions(+), 18 deletions(-)
 create mode 100644 examples/cortex-a/armv8/spider/ethernet/eth_gptp.c
 create mode 100644 examples/cortex-a/armv8/spider/ethernet/eth_gptp.h
 create mode 100644 examples/cortex-a/armv8/spider/ethernet/pfcmap.h
 create mode 100644 examples/cortex-a/armv8/spider/ethernet/rswitch_regs.h

diff --git a/examples/cortex-a/armv8/spider/ethernet/eth.oil b/examples/cortex-a/armv8/spider/ethernet/eth.oil
index 15c28c20..a6372054 100644
--- a/examples/cortex-a/armv8/spider/ethernet/eth.oil
+++ b/examples/cortex-a/armv8/spider/ethernet/eth.oil
@@ -20,6 +20,7 @@ CPU eth {
       APP_SRC = "utils.c";
       APP_SRC = "eth_serdes.c";
       APP_SRC = "rswitch.c";
+      APP_SRC = "eth_gptp.c";
       APP_NAME = "eth_exe.elf";
       CFLAGS  = "-O0";
       LDFLAGS = "-Map=eth_exe.map";
diff --git a/examples/cortex-a/armv8/spider/ethernet/eth_gptp.c b/examples/cortex-a/armv8/spider/ethernet/eth_gptp.c
new file mode 100644
index 00000000..eaedc3d4
--- /dev/null
+++ b/examples/cortex-a/armv8/spider/ethernet/eth_gptp.c
@@ -0,0 +1,35 @@
+#include "eth_gptp.h"
+#include "rswitch_regs.h"
+#include "utils.h"
+
+/* Note: we are only using Timer0 */
+
+#define PTPTIVC_INIT                0x19000000	/* 320MHz */
+#define RCAR_GEN4_PTP_CLOCK_S4      PTPTIVC_INIT
+
+#define PTPRO           RSWITCH_GPTP_ADDR
+
+#define PTPTMEC             PTPRO + 0x0010
+#define PTPTMDC             PTPRO + 0x0014
+#define PTPTIVC0            PTPRO + 0x0020
+#define PTPTOVC00           PTPRO + 0x0030
+#define PTPTOVC10           PTPRO + 0x0034
+#define PTPTOVC20           PTPRO + 0x0038
+#define PTPGPTPTM00         PTPRO + 0x0050
+#define PTPGPTPTM10         PTPRO + 0x0054
+#define PTPGPTPTM20         PTPRO + 0x0058
+
+void eth_gptp_init(void)
+{
+    reg_write32(RCAR_GEN4_PTP_CLOCK_S4, PTPTIVC0);
+    reg_write32(0x1, PTPTMEC);
+}
+
+void eth_gptp_get_time(struct gptp_time *output)
+{
+    output->nano = reg_read32(PTPGPTPTM00);
+    output->seconds = reg_read32(PTPGPTPTM10) + 
+                      (((uint64) reg_read32(PTPGPTPTM20)) << 32);
+    /* A better implementation might check that none of the above
+       registers overflowed while reading others... */
+}
\ No newline at end of file
diff --git a/examples/cortex-a/armv8/spider/ethernet/eth_gptp.h b/examples/cortex-a/armv8/spider/ethernet/eth_gptp.h
new file mode 100644
index 00000000..83d6c5bb
--- /dev/null
+++ b/examples/cortex-a/armv8/spider/ethernet/eth_gptp.h
@@ -0,0 +1,14 @@
+#ifndef _ETH_GPTP_H_
+#define _ETH_GPTP_H_
+
+#include "tpl_os.h"
+
+struct gptp_time {
+    uint64 seconds;
+    uint32 nano;
+};
+
+void eth_gptp_init(void);
+void eth_gptp_get_time(struct gptp_time *output);
+
+#endif /* _ETH_GPTP_H_ */ 
\ No newline at end of file
diff --git a/examples/cortex-a/armv8/spider/ethernet/main.c b/examples/cortex-a/armv8/spider/ethernet/main.c
index 50d446de..f87c247b 100644
--- a/examples/cortex-a/armv8/spider/ethernet/main.c
+++ b/examples/cortex-a/armv8/spider/ethernet/main.c
@@ -2,8 +2,7 @@
 #include "utils.h"
 #include "eth_serdes.h"
 #include "rswitch.h"
-
-#define debug_msg(x)	do { (void)x; } while(0)
+#include "eth_gptp.h"
 
 #define APP_Task_sample_init_START_SEC_CODE
 #include "tpl_memmap.h"
@@ -27,12 +26,23 @@ TASK(sample_init) {
 
 	ret = eth_serdes_initialize();
 	if (ret != 0) {
-		debug_msg("Error in serdes initialization\n");
+		debug_msg("Error in eth_serdes_initialize\n");
+		goto exit;
+	}
+
+	ret = rswitch_init();
+	if (ret != 0) {
+		debug_msg("Error in rswitch_init\n");
 		goto exit;
 	}
 
-	rswitch_init();
+	eth_gptp_init();
 
+	ret = rswitch_open();
+	if (ret != 0) {
+		debug_msg("Error in rswitch_open\n");
+		goto exit;
+	}
 
 exit:
 	TerminateTask();
@@ -49,6 +59,7 @@ exit:
 
 ISR(gwca1_rx_tx_int)
 {
+	debug_msg("gwca1_rx_tx_int");
 	//CallTerminateISR2();
 }
 
@@ -68,6 +79,7 @@ FUNC(void, OS_CODE) GWCA1_RX_TX_INT_ClearFlag(void)
 
 ISR(gwca1_rx_ts_int)
 {
+	debug_msg("gwca1_rx_ts_int");
 	//CallTerminateISR2();
 }
 
@@ -87,6 +99,7 @@ FUNC(void, OS_CODE) GWCA1_RX_TS_INT_ClearFlag(void)
 
 ISR(coma_err_int)
 {
+	debug_msg("coma_err_int");
 	//CallTerminateISR2();
 }
 
@@ -106,6 +119,7 @@ FUNC(void, OS_CODE) COMA_ERR_INT_ClearFlag(void)
 
 ISR(gwca1_err_int)
 {
+	debug_msg("gwca1_err_int");
 	//CallTerminateISR2();
 }
 
@@ -125,6 +139,7 @@ FUNC(void, OS_CODE) GWCA1_ERR_INT_ClearFlag(void)
 
 ISR(etha0_err_int)
 {
+	debug_msg("etha0_err_int");
 	//CallTerminateISR2();
 }
 
diff --git a/examples/cortex-a/armv8/spider/ethernet/pfcmap.h b/examples/cortex-a/armv8/spider/ethernet/pfcmap.h
new file mode 100644
index 00000000..f0b063f3
--- /dev/null
+++ b/examples/cortex-a/armv8/spider/ethernet/pfcmap.h
@@ -0,0 +1,60 @@
+#ifndef PFCMAP_H
+#define PFCMAP_H
+
+/* PFC base */
+#define PFC_GP0_BASE        (0xE6050000UL)
+#define PFC_GP4_BASE        (0xDFD90000UL)
+
+static const unsigned int PRTGRP_OFFSET[] =
+{
+  0x0000U,    /* PRTGRP0  */
+  0x0000U,    /* PRTGRP1  */
+  0x0800U,    /* PRTGRP2  */
+  0x0800U,    /* PRTGRP3  */
+  0x0000U,    /* PRTGRP4  */
+  0x0800U,    /* PRTGRP5  */
+  0x0000U,    /* PRTGRP6  */
+  0x0800U,    /* PRTGRP7  */
+  0x1000U,    /* PRTGRP8  */
+  0x1800U,    /* PRTGRP9  */
+  0x0000U,    /* PRTGRP10 */
+};
+
+#define PFC_GPSR_BmPlTt_OFFSET(m, l, t)    (0x0040U + PFC_GROUPS_OFFSET(m, l, t))
+
+/* PFC[0] = m=0~3, l=1, t=0~2 */
+/* PFC[1] = m=0~3, l=0, t=0~2 */
+/* PFC[2] = m=0~3, l=4,5, t=0~2 */
+/* PFC[3] = m=0~3, l=6~9, t=0~2 */
+
+#define PFC_IPnSR_BmPlTt_OFFSET(n, m, l, t)    (0x0060U + (0x0004U * n) + PFC_GROUPS_OFFSET(m, l, t))
+
+/* PFC[0] = m=0~3, l=1, t=0~2 */
+/* PFC[1] = m=0~3, l=0, t=0~2 */
+/* PFC[2] = m=0~3, l=4,5, t=0~2 */
+/* PFC[3] = m=0~3, l=6~9, t=0~2 */
+
+#define PFC_POC_BmPlTt_OFFSET(m, l, t)    (0x00A0U + PFC_GROUPS_OFFSET(m, l, t))
+
+/* PFC[0] = m=0~3, l=1, t=0~2 */
+/* PFC[1] = m=0~3, l=0, t=0~2 */
+/* PFC[2] = m=0~3, l=4,5, t=0~2 */
+/* PFC[3] = m=0~3, l=6~9, t=0~2 */
+
+#define PFC_DRVnCTRL_BmPlTt_OFFSET(n, m, l, t)    (0x0080U + (0x0004U * n) + PFC_GROUPS_OFFSET(m, l, t))
+
+#define PFC_GROUPS_OFFSET(m, l, t)    (((m) * 0x2000U) + PRTGRP_OFFSET[l] + ((t) * 0x0200U))
+/* R/W 32 LSI Multiplexed Pin Setting Mask Register */
+#define PFC_PMMR(addr)      ((addr & 0xFFFFF800U) + 0x0000UL)
+
+#define PFC_PORTGP_OFFSET   (0x800U)
+#define PFC_GPn_BASE(n)     (((n < 4)? PFC_GP0_BASE : PFC_GP4_BASE) + (PFC_PORTGP_OFFSET * (n % 4)))
+
+#define PFC_GPSR_GPn_DM0(n) (PFC_GPn_BASE(n) + PFC_GPSR_BmPlTt_OFFSET(0,0,0))
+#define PFC_IPSRm_GPn_DM0(m, n) (PFC_GPn_BASE(n) + PFC_IPnSR_BmPlTt_OFFSET(m,0,0,0))
+/* R/W 32 POC control register0 PortGroup 3 */
+#define PFC_POC_GPn_DM0(n)  (PFC_GPn_BASE(n) + PFC_POC_BmPlTt_OFFSET(0,0,0))
+/* R/W 32 POC control register0 PortGroup 3 */
+#define PFC_DRVCTRLm_GPn_DM0(m, n)  (PFC_GPn_BASE(n) + PFC_DRVnCTRL_BmPlTt_OFFSET(m,0,0,0))
+
+#endif /* PFCMAP_H */
diff --git a/examples/cortex-a/armv8/spider/ethernet/rswitch.c b/examples/cortex-a/armv8/spider/ethernet/rswitch.c
index 6627a82f..c653266d 100644
--- a/examples/cortex-a/armv8/spider/ethernet/rswitch.c
+++ b/examples/cortex-a/armv8/spider/ethernet/rswitch.c
@@ -1,29 +1,830 @@
 #include "tpl_os.h"
+#include "utils.h"
 #include "rswitch.h"
+#include "rswitch_regs.h"
+#include "err_codes.h"
+#include <string.h>
 
-#define RSWITCH_BASE            0xE6880000
+#define PORT_TSNA_N     3
+#define PORT_GWCA_N     2
+#define TOT_PORT_NUM    (PORT_TSNA_N + PORT_GWCA_N)
 
-#define RSWITCH_FWD_ADDR      (RSWITCH_BASE + 0x00000000)
-#define RSWITCH_FAB_ADDR      (RSWITCH_BASE + 0x00008000)
-#define RSWITCH_COMA_ADDR     (RSWITCH_BASE + 0x00009000)
-#define RSWITCH_ETHA0_ADDR    (RSWITCH_BASE + 0x0000a000)
-#define RSWITCH_ETHA1_ADDR    (RSWITCH_BASE + 0x0000c000)
-#define RSWITCH_ETHA2_ADDR    (RSWITCH_BASE + 0x0000e000)
-#define RSWITCH_GWCA0_ADDR    (RSWITCH_BASE + 0x00010000)
-#define RSWITCH_GWCA1_ADDR    (RSWITCH_BASE + 0x00012000)
-#define RSWITCH_GPTP_ADDR     (RSWITCH_BASE + 0x00018000)
+#define RSWITCH_GWCA_IDX_TO_HW_NUM(i)   ((i) + PORT_TSNA_N)
+#define RSWITCH_HW_NUM_TO_GWCA_IDX(i)   ((i) - PORT_TSNA_N)
 
-static inline uint32 reg_read32(void *addr)
+#define TSNA0_PORT_NUM   0
+#define TSNA1_PORT_NUM   1
+#define TSNA2_PORT_NUM   2
+#define GWCA0_PORT_NUM   3
+#define GWCA1_PORT_NUM   4
+
+#define TSN_PORT_IN_USE   TSNA0_PORT_NUM
+#define GWCA_PORT_IN_USE  GWCA1_PORT_NUM
+
+/* GWCA */
+enum rswitch_gwca_mode {
+    GWMC_OPC_RESET,
+    GWMC_OPC_DISABLE,
+    GWMC_OPC_CONFIG,
+    GWMC_OPC_OPERATION,
+};
+#define GWMS_OPS_MASK   GWMC_OPC_OPERATION
+#define GWVCC_VEM_SC_TAG    (0x3 << 16)
+#define GWMTIRM_MTIOG       BIT(0)
+#define GWMTIRM_MTR         BIT(1)
+#define GWARIRM_ARIOG       BIT(0)
+#define GWARIRM_ARR         BIT(1)
+
+#define GWDCC_BALR          BIT(24)
+#define GWDCC_DCP(q, idx)   ((q + (idx * 2)) << 16)
+#define GWDCC_DQT           BIT(11)
+#define GWDCC_ETS           BIT(9)
+#define GWDCC_EDE           BIT(8)
+#define GWDCC_OFFS(chain)   (GWDCC0 + (chain) * 4)
+
+enum DIE_DT {
+    /* Frame data */
+    DT_FSINGLE = 0x80,
+    DT_FSTART = 0x90,
+    DT_FMID = 0xA0,
+    DT_FEND = 0xB8,
+
+    /* Chain control */
+    DT_LEMPTY = 0xC0,
+    DT_EEMPTY = 0xD0,
+    DT_LINKFIX = 0x00,
+    DT_LINK = 0xE0,
+    DT_EOS = 0xF0,
+    /* HW/SW arbitration */
+    DT_FEMPTY = 0x40,
+    DT_FEMPTY_IS = 0x10,
+    DT_FEMPTY_IC = 0x20,
+    DT_FEMPTY_ND = 0x38,
+    DT_FEMPTY_START = 0x50,
+    DT_FEMPTY_MID = 0x60,
+    DT_FEMPTY_END = 0x70,
+
+    DT_MASK = 0xF0,
+    DIE = 0x08,     /* Descriptor Interrupt Enable */
+};
+
+struct rswitch_desc {
+    uint16 info_ds;	/* Descriptor size */
+    uint8 die_dt;	/* Descriptor interrupt enable and type */
+    uint8  dptrh;	/* Descriptor pointer MSB */
+    uint32 dptrl;	/* Descriptor pointer LSW */
+} __attribute__((packed));
+
+struct rswitch_ts_desc {
+    uint16 info_ds;	/* Descriptor size */
+    uint8 die_dt;	/* Descriptor interrupt enable and type */
+    uint8  dptrh;	/* Descriptor pointer MSB */
+    uint32 dptrl;	/* Descriptor pointer LSW */
+    uint32 ts_nsec;
+    uint32 ts_sec;
+} __attribute__((packed));
+
+struct rswitch_ext_desc {
+    uint16 info_ds;	/* Descriptor size */
+    uint8 die_dt;	/* Descriptor interrupt enable and type */
+    uint8  dptrh;	/* Descriptor pointer MSB */
+    uint32 dptrl;	/* Descriptor pointer LSW */
+    uint64 info1;
+} __attribute__((packed));
+
+struct rswitch_ext_ts_desc {
+    uint16 info_ds;	/* Descriptor size */
+    uint8 die_dt;	/* Descriptor interrupt enable and type */
+    uint8  dptrh;	/* Descriptor pointer MSB */
+    uint32 dptrl;	/* Descriptor pointer LSW */
+    uint64 info1;
+    uint32 ts_nsec;
+    uint32 ts_sec;
+} __attribute__((packed));
+
+#define NUM_DEVICES         3
+#define NUM_CHAINS_PER_NDEV 2
+
+#define RSWITCH_MAX_NUM_CHAINS  128
+#define RSWITCH_NUM_IRQ_REGS    (RSWITCH_MAX_NUM_CHAINS / BITS_PER_TYPE(uint32))
+
+#define MAC_ADDRESS_LEN     6
+struct rswitch_etha {
+    uint8 port_num;
+    uint32 base_addr;
+    uint8 mac_addr[MAC_ADDRESS_LEN];
+    int speed;
+} etha_props[PORT_TSNA_N] = 
 {
-    return *((volatile uint32*)addr);
+    { .port_num = TSNA0_PORT_NUM, .base_addr = RSWITCH_ETHA0_ADDR },
+    { .port_num = TSNA1_PORT_NUM, .base_addr = RSWITCH_ETHA1_ADDR },
+    { .port_num = TSNA2_PORT_NUM, .base_addr = RSWITCH_ETHA2_ADDR },
+};
+
+enum rswitch_etha_mode {
+    EAMC_OPC_RESET,
+    EAMC_OPC_DISABLE,
+    EAMC_OPC_CONFIG,
+    EAMC_OPC_OPERATION,
+};
+#define EAMS_OPS_MASK   EAMC_OPC_OPERATION
+#define EAVCC_VEM_SC_TAG    (0x3 << 16)
+
+#define MPIC_PIS_MII	0x00
+#define MPIC_PIS_GMII	0x02
+#define MPIC_PIS_XGMII	0x04
+#define MPIC_LSC_SHIFT	3
+#define MPIC_LSC_10M	(0 << MPIC_LSC_SHIFT)
+#define MPIC_LSC_100M	(1 << MPIC_LSC_SHIFT)
+#define MPIC_LSC_1G	(2 << MPIC_LSC_SHIFT)
+#define MPIC_LSC_2_5G	(3 << MPIC_LSC_SHIFT)
+#define MPIC_LSC_5G	(4 << MPIC_LSC_SHIFT)
+#define MPIC_LSC_10G	(5 << MPIC_LSC_SHIFT)
+#define MPIC_PSMCS_SHIFT	16
+#define MPIC_PSMCS_MASK		GENMASK(22, MPIC_PSMCS_SHIFT)
+#define MPIC_PSMCS(val)		((val) << MPIC_PSMCS_SHIFT)
+#define MPIC_PSMHT_SHIFT	24
+#define MPIC_PSMHT_MASK		GENMASK(26, MPIC_PSMHT_SHIFT)
+#define MPIC_PSMHT(val)		((val) << MPIC_PSMHT_SHIFT)
+#define MPSM_MFF_C45		BIT(2)
+#define MLVC_PLV	BIT(16)
+#define MDIO_READ_C45		0x03
+#define MDIO_WRITE_C45		0x01
+#define MII_ADDR_C45		(1<<30)
+#define MII_DEVADDR_C45_SHIFT	16
+#define MII_REGADDR_C45_MASK	GENMASK(15, 0)
+#define MMIS1_PAACS             BIT(2) /* Address */
+#define MMIS1_PWACS             BIT(1) /* Write */
+#define MMIS1_PRACS             BIT(0) /* Read */
+#define MMIS1_CLEAR_FLAGS       0xf
+#define MPSM_PSME		BIT(0)
+#define MPSM_MFF_C45		BIT(2)
+#define MPSM_PDA_SHIFT		3
+#define MPSM_PDA_MASK		GENMASK(7, MPSM_PDA_SHIFT)
+#define MPSM_PDA(val)		((val) << MPSM_PDA_SHIFT)
+#define MPSM_PRA_SHIFT		8
+#define MPSM_PRA_MASK		GENMASK(12, MPSM_PRA_SHIFT)
+#define MPSM_PRA(val)		((val) << MPSM_PRA_SHIFT)
+#define MPSM_POP_SHIFT		13
+#define MPSM_POP_MASK		GENMASK(14, MPSM_POP_SHIFT)
+#define MPSM_POP(val)		((val) << MPSM_POP_SHIFT)
+#define MPSM_PRD_SHIFT		16
+#define MPSM_PRD_MASK		GENMASK(31, MPSM_PRD_SHIFT)
+#define MPSM_PRD_WRITE(val)	((val) << MPSM_PRD_SHIFT)
+#define MPSM_PRD_READ(val)	((val) & MPSM_PRD_MASK >> MPSM_PRD_SHIFT)
+
+#define TX_RX_RING_SIZE     32
+#define PKT_BUF_SZ          1584
+#define PKT_BUF_SZ_ALIGN    1792 /* 0x700 to keep elements aligned to the value
+                                    specified by RSWITCH_ALIGN */
+#define RSWITCH_ALIGN       1 /* kenel uses 128, so 7 bits (=log_2(128)) are enough
+                               * to represent this value, so we round up to 1 byte. */
+
+/* These elements should either be rswitch_ext_desc or rswitch_ext_ts_desc
+ * depending if gptp is used or not. */
+struct rswitch_ext_ts_desc rx_ring[TX_RX_RING_SIZE];
+struct rswitch_ext_desc tx_ring[TX_RX_RING_SIZE];
+
+uint8 rx_data_buff[TX_RX_RING_SIZE][PKT_BUF_SZ_ALIGN] __attribute__((aligned(RSWITCH_ALIGN)));
+uint8 tx_data_buff[TX_RX_RING_SIZE][PKT_BUF_SZ_ALIGN] __attribute__((aligned(RSWITCH_ALIGN)));
+
+struct rswitch_gwca_chain {
+    uint8 chain_index;
+    uint8 dir_tx;
+    // bool gptp;
+    union {
+        struct rswitch_ext_desc *ring;
+        struct rswitch_ext_ts_desc *ts_ring;
+    };
+    uint32 num_ring;
+    uint32 cur;
+    uint32 dirty;
+    uint8 *data_buffers[TX_RX_RING_SIZE];
+};
+
+struct rswitch_gwca {
+    uint8 port_num;
+    uint32 base_addr;
+    struct rswitch_gwca_chain rx_chain;
+    struct rswitch_gwca_chain tx_chain;
+    uint32 irq_queue;
+    int speed;
+} gwca_props[PORT_GWCA_N] = {
+    { .port_num = GWCA0_PORT_NUM, .base_addr = RSWITCH_GWCA0_ADDR },
+    { .port_num = GWCA1_PORT_NUM, .base_addr = RSWITCH_GWCA1_ADDR },
+};
+
+struct rswitch_device {
+    struct rswitch_desc base_descriptors[NUM_CHAINS_PER_NDEV];
+    struct rswitch_etha *etha;
+    struct rswitch_gwca *gwca;
+    uint32 fwd_base_addr;
+    uint32 coma_base_addr;
+} rsw_dev = {
+    .fwd_base_addr = RSWITCH_FWD_ADDR,
+    .coma_base_addr = RSWITCH_COMA_ADDR,
+    .etha = &etha_props[TSN_PORT_IN_USE],
+    .gwca = &gwca_props[GWCA_PORT_IN_USE],
+};
+
+/* MFWD */
+#define FWPC0_LTHTA     BIT(0)
+#define FWPC0_IP4UE     BIT(3)
+#define FWPC0_IP4TE     BIT(4)
+#define FWPC0_IP4OE     BIT(5)
+#define FWPC0_L2SE      BIT(9)
+#define FWPC0_IP4EA     BIT(10)
+#define FWPC0_IPDSA     BIT(12)
+#define FWPC0_IPHLA     BIT(18)
+#define FWPC0_MACSDA    BIT(20)
+#define FWPC0_MACHLA    BIT(26)
+#define FWPC0_MACHMA    BIT(27)
+#define FWPC0_VLANSA    BIT(28)
+
+#define FWPC0(i)            (FWPC00 + (i) * 0x10)
+#define FWPC1(i)            (FWPC10 + (i) * 0x10)
+#define FWPC0_DEFAULT       (FWPC0_LTHTA | FWPC0_IP4UE | FWPC0_IP4TE | \
+                            FWPC0_IP4OE | FWPC0_L2SE | FWPC0_IP4EA | \
+                            FWPC0_IPDSA | FWPC0_IPHLA | FWPC0_MACSDA | \
+                            FWPC0_MACHLA |	FWPC0_MACHMA | FWPC0_VLANSA)
+#define FWPC1_DDE           BIT(0)
+#define	FWPBFC(i)           (FWPBFCi + (i) * 0x10)
+#define FWPBFCSDC(j, i)     (FWPBFCSDC00 + (i) * 0x10 + (j) * 0x04)
+
+/* Interrupts */
+#define GWCA0_RX_TX_IRQ       312
+#define GWCA1_RX_TX_IRQ       320
+#define GWCA0_RX_TS_IRQ       328
+#define GWCA1_RX_TS_IRQ       330
+#define COMA_ERR_IRQ          290
+#define GWCA0_ERR_IRQ         291
+#define GWCA1_ERR_IRQ         292
+#define ETHA0_ERR_IRQ         293
+#define ETHA1_ERR_IRQ         294
+#define ETHA2_ERR_IRQ         295
+
+#define RSWITCH_TIMEOUT_MS	1000
+static int rswitch_reg_wait(uint32 addr, uint32 mask, uint32 expected)
+{
+    int i;
+
+    for (i = 0; i < RSWITCH_TIMEOUT_MS; i++) {
+        if ((reg_read32(addr) & mask) == expected)
+            return 0;
+
+        ms_delay(1);
+    }
+
+    return ERR_TIMEOUT;
+}
+
+static void rswitch_modify(uint32 addr, uint32 clear, uint32 set)
+{
+    uint32 val = reg_read32(addr) & ~clear;
+    reg_write32(val | set, addr);
+}
+
+static int rswitch_mii_set_access(struct rswitch_etha *etha, uint8 read,
+                                    uint32 phyad, uint32 devad, uint32 regad,
+                                    uint16 *data)
+{
+    int pop = read ? MDIO_READ_C45 : MDIO_WRITE_C45;
+    uint32 val;
+    int ret;
+
+    /* Clear completion flags */
+    reg_write32(MMIS1_CLEAR_FLAGS, etha->base_addr + MMIS1);
+
+    /* Submit address to PHY (MDIO_ADDR_C45 << 13) */
+    val = MPSM_PSME | MPSM_MFF_C45;
+    reg_write32((regad << 16) | (devad << 8) | (phyad << 3) | val,
+                etha->base_addr + MPSM);
+
+    CHECK_RET(rswitch_reg_wait(etha->base_addr + MMIS1, MMIS1_PAACS, MMIS1_PAACS));
+
+    /* Clear address completion flag */
+    rswitch_modify(etha->base_addr + MMIS1, MMIS1_PAACS, MMIS1_PAACS);
+
+    /* Read/Write PHY register */
+    if (read) {
+        reg_write32((pop << 13) | (devad << 8) | (phyad << 3) | val,
+                        etha->base_addr + MPSM);
+
+        CHECK_RET(rswitch_reg_wait(etha->base_addr + MMIS1, MMIS1_PRACS, MMIS1_PRACS));
+
+        /* Read data */
+        ret = (reg_read32(etha->base_addr + MPSM) & MPSM_PRD_MASK) >> 16;
+        *data = ret;
+
+        /* Clear read completion flag */
+        rswitch_modify(etha->base_addr + MMIS1, MMIS1_PRACS, MMIS1_PRACS);
+    } else {
+        reg_write32((*data << 16) | (pop << 13) | (devad << 8) | (phyad << 3) | val,
+                    etha->base_addr + MPSM);
+
+        ret = rswitch_reg_wait(etha->base_addr + MMIS1, MMIS1_PWACS, MMIS1_PWACS);
+    }
+
+    return 0;
 }
 
-static inline void reg_write32(uint32 data, void *addr)
+static int rswitch_mii_read(struct rswitch_etha *etha, uint32 phyaddr, 
+                                uint32 devad, uint32 regad, uint32 *data)
 {
-    *((volatile uint32*)addr) = data;
+    uint16 tmp;
+    int ret;
+    ret = rswitch_mii_set_access(etha, TRUE, phyaddr, devad, regad, &tmp);
+    *data = tmp;
+
+    return ret;
+}
+
+static int rswitch_mii_write(struct rswitch_etha *etha, uint32 phyaddr, 
+                                uint32 devad, uint32 regad, uint32 *data)
+{
+    uint16 tmp = (uint16) *data;
+    return rswitch_mii_set_access(etha, FALSE, phyaddr, devad, regad, &tmp);
+}
+
+static int rswitch_agent_clock_is_enabled(int port)
+{
+    uint32 val = reg_read32(rsw_dev.coma_base_addr + RCEC);
+
+    if (val & RCEC_RCE)
+        return (val & BIT(port)) ? TRUE : FALSE;
+    else
+        return FALSE;
+}
+
+static void rswitch_agent_clock_ctrl(int port, int enable)
+{
+    uint32 val;
+
+    if (enable) {
+        val = reg_read32(rsw_dev.coma_base_addr + RCEC);
+        reg_write32(val | RCEC_RCE | BIT(port),
+                    rsw_dev.coma_base_addr + RCEC);
+    } else {
+        val = reg_read32(RCDC);
+        reg_write32(val | BIT(port), RCDC);
+    }
+}
+
+// mac is supposed to be an array of 6 bytes
+static void rswitch_etha_read_mac_address(struct rswitch_etha *eth_dev)
+{
+    uint32 mrmac0 = reg_read32(eth_dev->base_addr + MRMAC0);
+    uint32 mrmac1 = reg_read32(eth_dev->base_addr + MRMAC1);
+
+    eth_dev->mac_addr[0] = (mrmac0 >>  8) & 0xFF;
+    eth_dev->mac_addr[1] = (mrmac0 >>  0) & 0xFF;
+    eth_dev->mac_addr[2] = (mrmac1 >> 24) & 0xFF;
+    eth_dev->mac_addr[3] = (mrmac1 >> 16) & 0xFF;
+    eth_dev->mac_addr[4] = (mrmac1 >>  8) & 0xFF;
+    eth_dev->mac_addr[5] = (mrmac1 >>  0) & 0xFF;
+}
+
+/* This function sets only data in the global strucure, not on the hardware */
+static void rswitch_set_mac_address(struct rswitch_etha *eth_dev)
+{
+    int i;
+
+    // Values from MCAL
+    static const uint8 predefined_mac_address[PORT_TSNA_N][6] = {
+        {0x74U, 0x90U, 0x50U, 0x00U, 0x00U, 0x00U},
+        {0x74U, 0x90U, 0x50U, 0x00U, 0x00U, 0x01U},
+        {0x74U, 0x90U, 0x50U, 0x00U, 0x00U, 0x02U},
+    };
+
+    // Set only if not already valid (at least 1 value != 0)
+    for (i = 0; i < MAC_ADDRESS_LEN; i++) {
+        if (eth_dev->mac_addr[i] != 0) {
+            return;
+        }
+    }
+
+    /* The following works only because TSN port's numbers starts from 0 as
+     * if they were indexes. */
+    for (i = 0; i < MAC_ADDRESS_LEN; i++) {
+        eth_dev->mac_addr[i] = predefined_mac_address[eth_dev->port_num][i];
+    }
+}
+
+static void rswitch_soft_reset(void)
+{
+    reg_write32(RRC_RR, rsw_dev.coma_base_addr + RRC);
+    reg_write32(RRC_RR_CLR, rsw_dev.coma_base_addr + RRC);
+
+    /* Clock initialization and module reset have already been done before
+     * in utils.c */
+}
+
+static int rswitch_gwca_change_mode(struct rswitch_gwca *gwca,
+                                    enum rswitch_gwca_mode mode)
+{
+    int ret;
+
+    /* Enable clock */
+    if (!rswitch_agent_clock_is_enabled(gwca->port_num))
+        rswitch_agent_clock_ctrl(gwca->port_num, 1);
+
+    reg_write32(mode, gwca->base_addr + GWMC);
+
+    ret = rswitch_reg_wait(gwca->base_addr + GWMS, GWMS_OPS_MASK, mode);
+
+    /* Disable clock */
+    if (mode == GWMC_OPC_DISABLE)
+        rswitch_agent_clock_ctrl(gwca->port_num, 0);
+
+    return ret;
+}
+
+static int rswitch_gwca_mcast_table_reset(struct rswitch_gwca *gwca)
+{
+    reg_write32(GWMTIRM_MTIOG, gwca->base_addr + GWMTIRM);
+    return rswitch_reg_wait(gwca->base_addr + GWMTIRM, GWMTIRM_MTR, GWMTIRM_MTR);
+}
+
+static int rswitch_gwca_axi_ram_reset(struct rswitch_gwca *gwca)
+{
+    reg_write32(GWARIRM_ARIOG, gwca->base_addr + GWARIRM);
+    return rswitch_reg_wait(gwca->base_addr + GWARIRM, GWARIRM_ARR, GWARIRM_ARR);
+}
+
+static void rswitch_gwca_set_rate_limit(struct rswitch_gwca *gwca)
+{
+    uint32 gwgrlulc, gwgrlc;
+
+    switch (gwca->speed) {
+    case 1000:
+        gwgrlulc = 0x0000005f;
+        gwgrlc = 0x00010260;
+        break;
+    default:
+        debug_msg("%s: This rate is not supported (%d)\n", __func__, gwca->speed);
+        return;
+    }
+
+    reg_write32(gwgrlulc, gwca->base_addr + GWGRLULC);
+    reg_write32(gwgrlc, gwca->base_addr + GWGRLC);
+}
+
+static int rswitch_gwca_hw_init(struct rswitch_device *rswitch)
+{
+    int ret, i;
+    uint64 descriptors_addr = (uint32) rswitch->base_descriptors;
+    struct rswitch_gwca *gwca = rswitch->gwca;
+
+    /* Note: this is setting the base descriptors for all the devices but
+     * perhaps we should just set the one we are using. Setting for all
+     * might impact what Linux/Uboot has already configured on its side. */
+    for (i = 0; i < NUM_CHAINS_PER_NDEV * NUM_DEVICES; i++) {
+        rswitch->base_descriptors[i].die_dt = DT_EOS;
+    }
+
+    CHECK_RET(rswitch_gwca_change_mode(gwca, GWMC_OPC_DISABLE));
+    CHECK_RET(rswitch_gwca_change_mode(gwca, GWMC_OPC_CONFIG));
+    CHECK_RET(rswitch_gwca_mcast_table_reset(gwca));
+    CHECK_RET(rswitch_gwca_axi_ram_reset(gwca));
+    
+    /* Full setting flow */
+    reg_write32(GWVCC_VEM_SC_TAG, gwca->base_addr + GWVCC);
+    reg_write32(0, gwca->base_addr + GWTTFC);
+    reg_write32(0x00, gwca->base_addr + GWDCBAC1);
+    reg_write32((descriptors_addr >> 32) & 0xFFFFFFFF, gwca->base_addr + GWDCBAC0);
+
+    gwca->speed = 1000;
+    rswitch_gwca_set_rate_limit(gwca);
+
+    CHECK_RET(rswitch_gwca_change_mode(gwca, GWMC_OPC_DISABLE));
+    CHECK_RET(rswitch_gwca_change_mode(gwca, GWMC_OPC_OPERATION));
+
+    return 0;
+}
+
+static void rswitch_gwca_chain_format(struct rswitch_device *rswitch,
+                                      struct rswitch_gwca_chain *c)
+{
+    struct rswitch_ext_desc *ring;
+    struct rswitch_desc *base_desc;
+    int tx_ring_size = sizeof(*ring) * c->num_ring;
+    int i;
+
+    memset(c->ring, 0, tx_ring_size);
+    for (i = 0, ring = c->ring; i < c->num_ring; i++, ring++) {
+        if (!c->dir_tx) {
+            ring->info_ds = PKT_BUF_SZ;
+            ring->dptrl = (uint32) c->data_buffers[i];
+            ring->dptrh = 0x00;
+            ring->die_dt = DT_FEMPTY | DIE;
+        } else {
+            ring->die_dt = DT_EEMPTY | DIE;
+        }
+    }
+
+    /* Close the loop */
+    ring->dptrl = (uint32) c->ring;
+    ring->dptrh = 0x00;
+    ring->die_dt = DT_LINKFIX;
+
+    /* Configure the base descriptor */
+    base_desc = &rswitch->base_descriptors[c->chain_index];
+    base_desc->die_dt = DT_LINKFIX;
+    base_desc->dptrl = (uint32) c->ring;
+    base_desc->dptrh = 0x00;
+
+    /* FIXME: GWDCC_DCP */
+    reg_write32(GWDCC_BALR | (c->dir_tx ? GWDCC_DQT : 0) | GWDCC_EDE,
+                rswitch->gwca->base_addr + GWDCC_OFFS(c->chain_index));
+}
+
+static __unused void rswitch_gwca_chain_ts_format(struct rswitch_device *rswitch,
+                                         struct rswitch_gwca_chain *c)
+{
+    struct rswitch_ext_ts_desc *ring;
+    struct rswitch_desc *base_desc;
+    int tx_ring_size = sizeof(*ring) * c->num_ring;
+    int i;
+
+    memset(c->ring, 0, tx_ring_size);
+    for (i = 0, ring = c->ts_ring; i < c->num_ring; i++, ring++) {
+        if (!c->dir_tx) {
+            ring->info_ds = PKT_BUF_SZ;
+            ring->dptrl = (uint32) c->data_buffers[i];
+            ring->dptrh = 0x00;
+            ring->die_dt = DT_FEMPTY | DIE;
+        } else {
+            ring->die_dt = DT_EEMPTY | DIE;
+        }
+    }
+
+    /* Close the loop */
+    ring->dptrl = (uint32) c->ring;
+    ring->dptrh = 0x00;
+    ring->die_dt = DT_LINKFIX;
+
+    /* Configure the base descriptor */
+    base_desc = &rswitch->base_descriptors[c->chain_index];
+    base_desc->die_dt = DT_LINKFIX;
+    base_desc->dptrl = (uint32) c->ring;
+    base_desc->dptrh = 0x00;
+
+    /* FIXME: GWDCC_DCP */
+    reg_write32(GWDCC_BALR | (c->dir_tx ? GWDCC_DQT : 0) | GWDCC_ETS | GWDCC_EDE,
+                rswitch->gwca->base_addr + GWDCC_OFFS(c->chain_index));
+}
+
+static int rswitch_bpool_config(struct rswitch_device *rswitch)
+{
+    uint32 val;
+
+    val = reg_read32(rswitch->coma_base_addr + CABPIRM);
+    if (val & CABPIRM_BPR)
+        return 0;
+
+    reg_write32(CABPIRM_BPIOG, rswitch->coma_base_addr + CABPIRM);
+    return rswitch_reg_wait(rswitch->coma_base_addr + CABPIRM, CABPIRM_BPR, CABPIRM_BPR);
+}
+
+static void rswitch_fwd_init(struct rswitch_device *rswitch)
+{
+    int i;
+    int eth_port_num = rswitch->etha->port_num;
+    int gwca_port_num = rswitch->gwca->port_num;
+    int gwca_idx = RSWITCH_HW_NUM_TO_GWCA_IDX(gwca_port_num);
+
+    /* Note: this configures FWD for all the ports, but this might conflict
+     * with previous configurations... */
+    for (i = 0; i < TOT_PORT_NUM; i++) {
+        reg_write32(FWPC0_DEFAULT, rswitch->fwd_base_addr + FWPC0(i));
+        reg_write32(0, rswitch->fwd_base_addr + FWPBFC(i));
+    }
+
+    /* Static routing between the specified ETHA and GWCA ports. */
+    reg_write32(rswitch->gwca->rx_chain.chain_index,
+               rswitch->fwd_base_addr + FWPBFCSDC(gwca_idx, eth_port_num));
+    reg_write32(BIT(rswitch->gwca->port_num),
+                rswitch->fwd_base_addr + FWPBFC(eth_port_num));
+
+    /* For GWCA */
+    reg_write32(FWPC0_DEFAULT, rswitch->fwd_base_addr + FWPC0(gwca_port_num));
+    reg_write32(FWPC1_DDE, rswitch->fwd_base_addr + FWPC1(gwca_port_num));
+    reg_write32(0, rswitch->fwd_base_addr + FWPBFC(gwca_port_num));
+    reg_write32(eth_port_num, rswitch->fwd_base_addr + FWPBFC(gwca_port_num));
+}
+
+static __unused void rswitch_get_data_irq_status(struct rswitch_device *rswitch, uint32 *dis)
+{
+    int i;
+
+    for (i = 0; i < RSWITCH_NUM_IRQ_REGS; i++) {
+        dis[i] = reg_read32(rswitch->gwca->base_addr + GWDIS0 + i * 0x10);
+    }
+}
+
+static void rswitch_enadis_data_irq(struct rswitch_gwca *gwca, int index,
+                                    int enable)
+{
+    uint32 offs = (enable ? GWDIE0 : GWDID0) + (index / 32) * 0x10;
+    uint32 tmp = 0;
+
+    /* For VPF? */
+    if (enable)
+        tmp = reg_read32(gwca->base_addr + offs);
+
+    reg_write32(BIT(index % 32) | tmp, gwca->base_addr + offs);
+}
+
+static void rswitch_enable_irqs(void)
+{
+    /* Enables IRQs on the core side */
+    enable_int(GWCA1_RX_TX_IRQ);
+    enable_int(GWCA1_RX_TS_IRQ);
+    enable_int(GWCA1_ERR_IRQ);
+    enable_int(ETHA0_ERR_IRQ);
+    enable_int(COMA_ERR_IRQ);
 }
 
 int rswitch_init(void)
 {
+    int i, ret;
+
+    // Enable clocks to all the agents
+    for (i = 0; i < TOT_PORT_NUM; i++) {
+        rswitch_agent_clock_ctrl(i, 1);
+    }
+
+    /* Read MAC addresses (since it's only reading it does not affect other
+     * ports that we are not using) */
+    for (i = 0; i < NUM_DEVICES; i++) {
+        rswitch_etha_read_mac_address(&etha_props[i]);
+    }
+
+    rswitch_soft_reset();
+
+    CHECK_RET(rswitch_gwca_hw_init(&rsw_dev));
+
+    /* Copy speed property from GWCA */
+    rsw_dev.etha->speed = rsw_dev.gwca->speed;
+
+    rswitch_set_mac_address(rsw_dev.etha);
+
+    rsw_dev.gwca->rx_chain.chain_index = 0;
+    rsw_dev.gwca->rx_chain.ts_ring = rx_ring;
+    for (i = 0; i < TX_RX_RING_SIZE; i++) {
+        rsw_dev.gwca->rx_chain.data_buffers[i] = rx_data_buff[i];
+    }
+    rsw_dev.gwca->rx_chain.num_ring = TX_RX_RING_SIZE;
+    rsw_dev.gwca->rx_chain.dir_tx = 0;
+    rswitch_gwca_chain_ts_format(&rsw_dev, &(rsw_dev.gwca->rx_chain));
+    rsw_dev.gwca->irq_queue = 0;
+
+    rsw_dev.gwca->tx_chain.chain_index = 1;
+    rsw_dev.gwca->tx_chain.ring = tx_ring;
+    for (i = 0; i < TX_RX_RING_SIZE; i++) {
+        rsw_dev.gwca->tx_chain.data_buffers[i] = tx_data_buff[i];
+    }
+    rsw_dev.gwca->tx_chain.num_ring = TX_RX_RING_SIZE;
+    rsw_dev.gwca->tx_chain.dir_tx = 1;
+    rswitch_gwca_chain_format(&rsw_dev, &(rsw_dev.gwca->tx_chain));
+    rsw_dev.gwca->irq_queue = 1;
+
+    CHECK_RET(rswitch_bpool_config(&rsw_dev));
+
+    rswitch_fwd_init(&rsw_dev);
+
+    rswitch_enable_irqs();
+
+    return 0;
+}
+
+static int rswitch_etha_change_mode(struct rswitch_etha *etha,
+                                    enum rswitch_etha_mode mode)
+{
+    int ret;
+
+    /* Enable clock */
+    if (!rswitch_agent_clock_is_enabled(etha->port_num))
+        rswitch_agent_clock_ctrl(etha->port_num, 1);
+
+    reg_write32(mode, etha->base_addr + EAMC);
+
+    ret = rswitch_reg_wait(etha->base_addr + EAMS, EAMS_OPS_MASK, mode);
+
+    /* Disable clock */
+    if (mode == EAMC_OPC_DISABLE)
+        rswitch_agent_clock_ctrl(etha->port_num, 0);
+
+    return ret;
+}
+
+static void rswitch_rmac_setting(struct rswitch_etha *etha)
+{
+    uint32 val;
+
+    switch (etha->speed) {
+    case 10:
+        val = MPIC_LSC_10M;
+        break;
+    case 100:
+        val = MPIC_LSC_100M;
+        break;
+    case 1000:
+        val = MPIC_LSC_1G;
+        break;
+    default:
+        return;
+    }
+
+    reg_write32(MPIC_PIS_GMII | val, etha->base_addr + MPIC);
+}
+
+static void rswitch_etha_enable_mii(struct rswitch_etha *etha)
+{
+    rswitch_modify(etha->base_addr + MPIC, MPIC_PSMCS_MASK | MPIC_PSMHT_MASK,
+                    MPIC_PSMCS(0x05) | MPIC_PSMHT(0x06));
+    rswitch_modify(etha->base_addr + MPSM, 0, MPSM_MFF_C45);
+}
+
+static uint8 rswitch_etha_wait_link_verification(struct rswitch_etha *etha)
+{
+    /* Request Link Verification */
+    reg_write32(MLVC_PLV, etha->base_addr + MLVC);
+    return rswitch_reg_wait(etha->base_addr + MLVC, MLVC_PLV, 0);
+}
+
+static int rswitch_etha_hw_init(struct rswitch_etha *etha)
+{
+    int ret;
+
+    /* Change to CONFIG Mode */
+    CHECK_RET(rswitch_etha_change_mode(etha, EAMC_OPC_DISABLE));
+    CHECK_RET(rswitch_etha_change_mode(etha, EAMC_OPC_CONFIG));
+
+    reg_write32(EAVCC_VEM_SC_TAG, etha->base_addr + EAVCC);
+
+    rswitch_rmac_setting(etha);
+    rswitch_etha_enable_mii(etha);
+
+    /* Change to OPERATION Mode */
+    CHECK_RET(rswitch_etha_change_mode(etha, EAMC_OPC_OPERATION));
+
+    /* Link Verification */
+    return rswitch_etha_wait_link_verification(etha);
+}
+
+static int rswitch_phy_init(struct rswitch_etha *etha)
+{
+    uint32 reg_data;
+    int ret;
+
+    /* Reset */
+    CHECK_RET(rswitch_mii_read(etha, etha->port_num, 1, 0xC04A, &reg_data));
+    reg_data |= BIT(15);
+    CHECK_RET(rswitch_mii_write(etha, etha->port_num, 1, 0xC04A, &reg_data));
+    CHECK_RET(rswitch_mii_write(etha, etha->port_num, 1, 0xC04A, &reg_data)); /* MCAL does it twice... */
+
+    /* Check mode */
+    CHECK_RET(rswitch_mii_read(etha, etha->port_num, 1, 0xC04A, &reg_data));
+    if ((reg_data & 0x7) != 0x4 ) { /* 4 stands for SGMII */
+        reg_data &= ~0x7;
+        reg_data |= BIT(15) | 0x4;
+        CHECK_RET(rswitch_mii_write(etha, etha->port_num, 1, 0xC04A, &reg_data));
+
+        /* Run SERDES Init */
+        CHECK_RET(rswitch_mii_read(etha, etha->port_num, 1, 0x800F, &reg_data));
+        reg_data |= BIT(15) | BIT(13);
+        CHECK_RET(rswitch_mii_write(etha, etha->port_num, 1, 0x800F, &reg_data));
+        
+        reg_data = 0x0U;
+        do {
+            CHECK_RET(rswitch_mii_read(etha, etha->port_num, 1, 0x800F, &reg_data));
+        } while (reg_data & BIT(15));
+
+        /* Auto SERDES Init Disable */
+        reg_data &= ~BIT(13);
+        CHECK_RET(rswitch_mii_write(etha, etha->port_num, 1, 0x800F, &reg_data));
+    }
+
+    return 0;
+}
+
+int rswitch_open(void)
+{
+    int ret;
+
+    CHECK_RET(rswitch_etha_hw_init(rsw_dev.etha));
+    CHECK_RET(rswitch_phy_init(rsw_dev.etha));
+
+    /* Enable RX */
+    rswitch_modify(rsw_dev.gwca->base_addr + GWTRC0, 0,
+                   BIT(rsw_dev.gwca->rx_chain.chain_index));
+
+    /* Enable interrupt */
+    rswitch_enadis_data_irq(rsw_dev.gwca, rsw_dev.gwca->tx_chain.chain_index,
+                            TRUE);
+    rswitch_enadis_data_irq(rsw_dev.gwca, rsw_dev.gwca->rx_chain.chain_index,
+                            TRUE);
+
     return 0;
 }
\ No newline at end of file
diff --git a/examples/cortex-a/armv8/spider/ethernet/rswitch.h b/examples/cortex-a/armv8/spider/ethernet/rswitch.h
index 4bee5aae..fee6f633 100644
--- a/examples/cortex-a/armv8/spider/ethernet/rswitch.h
+++ b/examples/cortex-a/armv8/spider/ethernet/rswitch.h
@@ -2,5 +2,6 @@
 #define _ETH_H_
 
 int rswitch_init(void);
+int rswitch_open(void);
 
 #endif /* _ETH_H_ */
\ No newline at end of file
diff --git a/examples/cortex-a/armv8/spider/ethernet/rswitch_regs.h b/examples/cortex-a/armv8/spider/ethernet/rswitch_regs.h
new file mode 100644
index 00000000..20646df1
--- /dev/null
+++ b/examples/cortex-a/armv8/spider/ethernet/rswitch_regs.h
@@ -0,0 +1,659 @@
+#ifndef RSWITCH_REGS_H
+#define RSWITCH_REGS_H
+
+#define RSWITCH_BASE            0xE6880000
+
+#define RSWITCH_FWD_ADDR      (RSWITCH_BASE + 0x00000000)
+#define RSWITCH_FAB_ADDR      (RSWITCH_BASE + 0x00008000)
+#define RSWITCH_COMA_ADDR     (RSWITCH_BASE + 0x00009000)
+#define RSWITCH_ETHA0_ADDR    (RSWITCH_BASE + 0x0000a000)
+#define RSWITCH_ETHA1_ADDR    (RSWITCH_BASE + 0x0000c000)
+#define RSWITCH_ETHA2_ADDR    (RSWITCH_BASE + 0x0000e000)
+#define RSWITCH_GWCA0_ADDR    (RSWITCH_BASE + 0x00010000)
+#define RSWITCH_GWCA1_ADDR    (RSWITCH_BASE + 0x00012000)
+#define RSWITCH_GPTP_ADDR     (RSWITCH_BASE + 0x00018000)
+
+#define FWRO    0
+#define CARO    0
+#define GWRO    0
+#define TARO    0
+#define RMRO    0x1000
+
+#define FWGC        (FWRO + 0x0000)
+#define FWTTC0      (FWRO + 0x0010)
+#define FWTTC1      (FWRO + 0x0014)
+#define FWLBMC      (FWRO + 0x0018)
+#define FWCEPTC     (FWRO + 0x020)
+#define FWCEPRC0        (FWRO + 0x024)
+#define FWCEPRC1        (FWRO + 0x028)
+#define FWCEPRC2        (FWRO + 0x02C)
+#define FWCLPTC     (FWRO + 0x030)
+#define FWCLPRC     (FWRO + 0x034)
+#define FWCMPTC     (FWRO + 0x040)
+#define FWEMPTC     (FWRO + 0x044)
+#define FWSDMPTC        (FWRO + 0x050)
+#define FWSDMPVC        (FWRO + 0x054)
+#define FWLBWMC0        (FWRO + 0x080)
+#define FWPC00      (FWRO + 0x100)
+#define FWPC10      (FWRO + 0x104)
+#define FWPC20      (FWRO + 0x108)
+#define FWCTGC00        (FWRO + 0x400)
+#define FWCTGC10        (FWRO + 0x404)
+#define FWCTTC00        (FWRO + 0x408)
+#define FWCTTC10        (FWRO + 0x40C)
+#define FWCTTC200       (FWRO + 0x410)
+#define FWCTSC00        (FWRO + 0x420)
+#define FWCTSC10        (FWRO + 0x424)
+#define FWCTSC20        (FWRO + 0x428)
+#define FWCTSC30        (FWRO + 0x42C)
+#define FWCTSC40        (FWRO + 0x430)
+#define FWTWBFC0        (FWRO + 0x1000)
+#define FWTWBFVC0       (FWRO + 0x1004)
+#define FWTHBFC0        (FWRO + 0x1400)
+#define FWTHBFV0C0      (FWRO + 0x1404)
+#define FWTHBFV1C0      (FWRO + 0x1408)
+#define FWFOBFC0        (FWRO + 0x1800)
+#define FWFOBFV0C0      (FWRO + 0x1804)
+#define FWFOBFV1C0      (FWRO + 0x1808)
+#define FWRFC0      (FWRO + 0x1C00)
+#define FWRFVC0     (FWRO + 0x1C04)
+#define FWCFC0      (FWRO + 0x2000)
+#define FWCFMC00        (FWRO + 0x2004)
+#define FWIP4SC     (FWRO + 0x4008)
+#define FWIP6SC     (FWRO + 0x4018)
+#define FWIP6OC     (FWRO + 0x401C)
+#define FWL2SC      (FWRO + 0x4020)
+#define FWSFHEC     (FWRO + 0x4030)
+#define FWSHCR0     (FWRO + 0x4040)
+#define FWSHCR1     (FWRO + 0x4044)
+#define FWSHCR2     (FWRO + 0x4048)
+#define FWSHCR3     (FWRO + 0x404C)
+#define FWSHCR4     (FWRO + 0x4050)
+#define FWSHCR5     (FWRO + 0x4054)
+#define FWSHCR6     (FWRO + 0x4058)
+#define FWSHCR7     (FWRO + 0x405C)
+#define FWSHCR8     (FWRO + 0x4060)
+#define FWSHCR9     (FWRO + 0x4064)
+#define FWSHCR10        (FWRO + 0x4068)
+#define FWSHCR11        (FWRO + 0x406C)
+#define FWSHCR12        (FWRO + 0x4070)
+#define FWSHCR13        (FWRO + 0x4074)
+#define FWSHCRR     (FWRO + 0x4078)
+#define FWLTHHEC        (FWRO + 0x4090)
+#define FWLTHHC     (FWRO + 0x4094)
+#define FWLTHTL0        (FWRO + 0x40A0)
+#define FWLTHTL1        (FWRO + 0x40A4)
+#define FWLTHTL2        (FWRO + 0x40A8)
+#define FWLTHTL3        (FWRO + 0x40AC)
+#define FWLTHTL4        (FWRO + 0x40B0)
+#define FWLTHTL5        (FWRO + 0x40B4)
+#define FWLTHTL6        (FWRO + 0x40B8)
+#define FWLTHTL7        (FWRO + 0x40BC)
+#define FWLTHTL80       (FWRO + 0x40C0)
+#define FWLTHTL9        (FWRO + 0x40D0)
+#define FWLTHTLR        (FWRO + 0x40D4)
+#define FWLTHTIM        (FWRO + 0x40E0)
+#define FWLTHTEM        (FWRO + 0x40E4)
+#define FWLTHTS0        (FWRO + 0x4100)
+#define FWLTHTS1        (FWRO + 0x4104)
+#define FWLTHTS2        (FWRO + 0x4108)
+#define FWLTHTS3        (FWRO + 0x410C)
+#define FWLTHTS4        (FWRO + 0x4110)
+#define FWLTHTSR0       (FWRO + 0x4120)
+#define FWLTHTSR1       (FWRO + 0x4124)
+#define FWLTHTSR2       (FWRO + 0x4128)
+#define FWLTHTSR3       (FWRO + 0x412C)
+#define FWLTHTSR40      (FWRO + 0x4130)
+#define FWLTHTSR5       (FWRO + 0x4140)
+#define FWLTHTR     (FWRO + 0x4150)
+#define FWLTHTRR0       (FWRO + 0x4154)
+#define FWLTHTRR1       (FWRO + 0x4158)
+#define FWLTHTRR2       (FWRO + 0x415C)
+#define FWLTHTRR3       (FWRO + 0x4160)
+#define FWLTHTRR4       (FWRO + 0x4164)
+#define FWLTHTRR5       (FWRO + 0x4168)
+#define FWLTHTRR6       (FWRO + 0x416C)
+#define FWLTHTRR7       (FWRO + 0x4170)
+#define FWLTHTRR8       (FWRO + 0x4174)
+#define FWLTHTRR9       (FWRO + 0x4180)
+#define FWLTHTRR10      (FWRO + 0x4190)
+#define FWIPHEC     (FWRO + 0x4214)
+#define FWIPHC      (FWRO + 0x4218)
+#define FWIPTL0     (FWRO + 0x4220)
+#define FWIPTL1     (FWRO + 0x4224)
+#define FWIPTL2     (FWRO + 0x4228)
+#define FWIPTL3     (FWRO + 0x422C)
+#define FWIPTL4     (FWRO + 0x4230)
+#define FWIPTL5     (FWRO + 0x4234)
+#define FWIPTL6     (FWRO + 0x4238)
+#define FWIPTL7     (FWRO + 0x4240)
+#define FWIPTL8     (FWRO + 0x4250)
+#define FWIPTLR     (FWRO + 0x4254)
+#define FWIPTIM     (FWRO + 0x4260)
+#define FWIPTEM     (FWRO + 0x4264)
+#define FWIPTS0     (FWRO + 0x4270)
+#define FWIPTS1     (FWRO + 0x4274)
+#define FWIPTS2     (FWRO + 0x4278)
+#define FWIPTS3     (FWRO + 0x427C)
+#define FWIPTS4     (FWRO + 0x4280)
+#define FWIPTSR0        (FWRO + 0x4284)
+#define FWIPTSR1        (FWRO + 0x4288)
+#define FWIPTSR2        (FWRO + 0x428C)
+#define FWIPTSR3        (FWRO + 0x4290)
+#define FWIPTSR4        (FWRO + 0x42A0)
+#define FWIPTR      (FWRO + 0x42B0)
+#define FWIPTRR0        (FWRO + 0x42B4)
+#define FWIPTRR1        (FWRO + 0x42B8)
+#define FWIPTRR2        (FWRO + 0x42BC)
+#define FWIPTRR3        (FWRO + 0x42C0)
+#define FWIPTRR4        (FWRO + 0x42C4)
+#define FWIPTRR5        (FWRO + 0x42C8)
+#define FWIPTRR6        (FWRO + 0x42CC)
+#define FWIPTRR7        (FWRO + 0x42D0)
+#define FWIPTRR8        (FWRO + 0x42E0)
+#define FWIPTRR9        (FWRO + 0x42F0)
+#define FWIPHLEC        (FWRO + 0x4300)
+#define FWIPAGUSPC      (FWRO + 0x4500)
+#define FWIPAGC     (FWRO + 0x4504)
+#define FWIPAGM0        (FWRO + 0x4510)
+#define FWIPAGM1        (FWRO + 0x4514)
+#define FWIPAGM2        (FWRO + 0x4518)
+#define FWIPAGM3        (FWRO + 0x451C)
+#define FWIPAGM4        (FWRO + 0x4520)
+#define FWMACHEC        (FWRO + 0x4620)
+#define FWMACHC     (FWRO + 0x4624)
+#define FWMACTL0        (FWRO + 0x4630)
+#define FWMACTL1        (FWRO + 0x4634)
+#define FWMACTL2        (FWRO + 0x4638)
+#define FWMACTL3        (FWRO + 0x463C)
+#define FWMACTL4        (FWRO + 0x4640)
+#define FWMACTL5        (FWRO + 0x4650)
+#define FWMACTLR        (FWRO + 0x4654)
+#define FWMACTIM        (FWRO + 0x4660)
+#define FWMACTEM        (FWRO + 0x4664)
+#define FWMACTS0        (FWRO + 0x4670)
+#define FWMACTS1        (FWRO + 0x4674)
+#define FWMACTSR0       (FWRO + 0x4678)
+#define FWMACTSR1       (FWRO + 0x467C)
+#define FWMACTSR2       (FWRO + 0x4680)
+#define FWMACTSR3       (FWRO + 0x4690)
+#define FWMACTR     (FWRO + 0x46A0)
+#define FWMACTRR0       (FWRO + 0x46A4)
+#define FWMACTRR1       (FWRO + 0x46A8)
+#define FWMACTRR2       (FWRO + 0x46AC)
+#define FWMACTRR3       (FWRO + 0x46B0)
+#define FWMACTRR4       (FWRO + 0x46B4)
+#define FWMACTRR5       (FWRO + 0x46C0)
+#define FWMACTRR6       (FWRO + 0x46D0)
+#define FWMACHLEC       (FWRO + 0x4700)
+#define FWMACAGUSPC     (FWRO + 0x4880)
+#define FWMACAGC        (FWRO + 0x4884)
+#define FWMACAGM0       (FWRO + 0x4888)
+#define FWMACAGM1       (FWRO + 0x488C)
+#define FWVLANTEC       (FWRO + 0x4900)
+#define FWVLANTL0       (FWRO + 0x4910)
+#define FWVLANTL1       (FWRO + 0x4914)
+#define FWVLANTL2       (FWRO + 0x4918)
+#define FWVLANTL3       (FWRO + 0x4920)
+#define FWVLANTL4       (FWRO + 0x4930)
+#define FWVLANTLR       (FWRO + 0x4934)
+#define FWVLANTIM       (FWRO + 0x4940)
+#define FWVLANTEM       (FWRO + 0x4944)
+#define FWVLANTS        (FWRO + 0x4950)
+#define FWVLANTSR0      (FWRO + 0x4954)
+#define FWVLANTSR1      (FWRO + 0x4958)
+#define FWVLANTSR2      (FWRO + 0x4960)
+#define FWVLANTSR3      (FWRO + 0x4970)
+#define FWPBFCi     (FWRO + 0x4A00)
+#define FWPBFCSDC00     (FWRO + 0x4A04)
+#define FWL23URL0       (FWRO + 0x4E00)
+#define FWL23URL1       (FWRO + 0x4E04)
+#define FWL23URL2       (FWRO + 0x4E08)
+#define FWL23URL3       (FWRO + 0x4E0C)
+#define FWL23URLR       (FWRO + 0x4E10)
+#define FWL23UTIM       (FWRO + 0x4E20)
+#define FWL23URR        (FWRO + 0x4E30)
+#define FWL23URRR0      (FWRO + 0x4E34)
+#define FWL23URRR1      (FWRO + 0x4E38)
+#define FWL23URRR2      (FWRO + 0x4E3C)
+#define FWL23URRR3      (FWRO + 0x4E40)
+#define FWL23URMC0      (FWRO + 0x4F00)
+#define FWPMFGC0        (FWRO + 0x5000)
+#define FWPGFC0     (FWRO + 0x5100)
+#define FWPGFIGSC0      (FWRO + 0x5104)
+#define FWPGFENC0       (FWRO + 0x5108)
+#define FWPGFENM0       (FWRO + 0x510c)
+#define FWPGFCSTC00     (FWRO + 0x5110)
+#define FWPGFCSTC10     (FWRO + 0x5114)
+#define FWPGFCSTM00     (FWRO + 0x5118)
+#define FWPGFCSTM10     (FWRO + 0x511C)
+#define FWPGFCTC0       (FWRO + 0x5120)
+#define FWPGFCTM0       (FWRO + 0x5124)
+#define FWPGFHCC0       (FWRO + 0x5128)
+#define FWPGFSM0        (FWRO + 0x512C)
+#define FWPGFGC0        (FWRO + 0x5130)
+#define FWPGFGL0        (FWRO + 0x5500)
+#define FWPGFGL1        (FWRO + 0x5504)
+#define FWPGFGLR        (FWRO + 0x5518)
+#define FWPGFGR     (FWRO + 0x5510)
+#define FWPGFGRR0       (FWRO + 0x5514)
+#define FWPGFGRR1       (FWRO + 0x5518)
+#define FWPGFRIM        (FWRO + 0x5520)
+#define FWPMTRFC0       (FWRO + 0x5600)
+#define FWPMTRCBSC0     (FWRO + 0x5604)
+#define FWPMTRC0RC0     (FWRO + 0x5608)
+#define FWPMTREBSC0     (FWRO + 0x560C)
+#define FWPMTREIRC0     (FWRO + 0x5610)
+#define FWPMTRFM0       (FWRO + 0x5614)
+#define FWFTL0      (FWRO + 0x6000)
+#define FWFTL1      (FWRO + 0x6004)
+#define FWFTLR      (FWRO + 0x6008)
+#define FWFTOC      (FWRO + 0x6010)
+#define FWFTOPC     (FWRO + 0x6014)
+#define FWFTIM      (FWRO + 0x6020)
+#define FWFTR       (FWRO + 0x6030)
+#define FWFTRR0     (FWRO + 0x6034)
+#define FWFTRR1     (FWRO + 0x6038)
+#define FWFTRR2     (FWRO + 0x603C)
+#define FWSEQNGC0       (FWRO + 0x6100)
+#define FWSEQNGM0       (FWRO + 0x6104)
+#define FWSEQNRC        (FWRO + 0x6200)
+#define FWCTFDCN0       (FWRO + 0x6300)
+#define FWLTHFDCN0      (FWRO + 0x6304)
+#define FWIPFDCN0       (FWRO + 0x6308)
+#define FWLTWFDCN0      (FWRO + 0x630C)
+#define FWPBFDCN0       (FWRO + 0x6310)
+#define FWMHLCN0        (FWRO + 0x6314)
+#define FWIHLCN0        (FWRO + 0x6318)
+#define FWICRDCN0       (FWRO + 0x6500)
+#define FWWMRDCN0       (FWRO + 0x6504)
+#define FWCTRDCN0       (FWRO + 0x6508)
+#define FWLTHRDCN0      (FWRO + 0x650C)
+#define FWIPRDCN0       (FWRO + 0x6510)
+#define FWLTWRDCN0      (FWRO + 0x6514)
+#define FWPBRDCN0       (FWRO + 0x6518)
+#define FWPMFDCN0       (FWRO + 0x6700)
+#define FWPGFDCN0       (FWRO + 0x6780)
+#define FWPMGDCN0       (FWRO + 0x6800)
+#define FWPMYDCN0       (FWRO + 0x6804)
+#define FWPMRDCN0       (FWRO + 0x6808)
+#define FWFRPPCN0       (FWRO + 0x6A00)
+#define FWFRDPCN0       (FWRO + 0x6A04)
+#define FWEIS00     (FWRO + 0x7900)
+#define FWEIE00     (FWRO + 0x7904)
+#define FWEID00     (FWRO + 0x7908)
+#define FWEIS1      (FWRO + 0x7A00)
+#define FWEIE1      (FWRO + 0x7A04)
+#define FWEID1      (FWRO + 0x7A08)
+#define FWEIS2      (FWRO + 0x7A10)
+#define FWEIE2      (FWRO + 0x7A14)
+#define FWEID2      (FWRO + 0x7A18)
+#define FWEIS3      (FWRO + 0x7A20)
+#define FWEIE3      (FWRO + 0x7A24)
+#define FWEID3      (FWRO + 0x7A28)
+#define FWEIS4      (FWRO + 0x7A30)
+#define FWEIE4      (FWRO + 0x7A34)
+#define FWEID4      (FWRO + 0x7A38)
+#define FWEIS5      (FWRO + 0x7A40)
+#define FWEIE5      (FWRO + 0x7A44)
+#define FWEID5      (FWRO + 0x7A48)
+#define FWEIS60     (FWRO + 0x7A50)
+#define FWEIE60     (FWRO + 0x7A54)
+#define FWEID60     (FWRO + 0x7A58)
+#define FWEIS61     (FWRO + 0x7A60)
+#define FWEIE61     (FWRO + 0x7A64)
+#define FWEID61     (FWRO + 0x7A68)
+#define FWEIS62     (FWRO + 0x7A70)
+#define FWEIE62     (FWRO + 0x7A74)
+#define FWEID62     (FWRO + 0x7A78)
+#define FWEIS63     (FWRO + 0x7A80)
+#define FWEIE63     (FWRO + 0x7A84)
+#define FWEID63     (FWRO + 0x7A88)
+#define FWEIS70     (FWRO + 0x7A90)
+#define FWEIE70     (FWRO + 0x7A94)
+#define FWEID70     (FWRO + 0x7A98)
+#define FWEIS71     (FWRO + 0x7AA0)
+#define FWEIE71     (FWRO + 0x7AA4)
+#define FWEID71     (FWRO + 0x7AA8)
+#define FWEIS72     (FWRO + 0x7AB0)
+#define FWEIE72     (FWRO + 0x7AB4)
+#define FWEID72     (FWRO + 0x7AB8)
+#define FWEIS73     (FWRO + 0x7AC0)
+#define FWEIE73     (FWRO + 0x7AC4)
+#define FWEID73     (FWRO + 0x7AC8)
+#define FWEIS80     (FWRO + 0x7AD0)
+#define FWEIE80     (FWRO + 0x7AD4)
+#define FWEID80     (FWRO + 0x7AD8)
+#define FWEIS81     (FWRO + 0x7AE0)
+#define FWEIE81     (FWRO + 0x7AE4)
+#define FWEID81     (FWRO + 0x7AE8)
+#define FWEIS82     (FWRO + 0x7AF0)
+#define FWEIE82     (FWRO + 0x7AF4)
+#define FWEID82     (FWRO + 0x7AF8)
+#define FWEIS83     (FWRO + 0x7B00)
+#define FWEIE83     (FWRO + 0x7B04)
+#define FWEID83     (FWRO + 0x7B08)
+#define FWMIS0      (FWRO + 0x7C00)
+#define FWMIE0      (FWRO + 0x7C04)
+#define FWMID0      (FWRO + 0x7C08)
+#define FWSCR0      (FWRO + 0x7D00)
+#define FWSCR1      (FWRO + 0x7D04)
+#define FWSCR2      (FWRO + 0x7D08)
+#define FWSCR3      (FWRO + 0x7D0C)
+#define FWSCR4      (FWRO + 0x7D10)
+#define FWSCR5      (FWRO + 0x7D14)
+#define FWSCR6      (FWRO + 0x7D18)
+#define FWSCR7      (FWRO + 0x7D1C)
+#define FWSCR8      (FWRO + 0x7D20)
+#define FWSCR9      (FWRO + 0x7D24)
+#define FWSCR10     (FWRO + 0x7D28)
+#define FWSCR11     (FWRO + 0x7D2C)
+#define FWSCR12     (FWRO + 0x7D30)
+#define FWSCR13     (FWRO + 0x7D34)
+#define FWSCR14     (FWRO + 0x7D38)
+#define FWSCR15     (FWRO + 0x7D3C)
+#define FWSCR16     (FWRO + 0x7D40)
+#define FWSCR17     (FWRO + 0x7D44)
+#define FWSCR18     (FWRO + 0x7D48)
+#define FWSCR19     (FWRO + 0x7D4C)
+#define FWSCR20     (FWRO + 0x7D50)
+#define FWSCR21     (FWRO + 0x7D54)
+#define FWSCR22     (FWRO + 0x7D58)
+#define FWSCR23     (FWRO + 0x7D5C)
+#define FWSCR24     (FWRO + 0x7D60)
+#define FWSCR25     (FWRO + 0x7D64)
+#define FWSCR26     (FWRO + 0x7D68)
+#define FWSCR27     (FWRO + 0x7D6C)
+#define FWSCR28     (FWRO + 0x7D70)
+#define FWSCR29     (FWRO + 0x7D74)
+#define FWSCR30     (FWRO + 0x7D78)
+#define FWSCR31     (FWRO + 0x7D7C)
+#define FWSCR32     (FWRO + 0x7D80)
+#define FWSCR33     (FWRO + 0x7D84)
+#define FWSCR34     (FWRO + 0x7D88)
+#define FWSCR35     (FWRO + 0x7D8C)
+#define FWSCR36     (FWRO + 0x7D90)
+#define FWSCR37     (FWRO + 0x7D94)
+#define FWSCR38     (FWRO + 0x7D98)
+#define FWSCR39     (FWRO + 0x7D9C)
+#define FWSCR40     (FWRO + 0x7DA0)
+#define FWSCR41     (FWRO + 0x7DA4)
+#define FWSCR42     (FWRO + 0x7DA8)
+#define FWSCR43     (FWRO + 0x7DAC)
+#define FWSCR44     (FWRO + 0x7DB0)
+#define FWSCR45     (FWRO + 0x7DB4)
+#define FWSCR46     (FWRO + 0x7DB8)
+
+#define RIPV        (CARO + 0x0000)
+#define RRC     (CARO + 0x0004)
+#define RCEC        (CARO + 0x0008)
+#define RCDC        (CARO + 0x000C)
+#define RSSIS       (CARO + 0x0010)
+#define RSSIE       (CARO + 0x0014)
+#define RSSID       (CARO + 0x0018)
+#define CABPIBWMC       (CARO + 0x0020)
+#define CABPWMLC        (CARO + 0x0040)
+#define CABPPFLC0       (CARO + 0x0050)
+#define CABPPWMLC0      (CARO + 0x0060)
+#define CABPPPFLC00     (CARO + 0x00A0)
+#define CABPULC     (CARO + 0x0100)
+#define CABPIRM     (CARO + 0x0140)
+#define CABPPCM     (CARO + 0x0144)
+#define CABPLCM     (CARO + 0x0148)
+#define CABPCPM     (CARO + 0x0180)
+#define CABPMCPM        (CARO + 0x0200)
+#define CARDNM      (CARO + 0x0280)
+#define CARDMNM     (CARO + 0x0284)
+#define CARDCN      (CARO + 0x0290)
+#define CAEIS0      (CARO + 0x0300)
+#define CAEIE0      (CARO + 0x0304)
+#define CAEID0      (CARO + 0x0308)
+#define CAEIS1      (CARO + 0x0310)
+#define CAEIE1      (CARO + 0x0314)
+#define CAEID1      (CARO + 0x0318)
+#define CAMIS0      (CARO + 0x0340)
+#define CAMIE0      (CARO + 0x0344)
+#define CAMID0      (CARO + 0x0348)
+#define CAMIS1      (CARO + 0x0350)
+#define CAMIE1      (CARO + 0x0354)
+#define CAMID1      (CARO + 0x0358)
+#define CASCR       (CARO + 0x0380)
+
+/* Ethernet Agent Address space Empty in spec */
+#define EAMC        (TARO + 0x0000)
+#define EAMS        (TARO + 0x0004)
+#define EAIRC       (TARO + 0x0010)
+#define EATDQSC     (TARO + 0x0014)
+#define EATDQC      (TARO + 0x0018)
+#define EATDQAC     (TARO + 0x001C)
+#define EATPEC      (TARO + 0x0020)
+#define EATMFSC0        (TARO + 0x0040)
+#define EATDQDC0        (TARO + 0x0060)
+#define EATDQM0     (TARO + 0x0080)
+#define EATDQMLM0       (TARO + 0x00A0)
+#define EACTQC      (TARO + 0x0100)
+#define EACTDQDC        (TARO + 0x0104)
+#define EACTDQM     (TARO + 0x0108)
+#define EACTDQMLM       (TARO + 0x010C)
+#define EAVCC       (TARO + 0x0130)
+#define EAVTC       (TARO + 0x0134)
+#define EATTFC      (TARO + 0x0138)
+#define EACAEC      (TARO + 0x0200)
+#define EACC        (TARO + 0x0204)
+#define EACAIVC0        (TARO + 0x0220)
+#define EACAULC0        (TARO + 0x0240)
+#define EACOEM      (TARO + 0x0260)
+#define EACOIVM0        (TARO + 0x0280)
+#define EACOULM0        (TARO + 0x02A0)
+#define EACGSM      (TARO + 0x02C0)
+#define EATASC      (TARO + 0x0300)
+#define EATASENC0       (TARO + 0x0320)
+#define EATASCTENC      (TARO + 0x0340)
+#define EATASENM0       (TARO + 0x0360)
+#define EATASCTENM      (TARO + 0x0380)
+#define EATASCSTC0      (TARO + 0x03A0)
+#define EATASCSTC1      (TARO + 0x03A4)
+#define EATASCSTM0      (TARO + 0x03A8)
+#define EATASCSTM1      (TARO + 0x03AC)
+#define EATASCTC        (TARO + 0x03B0)
+#define EATASCTM        (TARO + 0x03B4)
+#define EATASGL0        (TARO + 0x03C0)
+#define EATASGL1        (TARO + 0x03C4)
+#define EATASGLR        (TARO + 0x03C8)
+#define EATASGR     (TARO + 0x03D0)
+#define EATASGRR        (TARO + 0x03D4)
+#define EATASHCC        (TARO + 0x03E0)
+#define EATASRIRM       (TARO + 0x03E4)
+#define EATASSM     (TARO + 0x03E8)
+#define EAUSMFSECN      (TARO + 0x0400)
+#define EATFECN     (TARO + 0x0404)
+#define EAFSECN     (TARO + 0x0408)
+#define EADQOECN        (TARO + 0x040C)
+#define EADQSECN        (TARO + 0x0410)
+#define EACKSECN        (TARO + 0x0414)
+#define EAEIS0      (TARO + 0x0500)
+#define EAEIE0      (TARO + 0x0504)
+#define EAEID0      (TARO + 0x0508)
+#define EAEIS1      (TARO + 0x0510)
+#define EAEIE1      (TARO + 0x0514)
+#define EAEID1      (TARO + 0x0518)
+#define EAEIS2      (TARO + 0x0520)
+#define EAEIE2      (TARO + 0x0524)
+#define EAEID2      (TARO + 0x0528)
+#define EASCR       (TARO + 0x0580)
+
+#define MPSM        (RMRO + 0x0000)
+#define MPIC        (RMRO + 0x0004)
+#define MPIM        (RMRO + 0x0008)
+#define MIOC        (RMRO + 0x0010)
+#define MIOM        (RMRO + 0x0014)
+#define MXMS        (RMRO + 0x0018)
+#define MTFFC       (RMRO + 0x0020)
+#define MTPFC       (RMRO + 0x0024)
+#define MTPFC2      (RMRO + 0x0028)
+#define MTPFC30     (RMRO + 0x0030)
+#define MTATC0      (RMRO + 0x0050)
+#define MTIM        (RMRO + 0x0060)
+#define MRGC        (RMRO + 0x0080)
+#define MRMAC0      (RMRO + 0x0084)
+#define MRMAC1      (RMRO + 0x0088)
+#define MRAFC       (RMRO + 0x008C)
+#define MRSCE       (RMRO + 0x0090)
+#define MRSCP       (RMRO + 0x0094)
+#define MRSCC       (RMRO + 0x0098)
+#define MRFSCE      (RMRO + 0x009C)
+#define MRFSCP      (RMRO + 0x00a0)
+#define MTRC        (RMRO + 0x00a4)
+#define MRIM        (RMRO + 0x00a8)
+#define MRPFM       (RMRO + 0x00aC)
+#define MPFC0       (RMRO + 0x0100)
+#define MLVC        (RMRO + 0x0180)
+#define MEEEC       (RMRO + 0x0184)
+#define MLBC        (RMRO + 0x0188)
+#define MXGMIIC     (RMRO + 0x0190)
+#define MPCH        (RMRO + 0x0194)
+#define MANC        (RMRO + 0x0198)
+#define MANM        (RMRO + 0x019C)
+#define MPLCA1      (RMRO + 0x01a0)
+#define MPLCA2      (RMRO + 0x01a4)
+#define MPLCA3      (RMRO + 0x01a8)
+#define MPLCA4      (RMRO + 0x01ac)
+#define MPLCAM      (RMRO + 0x01b0)
+#define MHDC1       (RMRO + 0x01c0)
+#define MHDC2       (RMRO + 0x01c4)
+#define MEIS        (RMRO + 0x0200)
+#define MEIE        (RMRO + 0x0204)
+#define MEID        (RMRO + 0x0208)
+#define MMIS0       (RMRO + 0x0210)
+#define MMIE0       (RMRO + 0x0214)
+#define MMID0       (RMRO + 0x0218)
+#define MMIS1       (RMRO + 0x0220)
+#define MMIE1       (RMRO + 0x0224)
+#define MMID1       (RMRO + 0x0228)
+#define MMIS2       (RMRO + 0x0230)
+#define MMIE2       (RMRO + 0x0234)
+#define MMID2       (RMRO + 0x0238)
+#define MMPFTCT     (RMRO + 0x0300)
+#define MAPFTCT     (RMRO + 0x0304)
+#define MPFRCT      (RMRO + 0x0308)
+#define MFCICT      (RMRO + 0x030c)
+#define MEEECT      (RMRO + 0x0310)
+#define MMPCFTCT0       (RMRO + 0x0320)
+#define MAPCFTCT0       (RMRO + 0x0330)
+#define MPCFRCT0        (RMRO + 0x0340)
+#define MHDCC       (RMRO + 0x0350)
+#define MROVFC      (RMRO + 0x0354)
+#define MRHCRCEC        (RMRO + 0x0358)
+#define MRXBCE      (RMRO + 0x0400)
+#define MRXBCP      (RMRO + 0x0404)
+#define MRGFCE      (RMRO + 0x0408)
+#define MRGFCP      (RMRO + 0x040C)
+#define MRBFC       (RMRO + 0x0410)
+#define MRMFC       (RMRO + 0x0414)
+#define MRUFC       (RMRO + 0x0418)
+#define MRPEFC      (RMRO + 0x041C)
+#define MRNEFC      (RMRO + 0x0420)
+#define MRFMEFC     (RMRO + 0x0424)
+#define MRFFMEFC        (RMRO + 0x0428)
+#define MRCFCEFC        (RMRO + 0x042C)
+#define MRFCEFC     (RMRO + 0x0430)
+#define MRRCFEFC        (RMRO + 0x0434)
+#define MRUEFC      (RMRO + 0x043C)
+#define MROEFC      (RMRO + 0x0440)
+#define MRBOEC      (RMRO + 0x0444)
+#define MTXBCE      (RMRO + 0x0500)
+#define MTXBCP      (RMRO + 0x0504)
+#define MTGFCE      (RMRO + 0x0508)
+#define MTGFCP      (RMRO + 0x050C)
+#define MTBFC       (RMRO + 0x0510)
+#define MTMFC       (RMRO + 0x0514)
+#define MTUFC       (RMRO + 0x0518)
+#define MTEFC       (RMRO + 0x051C)
+
+#define GWMC        (GWRO + 0x0000)
+#define GWMS        (GWRO + 0x0004)
+#define GWIRC       (GWRO + 0x0010)
+#define GWRDQSC     (GWRO + 0x0014)
+#define GWRDQC      (GWRO + 0x0018)
+#define GWRDQAC     (GWRO + 0x001C)
+#define GWRGC       (GWRO + 0x0020)
+#define GWRMFSC0        (GWRO + 0x0040)
+#define GWRDQDC0        (GWRO + 0x0060)
+#define GWRDQM0     (GWRO + 0x0080)
+#define GWRDQMLM0       (GWRO + 0x00A0)
+#define GWMTIRM     (GWRO + 0x0100)
+#define GWMSTLS     (GWRO + 0x0104)
+#define GWMSTLR     (GWRO + 0x0108)
+#define GWMSTSS     (GWRO + 0x010C)
+#define GWMSTSR     (GWRO + 0x0110)
+#define GWMAC0      (GWRO + 0x0120)
+#define GWMAC1      (GWRO + 0x0124)
+#define GWVCC       (GWRO + 0x0130)
+#define GWVTC       (GWRO + 0x0134)
+#define GWTTFC      (GWRO + 0x0138)
+#define GWTDCAC00       (GWRO + 0x0140)
+#define GWTDCAC10       (GWRO + 0x0144)
+#define GWTSDCC0        (GWRO + 0x0160)
+#define GWTNM       (GWRO + 0x0180)
+#define GWTMNM      (GWRO + 0x0184)
+#define GWAC        (GWRO + 0x0190)
+#define GWDCBAC0        (GWRO + 0x0194)
+#define GWDCBAC1        (GWRO + 0x0198)
+#define GWIICBSC        (GWRO + 0x019C)
+#define GWMDNC      (GWRO + 0x01A0)
+#define GWTRC0      (GWRO + 0x0200)
+#define GWTPC0      (GWRO + 0x0300)
+#define GWARIRM     (GWRO + 0x0380)
+#define GWDCC0      (GWRO + 0x0400)
+#define GWAARSS     (GWRO + 0x0800)
+#define GWAARSR0        (GWRO + 0x0804)
+#define GWAARSR1        (GWRO + 0x0808)
+#define GWIDAUAS0       (GWRO + 0x0840)
+#define GWIDASM0        (GWRO + 0x0880)
+#define GWIDASAM00      (GWRO + 0x0900)
+#define GWIDASAM10      (GWRO + 0x0904)
+#define GWIDACAM00      (GWRO + 0x0980)
+#define GWIDACAM10      (GWRO + 0x0984)
+#define GWGRLC      (GWRO + 0x0A00)
+#define GWGRLULC        (GWRO + 0x0A04)
+#define GWRLIVC0        (GWRO + 0x0A80)
+#define GWRLULC0        (GWRO + 0x0A84)
+#define GWIDPC      (GWRO + 0x0B00)
+#define GWIDC0      (GWRO + 0x0C00)
+#define GWDIS0      (GWRO + 0x1100)
+#define GWDIE0      (GWRO + 0x1104)
+#define GWDID0      (GWRO + 0x1108)
+#define GWTSDIS     (GWRO + 0x1180)
+#define GWTSDIE     (GWRO + 0x1184)
+#define GWTSDID     (GWRO + 0x1188)
+#define GWEIS0      (GWRO + 0x1190)
+#define GWEIE0      (GWRO + 0x1194)
+#define GWEID0      (GWRO + 0x1198)
+#define GWEIS1      (GWRO + 0x11A0)
+#define GWEIE1      (GWRO + 0x11A4)
+#define GWEID1      (GWRO + 0x11A8)
+#define GWEIS20     (GWRO + 0x1200)
+#define GWEIE20     (GWRO + 0x1204)
+#define GWEID20     (GWRO + 0x1208)
+#define GWEIS3      (GWRO + 0x1280)
+#define GWEIE3      (GWRO + 0x1284)
+#define GWEID3      (GWRO + 0x1288)
+#define GWEIS4      (GWRO + 0x1290)
+#define GWEIE4      (GWRO + 0x1294)
+#define GWEID4      (GWRO + 0x1298)
+#define GWEIS5      (GWRO + 0x12A0)
+#define GWEIE5      (GWRO + 0x12A4)
+#define GWEID5      (GWRO + 0x12A8)
+#define GWSCR0      (GWRO + 0x1800)
+#define GWSCR1      (GWRO + 0x1900)
+
+/* COMA */
+#define RRC_RR		BIT(0)
+#define RRC_RR_CLR	(0)
+#define RCEC_RCE	BIT(16)
+#define RCDC_RCD	BIT(16)
+
+#define CABPIRM_BPIOG	BIT(0)
+#define CABPIRM_BPR	BIT(1)
+
+#endif /* RSWITCH_REGS_H */
\ No newline at end of file
diff --git a/examples/cortex-a/armv8/spider/ethernet/utils.c b/examples/cortex-a/armv8/spider/ethernet/utils.c
index 7b0abd57..3f6b23f3 100644
--- a/examples/cortex-a/armv8/spider/ethernet/utils.c
+++ b/examples/cortex-a/armv8/spider/ethernet/utils.c
@@ -1,5 +1,6 @@
 #include "tpl_os.h"
 #include "utils.h"
+#include "pfcmap.h"
 
 #define ETH_CPUCLK_MHZ 1066UL
 #define ETH_WAIT_NS(t) \
@@ -60,6 +61,16 @@ void rswitch_enable_clock_and_reset(void)
     ETH_WAIT_NS(40*1000);
 }
 
+uint32 reg_read32(uint32 addr)
+{
+    return *((volatile uint32*)addr);
+}
+
+void reg_write32(uint32 data, uint32 addr)
+{
+    *((volatile uint32*)addr) = data;
+}
+
 extern volatile VAR(uint32, OS_VAR) tpl_time_counter;
 uint32 get_time(void)
 {
@@ -71,6 +82,13 @@ uint32 get_elapsed_time(uint32 start_val)
     return (tpl_time_counter - start_val);
 }
 
+void ms_delay(uint32 value)
+{
+    uint32 start_time = get_time();
+
+    while((get_elapsed_time(start_time) - start_time) < value);
+}
+
 void port_init(void)
 {
     uint32 dataL;
@@ -116,3 +134,10 @@ void port_init(void)
     *((volatile uint32 *)(PFC_PMMR(PFC_GPn_BASE(3)))) = ~dataL;
     *((volatile uint32 *)(PFC_POC_GPn_DM0(3))) = dataL;
 }
+
+#define GICD_ISENABLER     (0xf0000100)
+
+void enable_int(uint32 num)
+{
+	*((volatile uint32 *)(GICD_ISENABLER + 4 * (num / 32)))  = 1 << (num % 32);
+}
\ No newline at end of file
diff --git a/examples/cortex-a/armv8/spider/ethernet/utils.h b/examples/cortex-a/armv8/spider/ethernet/utils.h
index cfadb301..4ddd9b03 100644
--- a/examples/cortex-a/armv8/spider/ethernet/utils.h
+++ b/examples/cortex-a/armv8/spider/ethernet/utils.h
@@ -3,11 +3,34 @@
 
 #include "tpl_os.h"
 
+#define debug_msg(...)    do {} while(0) // TODO
+
 #define BIT(x)      (1UL << x)
+#define BITS_PER_TYPE(x)    (sizeof(x) * 8)
+#define BITS_PER_LONG       (BITS_PER_TYPE(unsigned long))
+
+#define GENMASK(h, l) \
+        (((~(0UL)) - ((1UL) << (l)) + 1) & \
+         (~(0UL) >> (BITS_PER_LONG - 1 - (h))))
+
+#define __unused    __attribute__((unused))
+
+#define CHECK_RET(f)    \
+    ret = (f); \
+    if (ret != 0) { \
+        return ret; \
+    }
+
+uint32 reg_read32(uint32 addr);
+void reg_write32(uint32 data, uint32 addr);
 
 void rswitch_enable_clock_and_reset(void);
 void port_init(void);
+
+void enable_int(uint32 irq);
+
 uint32 get_time(void);
 uint32 get_elapsed_time(uint32 start_val);
+void ms_delay(uint32 value);
 
 #endif /* _UTILS_H_ */
\ No newline at end of file
-- 
2.25.1

