feat: 完成上位机控制逻辑修改

仅在业务阶段传输上位机数据, 上位机下发数据入队, 待发送时隙进行发送, 收到数据自动上传

doc/HSJL 上位机串口通信协议.md

@@ -1,18 +1,24 @@
 # HSJL 上位机串口通信协议
 
-> 版本：v1.0
+> 版本：v1.2
-> 日期：2025 年 6 月 5 日
+> 日期：2025 年 6 月 12 日
 
 ---
 
 ## 总览
 
+HSL 上位机服务于 HSJL 演示验证，用于演示 3 节点组网通信功能（1 个主节点，2 个从节点）。
+
 - 物理接口：UART（串口通信）
 - 通信方式：PC 主动下发控制帧，节点响应或上传数据
 - 支持功能：
-  - 指令控制
+  - 状态查询：查询板卡的硬件状态
-  - 图像数据传输
+  - 指令控制：业务速率切换、节点角色切换、时隙表切换等
-  - 节点遥测与状态上报
+  - 图像数据传输：
+    - 业务部分 Payload 为图像数据，数据流向为 PC 下发 → 空口发送 → 空口接收 → 节点上传。
+    - 对于较大的图片，需要 PC 进行数据包切割与重组。
+    - 业务流向为主-从双向，主节点需要向不同从节点发送数据，不同从节点也会给主节点发送数据。
+  - （可选）节点遥测：读取节点工作状态日志，并进行存储。
   
 - UART 串口配置
 
@@ -28,25 +34,31 @@
 
 每帧数据通过串口独立传输，格式如下：
 
+``` mermaid
+block-beta
+  columns 6
+  a["帧头"] b["数据段"] c["Dummy Byte"] d["校验和"] e["数据段长度"] f["帧尾"]
 ```
-[帧头][数据段][Dummy Byte][校验和][数据长度][帧尾]
+
+```
+[帧头][数据段][Dummy Byte][校验和][数据段长度][帧尾]
 ```
 
 | 字段名     | 字节数 (Byte) | 说明 |
 |--|--|--|
-| 帧头       | 2 | 固定值：`0x5A 0x54` |
+| 帧头       | 2 | 固定值：`0x54 0x5A` |
-| 数据段    | ≤1000 | 业务数据内容 |
+| 数据段    | ≤950 | 业务数据内容 |
 | Dummy Byte | 1 | 固定填充：`0x00` |
 | 校验和     | 1 | 所有**数据段**字节累加和（无符号累加） |
 | 数据段长度 | 2 | 数据段字节数（不含 Dummy/校验/帧尾） |
-| 帧尾       | 2 | 固定值：`0x5A 0xFE` |
+| 帧尾       | 2 | 固定值：`0xFE 0x5A` |
 
 ### 帧内冲突规避规则
 
-为防止数据段中出现帧尾 `0x5A 0xFE`：
+为防止数据段中出现帧尾 `0xFE 0x5A`：
 
-- 若数据段中出现 `0x5A 0xFE`，需插入 `0x00` 变成 `0x00 0x5A 0xFE`
+- 若数据段中出现 `0xFE 0x5A`，需插入 `0x00` 变成 `0x00 0xFE 0x5A`
-- 接收端收到 `0x00 0x5A 0xFE` 还原为 `0x5A 0xFE`
+- 接收端收到 `0x00 0xFE 0x5A` 还原为 `0xFE 0x5A`
 
 ### 帧长说明
 
@@ -57,6 +69,14 @@
 
 数据段格式如下，下发（PC→节点）和上传（节点→PC）均使用一致的格式：
 
+``` mermaid
+block-beta
+  columns 8
+  HEAD["帧头"]:1 DATA["数据段"]:3 DUMMY["Dummy Byte"]:1 CHECKSUM["校验和"]:1 LENGTH["数据段长度"]:1 TAIL["帧尾"]:1
+  
+  NULL11[" "]:1 CMD["指令"]:1 TYPE["节点类型"]:1 PAYLOAD["Payload"]:1 NULL12[" "]:1 NULL13[" "]:1 NULL14[" "]:1 NULL15[" "]:1
+```
+
 ```
 [指令][节点类型][Payload]
 ```
@@ -65,29 +85,29 @@
 | -------- | ------------- | ---------------- |
 | 指令     | 1             | 控制指令         |
 | 节点类型 | 1             | 固定填充：`0xEE` |
-| Payload  | ≤1000         | 由指令类型决定   |
+| Payload  | ≤950          | 由指令类型决定   |
 
 指令类型如下：
 
 | 指令   | 含义                  | 下发 Payload 长度 (Byte) | 上传 Payload 长度 (Byte) |
 | ------ | --------------------- | ------------------------ | ------------------------ |
 | `0xE1` | 自检                  | 0, payload 无数据        | 21                       |
-| `0xE2` | Echo 测试             | ≤1000                    | 与下发长度一致           |
+| `0xE2` | Echo 测试             | ≤950                     | 与下发长度一致           |
 | `0xE3` | 节点配置              | 4                        | None，无上传             |
 | `0x33` | 系统信息 (目前可忽略) | 54                       | None，无上传             |
-| `0xE4` | 发送数据下发          | ≤1000                    | None，无上传             |
+| `0xE4` | 发送数据下发          | ≤950                     | None，无上传             |
-| `0xE5` | 接收数据上传          | None，无下发，自动上传   | ≤1000                    |
+| `0xE5` | 接收数据上传          | None，无下发，自动上传   | ≤950                     |
 | `0xE6` | 遥测                  | 0, payload 无数据        | 188                      |
 
 ### 自检指令
 
-自检指令上传的 Payload 格式如下，顺序存储，共 19 字节：
+自检指令上传的 Payload 格式如下，顺序存储，共 21 字节：
 
 | 字段名                | 字节数 (Byte) | 说明                                                         |
-| ------------------------- | ------------- | ------------------------------------------------------------ |
+| --------------------- | ------------- | ------------------------------------------------------------ |
 | 节点状态              | 1             | `0xAA`: 正常<br />`0x55`: 异常                               |
 | FPGA 电压状态         | 1             | `[0]`: VCCPINT 电源轨道状态<br />`[1]`: VCCPAUX 电源轨道状态<br />`[2]`: VCCO_DDR 电源轨道状态<br />`[3]`: VCCINT 电源轨道状态<br />`[4]`: VCCAUX 电源轨道状态<br />`[5]`: VCCBRAM 电源轨道状态<br />以上 `1` 表示正常，`0` 表示异常 |
-| (未使用) 时钟芯片锁定状态 | 1             | `[0]`:  PLL1 锁定状态<br />`[2]`: PLL2 锁定状态<br />以上 `1` 表示锁定，`0` 表示失锁 |
+| 时钟芯片锁定状态      | 1             | `[0]`:  PLL1 锁定状态<br />`[2]`: PLL2 锁定状态<br />以上 `1` 表示锁定，`0` 表示失锁 |
 | (未使用) ADC 锁定状态 | 1             | `[0]`: AD-1 BBPLL 锁定状态<br />`[2]`: AD-2 BBPLL 锁定状态<br />`[4]`: AD-1 RFPLL 锁定状态<br />`[6]`: AD-2 RFPLL 锁定状态<br />以上 `1` 表示锁定，`0` 表示失锁 |
 | (未使用) ADC-1 温度   | 1, int8       | 温度值，有符号数                                             |
 | (未使用) ADC-2 温度   | 1, int8       | 温度值，有符号数                                             |
@@ -104,28 +124,73 @@
 
 ### Echo 测试
 
-Echo 测试中节点将 PC 下发的数据直接回传，不做其他处理。
+Echo 测试中节点将 PC 下发的数据直接回传，不做其他处理。Echo 测试数据的 Payload 格式如下，顺序存储，长度可变。
+
+| 字段名   | 字节数 (Byte) | 说明                   |
+| -------- | ------------- | ---------------------- |
+| 数据长度 | 2             | 数据字节数             |
+| 数据     | ≤950          | 长度由数据长度字段决定 |
 
 ### 节点配置指令
 
-节点配置指令下发的 Payload 格式如下，顺序存储，共 19 字节：
+节点配置指令下发的 Payload 格式如下，顺序存储，共 4 字节：
 
 | 字段名               | 字节数 (Byte) | 说明                                                       |
-| ---------- | ------------- | ----------------------------------------------------------- |
+| -------------------- | ------------- | ---------------------------------------------------------- |
 | 节点使能             | 1             | `0xAA`: 启用<br />`0x55`: 停用                             |
-| 节点角色   | 1             | `0x11`: 簇首<br />`0x00`: 成员                              |
+| 节点 ID              | 1             | `0x01`: 簇首<br />`0x02`~`0x0A`: 成员                      |
 | 业务速率             | 1             | `0x22`: 128 kbps<br />`0x33`: 256 kbps<br />`0x44`: 2 Mbps |
-| 时隙表编号 | 1             | 编号值，0~2                                                 |
+| 时隙表编号（可扩展） | 1             | 编号值，0~2（可扩展）                                      |
+
+> [!warning]
+>
+> 时隙表单次运行周期为 250 ms，不同时隙表中主从节点数据交互次数、可用从节点 ID 范围不同，**从节点只与主节点进行通信**，目前暂定时隙表如下：
+>
+> - 0 号时隙表，主节点分别给 2 个从节点发送 1 次；每个从节点给主节点发送 1 次。
+> - 1 号时隙表，主节点分别给 2 个从节点发送 4 次；每个从节点给主节点发送 4 次。
+> - 2 号时隙表，待定。
 
 ### 发送数据下发
 
