add beampattern refreash method

comtool-plugin-duifei.py

@@ -34,48 +34,67 @@ class Plugin(Plugin_Base):
             "version": 1,
             "customSendItems": [
                 {
-                    "caption": "Self Test",
+                    "caption": "自检",
                     "icon": "fa5.question-circle",
                     "data": "5A 54 E1 DD 00 BE 02 5A FE"
                 },
+                # {
+                #     "caption": "数据发送测试 - 1",
+                #     "icon": "fa5.play-circle",
+                #     "data": ""
+                # },
+                # {
+                #     "caption": "数据发送测试 - 2",
+                #     "icon": "fa5.play-circle",
+                #     "data": ""
+                # },
                 {
-                    "caption": "Config - FLJ Low Power",
+                    "caption": "装订 - 【主】反拦截小功率",
                     "icon": "fa5.play-circle",
-                    "data": "5A 54 E3 DD 88 11 00 00 00 59 06 5A FE"
-                },{
-                    "caption": "Config - FLJ Full Power",
+                    "data": "5A 54 E3 DD 77 11 00 00 00 48 06 5A FE"
+                },
+                {
+                    "caption": "装订 - 【主】反拦截全功率",
                     "icon": "fa5.play-circle",
                     "data": "5A 54 E3 DD 66 11 00 00 00 37 06 5A FE"
-                }, {
-                    "caption": "Config - FLJ Silent",
+                },
+                {
+                    "caption": "装订 - 【主】反拦截静默",
                     "icon": "fa5.play-circle",
-                    "data": "5A 54 E3 DD 00 11 00 00 00 D1 06 5A FE"
-                },{
-                    "caption": "Config - LJ Low Power",
-                    "icon": "fa5.play-circle",
-                    "data": "5A 54 E3 DD 88 00 00 00 00 48 06 5A FE"
-                },{
-                    "caption": "Config - LJ Full Power",
-                    "icon": "fa5.play-circle",
-                    "data": "5A 54 E3 DD 66 00 00 00 00 26 06 5A FE"
-                },{
-                    "caption": "Config - LJ Silent",
-                    "icon": "fa5.play-circle",
-                    "data": "5A 54 E3 DD 00 00 00 00 00 C0 06 5A FE"
-                },{
-                    "caption": "StatusCheck - FLJ",
+                    "data": "5A 54 E3 DD 88 11 00 00 00 59 06 5A FE"
+                },
+                {
+                    "caption": "遥测 - 【主】反拦截",
                     "icon": "fa5.play-circle",
                     "data": "5A 54 E6 DD 99 00 5C 03 5A FE"
-                },{
-                    "caption": "StatusCheck - LJ",
-                    "icon": "fa5.play-circle",
-                    "data": "5A 54 F6 DD 99 00 6C 03 5A FE"
-                },{
-                    "caption": "DataRetrieve - FLJ",
+                },
+                {
+                    "caption": "点名 - 【主】反拦截",
                     "icon": "fa5.play-circle",
                     "data": "5A 54 E5 DD 77 00 39 03 5A FE"
-                },{
-                    "caption": "DataRetrieve - LJ",
+                },
+                {
+                    "caption": "装订 - 【从】拦截小功率",
+                    "icon": "fa5.play-circle",
+                    "data": "5A 54 E3 DD 77 00 00 00 00 37 06 5A FE"
+                },
+                {
+                    "caption": "装订 - 【从】拦截大功率",
+                    "icon": "fa5.play-circle",
+                    "data": "5A 54 E3 DD 66 00 00 00 00 26 06 5A FE"
+                },
+                {
+                    "caption": "装订 - 【从】拦截静默",
+                    "icon": "fa5.play-circle",
+                    "data": "5A 54 E3 DD 88 00 00 00 00 48 06 5A FE"
+                },
+                {
+                    "caption": "遥测 - 【从】拦截",
+                    "icon": "fa5.play-circle",
+                    "data": "5A 54 F6 DD 99 00 6C 03 5A FE"
+                },
+                {
+                    "caption": "点名 - 【从】拦截",
                     "icon": "fa5.play-circle",
                     "data": "5A 54 F5 DD 77 00 49 03 5A FE"
                 }
@@ -96,7 +115,7 @@ class Plugin(Plugin_Base):
         self.mainWidget = QSplitter(Qt.Vertical)
         # receive widget
         self.receiveWidget = QTextEdit("")
-        font = QFont('Menlo, Consolas, Bitstream Vera Sans Mono, Courier New, monospace, Microsoft YaHei', 10)
+        font = QFont('Consolas, DejaVu Sans Mono, Courier New, monospace, Microsoft YaHei', 10)
         self.receiveWidget.setFont(font)
         self.receiveWidget.setLineWrapMode(QTextEdit.NoWrap)
         # clear button
@@ -163,7 +182,10 @@ class Plugin(Plugin_Base):
         self.showReceiveDataSignal.connect(self.showReceivedData)
         # frame struct
         self.structSelfTestRsp = Struct("<BB B BBBbbb BbBBB H")
-        self.structStatusCheckRsp = Struct("<BB Q      HIII      IBBBBI BBHH HHHIIIIII i II I      IIII d Q dBB dB H I      BBBbbb BbBBB H")
+        self.structStatusCheckRsp = Struct("<BB Q      HIII      IBBBBI BBHH HHHIIIIII i II 4s      IIII d Q dBB dB H B 3s      BBBbbb BbBBB 2s")
+        self.structFljData = Struct("<dddd IHI fff fff")
+        self.structLjData = Struct("<IHI ffff IHI fff fff fff IHI fff fff fff IHI fff fff fff I")
+        self.structSysInfo = Struct("<IHI H B I B fff fff fff")
 
     def insertSendItem(self, item = None, load = False):
         caption = item["caption"]
@@ -225,6 +247,8 @@ class Plugin(Plugin_Base):
             self.showReceiveDataSignal.emit(self.evalSelfTestRsp(data) + "\n")
         elif data[0] == 0xE6 or data[0] == 0xF6:
             self.showReceiveDataSignal.emit(self.evalStatusCheckRsp(data) + "\n")
+        elif data[0] == 0xE5 or data[0] == 0xF5:
+            self.showReceiveDataSignal.emit('Retrieved Data Frame:\n' + self.printFrame(data) + "\n")
         else:
             self.showReceiveDataSignal.emit(f"\n### Unexpected response code 0x{data[0]:02x}.\n")
 
@@ -235,11 +259,22 @@ class Plugin(Plugin_Base):
             return f"\n### evalSelfTestRsp:: Failed to unpack data (data length {len(data)} byte).\n"
 
         ret = "----------- 自检回令 -----------\n"
+
         ret += "节点类型: "
-        ret += "K频段(0xDD)\n" if payload[1] == 0xDD else "S频段(0xDD)\n"
+        if payload[1] == 0xDD:
+            ret += f"K频段 (0x{payload[1]:02X})\n"
+        elif payload[1] == 0xEE:
+            ret += f"S频段 (0x{payload[1]:02X})\n"
+        else:
+            ret += f"【未知错误】(0x{payload[1]:02X})\n"
 
         ret += "自检结果: "
-        ret += "正常(0xAA)\n" if payload[2] == 0xAA else "异常(0x55 or other)\n"
+        if payload[2] == 0xAA:
+            ret += f"正常 (0x{payload[2]:02X})\n"
+        elif payload[2] == 0x55:
+            ret += f"异常 (0x{payload[2]:02X})\n"
+        else:
+            ret += f"【未知错误】(0x{payload[2]:02X})\n"
 
         hwOffset = 3
 
@@ -256,13 +291,15 @@ class Plugin(Plugin_Base):
         ret += f"MGC 3~4: {payload[hwOffset+10] & 0x0f:>2d} - {payload[hwOffset+10] >> 4:>2d}\n"
         ret += "--------------------------------\n"
 
-        return ret
+        return ret + '\n' + self.printFrame(data)
 
     def printFrame(self, data: bytes):
         ret = ''
-        for i in data:
-            ret += f'{i:02X} '
-        print(ret)
+        for i in range(len(data)):
+            ret += f'{data[i]:02X}'
+            ret += '\n' if i%16 == 15 else ' '
+        ret += '\n'
+        return ret
 
     def evalStatusCheckRsp(self, data: bytes):
         try:
@@ -271,15 +308,21 @@ class Plugin(Plugin_Base):
             return f"\n### evalStatusCheckRsp:: Failed to unpack data (data length {len(data)} byte).\n"
 
         ret = "----------- 遥测回令 -----------\n"
-        ret += "节点类型: "
-        ret += "K频段(0xDD)\n" if payload[1] == 0xDD else "S频段(0xDD)\n"
 
-        ret += f"时间戳: {payload[2]/1e2:.2f} us\n"
+        ret += "节点类型: "
+        if payload[1] == 0xDD:
+            ret += f"K频段 (0x{payload[1]:02X})\n"
+        elif payload[1] == 0xEE:
+            ret += f"S频段 (0x{payload[1]:02X})\n"
+        else:
+            ret += f"【未知错误】(0x{payload[1]:02X})\n"
+
+        ret += f"时间戳: {payload[2]/1e5:.2f} ms\n"
 
         sysbusOffset = 3
         linkOffset = 7
         phyOffset = 30
-        hwOffset = 43
+        hwOffset = 44
 
         ret += "\n  -- 总线状态\n"
         ret += f"配置装订:    {payload[sysbusOffset + 0]:04X}\n"
@@ -319,14 +362,14 @@ class Plugin(Plugin_Base):
         ret += f"通信波位号:  {payload[linkOffset + 6]:>2d}\n"
 
         ret += "通信波束宽度: "
-        if payload[linkOffset + 7] == 0x00:
-            ret += f"8° 窄波束 (0x{payload[linkOffset + 7]:02X})\n"
-        elif payload[linkOffset + 7] == 0x10:
-            ret += f"25° 次波束 (0x{payload[linkOffset + 7]:02X})\n"
-        elif payload[linkOffset + 7] == 0x20:
-            ret += f"60° 次波束 (0x{payload[linkOffset + 7]:02X})\n"
-        elif payload[linkOffset + 7] == 0x30:
+        if payload[linkOffset + 7] == 0x01:
             ret += f"120° 宽波束 (0x{payload[linkOffset + 7]:02X})\n"
+        elif payload[linkOffset + 7] == 0x02:
+            ret += f"60° 次波束 (0x{payload[linkOffset + 7]:02X})\n"
+        elif payload[linkOffset + 7] == 0x03:
+            ret += f"25° 次波束 (0x{payload[linkOffset + 7]:02X})\n"
+        elif payload[linkOffset + 7] == 0x04:
+            ret += f"8° 窄波束 (0x{payload[linkOffset + 7]:02X})\n"
         else:
             ret += f"【未知错误】(0x{payload[linkOffset + 7]:02X})\n"
 
@@ -358,6 +401,15 @@ class Plugin(Plugin_Base):
         ret += f"帧同步峰均比:   {payload[phyOffset + 9]:.2f}\n"
         ret += f"帧同步频偏通道: {payload[phyOffset + 10]:>3d}\n"
         ret += f"交织输入数据幅度统计:\t0x{payload[phyOffset + 11]:04X}\n"
+        ret += "天线工作模式: "
+        if payload[phyOffset + 12] == 0x01:
+            ret += f"静默模式 (0x{payload[phyOffset + 12]:02X})\n"
+        elif payload[phyOffset + 12] == 0x02:
+            ret += f"低功率模式 (0x{payload[phyOffset + 12]:02X})\n"
+        elif payload[phyOffset + 12] == 0x03:
+            ret += f"全功率模式 (0x{payload[phyOffset + 12]:02X})\n"
+        else:
+            ret += f"【未知错误】(0x{payload[phyOffset + 12]:02X})\n"
 
         ret += "\n  -- 硬件状态\n"
         ret += f"FPGA 电压监测: {bin(payload[hwOffset+0])[2:].zfill(8)}\n"
@@ -373,7 +425,7 @@ class Plugin(Plugin_Base):
         ret += f"MGC 3~4: {payload[hwOffset+10] & 0x0f:>2d} - {payload[hwOffset+10] >> 4:>2d}\n"
 
         ret += "--------------------------------\n"
-        return ret
+        return ret + '\n' + self.printFrame(data)
 
     def onSent(self, ok, msg, length, path):
         if ok:
@@ -385,4 +437,4 @@ class Plugin(Plugin_Base):
         data = item["data"]
         dateBytes = self.parseSendData(data, encoding = self.configGlobal["encoding"], isHexStr = True)
         if dateBytes:
-            self.send(dateBytes, callback=self.onSent)
+            self.send(dateBytes, callback=self.onSent)

df_datalink_ge/bsp/ps7_cortexa9_0/include/xparameters.h

@@ -359,20 +359,20 @@
 /* Definitions for driver GPIO */
 #define XPAR_XGPIO_NUM_INSTANCES 1
 
-/* Definitions for peripheral CONFIG_GPIO */
-#define XPAR_CONFIG_GPIO_BASEADDR 0x41200000
-#define XPAR_CONFIG_GPIO_HIGHADDR 0x4120FFFF
-#define XPAR_CONFIG_GPIO_DEVICE_ID 0
-#define XPAR_CONFIG_GPIO_INTERRUPT_PRESENT 0
-#define XPAR_CONFIG_GPIO_IS_DUAL 1
+/* Definitions for peripheral MPC_GPIO */
+#define XPAR_MPC_GPIO_BASEADDR 0x41200000
+#define XPAR_MPC_GPIO_HIGHADDR 0x4120FFFF
+#define XPAR_MPC_GPIO_DEVICE_ID 0
+#define XPAR_MPC_GPIO_INTERRUPT_PRESENT 0
+#define XPAR_MPC_GPIO_IS_DUAL 1
 
 
 /******************************************************************/
 
-/* Canonical definitions for peripheral CONFIG_GPIO */
+/* Canonical definitions for peripheral MPC_GPIO */
 #define XPAR_GPIO_0_BASEADDR 0x41200000
 #define XPAR_GPIO_0_HIGHADDR 0x4120FFFF
-#define XPAR_GPIO_0_DEVICE_ID XPAR_CONFIG_GPIO_DEVICE_ID
+#define XPAR_GPIO_0_DEVICE_ID XPAR_MPC_GPIO_DEVICE_ID
 #define XPAR_GPIO_0_INTERRUPT_PRESENT 0
 #define XPAR_GPIO_0_IS_DUAL 1
 
@@ -425,17 +425,14 @@
 
 /* Definitions for Fabric interrupts connected to ps7_scugic_0 */
 #define XPAR_FABRIC_PHY_IRQ_INTR 61U
-#define XPAR_FABRIC_FRAME_UART_0_RX_INTR_INTR 62U
-#define XPAR_FABRIC_FRAME_UART_0_TX_INTR_INTR 63U
-#define XPAR_FABRIC_MPC_IRQ_INTR 64U
-#define XPAR_FABRIC_PHY_5MS_INTR 65U
-#define XPAR_FABRIC_PHY_5MS_DELAY_2MS_INTR 66U
+#define XPAR_FABRIC_PHY_5MS_INTR 62U
+#define XPAR_FABRIC_PHY_5MS_DELAY_2MS_INTR 63U
+#define XPAR_FABRIC_MPC_RX_IRQ_INTR 64U
+#define XPAR_FABRIC_MPC_TX_IRQ_INTR 65U
 
 /******************************************************************/
 
 /* Canonical definitions for Fabric interrupts connected to ps7_scugic_0 */
-#define XPAR_FABRIC_FRAME_UART_0_RX_INTR_VEC_ID XPAR_FABRIC_FRAME_UART_0_RX_INTR_INTR
-#define XPAR_FABRIC_FRAME_UART_0_TX_INTR_VEC_ID XPAR_FABRIC_FRAME_UART_0_TX_INTR_INTR
 
 /******************************************************************/
 

df_datalink_ge/bsp/ps7_cortexa9_0/libsrc/gpio_v4_4/src/xgpio_g.c

@@ -43,10 +43,10 @@
 XGpio_Config XGpio_ConfigTable[XPAR_XGPIO_NUM_INSTANCES] =
 {
 	{
-		XPAR_CONFIG_GPIO_DEVICE_ID,
-		XPAR_CONFIG_GPIO_BASEADDR,
-		XPAR_CONFIG_GPIO_INTERRUPT_PRESENT,
-		XPAR_CONFIG_GPIO_IS_DUAL
+		XPAR_MPC_GPIO_DEVICE_ID,
+		XPAR_MPC_GPIO_BASEADDR,
+		XPAR_MPC_GPIO_INTERRUPT_PRESENT,
+		XPAR_MPC_GPIO_IS_DUAL
 	}
 };
 

df_datalink_ge/bsp/system.mss

@@ -18,18 +18,6 @@ BEGIN PROCESSOR
 END
 
 
-BEGIN DRIVER
- PARAMETER DRIVER_NAME = frame_uart
- PARAMETER DRIVER_VER = 1.0
- PARAMETER HW_INSTANCE = Frame_Uart_0
-END
-
-BEGIN DRIVER
- PARAMETER DRIVER_NAME = gpio
- PARAMETER DRIVER_VER = 4.4
- PARAMETER HW_INSTANCE = config_gpio
-END
-
 BEGIN DRIVER
  PARAMETER DRIVER_NAME = generic
  PARAMETER DRIVER_VER = 2.0
@@ -54,6 +42,12 @@ BEGIN DRIVER
  PARAMETER HW_INSTANCE = mpc_bram_interface_mpc_bram_ctrl
 END
 
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = gpio
+ PARAMETER DRIVER_VER = 4.4
+ PARAMETER HW_INSTANCE = mpc_gpio
+END
+
 BEGIN DRIVER
  PARAMETER DRIVER_NAME = bram
  PARAMETER DRIVER_VER = 4.3

df_datalink_ge/build/makefile

@@ -13,7 +13,7 @@ RM := rm -rf
 -include sources.mk
 -include src/util/subdir.mk
 -include src/time_ctrl/subdir.mk
--include src/systembus/subdir.mk
+-include src/mpc/subdir.mk
 -include src/networking/phy_layer/subdir.mk
 -include src/networking/link_layer/geometry/subdir.mk
 -include src/networking/link_layer/subdir.mk
@@ -38,7 +38,7 @@ endif
 
 -include ../makefile.defs
 
-# Add inputs and outputs from these tool invocations to the build variables 
+# Add inputs and outputs from these tool invocations to the build variables
 ELFSIZE += \
 df_datalink_ge.elf.size \
 

df_datalink_ge/build/src/mpc/subdir.mk

@@ -2,18 +2,18 @@
 # Automatically-generated file. Do not edit!
 ################################################################################
 
-# Add inputs and outputs from these tool invocations to the build variables 
+# Add inputs and outputs from these tool invocations to the build variables
 C_SRCS += \
-../src/systembus/systembus.c \
-../src/systembus/systembus_uart.c 
+../src/mpc/mpc.c \
+../src/mpc/mpc_hw.c
 
 OBJS += \
-./src/systembus/systembus.o \
-./src/systembus/systembus_uart.o 
+./src/mpc/mpc.o \
+./src/mpc/mpc_hw.o
 
 C_DEPS += \
-./src/systembus/systembus.d \
-./src/systembus/systembus_uart.d 
+./src/mpc/mpc.d \
+./src/mpc/mpc_hw.d
 
 
 # Each subdirectory must supply rules for building sources it contributes

df_datalink_ge/src/config/app_config.h

@@ -71,9 +71,10 @@
 #define TIME_CONTROLLER_BASEADDR XPAR_TMC_BASEADDR
 
 // System Bus Configs
-//#define SYSBUS_BASE_ADDR	XPAR_SYSTEMBUS_BASEADDR
-//#define SYSBUS_TX_INTR_ID	XPAR_FABRIC_SYSTEMBUS_TX_DONE_INTR
-//#define SYSBUS_RX_INTR_ID	XPAR_FABRIC_SYSTEMBUS_RX_DONE_INTR
+#define MPC_BRAM_ID	XPAR_MPC_BRAM_INTERFACE_MPC_BRAM_CTRL_DEVICE_ID
+#define MPC_GPIO_ID	XPAR_MPC_GPIO_DEVICE_ID
+#define MPC_TX_INTR_ID	XPAR_FABRIC_MPC_TX_IRQ_INTR
+#define MPC_RX_INTR_ID	XPAR_FABRIC_MPC_RX_IRQ_INTR
 
 // Global Timer config
 #define GLOBAL_TIMER_DEVICE_ID	XPAR_GLOBAL_TMR_DEVICE_ID

df_datalink_ge/src/dev_profile/timer_profile.c

@@ -11,7 +11,7 @@ struct xil_timer_init_param xil_scutimer_init_param = {
 struct timer_init_param scutimer_init_param = {
 	.id = SCUTIMER_DEVICE_ID,
 	.freq_hz = SCU_TIMER_CLK_HZ,
-	.ticks_count = (SCU_TIMER_CLK_HZ/10), //update log every 100ms
+	.ticks_count = (SCU_TIMER_CLK_HZ/2), //update log every 500ms
 	.platform_ops = &xil_timer_ops,
 	.extra = &xil_scutimer_init_param
 };

df_datalink_ge/src/main.c

@@ -36,7 +36,7 @@
 #include "timer.h"
 #include "config/app_config.h"
 #include "dev_profile/dev_profile.h"
-#include "systembus/systembus.h"
+#include "mpc/mpc.h"
 #include "networking/phy_layer/phy_layer.h"
 #include "networking/link_layer/link_layer.h"
 // RF-Driver
@@ -48,8 +48,8 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr);
 void Phy_IntrHandler(void *context);
 void PeriodicPull_Handler(void *callBackRef);
 void ScuTimer_IntrHandler(void *context);
-void Systembus_RxIntrHandler(void *context);
-void Systembus_TxIntrHandler(void *context);
+void Mpc_RxIntrHandler(void *context);
+void Mpc_TxIntrHandler(void *context);
 /************************** Constant Definitions *****************************/
 struct callback_desc phy_callback_desc =
 {
@@ -64,15 +64,15 @@ struct callback_desc periodicpull_callback_desc =
 };
 
 
-struct callback_desc sysbus_rx_callback_desc =
+struct callback_desc mpc_rx_callback_desc =
 {
-	.callback = Systembus_RxIntrHandler,
+	.callback = Mpc_RxIntrHandler,
 	.ctx = NULL
 };
 
-struct callback_desc sysbus_tx_callback_desc =
+struct callback_desc mpc_tx_callback_desc =
 {
-	.callback = Systembus_TxIntrHandler,
+	.callback = Mpc_TxIntrHandler,
 	.ctx = NULL
 };
 
@@ -126,15 +126,16 @@ TMC_CONFIG tmcConfig =
 	.reset = 10
 };
 
