diff --git a/bsp/SCALP-ROUTER-FIRMWARE-BSP-V0.2.bsp b/bsp/SCALP-ROUTER-FIRMWARE-BSP-V0.2.bsp
new file mode 100644
index 0000000000000000000000000000000000000000..30cb8e7f31b8d1195585d910757d13c3c035f566
Binary files /dev/null and b/bsp/SCALP-ROUTER-FIRMWARE-BSP-V0.2.bsp differ
diff --git a/scalp_router_firmware_petalinux/.petalinux/metadata b/scalp_router_firmware_petalinux/.petalinux/metadata
index 44c72fc697dfdefaaac7a9b3fb17f54610f8e244..254940ca766aebae0801e973c4deb87faa6d04ce 100644
--- a/scalp_router_firmware_petalinux/.petalinux/metadata
+++ b/scalp_router_firmware_petalinux/.petalinux/metadata
@@ -1,6 +1,6 @@
 PETALINUX_VER=2020.2
 VALIDATE_HW_CHKSUM=1
-HARDWARE_CHECKSUM=de7ad126b920e49a3bd0b3db07874706
+HARDWARE_CHECKSUM=e8965a168a89fe38c33ea91277f9cd5e
 YOCTO_SDK=5ff8fc5f85d1566b314bb73eaa378212
-RFSCONFIG_CHKSUM=e658a66255d397f49ad8494748910c66
+RFSCONFIG_CHKSUM=eddf2236984c1b8d0e87cb4a8e81ebef
 HARDWARE_PATH=/home/jo/Documents/Projets/Hepia/scalp_project/scalp_firmware/designs/vivado/scalp_router_firmware/2020.2/lin64/scalp_router_firmware/scalp_router_firmware.xsa
diff --git a/scalp_router_firmware_petalinux/SCALP-ROUTER-FIRMWARE-BSP-V0.2.bsp b/scalp_router_firmware_petalinux/SCALP-ROUTER-FIRMWARE-BSP-V0.2.bsp
new file mode 100644
index 0000000000000000000000000000000000000000..30cb8e7f31b8d1195585d910757d13c3c035f566
Binary files /dev/null and b/scalp_router_firmware_petalinux/SCALP-ROUTER-FIRMWARE-BSP-V0.2.bsp differ
diff --git a/scalp_router_firmware_petalinux/project-spec/configs/config b/scalp_router_firmware_petalinux/project-spec/configs/config
index aaea5216d4c97c5ffe10e2659db3a87992c33cc8..9312953f848e6e486957652b62f50f5b97593d2e 100644
--- a/scalp_router_firmware_petalinux/project-spec/configs/config
+++ b/scalp_router_firmware_petalinux/project-spec/configs/config
@@ -161,7 +161,7 @@ CONFIG_SUBSYSTEM_EXTRA_DT_FILES=""
 #
 CONFIG_SUBSYSTEM_BOOTARGS_AUTO=y
 CONFIG_SUBSYSTEM_BOOTARGS_EARLYPRINTK=y
-
+CONFIG_SUBSYSTEM_BOOTARGS_GENERATED="console=ttyPS0,115200 earlycon"
 CONFIG_SUBSYSTEM_DEVICETREE_COMPILER_FLAGS="-@"
 # CONFIG_SUBSYSTEM_DTB_OVERLAY is not set
 # CONFIG_SUBSYSTEM_REMOVE_PL_DTB is not set
@@ -253,4 +253,3 @@ CONFIG_YOCTO_NETWORK_SSTATE_FEEDS_URL="http://petalinux.xilinx.com/sswreleases/r
 # User Layers
 #
 CONFIG_USER_LAYER_0=""
-CONFIG_SUBSYSTEM_BOOTARGS_GENERATED="console=ttyPS0,115200 earlycon"
diff --git a/scalp_router_firmware_petalinux/project-spec/configs/rootfs_config b/scalp_router_firmware_petalinux/project-spec/configs/rootfs_config
index 40672bce213235f390c555aa6f02a777d6aaf5b2..55361d918f02e8368e31bd3d378c0421879fc0cc 100644
--- a/scalp_router_firmware_petalinux/project-spec/configs/rootfs_config
+++ b/scalp_router_firmware_petalinux/project-spec/configs/rootfs_config
@@ -4025,7 +4025,7 @@ CONFIG_imagefeature-debug-tweaks=y
 CONFIG_bitstream-conf=y
 CONFIG_bitstream-init=y
 # CONFIG_gpio-demo is not set