-需注意整个 UART 帧的长度（包括帧头、数据段、Dummy、校验和、数据段长度、帧尾）不超过 1024 Byte。
+发送数据的 Payload 格式如下，顺序存储，长度可变。
+
+```mermaid
+block-beta
+  columns 11
+  HEAD["帧头"]:1 DATA_CHUNK["数据段"]:6 DUMMY["Dummy Byte"]:1 CHECKSUM["校验和"]:1 LENGTH["数据段长度"]:1 TAIL["帧尾"]:1
+  
+  NULL11[" "]:1 CMD["指令"]:1 TYPE["节点类型"]:1 PAYLOAD["Payload"]:4 NULL12[" "]:1 NULL13[" "]:1 NULL14[" "]:1 NULL15[" "]:1
+  
+  NULL21[" "]:1 NULL22[" "]:1 NULL23[" "]:1 DATAID["数据 ID"]:1 DSTID["目的节点 ID"]:1 D_LEN["数据长度"]:1 DATA["数据"]:1 NULL24[" "]:1 NULL25[" "]:1 NULL26[" "]:1 NULL27[" "]:1
+```
+
+| 字段名      | 字节数 (Byte) | 说明                    |
+| ----------- | ------------- | ----------------------- |
+| 数据 ID     | 4             | 数据编号                |
+| 目的节点 ID | 1             | 消息目的节点 ID         |
+| 数据长度    | 2             | 数据长度（不含数据 ID） |
+| 数据        | ≤950          | 长度由数据长度字段决定  |
+
+> [!Warning]
+>
+> 1. 需注意整个 [UART 帧](#UART 帧结构定义)的长度（包括帧头、数据段、Dummy、校验和、数据段长度、帧尾）不超过 1024 Byte。
+> 2. 数据最大长度同时由业务速率档位决定：
+>    - 128 kbps 档位：数据长度最大为 40 Byte。
+>    - 512 kbps 档位：数据长度最大为 265 Byte。
+>    - 2 Mbps 档位：数据长度最大为 1220 Byte，但是受限于 PL buffer 的长度，[UART 帧](#UART 帧结构定义)的长度不超过 1024 Byte。
 
 ### 接收数据上传
 
-接收到的数据将自动打包上传，无需下发指令。
+接收到的数据将自动打包上传，无需下发指令。接收数据的 Payload 格式与发送的格式一致，顺序存储，长度可变。
 
-### 遥测指令
+| 字段名    | 字节数 (Byte) | 说明                    |
+| --------- | ------------- | ----------------------- |
+| 数据 ID   | 4             | 数据编号                |
+| 源节点 ID | 1             | 消息来源节点 ID         |
+| 数据长度  | 2             | 数据长度（不含数据 ID） |
+| 数据      | ≤950          | 长度由数据长度字段决定  |
+
+### （可选）遥测指令
 
 遥测指令上传的 Payload 由 5 部分组成，顺序存储，共 188 字节：
 

src/hwconfig.c

@@ -511,7 +511,7 @@ uint8_t Hardware_GetLMK04828LockStatus(void)
 	ret |= ((regVal & 0x02) == 0x02) ? CLK_SP_PLL1_LOCK : 0;
 
 	// pll2
-	lmk04828_read(ad9528_inst, 0x0183, &regVal); // [D1] is lock indicator
+	lmk04828_read(lmk04828_inst, 0x0183, &regVal); // [D1] is lock indicator
 	ret |= ((regVal & 0x02) == 0x02) ? CLK_SP_PLL2_LOCK : 0;
 
 	return ret;

src/hwconfig.h

@@ -27,4 +27,5 @@ int8_t Hardware_GetAD9361Temp_1(void);
 int8_t Hardware_GetAD9361Temp_2(void);
 uint8_t Hardware_GetAD9361LockStatus(void);
 uint8_t Hardware_GetHMC7044LockStatus(void);
+uint8_t Hardware_GetAD9528LockStatus(void);
 uint8_t Hardware_GetLMK04828LockStatus(void);

src/main.c

@@ -124,14 +124,19 @@ TMC_CONFIG tmcConfig = {
 
 static SYSBUS_CONFIG sysbusConfig = {
 	.baseAddr = SYSBUS_BASE_ADDR,
-	.txQueueMax = 16U,
+	.txQueueMax = 32U,
-	.rxQueueMax = 16U
+	.rxQueueMax = 32U
+};
+
+static SYSMON_CONFIG sysmonConfig = {
+	.sendQueueMax = 32U,
+	.recvQueueMax = 32U
 };
 
 /*********************** Driver Instance Definitions **************************/
 static struct irq_ctrl_desc *intcDescPtr = NULL; // General interrupt controller descriptor
 static struct timer_desc *logTmPtr = NULL;
-static SYSBUS *sysbusDescPtr = NULL;
+SYSBUS *sysbusDescPtr = NULL;
 static PHY_INST *phyInstPtr = NULL;
 static TMC_INST *tmcInstPtr = NULL;
 L2_INST *linkInstPtr = NULL;
@@ -448,7 +453,7 @@ int main(void)
 	DBG_PRINT("Application Version: %2d.%2d.%4d.\n", _APP_VER_MAIN, _APP_VER_MINOR, _APP_VER_PATCH);
 
 	/* System Monitor Initialize */
-	Status = SysMonitor_Init();
+	Status = SysMonitor_Init(&sysmonConfig);
 	if (Status == SUCCESS)
 	{
 		DBG_PRINT("System monitor Init SUCCESS! \n");
@@ -555,7 +560,7 @@ int main(void)
 				DBG_ERROR("SystemBus_HandleCmd exit with ErrorCode: %" PRId32 ".\n", Status);
 			}
 		}
-		if (SystemBus_IsRsqPending(sysbusDescPtr) && (!SystemBus_IsTxBusy(sysbusDescPtr)))
+		if (SystemBus_IsRespPending(sysbusDescPtr) && (!SystemBus_IsTxBusy(sysbusDescPtr)))
 		{
 			Status = SystemBus_MakeResp(sysbusDescPtr);
 			if (Status != SUCCESS)

src/networking/link_layer/link_layer.c

@@ -22,7 +22,6 @@
 #include "link_layer.h"
 #include "link_layer_types.h"
 #include "node_list.h"
-#include "msg_def.h"
 #include "error.h"
 #include "common.h"
 #include "util.h"
@@ -890,8 +889,8 @@ int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event)
 		else if (instPtr->nextState == L2_NORMAL_STATE)
 		{
 			UpdateMainSate(instPtr, L2_NORMAL_STATE);
-			instPtr->rtneConfig.rtneTable = normalTable;
+			instPtr->rtneConfig.rtneTable = normalTable0;
-			instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable);
+			instPtr->rtneConfig.rtnePrd = ARRAY_SIZE(normalTable0);
 		}
 		else
 		{

src/networking/link_layer/link_layer.h

@@ -24,6 +24,7 @@
 #include "link_layer_types.h"
 #include "schdevent.h"
 #include "node_list.h"
+#include "msg_def.h"
 #include "circ_queue.h"
 #include "../phy_layer/phy_layer.h"
 #include "../../system_monitor.h"
@@ -92,8 +93,8 @@ typedef struct
 	int64_t timeDev;	   ///< last time deviation estimation result
 	uint32_t reconnectCnt; ///< reconnect trial count
 
-	PAYLOAD rxLog;
+	L2_MSG_RECORD rxLog;
-	PAYLOAD txLog;
+	L2_MSG_RECORD txLog;
 } L2_LOG;
 
 /**
@@ -129,7 +130,9 @@ typedef struct
 
 /************************** Variable Definitions *****************************/
 extern L2_RTNE_TABLE_ENTRY *initTable[L2_INIT_RTNE_PERIOD];
-extern L2_RTNE_TABLE_ENTRY *normalTable[L2_NORMAL_RTNE_PERIOD];
+extern L2_RTNE_TABLE_ENTRY *normalTable0[L2_INIT_RTNE_PERIOD];
+extern L2_RTNE_TABLE_ENTRY *normalTable1[L2_INIT_RTNE_PERIOD];
+extern L2_RTNE_TABLE_ENTRY *normalTable2[L2_INIT_RTNE_PERIOD];
 /************************** Function Prototypes ******************************/
 /**
  * @defgroup LinkLayer_Main

src/networking/link_layer/link_layer_types.h

@@ -36,7 +36,9 @@
 #define L2_BROADCAST_ID1 0xF1 // broadcast
 #define L2_BROADCAST_ID2 0xF2
 #define L2_BROADCAST_ID3 0xF4
+#define L2_UNICAST_ID 0xF0
 #define L2_NONE_ID 0
+#define L2_HEAD_ID 0x01
 
 // Routine configure
 #define L2_INIT_RTNE_PERIOD 1
@@ -206,7 +208,6 @@ typedef struct
 	uint8_t dst;			   ///< destination node ID
 } L2_RTNE_TABLE_ENTRY;
 
-
 /**
  * @struct L2_SCAN_FSM
  * @brief Linklayer scan process FSM instance

src/networking/link_layer/msg_def.h

@@ -25,11 +25,11 @@
 
 #define L2_PACK_LEN_MAX 2048 // bytes
 
-#define L2_PAYLOAD_LEN_ADV 17
+#define L2_PAYLOAD_LEN_ADV 15
-#define L2_PAYLOAD_LEN_ACK 9
+#define L2_PAYLOAD_LEN_ACK 7
-#define L2_PAYLOAD_LEN_SVC_128K 42
+#define L2_PAYLOAD_LEN_SVC_128K 40
-#define L2_PAYLOAD_LEN_SVC_512K 267
+#define L2_PAYLOAD_LEN_SVC_512K 265
-#define L2_PAYLOAD_LEN_SVC_2M 1222
+#define L2_PAYLOAD_LEN_SVC_2M 1220
 #define L2_MSG_CRC_LEN 1
 
 
@@ -93,8 +93,9 @@ typedef struct
 	float srcPosY;			///< position y
 	float srcPosZ;			///< position z
 	uint32_t srcNetStatTime;///< net start time
-	uint32_t advDataId;		///< advertise payload id
+	uint32_t dataId;		///< advertise payload id
-	uint8_t advData[L2_PAYLOAD_LEN_ADV];	///< advertise payload data
+	uint16_t dataLen;		///< advertise payload length
+	uint8_t data[L2_PAYLOAD_LEN_ADV];	///< advertise payload data
 	uint8_t crc;			///< crc
 } L2_MSG_ADV;
 
@@ -111,8 +112,9 @@ typedef struct
 	float srcSpeedX;		///< speed x
 	float srcSpeedY;		///< speed y
 	float srcSpeedZ;		///< speed z
-	uint32_t ackDataId;		///< acknowledge payload id
+	uint32_t dataId;		///< acknowledge payload id
-	uint8_t ackData[L2_PAYLOAD_LEN_ACK];		///< acknowledge payload data
+	uint16_t dataLen;		///< acknowledge payload length
+	uint8_t data[L2_PAYLOAD_LEN_ACK];		///< acknowledge payload data
 	uint8_t crc;			///< crc
 } L2_MSG_ACK;
 
@@ -129,8 +131,9 @@ typedef struct
 	float srcSpeedX;		///< speed x
 	float srcSpeedY;		///< speed y
 	float srcSpeedZ;		///< speed z
-	uint32_t svcDataId;		///< service payload id
+	uint32_t dataId;		///< service payload id
-	uint8_t svcData[L2_PAYLOAD_LEN_SVC_128K];	///< service payload data
+	uint16_t dataLen;		///< service payload length
+	uint8_t data[L2_PAYLOAD_LEN_SVC_128K];	///< service payload data
 	uint8_t crc;			///< crc
 } L2_MSG_SVC_128K;
 
@@ -147,8 +150,9 @@ typedef struct
 	float srcSpeedX;		///< speed x
 	float srcSpeedY;		///< speed y
 	float srcSpeedZ;		///< speed z
-	uint32_t svcDataId;		///< service payload id
+	uint32_t dataId;		///< service payload id
-	uint8_t svcData[L2_PAYLOAD_LEN_SVC_512K];	///< service payload data
+	uint16_t dataLen;		///< service payload length
+	uint8_t data[L2_PAYLOAD_LEN_SVC_512K];	///< service payload data
 	uint8_t crc;			///< crc
 } L2_MSG_SVC_512K;
 
@@ -165,13 +169,24 @@ typedef struct
 	float srcSpeedX;		///< speed x
 	float srcSpeedY;		///< speed y
 	float srcSpeedZ;		///< speed z
-	uint32_t svcDataId;		///< service payload id
+	uint32_t dataId;		///< service payload id
-	uint8_t svcData[L2_PAYLOAD_LEN_SVC_2M];	///< service payload data
+	uint16_t dataLen;		///< service payload length
+	uint8_t data[L2_PAYLOAD_LEN_SVC_2M];	///< service payload data
 	uint8_t crc;			///< crc
 } L2_MSG_SVC_2M;
 
 #pragma pack(pop) // Restore original packing
 
+/**
+ * @struct SYSMON_SVC_PAYLOAD
+ * @brief Payload struct that holds the entire data packet
+ */
+typedef struct
+{
+	uint32_t dataId;  ///< data identifier
+	uint8_t data[L2_PACK_LEN_MAX];
+} L2_MSG_RECORD;
+
 #endif //__L2_MSG_DEF_H_
 
 /** @} LinkLayer_MSG_Constants*/

src/networking/link_layer/routine_table.c

@@ -25,7 +25,7 @@
 #include "link_layer.h"
 #include "link_layer_types.h"
 
-L2_RTNE_TABLE_ENTRY innerScanSubT[L2_SLOTS_PER_FRAME] =
+L2_RTNE_TABLE_ENTRY tableFor10Node[L2_SLOTS_PER_FRAME] =
 {
 	// 36 slots: advertise of antenna-1 ~ antenna-3
 	// 12 slots: idle for no antenna-4
@@ -119,20 +119,20 @@ L2_RTNE_TABLE_ENTRY innerScanSubT[L2_SLOTS_PER_FRAME] =
 	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x0A, 0x01},
 	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x0A, 0x01},
 	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x0A, 0x01},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x02},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x02},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x03},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x03},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x04},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x04},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x05},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x05},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x06},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x06},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x07},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x07},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x08},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x08},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
 
 	// 12 slots: network management, head <--> node
 	// 18 slots: message distribution, head ---> node
@@ -180,14 +180,14 @@ L2_RTNE_TABLE_ENTRY innerScanSubT[L2_SLOTS_PER_FRAME] =
 	{L2_SLOT_RTT, 0x01, 0x03, 0x01},
 	{L2_SLOT_RTT, 0x00, 0x04, 0x01},
 	{L2_SLOT_RTT, 0x01, 0x04, 0x01},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x09},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x09},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x0A},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x0A},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x01, 0x0B},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x01, 0x0B},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x02, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x02, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x02, 0x01},
+	{L2_SLOT_SERVICE, 0x01, 0x02, L2_UNICAST_ID},
 
 	// 12 slots: network management, head <--> node
 	// 18 slots: message distribution, head ---> node
@@ -235,14 +235,14 @@ L2_RTNE_TABLE_ENTRY innerScanSubT[L2_SLOTS_PER_FRAME] =
 	{L2_SLOT_RTT, 0x01, 0x06, 0x01},
 	{L2_SLOT_RTT, 0x00, 0x07, 0x01},
 	{L2_SLOT_RTT, 0x01, 0x07, 0x01},
-	{L2_SLOT_SERVICE, 0x00, 0x03, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x03, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x03, 0x01},
+	{L2_SLOT_SERVICE, 0x01, 0x03, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x04, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x04, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x04, 0x01},
+	{L2_SLOT_SERVICE, 0x01, 0x04, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x05, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x05, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x05, 0x01},
+	{L2_SLOT_SERVICE, 0x01, 0x05, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x06, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x06, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x06, 0x01},
+	{L2_SLOT_SERVICE, 0x01, 0x06, L2_UNICAST_ID},
 
 	// 12 slots: network management, head <--> node
 	// 18 slots: message distribution, head ---> node
@@ -290,24 +290,309 @@ L2_RTNE_TABLE_ENTRY innerScanSubT[L2_SLOTS_PER_FRAME] =
 	{L2_SLOT_RTT, 0x01, 0x09, 0x01},
 	{L2_SLOT_RTT, 0x00, 0x0A, 0x01},
 	{L2_SLOT_RTT, 0x01, 0x0A, 0x01},
-	{L2_SLOT_SERVICE, 0x00, 0x08, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x08, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x08, 0x01},
+	{L2_SLOT_SERVICE, 0x01, 0x08, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x09, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x09, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x09, 0x01},
+	{L2_SLOT_SERVICE, 0x01, 0x09, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x00, 0x0A, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x0A, L2_UNICAST_ID},
-	{L2_SLOT_SERVICE, 0x01, 0x0A, 0x01},
+	{L2_SLOT_SERVICE, 0x01, 0x0A, L2_UNICAST_ID},
 	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
 	{L2_SLOT_IDLE, 0x00, 0x00, 0x00}
 };
 