-//static SYSBUS_CONFIG sysbusConfig = {
-//	.baseAddr = SYSBUS_BASE_ADDR,
-//	.txQueueMax = 16,
-//	.rxQueueMax = 16
-//};
+static MPC_CONFIG mpcConfig = {
+	.bramId = MPC_BRAM_ID,
+	.gpioId = MPC_GPIO_ID,
+	.txQueueMax = 16,
+	.rxQueueMax = 16
+};
 /************************** Function Prototypes ******************************/
 static struct irq_ctrl_desc *intcDescPtr; // General interrupt controller descriptor
 static struct timer_desc *logTmPtr;
-static SYSBUS *sysbusDescPtr;
+static MPC *mpcDescPtr;
 static PHY_INST *phyInstPtr;
 static TMC_INST *tmcInstPtr = NULL;
 L2_INST *linkInstPtr;
@@ -213,64 +214,64 @@ int32_t SetupInterruptSystem(struct irq_ctrl_desc *descPtr)
 		return Status;
 	}
 
-//	/* Initialize system bus interrupt. */
-//	Status = irq_trigger_level_set(descPtr, SYSBUS_TX_INTR_ID, IRQ_EDGE_RISING);
-//	if (Status != SUCCESS)
-//	{
-//		DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %" PRId32 ".\n", Status);
-//		return Status;
-//	}
-//	// Set interrupt priority
-//	Status = irq_set_priority(descPtr, SYSBUS_TX_INTR_ID, 0x10);
-//	if (Status != SUCCESS)
-//	{
-//		DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %" PRId32 ".\n", Status);
-//		return Status;
-//	}
-//
-//	// Connect the interrupt service routine isr1 to the interrupt controller
-//	Status = irq_register_callback(descPtr, SYSBUS_TX_INTR_ID, &sysbus_tx_callback_desc);
-//	if (Status != SUCCESS)
-//	{
-//		DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %" PRId32 ".\n", Status);
-//		return Status;
-//	}
-//	Status = irq_trigger_level_set(descPtr, SYSBUS_RX_INTR_ID, IRQ_EDGE_RISING);
-//	if (Status != SUCCESS)
-//	{
-//		DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %" PRId32 ".\n", Status);
-//		return Status;
-//	}
-//	// Set interrupt priority
-//	Status = irq_set_priority(descPtr, SYSBUS_RX_INTR_ID, 0x18);
-//	if (Status != SUCCESS)
-//	{
-//		DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %" PRId32 ".\n", Status);
-//		return Status;
-//	}
-//
-//	// Connect the interrupt service routine isr1 to the interrupt controller
-//	Status = irq_register_callback(descPtr, SYSBUS_RX_INTR_ID, &sysbus_rx_callback_desc);
-//	if (Status != SUCCESS)
-//	{
-//		DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %" PRId32 ".\n", Status);
-//		return Status;
-//	}
-//
-//	/* Initialize Scu timer interrupt. */
-//	// Connect the interrupt service routine isr1 to the interrupt controller
-//	Status = irq_register_callback(descPtr, SCUTIMER_INTR_ID, &scutm_callback_desc);
-//	if (Status != SUCCESS)
-//	{
-//		DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %" PRId32 ".\n", Status);
-//		return Status;
-//	}
+	/* Initialize system bus interrupt. */
+	Status = irq_trigger_level_set(descPtr, MPC_TX_INTR_ID, IRQ_EDGE_RISING);
+	if (Status != SUCCESS)
+	{
+		DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %" PRId32 ".\n", Status);
+		return Status;
+	}
+	// Set interrupt priority
+	Status = irq_set_priority(descPtr, MPC_TX_INTR_ID, 0x10);
+	if (Status != SUCCESS)
+	{
+		DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %" PRId32 ".\n", Status);
+		return Status;
+	}
+
+	// Connect the interrupt service routine isr1 to the interrupt controller
+	Status = irq_register_callback(descPtr, MPC_TX_INTR_ID, &mpc_tx_callback_desc);
+	if (Status != SUCCESS)
+	{
+		DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %" PRId32 ".\n", Status);
+		return Status;
+	}
+	Status = irq_trigger_level_set(descPtr, MPC_RX_INTR_ID, IRQ_EDGE_RISING);
+	if (Status != SUCCESS)
+	{
+		DBG_ERROR("PHY_INTR trigger level set FAILED, ErrorCode: %" PRId32 ".\n", Status);
+		return Status;
+	}
+	// Set interrupt priority
+	Status = irq_set_priority(descPtr, MPC_RX_INTR_ID, 0x18);
+	if (Status != SUCCESS)
+	{
+		DBG_ERROR("PHY_INTR priority level set FAILED, ErrorCode: %" PRId32 ".\n", Status);
+		return Status;
+	}
+
+	// Connect the interrupt service routine isr1 to the interrupt controller
+	Status = irq_register_callback(descPtr, MPC_RX_INTR_ID, &mpc_rx_callback_desc);
+	if (Status != SUCCESS)
+	{
+		DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %" PRId32 ".\n", Status);
+		return Status;
+	}
+
+	/* Initialize Scu timer interrupt. */
+	// Connect the interrupt service routine isr1 to the interrupt controller
+	Status = irq_register_callback(descPtr, SCUTIMER_INTR_ID, &scutm_callback_desc);
+	if (Status != SUCCESS)
+	{
+		DBG_ERROR("PHY_INTR register callback FAILED, ErrorCode: %" PRId32 ".\n", Status);
+		return Status;
+	}
 
 	// Enable interrupts
 	irq_enable(descPtr, PHY_INTR_ID);
 	irq_enable(descPtr, PERIODIC_5MS_INTR_ID);
-//	irq_enable(descPtr, SYSBUS_RX_INTR_ID);
-//	irq_enable(descPtr, SYSBUS_TX_INTR_ID);
+	irq_enable(descPtr, MPC_RX_INTR_ID);
+	irq_enable(descPtr, MPC_TX_INTR_ID);
 	irq_enable(descPtr, SCUTIMER_INTR_ID);
 
 	// Enable Exceptions
@@ -328,13 +329,13 @@ void PeriodicPull_Handler(void *callBackRef)
  * @return  None.
  *
  ******************************************************************************/
-void Systembus_RxIntrHandler(void *context)
+void Mpc_RxIntrHandler(void *context)
 {
 	int32_t Status;
-	Status = SystemBus_RxIntrHandler(sysbusDescPtr);
+	Status = Mpc_RxIrqHandler(mpcDescPtr);
 	if (Status != SUCCESS)
 	{
-		DBG_ERROR("SystemBus_RxIntrHandler exit with error code %" PRId32 ",\n", Status);
+		DBG_ERROR("Mpc_RxIrqHandler exit with error code %" PRId32 ",\n", Status);
 	}
 }
 
@@ -347,13 +348,13 @@ void Systembus_RxIntrHandler(void *context)
  * @return  None.
  *
  ******************************************************************************/
-void Systembus_TxIntrHandler(void *context)
+void Mpc_TxIntrHandler(void *context)
 {
 	int32_t Status;
-	Status = SystemBus_TxIntrHandler(sysbusDescPtr);
+	Status = Mpc_TxIrqHandler(mpcDescPtr);
 	if (Status != SUCCESS)
 	{
-		DBG_ERROR("SystemBus_TxIntrHandler exit with error code %" PRId32 ",\n", Status);
+		DBG_ERROR("Mpc_TxIrqHandler exit with error code %" PRId32 ",\n", Status);
 	}
 }
 
@@ -374,6 +375,15 @@ void ScuTimer_IntrHandler(void *context)
 	{
 		// Refresh log
 		Phy_RefreshHwSate(phyInstPtr);
+
+		if (Mpc_IsEnable(mpcDescPtr))
+		{
+			if (Mpc_PendRemoteTest(mpcDescPtr) != SUCCESS)
+			{
+				DBG_ERROR("Failed to apply remote test.\n");
+			}
+		}
+
 		XScuTimer_ClearInterruptStatus((XScuTimer *)xdesc->instance);
 	}
 	else
@@ -458,13 +468,13 @@ int main(void)
 		return Status;
 	}
 
-	// /* System Bus Initialize */
-	// Status = SystemBus_Init(&sysbusDescPtr, &sysbusConfig);
-	// if (Status != SUCCESS)
-	// {
-	// 	DBG_ERROR("Systembus init FAILED, with error code: %" PRId32 "\n", Status);
-	// 	return Status;
-	// }
+	/* System Bus Initialize */
+	Status = Mpc_Init(&mpcDescPtr, &mpcConfig);
+	if (Status != SUCCESS)
+	{
+	DBG_ERROR("MPC init FAILED, with error code: %" PRId32 "\n", Status);
+	return Status;
+	}
 
 	/* System Monitor log timer Initialize */
 	Status = timer_init(&logTmPtr, &scutimer_init_param);
@@ -492,30 +502,24 @@ int main(void)
 	DBG_PRINT("Physical started...\n");
 	Phy_Start(phyInstPtr);
 
-	Linklayer_Start(linkInstPtr);
-
-	while (true);
-
-	// while (true)
-	// {
-	// 	// System bus handler
-	// 	if (SystemBus_IsReqPendding(sysbusDescPtr))
-	// 	{
-	// 		Status = SystemBus_HandleCmd(sysbusDescPtr);
-	// 		if (Status != SUCCESS)
-	// 		{
-	// 			DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %" PRId32 ".\n", Status);
-	// 		}
-	// 	}
-	// 	if (SystemBus_IsRsqPendding(sysbusDescPtr) && (!SystemBus_IsTxBusy(sysbusDescPtr)))
-	// 	{
-	// 		// FIXME: Required by 12 (CASC)
-	// 		udelay(200);
-	// 		Status = SystemBus_MakeResp(sysbusDescPtr);
-	// 		if (Status != SUCCESS)
-	// 		{
-	// 			DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %" PRId32 ".\n", Status);
-	// 		}
-	// 	}
-	// }
+	while (true)
+	{
+	// System bus handler
+	if (Mpc_IsReqPending(mpcDescPtr))
+	{
+		Status = Mpc_HandleCmd(mpcDescPtr);
+		if (Status != SUCCESS)
+		{
+			DBG_ERROR("Mpc_HandleCmd exit with ErrorCode: %" PRId32 ".\n", Status);
+		}
+	}
+	if (Mpc_IsRsqPending(mpcDescPtr) && (!Mpc_IsTxBusy(mpcDescPtr)))
+	{
+		Status = Mpc_MakeResp(mpcDescPtr);
+		if (Status != SUCCESS)
+		{
+			DBG_ERROR("Mpc_HandleCmd exit with ErrorCode: %" PRId32 ".\n", Status);
+		}
+	}
+	}
 } // main

df_datalink_ge/src/mpc/mpc.c

