ncsi: Update NC-SI to only use one port declared at compile time. (#70)

4 files changed, 102 insertions, 126 deletions

diff --git a/libs/NCSI/ncsi.c b/libs/NCSI/ncsi.c
index 511371f..b2a5c9c 100644
--- a/libs/NCSI/ncsi.c
+++ b/libs/NCSI/ncsi.c
@@ -59,7 +59,6 @@
 #define debug(...) printf(__VA_ARGS__)
 
 #define NUM_CHANNELS 1
-#define MAX_CHANNELS 4
 
 #define PACKAGE_ID_SHIFT 5
 #define PACKAGE_ID_MASK (0xE0)
@@ -235,24 +234,18 @@ typedef struct
 {
     bool selected;
     int numChannels;
-    NetworkPort_t *port[MAX_CHANNELS];
+    NetworkPort_t *port;
 } package_state_t;
 
 package_state_t gPackageState = {
     .selected = false,
-    .numChannels = NUM_CHANNELS,
-    .port = {
-        [0] = &gPort0,
-        [1] = &gPort1,
-        [2] = &gPort2,
-        [3] = &gPort3,
-    },
+    .port = &gPort,
 };
 
 void sendNCSIResponse(uint8_t InstanceID, uint8_t channelID, uint16_t controlID, uint16_t response_code, uint16_t reasons_code);
 void sendNCSILinkStatusResponse(uint8_t InstanceID, uint8_t channelID, uint32_t LinkStatus, uint32_t OEMLinkStatus, uint32_t OtherIndications);
 
-void resetChannel(int ch);
+void resetChannel(NetworkPort_t *port);
 
 #if CXX_SIMULATOR
 #include <stdio.h>
@@ -264,7 +257,6 @@ typedef struct
     bool packageCommand;
     int payloadLength;
     void (*fn)(NetworkFrame_t *);
-
 } ncsi_handler_t;
 
 void unknownHandler(NetworkFrame_t *frame)