-L2_RTNE_TABLE_ENTRY *initTable[L2_INIT_RTNE_PERIOD] =
+L2_RTNE_TABLE_ENTRY tableFor3Node[L2_SLOTS_PER_FRAME] =
 {
-	innerScanSubT
+	// 36 slots: advertise of antenna-1 ~ antenna-3
+	// 12 slots: idle for no antenna-4
+	// 2 slots: service slot, set to idle
+	{L2_SLOT_ADVERTISE, 0x00, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x01, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x02, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x03, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x04, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x05, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x06, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x07, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x08, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x09, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x0A, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x0B, 0x01, L2_BROADCAST_ID1},
+	{L2_SLOT_ADVERTISE, 0x00, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x01, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x02, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x03, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x04, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x05, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x06, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x07, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x08, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x09, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x0A, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x0B, 0x01, L2_BROADCAST_ID2},
+	{L2_SLOT_ADVERTISE, 0x00, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x01, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x02, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x03, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x04, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x05, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x06, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x07, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x08, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x09, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x0A, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_ADVERTISE, 0x0B, 0x01, L2_BROADCAST_ID3},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+
+	// 36 slots: acknowledge of member nodes
+	// 14 slots: service
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x02, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x02, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x02, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x02, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x03, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x03, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x03, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x03, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x04, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x04, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x04, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x04, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x05, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x05, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x05, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x05, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x06, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x06, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x06, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x06, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x07, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x07, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x07, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x07, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x08, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x08, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x08, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x08, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x09, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x09, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x09, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x09, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x00, 0x0A, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x01, 0x0A, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x02, 0x0A, 0x01},
+	{L2_SLOT_ACKNOWLEDGE, 0x03, 0x0A, 0x01},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_IDLE, 0x00, 0x00, 0x00},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
+
+	// 12 slots: network management, head <--> node
+	// 18 slots: message distribution, head ---> node
+	// 12 slots: rtt, head <--> node
+	// 8 slots: service
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x02},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x02},
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x03},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x03},
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x04},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x04},
+	{L2_SLOT_MANAGE, 0x00, 0x02, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x02, 0x01},
+	{L2_SLOT_MANAGE, 0x00, 0x03, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x03, 0x01},
+	{L2_SLOT_MANAGE, 0x00, 0x04, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x04, 0x01},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x02},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x02},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x03},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x03},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x04},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x04},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x05},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x05},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x06},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x06},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x07},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x07},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x08},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x08},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x09},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x09},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x0A},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x0A},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x02},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x02},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x03},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x03},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x04},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x04},
+	{L2_SLOT_RTT, 0x00, 0x02, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x02, 0x01},
+	{L2_SLOT_RTT, 0x00, 0x03, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x03, 0x01},
+	{L2_SLOT_RTT, 0x00, 0x04, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x04, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x02, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x02, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x02, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x02, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x02, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x02, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x02, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x02, L2_UNICAST_ID},
+
+	// 12 slots: network management, head <--> node
+	// 18 slots: message distribution, head ---> node
+	// 12 slots: rtt, head <--> node
+	// 8 slots: service
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x05},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x05},
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x06},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x06},
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x07},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x07},
+	{L2_SLOT_MANAGE, 0x00, 0x05, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x05, 0x01},
+	{L2_SLOT_MANAGE, 0x00, 0x06, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x06, 0x01},
+	{L2_SLOT_MANAGE, 0x00, 0x07, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x07, 0x01},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x02},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x02},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x03},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x03},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x04},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x04},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x05},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x05},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x06},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x06},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x07},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x07},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x08},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x08},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x09},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x09},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x0A},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x0A},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x05},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x05},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x06},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x06},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x07},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x07},
+	{L2_SLOT_RTT, 0x00, 0x05, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x05, 0x01},
+	{L2_SLOT_RTT, 0x00, 0x06, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x06, 0x01},
+	{L2_SLOT_RTT, 0x00, 0x07, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x07, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x01, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x01, L2_UNICAST_ID},
+
+	// 12 slots: network management, head <--> node
+	// 18 slots: message distribution, head ---> node
+	// 12 slots: rtt, head <--> node
+	// 8 slots: service, with one slot idle
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x08},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x08},
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x09},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x09},
+	{L2_SLOT_MANAGE, 0x00, 0x01, 0x0A},
+	{L2_SLOT_MANAGE, 0x01, 0x01, 0x0A},
+	{L2_SLOT_MANAGE, 0x00, 0x08, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x08, 0x01},
+	{L2_SLOT_MANAGE, 0x00, 0x09, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x09, 0x01},
+	{L2_SLOT_MANAGE, 0x00, 0x0A, 0x01},
+	{L2_SLOT_MANAGE, 0x01, 0x0A, 0x01},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x02},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x02},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x03},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x03},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x04},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x04},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x05},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x05},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x06},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x06},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x07},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x07},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x08},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x08},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x09},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x09},
+	{L2_SLOT_DISTRIBUTE, 0x00, 0x01, 0x0A},
+	{L2_SLOT_DISTRIBUTE, 0x01, 0x01, 0x0A},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x08},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x08},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x09},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x09},
+	{L2_SLOT_RTT, 0x00, 0x01, 0x0A},
+	{L2_SLOT_RTT, 0x01, 0x01, 0x0A},
+	{L2_SLOT_RTT, 0x00, 0x08, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x08, 0x01},
+	{L2_SLOT_RTT, 0x00, 0x09, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x09, 0x01},
+	{L2_SLOT_RTT, 0x00, 0x0A, 0x01},
+	{L2_SLOT_RTT, 0x01, 0x0A, 0x01},
+	{L2_SLOT_SERVICE, 0x00, 0x03, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x03, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x03, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x03, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x03, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x03, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x00, 0x03, L2_UNICAST_ID},
+	{L2_SLOT_SERVICE, 0x01, 0x03, L2_UNICAST_ID},
 };
 
-L2_RTNE_TABLE_ENTRY *normalTable[L2_NORMAL_RTNE_PERIOD] =
+L2_RTNE_TABLE_ENTRY *initTable[L2_INIT_RTNE_PERIOD] =
 {
-	innerScanSubT
+	tableFor10Node
+};
+
+L2_RTNE_TABLE_ENTRY *normalTable0[L2_INIT_RTNE_PERIOD] =
+{
+	tableFor10Node
+};
+
+L2_RTNE_TABLE_ENTRY *normalTable1[L2_INIT_RTNE_PERIOD] =
+{
+	tableFor3Node
+};
+
+L2_RTNE_TABLE_ENTRY *normalTable2[L2_INIT_RTNE_PERIOD] =
+{
+	tableFor10Node
 };
 
 /** @} RoutineTable*/

src/networking/link_layer/scan.c

@@ -63,13 +63,15 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 	case L2_FRAME_ADV:
 	{
 		L2_MSG_ADV *message = (L2_MSG_ADV *)(instPtr->sendPktData);
+		size_t msgLen = sizeof(L2_MSG_ADV);
+		size_t maxDataLen = L2_PAYLOAD_LEN_ADV;
 
 		// Common section of link layer package
 		message->common.frameNum = L2_GetNextFrameNum(instPtr);
 		message->common.slotNum = L2_GetNextSlotNum(instPtr);
 		message->common.src = L2_GetNodeId(instPtr);
 		message->common.dst = dstId;
-		message->common.msgType = L2_FRAME_ADV;
+		message->common.msgType = type;
 
 		// insert position and speed info
 		SysMonitor_GetSysinfo(&(instPtr->sysInfo));
@@ -80,16 +82,14 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 		// Insert net start time
 		message->srcNetStatTime = instPtr->netStartTime;
 
-		// Packing extra payload
+		// TODO: Packing extra payload
-		SysMonitor_GetSendPayload(message->advData, &(message->advDataId), L2_PAYLOAD_LEN_ADV);
+		message->dataId = DATA_ID_NONE;
-		if (message->advDataId == DATA_ID_NONE)
+		message->dataLen = 0;
-		{
+		memset(message->data, 0xAA, maxDataLen);
-			memset(message->advData, 0xAA, L2_PAYLOAD_LEN_ADV);
-		}
 
 		// Append CRC
 		uint32_t crcVal;
-		Status = L2_CrcGen((uint8_t *)message, sizeof(L2_MSG_ADV) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
+		Status = L2_CrcGen((uint8_t *)message, msgLen - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
 		if (Status != SUCCESS)
 		{
 			DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
@@ -98,14 +98,16 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 		message->crc = (uint8_t)crcVal;
 
 		// Logging...
-		memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ADV));
+		instPtr->log.txLog.dataId = message->dataId;
-		instPtr->log.txLog.dataId = message->advDataId;
+		memcpy(instPtr->log.txLog.data, message, msgLen);
 
 		break;
 	}
 	case L2_FRAME_ACK:
 	{
 		L2_MSG_ACK *message = (L2_MSG_ACK *)(instPtr->sendPktData);
+		size_t msgLen = sizeof(L2_MSG_ACK);
+		size_t maxDataLen = L2_PAYLOAD_LEN_ACK;
 
 		// Common section of link layer package
 		message->common.frameNum = L2_GetNextFrameNum(instPtr);
@@ -123,16 +125,14 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 		message->srcSpeedY = instPtr->sysInfo.vY;
 		message->srcSpeedZ = instPtr->sysInfo.vZ;
 
-		// Packing extra payload
+		// TODO: Packing extra payload
-		SysMonitor_GetSendPayload(message->ackData, &(message->ackDataId), L2_PAYLOAD_LEN_ACK);
+		message->dataId = DATA_ID_NONE;
-		if (message->ackDataId == DATA_ID_NONE)
+		message->dataLen = 0;
-		{
+		memset(message->data, 0xAA, maxDataLen);
-			memset(message->ackData, 0xAA, L2_PAYLOAD_LEN_ACK);
-		}
 
 		// Append CRC
 		uint32_t crcVal;
-		Status = L2_CrcGen((uint8_t *)message, sizeof(L2_MSG_ACK) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
+		Status = L2_CrcGen((uint8_t *)message, msgLen - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
 		if (Status != SUCCESS)
 		{
 			DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
@@ -141,8 +141,8 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
 		message->crc = crcVal;
 
 		// Logging...
-		memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ACK));
+		instPtr->log.txLog.dataId = message->dataId;
-		instPtr->log.txLog.dataId = message->ackDataId;
+		memcpy(instPtr->log.txLog.data, message, msgLen);
 
 		break;
 	}
@@ -168,7 +168,6 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 	ASSERT(instPtr != NULL);
 	ASSERT(msg != NULL);
 	int32_t Status;
-	L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM;
 	L2_FRAME_TYPE frameType = L2_FRAME_NONE;
 	L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType);
 
@@ -184,6 +183,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 		}
 
 		L2_MSG_ADV *message = (L2_MSG_ADV *)msg;
+		size_t msgLen = sizeof(L2_MSG_ADV);
+		size_t maxDataLen = L2_PAYLOAD_LEN_ADV;
 
 		// Update node status.
 		uint8_t nodeId = message->common.src;
@@ -241,13 +242,17 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 		// Time sync.
 		L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
 
-		// Get data payload
+		// Get data payload & upload
-		uint32_t dataId = message->advDataId;
+		SYSMON_SVC_PAYLOAD recvPayload;
-		SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV);
+		recvPayload.dataId = message->dataId;
+		recvPayload.nodeId = message->common.src;
+		recvPayload.dataLen = message->dataLen;
+		memcpy(recvPayload.data, message->data, maxDataLen);
+		SysMonitor_GenRecvRsp(&recvPayload);
 
 		// Logging...
-		memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ADV));
+		instPtr->log.rxLog.dataId = message->dataId;
-		instPtr->log.rxLog.dataId = dataId;
+		memcpy(instPtr->log.rxLog.data, message, msgLen);
 
 		Status = SUCCESS;
 		break;
@@ -262,6 +267,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 		}
 
 		L2_MSG_ACK *message = (L2_MSG_ACK *)msg;
+		size_t msgLen = sizeof(L2_MSG_ACK);
+		size_t dataLen = L2_PAYLOAD_LEN_ACK;
 
 		// Update node status.
 		uint8_t nodeId = message->common.src;
@@ -310,13 +317,17 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
 			(void)AssertL2Event(instPtr, &l2Event);
 		}
 
-		// Get data payload
+		// Get data payload & upload
-		uint32_t dataId = message->ackDataId;
+		SYSMON_SVC_PAYLOAD recvPayload;
-		SysMonitor_UpdateRecvPayload(message->ackData, dataId, L2_PAYLOAD_LEN_ACK);
+		recvPayload.dataId = message->dataId;
+		recvPayload.nodeId = message->common.src;
+		recvPayload.dataLen = message->dataLen;
+		memcpy(recvPayload.data, message->data, dataLen);
+		SysMonitor_GenRecvRsp(&recvPayload);
 
 		// Logging...
-		memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ACK));
+		instPtr->log.rxLog.dataId = message->dataId;
-		instPtr->log.rxLog.dataId = dataId;
+		memcpy(instPtr->log.rxLog.data, message, msgLen);
 
 		Status = SUCCESS;
 		break;
@@ -490,7 +501,7 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 			antIndex = PHY_ANT_INDEX_NONE;
 			torState = L2_TOR_SUSPEND;
 		}
-		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)Phy_SetAntIndex(phyRef, antIndex);
 		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ADV);
 		break;
 	case L2_SLOT_ACKNOWLEDGE:
@@ -523,7 +534,7 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
 			antIndex = PHY_ANT_INDEX_NONE;
 			torState = L2_TOR_SUSPEND;
 		}
-		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)Phy_SetAntIndex(phyRef, antIndex);
 		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ACK);
 		break;
 	default:

src/networking/link_layer/service.c

@@ -185,7 +185,7 @@ static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t
 
 	L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
 	size_t msgLen = sizeof(L2_MSG_SVC_128K);
-	size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;
+	size_t maxDataLen = L2_PAYLOAD_LEN_SVC_128K;
 
 	// insert position and speed info
 	SysMonitor_GetSysinfo(&(instPtr->sysInfo));
@@ -196,11 +196,23 @@ static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t
 	message->srcSpeedY = instPtr->sysInfo.vY;
 	message->srcSpeedZ = instPtr->sysInfo.vZ;
 
-	// Packing extra payload
+	// TODO: Packing extra payload
-	SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
+	if (type == L2_FRAME_SVC)
-	if (message->svcDataId == DATA_ID_NONE)
 	{
-		memset(message->svcData, 0xAA, dataLen);
+		SYSMON_SVC_PAYLOAD sendPayload;
+		SysMonitor_GetSendPayload(&sendPayload);
+		message->common.dst = sendPayload.nodeId;
+		message->dataId = sendPayload.dataId;
+		message->dataLen = sendPayload.dataLen;;
+		memset(message->data, 0xAA, maxDataLen);
+		memcpy(message->data, sendPayload.data, sendPayload.dataLen);
+	}
+	else
+	{
+		message->common.dst = L2_NONE_ID;
+		message->dataId = DATA_ID_NONE;
+		message->dataLen = 0;
+		memset(message->data, 0xAA, maxDataLen);
 	}
 
 	// Append CRC
@@ -214,8 +226,8 @@ static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t
 	message->crc = crcVal;
 
 	// Logging...
+	instPtr->log.txLog.dataId = message->dataId;
 	memcpy(instPtr->log.txLog.data, message, msgLen);
-	instPtr->log.txLog.dataId = message->svcDataId;
 
 	return SUCCESS;
 }
@@ -241,7 +253,7 @@ static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t
 
 	L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)(instPtr->sendPktData);
 	size_t msgLen = sizeof(L2_MSG_SVC_512K);
-	size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;
+	size_t maxDataLen = L2_PAYLOAD_LEN_SVC_512K;
 
 	// insert position and speed info
 	SysMonitor_GetSysinfo(&(instPtr->sysInfo));
@@ -252,12 +264,14 @@ static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t
 	message->srcSpeedY = instPtr->sysInfo.vY;
 	message->srcSpeedZ = instPtr->sysInfo.vZ;
 
-	// Packing extra payload
+	// TODO: Packing extra payload
-	SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
+	SYSMON_SVC_PAYLOAD sendPayload;
-	if (message->svcDataId == DATA_ID_NONE)
+	SysMonitor_GetSendPayload(&sendPayload);
-	{
+	message->common.dst = sendPayload.nodeId;
-		memset(message->svcData, 0xAA, dataLen);
+	message->dataId = sendPayload.dataId;
-	}
+	message->dataLen = sendPayload.dataLen;
+	memset(message->data, 0xAA, maxDataLen);
+	memcpy(message->data, sendPayload.data, sendPayload.dataLen);
 
 	// Append CRC
 	uint32_t crcVal;
@@ -270,9 +284,8 @@ static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t
 	message->crc = crcVal;
 
 	// Logging...
+	instPtr->log.txLog.dataId = message->dataId;
 	memcpy(instPtr->log.txLog.data, message, msgLen);
-	instPtr->log.txLog.dataId = message->svcDataId;
-
 
 	return SUCCESS;
 }