@@ -0,0 +1,547 @@
+/*****************************************************************************/
+/**
+ * Description: MPC protocol implementation file.
+ *
+ * Date: 2023-05-06 10:13
+ * Last Edit Time: [2023-05-06 21:45]
+ *
+ * Revisions: Rev 0.0
+ * History:
+ * [2023-05-06]: Initial version.
+ * [2023-09-13]: Update to 485 protocol-20220816
+ *
+ * Copyright(c) 2021 Beijing Institute of Technology.
+ * Lab of Communication and Network
+ *
+ ******************************************************************************/
+
+/***************************** Include Files *********************************/
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include "mpc.h"
+#include "util.h"
+#include "common.h"
+#include "error.h"
+#include "circ_queue.h"
+#include "xbram.h"
+#include "xgpio.h"
+
+#include "../system_monitor.h"
+
+/************************** Constant Definitions *****************************/
+// Common Field
+// -- Command word
+#define WD_CMD 0x00
+
+// -- Production type
+#define WD_PRD 0x01
+#define PRD_DL_K 0xDD
+#define PRD_DL_S 0xEE
+#define PRD_ALL 0xFF
+
+// Self-Test
+#define CMD_SELF_TEST 0xE1
+// -- Request
+// -- -- --
+// -- Response
+// -- -- Self-Test Result
+#define ST_RSP_LEN 16
+#define WD_ST_RET 0x02
+#define ST_RET_OK 0xAA
+#define ST_RET_ERROR 0x55
+#define ST_RET_INFO_LEN 13
+// -- -- Signal processor PS voltage
+#define WD_SP_FPGA_VOLT 0x03
+// -- -- Signal processor PS AUX voltage
+#define WD_SP_CLK_STATE 0x04
+// -- -- Signal processor DDR voltage
+#define WD_SP_AD_LOCK_STATE 0x05
+// -- -- Signal processor PL voltage
+#define WD_SP_AD_TEMP_0 0x06
+// -- -- Signal processor PL AUX voltage
+#define WD_SP_AD_TEMP_1 0x07
+// -- -- Signal processor Bram voltage
+#define WD_SP_FPGA_TEMP 0x08
+
+// Config load
+#define CMD_CONFIG 0xE3
+// -- Request
+#define WD_EN_CMD 0x02
+#define EN_CMD_ON_HIGH_POWER 0x66
+#define EN_CMD_ON_LOW_POWER 0x77
+#define EN_CMD_SILENT 0x88
+
+#define WD_MODE_CMD 0x03
+#define MODE_CMD_HEAD 0x11
+#define MODE_CMD_MEMBER 0x00
+// -- Response
+// -- -- none response
+
+// Remote Test
+#define RM_TEST_RESP_LEN 158
+// -- Request
+// Remote Test FLJ
+#define CMD_FLJ_RM_TEST 0xE6
+// Remote Test LJ
+#define CMD_LJ_RM_TEST 0xF6
+#define WD_RM_TEST 0x02
+#define RM_TEST_CODE 0x99
+// -- Response
+#define WD_TIME_STAMP 0x02
+#define WD_TIME_STAMP_LEN 8
+#define WD_SYSBUS_LOG 0x0A
+#define WD_SYSBUS_LOG_LEN 14
+#define WD_LINK_LOG 0x18
+#define WD_LINK_LOG_LEN 64
+#define WD_PHY_LOG 0x58
+#define WD_PHY_LOG_LEN 57
+#define WD_HARDWARE_ST 0x91
+#define WD_HARDWARE_ST_LEN 13
+
+// My Production Type
+#define MY_PRD_ID PRD_DL_K
+
+/************************** Function Prototypes ******************************/
+/**************************** Type Definitions *******************************/
+/************************** Variable Definitions *****************************/
+// TODO: better approach?
+#include "../networking/link_layer/link_layer.h"
+#include "../networking/phy_layer/phy_layer.h"
+extern L2_INST *linkInstPtr;
+extern L2_CONFIG linkConfig;
+
+inline bool Mpc_IsEnable(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	return (instPtr->isEnable);
+}
+
+inline bool Mpc_IsReqPending(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	return (!isCircQueueEmpty(instPtr->rxQueue));
+}
+
+inline bool Mpc_IsRsqPending(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	return (!isCircQueueEmpty(instPtr->txQueue));
+}
+
+inline bool Mpc_IsTxBusy(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	return (!instPtr->isTxDone);
+}
+
+inline bool Mpc_IsRxBusy(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	return (!instPtr->isRxDone);
+}
+
+/**
+ * @brief   Generate System Bus SelfTest response.
+ */
+static int32_t Mpc_GenSelfTestRsp(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	MPC_HW_FRAME *mpcRsp;
+	uint8_t *msg;
+	if (instPtr->isEnable == false)
+	{
+		DBG_ERROR("System is not enabled!\n");
+		return EIGNORE;
+	}
+	mpcRsp = malloc(sizeof(MPC_HW_FRAME));
+	if (mpcRsp == NULL) {
+		DBG_ERROR("Failed to allocate memory for LJ REMOTE TEST Response!\n");
+		return ENOMEM;
+	}
+	mpcRsp->len = ST_RSP_LEN;
+	msg = mpcRsp->data + FRAME_PAYLOAD_POFFSET;
+
+	HWSTATE *hwStatsPtr = &(instPtr->hwState);
+
+	msg[WD_CMD] = CMD_SELF_TEST;
+	msg[WD_PRD] = MY_PRD_ID;
+	SysMonitor_GetHardwareStatus(hwStatsPtr);
+	if (linkInstPtr->isEnable == false)
+	{
+		msg[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR;
+	}
+	else
+	{
+		msg[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR;
+	}
+	// apply status and omit 2 bytes of overhead.
+	memcpy(msg + WD_SP_FPGA_VOLT, &(hwStatsPtr->spFpgaVolt), ST_RET_INFO_LEN);
+
+	if (CircQueuePush(instPtr->txQueue, (void *)mpcRsp) == false)
+	{
+		free(mpcRsp);
+		DBG_ERROR("SystemBus::TX Queue overflowed!\n");
+		return EQUEUE;
+	}
+	return SUCCESS;
+}
+
+/**
+ * @brief   Apply system configuration
+ */
+static int32_t Mpc_SystemConfig(MPC *instPtr, uint8_t *msg)
+{
+	PHY_INST *phyRef = linkInstPtr->config.phyInstPtr;
+	TMC_INST *tmcRef = linkInstPtr->config.tmcInstPtr;
+	bool reconfig = false;
+
+	if (msg[WD_MODE_CMD] == MODE_CMD_HEAD)
+	{
+		// omit unnecessary reconfiguration
+		if (L2_GetNodeType(linkInstPtr) != L2_NODE_HEAD)
+		{
+			// head node config
+			linkConfig.myType = L2_NODE_HEAD;
+			linkConfig.myId = 1;
+			reconfig = true;
+		}
+	}
+
+	else if (msg[WD_MODE_CMD] == MODE_CMD_MEMBER )
+	{
+		// omit unnecessary reconfiguration
+		if (L2_GetNodeType(linkInstPtr) != L2_NODE_MEMBER)
+		{
+			// member node config
+			linkConfig.myType = L2_NODE_MEMBER;
+			linkConfig.myId = 2;
+			reconfig = true;
+		}
+	}
+	else
+	{
+		// Ignore current command
+		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
+			WD_MODE_CMD, msg[WD_MODE_CMD]);
+		return EPARAM;
+	}
+
+	if(msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
+	{
+		// high power config
+		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL);
+		instPtr->isEnable = true;
+	}
+	else if(msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
+	{
+		// low power config
+		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
+		instPtr->isEnable = true;
+	}
+	else if(msg[WD_EN_CMD] == EN_CMD_SILENT)
+	{
+		// silence config
+		// TODO: better approach?
+		instPtr->isEnable = false;
+		Linklayer_Halt(linkInstPtr);
+		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY);
+		TIME_WORD pullTime = {0, 0};
+		pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1;
+		if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS)
+		{
+			DBG_ERROR("Physical set pull time failed.\n");
+		}
+		Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
+	}
+	if (reconfig == true)
+	{
+		Linklayer_Reconfig(linkInstPtr, &linkConfig);
+	}
+	if (instPtr->isEnable == true)
+	{
+		Linklayer_Start(linkInstPtr);
+	}
+	instPtr->mpcLog.lastConfig[0] = msg[WD_MODE_CMD];
+	instPtr->mpcLog.lastConfig[1] = msg[WD_EN_CMD];
+	return SUCCESS;
+}
+
+
+/*****************************************************************************/
+/**
+ * @brief   Initialize MPC instance.
+ *
+ * @param   InstPtr A double pointer to MPC instance.
+ * @param   config  A pointer to MPC configurations.
+ *
+ * @return  None.
+ *
+ ******************************************************************************/
+int32_t Mpc_Init(MPC **instPtr, MPC_CONFIG *config)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(config != NULL);
+	MPC_HW_CONFIG mpcHwConfig;
+	int32_t Status;
+	(*instPtr) = malloc(sizeof(MPC));
+	if ((*instPtr) == NULL)
+	{
+		return FAILURE;
+	}
+	(*instPtr)->rxQueue = NULL;
+	(*instPtr)->txQueue = NULL;
+	(*instPtr)->mpchwDesc = NULL;
+	memcpy(&((*instPtr)->config), config, sizeof(MPC_CONFIG));
+	memset(&((*instPtr)->mpcLog), 0x00, sizeof(SYSBUSLOG));
+	memset(&((*instPtr)->linkLog), 0x00, sizeof(LINKLOG));
+	memset(&((*instPtr)->phyLog), 0x00, sizeof(PHYLOG));
+	memset(&((*instPtr)->hwState), 0x00, sizeof(HWSTATE));
+
+	XBram_Config *bramConfigPtr;
+	XBram mpcBram;
+
+	bramConfigPtr = XBram_LookupConfig(config->bramId);
+	if (bramConfigPtr == NULL)
+	{
+		Status = EDCONF;
+		goto mpc_init_error;
+	}
+	Status = XBram_CfgInitialize(&mpcBram, bramConfigPtr,
+								 bramConfigPtr->CtrlBaseAddress);
+	if (Status != SUCCESS)
+	{
+		goto mpc_init_error;
+	}
+	/*
+	 * Execute the BRAM driver selftest.
+	 */
+	Status = XBram_SelfTest(&mpcBram, 0);
+	if (Status != SUCCESS)
+	{
+		goto mpc_init_error;
+	}
+
+	mpcHwConfig.txBuffAddr = mpcBram.Config.MemBaseAddress;
+	mpcHwConfig.rxBuffAddr = mpcBram.Config.MemBaseAddress;
+	mpcHwConfig.gpioId = config->gpioId;
+
+	Status = MpcHw_Init(&((*instPtr)->mpchwDesc), &mpcHwConfig);
+	if (Status != SUCCESS)
+	{
+		Status = ENOMEM;
+		goto mpc_init_error;
+	}
+	if (CircQueueCreate(&((*instPtr)->txQueue), (*instPtr)->config.txQueueMax) == false)
+	{
+		Status = ENOMEM;
+		goto mpc_init_error;
+	}
+	if (CircQueueCreate(&((*instPtr)->rxQueue), (*instPtr)->config.rxQueueMax) == false)
+	{
+		Status = ENOMEM;
+		goto mpc_init_error;
+	}
+	(*instPtr)->isEnable = false; // nothing received.
+	(*instPtr)->isRxDone = false; // nothing received.
+	(*instPtr)->isTxDone = true;  // nothing to be sent.
+	return SUCCESS;
+
+mpc_init_error:
+	free((*instPtr)->mpchwDesc);
+	free((*instPtr)->txQueue);
+	free((*instPtr)->rxQueue);
+	free(*instPtr);
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+ * @brief	Generate a remote test response message.
+ *
+ * @param   instPtr A pointer to MPC instance.
+ *
+ * @return  SUCCESS
+ * 			error code
+ *
+ ******************************************************************************/
+int32_t Mpc_PendRemoteTest(MPC *instPtr)
+{
+	MPC_HW_FRAME *mpcRsp;
+	uint8_t *msg;
+	if (instPtr->isEnable == false)
+	{
+		DBG_ERROR("System is not enabled!\n");
+		return EIGNORE;
+	}
+	mpcRsp = malloc(sizeof(MPC_HW_FRAME));
+	if (mpcRsp == NULL) {
+		DBG_ERROR("Failed to allocate memory for LJ REMOTE TEST Response!\n");
+		return ENOMEM;
+	}
+	msg = mpcRsp->data + FRAME_PAYLOAD_POFFSET;
+	mpcRsp->len = RM_TEST_RESP_LEN;
+	if (L2_GetNodeType(linkInstPtr) == L2_NODE_HEAD)
+	{
+		// head node config
+		msg[WD_CMD] = CMD_FLJ_RM_TEST;
+	}
+	else
+	{
+		msg[WD_CMD] = CMD_LJ_RM_TEST;
+	}
+	msg[WD_PRD] = MY_PRD_ID;
+
+	TIME_WORD tmWord;
+	uint64_t time;
+	TimeCtrl_GetCurrentTime(linkInstPtr->config.tmcInstPtr, &tmWord);
+	time = (uint64_t)tmWord.int_part * TIME_FRAC_MAX + tmWord.frac_part;
+	memcpy(msg + WD_TIME_STAMP, &time, WD_TIME_STAMP_LEN);
+	SysMonitor_GetLinklayerLog(&(instPtr->linkLog));
+	SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog));
+	SysMonitor_GetHardwareStatus(&(instPtr->hwState));
+	memcpy(msg + WD_SYSBUS_LOG, &(instPtr->mpcLog), WD_SYSBUS_LOG_LEN);
+	memcpy(msg + WD_LINK_LOG, &(instPtr->linkLog), WD_LINK_LOG_LEN);
+	memcpy(msg + WD_PHY_LOG, &(instPtr->phyLog), WD_PHY_LOG_LEN);
+	memcpy(msg + WD_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_HARDWARE_ST_LEN);
+
+	if (CircQueuePush(instPtr->txQueue, (void *)mpcRsp) == false){
+		free(mpcRsp);
+		DBG_WARNING("SystemBus::TX Queue overflowed!\n");
+		return EQUEUE;
+	}
+	instPtr->mpcLog.remoteCnt++;
+	return SUCCESS;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   A handler function. This handler  evaluate the message from system
+ * 			bus receive queue, and make appropriate response.
+ *
+ * @param   instPtr A pointer to MPC instance.
+ *
+ * @return  None
+ *
+ ******************************************************************************/
+int32_t Mpc_HandleCmd(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	int32_t Status;
+	MPC_HW_FRAME *mpcReq;
+	uint8_t *reqMsg;
+	if (CircQueuePop(instPtr->rxQueue, (void **)(&mpcReq)) == false)
+	{
+		DBG_WARNING("SystemBus::Nothing to be handled.\n");
+		return EIGNORE;
+	}
+	reqMsg = mpcReq->data + FRAME_PAYLOAD_POFFSET;
+	if (reqMsg[WD_PRD] != MY_PRD_ID && reqMsg[WD_PRD] != PRD_ALL)
+	{
+		DBG_WARNING("SystemBus::Production code mismatch!\n");
+		return EIGNORE;
+	}
+
+	switch (reqMsg[WD_CMD])
+	{
+	case CMD_CONFIG:
+	{
+		Status = Mpc_SystemConfig(instPtr, reqMsg);
+		if (Status == SUCCESS)
+		{
+			Status = Mpc_GenSelfTestRsp(instPtr);
+		}
+		break;
+	}
+	default:
+	{
+		DBG_ERROR("SystemBus::Unknown command code!\n");
+		Status = FAILURE;
+		break;
+	}
+	}
+	free(mpcReq);
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   This function send a repose frame from MPC tx queue.
+ *
+ * @param   instPtr A pointer to MPC instance.
+ *
+ * @return  None
+ *
+ ******************************************************************************/
+int32_t Mpc_MakeResp(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	int32_t Status;
+	MPC_HW_FRAME *mpcRsp;
+	if (CircQueuePop(instPtr->txQueue, (void **)(&mpcRsp)) == false)
+	{
+		DBG_WARNING("SystemBus::Nothing to be sent.\n");
+		return EIGNORE;
+	}
+	instPtr->isTxDone = false; // A transmission procedure is started.
+	Status = MpcHw_Send(instPtr->mpchwDesc, mpcRsp);
+	free(mpcRsp);
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   This function handles the mpc tx irq events.
+ *
+ * @param   instPtr A pointer to MPC instance.
+ *
+ * @return  None
+ *
+ ******************************************************************************/
+int32_t Mpc_TxIrqHandler(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	// nothing special...
+	instPtr->isTxDone = true; // A transmission procedure finished.
+	return SUCCESS;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   This function handles the mpc rx irq events.
+ *
+ * @param   instPtr A pointer to MPC instance.
+ *
+ * @return  None
+ *
+ ******************************************************************************/
+int32_t Mpc_RxIrqHandler(MPC *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	int32_t Status;
+	MPC_HW_FRAME *mpcReq;
+	instPtr->isRxDone = true; // whether reception successes or not, receiving procedure ended.
+	mpcReq = malloc(sizeof(MPC_HW_FRAME));
+	if (mpcReq == NULL)
+	{
+		return ENOMEM;
+	}
+	Status = MpcHw_Recv(instPtr->mpchwDesc, mpcReq);
+	if (Status != SUCCESS)
+	{
+		goto recv_error;
+	}
+	if (CircQueuePush(instPtr->rxQueue, (void *)mpcReq) == false)
+	{
+		DBG_ERROR("SystemBus::RX Queue overflowed!\n");
+		Status = EQUEUE;
+		goto recv_error;
+	}
+	instPtr->isRxDone = false; // another reception is undergoing.
+	return SUCCESS;
+recv_error:
+	free(mpcReq);
+	return Status;
+}

df_datalink_ge/src/mpc/mpc.h

@@ -0,0 +1,71 @@
+/*****************************************************************************/
+/**
+ * Description: MPC protocol head file.
+ *
+ * Date: 2023-05-06 10:12
+ * Last Edit Time: [2023-05-06 10:13]
+ *
+ * Revisions: Rev 0.0
+ * History:
+ * [2023-05-06]: Initial version.
+ *
+ * Copyright(c) 2021 Beijing Institute of Technology.
+ *
+ ******************************************************************************/
+
+#ifndef __MPC_H_
+#define __MPC_H_
+
+/***************************** Include Files *********************************/
+#include <stdint.h>
+#include <stdbool.h>
+#include "mpc_hw.h" // MPC driver
+#include "circ_queue.h"
+#include "../system_monitor.h"
+
+/************************** Constant Definitions *****************************/
+/************************** Data Type Definitions ****************************/
+// Definition of MPC instance.
+typedef struct
+{
+	uint32_t bramId;
+	uint32_t gpioId;
+    uint32_t txQueueMax;
+    uint32_t rxQueueMax;
+} MPC_CONFIG;
+
+typedef struct
+{
+    MPC_CONFIG config;
+    SYSBUSLOG mpcLog; // mpc log
+	LINKLOG linkLog; // Link layer log
+	PHYLOG phyLog; // Physical layer log
+	HWSTATE hwState; // Hardware status
+    MPC_HW *mpchwDesc;
+    CIRC_QUEUE *txQueue;
+    CIRC_QUEUE *rxQueue;
+    bool isEnable;
+    bool isRxDone;
+    bool isTxDone;
+} MPC;
+
+/************************** Function Prototypes ******************************/
+
+int32_t Mpc_Init(MPC **instPtr, MPC_CONFIG *config);
+
+int32_t Mpc_HandleCmd(MPC *instPtr);
+int32_t Mpc_PendRemoteTest(MPC *instPtr);
+int32_t Mpc_MakeResp(MPC *instPtr);
+
+int32_t Mpc_TxIrqHandler(MPC *instPtr);
+int32_t Mpc_RxIrqHandler(MPC *instPtr);
+
+bool Mpc_IsEnable(MPC *instPtr);
+
+bool Mpc_IsReqPending(MPC *instPtr);
+bool Mpc_IsRsqPending(MPC *instPtr);
+
+bool Mpc_IsTxBusy(MPC *instPtr);
+bool Mpc_IsRxBusy(MPC *instPtr);
+
+#endif //__MPC_H_

df_datalink_ge/src/mpc/mpc_hw.c

@@ -0,0 +1,139 @@
+/*****************************************************************************/
+/**
+ * Description: MPC uart driver implementation file.
+ *
+ * Date: 2023-05-06 10:59
+ * LastEditTime: 2023-05-06 21:56
+ * Revisions: Rev 0.0
+ * History:
+ * [2023-05-06]: Initial version.
+ *
+ * Copyright(c) 2022 Beijing Institute of Technology.
+ *
+ ******************************************************************************/
+
+/***************************** Include Files *******************************/
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include "mpc_hw.h"
+#include "common.h"
+
+// Control and status register
+#define MODULE_VERSION 0
+#define MPC_STATUS 4
+#define MPC_SEND_CMD 8
+#define MPC_RECV_CNT 12
+
+// Status code
+#define MPC_STATUS_TX_BUSY 0x00010000
+#define MPC_STATUS_RX_BUSY 0x01000000
+// Data buffer register
+#define MPC_TXBUFF_MAX 512
+#define MPC_RXBUFF_MAX 512
+
+// Frame Data offsets
+#define FRAME_HEAD_LOW 0x02
+#define FRAME_HEAD_HIGH 0x53
+#define FRAME_TAIL_LOW 0x32
+#define FRAME_TAIL_HIGH 0x20
+
+#define FRAME_HEAD_LEN 2
+#define FRAME_TAIL_LEN 2
+#define FRAME_DUMMY_LEN 1	 // Dummy
+#define FRAME_OVERHEAD_LEN 2 // CheckSum, Dummy and
+
+#define MPC_STATUS_CH 1
+#define MPC_CONFIG_CH 2
+
+#define MPC_CONFIG_SEND_TRIG 0x00000002
+/*************************** Function Definitions ****************************/
+
+/*****************************************************************************/
+/**
+ * @brief   Instance initial
+ *
+ * @param   instPtr     A pointer to frame uart instance structure.
+ * @param   config    Base address of frame uart, refer to "xparameters.h"
+ *
+ * @return  None.
+ *
+ ******************************************************************************/
+int MpcHw_Init(MPC_HW **instPtr, MPC_HW_CONFIG *config)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(config != NULL);
+	uint32_t Status;
+	(*instPtr) = malloc(sizeof(MPC_HW));
+	if ((*instPtr) == NULL)
+	{
+		return FAILURE;
+	}
+	(*instPtr)->config.txBuffAddr = config->txBuffAddr;
+	(*instPtr)->config.rxBuffAddr = config->rxBuffAddr;
+	Status = XGpio_Initialize(&((*instPtr)->gpioDesc), config->gpioId);
+	if (Status != SUCCESS) {
+		DBG_ERROR("MPC gpio Initialization Failed.\n");
+		free(*instPtr);
+		return FAILURE;
+	}
+	XGpio_SetDataDirection(&((*instPtr)->gpioDesc), MPC_CONFIG_CH, 0x00000000);
+	XGpio_SetDataDirection(&((*instPtr)->gpioDesc), MPC_STATUS_CH, 0xFFFFFFFF);
+	return SUCCESS;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Send a MPC frame.
+ *
+ * @param   instPtr	A pointer to MPC_HW instance structure.
+ * @param   buff	A pointer to send data buffer
+ * @param   dLen	A integer indicate the send data buffer length in byte.
+ *
+ * @return
+ *
+ ******************************************************************************/
+int MpcHw_Send(MPC_HW *instPtr, MPC_HW_FRAME *frame)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(frame != NULL);
+	uint8_t sendLen;
+	uint8_t *sendBuff = (uint8_t*) instPtr->config.txBuffAddr;
+	sendLen = frame->len + FRAME_OVERHEAD_LEN;
+	if (sendLen > MPC_TXBUFF_MAX || frame->len == 0)
+	{
+		return FAILURE;
+	}
+	frame->data[0] = FRAME_HEAD_LOW;
+	frame->data[1] = FRAME_HEAD_HIGH;
+	memcpy(sendBuff, frame->data, sendLen);
+
+	// Start Sending
+	uint32_t gpioVal  = XGpio_DiscreteRead(&(instPtr->gpioDesc), MPC_CONFIG_CH);
+	gpioVal ^= MPC_CONFIG_SEND_TRIG;
+	XGpio_DiscreteWrite(&(instPtr->gpioDesc), MPC_CONFIG_CH, gpioVal);
+	return SUCCESS;
+}
+
+/*****************************************************************************/
+/**
+ * @brief   Get last MPC frame.
+ *
+ * @param   instPtr     A pointer to MPC_HW instance structure.
+ * @param   buff    A pointer to receive data buffer.
+ *
+ * @return
+ *
+ ******************************************************************************/
+int MpcHw_Recv(MPC_HW *instPtr, MPC_HW_FRAME *frame)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(frame != NULL);
+	uint8_t *recvBuff = (uint8_t *)(instPtr->config.rxBuffAddr);
+	memcpy(frame->data, recvBuff, MPC_FRAME_MAX);
+	frame->len = MPC_FRAME_MAX;
+	frame->isValid = true;
+	return SUCCESS;
+}
+

df_datalink_ge/src/mpc/mpc_hw.h

@@ -0,0 +1,98 @@
+/*****************************************************************************/
+/**
+ * Description: MPC uart driver implementation file.
+ *
+ * Date: 2023-05-06 10:59
+ * LastEditTime: 2023-05-06 21:48
+ * Revisions: Rev 0.0
+ * History:
+ * [2023-05-06]: Initial version.
+ *
+ * Copyright(c) 2022 Beijing Institute of Technology.
+ *
+ ******************************************************************************/
+
+#ifndef __MPC_HW_H
+#define __MPC_HW_H
+
+/**************************** Include Files *****************************/
+#include <stdint.h>
+#include <stdbool.h>
+#include "util.h"
+#include "xgpio.h"
+
+#define MPC_FRAME_MAX 256
+#define FRAME_PAYLOAD_POFFSET 2 // CheckSum, Dummy and DataLen
+/**************************** Type Definitions *****************************/
+/**
+ *
+ * Write a value to a SYSTEMBUS_UART register. A 32 bit write is performed.
+ * If the component is implemented in a smaller width, only the least
+ * significant data is written.
+ *
+ * @param   baseAddr is the base address of the SYSTEMBUS_UARTdevice.
+ * @param   offset is the register offset from the base to write to.
+ * @param   data is the data written to the register.
+ *
+ * @return  None.
+ *
+ * @note
+ * C-style signature:
+ * 	void MPC_HW_mWriteReg(uint32_t BaseAddress, unsigned RegOffset, uint32_t Data)
+ *
+ */
+#define MPC_HW_mWriteReg(baseAddr, offset, data) \
+	IO_Out32((baseAddr) + (offset), (uint32_t)(data))
+
+/**
+ *
+ * Read a value from a SYSTEMBUS_UART register. A 32 bit read is performed.
+ * If the component is implemented in a smaller width, only the least
+ * significant data is read from the register. The most significant data
+ * will be read as 0.
+ *
+ * @param   baseAddr is the base address of the SYSTEMBUS_UART device.
+ * @param   offset is the register offset from the base to write to.
+ *
+ * @return  Data is the data from the register.
+ *
+ * @note
+ * C-style signature:
+ * 	uint32_t MPC_HW_mReadReg(uint32_t BaseAddress, unsigned RegOffset)
+ *
+ */
+#define MPC_HW_mReadReg(baseAddr, offset) \
+	IO_In32((baseAddr) + (offset))
+
+typedef struct
+{
+	uintptr_t txBuffAddr;
+	uintptr_t rxBuffAddr;
+	uint32_t gpioId;
+} MPC_HW_CONFIG;
+
+typedef struct
+{
+	MPC_HW_CONFIG config;
+	XGpio gpioDesc;
+} MPC_HW;
+
+typedef struct
+{
+	uint16_t len;
+	uint8_t checkSum;
+	uint8_t isValid;
+	uint8_t data[MPC_FRAME_MAX];
+} MPC_HW_FRAME;
+
+/************************** Function Prototypes ******************************/
+// Instance initial
+int MpcHw_Init(MPC_HW **instPtr, MPC_HW_CONFIG *config);
+
+// Send a MPC frame.
+int MpcHw_Send(MPC_HW *instPtr, MPC_HW_FRAME *frame);
+
+// Get last MPC frame.
+int MpcHw_Recv(MPC_HW *instPtr, MPC_HW_FRAME *frame);
+
+#endif // __MPC_HW_H

df_datalink_ge/src/systembus

@@ -1 +0,0 @@
-../../df_datalink_sp/src/systembus

df_datalink_sp/src/config/app_config.h

@@ -54,7 +54,6 @@
 // Physical Layer Configs
 #define PHY_DEVICE_ID 0
 #define PHY_BASEADDR	XPAR_PHYC_BASEADDR
-#define PHY_BEAM_SET_PRESET
 #define PHY_BEAM_FORMING_ENABLE
 // -- Antenna Bram Configs
 #define PHY_ANTCONFIG_BRAM_DEVICE_ID XPAR_PHY_DATA_INTERFACE_ANT_INFO_BRAM_CTRL_DEVICE_ID

df_datalink_sp/src/main.c

@@ -484,7 +484,7 @@ int main(void)
 	while (true)
 	{
 		// System bus handler
-		if (SystemBus_IsReqPendding(sysbusDescPtr))
+		if (SystemBus_IsReqPending(sysbusDescPtr))
 		{
 			Status = SystemBus_HandleCmd(sysbusDescPtr);
 			if (Status != SUCCESS)
@@ -492,7 +492,7 @@ int main(void)
 				DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %" PRId32 ".\n", Status);
 			}
 		}
-		if (SystemBus_IsRsqPendding(sysbusDescPtr) && (!SystemBus_IsTxBusy(sysbusDescPtr)))
+		if (SystemBus_IsRsqPending(sysbusDescPtr) && (!SystemBus_IsTxBusy(sysbusDescPtr)))
 		{
 			// FIXME: Required by 12 (CASC)
 			udelay(4000);

df_datalink_sp/src/networking/link_layer/link_layer.c

@@ -32,6 +32,7 @@
 /************************** Constant Definitions *****************************/
 #define NET_START_TIME_INFTY UINT32_MAX
 #define LINK_BROCK_THRESHOLD (3 * 200) // 3 secods
+
 /************************** Variable Definitions *****************************/
 /***************** Macros (Inline Functions) Definitions *********************/
 /**************************** Type Definitions *******************************/
@@ -340,48 +341,48 @@ void L2_FrameStateUpdate(L2_INST *instPtr)
 	}
 	else
 	{
-	DBG_INFO("A frame received, checking data validation...\n");
-	DBG_INFO("Last frame type is: %" PRId32 ".\n", instPtr->frame.last);
-	Phy_RetrieveRecvData(phyRef, instPtr->recvPktData);
-	instPtr->log.frameRecv++;
+		DBG_INFO("A frame received, checking data validation...\n");
+		DBG_INFO("Last frame type is: %" PRId32 ".\n", instPtr->frame.last);
+		Phy_RetrieveRecvData(phyRef, instPtr->recvPktData);
+		instPtr->log.frameRecv++;
 
-	// Update frame state.
-	switch (recvType)
-	{
-	case L2_SCAN_FRAME:
-	case L2_ADV_FRAME:
-		isValid = L2_CrcCheck(instPtr->recvPktData, L2_ADV_PACK_LEN, L2_CRC16_1021);
-		instPtr->log.advRecvCnt++;
-		instPtr->log.advCorrectCnt = isValid ? instPtr->log.advCorrectCnt + 1 : instPtr->log.advCorrectCnt;
-		break;
-	case L2_CFM_FRAME:
-		isValid = L2_CrcCheck(instPtr->recvPktData, L2_CMF_PACK_LEN, L2_CRC16_1021);
-		instPtr->log.cfmRecvCnt++;
-		instPtr->log.cfmCorrectCnt = isValid ? instPtr->log.cfmCorrectCnt + 1 : instPtr->log.cfmCorrectCnt;
-		break;
-	case L2_SVC_FRAME:
+		// Update frame state.
+		switch (recvType)
+		{
+		case L2_SCAN_FRAME:
+		case L2_ADV_FRAME:
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_ADV_PACK_LEN, L2_CRC16_1021);
+			instPtr->log.advRecvCnt++;
+			instPtr->log.advCorrectCnt = isValid ? instPtr->log.advCorrectCnt + 1 : instPtr->log.advCorrectCnt;
+			break;
+		case L2_CFM_FRAME:
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_CMF_PACK_LEN, L2_CRC16_1021);
+			instPtr->log.cfmRecvCnt++;
+			instPtr->log.cfmCorrectCnt = isValid ? instPtr->log.cfmCorrectCnt + 1 : instPtr->log.cfmCorrectCnt;
+			break;
+		case L2_SVC_FRAME:
 #ifndef DATALINK_ENABLE_BERTEST
-		isValid = L2_CrcCheck(instPtr->recvPktData, L2_SVC_PACK_LEN, L2_CRC16_1021);
+			isValid = L2_CrcCheck(instPtr->recvPktData, L2_SVC_PACK_LEN, L2_CRC16_1021);
 #else
-		isValid = true;
+			isValid = true;
 #endif
-		instPtr->log.svcRecvCnt++;
-		instPtr->log.svcCorrectCnt = isValid ? instPtr->log.svcCorrectCnt + 1 : instPtr->log.svcCorrectCnt;
-		break;
+			instPtr->log.svcRecvCnt++;
+			instPtr->log.svcCorrectCnt = isValid ? instPtr->log.svcCorrectCnt + 1 : instPtr->log.svcCorrectCnt;
+			break;
 
-	default:
-		isValid = false;
-		break;
-	}
-	if(isValid)
-	{
-		instPtr->log.frameCorrect++;
-		L2_DataAvailUpdate(instPtr, true);
-	}
-	else
-	{
-		L2_DataAvailUpdate(instPtr, false);
-	}
+		default:
+			isValid = false;
+			break;
+		}
+		if (isValid)
+		{
+			instPtr->log.frameCorrect++;
+			L2_DataAvailUpdate(instPtr, true);
+		}
+		else
+		{
+			L2_DataAvailUpdate(instPtr, false);
+		}
 	}
 }
 