-# CONFIG_peekpoke is not set
+CONFIG_peekpoke=y
 # CONFIG_sampleapp is not set
 CONFIG_sampleread=y
 CONFIG_samplewrite=y
diff --git a/scalp_router_firmware_petalinux/project-spec/hw-description/scalp_router_firmware.bit b/scalp_router_firmware_petalinux/project-spec/hw-description/scalp_router_firmware.bit
index bd8332a1f46259a07967b24acc3ac5364e20ba07..95e96c38ab0959923edb8d8424825fe64978cdba 100644
Binary files a/scalp_router_firmware_petalinux/project-spec/hw-description/scalp_router_firmware.bit and b/scalp_router_firmware_petalinux/project-spec/hw-description/scalp_router_firmware.bit differ
diff --git a/scalp_router_firmware_petalinux/project-spec/hw-description/system.xsa b/scalp_router_firmware_petalinux/project-spec/hw-description/system.xsa
index b629939e32c6e810cf5555b1f58d71d8de1d19c1..3f9c0effa2b24d540a9b6f78b58dd9138fdf59a7 100644
Binary files a/scalp_router_firmware_petalinux/project-spec/hw-description/system.xsa and b/scalp_router_firmware_petalinux/project-spec/hw-description/system.xsa differ
diff --git a/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/libscalp/files/scalp.cc b/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/libscalp/files/scalp.cc
index c84de67fbb76a4d081e80afee25fd82d35cfa023..8ea1b887bf1e550f7a91bd5dcfb13c18449948cf 100644
--- a/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/libscalp/files/scalp.cc
+++ b/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/libscalp/files/scalp.cc
@@ -153,7 +153,7 @@ uint32_t libscalp::scalp_noc_get_locaddr(void *regs)
 bool libscalp::scalp_noc_data_is_valid(void *regs, bool new_packet)
 {
     libscalp::scalp_noc_regs *_regs = NULL;
-    
+
     if(regs == NULL)
         return false;
 
@@ -173,18 +173,32 @@ bool libscalp::scalp_noc_data_is_valid(void *regs, bool new_packet)
         return false;
 }
 
-uint32_t libscalp::scalp_noc_read_data(void *regs, bool *is_last)
+uint32_t libscalp::scalp_noc_read_data(void *regs, bool *is_last, bool new_packet, bool *nodata)
 {
     libscalp::scalp_noc_regs *_regs = NULL;
     uint32_t data = 0;
-    
-    if(regs == NULL)
+
+    if((regs == NULL) || (is_last == NULL) || (libscalp::scalp_noc_data_is_valid(regs, new_packet) == false))
+    {
+        *nodata = true;
         return 0;
+    }
 
     _regs = reinterpret_cast<libscalp::scalp_noc_regs *>(regs);
+    
+    *nodata = false;    
 
     data = (uint32_t)(_regs->rd_data);
 
+    if(_regs->status & libscalp::E_RD_LAST)
+    {
+        *is_last = true;
+    }
+    else
+    {
+        *is_last = false;
+    }
+    
     while(!(_regs->status & libscalp::E_RD_WAIT_NEXT)){}
     
     if(_regs->status & libscalp::E_RD_WAIT_NEXT)
@@ -193,40 +207,42 @@ uint32_t libscalp::scalp_noc_read_data(void *regs, bool *is_last)
         _regs->clr_ctrl = libscalp::E_RD_NEXT;
     }
 
-    if(_regs->status & libscalp::E_RD_LAST)
-    {
-        *is_last = true;
+    if(*is_last)
         _regs->ctrl = 0x00000000;
-    }
-    else
-    {
-        *is_last = false;
-    }
 
     return data;
 }
 