@@ -298,7 +311,7 @@ static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t d
 
 	L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)(instPtr->sendPktData);
 	size_t msgLen = sizeof(L2_MSG_SVC_2M);
-	size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;
+	size_t maxDataLen = L2_PAYLOAD_LEN_SVC_2M;
 
 	// insert position and speed info
 	SysMonitor_GetSysinfo(&(instPtr->sysInfo));
@@ -309,12 +322,14 @@ static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t d
 	message->srcSpeedY = instPtr->sysInfo.vY;
 	message->srcSpeedZ = instPtr->sysInfo.vZ;
 
-	// Packing extra payload
+	// TODO: Packing extra payload
-	SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
+	SYSMON_SVC_PAYLOAD sendPayload;
-	if (message->svcDataId == DATA_ID_NONE)
+	SysMonitor_GetSendPayload(&sendPayload);
-	{
+	message->common.dst = sendPayload.nodeId;
-		memset(message->svcData, 0xAA, dataLen);
+	message->dataId = sendPayload.dataId;
-	}
+	message->dataLen = sendPayload.dataLen;;
+	memset(message->data, 0xAA, maxDataLen);
+	memcpy(message->data, sendPayload.data, sendPayload.dataLen);
 
 	// Append CRC
 	uint32_t crcVal;
@@ -327,9 +342,8 @@ static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t d
 	message->crc = crcVal;
 
 	// Logging...
+	instPtr->log.txLog.dataId = message->dataId;
 	memcpy(instPtr->log.txLog.data, message, msgLen);
-	instPtr->log.txLog.dataId = message->svcDataId;
-
 
 	return SUCCESS;
 }
@@ -438,7 +452,7 @@ static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg)
 
 	L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)msg;
 	size_t msgLen = sizeof(L2_MSG_SVC_128K);
-	size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;
+	size_t maxDataLen = L2_PAYLOAD_LEN_SVC_128K;
 
 	// Update node status.
 	uint8_t nodeId = message->common.src;
@@ -477,11 +491,16 @@ static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg)
 	}
 
 	// Get data payload
-	SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
+	SYSMON_SVC_PAYLOAD recvPayload;
+	recvPayload.dataId = message->dataId;
+	recvPayload.nodeId = message->common.src;
+	recvPayload.dataLen = message->dataLen;
+	memcpy(recvPayload.data, message->data, maxDataLen);
+	SysMonitor_GenRecvRsp(&recvPayload);
 
 	// Logging...
-	memcpy(instPtr->log.rxLog.data, msg, msgLen);
+	instPtr->log.rxLog.dataId = message->dataId;
-	instPtr->log.rxLog.dataId = message->svcDataId;
+	memcpy(instPtr->log.rxLog.data, message, msgLen);
 
 #ifdef DATALINK_ENABLE_AUTOTIMESYNC
 	// Implement a auto time sync.
@@ -513,7 +532,7 @@ static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg)
 
 	L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)msg;
 	size_t msgLen = sizeof(L2_MSG_SVC_512K);
-	size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;
+	size_t maxDataLen = L2_PAYLOAD_LEN_SVC_512K;
 
 	// Update node status.
 	uint8_t nodeId = message->common.src;
@@ -552,11 +571,16 @@ static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg)
 	}
 
 	// Get data payload
-	SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
+	SYSMON_SVC_PAYLOAD recvPayload;
+	recvPayload.dataId = message->dataId;
+	recvPayload.nodeId = message->common.src;
+	recvPayload.dataLen = message->dataLen;
+	memcpy(recvPayload.data, message->data, maxDataLen);
+	SysMonitor_GenRecvRsp(&recvPayload);
 
 	// Logging...
-	memcpy(instPtr->log.rxLog.data, msg, msgLen);
+	instPtr->log.rxLog.dataId = message->dataId;
-	instPtr->log.rxLog.dataId = message->svcDataId;
+	memcpy(instPtr->log.rxLog.data, message, msgLen);
 
 #ifdef DATALINK_ENABLE_AUTOTIMESYNC
 	// Implement a auto time sync.
@@ -588,7 +612,7 @@ static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg)
 
 	L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)msg;
 	size_t msgLen = sizeof(L2_MSG_SVC_2M);
-	size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;
+	size_t maxDataLen = L2_PAYLOAD_LEN_SVC_2M;
 
 	// Update node status.
 	uint8_t nodeId = message->common.src;
@@ -627,11 +651,16 @@ static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg)
 	}
 
 	// Get data payload
-	SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
+	SYSMON_SVC_PAYLOAD recvPayload;
+	recvPayload.dataId = message->dataId;
+	recvPayload.nodeId = message->common.src;
+	recvPayload.dataLen = message->dataLen;
+	memcpy(recvPayload.data, message->data, message->dataLen);
+	SysMonitor_GenRecvRsp(&recvPayload);
 
 	// Logging...
-	memcpy(instPtr->log.rxLog.data, msg, msgLen);
+	instPtr->log.rxLog.dataId = message->dataId;
-	instPtr->log.rxLog.dataId = message->svcDataId;
+	memcpy(instPtr->log.rxLog.data, message, msgLen);
 
 #ifdef DATALINK_ENABLE_AUTOTIMESYNC
 	// Implement a auto time sync.
@@ -763,7 +792,7 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 			antIndex = PHY_ANT_INDEX_NONE;
 			torState = L2_TOR_SUSPEND;
 		}
-		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)Phy_SetAntIndex(phyRef, antIndex);
 		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_DIS);
 		break;
 	case L2_SLOT_MANAGE:
@@ -797,7 +826,7 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 			antIndex = PHY_ANT_INDEX_NONE;
 			torState = L2_TOR_SUSPEND;
 		}
-		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)Phy_SetAntIndex(phyRef, antIndex);
 		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_MNG);
 		break;
 	case L2_SLOT_RTT:
@@ -831,7 +860,7 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 			antIndex = PHY_ANT_INDEX_NONE;
 			torState = L2_TOR_SUSPEND;
 		}
-		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
+		(void)Phy_SetAntIndex(phyRef, antIndex);
 		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_RTT);
 		break;
 	case L2_SLOT_SERVICE:
@@ -858,9 +887,10 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 			memcpy(instPtr->sendPktData, testData, msgLen);
 #endif
 		}
-		else if (rtneEntry->dst == L2_GetNodeId(instPtr))
+		else
 		{
-			commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
+			// In services slots, rx mode is always enabled when other nodes are transmitting
+			commNode = L2_GetNodeById(instPtr->nodeList, L2_HEAD_ID);
 			// antenna selection based on node list
 			antIndex = commNode->nodeInfo.antIndex;
 			// member node receives message in advertise slot
@@ -868,13 +898,7 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
 
 			svcFSMPtr->rxCnt++;
 		}
-		else
+		(void)Phy_SetAntIndex(phyRef, antIndex);
-		{
-			// otherwise suspend
-			antIndex = PHY_ANT_INDEX_NONE;
-			torState = L2_TOR_SUSPEND;
-		}
-		(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
 		(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_SVC);
 		break;
 	default:

src/networking/phy_layer/phy_antenna.c

@@ -63,142 +63,142 @@ const uint16_t freqTable[L1_FRAME_MAX_HOPS] =
 };
 
 