@@ -660,10 +661,10 @@ int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config)
 		DBG_ERROR("Allocate memory for L2_InstPtr failed.\n");
 		return ENOMEM;
 	}
+
+	// Clear random content
 	memset(*instPtr, 0x00, sizeof(L2_INST));
 	memcpy(&((*instPtr)->config), config, sizeof(L2_CONFIG));
-	// Clear log
-	memset(&((*instPtr)->log), 0x00, sizeof((*instPtr)->log));
 
 	// Packet buffer
 	(*instPtr)->sendPktData = calloc(L2_PACK_LEN_MAX / sizeof(uint64_t), sizeof(uint64_t));
@@ -686,7 +687,7 @@ int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config)
 	(*instPtr)->rtneConfig.curr = NULL;
 	(*instPtr)->rtneConfig.next = NULL;
 	(*instPtr)->isEnable = false;
-	(*instPtr)->cxn2Net = (config->myType == L2_NODE_HEAD) ? true : false;;
+	(*instPtr)->cxn2Net = (config->myType == L2_NODE_HEAD) ? true : false;
 	(*instPtr)->dataAvail = false;
 	(*instPtr)->frameNum.last = 0;
 	(*instPtr)->frameNum.curr = 0;
@@ -854,7 +855,8 @@ int32_t Linklayer_Start(L2_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	// Set netStartTime at first call.
-	if (instPtr->netStartTime == NET_START_TIME_INFTY) {
+	if (instPtr->netStartTime == NET_START_TIME_INFTY)
+	{
 		instPtr->netStartTime = TimeCtrl_GetIntCnt(instPtr->config.tmcInstPtr) / L2_SLOTS_PER_FRAME + 1;
 	}
 	instPtr->isEnable = true;
@@ -965,7 +967,7 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
 		L2_Scan_Reset(instPtr->scanFSM);
 		L2_Service_Reset(instPtr->serviceFSM);
 		UpdateMainSate(instPtr, L2_INIT_STATE);
-		// Log reconnect 
+		// Log reconnect
 		instPtr->log.reconnectCnt++;
 		break;
 	}
@@ -988,7 +990,8 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
 		}
 		else
 		{
-			DBG_WARNING("Unsupported routine state [%" PRId8 "], Continue withe previous routine table.\n", instPtr->nextState);
+			DBG_WARNING("Unsupported routine state [%" PRId8 "], "
+					"Continue with previous routine table.\n", instPtr->nextState);
 		}
 		break;
 	}
@@ -1117,7 +1120,7 @@ static int32_t Linklayer_Routine(L2_INST *instPtr)
 	case L2_SLOT_CONFIRM:
 		Status = L2_Scan_Routine(instPtr, &rtneEntry);
 		break;
-	 case L2_SLOT_SERVICE:
+	case L2_SLOT_SERVICE:
 		Status = L2_Service_Routine(instPtr, &rtneEntry);
 		break;
 	default:
@@ -1137,15 +1140,18 @@ static int32_t Linklayer_Routine(L2_INST *instPtr)
 	L2_IterEditNode(instPtr->nodeList, &increment, IntcNodeTTL);
 
 #ifdef DATALINK_ENABLE_AUTORECONNECT
-	L2_IterDeleteNode(instPtr->nodeList, IsNodeConnected);
-	if (L2_GetNodeListLen(instPtr->nodeList) < instPtr->config.netScale - 1)
+	if (L2_GetMainState(instPtr) != L2_INIT_STATE)
 	{
-		// reset scan process
-		L2_EVENT l2Event;
-		// Assert Net-Setup Done.
-		l2Event.event = (L2_GetNodeType(instPtr) == L2_NODE_HEAD) ? L2_EV_LOST_NODE : L2_EV_LOST_CXN;
-		l2Event.msg = NULL;
-		AssertL2Event(instPtr, &l2Event);
+		L2_IterDeleteNode(instPtr->nodeList, IsNodeConnected);
+		if (L2_GetNodeListLen(instPtr->nodeList) < instPtr->config.netScale - 1)
+		{
+			// reset scan process
+			L2_EVENT l2Event;
+			// Assert Net-Setup Done.
+			l2Event.event = (L2_GetNodeType(instPtr) == L2_NODE_HEAD) ? L2_EV_LOST_NODE : L2_EV_LOST_CXN;
+			l2Event.msg = NULL;
+			AssertL2Event(instPtr, &l2Event);
+		}
 	}
 #endif
 	return Status;
@@ -1222,7 +1228,7 @@ int32_t Linklayer_Proc(L2_INST *instPtr)
 		}
 	}
 
-		// Check if previous runs has error occurred.
+	// Check if previous runs has error occurred.
 	if (Status != SUCCESS)
 	{
 		return Status;
@@ -1233,17 +1239,16 @@ int32_t Linklayer_Proc(L2_INST *instPtr)
 
 	// Update link layer state.
 	instPtr->mainState = instPtr->nextState;
-
 	instPtr->linkLog.tuNum = instPtr->frameNum.curr;
 	instPtr->linkLog.slotNum = (uint8_t)instPtr->slotNum.curr;
 	instPtr->linkLog.nodeType = (instPtr->config.myType == L2_NODE_HEAD) ? 0xAA : 0xCC;
 	instPtr->linkLog.cxnStatus = (instPtr->cxn2Net) ? 0x11 : 0x00;
 	instPtr->linkLog.linkStage = (instPtr->mainState == L2_INIT_STATE) ? 0x11 : 0x22;
 	instPtr->linkLog.recxnCnt = instPtr->log.reconnectCnt;
-	instPtr->linkLog.beamNo = instPtr->log.scanBeamNo;
+	instPtr->linkLog.beamNo = instPtr->scanFSM->beamNo;
 	instPtr->linkLog.beamWidth = instPtr->log.lastBeamWidth;
-	instPtr->linkLog.beamAz = antSubnrrwPattern[instPtr->log.scanBeamNo].oaxis;
-	instPtr->linkLog.beamEl = antSubnrrwPattern[instPtr->log.scanBeamNo].raxis;
+	instPtr->linkLog.beamAz = instPtr->config.phyInstPtr->antNrrwPattern[instPtr->scanFSM->beamNo].oaxis;
+	instPtr->linkLog.beamEl = instPtr->config.phyInstPtr->antNrrwPattern[instPtr->scanFSM->beamNo].raxis;
 	instPtr->linkLog.advTxCnt = (uint16_t)instPtr->log.advTxCnt;
 	instPtr->linkLog.advRecvCnt = (uint16_t)instPtr->log.advRecvCnt;
 	instPtr->linkLog.advCorrectCnt = (uint16_t)instPtr->log.advCorrectCnt;
@@ -1253,9 +1258,8 @@ int32_t Linklayer_Proc(L2_INST *instPtr)
 	instPtr->linkLog.svcTxCnt = instPtr->log.svcTxCnt;
 	instPtr->linkLog.svcRecvCnt = instPtr->log.svcRecvCnt;
 	instPtr->linkLog.svcCorrectCnt = instPtr->log.svcCorrectCnt;
-	instPtr->linkLog.timeDev = (instPtr->log.timeDev > INT32_MAX) ?
-								INT32_MAX :
-								((instPtr->log.timeDev < INT32_MIN) ? INT32_MIN : (int32_t) instPtr->log.timeDev);
+	instPtr->linkLog.timeDev = (instPtr->log.timeDev > INT32_MAX) 
+								? INT32_MAX : ((instPtr->log.timeDev < INT32_MIN) ? INT32_MIN : (int32_t)instPtr->log.timeDev);
 	instPtr->linkLog.sendId = instPtr->serviceFSM->txLog.dataId;
 	instPtr->linkLog.recvId = instPtr->serviceFSM->rxLog.dataId;
 

df_datalink_sp/src/networking/link_layer/link_layer.h

@@ -66,7 +66,6 @@ typedef struct
 	uint32_t svcCorrectCnt;
 
 	int64_t timeDev;
-	uint8_t scanBeamNo;
 	uint8_t lastBeamWidth;
 	uint32_t reconnectCnt;
 } L2_LOG;
@@ -132,6 +131,7 @@ bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
 
 // Scanning
 int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr);
+void L2_Scan_RefreashScanTable(L2_INST *instPtr);
 void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr);
 void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr);
 int32_t L2_Scan_Proc(L2_INST *instPtr);

df_datalink_sp/src/networking/link_layer/link_layer_types.h

@@ -195,10 +195,11 @@ typedef struct
 	bool isAdvRecvd; // bm received.
 	bool isAckPending;	// 2way acq done.
 	uint8_t scanCnt;
-	uint8_t beamNo;
 	uint8_t advTimeout;
 	uint8_t cfmTimeout;
 	SCAN_STAGE scanStage;  // only for member node
+	uint8_t beamNo;
+	uint8_t tableNo;
 } L2_SCAN_FSM;
 
 

df_datalink_sp/src/networking/link_layer/msg_def.h

@@ -79,6 +79,10 @@
 #define L2_MSG_CFM_SRC_SPEEDY_LEN 4
 #define L2_MSG_CFM_SRC_SPEEDZ 28
 #define L2_MSG_CFM_SRC_SPEEDZ_LEN 4
+#define L2_MSG_CFM_PAYLOAD_ID 32
+#define L2_MSG_CFM_PAYLOAD_ID_LEN 4
+#define L2_MSG_CFM_PAYLOAD 36
+#define L2_MSG_CFM_PAYLOAD_LEN 230
 #define L2_MSG_CFM_CRC 266
 #define L2_MSG_CFM_CRC_LEN 2
 

df_datalink_sp/src/networking/link_layer/node_list.c

@@ -130,6 +130,10 @@ void L2_NodeList_Destroy(NODE_LIST *list)
 NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId)
 {
 	ASSERT(list != NULL);
+	if (nodeId == L2_BROADCAST_ID || nodeId == L2_NONE_ID)
+	{
+		return NULL;
+	}
 	util_entry.nodeInfo.nodeId = nodeId;
 	clist_node_t *node = clist_find(list, &util_entry);
 	if (node == NULL)

df_datalink_sp/src/networking/link_layer/scan.c

@@ -28,13 +28,95 @@
 #define ADV_LISTEN_TIMEOUT 40
 #define CFM_LISTEN_TIMEOUT 25
 #define ADV_FRAME_SLOTS 4
-#define CFM_FRAME_SLOTS 4
+#define CFM_FRAME_SLOTS 5
 /***************** Macros (In-line Functions) Definitions *********************/
 
 /**************************** Type Definitions *******************************/
 
 /************************** Variable Definitions *****************************/
+// Pair by regex
+// Regex: ((\{\.oaxis = [0-9]+[ ,]+\.raxis = [0-9]+[ ]*\}, ){2})
+PHY_ANT_ANGLE antNrrwPattern_0[L1_NUM_ANT_NRRW_PATTERN]  =
+{
+	{.oaxis = 0  , .raxis = 0   },
+	{.oaxis = 139, .raxis = 2225},
+	{.oaxis = 152, .raxis = 850 },
+	{.oaxis = 138, .raxis = 3155},
+	{.oaxis = 159, .raxis = 1680},
+	{.oaxis = 168, .raxis = 285 },
+	{.oaxis = 195, .raxis = 2630},
+	{.oaxis = 211, .raxis = 1204},
+	{.oaxis = 225, .raxis = 3429},
+	{.oaxis = 249, .raxis = 2022},
+	{.oaxis = 272, .raxis = 629 },
+	{.oaxis = 265, .raxis = 2934},
+	{.oaxis = 277, .raxis = 1499},
+	{.oaxis = 289, .raxis = 124 },
+	{.oaxis = 300, .raxis = 2349},
+	{.oaxis = 311, .raxis = 959 },
+	{.oaxis = 321, .raxis = 3199},
+	{.oaxis = 331, .raxis = 1824},
+	{.oaxis = 341, .raxis = 389 },
+	{.oaxis = 351, .raxis = 2674},
+	{.oaxis = 360, .raxis = 1268},
+	{.oaxis = 364, .raxis = 3483},
+	{.oaxis = 378, .raxis = 2118},
+	{.oaxis = 387, .raxis = 733 },
+	{.oaxis = 416, .raxis = 3028},
+	{.oaxis = 404, .raxis = 1623},
+	{.oaxis = 433, .raxis = 248 },
+	{.oaxis = 421, .raxis = 2473},
+	{.oaxis = 429, .raxis = 1098},
+	{.oaxis = 437, .raxis = 3303},
+	{.oaxis = 445, .raxis = 1948},
+	{.oaxis = 443, .raxis = 543 },
+	{.oaxis = 450, .raxis = 2828},
+	{.oaxis = 448, .raxis = 1422},
+	{.oaxis = 455, .raxis = 47  },
+	{.oaxis = 453, .raxis = 2272},
+	{.oaxis = 450, .raxis = 897 }
+};
 
+PHY_ANT_ANGLE antNrrwPattern_1[L1_NUM_ANT_NRRW_PATTERN]  =
+{
+	{.oaxis = 0  , .raxis = 0   },
+	{.oaxis = 139, .raxis = 1155},
+	{.oaxis = 152, .raxis = 3380},
+	{.oaxis = 138, .raxis = 2085},
+	{.oaxis = 159, .raxis = 610 },
+	{.oaxis = 168, .raxis = 2815},
+	{.oaxis = 195, .raxis = 1560},
+	{.oaxis = 211, .raxis = 134 },
+	{.oaxis = 225, .raxis = 2359},
+	{.oaxis = 249, .raxis = 952 },
+	{.oaxis = 272, .raxis = 3159},
+	{.oaxis = 265, .raxis = 1864},
+	{.oaxis = 277, .raxis = 429 },
+	{.oaxis = 289, .raxis = 2654},
+	{.oaxis = 300, .raxis = 1279},
+	{.oaxis = 311, .raxis = 3489},
+	{.oaxis = 321, .raxis = 2129},
+	{.oaxis = 331, .raxis = 754 },
+	{.oaxis = 341, .raxis = 2919},
+	{.oaxis = 351, .raxis = 1604},
+	{.oaxis = 360, .raxis = 198 },
+	{.oaxis = 364, .raxis = 2413},
+	{.oaxis = 378, .raxis = 1048},
+	{.oaxis = 387, .raxis = 3263},
+	{.oaxis = 416, .raxis = 1958},
+	{.oaxis = 404, .raxis = 553 },
+	{.oaxis = 433, .raxis = 2778},
+	{.oaxis = 421, .raxis = 1403},
+	{.oaxis = 429, .raxis = 28  },
+	{.oaxis = 437, .raxis = 2233},
+	{.oaxis = 445, .raxis = 878 },
+	{.oaxis = 443, .raxis = 3073},
+	{.oaxis = 450, .raxis = 1758},
+	{.oaxis = 448, .raxis = 352 },
+	{.oaxis = 455, .raxis = 2577},
+	{.oaxis = 453, .raxis = 1202},
+	{.oaxis = 450, .raxis = 3427},
+};
 /************************** Function Prototypes ******************************/
 static void LockStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
 static void AdvRecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
@@ -112,9 +194,23 @@ static void StageUpdate(L2_SCAN_FSM *scanFsmPtr, SCAN_STAGE newStage)
 static int32_t Scan_InitScanHandler(L2_INST *instPtr)
 {
 	ASSERT(instPtr != NULL);
+	PHY_INST *phyRef = instPtr->config.phyInstPtr;
 	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
 	DBG_INFO("** Net-Setup - InitScan Procedure (Attempts: %" PRIu8 ")**\n", scanFsmPtr->scanCnt);
 
+	if(scanFsmPtr->scanCnt == 0)
+	{
+		// Update scan beam table
+		L2_Scan_RefreashScanTable(instPtr);
+	}
+	else if(Phy_IsFrameDetected(phyRef))
+	{
+		// last scan beam number
+		instPtr->scanFSM->beamNo = Phy_LastScanBeamNo(instPtr->config.phyInstPtr);
+		// Update scan beam table
+		L2_Scan_RefreashScanTable(instPtr);
+	}
+
 	if (scanFsmPtr->isLocked == true)
 	{
 		StageUpdate(scanFsmPtr, L2_SCAN_ADV_STAGE);
@@ -149,6 +245,9 @@ static int32_t Scan_AdvHandler(L2_INST *instPtr)
 	// if received a valid bm frame.
 	if (scanFsmPtr->isAdvRecvd == true)
 	{
+		// last scan beam number
+		instPtr->scanFSM->beamNo = Phy_LastScanBeamNo(instPtr->config.phyInstPtr);
+
 		scanFsmPtr->advTimeout = 0; /* reset listen counter. */
 		DBG_INFO("Received Adv frame, skipping 10ms for turn around...\n");
 		// TODO: check functionality.
@@ -201,9 +300,12 @@ static int32_t Scan_CfmHandler(L2_INST *instPtr)
 	{
 		// using pull mode to receive confirm frame
 		// since first time sync procedure is accomplished in ADV stage.
+		Phy_SetBeamWidth(instPtr->config.phyInstPtr, PHY_ANT_BEAM_16);
+		Phy_SetBeamNo(instPtr->config.phyInstPtr, scanFsmPtr->beamNo, PHY_ANT_BEAM_16);
 		Phy_SetMode(instPtr->config.phyInstPtr, PHY_MODE_PULL);
 		L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_CFM_FRAME);
 		scanFsmPtr->cfmTimeout++; // increase counter.
+		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16;
 	}
 	return SUCCESS;
 }
@@ -278,6 +380,23 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 		L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_SPEEDY, L2_MSG_CFM_SRC_SPEEDY_LEN, &(instPtr->sysInfo.vY));
 		L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_SPEEDZ, L2_MSG_CFM_SRC_SPEEDZ_LEN, &(instPtr->sysInfo.vZ));
 