@@ -283,10 +275,8 @@ void unknownHandler(NetworkFrame_t *frame)
 
 static void clearInitialStateHandler(NetworkFrame_t *frame)
 {
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-
-    gPackageState.port[ch]->shm_channel->NcsiChannelInfo.bits.Ready = true;
-    debug("Clear initial state: channel %x\n", ch);
+    gPackageState.port->shm_channel->NcsiChannelInfo.bits.Ready = true;
+    debug("Clear initial state\n");
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -310,12 +300,12 @@ static void deselectPackageHandler(NetworkFrame_t *frame)
 
 static void enableChannelHandler(NetworkFrame_t *frame)
 {
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
+    NetworkPort_t *port = gPackageState.port;
 
-    debug("Enable Channel: %x\n", ch);
-    gPackageState.port[ch]->shm_channel->NcsiChannelInfo.bits.Enabled = true;
+    debug("Enable Channel\n");
+    port->shm_channel->NcsiChannelInfo.bits.Enabled = true;
 
-    Network_InitPort(gPackageState.port[ch], AS_NEEDED);
+    Network_InitPort(port, AS_NEEDED);
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -323,10 +313,10 @@ static void enableChannelHandler(NetworkFrame_t *frame)
 
 static void disableChannelHandler(NetworkFrame_t *frame)
 {
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
+    NetworkPort_t *port = gPackageState.port;
 
-    debug("Disable Channel: %x\n", ch);
-    gPackageState.port[ch]->shm_channel->NcsiChannelInfo.bits.Enabled = false;
+    debug("Disable Channel\n");
+    port->shm_channel->NcsiChannelInfo.bits.Enabled = false;
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -334,10 +324,10 @@ static void disableChannelHandler(NetworkFrame_t *frame)
 
 static void resetChannelHandler(NetworkFrame_t *frame)
 {
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
+    NetworkPort_t *port = gPackageState.port;
 
-    debug("Reset Channel: %x\n", ch);
-    resetChannel(ch);
+    debug("Reset Channel\n");
+    resetChannel(port);
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -345,10 +335,10 @@ static void resetChannelHandler(NetworkFrame_t *frame)
 
 static void enableChannelNetworkTXHandler(NetworkFrame_t *frame)
 {
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
+    NetworkPort_t *port = gPackageState.port;
 
-    debug("Enable Channel Network TX: channel %x\n", ch);
-    gPackageState.port[ch]->shm_channel->NcsiChannelInfo.bits.TXPassthrough = true;
+    debug("Enable Channel Network TX\n");
+    port->shm_channel->NcsiChannelInfo.bits.TXPassthrough = true;
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -356,10 +346,10 @@ static void enableChannelNetworkTXHandler(NetworkFrame_t *frame)
 
 static void disableChannelNetworkTXHandler(NetworkFrame_t *frame)
 {
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
+    NetworkPort_t *port = gPackageState.port;
 
-    debug("Disable Channel Network TX: %x\n", ch);
-    gPackageState.port[ch]->shm_channel->NcsiChannelInfo.bits.TXPassthrough = false;
+    debug("Disable Channel Network TX\n");
+    port->shm_channel->NcsiChannelInfo.bits.TXPassthrough = false;
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -367,13 +357,13 @@ static void disableChannelNetworkTXHandler(NetworkFrame_t *frame)
 
 static void AENEnableHandler(NetworkFrame_t *frame)
 {
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
+    NetworkPort_t *port = gPackageState.port;
     uint32_t AENControl = (frame->AENEnable.AENControl_Low | (frame->AENEnable.AENControl_High << 16));
     debug("AEN Enable: AEN_MC_ID %x\n", frame->AENEnable.AEN_MC_ID);
     debug("AEN Enable: AENControl %x\n", AENControl);
 
-    gPackageState.port[ch]->shm_channel->NcsiChannelMcid.r32 = frame->AENEnable.AEN_MC_ID;
-    gPackageState.port[ch]->shm_channel->NcsiChannelAen.r32 = AENControl;
+    port->shm_channel->NcsiChannelMcid.r32 = frame->AENEnable.AEN_MC_ID;
+    port->shm_channel->NcsiChannelAen.r32 = AENControl;
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -381,13 +371,13 @@ static void AENEnableHandler(NetworkFrame_t *frame)
 
 static void setLinkHandler(NetworkFrame_t *frame)
 {
+    NetworkPort_t *port = gPackageState.port;
     uint32_t LinkSettings = (frame->setLink.LinkSettings_Low | (frame->setLink.LinkSettings_High << 16));
     uint32_t OEMLinkSettings = (frame->setLink.OEMLinkSettings_Low | (frame->setLink.OEMLinkSettings_High << 16));
+
     debug("Set Link: LinkSettings %x\n", LinkSettings);
     debug("Set Link: OEMLinkSettings %x\n", OEMLinkSettings);
 
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    NetworkPort_t *port = gPackageState.port[ch];
     port->shm_channel->NcsiChannelSetting1.r32 = LinkSettings;
     port->shm_channel->NcsiChannelSetting2.r32 = OEMLinkSettings;
 
@@ -399,8 +389,7 @@ static void getLinkStatusHandler(NetworkFrame_t *frame)
 {
     RegMIIAuxiliaryStatusSummary_t stat;
 
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    NetworkPort_t *port = gPackageState.port[ch];
+    NetworkPort_t *port = gPackageState.port;
     uint8_t phy = MII_getPhy(port->device);
 
     uint32_t rx = port->shm_channel->NcsiChannelCtrlstatAllRx.r32;
@@ -414,7 +403,8 @@ static void getLinkStatusHandler(NetworkFrame_t *frame)
 
     uint32_t tx_used = APE_TX_TO_NET_BUFFER_RING_FREE_MAX - port->tx_ring->bits.Free;
     uint32_t rx_avail = APE_RX_POOL_FREE_POINTER_FREE_COUNT_MAX - port->rx_ring->bits.FreeCount;
-    debug("Link Status [%d] %s, TX %d [%d used], RX %d [%d avail]\n", frame->controlPacket.ChannelID, stat.bits.LinkStatus ? "up" : "down", tx, tx_used, rx, rx_avail);
+    debug("Link Status %s, TX %d [%d used], RX %d [%d avail]\n", stat.bits.LinkStatus ? "up" : "down", tx, tx_used, rx,
+          rx_avail);
 
     if (!stat.bits.LinkStatus)
     {
@@ -442,11 +432,10 @@ static void getLinkStatusHandler(NetworkFrame_t *frame)
 static void disableVLANHandler(NetworkFrame_t *frame)
 {
     // TODO
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    NetworkPort_t *port = gPackageState.port[ch];
+    NetworkPort_t *port = gPackageState.port;
     port->shm_channel->NcsiChannelInfo.bits.VLAN = false;
 
-    debug("Disable VLAN: channel %x\n", ch);
+    debug("Disable VLAN\n");
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -455,7 +444,7 @@ static void disableVLANHandler(NetworkFrame_t *frame)
 static void getCapabilities(NetworkFrame_t *frame)
 {
     int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    // NetworkPort_t *port = gPackageState.port[ch];
+    // NetworkPort_t *port = gPackageState.port;
     uint32_t packetSize = MAX(sizeof(gCapabilitiesFrame.capabilities), ETHERNET_FRAME_MIN - 4);
 
     gCapabilitiesFrame.capabilities.ChannelID = ch;
@@ -470,7 +459,7 @@ static void getCapabilities(NetworkFrame_t *frame)
 static void getVersionID(NetworkFrame_t *frame)
 {
     int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    NetworkPort_t *port = gPackageState.port[ch];
+    NetworkPort_t *port = gPackageState.port;
     DEVICE_t *device = (DEVICE_t *)port->device;
     uint32_t packetSize = MAX(sizeof(gVersionFrame.version), ETHERNET_FRAME_MIN - 4);
 
@@ -491,11 +480,10 @@ static void getVersionID(NetworkFrame_t *frame)
 static void enableVLANHandler(NetworkFrame_t *frame)
 {
     // TODO
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    NetworkPort_t *port = gPackageState.port[ch];
+    NetworkPort_t *port = gPackageState.port;
     port->shm_channel->NcsiChannelInfo.bits.VLAN = false;
 
-    debug("Enable VLAN: channel %x\n", ch);
+    debug("Enable VLAN\n");
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -504,11 +492,10 @@ static void enableVLANHandler(NetworkFrame_t *frame)
 static void setVLANFilter(NetworkFrame_t *frame)
 {
     // TODO
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    NetworkPort_t *port = gPackageState.port[ch];
+    NetworkPort_t *port = gPackageState.port;
     port->shm_channel->NcsiChannelInfo.bits.VLAN = false;
 
-    debug("Set VLAN Filter: channel %x\n", ch);
+    debug("Set VLAN Filter\n");
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -516,11 +503,9 @@ static void setVLANFilter(NetworkFrame_t *frame)
 
 static void setMACAddressHandler(NetworkFrame_t *frame)
 {
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    NetworkPort_t *port = gPackageState.port[ch];
-    // port->shm_channel->NcsiChannelInfo.bits.Enabled = false;
+    NetworkPort_t *port = gPackageState.port;
 
-    debug("Set MAC: channel %x\n", ch);
+    debug("Set MAC\n");
     debug("  MAC: 0x%04x%04x%04x\n", frame->setMACAddr.MAC54, frame->setMACAddr.MAC32, frame->setMACAddr.MAC10);
     debug("  Enable: %d\n", frame->setMACAddr.Enable);
     debug("  AT: %d\n", frame->setMACAddr.AT);
@@ -544,11 +529,7 @@ static void setMACAddressHandler(NetworkFrame_t *frame)
 static void enableBroadcastFilteringHandler(NetworkFrame_t *frame)
 {
     // TODO
-    // channel_state_t* channel = gPackageState.port[ch];
-    // port->shm_channel->NcsiChannelInfo.bits.Enabled = false;
-
-    int ch = frame->controlPacket.ChannelID & CHANNEL_ID_MASK;
-    debug("Enable Broadcast Filtering: channel %x\n", ch);
+    debug("Enable Broadcast Filtering\n");
 
     sendNCSIResponse(frame->controlPacket.InstanceID, frame->controlPacket.ChannelID, frame->controlPacket.ControlPacketType,
                      NCSI_RESPONSE_CODE_COMMAND_COMPLETE, NCSI_REASON_CODE_NONE);
@@ -558,9 +539,9 @@ static void enableBroadcastFilteringHandler(NetworkFrame_t *frame)
 // AEN ENABLE, SET LINK;   then you need GET LINK STATUS
 ncsi_handler_t gNCSIHandlers[] = {
     /* Package / Initialization commands */
-    [0x00] = { .ignoreInit = true,  .packageCommand = false, .payloadLength = 0, .fn = clearInitialStateHandler },
-    [0x01] = { .ignoreInit = false, .packageCommand = true,  .payloadLength = 4, .fn = selectPackageHandler },
-    [0x02] = { .ignoreInit = false, .packageCommand = true,  .payloadLength = 0, .fn = deselectPackageHandler },
+    [0x00] = { .ignoreInit = true, .packageCommand = false, .payloadLength = 0, .fn = clearInitialStateHandler },
+    [0x01] = { .ignoreInit = false, .packageCommand = true, .payloadLength = 4, .fn = selectPackageHandler },
+    [0x02] = { .ignoreInit = false, .packageCommand = true, .payloadLength = 0, .fn = deselectPackageHandler },
 
     /* Channel Specific commands */
     [0x03] = { .ignoreInit = false, .packageCommand = false, .payloadLength = 0, .fn = enableChannelHandler },
@@ -602,7 +583,7 @@ void handleNCSIFrame(NetworkFrame_t *frame)
     uint8_t command = frame->controlPacket.ControlPacketType;
     uint16_t payloadLength = frame->controlPacket.PayloadLength;
     ncsi_handler_t *handler = &gNCSIHandlers[command];
-    NetworkPort_t *port = ((ch >= gPackageState.numChannels) ? 0 : gPackageState.port[ch]);
+    NetworkPort_t *port = gPackageState.port;
 
     if (handler->fn)
     {
@@ -631,7 +612,7 @@ void handleNCSIFrame(NetworkFrame_t *frame)
         }
         else
         {
-            if (ch >= gPackageState.numChannels)
+            if (ch >= NUM_CHANNELS)
             {
 
                 debug("[%x] Invalid channel: %d\n", command, ch);
@@ -642,7 +623,7 @@ void handleNCSIFrame(NetworkFrame_t *frame)
             else
             {
                 gPackageState.selected = true;
-                if (false == gPackageState.port[ch]->shm_channel->NcsiChannelInfo.bits.Ready)
+                if (false == port->shm_channel->NcsiChannelInfo.bits.Ready)
                 {
                     debug("[%x] Channel not initialized: %d\n", command, ch);
                     // Initialization required for the channel
@@ -671,35 +652,28 @@ void handleNCSIFrame(NetworkFrame_t *frame)
     }
 }
 
-void resetChannel(int ch)
+void resetChannel(NetworkPort_t *port)
 {
-    NetworkPort_t *port = gPackageState.port[ch];
-
     port->shm_channel->NcsiChannelInfo.r32 = 0;
     port->shm_channel->NcsiChannelCtrlstatRx.r32 = 0;
     port->shm_channel->NcsiChannelCtrlstatAllTx.r32 = 0;
     port->shm_channel->NcsiChannelCtrlstatAllRx.r32 = 0;
     port->shm_channel->NcsiChannelInfo.bits.Ready = false;
 
-    uint8_t phy = MII_getPhy(port->device);
-    APE_aquireLock();
-    MII_writeRegister(port->device, phy, (mii_reg_t)REG_MII_CONTROL, MII_CONTROL_RESET_MASK);
-    APE_releaseLock();
+    Network_resetLink(port);
 }
 
-void reloadChannel(int ch, reload_type_t reset_phy)
+void reloadChannel(NetworkPort_t *port, reload_type_t reset_phy)
 {
-    NetworkPort_t *port = gPackageState.port[ch];
-
     uint32_t low = port->shm_channel->NcsiChannelMac0Mid.r32 << 16 | port->shm_channel->NcsiChannelMac0Low.r32;
     uint16_t high = port->shm_channel->NcsiChannelMac0High.r32;
     Network_SetMACAddr(port, high, low, /* TBD */ 0, 1);
 
-    if (gPackageState.port[ch]->shm_channel->NcsiChannelInfo.bits.Enabled)
+    if (port->shm_channel->NcsiChannelInfo.bits.Enabled)
     {
-        printf("[ch %d] Reusing MAC: 0x%02X%04X\n", ch, high, low);
+        printf("Reusing MAC: 0x%02X%04X\n", high, low);
 
-        Network_InitPort(gPackageState.port[ch], reset_phy);
+        Network_InitPort(port, reset_phy);
     }
 }
 
@@ -769,31 +743,25 @@ void sendNCSIResponse(uint8_t InstanceID, uint8_t channelID, uint16_t controlID,
 void NCSI_init(void)
 {
     debug("Resetting channels...\n");
-    for (int i = 0; i < ARRAY_ELEMENTS(gPackageState.port); i++)
-    {
-        resetChannel(i);
-    }
+
+    NetworkPort_t *port = gPackageState.port;
+    resetChannel(port);
     SHM.SegSig.r32 = 0; // (1 << command);
 }
 
 void NCSI_reload(reload_type_t reset_phy)
 {
-    for (int i = 0; i < ARRAY_ELEMENTS(gPackageState.port); i++)
-    {
-        reloadChannel(i, reset_phy);
-    }
+    NetworkPort_t *port = gPackageState.port;
+    reloadChannel(port, reset_phy);
 }
 
 void NCSI_handlePassthrough(void)
 {
-    for (int ch = 0; ch < ARRAY_ELEMENTS(gPackageState.port); ch++)
+    NetworkPort_t *port = gPackageState.port;
+    VOLATILE SHM_CHANNEL_t *shm_ch = port->shm_channel;
+    if (shm_ch->NcsiChannelInfo.bits.Ready)
     {
-        NetworkPort_t *port = gPackageState.port[ch];
-        VOLATILE SHM_CHANNEL_t *shm_ch = port->shm_channel;
-        if (shm_ch->NcsiChannelInfo.bits.Ready)
-        {
-            Network_PassthroughRxPatcket(port);
-            ++shm_ch->NcsiChannelCtrlstatAllTx.r32;
-        }
+        Network_PassthroughRxPatcket(port);
+        ++shm_ch->NcsiChannelCtrlstatAllTx.r32;
     }
 }
diff --git a/libs/Network/include/Network.h b/libs/Network/include/Network.h
index 24e8e77..86b62a9 100644
--- a/libs/Network/include/Network.h
+++ b/libs/Network/include/Network.h
@@ -107,10 +107,7 @@ typedef enum {
     ALWAYS_RESET /* Host was just turned off, reset phy */
 } reload_type_t;
 
-extern NetworkPort_t gPort0;
-extern NetworkPort_t gPort1;
-extern NetworkPort_t gPort2;
-extern NetworkPort_t gPort3;
+extern NetworkPort_t gPort;
 
 void Network_InitPort(NetworkPort_t *port, reload_type_t force_reset);
 
diff --git a/libs/Network/ports.c b/libs/Network/ports.c
index 2b0c614..db6fcab 100644
--- a/libs/Network/ports.c
+++ b/libs/Network/ports.c
@@ -68,7 +68,12 @@
 #include <printf.h>
 #endif
 
-NetworkPort_t gPort0 = {
+#ifndef NETWORK_PORT
+#define NETWORK_PORT 0
+#endif
+
+#if NETWORK_PORT == 0
+NetworkPort_t gPort = {
     .device = &DEVICE,
     .filters = &FILTERS0,
     .shm_channel = &SHM_CHANNEL0,
@@ -94,8 +99,8 @@ NetworkPort_t gPort0 = {
     },
 #endif
 };
-
-NetworkPort_t gPort1 = {
+#elif NETWORK_PORT == 1
+NetworkPort_t gPort = {
     .device = &DEVICE1,
     .filters = &FILTERS1,
     .shm_channel = &SHM_CHANNEL1,
@@ -121,8 +126,8 @@ NetworkPort_t gPort1 = {
     },
 #endif
 };
-
-NetworkPort_t gPort2 = {
+#elif NETWORK_PORT == 2
+NetworkPort_t gPort = {
     .device = &DEVICE2,
     .filters = &FILTERS2,
     .shm_channel = &SHM_CHANNEL2,
@@ -148,8 +153,8 @@ NetworkPort_t gPort2 = {
     },
 #endif
 };
-
-NetworkPort_t gPort3 = {
+#elif NETWORK_PORT == 3
+NetworkPort_t gPort = {
     .device = &DEVICE3,
     .filters = &FILTERS3,
     .shm_channel = &SHM_CHANNEL3,
@@ -175,6 +180,7 @@ NetworkPort_t gPort3 = {
     },
 #endif
 };
+#endif
 
 #ifndef CXX_SIMULATOR
 typedef struct
diff --git a/libs/Network/rx.c b/libs/Network/rx.c
index ce2d277..463e6aa 100644
--- a/libs/Network/rx.c
+++ b/libs/Network/rx.c
@@ -51,6 +51,8 @@
 
 #ifdef CXX_SIMULATOR
 #include <stdio.h>
+#else
+#include <printf.h>
 #endif
 
 bool Network_RxLePatcket(uint32_t *buffer, uint32_t *bytes, NetworkPort_t *port)
@@ -124,7 +126,7 @@ bool Network_RxLePatcket(uint32_t *buffer, uint32_t *bytes, NetworkPort_t *port)
         return false;
     }
 }
-#include <printf.h>
+
 bool Network_PassthroughRxPatcket(NetworkPort_t *port)
 {
     RegAPERxbufoffset_t rxbuf;
@@ -134,15 +136,21 @@ bool Network_PassthroughRxPatcket(NetworkPort_t *port)
 #if CXX_SIMULATOR
         rxbuf.print();
 #endif
+
+        VOLATILE RX_PORT_t *rx_port = port->rx_port;
         network_control_t control;
         int count = rxbuf.bits.Count;
         // int tailid = rxbuf.bits.Tail;
         int blockid = rxbuf.bits.Head;
 
+        RegAPERxPoolRetire_t retire;
+        retire.r32 = (1 << 24);
+        retire.bits.Count = 1;
+
         while (count--)
         {
             // printf("Block at %x\n", blockid);
-            RegRX_PORTIn_t *block = (RegRX_PORTIn_t *)&port->rx_port->In[RX_PORT_IN_ALL_BLOCK_WORDS * blockid];
+            RegRX_PORTIn_t *block = (RegRX_PORTIn_t *)&rx_port->In[RX_PORT_IN_ALL_BLOCK_WORDS * blockid];
             // printf("Control %x\n", (uint32_t)block[0].r32);
             control.r32 = block[0].r32;
             // printf(" Payload Len %d\n", control.bits.payload_length);
@@ -152,8 +160,10 @@ bool Network_PassthroughRxPatcket(NetworkPort_t *port)
 #if CXX_SIMULATOR
             printf("%d bytes in block.\n", control.bits.payload_length);
 #endif
-            int32_t words = DIVIDE_RND_UP(control.bits.payload_length, sizeof(uint32_t));
+            int i;
             int32_t offset;
+            int32_t words = DIVIDE_RND_UP(control.bits.payload_length, sizeof(uint32_t));
+
             if (control.bits.first)
             {
                 offset = RX_PORT_IN_ALL_FIRST_PAYLOAD_WORD;
@@ -163,35 +173,33 @@ bool Network_PassthroughRxPatcket(NetworkPort_t *port)
                 offset = RX_PORT_IN_ALL_ADDITIONAL_PAYLOAD_WORD;
             }
 
-            // Wait for enough free space.
-            while (APE_PERI.BmcToNcTxStatus.bits.InFifo < words)
-                ;
-
-            int i;
-            uint32_t data;
             if (!control.bits.not_last)
             {
                 // Ignore last word - drop the FCS
                 words--;
             }
 
-            for (i = 0; i < words - 1; i++)
-            {
-                data = block[i + offset].r32;
-                APE_PERI.BmcToNcTxBuffer.r32 = data;
-            }
+            // Wait for enough free space.
+            while (APE_PERI.BmcToNcTxStatus.bits.InFifo < words)
+                ;
 
-            data = block[i + offset].r32;
             if (control.bits.not_last)
             {
-                APE_PERI.BmcToNcTxBuffer.r32 = data;
+                for (i = 0; i < words; i++)
+                {
+                    APE_PERI.BmcToNcTxBuffer.r32 = block[i + offset].r32;
+                }
             }
             else
             {
-                // data = block[i + offset].r32;
+                for (i = 0; i < words - 1; i++)
+                {
+                    APE_PERI.BmcToNcTxBuffer.r32 = block[i + offset].r32;
+                }
+
                 // Last word to send.
                 APE_PERI.BmcToNcTxControl.r32 = control.bits.payload_length % sizeof(uint32_t);
-                APE_PERI.BmcToNcTxBufferLast.r32 = data;
+                APE_PERI.BmcToNcTxBufferLast.r32 = block[i + offset].r32;
 
                 // Ignore last word - drop the FCS.
                 // data = block[i + offset + 1].r32;
@@ -199,11 +207,8 @@ bool Network_PassthroughRxPatcket(NetworkPort_t *port)
             }
 
             // Retire this block.
-            RegAPERxPoolRetire_t retire;
-            retire.r32 = (1 << 24);
             retire.bits.Head = blockid;
             retire.bits.Tail = blockid;
-            retire.bits.Count = 1;
             *((RegAPERxPoolRetire_t *)port->rx_retire) = retire;
 
             blockid = control.bits.next_block;