-const uint8_t antCalTable[L1_ANT_NUM][L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM] =
+//const uint8_t antCalTable[L1_ANT_NUM][L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM] =
-//Antenna-JG01
+////Antenna-JG01
-{
+//{
-	// Antenna-1
+//	// Antenna-1
-	{
+//	{
-		{     0,   2525,  -7533,  -6846}, {     0,  12726,  -7818,  -6860},
+//		{     0,   2525,  -7533,  -6846}, {     0,  12726,  -7818,  -6860},
-		{     0,   2490,  -7836,  -6907}, {     0,   4479,  -7716,  -6837},
+//		{     0,   2490,  -7836,  -6907}, {     0,   4479,  -7716,  -6837},
-		{     0,   6409,  -7770,  -6776}, {     0,  14616,  -7977,  -6816},
+//		{     0,   6409,  -7770,  -6776}, {     0,  14616,  -7977,  -6816},
-		{     0, -22557,  -8002,  -6911}, {     0, -22587,  -8121,  -6918},
+//		{     0, -22557,  -8002,  -6911}, {     0, -22587,  -8121,  -6918},
-		{     0,  -1124,  -8123,  -7030}, {     0,   -152,  -8219,  -7042},
+//		{     0,  -1124,  -8123,  -7030}, {     0,   -152,  -8219,  -7042},
-		{     0,   1823,  -8209,  -7130}, {     0,   1790,  -8290,  -7214},
+//		{     0,   1823,  -8209,  -7130}, {     0,   1790,  -8290,  -7214},
-		{     0,   1669,  -8362,  -7283}, {     0,   1634,  -8459,  -7276},
+//		{     0,   1669,  -8362,  -7283}, {     0,   1634,  -8459,  -7276},
-		{     0,   1615,  -8315,  -7415}, {     0,   1698,  -8168,  -7366},
+//		{     0,   1615,  -8315,  -7415}, {     0,   1698,  -8168,  -7366},
-		{     0,   1535,  -8285,  -7520}, {     0,   1619,  -8285,  -7532},
+//		{     0,   1535,  -8285,  -7520}, {     0,   1619,  -8285,  -7532},
-		{     0,   1609,  -8201,  -7693}, {     0,   1671,  -8145,  -7769},
+//		{     0,   1609,  -8201,  -7693}, {     0,   1671,  -8145,  -7769},
-		{     0,   1676,  -8135,  -7805}, {     0,    951, -10279,  -8871},
+//		{     0,   1676,  -8135,  -7805}, {     0,    951, -10279,  -8871},
-		{     0,    976, -10235,  -8978}, {     0,   1118, -10127,  -9019},
+//		{     0,    976, -10235,  -8978}, {     0,   1118, -10127,  -9019},
-		{     0,   1175, -10003,  -9007}, {     0,   1386,  -9847,  -9039},
+//		{     0,   1175, -10003,  -9007}, {     0,   1386,  -9847,  -9039},
-		{     0,   1535,  -9572,  -9058}, {     0,   1729,  -9493,  -9006},
+//		{     0,   1535,  -9572,  -9058}, {     0,   1729,  -9493,  -9006},
-		{     0,  -1158,  -9312,  -9003}, {     0,  23771,  -9147,  -8928},
+//		{     0,  -1158,  -9312,  -9003}, {     0,  23771,  -9147,  -8928},
-		{     0,  20733,  -9074,  -8924}, {     0,   2455,  -8841,  -8886},
+//		{     0,  20733,  -9074,  -8924}, {     0,   2455,  -8841,  -8886},
-		{     0,   2622,  -8718,  -8735}, {     0,   2791,  -8615,  -8705},
+//		{     0,   2622,  -8718,  -8735}, {     0,   2791,  -8615,  -8705},
-		{     0,   2861,  -8168,  -8331}, {     0,   3076,  -8091,  -8218},
+//		{     0,   2861,  -8168,  -8331}, {     0,   3076,  -8091,  -8218},
-		{     0,   3257,  -8046,  -8100}, {     0,   3331,  -7923,  -8073},
+//		{     0,   3257,  -8046,  -8100}, {     0,   3331,  -7923,  -8073},
-		{     0,   3396,  -7826,  -7994}, {     0,   3478,  -7842,  -8016},
+//		{     0,   3396,  -7826,  -7994}, {     0,   3478,  -7842,  -8016},
-		{     0,   3518,  -7846,  -7765}, {     0,   3585,  -7753,  -7715},
+//		{     0,   3518,  -7846,  -7765}, {     0,   3585,  -7753,  -7715},
-		{     0,    150,  -8145,  -8591}, {     0,    192,  -8090,  -8570},
+//		{     0,    150,  -8145,  -8591}, {     0,    192,  -8090,  -8570},
-		{     0,    207,  -8150,  -8597}, {     0,    178,  -8142,  -8656},
+//		{     0,    207,  -8150,  -8597}, {     0,    178,  -8142,  -8656},
-		{     0,     89,  -8557,  -8688}, {     0,    107,  -8614,  -8733},
+//		{     0,     89,  -8557,  -8688}, {     0,    107,  -8614,  -8733},
-		{     0,    194,  -8605,  -8782}, {     0,    153,  -8655,  -8881},
+//		{     0,    194,  -8605,  -8782}, {     0,    153,  -8655,  -8881},
-		{     0,     70,  -8615,  -8952}, {     0,     35,  -8596,  -9148},
+//		{     0,     70,  -8615,  -8952}, {     0,     35,  -8596,  -9148},
-		{     0,     49,  -8636,  -9286}, {     0,      6,  -8587,  -9435},
+//		{     0,     49,  -8636,  -9286}, {     0,      6,  -8587,  -9435},
-		{     0,    -51,  -8540,  -9549}, {     0,    -65,  -8588,  -9700},
+//		{     0,    -51,  -8540,  -9549}, {     0,    -65,  -8588,  -9700},
-		{     0,    -55,  -8503,  -9820}, {     0,    -20,  -8414,  -9907},
+//		{     0,    -55,  -8503,  -9820}, {     0,    -20,  -8414,  -9907},
-		{     0,   -112,  -8481, -10031}, {     0,    -69,  -8535, -10185},
+//		{     0,   -112,  -8481, -10031}, {     0,    -69,  -8535, -10185},
-		{     0,      2,  -8550, -10052}, {     0,     -1,  -8493, -10291},
+//		{     0,      2,  -8550, -10052}, {     0,     -1,  -8493, -10291},
-		{     0,    -73,  -8441, -10380}, {     0,   1958,  -6930,  -8295},
+//		{     0,    -73,  -8441, -10380}, {     0,   1958,  -6930,  -8295},
-		{     0,  -1148,  -6939,  -8396}, {     0,   1915,  -6784,  -8370},
+//		{     0,  -1148,  -6939,  -8396}, {     0,   1915,  -6784,  -8370},
-		{     0,   1871,  -6970,  -8514}, {     0,   1821,  -6974,  -8387},
+//		{     0,   1871,  -6970,  -8514}, {     0,   1821,  -6974,  -8387},
-		{     0,   1781,  -6906,  -8431}, {     0,   1736,  -6897,  -8462},
+//		{     0,   1781,  -6906,  -8431}, {     0,   1736,  -6897,  -8462},
-		{     0,   1664,  -7024,  -8377}, {     0,   1629,  -7127,  -8436},
+//		{     0,   1664,  -7024,  -8377}, {     0,   1629,  -7127,  -8436},
-		{     0,   1603,  -7148,  -8528}, {     0,   1577,  -7198,  -8500},
+//		{     0,   1603,  -7148,  -8528}, {     0,   1577,  -7198,  -8500},
-		{     0,   1528,  -7311,  -8512}, {     0,   1577,  -7390,  -8570},
+//		{     0,   1528,  -7311,  -8512}, {     0,   1577,  -7390,  -8570},
-		{     0,   1616,  -7436,  -8572}, {     0,   1589,  -7597,  -8590},
+//		{     0,   1616,  -7436,  -8572}, {     0,   1589,  -7597,  -8590},
-		{     0,   1722,  -7550,  -8568}, {     0,   1741,  -7691,  -8449},
+//		{     0,   1722,  -7550,  -8568}, {     0,   1741,  -7691,  -8449},
-		{     0,   1767,  -7775,  -8328},
+//		{     0,   1767,  -7775,  -8328},
-	},
+//	},
-	// Antenna-2
+//	// Antenna-2
-	{
+//	{
-		{     0,   2525,  -7533,  -6846}, {     0,  12726,  -7818,  -6860},
+//		{     0,   2525,  -7533,  -6846}, {     0,  12726,  -7818,  -6860},
-		{     0,   2490,  -7836,  -6907}, {     0,   4479,  -7716,  -6837},
+//		{     0,   2490,  -7836,  -6907}, {     0,   4479,  -7716,  -6837},
-		{     0,   6409,  -7770,  -6776}, {     0,  14616,  -7977,  -6816},
+//		{     0,   6409,  -7770,  -6776}, {     0,  14616,  -7977,  -6816},
-		{     0, -22557,  -8002,  -6911}, {     0, -22587,  -8121,  -6918},
+//		{     0, -22557,  -8002,  -6911}, {     0, -22587,  -8121,  -6918},
-		{     0,  -1124,  -8123,  -7030}, {     0,   -152,  -8219,  -7042},
+//		{     0,  -1124,  -8123,  -7030}, {     0,   -152,  -8219,  -7042},
-		{     0,   1823,  -8209,  -7130}, {     0,   1790,  -8290,  -7214},
+//		{     0,   1823,  -8209,  -7130}, {     0,   1790,  -8290,  -7214},
-		{     0,   1669,  -8362,  -7283}, {     0,   1634,  -8459,  -7276},
+//		{     0,   1669,  -8362,  -7283}, {     0,   1634,  -8459,  -7276},
-		{     0,   1615,  -8315,  -7415}, {     0,   1698,  -8168,  -7366},
+//		{     0,   1615,  -8315,  -7415}, {     0,   1698,  -8168,  -7366},
-		{     0,   1535,  -8285,  -7520}, {     0,   1619,  -8285,  -7532},
+//		{     0,   1535,  -8285,  -7520}, {     0,   1619,  -8285,  -7532},
-		{     0,   1609,  -8201,  -7693}, {     0,   1671,  -8145,  -7769},
+//		{     0,   1609,  -8201,  -7693}, {     0,   1671,  -8145,  -7769},
-		{     0,   1676,  -8135,  -7805}, {     0,    951, -10279,  -8871},
+//		{     0,   1676,  -8135,  -7805}, {     0,    951, -10279,  -8871},
-		{     0,    976, -10235,  -8978}, {     0,   1118, -10127,  -9019},
+//		{     0,    976, -10235,  -8978}, {     0,   1118, -10127,  -9019},
-		{     0,   1175, -10003,  -9007}, {     0,   1386,  -9847,  -9039},
+//		{     0,   1175, -10003,  -9007}, {     0,   1386,  -9847,  -9039},
-		{     0,   1535,  -9572,  -9058}, {     0,   1729,  -9493,  -9006},
+//		{     0,   1535,  -9572,  -9058}, {     0,   1729,  -9493,  -9006},
-		{     0,  -1158,  -9312,  -9003}, {     0,  23771,  -9147,  -8928},
+//		{     0,  -1158,  -9312,  -9003}, {     0,  23771,  -9147,  -8928},
-		{     0,  20733,  -9074,  -8924}, {     0,   2455,  -8841,  -8886},
+//		{     0,  20733,  -9074,  -8924}, {     0,   2455,  -8841,  -8886},
-		{     0,   2622,  -8718,  -8735}, {     0,   2791,  -8615,  -8705},
+//		{     0,   2622,  -8718,  -8735}, {     0,   2791,  -8615,  -8705},
-		{     0,   2861,  -8168,  -8331}, {     0,   3076,  -8091,  -8218},
+//		{     0,   2861,  -8168,  -8331}, {     0,   3076,  -8091,  -8218},
-		{     0,   3257,  -8046,  -8100}, {     0,   3331,  -7923,  -8073},
+//		{     0,   3257,  -8046,  -8100}, {     0,   3331,  -7923,  -8073},
-		{     0,   3396,  -7826,  -7994}, {     0,   3478,  -7842,  -8016},
+//		{     0,   3396,  -7826,  -7994}, {     0,   3478,  -7842,  -8016},
-		{     0,   3518,  -7846,  -7765}, {     0,   3585,  -7753,  -7715},
+//		{     0,   3518,  -7846,  -7765}, {     0,   3585,  -7753,  -7715},
-		{     0,    150,  -8145,  -8591}, {     0,    192,  -8090,  -8570},
+//		{     0,    150,  -8145,  -8591}, {     0,    192,  -8090,  -8570},
-		{     0,    207,  -8150,  -8597}, {     0,    178,  -8142,  -8656},
+//		{     0,    207,  -8150,  -8597}, {     0,    178,  -8142,  -8656},
-		{     0,     89,  -8557,  -8688}, {     0,    107,  -8614,  -8733},
+//		{     0,     89,  -8557,  -8688}, {     0,    107,  -8614,  -8733},
-		{     0,    194,  -8605,  -8782}, {     0,    153,  -8655,  -8881},
+//		{     0,    194,  -8605,  -8782}, {     0,    153,  -8655,  -8881},
-		{     0,     70,  -8615,  -8952}, {     0,     35,  -8596,  -9148},
+//		{     0,     70,  -8615,  -8952}, {     0,     35,  -8596,  -9148},
-		{     0,     49,  -8636,  -9286}, {     0,      6,  -8587,  -9435},
+//		{     0,     49,  -8636,  -9286}, {     0,      6,  -8587,  -9435},
-		{     0,    -51,  -8540,  -9549}, {     0,    -65,  -8588,  -9700},
+//		{     0,    -51,  -8540,  -9549}, {     0,    -65,  -8588,  -9700},
-		{     0,    -55,  -8503,  -9820}, {     0,    -20,  -8414,  -9907},
+//		{     0,    -55,  -8503,  -9820}, {     0,    -20,  -8414,  -9907},
-		{     0,   -112,  -8481, -10031}, {     0,    -69,  -8535, -10185},
+//		{     0,   -112,  -8481, -10031}, {     0,    -69,  -8535, -10185},
-		{     0,      2,  -8550, -10052}, {     0,     -1,  -8493, -10291},
+//		{     0,      2,  -8550, -10052}, {     0,     -1,  -8493, -10291},
-		{     0,    -73,  -8441, -10380}, {     0,   1958,  -6930,  -8295},
+//		{     0,    -73,  -8441, -10380}, {     0,   1958,  -6930,  -8295},
-		{     0,  -1148,  -6939,  -8396}, {     0,   1915,  -6784,  -8370},
+//		{     0,  -1148,  -6939,  -8396}, {     0,   1915,  -6784,  -8370},
-		{     0,   1871,  -6970,  -8514}, {     0,   1821,  -6974,  -8387},
+//		{     0,   1871,  -6970,  -8514}, {     0,   1821,  -6974,  -8387},
-		{     0,   1781,  -6906,  -8431}, {     0,   1736,  -6897,  -8462},
+//		{     0,   1781,  -6906,  -8431}, {     0,   1736,  -6897,  -8462},
-		{     0,   1664,  -7024,  -8377}, {     0,   1629,  -7127,  -8436},
+//		{     0,   1664,  -7024,  -8377}, {     0,   1629,  -7127,  -8436},
-		{     0,   1603,  -7148,  -8528}, {     0,   1577,  -7198,  -8500},
+//		{     0,   1603,  -7148,  -8528}, {     0,   1577,  -7198,  -8500},
-		{     0,   1528,  -7311,  -8512}, {     0,   1577,  -7390,  -8570},
+//		{     0,   1528,  -7311,  -8512}, {     0,   1577,  -7390,  -8570},
-		{     0,   1616,  -7436,  -8572}, {     0,   1589,  -7597,  -8590},
+//		{     0,   1616,  -7436,  -8572}, {     0,   1589,  -7597,  -8590},
-		{     0,   1722,  -7550,  -8568}, {     0,   1741,  -7691,  -8449},
+//		{     0,   1722,  -7550,  -8568}, {     0,   1741,  -7691,  -8449},
-		{     0,   1767,  -7775,  -8328},
+//		{     0,   1767,  -7775,  -8328},
-	},
+//	},
-	// Antenna-3
+//	// Antenna-3
-	{
+//	{
-		{     0,   2525,  -7533,  -6846}, {     0,  12726,  -7818,  -6860},
+//		{     0,   2525,  -7533,  -6846}, {     0,  12726,  -7818,  -6860},
-		{     0,   2490,  -7836,  -6907}, {     0,   4479,  -7716,  -6837},
+//		{     0,   2490,  -7836,  -6907}, {     0,   4479,  -7716,  -6837},
-		{     0,   6409,  -7770,  -6776}, {     0,  14616,  -7977,  -6816},
+//		{     0,   6409,  -7770,  -6776}, {     0,  14616,  -7977,  -6816},
-		{     0, -22557,  -8002,  -6911}, {     0, -22587,  -8121,  -6918},
+//		{     0, -22557,  -8002,  -6911}, {     0, -22587,  -8121,  -6918},
-		{     0,  -1124,  -8123,  -7030}, {     0,   -152,  -8219,  -7042},
+//		{     0,  -1124,  -8123,  -7030}, {     0,   -152,  -8219,  -7042},
-		{     0,   1823,  -8209,  -7130}, {     0,   1790,  -8290,  -7214},
+//		{     0,   1823,  -8209,  -7130}, {     0,   1790,  -8290,  -7214},
-		{     0,   1669,  -8362,  -7283}, {     0,   1634,  -8459,  -7276},
+//		{     0,   1669,  -8362,  -7283}, {     0,   1634,  -8459,  -7276},
-		{     0,   1615,  -8315,  -7415}, {     0,   1698,  -8168,  -7366},
+//		{     0,   1615,  -8315,  -7415}, {     0,   1698,  -8168,  -7366},
-		{     0,   1535,  -8285,  -7520}, {     0,   1619,  -8285,  -7532},
+//		{     0,   1535,  -8285,  -7520}, {     0,   1619,  -8285,  -7532},
-		{     0,   1609,  -8201,  -7693}, {     0,   1671,  -8145,  -7769},
+//		{     0,   1609,  -8201,  -7693}, {     0,   1671,  -8145,  -7769},
-		{     0,   1676,  -8135,  -7805}, {     0,    951, -10279,  -8871},
+//		{     0,   1676,  -8135,  -7805}, {     0,    951, -10279,  -8871},
-		{     0,    976, -10235,  -8978}, {     0,   1118, -10127,  -9019},
+//		{     0,    976, -10235,  -8978}, {     0,   1118, -10127,  -9019},
-		{     0,   1175, -10003,  -9007}, {     0,   1386,  -9847,  -9039},
+//		{     0,   1175, -10003,  -9007}, {     0,   1386,  -9847,  -9039},
-		{     0,   1535,  -9572,  -9058}, {     0,   1729,  -9493,  -9006},
+//		{     0,   1535,  -9572,  -9058}, {     0,   1729,  -9493,  -9006},
-		{     0,  -1158,  -9312,  -9003}, {     0,  23771,  -9147,  -8928},
+//		{     0,  -1158,  -9312,  -9003}, {     0,  23771,  -9147,  -8928},
-		{     0,  20733,  -9074,  -8924}, {     0,   2455,  -8841,  -8886},
+//		{     0,  20733,  -9074,  -8924}, {     0,   2455,  -8841,  -8886},
-		{     0,   2622,  -8718,  -8735}, {     0,   2791,  -8615,  -8705},
+//		{     0,   2622,  -8718,  -8735}, {     0,   2791,  -8615,  -8705},
-		{     0,   2861,  -8168,  -8331}, {     0,   3076,  -8091,  -8218},
+//		{     0,   2861,  -8168,  -8331}, {     0,   3076,  -8091,  -8218},
-		{     0,   3257,  -8046,  -8100}, {     0,   3331,  -7923,  -8073},
+//		{     0,   3257,  -8046,  -8100}, {     0,   3331,  -7923,  -8073},
-		{     0,   3396,  -7826,  -7994}, {     0,   3478,  -7842,  -8016},
+//		{     0,   3396,  -7826,  -7994}, {     0,   3478,  -7842,  -8016},
-		{     0,   3518,  -7846,  -7765}, {     0,   3585,  -7753,  -7715},
+//		{     0,   3518,  -7846,  -7765}, {     0,   3585,  -7753,  -7715},
-		{     0,    150,  -8145,  -8591}, {     0,    192,  -8090,  -8570},
+//		{     0,    150,  -8145,  -8591}, {     0,    192,  -8090,  -8570},
-		{     0,    207,  -8150,  -8597}, {     0,    178,  -8142,  -8656},
+//		{     0,    207,  -8150,  -8597}, {     0,    178,  -8142,  -8656},
-		{     0,     89,  -8557,  -8688}, {     0,    107,  -8614,  -8733},
+//		{     0,     89,  -8557,  -8688}, {     0,    107,  -8614,  -8733},
-		{     0,    194,  -8605,  -8782}, {     0,    153,  -8655,  -8881},
+//		{     0,    194,  -8605,  -8782}, {     0,    153,  -8655,  -8881},
-		{     0,     70,  -8615,  -8952}, {     0,     35,  -8596,  -9148},
+//		{     0,     70,  -8615,  -8952}, {     0,     35,  -8596,  -9148},
-		{     0,     49,  -8636,  -9286}, {     0,      6,  -8587,  -9435},
+//		{     0,     49,  -8636,  -9286}, {     0,      6,  -8587,  -9435},
-		{     0,    -51,  -8540,  -9549}, {     0,    -65,  -8588,  -9700},
+//		{     0,    -51,  -8540,  -9549}, {     0,    -65,  -8588,  -9700},
-		{     0,    -55,  -8503,  -9820}, {     0,    -20,  -8414,  -9907},
+//		{     0,    -55,  -8503,  -9820}, {     0,    -20,  -8414,  -9907},
-		{     0,   -112,  -8481, -10031}, {     0,    -69,  -8535, -10185},
+//		{     0,   -112,  -8481, -10031}, {     0,    -69,  -8535, -10185},
-		{     0,      2,  -8550, -10052}, {     0,     -1,  -8493, -10291},
+//		{     0,      2,  -8550, -10052}, {     0,     -1,  -8493, -10291},
-		{     0,    -73,  -8441, -10380}, {     0,   1958,  -6930,  -8295},
+//		{     0,    -73,  -8441, -10380}, {     0,   1958,  -6930,  -8295},
-		{     0,  -1148,  -6939,  -8396}, {     0,   1915,  -6784,  -8370},
+//		{     0,  -1148,  -6939,  -8396}, {     0,   1915,  -6784,  -8370},
-		{     0,   1871,  -6970,  -8514}, {     0,   1821,  -6974,  -8387},
+//		{     0,   1871,  -6970,  -8514}, {     0,   1821,  -6974,  -8387},
-		{     0,   1781,  -6906,  -8431}, {     0,   1736,  -6897,  -8462},
+//		{     0,   1781,  -6906,  -8431}, {     0,   1736,  -6897,  -8462},
-		{     0,   1664,  -7024,  -8377}, {     0,   1629,  -7127,  -8436},
+//		{     0,   1664,  -7024,  -8377}, {     0,   1629,  -7127,  -8436},
-		{     0,   1603,  -7148,  -8528}, {     0,   1577,  -7198,  -8500},
+//		{     0,   1603,  -7148,  -8528}, {     0,   1577,  -7198,  -8500},
-		{     0,   1528,  -7311,  -8512}, {     0,   1577,  -7390,  -8570},
+//		{     0,   1528,  -7311,  -8512}, {     0,   1577,  -7390,  -8570},
-		{     0,   1616,  -7436,  -8572}, {     0,   1589,  -7597,  -8590},
+//		{     0,   1616,  -7436,  -8572}, {     0,   1589,  -7597,  -8590},
-		{     0,   1722,  -7550,  -8568}, {     0,   1741,  -7691,  -8449},
+//		{     0,   1722,  -7550,  -8568}, {     0,   1741,  -7691,  -8449},
-		{     0,   1767,  -7775,  -8328},
+//		{     0,   1767,  -7775,  -8328},
-	},
+//	},
-};
+//};
 
 const uint8_t hopPattern_ADV_24MS[L1_FRAME_ADV_24MS_HOPS] =
 {

src/networking/phy_layer/phy_layer.c

@@ -64,7 +64,7 @@
 
 /************************** Variable Definitions *****************************/
 extern const uint16_t freqTable[L1_FRAME_MAX_HOPS];
-extern const int16_t antCalTable[L1_ANT_NUM][L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM];
+//extern const int16_t antCalTable[L1_ANT_NUM][L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM];
 extern const uint8_t hopPattern_ADV_24MS[L1_FRAME_ADV_24MS_HOPS];
 extern const uint8_t hopPattern_ACK_8MS[L1_FRAME_ACK_8MS_HOPS];
 extern const uint8_t hopPattern_128KBPS_4MS[L1_FRAME_128KBPS_4MS_HOPS];

src/system_monitor.c

@@ -8,7 +8,9 @@
 #include <stdlib.h>
 #include <stdint.h>
 #include "common.h"
+#include "circ_queue.h"
 #include "system_monitor.h"
+#include "systembus/systembus.h"
 /************************** Constant Definitions *****************************/
 /**
  * @name SystemMonitor_Private_Constants
@@ -42,10 +44,9 @@ typedef struct
  */
 typedef struct
 {
-	PINGPONG recvCtrl;
+	SYSMON_CONFIG config; ///< system monitor initialization parameters
-	PAYLOAD recv[2];
+	CIRC_QUEUE *sendQueue;    ///< send queue
-	PINGPONG sendCtrl;
+	CIRC_QUEUE *recvQueue;    ///< receive queue
-	PAYLOAD send[2];
 	PINGPONG sysInfoCtrl;
 	SYSINFO sysInfo[2];
 	PINGPONG linkLogCtrl;
@@ -61,6 +62,7 @@ static uint8_t GetWritable(PINGPONG ctrllor);
 static uint8_t GetReadable(PINGPONG ctrllor);
 /************************** Variable Definitions *****************************/
 static SYSMONITOR *sysMonitorInst;
+extern SYSBUS *sysbusDescPtr;
 
 /*********************** Function Implementations  ***************************/
 inline static uint8_t GetWritable(PINGPONG ctrllor)
@@ -86,24 +88,37 @@ inline static uint8_t GetReadable(PINGPONG ctrllor)
  *
  * @return SUCCESS
  */
-int32_t SysMonitor_Init(void)
+int32_t SysMonitor_Init(SYSMON_CONFIG *config)
 {
+	ASSERT(config != NULL);
+
 	sysMonitorInst = calloc(1, sizeof(SYSMONITOR));
 	if (sysMonitorInst == NULL)
 	{
 		return ENOMEM;
 	}
-	memset(&(sysMonitorInst->recvCtrl), BUFF_NONE, sizeof(PINGPONG));
+
-	memset(&(sysMonitorInst->sendCtrl), BUFF_NONE, sizeof(PINGPONG));
+	memcpy(&(sysMonitorInst->config), config, sizeof(SYSMON_CONFIG));
+	sysMonitorInst->sendQueue = NULL;
+	sysMonitorInst->recvQueue = NULL;
 	memset(&(sysMonitorInst->sysInfoCtrl), BUFF_NONE, sizeof(PINGPONG));
 	memset(&(sysMonitorInst->linkLogCtrl), BUFF_NONE, sizeof(PINGPONG));
 	memset(&(sysMonitorInst->phyLogCtrl), BUFF_NONE, sizeof(PINGPONG));
 	memset(&(sysMonitorInst->hwStateCtrl), BUFF_NONE, sizeof(PINGPONG));
 
-	sysMonitorInst->recv[0].dataId = DATA_ID_NONE;
+	if (CircQueueCreate(&(sysMonitorInst->sendQueue),
-	sysMonitorInst->recv[1].dataId = DATA_ID_NONE;
+			sysMonitorInst->config.sendQueueMax) == false)
-	sysMonitorInst->send[0].dataId = DATA_ID_NONE;
+	{
-	sysMonitorInst->send[1].dataId = DATA_ID_NONE;
+		SysMonitor_Destroy();
+		return ENOMEM;
+	}
+	if (CircQueueCreate(&(sysMonitorInst->recvQueue),
+			sysMonitorInst->config.recvQueueMax) == false)
+	{
+		SysMonitor_Destroy();
+		return ENOMEM;
+	}
+
 	return SUCCESS;
 }
 
@@ -165,113 +180,70 @@ void SysMonitor_GetSysinfo(SYSINFO *info)
 /**
  * @brief Update send payload by
  *
- * @param buff
+ * @param payload
- * @param len
  */
-void SysMonitor_UpdateSendPayload(const uint8_t *buff, size_t len)
+int32_t SysMonitor_UpdateSendPayload(SYSMON_SVC_PAYLOAD *payload)
 {
 	ASSERT(sysMonitorInst != NULL);
-	if (len > PAYLOAD_LEN_MAX)
+	ASSERT(payload != NULL);
+
+	// validity check
+	if (payload->dataLen > PAYLOAD_LEN_MAX)
 	{
-		return;
+		DBG_ERROR("Payload length %d exceeds valid range.", payload->dataLen);
+		return EPARAM;
 	}
-	uint8_t buffIndx;
+
-	buffIndx = GetWritable(sysMonitorInst->sendCtrl);
+	// push message into send queue
-	sysMonitorInst->sendCtrl.wBusy = buffIndx;
+	if (CircQueuePush(sysMonitorInst->sendQueue, (void *)payload) == false)
-	memcpy(sysMonitorInst->send[buffIndx].data, buff, len);
+	{
-	sysMonitorInst->send[buffIndx].dataId++;
+		DBG_WARNING("SystemMonitor::SEND Queue overflowed!\n");
-	sysMonitorInst->sendCtrl.latest = buffIndx;
+		return EQUEUE;
-	sysMonitorInst->sendCtrl.wBusy = BUFF_NONE;
+	}
+
+	return SUCCESS;
 }
 
 /**
  * @brief Get the latest send payload.
  *
- * @param buff
+ * @param payload
- * @param id
- * @param len
  */
-void SysMonitor_GetSendPayload(uint8_t *buff, uint32_t *id, size_t len)
+void SysMonitor_GetSendPayload(SYSMON_SVC_PAYLOAD *payload)
 {
 	ASSERT(sysMonitorInst != NULL);
-	ASSERT(buff != NULL);
+	ASSERT(payload != NULL);
-	ASSERT(id != NULL);
+
-	if (len > PAYLOAD_LEN_MAX)
+	if (CircQueuePop(sysMonitorInst->sendQueue, (void *)payload) == false)
 	{
-		return;
+		payload->dataId = DATA_ID_NONE;
+		payload->nodeId = 0;
+		payload->dataLen = 0;
+		// set payload to be 0xAA to balanced average signal power
+		memset(payload->data, 0xAA, PAYLOAD_LEN_MAX);
+		DBG_WARNING("SystemMonitor::Nothing to be sent.\n");
 	}
-	uint8_t buffIndx, buffIndex_old;
-	buffIndex_old = sysMonitorInst->sendCtrl.rBusy;
-	if (buffIndex_old != BUFF_NONE)
-	{
-		buffIndx = buffIndex_old; // Keep it busy
-	}
-	else
-	{
-		buffIndx = GetReadable(sysMonitorInst->sendCtrl);
-	}
-	sysMonitorInst->sendCtrl.rBusy = buffIndx;
-	if (sysMonitorInst->send[buffIndx].dataId != DATA_ID_NONE)
-	{
-		memcpy(buff, sysMonitorInst->send[buffIndx].data, len);
-	}
-	*id = sysMonitorInst->send[buffIndx].dataId;
-	sysMonitorInst->sendCtrl.rBusy = buffIndex_old;
 }
 
 /**
  * @brief Update receive payload.
  *
- * @param buff
+ * @param payload
- * @param id
- * @param len
  */
-void SysMonitor_UpdateRecvPayload(const uint8_t *buff, uint32_t id, size_t len)
+int32_t SysMonitor_GenRecvRsp(SYSMON_SVC_PAYLOAD *payload)
 {
 	ASSERT(sysMonitorInst != NULL);
-	ASSERT(buff != NULL);
+	ASSERT(sysbusDescPtr != NULL);
-	if (id == DATA_ID_NONE || len > PAYLOAD_LEN_MAX)
+	int32_t Status;
+
+	if (payload->dataId == DATA_ID_NONE || payload->dataLen > PAYLOAD_LEN_MAX)
 	{
-		return;
+		return EIGNORE;
-	}
-	uint8_t buffIndx;
-	buffIndx = GetWritable(sysMonitorInst->recvCtrl);
-	sysMonitorInst->recvCtrl.wBusy = buffIndx;
-	memcpy(sysMonitorInst->recv[buffIndx].data, buff, len);
-	sysMonitorInst->recv[buffIndx].dataId = id;
-	sysMonitorInst->recvCtrl.latest = buffIndx;
-	sysMonitorInst->recvCtrl.wBusy = BUFF_NONE;
 	}
 
-/**
+	Status = SystemBus_GenRecvRsp(sysbusDescPtr, payload);
- * @brief Get the latest receive payload.
+
- *
+	return Status;
- * @param buff
- * @param id
- * @param len
- */
-void SysMonitor_GetRecvPayload(uint8_t *buff, uint32_t *id, size_t len)
-{
-	ASSERT(sysMonitorInst != NULL);
-	ASSERT(buff != NULL);
-	ASSERT(id != NULL);
-	if (len > PAYLOAD_LEN_MAX)
-	{
-		return;
-	}
-	uint8_t buffIndx, buffIndex_old;
-	buffIndex_old = sysMonitorInst->recvCtrl.rBusy;
-	if (buffIndex_old != BUFF_NONE)
-		buffIndx = buffIndex_old; // Keep it busy
-	else
-		buffIndx = GetReadable(sysMonitorInst->recvCtrl);
-	sysMonitorInst->recvCtrl.rBusy = buffIndx;
-	if (sysMonitorInst->recv[buffIndx].dataId != DATA_ID_NONE)
-	{
-		memcpy(buff, sysMonitorInst->recv[buffIndx].data, len);
-	}
-	*id = sysMonitorInst->recv[buffIndx].dataId;
-	sysMonitorInst->recvCtrl.rBusy = buffIndex_old;
 }
 /** @} ServicePayload_API*/
 

src/system_monitor.h

@@ -16,11 +16,7 @@
  * @name SystemMonitor_Constants
  * @{
  */
-#define PAYLOAD_LEN_MAX 256
+#define PAYLOAD_LEN_MAX 1000
-#define PAYLOAD_LEN_FLJ_RX 168
-#define PAYLOAD_LEN_FLJ_TX 66
-#define PAYLOAD_LEN_LJ_RX 66
-#define PAYLOAD_LEN_LJ_TX 168
 
 #define DATA_ID_NONE UINT32_MAX
 #define DATA_ID_TEST 0x02030203
@@ -120,11 +116,13 @@ typedef struct __attribute__((packed))
 {
 	// SystemBus log
 
-	uint8_t lastConfig[2]; ///< Last valid config \n
+	uint8_t lastConfig[4]; ///< Last valid config \n
-						   //  byte 0: 0x11 FLJ, 0x00 LJ \n
+						   //  byte 0: Enable control, 0xAA - enable, 0x55 - disable \n
-						   //   byte 1: 0x66 Full Power, 0x88 Low Power
+						   //  byte 1: Node ID \n
-	uint32_t pollCnt;	   ///< number of poll commands
+						   //  byte 2: Service rate \n
-	uint32_t svcCnt;	   ///< number of service commands
+						   //  byte 3: routing table number
+	uint32_t recvCnt;	   ///< number of received data packet
+	uint32_t sendCnt;	   ///< number of send data packet
 	uint32_t remoteCnt;	   ///< number of remote check commands
 } SYSBUSLOG;
 
@@ -142,9 +140,9 @@ typedef struct __attribute__((packed))
 	uint8_t linkStage;		///< current linklayer working stage
 	uint32_t recxnCnt;		///< reconnect trial count
 	uint8_t antIndex;		///< current working antenna index
-	uint16_t advTxCnt;		///< adv frame send count
+	uint32_t advTxCnt;		///< adv frame send count
-	uint16_t advRecvCnt;	///< adv frame received count
+	uint32_t advRecvCnt;	///< adv frame received count
-	uint16_t advCorrectCnt; ///< adv frame received correctly count
+	uint32_t advCorrectCnt; ///< adv frame received correctly count
 	uint32_t ackTxCnt;		///< ack frame send count
 	uint32_t ackRecvCnt;	///< ack frame received count
 	uint32_t ackCorrectCnt; ///< ack frame received correctly count
@@ -163,7 +161,7 @@ typedef struct __attribute__((packed))
 	int32_t timeDev;		///< last time deviation result
 	uint32_t sendId;		///< last send data id
 	uint32_t recvId;		///< last received data id
-	uint8_t reserve[4];		///< reserved data
+	uint8_t reserve[3];		///< reserved data
 } LINKLOG;
 
 /**
@@ -172,7 +170,7 @@ typedef struct __attribute__((packed))
  */
 typedef struct __attribute__((packed))
 {
-	uint32_t acqCnt;		///< aquisition count
+	uint32_t acqCnt;		///< acquisition count
 	uint32_t fsyncCnt;		///< frame sync count
 	uint32_t decodeCnt;		///< decode count
 	uint32_t crcCnt;		///< crc valid count
@@ -185,7 +183,7 @@ typedef struct __attribute__((packed))
 	uint8_t fsyncFreqCh;	///< last frame sync frequency estimation
 	uint16_t deintlvAmpEst; ///< de-interleave amplitude estimation
 	uint8_t antControl;		///< current antenna rf mode
-	uint8_t reserve[3];		///< reserved data
+	uint8_t reserve[2];		///< reserved data
 } PHYLOG;
 
 /**
@@ -212,30 +210,41 @@ typedef struct __attribute__((packed))
 	int8_t antTemp;	   ///< Antenna rf temperature
 	uint8_t antRsvd;   ///< Antenna reserved status register
 	uint8_t phyId;
-	uint8_t reserve[1];
+	uint8_t reserve[3];
 } HWSTATE;
 
 /**
- * @struct PAYLOAD
+ * @struct SYSMON_CONFIG
- * @brief Payload struct holds the service payload
+ * @brief System monitor config struct holds system monitor initialization parameters
  */
 typedef struct
 {
-	uint32_t dataId;
+	uint32_t sendQueueMax; ///< maximum send queue length
+	uint32_t recvQueueMax; ///< maximum receive queue length
+} SYSMON_CONFIG;
+
+/**
+ * @struct SYSMON_SVC_PAYLOAD
+ * @brief Payload struct that holds the service payload
+ */
+typedef struct
+{
+	uint32_t dataId;  ///< data identifier
+	uint8_t nodeId;   ///< destination ID for tx, source ID for rx
+	uint16_t dataLen; ///< data length
 	uint8_t data[PAYLOAD_LEN_MAX];
-} PAYLOAD;
+} SYSMON_SVC_PAYLOAD;
 
 /************************** Function Prototypes ******************************/
-int32_t SysMonitor_Init(void);
+int32_t SysMonitor_Init(SYSMON_CONFIG *config);
 void SysMonitor_Destroy(void);
 
 void SysMonitor_UpdateSysinfo(const SYSINFO *info);
 void SysMonitor_GetSysinfo(SYSINFO *info);
 
-void SysMonitor_UpdateSendPayload(const uint8_t *buff, size_t len);
+int32_t SysMonitor_UpdateSendPayload(SYSMON_SVC_PAYLOAD *payload);
-void SysMonitor_UpdateRecvPayload(const uint8_t *buff, uint32_t id, size_t len);
+void SysMonitor_GetSendPayload(SYSMON_SVC_PAYLOAD *payload);
-void SysMonitor_GetSendPayload(uint8_t *buff, uint32_t *id, size_t len);
+int32_t SysMonitor_GenRecvRsp(SYSMON_SVC_PAYLOAD *payload);
-void SysMonitor_GetRecvPayload(uint8_t *buff, uint32_t *id, size_t len);
 
 void SysMonitor_UpdateLinklayerLog(const LINKLOG *log);
 void SysMonitor_UpdatePhysicalLayerLog(const PHYLOG *log);

src/systembus/systembus.c

@@ -46,7 +46,7 @@
 // -- Production type
 #define WD_PRD 0x01
 #define PRD_DL_K 0xDD
-#define PRD_DL_S 0xEE
+#define PRD_DL_C 0xEE
 #define PRD_ALL 0xFF
 
 // Self-Test
@@ -54,158 +54,104 @@
 // -- Request
 // -- -- --
 // -- Response
+#define ST_RSP_LEN 23
 // -- -- 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
+// -- -- Hardware info offset
-// -- -- Signal processor PS voltage
+#define SP_INFO_OFFSET 0x03
-#define WD_SP_FPGA_VOLT 0x03
+#define ST_INFO_LEN 20
-// -- -- 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
 
-// -- -- Signal processor antenna voltage status
+// Data Echo Test Command
-#define WD_ANT_VOLT 0x09
+#define CMD_ECHO 0xE2
 
-// -- -- Signal processor antenna RF temperature
+// Configuration
-#define WD_ANT_RF_TEMP 0x0A
-// -- -- Signal processor antenna PLL lock status
-#define WD_ANT_RF_PLL 0x0B
-// -- -- Signal processor antenna MGC
-#define WD_ANT_MGC_0 0x0C
-#define WD_ANT_MGC_1 0x0D
-// -- -- Reserved
-#define WD_ANT_RESERVE 0x0E
-
-// Data Send Test Command
-#define CMD_DATA_TEST_A 0xE2
-// -- Request
-#define DATA_TEST_REQ_A_LEN 68
-// -- -- 68 byte payload
-// -- Response
-#define DATA_TEST_RSP_A_LEN 68
-// -- -- 68 byte payload
-
-// Data Send Test Command
-#define CMD_DATA_TEST_B 0xF2
-// -- Request
-#define DATA_TEST_REQ_B_LEN 170
-// -- -- 170 byte payload
-// -- Response
-#define DATA_TEST_RSP_B_LEN 170
-// -- -- 170 byte payload
-
-// Config load
 #define CMD_CONFIG 0xE3
 // -- Request
+// -- -- Enable control
 #define WD_EN_CMD 0x02
-#define EN_CMD_ON_HIGH_POWER 0x66
+#define EN_CMD_ENABLE 0x66
-#define EN_CMD_ON_LOW_POWER 0x77
 #define EN_CMD_SILENT 0x88
-
+// -- -- Node mode control
-#define WD_MODE_CMD 0x03
+#define WD_ID_CMD 0x03
-#define MODE_CMD_HEAD 0x11
+// -- -- Service Rate control
-#define MODE_CMD_MEMBER 0x00
+#define WD_RATE_CMD 0x04
+#define RATE_CMD_128K 0x22
+#define RATE_CMD_512K 0x33
+#define RATE_CMD_2M 0x44
+// -- -- Routing table select
+#define WD_RTNE_CMD 0x05
 // -- Response
 // -- -- none response
 
 // System info update
 #define CMD_SYS_INFO 0x33
-#define SYS_INFO_OFFSET 2
 // -- Request
 // -- -- 54 byte system info
+#define SYS_INFO_OFFSET 0x02
 // -- Response
 // -- -- none response
 
-// Data Send Test Command
+// Data Send Command
-#define CMD_FLJ_SEND 0xE4
+#define CMD_SEND_DATA 0xE4
-#define FLJ_DATA_OFFSET 2
 // -- Request
-// -- -- 78 byte payload
+// -- -- destination ID
+#define SEND_DATA_ID_OFFSET 0x02
+#define SEND_DATA_ID_LEN 4
+#define WD_DST_ID 0x06
+// -- -- data length
+#define SEND_DATA_LEN_OFFSET 0x07
+#define SEND_DATA_LEN_LEN 2
+// -- -- <= 1000 byte payload
+#define SEND_DATA_OFFSET 0x09
 // -- Response
 // -- -- none response
 
-// Data Send Test Command
+// Data Receive Command
-#define CMD_LJ_SEND 0xF4
+#define CMD_RECV_DATA 0xE5
-#define LJ_DATA_OFFSET 2
 // -- Request
-// -- -- 156 byte payload
+// -- -- --
 // -- Response
-// -- -- none response
+// -- -- source ID
+#define RECV_DATA_ID_OFFSET 0x02
+#define RECV_DATA_ID_LEN 4
+#define WD_SRC_ID 0x06
+// -- -- data length
+#define RECV_DATA_LEN_OFFSET 0x07
+#define RECV_DATA_LEN_LEN 2
+// -- -- <= 1000 byte payload
+#define RECV_DATA_OFFSET 0x09
 
-// FLJ Poll
+// Remote Test
-#define CMD_FLJ_POLL 0xE5
+#define CMD_RM_TEST 0xE6
 // -- Request
-#define WD_FLJ_POLL 0x02
+// -- -- --
-#define FLJ_POLL_CODE 0x77
 // -- Response
-#define FLJ_POLL_RESP_LEN 170
+#define RM_TEST_RSP_LEN 190
-// -- -- received 170 byte payload
+// -- -- 8 byte time stamp
-
+#define RT_TIME_STAMP_OFFSET 0x02
-// LJ Poll
+#define RT_TIME_STAMP_LEN 8
-#define CMD_LJ_POLL 0xF5
+// -- -- 16 byte system bus log
-// -- Request
+#define RT_SYSBUS_LOG_OFFSET 0x0A
-#define WD_LJ_POLL 0x02
+#define RT_SYSBUS_LOG_LEN 16
-#define LJ_POLL_CODE 0x77
+// -- -- 88 byte link layer log
-// -- Response
+#define RT_LINK_LOG_OFFSET 0x1A
-#define LJ_POLL_RESP_LEN 68
+#define RT_LINK_LOG_LEN 88
-// -- -- received 68 byte payload
+// -- -- 56 byte physical layer log
-
+#define RT_PHY_LOG_OFFSET 0x72
-// Remote Test FLJ
+#define RT_PHY_LOG_LEN 56
-#define CMD_FLJ_RM_TEST 0xE6
+// -- -- 20 byte hardware log
-// -- Request
+#define RT_HARDWARE_ST_OFFSET 0xAA
-#define WD_FLJ_RM_TEST 0x02
+#define RT_HARDWARE_ST_LEN 20
-#define FLJ_RM_TEST_CODE 0x99
-// -- Response
-#define WD_FLJ_TIME_STAMP 0x02
-#define WD_FLJ_TIME_STAMP_LEN 8
-#define WD_FLJ_SYSBUS_LOG 0x0A
-#define WD_FLJ_SYSBUS_LOG_LEN 14
-#define WD_FLJ_LINK_LOG 0x18
-#define WD_FLJ_LINK_LOG_LEN 64
-#define WD_FLJ_PHY_LOG 0x58
-#define WD_FLJ_PHY_LOG_LEN 57
-#define WD_FLJ_HARDWARE_ST 0x91
-#define WD_FLJ_HARDWARE_ST_LEN 13
-
-#define FLJ_RM_TEST_RESP_LEN 158
-
-// Remote Test LJ
-#define CMD_LJ_RM_TEST 0xF6
-// -- Request
-#define WD_LJ_RM_TEST 0x02
-#define LJ_RM_TEST_CODE 0x99
-// -- Response
-#define WD_LJ_TIME_STAMP 0x02
-#define WD_LJ_TIME_STAMP_LEN 8
-#define WD_LJ_SYSBUS_LOG 0x0A
-#define WD_LJ_SYSBUS_LOG_LEN 14
-#define WD_LJ_LINK_LOG 0x18
-#define WD_LJ_LINK_LOG_LEN 64
-#define WD_LJ_PHY_LOG 0x58
-#define WD_LJ_PHY_LOG_LEN 57
-#define WD_LJ_HARDWARE_ST 0x91
-#define WD_LJ_HARDWARE_ST_LEN 13
-
-#define LJ_RM_TEST_RESP_LEN 158
 
 // My Production Type
-#define MY_PRD_ID PRD_DL_K
+#define MY_PRD_ID PRD_DL_C
 /** @} SystemBus_MSG_Constants*/
 /************************** 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 *****************************/
@@ -221,7 +167,7 @@ inline bool SystemBus_IsReqPending(SYSBUS *instPtr)
 	return (!isCircQueueEmpty(instPtr->rxQueue));
 }
 