+		// Packing extra payload
+		uint32_t dataId;
+		if(L2_GetNodeType(instPtr) == L2_NODE_HEAD)
+		{
+			SysMonitor_GetSendPayload(buff + L2_MSG_CFM_PAYLOAD, &dataId, PAYLOAD_LEN_FLJ_TX);
+		}
+		else
+		{
+			SysMonitor_GetSendPayload(buff + L2_MSG_CFM_PAYLOAD, &dataId, PAYLOAD_LEN_LJ_TX);
+		}
+
+		L2_FrameInsertChunk(buff, L2_MSG_CFM_PAYLOAD_ID, L2_MSG_CFM_PAYLOAD_ID_LEN, &dataId);
+		if (dataId == DATA_ID_NONE)
+		{
+			memset(buff + L2_MSG_CFM_PAYLOAD, 0xAA, L2_MSG_CFM_PAYLOAD_LEN);
+		}
+
 		// Append CRC
 		Status = L2_CrcAppend(buff, L2_CMF_PACK_LEN - L2_MSG_CFM_CRC_LEN, L2_MSG_CFM_CRC, L2_CRC16_1021);
 		if (Status != SUCCESS)
@@ -315,6 +434,8 @@ int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr)
 		return ENOMEM;
 	}
 	L2_Scan_Reset(*scanFSMPtr);
+	(*scanFSMPtr)->beamNo = 0;
+	(*scanFSMPtr)->tableNo = 0;
 	return SUCCESS;
 }
 
@@ -334,7 +455,6 @@ void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr)
 	scanFSMPtr->isAdvRecvd = false;
 	scanFSMPtr->isAckPending = false;
 	scanFSMPtr->scanCnt = 0;
-	scanFSMPtr->beamNo = 0;
 	scanFSMPtr->advTimeout = 0;
 	scanFSMPtr->cfmTimeout = 0;
 	scanFSMPtr->scanStage = L2_SCAN_NONE_STAGE;
@@ -354,10 +474,37 @@ void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr)
 	free(scanFSMPtr);
 }
 
+/*****************************************************************************/
+/**
+ * @brief  Refresh scan beam table.
+ *
+ * @param   instPtr	a pointer to linklayer layer instance
+ *
+ * @return  NONE
+ *
+ ******************************************************************************/
+void L2_Scan_RefreashScanTable(L2_INST *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
+	PHY_INST *phyRef = instPtr->config.phyInstPtr;
+	if (scanFsmPtr->tableNo == 0)
+	{
+		Phy_SetBeamTable(phyRef, antNrrwPattern_0, scanFsmPtr->beamNo);
+		scanFsmPtr->tableNo = 1;
+	}
+	else
+	{
+		Phy_SetBeamTable(phyRef, antNrrwPattern_1, scanFsmPtr->beamNo);
+		scanFsmPtr->tableNo = 0;
+	}
+}
+
 /*****************************************************************************/
 /**
  * @brief   Scan net packet handler.
  *
+ * @param   instPtr	a pointer to linklayer layer instance
  * @param   msg A pointer to frame data buffer.
  *
  * @return
@@ -407,9 +554,6 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 		node->nodeInfo.pos.z = (double) nodePos;
 		node->nodeInfo.beamNo = Phy_LastScanBeamNo(instPtr->config.phyInstPtr);
 
-		// log last scan beam number
-		instPtr->log.scanBeamNo = node->nodeInfo.beamNo;
-
 		// Reset node ttl
 		node->ttlCnt = 0;
 
@@ -525,6 +669,23 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 			// Time sync.
 			L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
 		}
+
+		// Get data payload
+		uint32_t dataId;
+		L2_FrameGetChunk(msg, L2_MSG_CFM_PAYLOAD_ID, L2_MSG_CFM_PAYLOAD_ID_LEN, &dataId);
+
+		if(dataId != DATA_ID_NONE)
+		{
+			if(L2_GetNodeType(instPtr) == L2_NODE_HEAD)
+			{
+				SysMonitor_UpdateRecvPayload(msg+L2_MSG_CFM_PAYLOAD, PAYLOAD_LEN_FLJ_RX);
+			}
+			else
+			{
+				SysMonitor_UpdateRecvPayload(msg+L2_MSG_CFM_PAYLOAD, PAYLOAD_LEN_LJ_RX);
+			}
+		}
+
 		Status = SUCCESS;
 		break;
 	}
@@ -608,23 +769,24 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 		if (rtneEntry->dst == L2_GetNodeId(instPtr) || rtneEntry->dst == L2_BROADCAST_ID)
 		{
 			torState = L2_TOR_RX;
-			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
 		}
 		else if (rtneEntry->src == L2_GetNodeId(instPtr) || rtneEntry->src == L2_BROADCAST_ID)
 		{
 			torState = L2_TOR_TX;
-			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
+			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
 		}
 		else
 		{
 			torState = L2_TOR_SUSPEND;
 		}
 
-		Phy_SetBeamWidth(instPtr->config.phyInstPtr, PHY_ANT_BEAM_25);
-		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_25;
+		// Setup antenna beam
+		Phy_SetBeamWidth(instPtr->config.phyInstPtr, PHY_ANT_BEAM_16);
+		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16;
 		if (commNode != NULL)
 		{
-			Phy_SetBeamNo(instPtr->config.phyInstPtr, commNode->nodeInfo.beamNo);
+			Phy_SetBeamNo(instPtr->config.phyInstPtr, commNode->nodeInfo.beamNo, PHY_ANT_BEAM_16);
 		}
 	}
 	else
@@ -641,6 +803,7 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 		{
 			torState = L2_TOR_SUSPEND;
 		}
+		// Setup antenna beam
 		Phy_SetBeamWidth(instPtr->config.phyInstPtr, PHY_ANT_BEAM_120);
 		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_120;
 	}

df_datalink_sp/src/networking/link_layer/service.c

@@ -134,7 +134,6 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 		// L2_FrameInsertChunk(buff, L2_MSG_SVC_PAYLOAD_ID, L2_MSG_SVC_PAYLOAD_ID_LEN, &dataId);
 		// L2_FrameInsertChunk(buff, L2_MSG_SVC_PAYLOAD, L2_MSG_SVC_PAYLOAD_LEN, testData);
 
-
 		Status = L2_CrcAppend(buff, L2_SVC_PACK_LEN - L2_MSG_SVC_CRC_LEN, L2_MSG_SVC_CRC, L2_CRC16_1021);
 		if (Status != SUCCESS)
 		{
@@ -373,7 +372,7 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 	if (rtneEntry->src == L2_GetNodeId(instPtr))
 	{
 		torState = L2_TOR_TX;
-		commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
+		commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst);
 	}
 	else if (rtneEntry->dst == L2_GetNodeId(instPtr))
 	{
@@ -385,11 +384,12 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 		torState = L2_TOR_SUSPEND;
 	}
 
+	// Setup antenna beam
 	if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && commNode != NULL)
 	{
-		Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_25);
-		Phy_SetBeamNo(phyRef, commNode->nodeInfo.beamNo);
-		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_25;
+		Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_16);
+		Phy_SetBeamNo(phyRef, commNode->nodeInfo.beamNo, PHY_ANT_BEAM_16);
+		instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16;
 	}
 	else
 	{
@@ -409,9 +409,6 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 			memcpy(instPtr->sendPktData, testData, L2_SVC_PACK_LEN);
 #endif
 		}
-		// FIXME: Steer antenna to point to destination.
-		Phy_SetBeamAngle(phyRef, 0.0, 0.0);
-		Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_8);
 		L2_SetupNextSlot(instPtr, torState, L2_SVC_FRAME);
 		break;
 	default:

df_datalink_sp/src/networking/phy_layer/phy_antenna.c

@@ -29,16 +29,19 @@
 #define CTRL_WORD_DEFAULT 0x00
 
 #define ANT_MODE_BEAM_MASK 0xF0
-#define ANT_MODE_NARROR_BEAM 0x00 // 8
-#define ANT_MODE_WIDE_BEAM_1 0x10 // 25
-#define ANT_MODE_WIDE_BEAM_2 0x20 // 60
-#define ANT_MODE_WIDE_BEAM_3 0x30 // 120
+#define ANT_BEAM_NARROR 0x00 // 8
+#define ANT_BEAM_SUBNARROR_0 0x10 // 25
+#define ANT_BEAM_SUBNARROR_1 0x20 // 60
+#define ANT_BEAM_WIDE 0x30 // 120
 
-#define ANT_MODE_HIGH_POWER 0x00
-#define ANT_MODE_LOW_POWER 0x04
-#define ANT_MODE_STANDBY 0x0F
-#define ATN_MODE_DEFAULT (ANT_MODE_WIDE_BEAM_1 | ANT_MODE_LOW_POWER)
-#define ATN_MODE_HIGH (ANT_MODE_WIDE_BEAM_1 | ANT_MODE_HIGH_POWER)
+#define ANT_MODE_POWER_MASK 0x0F
+#define ANT_POWER_FULL 0x00
+#define ANT_POWER_LOW 0x04
+#define ANT_POWER_STANDBY 0x0F
+#define ATN_MODE_SILENT (ANT_BEAM_SUBNARROR_0 | ANT_POWER_STANDBY)
+#define ATN_MODE_LOWPOWER (ANT_BEAM_SUBNARROR_0 | ANT_POWER_LOW)
+#define ATN_MODE_FULLPOWER (ANT_BEAM_SUBNARROR_0 | ANT_POWER_FULL)
+#define ATN_MODE_DEFAULT ATN_MODE_SILENT
 
 #define DEFAULT_CENTRTAL_FREQ 0x06D7 // 26.25GHz
 
@@ -54,21 +57,21 @@
 
 // Pair by regex
 // Regex: (([0-9]+, ){8})
-uint16_t hopPattern_Config[L1_FRAME_MAX_HOPS] = 
+uint16_t hopPattern_Config[L1_FRAME_MAX_HOPS] =
 {
 	0
 };
 
-uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] = 
+uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] =
 {
 	1500
 };
 
-uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS] = 
+uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS] =
 {
-	0,   44,    88,   131,  175,  219,  263,  306,
-	350,  394,  438,  481,  525,  569,  613,  656,
-	700,  744,  788,  831,  875,  919,  963,  1006,
+	   0,   44,   88,  131,  175,  219,  263,  306,
+	 350,  394,  438,  481,  525,  569,  613,  656,
+	 700,  744,  788,  831,  875,  919,  963, 1006,
 	1050, 1094, 1138, 1181, 1225, 1269, 1313, 1356,
 	1400, 1444, 1488, 1531, 1575, 1619, 1663, 1706,
 	1750, 1794, 1838, 1881, 1925, 1969, 2013, 2056,
@@ -79,120 +82,64 @@ uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS] =
 	3500
 };
 
-uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS] = 
+uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS] =
 {
 	1500
 };
 
-uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] = 
+//uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
+//{
+//	1500, 1600, 1700, 1800, 1900, 1400, 1300, 1200,
+//	1100, 1000,  900
+//};
+
+uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
 {
-	1500, 1600, 1700, 1800, 1900, 1400, 1300, 1200, 
-	1100, 1000, 900
+	1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,
+	1500, 1500,  1500
 };
 
-uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] = 
+uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] =
 {
-	1600, 1650, 1700, 1750, 1800, 1850, 1900, 1550, 
+	1600, 1650, 1700, 1750, 1800, 1850, 1900, 1550,
 	1500
 };
 
-uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] = 
+//uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
+//{
+//	1600, 1650, 1700, 1750, 1800, 1850, 1900, 1550,
+//	1500, 1450, 1400, 1350, 1300, 1250, 1200, 1150,
+//	1100, 1050, 1000
+//};
+
+uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
 {
-	1600, 1650, 1700, 1750, 1800, 1850, 1900, 1550, 
-	1500, 1450, 1400, 1350, 1300, 1250, 1200, 1150, 
-	1100, 1050, 1000
+	1700, 1800, 1900, 2000, 1700, 1700, 1700, 1700,
+	1700, 1700, 1700, 1700, 1700, 1700, 1700, 1700,
+	1700, 1700, 1700
 };
 
 uint16_t hopPattern_200KBPS_5MS[L1_FRAME_200KBPS_5MS_HOPS] =
 {
-	500,  1600, 1700, 1800, 1900, 2000, 1500, 1600,
+	 500, 1600, 1700, 1800, 1900, 2000, 1500, 1600,
 	1700, 1800, 1900, 2000, 1500, 1600, 1700, 1800,
 	1900, 2000, 1500, 1500
 };
 
-uint16_t hopPattern_1M2BPS_5MS[L1_FRAME_1M2BPS_5MS_HOPS] = 
+uint16_t hopPattern_1M2BPS_5MS[L1_FRAME_1M2BPS_5MS_HOPS] =
 {
-	500, 600, 700, 800, 900, 1000, 500, 600,
-	700, 800, 900, 1000, 500, 600, 700, 800,
-	900, 1000, 500, 500
+	 500,  600,  700,  800,  900, 1000,  500,  600,
+	 700,  800,  900, 1000,  500,  600,  700,  800,
+	 900, 1000,  500,  500
 };
 
-// Pair by regex
-// Regex: ((\{\.oaxis = [0-9]+[ ,]+\.raxis = [0-9]+[ ]*\}, ){2})
-#ifdef PHY_BEAM_SET_PRESET
-PHY_ANT_ANGLE antSubnrrwPattern[L1_NUM_ANT_SUBNRRW_PATTERN] = 
-{
-	{.oaxis = 0 , .raxis = 0  }, {.oaxis = 10, .raxis = 85 },
-	{.oaxis = 14, .raxis = 307}, {.oaxis = 17, .raxis = 170},
-	{.oaxis = 19, .raxis = 32 }, {.oaxis = 21, .raxis = 255},
-	{.oaxis = 24, .raxis = 117}, {.oaxis = 25, .raxis = 340},
-	{.oaxis = 27, .raxis = 202}, {.oaxis = 29, .raxis = 65 },
-	{.oaxis = 31, .raxis = 287}, {.oaxis = 32, .raxis = 150},
-	{.oaxis = 34, .raxis = 12 }, {.oaxis = 35, .raxis = 235},
-	{.oaxis = 36, .raxis = 97 }, {.oaxis = 38, .raxis = 320},
-	{.oaxis = 39, .raxis = 182}, {.oaxis = 40, .raxis = 45 },
-	{.oaxis = 41, .raxis = 267}, {.oaxis = 43, .raxis = 130},
-	{.oaxis = 44, .raxis = 352}, {.oaxis = 45, .raxis = 215},
-	{.oaxis = 46, .raxis = 77 }, {.oaxis = 47, .raxis = 300},
-	{.oaxis = 48, .raxis = 162}, {.oaxis = 49, .raxis = 25 },
-	{.oaxis = 50, .raxis = 247}, {.oaxis = 51, .raxis = 110},
-	{.oaxis = 52, .raxis = 332}, {.oaxis = 53, .raxis = 195},
-	{.oaxis = 54, .raxis = 57 }, {.oaxis = 55, .raxis = 280},
-	{.oaxis = 56, .raxis = 142}, {.oaxis = 57, .raxis = 5  },
-	{.oaxis = 58, .raxis = 227}, {.oaxis = 59, .raxis = 90 },
-	{.oaxis = 60, .raxis = 312}
-};
-PHY_ANT_ANGLE antNrrwPattern[L1_NUM_ANT_NRRW_PATTERN] = 
-{
-	{.oaxis = 0 , .raxis = 0  }, {.oaxis = 7 , .raxis = 85 },
-	{.oaxis = 10, .raxis = 307}, {.oaxis = 13, .raxis = 170},
-	{.oaxis = 15, .raxis = 32 }, {.oaxis = 17, .raxis = 255},
-	{.oaxis = 18, .raxis = 117}, {.oaxis = 20, .raxis = 340},
-	{.oaxis = 21, .raxis = 202}, {.oaxis = 22, .raxis = 65 },
-	{.oaxis = 24, .raxis = 287}, {.oaxis = 25, .raxis = 150},
-	{.oaxis = 26, .raxis = 12 }, {.oaxis = 27, .raxis = 235},
-	{.oaxis = 28, .raxis = 97 }, {.oaxis = 29, .raxis = 320},
-	{.oaxis = 30, .raxis = 182}, {.oaxis = 31, .raxis = 45 },
-	{.oaxis = 32, .raxis = 267}, {.oaxis = 33, .raxis = 130},
-	{.oaxis = 34, .raxis = 352}, {.oaxis = 34, .raxis = 215},
-	{.oaxis = 35, .raxis = 77 }, {.oaxis = 36, .raxis = 300},
-	{.oaxis = 37, .raxis = 162}, {.oaxis = 38, .raxis = 25 },
-	{.oaxis = 38, .raxis = 247}, {.oaxis = 39, .raxis = 110},
-	{.oaxis = 40, .raxis = 332}, {.oaxis = 41, .raxis = 195},
-	{.oaxis = 41, .raxis = 57 }, {.oaxis = 42, .raxis = 280},
-	{.oaxis = 43, .raxis = 142}, {.oaxis = 44, .raxis = 5  },
-	{.oaxis = 44, .raxis = 227}, {.oaxis = 45, .raxis = 90 },
-	{.oaxis = 46, .raxis = 312}, {.oaxis = 46, .raxis = 175},
-	{.oaxis = 47, .raxis = 37 }, {.oaxis = 48, .raxis = 260},
-	{.oaxis = 48, .raxis = 122}, {.oaxis = 49, .raxis = 345},
-	{.oaxis = 49, .raxis = 207}, {.oaxis = 50, .raxis = 70 },
-	{.oaxis = 51, .raxis = 292}, {.oaxis = 51, .raxis = 155},
-	{.oaxis = 52, .raxis = 17 }, {.oaxis = 53, .raxis = 240},
-	{.oaxis = 53, .raxis = 102}, {.oaxis = 54, .raxis = 325},
-	{.oaxis = 54, .raxis = 187}, {.oaxis = 55, .raxis = 50 },
-	{.oaxis = 55, .raxis = 272}, {.oaxis = 56, .raxis = 135},
-	{.oaxis = 57, .raxis = 357}, {.oaxis = 57, .raxis = 220},
-	{.oaxis = 58, .raxis = 82 }, {.oaxis = 58, .raxis = 305},
-	{.oaxis = 59, .raxis = 167}, {.oaxis = 59, .raxis = 30 },
-	{.oaxis = 60, .raxis = 252}
-};
-#else
-PHY_ANT_ANGLE antSubnrrwPattern[L1_NUM_ANT_SUBNRRW_PATTERN]  = 
-{
-	0
-};
-PHY_ANT_ANGLE antNrrwPattern[L1_NUM_ANT_NRRW_PATTERN] = 
-{
-	0
-};
-#endif
 /*****************************************************************************/
 /**
- * @brief
+ * @brief	Initialize antenna configurations (load preset values)
  *
- * @param   antConfig
+ * @param   antConfig	pointer to antenna configure
  *
- * @return
+ * @return	None.
  *
  ******************************************************************************/
 void Phy_AntConfigInit(PHY_ANT_CONFIG *antConfig)