-uint32_t libscalp::scalp_noc_read_packet(void *regs, uint32_t *scalp_packet)
+uint32_t libscalp::scalp_noc_read_packet(void *regs, uint32_t *scalp_packet, bool blocking)
 {
-    // void *regs = NULL;
+    libscalp::scalp_noc_regs *_regs = NULL;
     uint32_t spword = 0;
+    bool last_wait = false;
     bool is_last = false;
     uint32_t size = 0;
+    bool nodata = true;
 
-    // if((regs = libscalp::scalp_noc_init("/dev/uio0")) == NULL)
-    //     return EXIT_FAILURE;
+    if((regs == NULL) || (scalp_packet == NULL))
+        return 0;
 
+    _regs = reinterpret_cast<libscalp::scalp_noc_regs *>(regs);
+    
     do
     {
-        if(libscalp::scalp_noc_data_is_valid(regs, (size == 0) ? true : false) == true)
+        spword = (uint32_t)libscalp::scalp_noc_read_data(regs, &is_last, (size == 0) ? true : false, &nodata);
+
+        if(!nodata)
         {
-            spword = (uint32_t)libscalp::scalp_noc_read_data(regs, &is_last);
             scalp_packet[size] = spword;
-            size++;            
+            size++;
         }
-        else
+
+        if(nodata && !blocking)
             break;
-    }while(true);
+        
+    }while(!is_last);
+
+    _regs->ctrl = 0x00000000;
 
     return size;
 }
@@ -252,15 +268,15 @@ bool libscalp::scalp_noc_ready_for_data(void *regs, bool new_packet)
         return false;
 }
 
-bool libscalp::scalp_noc_write_data(void *regs, uint32_t data, bool is_last)
+bool libscalp::scalp_noc_write_data(void *regs, uint32_t data, bool is_last, bool new_packet)
 {
     libscalp::scalp_noc_regs *_regs = NULL;
     
-    if(regs == NULL)
+    if((regs == NULL) || (libscalp::scalp_noc_ready_for_data(regs, new_packet) == false))
         return false;
 
     _regs = reinterpret_cast<libscalp::scalp_noc_regs *>(regs);
-
+    
     _regs->wr_data = data;
 
     // Unset next header word and valid current header word.
@@ -278,37 +294,69 @@ bool libscalp::scalp_noc_write_data(void *regs, uint32_t data, bool is_last)
     return true;
 }
 
-uint32_t libscalp::scalp_noc_write_packet(void *regs, uint32_t *scalp_packet, uint32_t size)
+bool libscalp::scalp_noc_write_packet(void *regs, uint32_t *scalp_packet, uint32_t size)
 {
-    // void *regs = NULL;
+    libscalp::scalp_noc_regs *_regs = NULL;
     uint32_t i = 0;
 
-    // if((regs = libscalp::scalp_noc_init("/dev/uio0")) == NULL)
-    //     return EXIT_FAILURE;
+    if((regs == NULL) || (scalp_packet == NULL))
+        return false;
 
+     _regs = reinterpret_cast<libscalp::scalp_noc_regs *>(regs);
+    
     for(i = 0; i < size;)
     {
         if(i < (size - 1))
         {
-            if(libscalp::scalp_noc_ready_for_data(regs, (i == 0) ? true : false) == true)
+            if(libscalp::scalp_noc_write_data(regs, scalp_packet[i], false, (i == 0) ? true : false) == true)
             {
-                if(libscalp::scalp_noc_write_data(regs, scalp_packet[i], false) == true)
-                {
-                    i++;
-                }
+                i++;
             }
         }
         else
         {
-            if(libscalp::scalp_noc_ready_for_data(regs, false) == true)
+            if(libscalp::scalp_noc_write_data(regs, scalp_packet[i], true, false) == true)
             {
-                if(libscalp::scalp_noc_write_data(regs, scalp_packet[i], true) == true)
-                {
-                    i++;
-                }
+                i++;
             }
         }
     }
 
-    return i;
+     _regs->ctrl = 0x00000000;
+    
+    return true;
+}
+
+uint32_t libscalp::scalp_noc_create_packet(void *regs, uint32_t x, uint32_t y, uint32_t z, uint32_t *payload, uint32_t payload_size, uint32_t *packet)
+{
+    uint32_t default_type = 1;
+    uint32_t h0 = 0;
+    uint32_t h1 = 0;
+    uint32_t h2 = 0;
+    uint32_t loc_x = 0;
+    uint32_t loc_y = 0;
+    uint32_t loc_z = 0;
+    uint32_t packet_size = 0;
+    
+    if((regs == NULL) || (payload_size > (SCALP_MAX_SP_SIZE - SCALP_HDR_SIZE)) || (payload == NULL) || (packet == NULL))
+        return 0;
+
+    loc_x = (libscalp::scalp_noc_get_locaddr(regs)) & 0x000000ff;
+    loc_y = ((libscalp::scalp_noc_get_locaddr(regs)) & 0x0000ff00) >> 8;
+    loc_z = ((libscalp::scalp_noc_get_locaddr(regs)) & 0x00ff0000) >> 16;
+    
+    h0 = (uint32_t)(x << 24) | (uint32_t)(y << 16) | (uint32_t)(z << 8) | (uint32_t)0x00000001;
+    h1 = (uint32_t)(loc_x << 24) | (uint32_t)(loc_y << 16) | (uint32_t)(loc_z << 8) | (uint32_t)0x00000000;
+    h2 = (uint32_t)(payload_size << 16) | (uint32_t)0x00000000;
+
+    packet[0] = h0;
+    packet[1] = h1;
+    packet[2] = h2;
+
+    for(int i = 0; i < payload_size; i++)
+        packet[i + SCALP_HDR_SIZE] = payload[i];
+
+    packet_size = payload_size + SCALP_HDR_SIZE;
+
+    return packet_size;
 }