-inline bool SystemBus_IsRsqPending(SYSBUS *instPtr)
+inline bool SystemBus_IsRespPending(SYSBUS *instPtr)
 {
 	ASSERT(instPtr != NULL);
 	return (!isCircQueueEmpty(instPtr->txQueue));
@@ -265,7 +211,7 @@ static int32_t SystemBus_GenSelfTestRsp(SYSBUS *instPtr)
 		sysbusRsp->data[WD_ST_RET] = (hwStatsPtr->isSpOk) ? 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);
+	memcpy(sysbusRsp->data + SP_INFO_OFFSET, &(hwStatsPtr->spFpgaVolt), ST_INFO_LEN);
 
 	if (CircQueuePush(instPtr->txQueue, (void *)sysbusRsp) == false)
 	{
@@ -287,72 +233,137 @@ static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg)
 	TMC_INST *tmcRef = linkInstPtr->config.tmcInstPtr;
 	bool reconfig = false;
 
-	if (msg[WD_MODE_CMD] == MODE_CMD_HEAD)
+	// Node mode control
+	if (msg[WD_ID_CMD] >= 0 && msg[WD_ID_CMD] < linkConfig.netScale)
 	{
-		// head node config
+		reconfig |= (msg[WD_ID_CMD] != linkConfig.myId);
-		reconfig = (L2_NODE_HEAD != L2_GetNodeType(linkInstPtr));
+		linkConfig.myType = (msg[WD_ID_CMD] == 0) ? L2_NODE_HEAD : L2_NODE_MEMBER;
-		linkConfig.myType = L2_NODE_HEAD;
+		linkConfig.myId = msg[WD_ID_CMD];
-		linkConfig.myId = 1;
-	}
-
-	else if (msg[WD_MODE_CMD] == MODE_CMD_MEMBER)
-	{
-		// member node config
-		reconfig = (L2_NODE_MEMBER != L2_GetNodeType(linkInstPtr));
-		linkConfig.myType = L2_NODE_MEMBER;
-		linkConfig.myId = 2;
 	}
 	else
 	{
 		// Ignore current command
 		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
-				  WD_MODE_CMD, msg[WD_MODE_CMD]);
+				  WD_ID_CMD, msg[WD_ID_CMD]);
 		return EPARAM;
 	}
 