@@ -246,32 +193,78 @@ void Phy_AntConfigInit(PHY_ANT_CONFIG *antConfig)
 	// ***********************
 }
 
-void Phy_AntConfigMode(PHY_INST *instPtr, PHY_ANT_MODE mode) 
+/*****************************************************************************/
+/**
+ * @brief	Update antenna power mode setting
+ *
+ * @param   instPtr	pointer to physical layer instance
+ * @param   mode	new antenna mode
+ *
+ * @return	None.
+ *
+ ******************************************************************************/
+void Phy_AntConfigMode(PHY_INST *instPtr, PHY_ANT_MODE mode)
 {
 	ASSERT(instPtr != NULL);
 	uint8_t config = instPtr->antConfig.antMode & ANT_MODE_BEAM_MASK;
 	switch (mode)
 	{
-	case PHY_ANT_MODE_SILENT:
-		config |= ANT_MODE_STANDBY;
+	case PHY_ANT_MODE_STANDBY:
+		config |= ANT_POWER_STANDBY;
 		break;
-	case PHY_ANT_MODE_LOW_POWER:
-		config |= ANT_MODE_LOW_POWER;
+	case PHY_ANT_MODE_POWER_LOW:
+		config |= ANT_POWER_LOW;
 		break;
-	case PHY_ANT_MODE_HIGH_POWER:
-		config |= ANT_MODE_HIGH_POWER;
+	case PHY_ANT_MODE_POWER_FULL:
+		config |= ANT_POWER_FULL;
 		break;
 	default:
 		DBG_ERROR("Unknown antenna mode: %" PRIu8 ", remain previous mode.\n", mode);
 		return;
 	}
+	instPtr->phyLog.antMode = mode;
 	instPtr->antConfig.antMode = config;
 }
 
-int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
+/*****************************************************************************/
+/**
+ * @brief	Update antenna beam width setting
+ *
+ * @param   instPtr	pointer to physical layer instance
+ * @param   table	new beam table
+ * @param   offset	table offset
+ *
+ * @return	None
+ *
+ ******************************************************************************/
+void Phy_SetBeamTable(PHY_INST *instPtr, PHY_ANT_ANGLE *table, uint32_t offset)
 {
 	ASSERT(instPtr != NULL);
-	if (instPtr->beamWd.next == PHY_ANT_BEAM_8)
+	ASSERT(table != NULL);
+	for (int i = 0; i < L1_NUM_ANT_NRRW_PATTERN; i++)
+	{
+		instPtr->antNrrwPattern[i] = table[(i+offset) % L1_NUM_ANT_NRRW_PATTERN];
+	}
+	memcpy((uint32_t*)instPtr->antInfoAddr, instPtr->antNrrwPattern,
+				L1_NUM_ANT_NRRW_PATTERN*sizeof(PHY_ANT_ANGLE));
+}
+
+/*****************************************************************************/
+/**
+ * @brief	Update antenna point direction using predefined beam table
+ *
+ * @param   instPtr	pointer to physical layer instance
+ * @param   beamNo	new beam index
+ *
+ * @return	SUCCESS.
+ * 			FAILURE beamNo exceeds the table size
+ *
+ ******************************************************************************/
+int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo, PHY_ANT_BEAM_WIDTH beamWidth)
+{
+	ASSERT(instPtr != NULL);
+
+	if (beamWidth == PHY_ANT_BEAM_16)
 	{
 		if (beamNo >= L1_NUM_ANT_NRRW_PATTERN)
 		{
@@ -280,26 +273,10 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
 		else
 		{
 			instPtr->beamNo.next = beamNo;
-			instPtr->antConfig.oaxis[1] = (uint8_t)(antNrrwPattern[beamNo].oaxis >> 8);
-			instPtr->antConfig.oaxis[0] = (uint8_t)(antNrrwPattern[beamNo].oaxis);
-			instPtr->antConfig.raxis[1] = (uint8_t)(antNrrwPattern[beamNo].raxis >> 8);
-			instPtr->antConfig.raxis[0] = (uint8_t)(antNrrwPattern[beamNo].raxis);
-			return SUCCESS;
-		}
-	}
-	else if (instPtr->beamWd.next == PHY_ANT_BEAM_25)
-	{
-		if (beamNo >= L1_NUM_ANT_SUBNRRW_PATTERN)
-		{
-			return FAILURE;
-		}
-		else
-		{
-			instPtr->beamNo.next = beamNo;
-			instPtr->antConfig.oaxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].oaxis >> 8);
-			instPtr->antConfig.oaxis[0] = (uint8_t)(antSubnrrwPattern[beamNo].oaxis);
-			instPtr->antConfig.raxis[1] = (uint8_t)(antSubnrrwPattern[beamNo].raxis >> 8);
-			instPtr->antConfig.raxis[0] = (uint8_t)(antSubnrrwPattern[beamNo].raxis);
+			instPtr->antConfig.oaxis[0] = (uint8_t)(instPtr->antNrrwPattern[beamNo].oaxis >> 8);
+			instPtr->antConfig.oaxis[1] = (uint8_t)(instPtr->antNrrwPattern[beamNo].oaxis);
+			instPtr->antConfig.raxis[0] = (uint8_t)(instPtr->antNrrwPattern[beamNo].raxis >> 8);
+			instPtr->antConfig.raxis[1] = (uint8_t)(instPtr->antNrrwPattern[beamNo].raxis);
 			return SUCCESS;
 		}
 	}
@@ -308,15 +285,26 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
 		// Beam number assign not support.
 		// Assign default angle (0,0)
 		instPtr->beamNo.next = 0;
-		instPtr->antConfig.oaxis[1] = 0;
 		instPtr->antConfig.oaxis[0] = 0;
-		instPtr->antConfig.raxis[1] = 0;
+		instPtr->antConfig.oaxis[1] = 0;
 		instPtr->antConfig.raxis[0] = 0;
+		instPtr->antConfig.raxis[1] = 0;
 		DBG_WARNING("Using default beam number...\n");
 		return SUCCESS;
 	}
 }
 
+/*****************************************************************************/
+/**
+ * @brief	Update antenna beam width setting
+ *
+ * @param   instPtr		pointer to physical layer instance
+ * @param   beamWidth	new beam width
+ *
+ * @return	SUCCESS.
+ * 			EPARAM - the direction is exceeds predefined range
+ *
+ ******************************************************************************/
 int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth)
 {
 	ASSERT(instPtr != NULL);
@@ -324,16 +312,16 @@ int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth)
 	switch (beamWidth)
 	{
 	case PHY_ANT_BEAM_120:
-		beamConfig |= ANT_MODE_WIDE_BEAM_3;
+		beamConfig |= ANT_BEAM_WIDE;
 		break;
 	case PHY_ANT_BEAM_60:
-		beamConfig |= ANT_MODE_WIDE_BEAM_2;
+		beamConfig |= ANT_BEAM_SUBNARROR_1;
 		break;
 	case PHY_ANT_BEAM_25:
-		beamConfig |= ANT_MODE_WIDE_BEAM_1;
+		beamConfig |= ANT_BEAM_SUBNARROR_0;
 		break;
-	case PHY_ANT_BEAM_8:
-		beamConfig |= ANT_MODE_NARROR_BEAM;
+	case PHY_ANT_BEAM_16:
+		beamConfig |= ANT_BEAM_NARROR;
 		break;
 	default:
 		DBG_ERROR("Beam width code: 0x%08X is illegal.\n\r", beamWidth);
@@ -344,7 +332,18 @@ int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth)
 	return SUCCESS;
 }
 
-// Common version radius double input
+/*****************************************************************************/
+/**
+ * @brief	Set antenna to a specific direction
+ *
+ * @param   instPtr	pointer to physical layer instance
+ * @param   offAx	elevation angle
+ * @param   rndAx	azimuth angle
+ *
+ * @return	SUCCESS.
+ * 			EPARAM - the direction is exceeds predefined range
+ *
+ ******************************************************************************/
 int32_t Phy_SetBeamAngle(PHY_INST *instPtr, double offAx, double rndAx)
 {
 	ASSERT(instPtr != NULL);
@@ -360,9 +359,9 @@ int32_t Phy_SetBeamAngle(PHY_INST *instPtr, double offAx, double rndAx)
 	}
 	offAxDeg = offAxDeg * 10;
 	rndAxDeg = rndAxDeg * 10;
-	instPtr->antConfig.oaxis[1] = (uint8_t)offAxDeg;
 	instPtr->antConfig.oaxis[0] = (uint8_t)(offAxDeg >> 8);
-	instPtr->antConfig.raxis[1] = (uint8_t)rndAxDeg;
+	instPtr->antConfig.oaxis[1] = (uint8_t)offAxDeg;
 	instPtr->antConfig.raxis[0] = (uint8_t)(rndAxDeg >> 8);
+	instPtr->antConfig.raxis[1] = (uint8_t)rndAxDeg;
 	return SUCCESS;
 }

df_datalink_sp/src/networking/phy_layer/phy_layer.c

@@ -34,7 +34,6 @@
 #include "xadcps.h"
 #endif
 
-
 /************************** Constant Definitions *****************************/
 #define L1_FRAME_LEN_MAX (8192 / 8)			// byte
 #define L1_FRAME_2KBPS_20MS_LEN 0			// byte
@@ -185,24 +184,25 @@ inline L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr)
 		return L1_NONE_FRAME;
 	}
 	int32_t frame_type = (PhyRead(instPtr, COASE_FREQEST) & COASE_TYPE_MASK);
-	switch (frame_type) {
-		case COASE_TYPE_2KBPS_20MS:
+	switch (frame_type)
+	{
+	case COASE_TYPE_2KBPS_20MS:
 		return L1_2KBPS_20MS_FRAME;
-		case COASE_TYPE_2KBPS_100MS:
+	case COASE_TYPE_2KBPS_100MS:
 		return L1_2KBPS_100MS_FRAME;
-		case COASE_TYPE_10KBPS_10MS:
+	case COASE_TYPE_10KBPS_10MS:
 		return L1_10KBPS_10MS_FRAME;
-		case COASE_TYPE_10KBPS_20MS:
+	case COASE_TYPE_10KBPS_20MS:
 		return L1_10KBPS_20MS_FRAME;
-		case COASE_TYPE_40KBPS_20MS:
+	case COASE_TYPE_40KBPS_20MS:
 		return L1_40KBPS_20MS_FRAME;
-		case COASE_TYPE_40KBPS_10MS:
+	case COASE_TYPE_40KBPS_10MS:
 		return L1_40KBPS_10MS_FRAME;
-		case COASE_TYPE_200KBPS_5MS:
+	case COASE_TYPE_200KBPS_5MS:
 		return L1_200KBPS_5MS_FRAME;
-		case COASE_TYPE_1M2BPS_5MS:
+	case COASE_TYPE_1M2BPS_5MS:
 		return L1_1M2BPS_5MS_FRAME;
-		default:
+	default:
 		return L1_NONE_FRAME;
 	}
 }
@@ -308,6 +308,8 @@ void Phy_UpdateState(PHY_INST *instPtr)
 	{
 		instPtr->isValid = false;
 	}
+
+	// Logging
 	instPtr->recvSlotTime.int_part = PhyRead(instPtr, TM_TARGET_H);
 	instPtr->recvSlotTime.frac_part = PhyRead(instPtr, TM_TARGET_L);
 	instPtr->recvFsTime.int_part = PhyRead(instPtr, FSTIME_H);
@@ -334,7 +336,7 @@ void Phy_UpdateState(PHY_INST *instPtr)
 	instPtr->phyLog.fsyncFreqCh = (PhyRead(instPtr, PHY_FRAME_SYNC_FREQEST) & PHY_FRAME_SYNC_FREQEST_CHANNEL_MASK) >> PHY_FRAME_SYNC_FREQEST_CHANNEL_SHIFT;
 	instPtr->phyLog.deintlvAmpEst = PhyRead(instPtr, PHY_DEINTLV_AMPEST) & PHY_DEINTLV_AMPEST_MASK;
 	memset(instPtr->phyLog.reserve, 0x00, sizeof(instPtr->phyLog.reserve));
-
+	// Sync logs to system monitor
 	SysMonitor_UpdatePhysicalLayerLog(&(instPtr->phyLog));
 }
 
@@ -360,6 +362,7 @@ int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
 	{
 		return ENOMEM;
 	}
+	memset(*instPtr, 0x00, sizeof(PHY_INST));
 
 	memcpy(&((*instPtr)->phyCofig), phyConfig, sizeof(PHY_CONFIG));
 	(*instPtr)->isPhyEn = false;
@@ -468,28 +471,30 @@ int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
 			goto init_error;
 		}
 		Status = XBram_CfgInitialize(&antInfoBram, bramConfigPtr,
-									bramConfigPtr->CtrlBaseAddress);
+									 bramConfigPtr->CtrlBaseAddress);
 		if (Status != SUCCESS)
 		{
 			goto init_error;
 		}
 		/*
-		* Execute the BRAM driver selftest.
-		*/
-		Status = XBram_SelfTest(&antConfigBram, 0);
+		 * Execute the BRAM driver self test.
+		 */
+		Status = XBram_SelfTest(&antInfoBram, 0);
 		if (Status != SUCCESS)
 		{
 			goto init_error;
 		}
-
-		memcpy((void*)antConfigBram.Config.MemBaseAddress,
-				antSubnrrwPattern, sizeof(antSubnrrwPattern));
+		(*instPtr)->antInfoAddr = antInfoBram.Config.MemBaseAddress;
 		uint32_t phyMainCtrl = PhyRead(*instPtr, PHY_MAIN_CTRL);
 		phyMainCtrl &= ~PHY_MAIN_CTRL_NSCAN_MASK;
 		phyMainCtrl |= PHY_MAIN_CTRL_NSCAN_MASK &
-						(L1_NUM_ANT_SUBNRRW_PATTERN << PHY_MAIN_CTRL_NSCAN_SHIFT);
+						(L1_NUM_ANT_NRRW_PATTERN << PHY_MAIN_CTRL_NSCAN_SHIFT);
 		PhyWrite(*instPtr, PHY_MAIN_CTRL, phyMainCtrl);
 	}
+	else
+	{
+		(*instPtr)->antInfoAddr = NULL;
+	}
 
 	/* Initialize the XAdc driver. */
 	XAdcPs_Config *xadcConfigPtr;
@@ -685,52 +690,52 @@ int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
 	}
 	else
 	{
-	switch (frameType)
-	{
-	case L1_2KBPS_20MS_FRAME:
-		phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_20MS;
-		hopNum = L1_FRAME_2KBPS_20MS_HOPS;
-		hopPattern = hopPattern_2KBPS_20MS;
-		break;
-	case L1_2KBPS_100MS_FRAME:
-		phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_100MS;
-		hopNum = L1_FRAME_2KBPS_100MS_HOPS;
-		hopPattern = hopPattern_2KBPS_100MS;
-		break;
-	case L1_10KBPS_10MS_FRAME:
-		phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_10MS;
-		hopNum = L1_FRAME_10KBPS_20MS_HOPS;
-		hopPattern = hopPattern_10KBPS_20MS;
-		break;
-	case L1_10KBPS_20MS_FRAME:
-		phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_20MS;
-		hopNum = L1_FRAME_10KBPS_20MS_HOPS;
-		hopPattern = hopPattern_10KBPS_20MS;
-		break;
-	case L1_40KBPS_20MS_FRAME:
-		phyModeCtrl |= PHY_MODE_CTRL_FRAME_40KBPS_20MS;
-		hopNum = L1_FRAME_40KBPS_20MS_HOPS;
-		hopPattern = hopPattern_40KBPS_20MS;
-		break;
-	case L1_40KBPS_10MS_FRAME:
-		phyModeCtrl |= PHY_MODE_CTRL_FRAME_40KBPS_10MS;
-		hopNum = L1_FRAME_40KBPS_10MS_HOPS;
-		hopPattern = hopPattern_40KBPS_10MS;
-		break;
-	case L1_200KBPS_5MS_FRAME:
-		phyModeCtrl |= PHY_MODE_CTRL_FRAME_200KBPS_5MS;
-		hopNum = L1_FRAME_200KBPS_5MS_HOPS;
-		hopPattern = hopPattern_200KBPS_5MS;
-		break;
-	case L1_1M2BPS_5MS_FRAME:
-		phyModeCtrl |= PHY_MODE_CTRL_FRAME_1M2BPS_5MS;
-		hopNum = L1_FRAME_1M2BPS_5MS_HOPS;
-		hopPattern = hopPattern_1M2BPS_5MS;
-		break;
-	default:
-		return EPARAM;
-		break;
-	}
+		switch (frameType)
+		{
+		case L1_2KBPS_20MS_FRAME:
+			phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_20MS;
+			hopNum = L1_FRAME_2KBPS_20MS_HOPS;
+			hopPattern = hopPattern_2KBPS_20MS;
+			break;
+		case L1_2KBPS_100MS_FRAME:
+			phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_100MS;
+			hopNum = L1_FRAME_2KBPS_100MS_HOPS;
+			hopPattern = hopPattern_2KBPS_100MS;
+			break;
+		case L1_10KBPS_10MS_FRAME:
+			phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_10MS;
+			hopNum = L1_FRAME_10KBPS_20MS_HOPS;
+			hopPattern = hopPattern_10KBPS_20MS;
+			break;
+		case L1_10KBPS_20MS_FRAME:
+			phyModeCtrl |= PHY_MODE_CTRL_FRAME_10KBPS_20MS;
+			hopNum = L1_FRAME_10KBPS_20MS_HOPS;
+			hopPattern = hopPattern_10KBPS_20MS;
+			break;
+		case L1_40KBPS_20MS_FRAME:
+			phyModeCtrl |= PHY_MODE_CTRL_FRAME_40KBPS_20MS;
+			hopNum = L1_FRAME_40KBPS_20MS_HOPS;
+			hopPattern = hopPattern_40KBPS_20MS;
+			break;
+		case L1_40KBPS_10MS_FRAME:
+			phyModeCtrl |= PHY_MODE_CTRL_FRAME_40KBPS_10MS;
+			hopNum = L1_FRAME_40KBPS_10MS_HOPS;
+			hopPattern = hopPattern_40KBPS_10MS;
+			break;
+		case L1_200KBPS_5MS_FRAME:
+			phyModeCtrl |= PHY_MODE_CTRL_FRAME_200KBPS_5MS;
+			hopNum = L1_FRAME_200KBPS_5MS_HOPS;
+			hopPattern = hopPattern_200KBPS_5MS;
+			break;
+		case L1_1M2BPS_5MS_FRAME:
+			phyModeCtrl |= PHY_MODE_CTRL_FRAME_1M2BPS_5MS;
+			hopNum = L1_FRAME_1M2BPS_5MS_HOPS;
+			hopPattern = hopPattern_1M2BPS_5MS;
+			break;
+		default:
+			return EPARAM;
+			break;
+		}
 	}
 
 	// Set frame configs
