diff --git a/lib/include/srslte/upper/gtpu.h b/lib/include/srslte/upper/gtpu.h
index 3b4bd1d34..8d1005f26 100644
--- a/lib/include/srslte/upper/gtpu.h
+++ b/lib/include/srslte/upper/gtpu.h
@@ -79,6 +79,7 @@ typedef struct{
 
 bool gtpu_read_header(srslte::byte_buffer_t *pdu, gtpu_header_t *header, srslte::log *gtpu_log);
 bool gtpu_write_header(gtpu_header_t *header, srslte::byte_buffer_t *pdu, srslte::log *gtpu_log);
+std::string gtpu_ntoa(uint32_t addr);
 
 inline bool gtpu_supported_flags_check(gtpu_header_t *header, srslte::log *gtpu_log)
 {
diff --git a/lib/include/srslte/upper/pdcp_entity.h b/lib/include/srslte/upper/pdcp_entity.h
index 0a658c9b2..590c2cb7b 100644
--- a/lib/include/srslte/upper/pdcp_entity.h
+++ b/lib/include/srslte/upper/pdcp_entity.h
@@ -110,6 +110,7 @@ private:
   uint32_t rx_hfn;
   uint32_t next_pdcp_rx_sn;
   uint32_t reordering_window;
+  uint32_t last_submitted_pdcp_rx_sn;
 
   CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
   INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
diff --git a/lib/src/upper/gtpu.cc b/lib/src/upper/gtpu.cc
index 17eda506a..99c2ac33d 100644
--- a/lib/src/upper/gtpu.cc
+++ b/lib/src/upper/gtpu.cc
@@ -28,7 +28,6 @@ namespace srslte {
  * Header pack/unpack helper functions
  * Ref: 3GPP TS 29.281 v10.1.0 Section 5
  ***************************************************************************/
-
 bool gtpu_write_header(gtpu_header_t *header, srslte::byte_buffer_t *pdu, srslte::log *gtpu_log)
 {
   //flags
@@ -141,4 +140,18 @@ bool gtpu_read_header(srslte::byte_buffer_t *pdu, gtpu_header_t *header, srslte:
   return true;
 }
 
+// Helper function to return a string from IPv4 address for easy printing
+std::string gtpu_ntoa(uint32_t addr)
+{
+  char tmp_str[INET_ADDRSTRLEN + 1];
+  bzero(tmp_str, sizeof(tmp_str));
+  struct in_addr tmp_addr;
+  tmp_addr.s_addr     = addr;
+  const char* tmp_ptr = inet_ntop(AF_INET, &tmp_addr, tmp_str, INET_ADDRSTRLEN);
+  if (tmp_ptr == NULL) {
+    return std::string("Invalid IPv4 address");
+  }
+  return std::string(tmp_str);
+}
+
 } // namespace srslte
diff --git a/lib/src/upper/pdcp_entity.cc b/lib/src/upper/pdcp_entity.cc
index 30ce75c2a..8eedecc6a 100644
--- a/lib/src/upper/pdcp_entity.cc
+++ b/lib/src/upper/pdcp_entity.cc
@@ -81,7 +81,11 @@ void pdcp_entity::init(srsue::rlc_interface_pdcp      *rlc_,
     reordering_window = 2048;
   }
 
+  rx_hfn          = 0;
+  next_pdcp_rx_sn = 0;
   maximum_pdcp_sn = (1 << cfg.sn_len) - 1;
+  printf("Maximum PDCP SN %d\n", maximum_pdcp_sn);
+  last_submitted_pdcp_rx_sn = maximum_pdcp_sn;
   log->info("Init %s with bearer ID: %d\n", rrc->get_rb_name(lcid).c_str(), cfg.bearer_id);
 }
 
@@ -281,9 +285,54 @@ void pdcp_entity::handle_um_drb_pdu(srslte::byte_buffer_t* pdu)
 // DRBs mapped on RLC AM, without re-ordering (5.1.2.1.2)
 void pdcp_entity::handle_am_drb_pdu(srslte::byte_buffer_t* pdu)
 {
-  handle_um_drb_pdu(pdu); //FIXME!!!
+  uint32_t sn, count;
+  pdcp_unpack_data_pdu_long_sn(pdu, &sn);
+
+  int32_t last_submit_diff_sn     = last_submitted_pdcp_rx_sn - sn;
+  int32_t sn_diff_last_submit     = sn - last_submitted_pdcp_rx_sn;
+  int32_t sn_diff_next_pdcp_rx_sn = sn - next_pdcp_rx_sn;
+
+  log->debug("HFN: %d, SN: %d, Last_Submitted_PDCP_RX_SN: %d, Next_PDCP_RX_SN %d\n",
+             rx_hfn,
+             sn,
+             last_submitted_pdcp_rx_sn,
+             next_pdcp_rx_sn);
+
+  if ((0 <= sn_diff_last_submit && sn_diff_last_submit > (int32_t)reordering_window) ||
+      (0 <= last_submit_diff_sn && last_submit_diff_sn > (int32_t)reordering_window)) {
+    log->debug("|SN - last_submitted_sn| is larger than re-ordering window.\n");
+    if (sn > next_pdcp_rx_sn) {
+      count = (rx_hfn - 1) << cfg.sn_len | sn;
+    } else {
+      count = rx_hfn << cfg.sn_len | sn;
+    }
+  } else if ((int32_t)(next_pdcp_rx_sn - sn) > (int32_t)reordering_window) {
+    log->debug("(Next_PDCP_RX_SN - SN) is larger than re-ordering window.\n");
+    rx_hfn++;
+    count = (rx_hfn << cfg.sn_len) | sn;
+    next_pdcp_rx_sn = sn + 1;
+  } else if (sn_diff_next_pdcp_rx_sn >= (int32_t)reordering_window) {
+    log->debug("(SN - Next_PDCP_RX_SN) is larger or equal than re-ordering window.\n");
+    count = ((rx_hfn - 1) << cfg.sn_len) | sn;
+  } else if (sn >= next_pdcp_rx_sn) {
+    log->debug("SN is larger or equal than Next_PDCP_RX_SN.\n");
+    count = (rx_hfn << cfg.sn_len) | sn;
+    next_pdcp_rx_sn = sn + 1;
+    if (next_pdcp_rx_sn > maximum_pdcp_sn) {
+      next_pdcp_rx_sn = 0;
+      rx_hfn++;
+    }
+  } else if (sn < next_pdcp_rx_sn) {
+    log->debug("SN is smaller than Next_PDCP_RX_SN.\n");
+    count = (rx_hfn << cfg.sn_len) | sn;
+  }
+
+  // FIXME Check if PDU is not due to re-establishment of lower layers?
+  cipher_decrypt(pdu->msg, count, pdu->N_bytes, pdu->msg);
+  log->debug_hex(pdu->msg, pdu->N_bytes, "RX %s PDU (decrypted)", rrc->get_rb_name(lcid).c_str());
+  last_submitted_pdcp_rx_sn = sn; 
   return;
-}
+  }
 
 /****************************************************************************
  * Security functions
diff --git a/srsenb/src/stack/upper/gtpu.cc b/srsenb/src/stack/upper/gtpu.cc
index dded6437b..33f2d28b5 100644
--- a/srsenb/src/stack/upper/gtpu.cc
+++ b/srsenb/src/stack/upper/gtpu.cc
@@ -119,9 +119,17 @@ void gtpu::write_pdu(uint16_t rnti, uint32_t lcid, srslte::unique_byte_buffer_t
   // Check valid IP version
   struct iphdr*   ip_pkt  = (struct iphdr*)pdu->msg;
   if (ip_pkt->version != 4 && ip_pkt->version != 6) {
-    gtpu_log->error("Invalid IP version to TUN\n");
+    gtpu_log->error("Invalid IP version to SPGW\n");
+  } else if (ip_pkt->version == 4) {
+    if(ntohs(ip_pkt->tot_len) != pdu->N_bytes){
+      gtpu_log->error("IP Len and PDU N_bytes mismatch\n");
+    }
+    gtpu_log->debug("S1-U PDU -- IP version %d, Total length %d\n", ip_pkt->version, ntohs(ip_pkt->tot_len));
+    gtpu_log->debug("S1-U PDU -- IP src addr %s\n", srslte::gtpu_ntoa(ip_pkt->saddr).c_str());
+    gtpu_log->debug("S1-U PDU -- IP dst addr %s\n", srslte::gtpu_ntoa(ip_pkt->daddr).c_str());
   }
 
+
   gtpu_header_t header;
   header.flags        = GTPU_FLAGS_VERSION_V1 | GTPU_FLAGS_GTP_PROTOCOL;
   header.message_type = GTPU_MSG_DATA_PDU;
diff --git a/srsepc/hdr/spgw/gtpu.h b/srsepc/hdr/spgw/gtpu.h
index 882c1072b..1ce202467 100644
--- a/srsepc/hdr/spgw/gtpu.h
+++ b/srsepc/hdr/spgw/gtpu.h
@@ -56,8 +56,6 @@ public:
   virtual void send_all_queued_packets(srslte::gtp_fteid_t                 dw_user_fteid,
                                        std::queue<srslte::byte_buffer_t*>& pkt_queue);
 
-  std::string gtpu_ntoa(uint32_t addr);
-
   spgw*                m_spgw;
   gtpc_interface_gtpu* m_gtpc;
 
@@ -94,19 +92,6 @@ inline in_addr_t spgw::gtpu::get_s1u_addr()
   return m_s1u_addr.sin_addr.s_addr;
 }
 
-// Helper function to return a string from IPv4 address for easy printing
-inline std::string spgw::gtpu::gtpu_ntoa(uint32_t addr)
-{
-  char tmp_str[INET_ADDRSTRLEN + 1];
-  bzero(tmp_str, sizeof(tmp_str));
-  struct in_addr tmp_addr;
-  tmp_addr.s_addr     = addr;
-  const char* tmp_ptr = inet_ntop(AF_INET, &tmp_addr, tmp_str, INET_ADDRSTRLEN);
-  if (tmp_ptr == NULL) {
-    return std::string("Invalid IPv4 address");
-  }
-  return std::string(tmp_str);
-}
 
 } // namespace srsepc
 #endif // SRSEPC_GTPU_H
diff --git a/srsepc/src/spgw/gtpu.cc b/srsepc/src/spgw/gtpu.cc
index 9cd136a77..1d9364669 100644
--- a/srsepc/src/spgw/gtpu.cc
+++ b/srsepc/src/spgw/gtpu.cc
@@ -220,8 +220,8 @@ void spgw::gtpu::handle_sgi_pdu(srslte::byte_buffer_t* msg)
 
   // Logging PDU info
   m_gtpu_log->debug("SGi PDU -- IP version %d, Total length %d\n", iph->version, ntohs(iph->tot_len));
-  m_gtpu_log->debug("SGi PDU -- IP src addr %s\n", gtpu_ntoa(iph->saddr).c_str());
-  m_gtpu_log->debug("SGi PDU -- IP dst addr %s\n", gtpu_ntoa(iph->daddr).c_str());
+  m_gtpu_log->debug("SGi PDU -- IP src addr %s\n", srslte::gtpu_ntoa(iph->saddr).c_str());
+  m_gtpu_log->debug("SGi PDU -- IP dst addr %s\n", srslte::gtpu_ntoa(iph->daddr).c_str());
 
   // Find user and control tunnel
   gtpu_fteid_it = m_ip_to_usr_teid.find(iph->daddr);
@@ -331,8 +331,8 @@ void spgw::gtpu::send_all_queued_packets(srslte::gtp_fteid_t                 dw_
 bool spgw::gtpu::modify_gtpu_tunnel(in_addr_t ue_ipv4, srslte::gtpc_f_teid_ie dw_user_fteid, uint32_t up_ctrl_teid)
 {
   m_gtpu_log->info("Modifying GTP-U Tunnel.\n");
-  m_gtpu_log->info("UE IP %s\n", gtpu_ntoa(ue_ipv4).c_str());
-  m_gtpu_log->info("Downlink eNB addr %s, U-TEID 0x%x\n", gtpu_ntoa(dw_user_fteid.ipv4).c_str(), dw_user_fteid.teid);
+  m_gtpu_log->info("UE IP %s\n", srslte::gtpu_ntoa(ue_ipv4).c_str());
+  m_gtpu_log->info("Downlink eNB addr %s, U-TEID 0x%x\n", srslte::gtpu_ntoa(dw_user_fteid.ipv4).c_str(), dw_user_fteid.teid);
   m_gtpu_log->info("Uplink C-TEID: 0x%x\n", up_ctrl_teid);
   m_ip_to_usr_teid[ue_ipv4] = dw_user_fteid;
   m_ip_to_ctr_teid[ue_ipv4] = up_ctrl_teid;