diff --git a/fpga/pcie_driver/jfjoch_drv.h b/fpga/pcie_driver/jfjoch_drv.h
index c6a1d7b3..aaa0051d 100644
--- a/fpga/pcie_driver/jfjoch_drv.h
+++ b/fpga/pcie_driver/jfjoch_drv.h
@@ -74,6 +74,7 @@
 #define ADDR_CMS_3V3_PEX_INS_REG      0x028034 // in mV
 #define ADDR_CMS_HBM_TEMP1_INS_REG    0x028268 // in C
 #define ADDR_CMS_HBM_TEMP2_INS_REG    0x0282BC // in C
+#define ADDR_CMS_HOST_STATUS2_REG     0x02830C
 
 struct jfjoch_buf {
     dma_addr_t           dma_address;
diff --git a/fpga/pcie_driver/jfjoch_function.c b/fpga/pcie_driver/jfjoch_function.c
index dd76c7e8..ace23e81 100644
--- a/fpga/pcie_driver/jfjoch_function.c
+++ b/fpga/pcie_driver/jfjoch_function.c
@@ -228,8 +228,27 @@ void jfjoch_is_idle(struct jfjoch_drvdata *drvdata, uint32_t *output) {
 }
 
 void jfjoch_setup_cms(struct jfjoch_drvdata *drvdata) {
+    struct device *const dev = &drvdata->pdev->dev;
+    u32 reg_map_ready, i;
+
     iowrite32(0x1, drvdata->bar0 + CMS_OFFSET + ADDR_CMS_MB_RESETN_REG);
     iowrite32(1 << 27, drvdata->bar0 + CMS_OFFSET + ADDR_CMS_CONTROL_REG); // enable HBM monitoring
+
+    reg_map_ready = ioread32(drvdata->bar0 + CMS_OFFSET + ADDR_CMS_HOST_STATUS2_REG) & 0x1;
+    i = 0;
+    while (i < 10000) {
+        if (reg_map_ready)
+            break;
+        msleep(1);
+        reg_map_ready = ioread32(drvdata->bar0 + CMS_OFFSET + ADDR_CMS_HOST_STATUS2_REG) & 0x1;
+        i++;
+    }
+
+    if (i == 10000)
+        dev_err(dev, "CMS not ready after 10s");
+    else
+        dev_info(dev, "CMS ready after restart in %d ms", i);
+
 }
 
 void jfjoch_setup_network(struct jfjoch_drvdata *drvdata) {