diff --git a/src/periph/fpga.cpp b/src/periph/fpga.cpp
index 5059405ba40e5ae1131b59b33c72c481baa8e066..df34df3357bd648345302a2891e64e2238f66051 100644
--- a/src/periph/fpga.cpp
+++ b/src/periph/fpga.cpp
@@ -585,8 +585,10 @@ bool Periph_fpga::Read(const string addr_str, uint32_t *data_ptr, bool use_mask_
 
     ret = ucp->readRegister(addr, nvalues, data_ptr, isfifo);
     if (use_mask_shift) {
+        uint32_t shift = mmap->getShift((addr_str));
+        uint32_t mask = mmap->getMask((addr_str));
         for (uint32_t i=0; i<nvalues; i++) {
-            data_ptr[i] = mask_shift(addr_str, data_ptr[i]);
+            data_ptr[i] = mask_shift(shift, mask, data_ptr[i]);
         }
     }
     return ret;
@@ -615,13 +617,30 @@ bool Periph_fpga::Write(const string addr_str, uint32_t *data_ptr, bool use_shif
 
     bool isfifo = mmap->type_isfifo((addr_str));
     if (use_shift_mask) {
+        uint32_t shift = mmap->getShift((addr_str));
+        uint32_t mask = mmap->getMask((addr_str));
         for (uint32_t i=0; i<span; i++) {
-            data_ptr[i] = shift_mask(addr_str, data_ptr[i]);
+            data_ptr[i] = shift_mask(shift, mask, data_ptr[i]);
         }
     }
     return ucp->writeRegister(addr, span, data_ptr, isfifo);
 }
 
+/*
+* mask_shift (for reading)
+* mask data and shift bits using information from the mmap.
+*/
+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;
+}
+
 /*
 * mask_shift (for reading)
 * mask data and shift bits using information from the mmap.
@@ -639,6 +658,21 @@ uint32_t Periph_fpga::mask_shift(const string addr_str, uint32_t data)
     return _data;
 }
 
+/*
+* shift_mask (for writing)
+* shift data and mask bits using information from the mmap.
+*/
+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;
+}
+
 /*
 * shift_mask (for writing)
 * shift data and mask bits using information from the mmap.
diff --git a/src/periph/fpga.h b/src/periph/fpga.h
index 41b7a1651ecc06fbc276c962559de408863d2bd3..3c1c09816b7bfea2c050d0f3b97d0f688d72101f 100644
--- a/src/periph/fpga.h
+++ b/src/periph/fpga.h
@@ -84,7 +84,9 @@ private:
 
   bool Read(const std::string addr_str, uint32_t *data_ptr, bool use_mask_shift);
   bool Write(const std::string addr_str, uint32_t *data_ptr, bool use_shift_mask);
+  uint32_t mask_shift(const uint32_t shift, const uint32_t mask, uint32_t data);
   uint32_t mask_shift(const std::string addr_str, uint32_t data);
+  uint32_t shift_mask(const uint32_t shift, const uint32_t mask, uint32_t data);
   uint32_t shift_mask(const std::string addr_str, uint32_t data);
 
   bool flash_erase_sector(uint32_t sector);