-//	if (msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
+	// Service Rate control
-//	{
+	switch (msg[WD_RATE_CMD])
-//		// full power config
+	{
-//		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL);
+	case RATE_CMD_128K:
-//		if (reconfig || !L2_isStarted(linkInstPtr))
+	{
-//		{
+		reconfig |= (linkConfig.serviceRate != L2_SVC_128K);
-//			Linklayer_Reconfig(linkInstPtr, &linkConfig);
+		linkConfig.serviceRate = L2_SVC_128K;
-//		}
+		break;
-//		Linklayer_Start(linkInstPtr);
+	}
-//	}
+	case RATE_CMD_512K:
-//	else if (msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
+	{
-//	{
+		reconfig |= (linkConfig.serviceRate != L2_SVC_512K);
-//		// low power config
+		linkConfig.serviceRate = L2_SVC_512K;
-//		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
+		break;
-//		if (reconfig || !L2_isStarted(linkInstPtr))
+	}
-//		{
+	case RATE_CMD_2M:
-//			Linklayer_Reconfig(linkInstPtr, &linkConfig);
+	{
-//		}
+		reconfig |= (linkConfig.serviceRate != L2_SVC_2M);
-//		Linklayer_Start(linkInstPtr);
+		linkConfig.serviceRate = L2_SVC_2M;
-//	}
+		break;
-//	else if (msg[WD_EN_CMD] == EN_CMD_SILENT)
+	}
-//	{
+	default:
-//		// silence config
+	{
-//		// TODO: better approach?
+		// Ignore current command
-//		Linklayer_Halt(linkInstPtr);
+		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
-//		Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY);
+				  WD_RATE_CMD, msg[WD_RATE_CMD]);
-//		TIME_WORD pullTime = {0, 0};
+		return EPARAM;
-//		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");
+	// Enable control
-//		}
+	if (msg[WD_EN_CMD] == EN_CMD_ENABLE)
-//		Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
+	{
-//	}
+		if (reconfig || !L2_isStarted(linkInstPtr))
-	// else
+		{
-	// {
+			Linklayer_Reconfig(linkInstPtr, &linkConfig);
-	// 	// Ignore current command
+		}
-	// 	DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
+		Linklayer_Start(linkInstPtr);
-	// 			  WD_EN_CMD, msg[WD_EN_CMD]);
+	}
-	// 	return EIGNORE;
+	else if (msg[WD_EN_CMD] == EN_CMD_SILENT)
-	// }
+	{
-	instPtr->sysbusLog.lastConfig[0] = msg[WD_MODE_CMD];
+		Linklayer_Halt(linkInstPtr);
-	instPtr->sysbusLog.lastConfig[1] = msg[WD_EN_CMD];
+		// configure phy to generate remote test
+		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_FRAME_ADV_24MS, NULL);
+	}
+	else
+	{
+		// Ignore current command
+		DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
+				  WD_EN_CMD, msg[WD_EN_CMD]);
+		return EIGNORE;
+	}
+
+	// Routing table selection
+	if (msg[WD_RTNE_CMD] != instPtr->sysbusLog.lastConfig[3])
+	{
+		L2_RTNE_CONFIG newRtneConfig;
+		switch (msg[WD_RTNE_CMD])
+		{
+		case 0:
+		{
+			newRtneConfig.rtneTable = normalTable0;
+			newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable0);
+			L2_EVENT l2Event =
+			{
+				.event = L2_ROUTINE_UPDATE,
+				.msg = &newRtneConfig
+			};
+			AssertL2Event(instPtr, &l2Event);
+			break;
+		}
+		case 1:
+		{
+			newRtneConfig.rtneTable = normalTable1;
+			newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable1);
+			L2_EVENT l2Event =
+			{
+				.event = L2_ROUTINE_UPDATE,
+				.msg = &newRtneConfig
+			};
+			AssertL2Event(instPtr, &l2Event);
+			break;
+		}
+		case 2:
+		{
+			newRtneConfig.rtneTable = normalTable2;
+			newRtneConfig.rtnePrd = ARRAY_SIZE(normalTable2);
+			L2_EVENT l2Event =
+			{
+				.event = L2_ROUTINE_UPDATE,
+				.msg = &newRtneConfig
+			};
+			AssertL2Event(instPtr, &l2Event);
+			break;
+		}
+		default:
+		{
+			// Ignore current command
+			DBG_ERROR("Unexpected config command 0x%02X:0x%02X",
+					WD_RTNE_CMD, msg[WD_RTNE_CMD]);
+			return EIGNORE;
+		}
+		}
+	}
+
+	instPtr->sysbusLog.lastConfig[0] = msg[WD_EN_CMD];
+	instPtr->sysbusLog.lastConfig[1] = msg[WD_ID_CMD];
+	instPtr->sysbusLog.lastConfig[2] = msg[WD_RATE_CMD];
+	instPtr->sysbusLog.lastConfig[3] = msg[WD_RTNE_CMD];
+
 	return SUCCESS;
 }
 