diff --git a/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/libscalp/files/scalp.hh b/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/libscalp/files/scalp.hh
index 2cc5ce14799637808256e61465f89caf3340f8c2..0a7c74ea0786dc92c22ad7468b4bcfc8ce13d282 100644
--- a/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/libscalp/files/scalp.hh
+++ b/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/libscalp/files/scalp.hh
@@ -7,6 +7,7 @@
 
 #define SCALP_ROUTER_UIO_DEV   "/dev/uio0"
 #define SCALP_ROUTER_MMAP_SIZE 0x1000
+#define SCALP_HDR_SIZE         3
 #define SCALP_MAX_SP_SIZE      64
 #define SCALP_TX_FIFO_SIZE     1024
 
@@ -89,23 +90,70 @@ namespace libscalp
     uint32_t scalp_noc_get_locaddr   (void *regs);
     void     scalp_noc_free          (void *devuio, void *mmap_regs);
     bool     scalp_noc_data_is_valid (void *regs, bool new_packet);
-    uint32_t scalp_noc_read_data     (void *regs, bool *is_last);
-    uint32_t scalp_noc_read_packet   (void *regs, uint32_t *scalp_packet);
+    uint32_t scalp_noc_read_data     (void *regs, bool *is_last, bool new_packet, bool *nodata);
+    uint32_t scalp_noc_read_packet   (void *regs, uint32_t *scalp_packet, bool blocking);
     bool     scalp_noc_ready_for_data(void *regs, bool new_packet);
