diff --git a/src/opcua/ua_server.cpp b/src/opcua/ua_server.cpp
index 1b4dd8804a9d98642d9ef5ffc1ec26efdf25bb29..a5197607d4027762047410f962550a658b3b49d1 100644
--- a/src/opcua/ua_server.cpp
+++ b/src/opcua/ua_server.cpp
@@ -985,11 +985,11 @@ int ua_server_init(bool warm_start)
             ua_add_Variable(mUaServer, m, format, size, perm);
         }
     }
-    cout << "sizeof bool=" << UA_TYPES[UA_TYPES_BOOLEAN].memSize << endl;
-    cout << "sizeof int16=" << UA_TYPES[UA_TYPES_INT16].memSize << endl;
-    cout << "sizeof int32=" << UA_TYPES[UA_TYPES_INT32].memSize << endl;
-    cout << "sizeof float=" << UA_TYPES[UA_TYPES_FLOAT].memSize << endl;
-    cout << "sizeof double=" << UA_TYPES[UA_TYPES_DOUBLE].memSize << endl;
+    // cout << "sizeof bool=" << UA_TYPES[UA_TYPES_BOOLEAN].memSize << endl;
+    // cout << "sizeof int16=" << UA_TYPES[UA_TYPES_INT16].memSize << endl;
+    // cout << "sizeof int32=" << UA_TYPES[UA_TYPES_INT32].memSize << endl;
+    // cout << "sizeof float=" << UA_TYPES[UA_TYPES_FLOAT].memSize << endl;
+    // cout << "sizeof double=" << UA_TYPES[UA_TYPES_DOUBLE].memSize << endl;
     return 0;
 }
 
diff --git a/src/periph/fpga.cpp b/src/periph/fpga.cpp
index df34df3357bd648345302a2891e64e2238f66051..fe11864e5b7c53cfaaeccfd468a4d870f43b06e2 100644
--- a/src/periph/fpga.cpp
+++ b/src/periph/fpga.cpp
@@ -632,13 +632,7 @@ bool Periph_fpga::Write(const string addr_str, uint32_t *data_ptr, bool use_shif
 */
 uint32_t Periph_fpga::mask_shift(const uint32_t shift, const uint32_t mask, uint32_t data)
 {
-    uint32_t _data = data;
-
-    if (shift != 0 || mask != 0xffffffff) {
-        _data &= mask;
-        _data = _data >> shift;
-    }
-    return _data;
+    return (uint32_t)((data & mask) >> shift);
 }
 
 /*
@@ -664,13 +658,7 @@ uint32_t Periph_fpga::mask_shift(const string addr_str, uint32_t data)
 */
 uint32_t Periph_fpga::shift_mask(const uint32_t shift, const uint32_t mask, uint32_t data)
 {
-    uint32_t _data = data;
-
-    if (shift != 0 || mask != 0xffffffff) {
-        _data = _data << shift;
-        _data &= mask;
-    }
-    return _data;
+    return ((uint32_t)(data << shift) & mask);
 }
 
 /*
@@ -1654,7 +1642,7 @@ bool Periph_fpga::write_wg_enable(const char *data) {
             regname1 = "mm/0/REG_WG/" + to_string(i) + "/mode";
             regname2 = "mm/0/REG_WG/" + to_string(i) + "/nof_samples";
             reg[0] = shift_mask(regname1, C_WG_MODE_OFF) | shift_mask(regname2, 1024);  // TODO: make constant
-            retval = Write(regname1, reg, false);
+            retval &= Write(regname1, reg, false);
         }
     }
     // turn on waveform
@@ -1664,29 +1652,23 @@ bool Periph_fpga::write_wg_enable(const char *data) {
             regname1 = "mm/0/REG_WG/" + to_string(i) + "/mode";
             regname2 = "mm/0/REG_WG/" + to_string(i) + "/nof_samples";
             reg[0] = shift_mask(regname1, C_WG_MODE_CALC) | shift_mask(regname2, 1024);  // TODO: make constant
-            retval = Write(regname1, reg, false);
+            retval &= Write(regname1, reg, false);
         }
     }
     // get bsn and add latency
     wg_enable = _ptr[0];
     if (wg_enable == true) {
         regname1 = "mm/0/REG_BSN_SCHEDULER/0/scheduled_bsn";
-        retval = Read(regname1, reg);
+        retval &= Read(regname1, reg);
         scheduled_bsn = (((uint64_t)reg[1] << 32) + reg[0]);
         cout << "bsn=" << to_string(scheduled_bsn) << endl;
         scheduled_bsn += C_BSN_LATENCY;
         cout << "new bsn=" << to_string(scheduled_bsn) << endl;
         reg[0] = (uint32_t)(scheduled_bsn & 0xffffffff);
         reg[1] = (uint32_t)((scheduled_bsn >> 32) & 0xffffffff);
+        retval &= Write(regname1, reg);
     }
-    // write sheduled bsn
-    for (uint i=0; i< C_S_pn; i++) {
-        wg_enable = _ptr[i];
-        if (wg_enable == true) {
-            regname1 = "mm/0/REG_BSN_SCHEDULER/" + to_string(i) + "/scheduled_bsn";
-            retval = Write(regname1, reg);
-        }
-    }
+
     delete[] reg;
     return retval;
 }
@@ -2157,7 +2139,7 @@ bool Periph_fpga::read_all_from_port(TermOutput& termout, const string& port_nam
     for (uint i=0; i<n_periph; i++) {
         for (uint j=0; j<n_ports; j++) {
             regname = "mm/" + to_string(i) + "/" + port_name + "/" + to_string(j) + "/" + field_name;
-            cout << "regname=" << regname << endl;
+            // cout << "regname=" << regname << endl;
 
             memset((void *)data, 0, (span * sizeof(uint32_t)));
             retval &= Read(regname, data);