@@ -366,7 +377,7 @@ static int32_t SystemBus_Echo(SYSBUS *instPtr, SYSBUS_UART_FRAME *frame)
 	SYSBUS_UART_FRAME *sysbusRsp = malloc(sizeof(SYSBUS_UART_FRAME));
 	if (sysbusRsp == NULL)
 	{
-		DBG_ERROR("Failed to allocate memory for DATA TEST A Response!\n");
+		DBG_ERROR("Failed to allocate memory for ECHO TEST Response!\n");
 		return ENOMEM;
 	}
 	sysbusRsp->len = frame->len;
@@ -381,58 +392,33 @@ static int32_t SystemBus_Echo(SYSBUS *instPtr, SYSBUS_UART_FRAME *frame)
 }
 
 /**
- * @brief   Generate System Bus poll call command response
+ * @brief   Generate System Bus receive call command response
  */
-static int32_t SystemBus_GenPollRsp(SYSBUS *instPtr, uint8_t *msg)
+int32_t SystemBus_GenRecvRsp(SYSBUS *instPtr, SYSMON_SVC_PAYLOAD *payload)
 {
 	ASSERT(instPtr != NULL);
-	ASSERT(msg != NULL);
+	ASSERT(payload != 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");
+		DBG_ERROR("Failed to allocate memory for Receive Response!\n");
 		return ENOMEM;
 	}
 
-	if (msg[WD_CMD] == CMD_FLJ_POLL)
+	sysbusRsp->len = payload->dataLen + RECV_DATA_LEN_OFFSET;
-	{
+
-		sysbusRsp->len = FLJ_POLL_RESP_LEN;
+	sysbusRsp->data[WD_CMD] = CMD_RECV_DATA;
-		sysbusRsp->data[WD_CMD] = CMD_FLJ_POLL;
 	sysbusRsp->data[WD_PRD] = MY_PRD_ID;
-		dataLen = PAYLOAD_LEN_FLJ_RX;
+	memcpy(sysbusRsp->data + RECV_DATA_ID_OFFSET, payload, sizeof(SYSMON_SVC_PAYLOAD));
-		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++;
+	instPtr->sysbusLog.recvCnt++;
 	return SUCCESS;
 }
 
@@ -455,36 +441,21 @@ static int32_t SystemBus_GenRemoteTestRsp(SYSBUS *instPtr, uint8_t *msg)
 	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)
+	if (msg[WD_CMD] == CMD_RM_TEST)
 	{
-		sysbusRsp->len = FLJ_RM_TEST_RESP_LEN;
+		sysbusRsp->len = RM_TEST_RSP_LEN;
-		memset(sysbusRsp->data, 0x00, FLJ_RM_TEST_RESP_LEN);
+		memset(sysbusRsp->data, 0x00, RM_TEST_RSP_LEN);
-		sysbusRsp->data[WD_CMD] = CMD_FLJ_RM_TEST;
+		sysbusRsp->data[WD_CMD] = CMD_RM_TEST;
 		sysbusRsp->data[WD_PRD] = MY_PRD_ID;
 
-		memcpy(sysbusRsp->data + WD_FLJ_TIME_STAMP, &time, WD_FLJ_TIME_STAMP_LEN);
+		memcpy(sysbusRsp->data + RT_TIME_STAMP_OFFSET, &time, RT_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 + RT_SYSBUS_LOG_OFFSET, &(instPtr->sysbusLog), RT_SYSBUS_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_FLJ_LINK_LOG, &(instPtr->linkLog), WD_FLJ_LINK_LOG_LEN);
+		memcpy(sysbusRsp->data + RT_LINK_LOG_OFFSET, &(instPtr->linkLog), RT_LINK_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_FLJ_PHY_LOG, &(instPtr->phyLog), WD_FLJ_PHY_LOG_LEN);
+		memcpy(sysbusRsp->data + RT_PHY_LOG_OFFSET, &(instPtr->phyLog), RT_PHY_LOG_LEN);
-		memcpy(sysbusRsp->data + WD_FLJ_HARDWARE_ST, &(instPtr->hwState.spFpgaVolt), WD_FLJ_HARDWARE_ST_LEN);
+		memcpy(sysbusRsp->data + RT_HARDWARE_ST_OFFSET, &(instPtr->hwState.spFpgaVolt), RT_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
 	{
@@ -538,8 +509,7 @@ int32_t SystemBus_HandleCmd(SYSBUS *instPtr)
 		Status = SystemBus_GenSelfTestRsp(instPtr);
 		break;
 	}
-	case CMD_DATA_TEST_A:
+	case CMD_ECHO:
-	case CMD_DATA_TEST_B:
 	{
 		Status = SystemBus_Echo(instPtr, sysbusReq);
 		break;
@@ -555,61 +525,17 @@ int32_t SystemBus_HandleCmd(SYSBUS *instPtr)
 		Status = SUCCESS;
 		break;
 	}
-	case CMD_FLJ_SEND:
+	case CMD_SEND_DATA:
 	{
-		SysMonitor_UpdateSendPayload(msg + FLJ_DATA_OFFSET, PAYLOAD_LEN_FLJ_TX); // omit control info
+		Status = SysMonitor_UpdateSendPayload((SYSMON_SVC_PAYLOAD *)(msg + SEND_DATA_ID_OFFSET));
-		instPtr->sysbusLog.svcCnt++;
+		if (Status == SUCCESS)
-		Status = SUCCESS;
+		{
+			instPtr->sysbusLog.sendCnt++;
+		}
 		break;
 	}
-	case CMD_LJ_SEND:
+	case CMD_RM_TEST:
 	{
-		SysMonitor_UpdateSendPayload(msg + LJ_DATA_OFFSET, PAYLOAD_LEN_LJ_TX); // omit control info
-		instPtr->sysbusLog.svcCnt++;
-		Status = SUCCESS;
-		break;
-	}
-	case CMD_FLJ_POLL:
-	{
-		if (msg[WD_FLJ_POLL] != FLJ_POLL_CODE)
-		{
-			DBG_ERROR("SystemBus::Invalid roll call code!\n");
-			Status = EIGNORE;
-			break;
-		}
-		Status = SystemBus_GenPollRsp(instPtr, msg);
-		break;
-	}
-	case CMD_LJ_POLL:
-	{
-		if (msg[WD_LJ_POLL] != LJ_POLL_CODE)
-		{
-			DBG_WARNING("SystemBus::Invalid roll call code!\n");
-			Status = EIGNORE;
-			break;
-		}
-		Status = SystemBus_GenPollRsp(instPtr, msg);
-		break;
-	}
-	case CMD_FLJ_RM_TEST:
-	{
-		if (msg[WD_FLJ_RM_TEST] != FLJ_RM_TEST_CODE)
-		{
-			DBG_WARNING("SystemBus::Invalid remote test code!\n");
-			Status = EIGNORE;
-			break;
-		}
-		Status = SystemBus_GenRemoteTestRsp(instPtr, msg);
-		break;
-	}
-	case CMD_LJ_RM_TEST:
-	{
-		if (msg[WD_LJ_RM_TEST] != LJ_RM_TEST_CODE)
-		{
-			DBG_WARNING("SystemBus::Invalid remote test code!\n");
-			Status = EIGNORE;
-			break;
-		}
 		Status = SystemBus_GenRemoteTestRsp(instPtr, msg);
 		break;
 	}

src/systembus/systembus.h

@@ -59,6 +59,7 @@ typedef struct
 
 /************************** Function Prototypes ******************************/
 
+int32_t SystemBus_GenRecvRsp(SYSBUS *instPtr, SYSMON_SVC_PAYLOAD *payload);
 int32_t SystemBus_Init(SYSBUS **instPtr, SYSBUS_CONFIG *config);
 void SystemBus_Destroy(SYSBUS *instPtr);
 
@@ -74,7 +75,7 @@ int32_t SystemBus_TxIntrHandler(SYSBUS *instPtr);
 int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr);
 
 bool SystemBus_IsReqPending(SYSBUS *instPtr);
-bool SystemBus_IsRsqPending(SYSBUS *instPtr);
+bool SystemBus_IsRespPending(SYSBUS *instPtr);
 
 bool SystemBus_IsTxBusy(SYSBUS *instPtr);
 bool SystemBus_IsRxBusy(SYSBUS *instPtr);

src/systembus/systembus_uart.c

@@ -100,8 +100,8 @@ int SysbusUart_Send(SYSBUS_UART *instPtr, SYSBUS_UART_FRAME *frame)
 {
 	ASSERT(instPtr != NULL);
 	ASSERT(frame != NULL);
-	uint8_t dLen, checkSum;
+	uint8_t checkSum;
-	uint8_t sendLen;
+	uint16_t dLen, sendLen;
 	uint8_t *sendBuff;
 	if (frame->len > UART_TXBUFF_MAX || frame->len == 0)
 	{
@@ -168,7 +168,8 @@ int SysbusUart_Recv(SYSBUS_UART *instPtr, SYSBUS_UART_FRAME *frame)
 	ASSERT(instPtr != NULL);
 	ASSERT(frame != NULL);
 
-	uint8_t dLen, dWLen, plLen, checkSum, checkSumCal;
+	uint16_t dLen, dWLen, plLen;
+	uint8_t checkSum, checkSumCal;
 	uint8_t *plBuff, *dBuff;
 	dLen = SYSTEMBUS_UART_mReadReg(instPtr->config.baseAddr, UART_RECV_CNT);
 	if (dLen == 0 || dLen > UART_TXBUFF_MAX)

src/systembus/systembus_uart.h

@@ -96,7 +96,7 @@ typedef struct
 	uint16_t len;
 	uint8_t checkSum;
 	uint8_t isValid;
-	uint8_t data[256];
+	uint8_t data[1024];
 } SYSBUS_UART_FRAME;
 
 /************************** Function Prototypes ******************************/