-    bool     scalp_noc_write_data    (void *regs, uint32_t data,  bool is_last);
-    uint32_t scalp_noc_write_packet  (void *regs, uint32_t *scalp_packet, uint32_t size);
+    bool     scalp_noc_write_data    (void *regs, uint32_t data,  bool is_last, bool new_packet);
+    bool     scalp_noc_write_packet  (void *regs, uint32_t *scalp_packet, uint32_t size);
+    uint32_t scalp_noc_create_packet (void *regs, uint32_t x, uint32_t y, uint32_t z, uint32_t *payload, uint32_t payload_size, uint32_t *packet);
 };
 
 extern "C"
 {
-    libscalp::uio *      uio_new(const char *dev){ return new libscalp::uio(dev); }
-    void                 uio_del(libscalp::uio *obj_uio){ obj_uio->~uio(); }
-    void                 uio_unmask_interrupt(libscalp::uio *obj_uio){ obj_uio->unmask_interrupt(); }
-    void                 uio_wait_interrupt(libscalp::uio *obj_uio, uint32_t timeout_ms){ obj_uio->wait_interrupt(timeout_ms); }
-    libscalp::uio_mmap * uio_mmap_new(const libscalp::uio &u, uint32_t index, size_t size) { return new libscalp::uio_mmap(u, index, size); }
-    void                 uio_mmap_del(libscalp::uio_mmap *obj_uio_mmap){ obj_uio_mmap->~uio_mmap(); }
-    size_t               uio_mmap_size(libscalp::uio_mmap *obj_uio_mmap) { return obj_uio_mmap->size(); }
-    void *               uio_mmap_get_ptr(libscalp::uio_mmap *obj_uio_mmap) { return obj_uio_mmap->get_ptr(); }                 
+    void * sn_init(const char *dev, void **devuio, void **mmap_regs)
+    {
+        return libscalp::scalp_noc_init(dev, devuio, mmap_regs);
+    }
+
+    uint32_t sn_set_locaddr(void *regs, uint32_t x, uint32_t y, uint32_t z)
+    {
+        return (uint32_t)(libscalp::scalp_noc_set_locaddr(regs, x, y, z));
+    }
+
+    uint32_t sn_get_locaddr(void *regs)
+    {
+        return libscalp::scalp_noc_get_locaddr(regs);
+    }
+
+    void sn_free(void *devuio, void *mmap_regs)
+    {
+        return libscalp::scalp_noc_free(devuio, mmap_regs);
+    }
+
+    uint32_t sn_data_is_valid(void *regs, bool new_packet)
+    {
+        return (uint32_t)(libscalp::scalp_noc_data_is_valid(regs, new_packet));
+    }
+
+    uint32_t sn_read_data(void *regs, bool *is_last, bool new_packet, bool *nodata)
+    {
+        return libscalp::scalp_noc_read_data(regs, is_last, new_packet, nodata);
+    }
+
+    uint32_t sn_read_packet(void *regs, uint32_t *scalp_packet, bool blocking)
+    {
+        return libscalp::scalp_noc_read_packet(regs, scalp_packet, blocking);
+    }
+
+    uint32_t sn_ready_for_data(void *regs, bool new_packet)
+    {
+        return (uint32_t)(libscalp::scalp_noc_ready_for_data(regs, new_packet));
+    }
+
+    uint32_t sn_write_data(void *regs, uint32_t data,  bool is_last, bool new_packet)
+    {
+        return (uint32_t)(libscalp::scalp_noc_write_data(regs, data, is_last, new_packet));
+    }
+
+    uint32_t sn_write_packet(void *regs, uint32_t *scalp_packet, uint32_t size)
+    {
+        return (uint32_t)(libscalp::scalp_noc_write_packet(regs, scalp_packet, size));
+    }
+
+    uint32_t sn_create_packet(void *regs, uint32_t x, uint32_t y, uint32_t z, uint32_t *payload, uint32_t payload_size, uint32_t *packet)
+    {
+        return (uint32_t)(libscalp::scalp_noc_create_packet(regs, x, y , z, payload, payload_size, packet));
+    }
 }
 
 #endif
diff --git a/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/sampleread/files/sampleread.cc b/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/sampleread/files/sampleread.cc
index 33f51a519e650e61ec74ff9bca49778880ac32cb..c2131214d59c3b7e4ae38aad774e89e7de032fc3 100644
--- a/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/sampleread/files/sampleread.cc
+++ b/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/sampleread/files/sampleread.cc
@@ -34,19 +34,15 @@ int main(int argc, char **argv)
     void *devuio = NULL;
     void *mmap_regs = NULL;
     
-    uint32_t scalp_packet[SCALP_MAX_SP_SIZE];
+    uint32_t scalp_packet[SCALP_TX_FIFO_SIZE];
     uint32_t size = 0;
 
-    // uint32_t spword = 0;
-    // bool is_last = false;
-    // uint32_t i = 0;
-
     if((regs = libscalp::scalp_noc_init("/dev/uio0", &devuio, &mmap_regs)) == NULL)
         return EXIT_FAILURE;
 
     std::cout << "[INFO] Scalp packet is receiving..." << std::endl;    
 
-    size = libscalp::scalp_noc_read_packet(regs, scalp_packet);
+    size = libscalp::scalp_noc_read_packet(regs, scalp_packet, false);
 
     std::cout << "[INFO] Scalp packet size = " << size << std::endl;
 
diff --git a/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/samplewrite/files/samplewrite.cc b/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/samplewrite/files/samplewrite.cc
index 5f18de6ffb404842b94dbd445ea6f169ba42b49c..db0db182b1036ea236270d7a9e1a5363f7bc3b92 100644
--- a/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/samplewrite/files/samplewrite.cc
+++ b/scalp_router_firmware_petalinux/project-spec/meta-user/recipes-apps/samplewrite/files/samplewrite.cc
@@ -42,13 +42,10 @@ int main(int argc, char **argv)
     uint32_t h1 = 0;
     uint32_t h2 = 0;
     uint32_t pld = 0;
-    uint32_t len = 0;
-    uint32_t scalp_packet[SCALP_MAX_SP_SIZE];
-    uint32_t size = 0;    
-    
-    // libscalp::uio devuio("/dev/uio0");
-    // libscalp::uio_mmap mmap_regs(devuio, 0, 4096);
-    // libscalp::scalp_noc_regs *_regs = NULL;
+    uint32_t scalp_payload[SCALP_TX_FIFO_SIZE];
+    uint32_t scalp_packet[SCALP_TX_FIFO_SIZE];
+    uint32_t payload_size = 0;
+    uint32_t packet_size = 0;
     
     void *regs = NULL;
     void *devuio = NULL;
