1 files changed, 29 insertions, 10 deletions

diff --git a/firmware/microblaze/lib/pkt_ctrl.c b/firmware/microblaze/lib/pkt_ctrl.c
index f7f808cfc..a1e9ec1fe 100644
--- a/firmware/microblaze/lib/pkt_ctrl.c
+++ b/firmware/microblaze/lib/pkt_ctrl.c
@@ -30,36 +30,55 @@ static void set_control(uint32_t value, uint32_t mask){
     buffer_pool_ctrl->ctrl = ctrl_shadow;
 }
 
+static void set_control_bit(int bit){
+    set_control(1 << bit, 1 << bit);
+}
+
+static void clr_control_bit(int bit){
+    set_control(0 << bit, 1 << bit);
+}
+
+static bool is_status_bit_set(int bit){
+    return buffer_pool_status->status & (1 << bit);
+}
+
+#define INP_HS_BIT 0 //CPU out in packet_router.v
+#define OUT_HS_BIT 1 //CPU inp in packet_router.v
+#define MODE_BIT 2
+#define CLR_BIT 8
+
 void pkt_ctrl_set_routing_mode(pkt_ctrl_routing_mode_t mode){
     switch(mode){
     case PKT_CTRL_ROUTING_MODE_SLAVE:
-        set_control(0x0, 0x4);
+        clr_control_bit(MODE_BIT);
         break;
     case PKT_CTRL_ROUTING_MODE_MASTER:
-        set_control(0x4, 0x4);
+        set_control_bit(MODE_BIT);
         break;
     }
+    set_control_bit(CLR_BIT); //clear after mode change
+    clr_control_bit(CLR_BIT); //unset the clear signal
 }
 
 void *pkt_ctrl_claim_incoming_buffer(size_t *num_lines){
-    if ((buffer_pool_status->status & 0x1) != 1) return NULL;
+    if (!is_status_bit_set(INP_HS_BIT)) return NULL;
     *num_lines = (buffer_pool_status->status >> 16) & 0xffff;
-    set_control(0x1, 0x1);
     return buffer_ram(0);
 }
 
 void pkt_ctrl_release_incoming_buffer(void){
-    set_control(0x0, 0x1);
-    while ((buffer_pool_status->status & 0x1) != 0){}
+    set_control_bit(INP_HS_BIT);
+    while (is_status_bit_set(INP_HS_BIT)){}
+    clr_control_bit(INP_HS_BIT);
 }
 
 void *pkt_ctrl_claim_outgoing_buffer(void){
-    while ((buffer_pool_status->status & 0x2) != 0x2){}
+    while (!is_status_bit_set(OUT_HS_BIT)){}
     return buffer_ram(1);
 }
 
 void pkt_ctrl_commit_outgoing_buffer(size_t num_lines){
-    set_control(0x2 | (num_lines << 16), 0x2 | (0xffff << 16));
-    while ((buffer_pool_status->status & 0x2) != 0x0){}
-    set_control(0x0, 0x2);
+    set_control((1 << OUT_HS_BIT) | (num_lines << 16), (1 << OUT_HS_BIT) | (0xffff << 16));
+    while (is_status_bit_set(OUT_HS_BIT)){}
+    clr_control_bit(OUT_HS_BIT);
 }