@@ -740,16 +745,14 @@ int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
 	const uint8_t *dataSrcPtr = (uint8_t *)(&(instPtr->antConfig));
 	for (int32_t i = 0; i < hopNum; i++)
 	{
-		instPtr->antConfig.antFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
-		instPtr->antConfig.antFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
-		instPtr->antConfig.rfFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
-		instPtr->antConfig.rfFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
-		memcpy((uint8_t *)(antConfigAddr + i *ANT_COINFO_LEN),
+		instPtr->antConfig.antFreq[0] = (uint8_t)(hopPattern[i] >> 8);
+		instPtr->antConfig.antFreq[1] = (uint8_t)hopPattern[i];
+		instPtr->antConfig.rfFreq[0] = (uint8_t)(hopPattern[i] >> 8);
+		instPtr->antConfig.rfFreq[1] = (uint8_t)hopPattern[i];
+		memcpy((uint8_t *)(antConfigAddr + i * ANT_COINFO_LEN),
 			   dataSrcPtr, sizeof(PHY_ANT_CONFIG));
 	}
-	uint32_t antCtrl = (instPtr->antConfigOffset & ANT_CONFIG_OFFSET_MASK)
-						| ((hopNum << ANT_CONFIG_NUM_SHIFT) & ANT_CONFIG_NUM_MASK)
-						| ((ANT_COINFO_LEN << ANT_CONFIG_LEN_SHIFT) & ANT_CONFIG_LEN_MASK);
+	uint32_t antCtrl = (instPtr->antConfigOffset & ANT_CONFIG_OFFSET_MASK) | ((hopNum << ANT_CONFIG_NUM_SHIFT) & ANT_CONFIG_NUM_MASK) | ((ANT_COINFO_LEN << ANT_CONFIG_LEN_SHIFT) & ANT_CONFIG_LEN_MASK);
 
 	/*
 		memcpy((uint8_t*)antConfigDataAddr, hopPattern, hopNum*sizeof(uint16_t));
@@ -889,7 +892,6 @@ double Phy_GetFreqEst(PHY_INST *instPtr)
 	L1_FRAME_TYPE frameType;
 	double coarse, fine;
 
-
 	frameType = Phy_GetLastRecvFrameType(instPtr);
 	if (frameType > L1_1M2BPS_5MS_FRAME || frameType <= L1_NONE_FRAME)
 	{
@@ -907,7 +909,7 @@ double Phy_GetFreqEst(PHY_INST *instPtr)
 		coarse = (double)(8 - acqFreqIdx) * (250e3 / 4);
 		coarse += (double)syncFreqIdx * 12e6 / 64 / 128;
 
-		fine = 48e6/2048*((double)estAbs)/8192.0;
+		fine = 48e6 / 2048 * ((double)estAbs) / 8192.0;
 	}
 	else
 	{
@@ -915,11 +917,10 @@ double Phy_GetFreqEst(PHY_INST *instPtr)
 		coarse = (double)(80 - acqFreqIdx) * (31.25e3 / 4);
 		coarse += (double)syncFreqIdx * 1.5e6 / 64 / 128;
 
-		fine = 12e6/2048*((double)estAbs)/8192.0;
+		fine = 12e6 / 2048 * ((double)estAbs) / 8192.0;
 	}
 
-	fine = ((fineReg & FINE_FREQEST_SIGN_MASK) == FINE_FREQEST_SIGN_NEG) ?
-			-fine : fine;
+	fine = ((fineReg & FINE_FREQEST_SIGN_MASK) == FINE_FREQEST_SIGN_NEG) ? -fine : fine;
 
 	return (coarse + fine);
 }
@@ -991,15 +992,14 @@ double Phy_GetFineFreqEst(PHY_INST *instPtr)
 	if (frameType >= L1_200KBPS_5MS_FRAME)
 	{
 		// 12M Rc
-		freqEst = 48e6/2048*((double)estAbs)/8192.0;
+		freqEst = 48e6 / 2048 * ((double)estAbs) / 8192.0;
 	}
 	else
 	{
 		// 1.5M Rc
-		freqEst = 12e6/2048*((double)estAbs)/8192.0;
+		freqEst = 12e6 / 2048 * ((double)estAbs) / 8192.0;
 	}
-	freqEst = ((regValue & FINE_FREQEST_SIGN_MASK) == FINE_FREQEST_SIGN_NEG) ?
-			-freqEst : freqEst;
+	freqEst = ((regValue & FINE_FREQEST_SIGN_MASK) == FINE_FREQEST_SIGN_NEG) ? -freqEst : freqEst;
 	return freqEst;
 }
 
@@ -1130,7 +1130,6 @@ void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotSta
 	memcpy(slotStart, &(instPtr->recvSlotTime), sizeof(TIME_WORD));
 }
 
-
 /*****************************************************************************/
 /**
  * @brief   Copy ans store the received data, if received data valid.
@@ -1270,11 +1269,11 @@ void Phy_RefreshHwSate(PHY_INST *instPtr)
 	rawData = XAdcPs_GetAdcData(&(instPtr->hwXadc), XADCPS_CH_TEMP);
 	evalData = XAdcPs_RawToTemperature(rawData);
 	instPtr->hwSate.spFpgaTemp = (evalData > TEMPERATURE_MAX)
-								? INT8_MAX
-								: ((evalData < TEMPERATURE_MIN) ? INT8_MIN : (int8_t) round(evalData/TEMPERATURE_LSB));
+									 ? INT8_MAX
+									 : ((evalData < TEMPERATURE_MIN) ? INT8_MIN : (int8_t)round(evalData / TEMPERATURE_LSB));
 	instPtr->hwSate.isSpOk = (instPtr->hwSate.spFpgaVolt == (VOLT_PS_OK | VOLT_PSAUX_OK | VOLT_DDR_OK | VOLT_PL_OK | VOLT_PLAUX_OK | VOLT_BRAM_OK))
-						&& (instPtr->hwSate.spClkState == (CLK_SP_PLL0_LOCK | CLK_SP_PLL1_LOCK))
-						&& (instPtr->hwSate.spAdLockState == (CLK_AD0_BBPLL_LOCK | CLK_AD0_RFPLL_LOCK | CLK_AD1_BBPLL_LOCK | CLK_AD1_RFPLL_LOCK));
+															&& (instPtr->hwSate.spClkState == (CLK_SP_PLL0_LOCK | CLK_SP_PLL1_LOCK))
+															&& (instPtr->hwSate.spAdLockState == (CLK_AD0_BBPLL_LOCK | CLK_AD0_RFPLL_LOCK | CLK_AD1_BBPLL_LOCK | CLK_AD1_RFPLL_LOCK));
 #else
 	instPtr->hwSate.isSpOk = true;
 #endif
@@ -1316,7 +1315,7 @@ void Phy_RefreshHwSate(PHY_INST *instPtr)
 
 	// Determin if antenna is ok.
 	instPtr->hwSate.isAntOk = (instPtr->hwSate.antVolt == (VOLT_ANT_PROC_OK | VOLT_ANT_RF_OK | VOLT_ANT_NIL_OK | ANT_STATE_OK))
-								&& (instPtr->hwSate.antPll == (CLK_ANT_BBPLL_LOCK | CLK_ANT_RFPLL0_LOCK | CLK_ANT_RFPLL1_LOCK));
+														&& (instPtr->hwSate.antPll == (CLK_ANT_BBPLL_LOCK | CLK_ANT_RFPLL0_LOCK | CLK_ANT_RFPLL1_LOCK));
 
 	memset(instPtr->hwSate.reserve, 0x00, sizeof(instPtr->hwSate.reserve));
 

df_datalink_sp/src/networking/phy_layer/phy_layer.h

@@ -30,12 +30,12 @@
 #ifndef EMULATION
 #include "xbram.h"
 #include "xadcps.h"
+#include "../config/app_config.h"
 #endif
 #include "../../system_monitor.h"
 #include "../../time_ctrl/time_ctrl.h"
 /************************** Constant Definitions *****************************/
-#define L1_NUM_ANT_SUBNRRW_PATTERN 37
-#define L1_NUM_ANT_NRRW_PATTERN 61
+#define L1_NUM_ANT_NRRW_PATTERN 37
 #define L1_INIT_BEAM_NO 0
 #define L1_INIT_ANT_WD PHY_ANT_BEAM_120
 #define L1_INIT_ANT_NO 0
@@ -148,7 +148,7 @@ typedef enum
 	PHY_ANT_BEAM_120 = 1,
 	PHY_ANT_BEAM_60 = 2,
 	PHY_ANT_BEAM_25 = 3,
-	PHY_ANT_BEAM_8 = 4
+	PHY_ANT_BEAM_16 = 4
 } PHY_ANT_BEAM_WIDTH;
 
 typedef enum
@@ -169,9 +169,9 @@ typedef enum
 typedef enum
 {
 	PHY_ANT_MODE_NONE = 0,
-	PHY_ANT_MODE_SILENT = 1,
-	PHY_ANT_MODE_LOW_POWER = 2,
-	PHY_ANT_MODE_HIGH_POWER = 3,
+	PHY_ANT_MODE_STANDBY = 1,
+	PHY_ANT_MODE_POWER_LOW = 2,
+	PHY_ANT_MODE_POWER_FULL = 3,
 } PHY_ANT_MODE;
 
 typedef struct
@@ -204,6 +204,7 @@ typedef struct
 	uintptr_t recvDataAddr;
 	uintptr_t sendDataAddr;
 	uintptr_t antConfigAddr;
+	uintptr_t antInfoAddr;
 	bool isPhyEn;			  // if physical layer running
 	bool isAcq;				  // if last reception acquisition success
 	bool isFrameSync;		  // if last reception frame sync success
@@ -223,6 +224,7 @@ typedef struct
 #ifndef EMULATION
 	XAdcPs hwXadc;
 #endif
+	PHY_ANT_ANGLE antNrrwPattern[L1_NUM_ANT_NRRW_PATTERN];
 	HWSTATE hwSate;
 	PHYLOG phyLog;
 } PHY_INST;
@@ -239,9 +241,6 @@ extern uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS];
 extern uint16_t hopPattern_200KBPS_5MS[L1_FRAME_200KBPS_5MS_HOPS];
 extern uint16_t hopPattern_1M2BPS_5MS[L1_FRAME_1M2BPS_5MS_HOPS];
 
-extern PHY_ANT_ANGLE antSubnrrwPattern[L1_NUM_ANT_SUBNRRW_PATTERN];
-extern PHY_ANT_ANGLE antNrrwPattern[L1_NUM_ANT_NRRW_PATTERN];
-
 /************************** Function Prototypes ******************************/
 int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig);
 void Phy_Destroy(PHY_INST *instPtr);
@@ -271,8 +270,9 @@ uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType);
 
 void Phy_AntConfigInit(PHY_ANT_CONFIG *antConfig);
 void Phy_AntConfigMode(PHY_INST *instPtr, PHY_ANT_MODE mode);
+void Phy_SetBeamTable(PHY_INST *instPtr, PHY_ANT_ANGLE *table, uint32_t offset);
 int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth);
-int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo);
+int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo, PHY_ANT_BEAM_WIDTH beamWidth);
 int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx);
 int32_t Phy_SetTimeWord(PHY_INST *instPtr, const TIME_WORD *tmWord, PHY_TIME_WORD_TYPE tmType);
 
@@ -284,4 +284,4 @@ void Phy_GetFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT);
 void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT);
 void Phy_GetSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart);
 void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart);
-#endif /* PHY_CTRL_H_ */
+#endif // PHY_CTRL_H_

df_datalink_sp/src/system_monitor.h

@@ -152,7 +152,8 @@ typedef struct __attribute__((packed))
 	double fsyncPAPR;
 	uint8_t fsyncFreqCh;
 	uint16_t deintlvAmpEst;
-	uint8_t reserve[4];
+	uint8_t antMode;
+	uint8_t reserve[3];
 } PHYLOG;
 
 typedef struct __attribute__((packed))

df_datalink_sp/src/systembus/systembus.c

@@ -101,7 +101,8 @@
 // -- Request
 #define WD_EN_CMD 0x02
 #define EN_CMD_ON_HIGH_POWER 0x66
-#define EN_CMD_ON_LOW_POWER 0x88
+#define EN_CMD_ON_LOW_POWER 0x77
+#define EN_CMD_SILENT 0x88
 
 #define WD_MODE_CMD 0x03
 #define MODE_CMD_HEAD 0x11
@@ -194,6 +195,11 @@
 #define MY_PRD_ID PRD_DL_K
 
 /************************** Function Prototypes ******************************/
+static int32_t SystemBus_GenSelfTestRsp(SYSBUS *instPtr);
+static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg);
+static int32_t SystemBus_Echo(SYSBUS *instPtr, SYSBUS_UART_FRAME *frame);
+static int32_t SystemBus_GenPollRsp(SYSBUS *instPtr, uint8_t *msg);
+static int32_t SystemBus_GenRemoteTestRsp(SYSBUS *instPtr, uint8_t *msg);
 /**************************** Type Definitions *******************************/
 /************************** Variable Definitions *****************************/
 // TODO: better approach?
@@ -202,13 +208,13 @@
 extern L2_INST *linkInstPtr;
 extern L2_CONFIG linkConfig;
 
-inline bool SystemBus_IsReqPendding(SYSBUS *instPtr)
+inline bool SystemBus_IsReqPending(SYSBUS *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	return (!isCircQueueEmpty(instPtr->rxQueue));
 }
 
-inline bool SystemBus_IsRsqPendding(SYSBUS *instPtr)
+inline bool SystemBus_IsRsqPending(SYSBUS *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	return (!isCircQueueEmpty(instPtr->txQueue));
@@ -225,6 +231,269 @@ inline bool SystemBus_IsRxBusy(SYSBUS *instPtr)
 	return SysbusUart_IsRxBusy(instPtr->uartInst);
 }
 