@@ -57,45 +54,55 @@ int main(int argc, char **argv)
     if((regs = libscalp::scalp_noc_init("/dev/uio0", &devuio, &mmap_regs)) == NULL)
         return EXIT_FAILURE;
 
-    // _regs = reinterpret_cast<libscalp::scalp_noc_regs *>(regs);
-
     std::cout << "[INFO] Set dest addr" << std::endl;
     std::cout << "[INFO] Set X : ";
     cin >> x_addr;
-    //x_addr = (uint8_t)atoi(c_x_addr);
+
     std::cout << "[INFO] Set Y : ";
     cin >> y_addr;
-    //y_addr = (uint8_t)atoi(c_y_addr);
+
     std::cout << "[INFO] Set Z : ";
     cin >> z_addr;
-    //z_addr = (uint8_t)atoi(c_z_addr);
+
     std::cout << "[INFO] X : " << x_addr << " Y : " << y_addr << " Z : " << z_addr << std::endl;
 
-    std::cout << "[INFO] Set length" << std::endl;
-    std::cout << "[INFO] Len : ";
-    cin >> len;
+    std::cout << "[INFO] Set payload size" << std::endl;
+    std::cout << "[INFO] Size : ";
+    cin >> payload_size;
+
+    if(payload_size > (SCALP_MAX_SP_SIZE - SCALP_HDR_SIZE))
+    {
+        std::cout << "[INFO] Maximum size is " << (SCALP_MAX_SP_SIZE - SCALP_HDR_SIZE) << std::endl;
+        return EXIT_FAILURE;
+    }       
+
+    for(int i = 0; i < payload_size; i++)
+        scalp_payload[i] = 0;
     
-    std::cout << "[INFO] Scalp packet is sending..." << std::endl;
+    if((packet_size = libscalp::scalp_noc_create_packet(regs, x_addr, y_addr, z_addr, scalp_payload, payload_size, scalp_packet)) == 0)
+        return 0;
 
-    h0 = (uint32_t)(x_addr << 24) | (uint32_t)(y_addr << 16) | (uint32_t)(z_addr << 8) | (uint32_t)0x00000001;
-    h1 = 0x01000000;
-    h2 = 0x00080000;
+    std::cout << "[INFO] Scalp packet is sending..." << std::endl;
+    
+    // h0 = (uint32_t)(x_addr << 24) | (uint32_t)(y_addr << 16) | (uint32_t)(z_addr << 8) | (uint32_t)0x00000001;
+    // h1 = 0x01000000;
+    // h2 = 0x00080000;
 
-    scalp_packet[0] = h0;
-    scalp_packet[1] = h1;
-    scalp_packet[2] = h2;
+    // scalp_packet[0] = h0;
+    // scalp_packet[1] = h1;
+    // scalp_packet[2] = h2;
 
-    std::cout << "[INFO] H0 : 0x" << std::hex << h0 << std::endl;
-    std::cout << "[INFO] H1 : 0x" << std::hex << h1 << std::endl;
-    std::cout << "[INFO] H2 : 0x" << std::hex << h2 << std::endl;
+    // std::cout << "[INFO] H0 : 0x" << std::hex << h0 << std::endl;
+    // std::cout << "[INFO] H1 : 0x" << std::hex << h1 << std::endl;
+    // std::cout << "[INFO] H2 : 0x" << std::hex << h2 << std::endl;
     
-    for(int i = 0; i < len; i++)
-    {
-        scalp_packet[i + 3] = i;
-        std::cout << "[INFO] PLD : 0x" << std::hex << i << std::endl;
-    }
+    // for(int i = 0; i < len; i++)
+    // {
+    //     scalp_packet[i + 3] = i;
+    //     std::cout << "[INFO] PLD : 0x" << std::hex << i << std::endl;
+    // }
 
-    if((size = libscalp::scalp_noc_write_packet(regs, scalp_packet, (len + 3))) == (len + 3))
+    if(libscalp::scalp_noc_write_packet(regs, scalp_packet, packet_size) == true)
     {
         std::cout << "[INFO] Scalp packet has been sent!" << std::endl;
     }