+/**
+ * @brief   Generate System Bus SelfTest response.
+ */
+static int32_t SystemBus_GenSelfTestRsp(SYSBUS *instPtr)
+{
+	ASSERT(instPtr != NULL);
+	SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
+	if (sysbusRsp == NULL) {
+		DBG_ERROR("Failed to allocate memory for SELF TEST Response!\n");
+		return ENOMEM;
+	}
+	HWSTATE *hwStatsPtr = &(instPtr->hwState);
+	sysbusRsp->len = ST_RSP_LEN;
+	memset(sysbusRsp->data, 0x00, ST_RSP_LEN);
+	sysbusRsp->data[WD_CMD] = CMD_SELF_TEST;
+	sysbusRsp->data[WD_PRD] = MY_PRD_ID;
+	SysMonitor_GetHardwareStatus(hwStatsPtr);
+	if (linkInstPtr->isEnable == false)
+	{
+		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR;
+	}
+	else
+	{
+		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR;
+	}
+	// apply status and omit 2 bytes of overhead.
+	memcpy(sysbusRsp->data + WD_SP_FPGA_VOLT, &(hwStatsPtr->spFpgaVolt), ST_RET_INFO_LEN);
+
+	if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
+	{
+		free(sysbusRsp);
+		DBG_ERROR("SystemBus::TX Queue overflowed!\n");
+		return EQUEUE;
+	}
+	return SUCCESS;
+}
+
+/**
+ * @brief   Apply system configuration
+ */
+static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(msg != NULL);
+	PHY_INST *phyRef = linkInstPtr->config.phyInstPtr;
+	TMC_INST *tmcRef = linkInstPtr->config.tmcInstPtr;
+	bool reconfig = false;
+	bool start = false;
+
+	if (msg[WD_MODE_CMD] == MODE_CMD_HEAD)
+	{
+		// omit unnecessary reconfiguration
+		if (L2_GetNodeType(linkInstPtr) != L2_NODE_HEAD)
+		{
+			// head node config
+			linkConfig.myType = L2_NODE_HEAD;
+			linkConfig.myId = 1;
+			reconfig = true;
+		}
+	}
+
+	else if (msg[WD_MODE_CMD] == MODE_CMD_MEMBER )
+	{
+		// omit unnecessary reconfiguration
+		if (L2_GetNodeType(linkInstPtr) != L2_NODE_MEMBER)
+		{
+			// member node config
+			linkConfig.myType = L2_NODE_MEMBER;
+			linkConfig.myId = 2;
+			reconfig = true;
+		}
+	}
+	else
+	{
+		// Ignore current command
+		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
+			WD_MODE_CMD, msg[WD_MODE_CMD]);
+		return EPARAM;
+	}
+
+	if(msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
+	{
+		// high power config
+		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL);
+		start = true;
+	}
+	else if(msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
+	{
+		// low power config
+		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
+		start = true;
+	}
+	else if(msg[WD_EN_CMD] == EN_CMD_SILENT)
+	{
+		// silence config
+		// TODO: better approach?
+		start = false;
+		Linklayer_Halt(linkInstPtr);
+		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY);
+		TIME_WORD pullTime = {0, 0};
+		pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1;
+		if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS)
+		{
+			DBG_ERROR("Physical set pull time failed.\n");
+		}
+		Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
+	}
+	if (reconfig == true)
+	{
+		Linklayer_Reconfig(linkInstPtr, &linkConfig);
+	}
+	if (start == true)
+	{
+		Linklayer_Start(linkInstPtr);
+	}
+	instPtr->sysbusLog.lastConfig[0] = msg[WD_MODE_CMD];
+	instPtr->sysbusLog.lastConfig[1] = msg[WD_EN_CMD];
+	return SUCCESS;
+}
+
+/**
+ * @brief   Echo System Bus frame
+ */
+static int32_t SystemBus_Echo(SYSBUS *instPtr, SYSBUS_UART_FRAME *frame)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(frame != NULL);
+	SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
+	if (sysbusRsp == NULL) {
+		DBG_ERROR("Failed to allocate memory for DATA TEST A Response!\n");
+		return ENOMEM;
+	}
+	sysbusRsp->len = frame->len;
+	memcpy(sysbusRsp->data, frame->data, sysbusRsp->len);
+	if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
+	{
+		free(sysbusRsp);
+		DBG_ERROR("SystemBus::TX Queue overflowed!\n");
+		return EQUEUE;
+	}
+	return SUCCESS;
+}
+
+/**
+ * @brief   Generate System Bus poll call command response
+ */
+static int32_t SystemBus_GenPollRsp(SYSBUS *instPtr, uint8_t *msg)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(msg != NULL);
+	size_t dataLen;
+	uint8_t *payload;
+	SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
+	if (sysbusRsp == NULL) {
+		DBG_ERROR("Failed to allocate memory for FLJ POLL Response!\n");
+		return ENOMEM;
+	}
+
+	if(msg[WD_CMD] == CMD_FLJ_POLL)
+	{
+		sysbusRsp->len = FLJ_POLL_RESP_LEN;
+		sysbusRsp->data[WD_CMD] = CMD_FLJ_POLL;
+		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
+		dataLen = PAYLOAD_LEN_FLJ_RX;
+		payload = sysbusRsp->data + FLJ_DATA_OFFSET;
+	}
+	else if(msg[WD_CMD] == CMD_LJ_POLL)
+	{
+		sysbusRsp->len = LJ_POLL_RESP_LEN;
+		sysbusRsp->data[WD_CMD] = CMD_LJ_POLL;
+		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
+		dataLen = PAYLOAD_LEN_LJ_RX;
+		payload = sysbusRsp->data + LJ_DATA_OFFSET;
+	}
+	else
+	{
+		DBG_ERROR("Unexpected command 0x%02X.\n", msg[WD_CMD]);
+		free(sysbusRsp);
+		return EUNEXP;
+	}
+	uint32_t dataId;
+	SysMonitor_GetRecvPayload(payload, &dataId, dataLen); // omit control info
+	if (dataId == DATA_ID_NONE)
+	{
+		free(sysbusRsp);
+		DBG_WARNING("SystemBus::no RX data!\n");
+		return SUCCESS;
+	}
+	if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
+	{
+		free(sysbusRsp);
+		DBG_WARNING("SystemBus::TX Queue overflowed!\n");
+		return EQUEUE;
+	}
+	instPtr->sysbusLog.pollCnt++;
+	return SUCCESS;
+}
+
+/**
+ * @brief   Generate System Bus remote test command response
+ */
+static int32_t SystemBus_GenRemoteTestRsp(SYSBUS *instPtr, uint8_t *msg)
+{
+	ASSERT(instPtr != NULL);
+	ASSERT(msg != NULL);
+	SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
+	if (sysbusRsp == NULL) {
+		DBG_ERROR("Failed to allocate memory for LJ REMOTE TEST Response!\n");
+		return ENOMEM;
+	}
+
+	TIME_WORD tmWord;
+	uint64_t time;
+	TimeCtrl_GetCurrentTime(linkInstPtr->config.tmcInstPtr, &tmWord);
+	time = (uint64_t)tmWord.int_part * TIME_FRAC_MAX + tmWord.frac_part;
+
+	if(msg[WD_CMD] == CMD_FLJ_RM_TEST)
+	{
+		sysbusRsp->len = FLJ_RM_TEST_RESP_LEN;
+		memset(sysbusRsp->data, 0x00, FLJ_RM_TEST_RESP_LEN);
+		sysbusRsp->data[WD_CMD] = CMD_FLJ_RM_TEST;
+		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
+
+		memcpy(sysbusRsp->data + WD_FLJ_TIME_STAMP, &time, WD_FLJ_TIME_STAMP_LEN);
+		SysMonitor_GetLinklayerLog(&(instPtr->linkLog));
+		SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog));
+		SysMonitor_GetHardwareStatus(&(instPtr->hwState));
+		memcpy(sysbusRsp->data + WD_FLJ_SYSBUS_LOG, &(instPtr->sysbusLog), WD_FLJ_SYSBUS_LOG_LEN);
+		memcpy(sysbusRsp->data + WD_FLJ_LINK_LOG, &(instPtr->linkLog), WD_FLJ_LINK_LOG_LEN);
+		memcpy(sysbusRsp->data + WD_FLJ_PHY_LOG, &(instPtr->phyLog), WD_FLJ_PHY_LOG_LEN);
+		memcpy(sysbusRsp->data + WD_FLJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_FLJ_HARDWARE_ST_LEN);
+	}
+	else if (msg[WD_CMD] == CMD_LJ_RM_TEST)
+	{
+		sysbusRsp->len = LJ_RM_TEST_RESP_LEN;
+		sysbusRsp->data[WD_CMD] = CMD_LJ_RM_TEST;
+		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
+
+		memcpy(sysbusRsp->data + WD_LJ_TIME_STAMP, &time, WD_LJ_TIME_STAMP_LEN);
+		SysMonitor_GetLinklayerLog(&(instPtr->linkLog));
+		SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog));
+		SysMonitor_GetHardwareStatus(&(instPtr->hwState));
+		memcpy(sysbusRsp->data + WD_LJ_SYSBUS_LOG, &(instPtr->sysbusLog), WD_LJ_SYSBUS_LOG_LEN);
+		memcpy(sysbusRsp->data + WD_LJ_LINK_LOG, &(instPtr->linkLog), WD_LJ_LINK_LOG_LEN);
+		memcpy(sysbusRsp->data + WD_LJ_PHY_LOG, &(instPtr->phyLog), WD_LJ_PHY_LOG_LEN);
+		memcpy(sysbusRsp->data + WD_LJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_LJ_HARDWARE_ST_LEN);
+	}
+	else
+	{
+		DBG_ERROR("Unexpected command 0x%02X.\n", msg[WD_CMD]);
+		free(sysbusRsp);
+		return EUNEXP;
+	}
+
+	if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){
+		free(sysbusRsp);
+		DBG_WARNING("SystemBus::TX Queue overflowed!\n");
+		return EQUEUE;
+	}
+	instPtr->sysbusLog.remoteCnt++;
+	return SUCCESS;
+}
+
 /*****************************************************************************/
 /**
  * @brief   Initialize System Bus instance.
@@ -297,349 +566,100 @@ int32_t SystemBus_HandleCmd(SYSBUS *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	int32_t Status;
-	SYSBUS_UART_FRAME *sysbusReq, *sysbusRsp;
-	uint8_t *data;
+	SYSBUS_UART_FRAME *sysbusReq;
+	uint8_t *msg;
 	if (CircQueuePop(instPtr->rxQueue, (void **)(&sysbusReq)) == false)
 	{
 		DBG_WARNING("SystemBus::Nothing to be handled.\n");
 		return EIGNORE;
 	}
-	data = sysbusReq->data;
-	if (data[WD_PRD] != MY_PRD_ID && data[WD_PRD] != PRD_ALL)
+	msg = sysbusReq->data;
+	if (msg[WD_PRD] != MY_PRD_ID && msg[WD_PRD] != PRD_ALL)
 	{
 		DBG_WARNING("SystemBus::Production code mismatch!\n");
 		return EIGNORE;
 	}
 
-	switch (data[WD_CMD])
+	switch (msg[WD_CMD])
 	{
 	case CMD_SELF_TEST:
 	{
-		HWSTATE *hwStatsPtr = &(instPtr->hwState);
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		if (sysbusRsp == NULL) {
-			DBG_ERROR("Failed to allocate memory for SELF TEST Response!\n");
-			Status = ENOMEM;
-			break;
-		}
-		sysbusRsp->len = ST_RSP_LEN;
-		memset(sysbusRsp->data, 0x00, ST_RSP_LEN);
-		sysbusRsp->data[WD_CMD] = CMD_SELF_TEST;
-		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
-		SysMonitor_GetHardwareStatus(hwStatsPtr);
-		if (linkInstPtr->isEnable == false)
-		{
-			sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk) ? ST_RET_OK : ST_RET_ERROR;
-		}
-		else
-		{
-			sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk && hwStatsPtr->isAntOk) ? ST_RET_OK : ST_RET_ERROR;
-		}
-		// apply status and omit 2 bytes of overhead.
-		memcpy(sysbusRsp->data + WD_SP_FPGA_VOLT, &(hwStatsPtr->spFpgaVolt), ST_RET_INFO_LEN);
-
-		if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
-		{
-			free(sysbusRsp);
-			DBG_ERROR("SystemBus::TX Queue overflowed!\n");
-			Status = EQUEUE;
-			break;
-		}
-		Status = SUCCESS;
+		Status = SystemBus_GenSelfTestRsp(instPtr);
 		break;
 	}
 	case CMD_DATA_TEST_A:
-	{
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		if (sysbusRsp == NULL) {
-			DBG_ERROR("Failed to allocate memory for DATA TEST A Response!\n");
-			Status = ENOMEM;
-			break;
-		}
-		sysbusRsp->len = DATA_TEST_RSP_A_LEN;
-		memcpy(sysbusRsp->data, sysbusReq->data, DATA_TEST_RSP_A_LEN);
-		if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
-		{
-			free(sysbusRsp);
-			DBG_ERROR("SystemBus::TX Queue overflowed!\n");
-			Status = EQUEUE;
-			break;
-		}
-		Status = SUCCESS;
-		break;
-	}
 	case CMD_DATA_TEST_B:
 	{
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		if (sysbusRsp == NULL) {
-			DBG_ERROR("Failed to allocate memory for DATA TEST B Response!\n");
-			Status = ENOMEM;
-			break;
-		}
-		sysbusRsp->len = DATA_TEST_RSP_B_LEN;
-		memcpy(sysbusRsp->data, sysbusReq->data, DATA_TEST_RSP_B_LEN);
-		if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
-		{
-			free(sysbusRsp);
-			DBG_ERROR("SystemBus::TX Queue overflowed!\n");
-			Status = EQUEUE;
-			break;
-		}
-		Status = SUCCESS;
+		Status = SystemBus_Echo(instPtr, sysbusReq);
 		break;
 	}
 	case CMD_CONFIG:
 	{
-		PHY_INST *phyRef = linkInstPtr->config.phyInstPtr;
-		TMC_INST *tmcRef = linkInstPtr->config.tmcInstPtr;
-
-		if (data[WD_MODE_CMD] == MODE_CMD_HEAD)
-		{
-			// omit unnecessary reconfiguration
-			if (L2_GetNodeType(linkInstPtr) != L2_NODE_HEAD)
-			{
-				// head node config
-				linkConfig.myType = L2_NODE_HEAD;
-				linkConfig.myId = 1;
-				Linklayer_Reconfig(linkInstPtr, &linkConfig);
-			}
-		}
-
-		else if (data[WD_MODE_CMD] == MODE_CMD_MEMBER )
-		{
-			// omit unnecessary reconfiguration
-			if (L2_GetNodeType(linkInstPtr) != L2_NODE_MEMBER)
-			{
-				// member node config
-				linkConfig.myType = L2_NODE_MEMBER;
-				linkConfig.myId = 2;
-				Linklayer_Reconfig(linkInstPtr, &linkConfig);
-			}
-		}
-		else
-		{
-			// FIXME: to confirm
-			// Halt linklayer
-			Linklayer_Halt(linkInstPtr);
-			DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
-				WD_MODE_CMD, data[WD_MODE_CMD]);
-			Status = EPARAM;
-			break;
-		}
-
-		if(data[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
-		{
-			// high power config
-			Phy_AntConfigMode(phyRef, PHY_ANT_MODE_HIGH_POWER);
-		}
-		else if(data[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
-		{
-			// low power config
-			Phy_AntConfigMode(phyRef, PHY_ANT_MODE_LOW_POWER);
-		}
-		else
-		{
-			// silence config
-			// FIXME: confirm
-			// TODO: better approach?
-			Linklayer_Halt(linkInstPtr);
-			Phy_AntConfigMode(phyRef, PHY_ANT_MODE_SILENT);
-			TIME_WORD pullTime = {0, 0};
-			pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1;
-			Status = Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL);
-			if (Status != SUCCESS)
-			{
-				DBG_ERROR("Physical set pull time failed, with error code: %" PRId32 ".\n", Status);
-			}
-			Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
-			break;
-		}
-
-		Linklayer_Start(linkInstPtr);
-		instPtr->sysbusLog.lastConfig[0] = data[WD_MODE_CMD];
-		instPtr->sysbusLog.lastConfig[1] = data[WD_EN_CMD];
-		Status = SUCCESS;
+		Status = SystemBus_SystemConfig(instPtr, msg);
 		break;
 	}
 	case CMD_SYS_INFO:
 	{
-		SysMonitor_UpdateSysinfo((SYSINFO *)(data + SYS_INFO_OFFSET));
+		SysMonitor_UpdateSysinfo((SYSINFO *)(msg + SYS_INFO_OFFSET));
 		Status = SUCCESS;
 		break;
 	}
-	// TODO
 	case CMD_FLJ_SEND:
 	{
-		SysMonitor_UpdateSendPayload(data + FLJ_DATA_OFFSET, PAYLOAD_LEN_FLJ_TX); // omit control info
+		SysMonitor_UpdateSendPayload(msg + FLJ_DATA_OFFSET, PAYLOAD_LEN_FLJ_TX); // omit control info
 		instPtr->sysbusLog.svcCnt++;
 		Status = SUCCESS;
 		break;
 	}
 	case CMD_LJ_SEND:
 	{
-		SysMonitor_UpdateSendPayload(data + LJ_DATA_OFFSET, PAYLOAD_LEN_LJ_TX); // omit control info
+		SysMonitor_UpdateSendPayload(msg + LJ_DATA_OFFSET, PAYLOAD_LEN_LJ_TX); // omit control info
 		instPtr->sysbusLog.svcCnt++;
 		Status = SUCCESS;
 		break;
 	}
 	case CMD_FLJ_POLL:
 	{
-		if (data[WD_FLJ_POLL] != FLJ_POLL_CODE)
+		if (msg[WD_FLJ_POLL] != FLJ_POLL_CODE)
 		{
 			DBG_ERROR("SystemBus::Invalid roll call code!\n");
 			Status = EIGNORE;
 			break;
 		}
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		if (sysbusRsp == NULL) {
-			DBG_ERROR("Failed to allocate memory for FLJ POLL Response!\n");
-			Status = ENOMEM;
-			break;
-		}
-		sysbusRsp->len = FLJ_POLL_RESP_LEN;
-		sysbusRsp->data[WD_CMD] = CMD_FLJ_POLL;
-		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
-		uint32_t dataId;
-		SysMonitor_GetRecvPayload(sysbusRsp->data + FLJ_DATA_OFFSET, &dataId, PAYLOAD_LEN_FLJ_RX); // omit control info
-		if (dataId == DATA_ID_NONE)
-		{
-			free(sysbusRsp);
-			DBG_WARNING("SystemBus::no RX data!\n");
-			Status = SUCCESS;
-			break;
-		}
-		if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
-		{
-			free(sysbusRsp);
-			DBG_WARNING("SystemBus::TX Queue overflowed!\n");
-			Status = EQUEUE;
-			break;
-		}
-		instPtr->sysbusLog.pollCnt++;
-		Status = SUCCESS;
+		Status = SystemBus_GenPollRsp(instPtr, msg);
 		break;
 	}
 	case CMD_LJ_POLL:
 	{
-		if (data[WD_LJ_POLL] != LJ_POLL_CODE)
+		if (msg[WD_LJ_POLL] != LJ_POLL_CODE)
 		{
 			DBG_WARNING("SystemBus::Invalid roll call code!\n");
 			Status = EIGNORE;
 			break;
 		}
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		if (sysbusRsp == NULL) {
-			DBG_ERROR("Failed to allocate memory for LJ POLL Response!\n");
-			Status = ENOMEM;
-			break;
-		}
-		sysbusRsp->len = LJ_POLL_RESP_LEN;
-		sysbusRsp->data[WD_CMD] = CMD_LJ_POLL;
-		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
-		uint32_t dataId;
-		SysMonitor_GetRecvPayload(sysbusRsp->data + LJ_DATA_OFFSET, &dataId, PAYLOAD_LEN_LJ_RX); // omit control info
-		if (dataId == DATA_ID_NONE)
-		{
-			free(sysbusRsp);
-			DBG_WARNING("SystemBus::no RX data!\n");
-			Status = SUCCESS;
-			break;
-		}
-		if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
-		{
-			free(sysbusRsp);
-			DBG_WARNING("SystemBus::TX Queue overflowed!\n");
-			Status = EQUEUE;
-			break;
-		}
-		instPtr->sysbusLog.pollCnt++;
-		Status = SUCCESS;
+		Status = SystemBus_GenPollRsp(instPtr, msg);
 		break;
 	}
 	case CMD_FLJ_RM_TEST:
 	{
-		if (data[WD_FLJ_RM_TEST] != FLJ_RM_TEST_CODE)
+		if (msg[WD_FLJ_RM_TEST] != FLJ_RM_TEST_CODE)
 		{
 			DBG_WARNING("SystemBus::Invalid remote test code!\n");
 			Status = EIGNORE;
 			break;
 		}
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		if (sysbusRsp == NULL) {
-			DBG_ERROR("Failed to allocate memory for FLJ REMOTE TEST Response!\n");
-			Status = ENOMEM;
-			break;
-		}
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		sysbusRsp->len = FLJ_RM_TEST_RESP_LEN;
-		memset(sysbusRsp->data, 0x00, FLJ_RM_TEST_RESP_LEN);
-		sysbusRsp->data[WD_CMD] = CMD_FLJ_RM_TEST;
-		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
-
-		TIME_WORD tmWord;
-		uint64_t time;
-		TimeCtrl_GetCurrentTime(linkInstPtr->config.tmcInstPtr, &tmWord);
-		time = tmWord.int_part * TIME_FRAC_MAX + tmWord.frac_part;
-		memcpy(sysbusRsp->data + WD_FLJ_TIME_STAMP, &time, WD_FLJ_TIME_STAMP_LEN);
-		SysMonitor_GetLinklayerLog(&(instPtr->linkLog));
-		SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog));
-		SysMonitor_GetHardwareStatus(&(instPtr->hwState));
-		memcpy(sysbusRsp->data + WD_FLJ_SYSBUS_LOG, &(instPtr->sysbusLog), WD_FLJ_SYSBUS_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_FLJ_LINK_LOG, &(instPtr->linkLog), WD_FLJ_LINK_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_FLJ_PHY_LOG, &(instPtr->phyLog), WD_FLJ_PHY_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_FLJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_FLJ_HARDWARE_ST_LEN);
-
-		if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){
-			free(sysbusRsp);
-			DBG_WARNING("SystemBus::TX Queue overflowed!\n");
-			Status = EQUEUE;
-			break;
-		}
-		instPtr->sysbusLog.remoteCnt++;
-		Status = SUCCESS;
+		Status = SystemBus_GenRemoteTestRsp(instPtr, msg);
 		break;
 	}
 	case CMD_LJ_RM_TEST:
 	{
-		if (data[WD_LJ_RM_TEST] != LJ_RM_TEST_CODE)
+		if (msg[WD_LJ_RM_TEST] != LJ_RM_TEST_CODE)
 		{
 			DBG_WARNING("SystemBus::Invalid remote test code!\n");
 			Status = EIGNORE;
 			break;
 		}
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		if (sysbusRsp == NULL) {
-			DBG_ERROR("Failed to allocate memory for LJ REMOTE TEST Response!\n");
-			Status = ENOMEM;
-			break;
-		}
-		sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
-		sysbusRsp->len = LJ_RM_TEST_RESP_LEN;
-		memset(sysbusRsp->data, 0x00, LJ_RM_TEST_RESP_LEN);
-		sysbusRsp->data[WD_CMD] = CMD_LJ_RM_TEST;
-		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
-
-		TIME_WORD tmWord;
-		uint64_t time;
-		TimeCtrl_GetCurrentTime(linkInstPtr->config.tmcInstPtr, &tmWord);
-		time = tmWord.int_part * TIME_FRAC_MAX + tmWord.frac_part;
-		memcpy(sysbusRsp->data + WD_LJ_TIME_STAMP, &time, WD_LJ_TIME_STAMP_LEN);
-		SysMonitor_GetLinklayerLog(&(instPtr->linkLog));
-		SysMonitor_GetPhysicalLayerLog(&(instPtr->phyLog));
-		SysMonitor_GetHardwareStatus(&(instPtr->hwState));
-		memcpy(sysbusRsp->data + WD_LJ_SYSBUS_LOG, &(instPtr->sysbusLog), WD_LJ_SYSBUS_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_LJ_LINK_LOG, &(instPtr->linkLog), WD_LJ_LINK_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_LJ_PHY_LOG, &(instPtr->phyLog), WD_LJ_PHY_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_LJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_LJ_HARDWARE_ST_LEN);
-
-		if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false){
-			free(sysbusRsp);
-			DBG_WARNING("SystemBus::TX Queue overflowed!\n");
-			Status = EQUEUE;
-			break;
-		}
-		instPtr->sysbusLog.remoteCnt++;
-		Status = SUCCESS;
+		Status = SystemBus_GenRemoteTestRsp(instPtr, msg);
 		break;
 	}
 	default:
@@ -707,10 +727,9 @@ int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr)
 int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr)
 {
 	ASSERT(instPtr != NULL);
-	int32_t Status;
-	SYSBUS_UART_FRAME *sysbusReq;
 	instPtr->isRxDone = true; // whether reception successes or not, receiving procedure ended.
-	sysbusReq = malloc(sizeof(SYSBUS_UART_FRAME));
+	int32_t Status;
+	SYSBUS_UART_FRAME *sysbusReq = malloc(sizeof(SYSBUS_UART_FRAME));
 	if (sysbusReq == NULL)
 	{
 		return ENOMEM;

df_datalink_sp/src/systembus/systembus.h

@@ -57,8 +57,8 @@ int32_t SystemBus_MakeResp(SYSBUS *instPtr);
 int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr);
 int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr);
 
-bool SystemBus_IsReqPendding(SYSBUS *instPtr);
-bool SystemBus_IsRsqPendding(SYSBUS *instPtr);
+bool SystemBus_IsReqPending(SYSBUS *instPtr);
+bool SystemBus_IsRsqPending(SYSBUS *instPtr);
 
 bool SystemBus_IsTxBusy(SYSBUS *instPtr);
 bool SystemBus_IsRxBusy(SYSBUS *instPtr);

df_datalink_sp/src/systembus/systembus_uart.c

@@ -30,9 +30,9 @@
 #define UART_STATUS_TX_BUSY 0x00010000
 #define UART_STATUS_RX_BUSY 0x01000000
 // Data buffer register
-#define UART_TXBUFF_LOW 1024
+#define UART_TXBUFF_OFFSET 1024
 #define UART_TXBUFF_MAX 1024
-#define UART_RXBUFF_LOW 2048
+#define UART_RXBUFF_OFFSET 2048
 #define UART_RXBUFF_MAX 1024
 
 // Frame Data offsets
@@ -133,7 +133,7 @@ int SysbusUart_Send(SYSBUS_UART *instPtr, SYSBUS_UART_FRAME *frame)
 		free(sendBuff);
 		return FAILURE;
 	}
-	memcpy((void *)(instPtr->config.baseAddr + UART_TXBUFF_LOW), sendBuff, (sendLen + sizeof(uint32_t) - 1) / sizeof(uint32_t) * sizeof(uint32_t));
+	memcpy((void *)(instPtr->config.baseAddr + UART_TXBUFF_OFFSET), sendBuff, (sendLen + sizeof(uint32_t) - 1) / sizeof(uint32_t) * sizeof(uint32_t));
 	free(sendBuff);
 	// Start Sending
 	SYSTEMBUS_UART_mWriteReg(instPtr->config.baseAddr, UART_SEND_CMD, sendLen);
@@ -169,7 +169,7 @@ int SysbusUart_Recv(SYSBUS_UART *instPtr, SYSBUS_UART_FRAME *frame)
 	{
 		return ENOMEM;
 	}
-	memcpy(dBuff, (void *)(instPtr->config.baseAddr + UART_RXBUFF_LOW), sizeof(uint32_t) * dWLen);
+	memcpy(dBuff, (void *)(instPtr->config.baseAddr + UART_RXBUFF_OFFSET), sizeof(uint32_t) * dWLen);
 	plBuff = dBuff + FRAME_HEAD_LEN;
 	checkSumCal = 0;
 	checkSum = plBuff[dLen + FRAME_DUMMY_LEN];

df_datalink_sp/src/systembus/systembus_uart.h

@@ -73,10 +73,9 @@ typedef struct
 
 typedef struct
 {
-	uint8_t len;
+	uint16_t len;
 	uint8_t checkSum;
 	uint8_t isValid;
-	uint8_t dummy;
 	uint8_t data[256];
 } SYSBUS_UART_FRAME;