adding upper layer embms support to the UE

srsue/hdr/mac/demux.h

@@ -58,7 +58,7 @@ public:
   bool     get_uecrid_successful();
 
   void     process_pdu(uint8_t *pdu, uint32_t nof_bytes, srslte::pdu_queue::channel_t channel, uint32_t tstamp);
-  
+  void     mch_start_rx(uint32_t lcid);
 private:
   const static int MAX_PDU_LEN     = 150*1024/8; // ~ 150 Mbps  
   const static int NOF_BUFFER_PDUS = 64; // Number of PDU buffers per HARQ pid
@@ -69,9 +69,13 @@ private:
   void *uecrid_callback_arg; 
   
   srslte::sch_pdu mac_msg;
+  srslte::mch_pdu mch_mac_msg;
   srslte::sch_pdu pending_mac_msg;
-  
+  uint8_t      mch_lcids[SRSLTE_N_MCH_LCIDS];
   void process_sch_pdu(srslte::sch_pdu *pdu);
+  void process_mch_pdu(srslte::mch_pdu *pdu);
+  
+   
   bool process_ce(srslte::sch_subh *subheader);
   
   bool       is_uecrid_successful; 

srsue/hdr/mac/mac.h

@@ -65,17 +65,23 @@ public:
   void new_grant_ul_ack(mac_grant_t grant, bool ack, tb_action_ul_t *action);
   void harq_recv(uint32_t tti, bool ack, tb_action_ul_t *action);
   void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action);
+  void new_mch_dl(srslte_ra_dl_grant_t phy_grant, tb_action_dl_t *action);
   void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid);
   void bch_decoded_ok(uint8_t *payload, uint32_t len);
+
   void pch_decoded_ok(uint32_t len);
+  void mch_decoded_ok(uint32_t len);
+  void process_mch_pdu(uint32_t len);
 
+  void set_mbsfn_config(uint32_t nof_mbsfn_services);
   
-  /******** Interface from RLC (RLC -> MAC) ****************/ 
+  /******** Interface from RRC (RRC -> MAC) ****************/
   void bcch_start_rx(); 
   void bcch_start_rx(int si_window_start, int si_window_length);
   void pcch_start_rx(); 
   void clear_rntis();
   void setup_lcid(uint32_t lcid, uint32_t lcg, uint32_t priority, int PBR_x_tti, uint32_t BSD);
+  void mch_start_rx(uint32_t lcid);
   void reconfiguration(); 
   void reset();
   void wait_uplink();
@@ -119,6 +125,7 @@ private:
   rlc_interface_mac    *rlc_h; 
   rrc_interface_mac    *rrc_h; 
   srslte::log          *log_h;
+  mac_interface_phy::mac_phy_cfg_mbsfn_t phy_mbsfn_cfg;
   
   // MAC configuration 
   mac_cfg_t     config; 
@@ -147,6 +154,13 @@ private:
   srslte_softbuffer_rx_t pch_softbuffer;
   uint8_t                pch_payload_buffer[pch_payload_buffer_sz];
 
+  /* Buffers for MCH reception (not included in DL HARQ) */
+  const static uint32_t  mch_payload_buffer_sz = SRSLTE_MAX_BUFFER_SIZE_BYTES;
+  srslte_softbuffer_rx_t mch_softbuffer;
+  uint8_t                mch_payload_buffer[mch_payload_buffer_sz];
+  srslte::mch_pdu        mch_msg;
+ 
+
 
   /* Functions for MAC Timers */
   uint32_t        timer_alignment;

srsue/hdr/ue.h

@@ -73,6 +73,8 @@ public:
   bool deattach();
   bool is_attached();
   void start_plot();
+  void print_mbms();
+  void mbms_service_start(uint32_t serv, uint32_t port);
 
   void print_pool();
 

srsue/hdr/ue_base.h

@@ -166,6 +166,9 @@ public:
 
   virtual void radio_overflow() = 0;
 
+  virtual void print_mbms() = 0;
+  virtual void mbms_service_start(uint32_t serv, uint32_t port) = 0;
+  
   void handle_rf_msg(srslte_rf_error_t error);
 
   // UE metrics interface

srsue/hdr/upper/gw.h

@@ -42,6 +42,7 @@ namespace srsue {
 class gw
     :public gw_interface_pdcp
     ,public gw_interface_nas
+    ,public gw_interface_rrc
     ,public thread
 {
 public:
@@ -50,14 +51,17 @@ public:
   void stop();
 
   void get_metrics(gw_metrics_t &m);
+  void set_netmask(std::string netmask);
 
   // PDCP interface
   void write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu);
+  void write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *pdu);
 
   // NAS interface
   srslte::error_t setup_if_addr(uint32_t ip_addr, char *err_str);
 
-  void set_netmask(std::string netmask);
+  // RRC interface
+  void add_mch_port(uint32_t lcid, uint32_t port);
 
 private:
 
@@ -88,6 +92,12 @@ private:
 
   void                run_thread();
   srslte::error_t     init_if(char *err_str);
+
+  // MBSFN
+  int      mbsfn_sock_fd;                   // Sink UDP socket file descriptor
+  struct   sockaddr_in mbsfn_sock_addr;     // Target address
+  uint32_t mbsfn_ports[SRSLTE_N_MCH_LCIDS]; // Target ports for MBSFN data
+
 };
 
 } // namespace srsue

srsue/hdr/upper/rrc.h

@@ -180,6 +180,9 @@ class cell_t
   LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT *sib3ptr() {
     return &sib3;
   }
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT *sib13ptr() {
+    return &sib13;
+  }
 
   uint32_t get_cell_id() {
     return sib1.cell_id;
@@ -232,19 +235,22 @@ class cell_t
 
   phy_interface_rrc::phy_cell_t phy_cell;
   bool     in_sync; 
+  bool     has_mcch;
+   LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT  sib1;
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT  sib2;
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT  sib3;
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT sib13;
+  LIBLTE_RRC_MCCH_MSG_STRUCT               mcch;
 
 private:
   float    rsrp;
+  
   struct timeval last_update;
 
   bool     has_valid_sib1;
   bool     has_valid_sib2;
   bool     has_valid_sib3;
   bool     has_valid_sib13;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT  sib1;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT  sib2;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT  sib3;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT sib13;
 };
 
 class rrc
@@ -266,6 +272,7 @@ public:
             pdcp_interface_rrc *pdcp_,
             nas_interface_rrc *nas_,
             usim_interface_rrc *usim_,
+            gw_interface_rrc   *gw_,
             srslte::mac_interface_timers *mac_timers_,
             srslte::log *rrc_log_);
 
@@ -278,6 +285,9 @@ public:
   void timer_expired(uint32_t timeout_id);
   void liblte_rrc_log(char *str);
   
+  void print_mbms();
+  void mbms_service_start(uint32_t serv, uint32_t port);
+
   // NAS interface
   void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
   void enable_capabilities();
@@ -309,7 +319,7 @@ public:
   void write_pdu_bcch_bch(byte_buffer_t *pdu);
   void write_pdu_bcch_dlsch(byte_buffer_t *pdu);
   void write_pdu_pcch(byte_buffer_t *pdu);
-
+  void write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *pdu);
 
 private:
 
@@ -335,6 +345,7 @@ private:
   pdcp_interface_rrc *pdcp;
   nas_interface_rrc *nas;
   usim_interface_rrc *usim;
+  gw_interface_rrc    *gw;
   
   LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
   LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
@@ -435,7 +446,7 @@ private:
   uint32_t           sib_start_tti(uint32_t tti, uint32_t period, uint32_t offset, uint32_t sf);
   const static int SIB_SEARCH_TIMEOUT_MS = 1000;
 
-  const static uint32_t NOF_REQUIRED_SIBS = 3; // SIB1, SIB2 and SIB3
+  const static uint32_t NOF_REQUIRED_SIBS = 13; // SIB1, SIB2 and SIB3
 
   bool initiated;
   bool ho_start;
@@ -618,12 +629,14 @@ private:
   void          handle_sib13();
 
   void          apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
+  void          apply_sib13_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT *sib13);
   void          handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
   void          handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
   void          handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig);
   void          add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg);
   void          add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg);
   void          release_drb(uint8_t lcid);
+   void         add_mrb(uint32_t lcid, uint32_t port);
   bool          apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg);
   void          apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults); 
   void          apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cfg, bool apply_defaults); 

srsue/src/mac/demux.cc

@@ -36,7 +36,7 @@
 
 namespace srsue {
     
-demux::demux() : mac_msg(20), pending_mac_msg(20), rlc(NULL)
+demux::demux() : mac_msg(20), mch_mac_msg(20), pending_mac_msg(20), rlc(NULL)
 {
 }
 
@@ -47,6 +47,7 @@ void demux::init(phy_interface_mac_common* phy_h_, rlc_interface_mac *rlc_, srsl
   rlc       = rlc_;
   time_alignment_timer = time_alignment_timer_;
   pdus.init(this, log_h);
+   bzero(&mch_lcids, SRSLTE_N_MCH_LCIDS);
 }
 
 void demux::set_uecrid_callback(bool (*callback)(void*,uint64_t), void *arg) {
@@ -129,7 +130,10 @@ void demux::push_pdu_bcch(uint8_t *buff, uint32_t nof_bytes, uint32_t tstamp) {
 }
 
 void demux::push_pdu_mch(uint8_t *buff, uint32_t nof_bytes, uint32_t tstamp) {
-  pdus.push(buff, nof_bytes, srslte::pdu_queue::MCH, tstamp);
+  uint8_t *mch_buffer_ptr = request_buffer(nof_bytes);
+  memcpy(mch_buffer_ptr, buff, nof_bytes);
+  pdus.push(mch_buffer_ptr, nof_bytes, srslte::pdu_queue::MCH, tstamp);
+  mch_buffer_ptr = NULL;
 }
 
 bool demux::process_pdus()
@@ -145,16 +149,17 @@ void demux::process_pdu(uint8_t *mac_pdu, uint32_t nof_bytes, srslte::pdu_queue:
       // Unpack DLSCH MAC PDU
       mac_msg.init_rx(nof_bytes);
       mac_msg.parse_packet(mac_pdu);
-
       process_sch_pdu(&mac_msg);
-      //srslte_vec_fprint_byte(stdout, mac_pdu, nof_bytes);
-
       pdus.deallocate(mac_pdu);
       break;
     case srslte::pdu_queue::BCH:
       rlc->write_pdu_bcch_dlsch(mac_pdu, nof_bytes);
       break;
     case srslte::pdu_queue::MCH:
+      mch_mac_msg.init_rx(nof_bytes);
+      mch_mac_msg.parse_packet(mac_pdu);
+      deallocate(mac_pdu);
+      process_mch_pdu(&mch_mac_msg);
       // Process downlink MCH
       break;
   }
@@ -168,7 +173,7 @@ void demux::process_sch_pdu(srslte::sch_pdu *pdu_msg)
       if (pdu_msg->get()->get_sdu_lcid() == 0) {
         uint8_t *x = pdu_msg->get()->get_sdu_ptr();
         uint32_t sum = 0; 
-        for (int i=0;i<pdu_msg->get()->get_payload_size();i++) {
+        for (uint32_t i=0;i<pdu_msg->get()->get_payload_size();i++) {
           sum += x[i];
         }
         if (sum == 0) {
@@ -196,6 +201,42 @@ void demux::process_sch_pdu(srslte::sch_pdu *pdu_msg)
     }
   }      
 }
+void demux::process_mch_pdu(srslte::mch_pdu *mch_msg){
+ 
+    //disgarding headers that have already been processed
+  //printf("in process cur idx, %d  subheaders %d\n",mch_msg->cur_idx,mch_msg->nof_subheaders);
+  while(mch_msg->next()){
+    
+    if(srslte::mch_subh::MCH_SCHED_INFO == mch_msg->get()->ce_type()){
+       uint16_t stop;
+       uint8_t  lcid;
+       if(mch_msg->get()->get_next_mch_sched_info(&lcid, &stop)) {
+         Info("MCH Sched Info: LCID: %d, Stop: %d, tti is %d \n", lcid, stop, phy_h->get_current_tti());
+       }
+    }
+    if(mch_msg->get()->is_sdu()) {
+      uint32_t lcid = mch_msg->get()->get_sdu_lcid();
+      if(lcid < 0 || lcid >= SRSLTE_N_MCH_LCIDS) {
+        Error("Radio bearer id must be in [0:%d] - %d\n", SRSLTE_N_MCH_LCIDS, lcid);
+        return;
+      }
+      Info("Wrote MCH LCID=%d to RLC\n", lcid);
+      if(1 == mch_lcids[lcid]) {
+        rlc->write_pdu_mch(lcid, mch_msg->get()->get_sdu_ptr(), mch_msg->get()->get_payload_size());
+      } 
+    }
+  }
+}
+
+void demux::mch_start_rx(uint32_t lcid)
+{
+  if(lcid>=0 && lcid<32) {
+    Info("MCH Channel Setup: LCID=%d\n", lcid);
+    mch_lcids[lcid] = 1;
+  } else {
+    Warning("MCH Channel Setup: invalid LCID=%d\n", lcid);
+  }
+}
 
 bool demux::process_ce(srslte::sch_subh *subh) {
   switch(subh->ce_type()) {

srsue/src/mac/mac.cc

@@ -43,7 +43,8 @@ namespace srsue {
 
 mac::mac() : timers(64),
              mux_unit(MAC_NOF_HARQ_PROC),
-             pdu_process_thread(&demux_unit)
+             pdu_process_thread(&demux_unit),
+             mch_msg(10)
 {
   pcap    = NULL;
   bzero(&metrics, sizeof(mac_metrics_t));
@@ -58,6 +59,7 @@ bool mac::init(phy_interface_mac *phy, rlc_interface_mac *rlc, rrc_interface_mac
   tti = 0;
 
   srslte_softbuffer_rx_init(&pch_softbuffer, 100);
+  srslte_softbuffer_rx_init(&mch_softbuffer, 100);
 
   timer_alignment             = timers.get_unique_id();
   contention_resolution_timer = timers.get_unique_id();
@@ -220,6 +222,32 @@ void mac::pch_decoded_ok(uint32_t len)
   }
 }
 
+void mac::mch_decoded_ok(uint32_t len)
+{
+  // Parse MAC header
+  mch_msg.init_rx(len);
+  
+  mch_msg.parse_packet(mch_payload_buffer);
+  while(mch_msg.next()) {
+    for(uint32_t i = 0; i < phy_mbsfn_cfg.nof_mbsfn_services;i++) {
+      if(srslte::mch_subh::MCH_SCHED_INFO == mch_msg.get()->ce_type()) {
+        uint16_t stop;
+        uint8_t  lcid;
+        if(mch_msg.get()->get_next_mch_sched_info(&lcid, &stop)) {
+          phy_h->set_mch_period_stop(stop);
+          Info("MCH Sched Info: LCID: %d, Stop: %d, tti is %d \n", lcid, stop, phy_h->get_current_tti());
+        }
+      }
+    }
+  }
+  
+  demux_unit.push_pdu_mch(mch_payload_buffer, len, 0);
+  pdu_process_thread.notify();
+  if (pcap) {
+    pcap->write_dl_mch(mch_payload_buffer, len, true, phy_h->get_current_tti());
+  }
+}
+
 void mac::tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
 {
   if (rnti_type == SRSLTE_RNTI_RAR) {
@@ -303,6 +331,16 @@ void mac::new_grant_ul_ack(mac_interface_phy::mac_grant_t grant, bool ack, mac_i
   }
 }
 
+void mac::new_mch_dl(srslte_ra_dl_grant_t phy_grant, tb_action_dl_t *action)
+{
+  memcpy(&action->phy_grant, &phy_grant, sizeof(srslte_phy_grant_t));
+  action->generate_ack = false;
+  action->decode_enabled[0] = true;
+  srslte_softbuffer_rx_reset_cb(&mch_softbuffer, 1);
+  action->payload_ptr[0] = mch_payload_buffer;
+  action->softbuffers[0]  = &mch_softbuffer;
+}
+
 void mac::harq_recv(uint32_t tti, bool ack, mac_interface_phy::tb_action_ul_t* action)
 {
   int tbs = ul_harq.get_current_tbs(tti);
@@ -378,6 +416,13 @@ void mac::get_config(mac_cfg_t* mac_cfg)
   memcpy(mac_cfg, &config, sizeof(mac_cfg_t));
 }
 
+void mac::set_mbsfn_config(uint32_t nof_mbsfn_services)
+{
+  //cfg->nof_mbsfn_services = config.mbsfn.mcch.pmch_infolist_r9[0].mbms_sessioninfolist_r9_size;
+  phy_mbsfn_cfg.nof_mbsfn_services = nof_mbsfn_services;
+}
+
+
 void mac::set_config(mac_cfg_t* mac_cfg)
 {
   memcpy(&config, mac_cfg, sizeof(mac_cfg_t));
@@ -410,6 +455,11 @@ void mac::setup_lcid(uint32_t lcid, uint32_t lcg, uint32_t priority, int PBR_x_t
   bsr_procedure.set_priority(lcid, priority);
 }
 
+void mac::mch_start_rx(uint32_t lcid)
+{
+  demux_unit.mch_start_rx(lcid);
+}
+
 void mac::get_metrics(mac_metrics_t &m)
 {
   Info("DL retx: %.2f \%%, perpkt: %.2f, UL retx: %.2f \%% perpkt: %.2f\n", 

srsue/src/main.cc

@@ -408,6 +408,9 @@ static int sigcnt = 0;
 static bool running = true;
 static bool do_metrics = false;
 metrics_stdout metrics_screen;
+static bool show_mbms = false;
+static bool mbms_service_start = false;
+uint32_t serv, port;
 
 void sig_int_handler(int signo) {
   sigcnt++;
@@ -419,14 +422,14 @@ void sig_int_handler(int signo) {
 }
 
 void *input_loop(void *m) {
-  char key;
+  string key;
   while (running) {
-    cin >> key;
+    getline(cin, key);
     if (cin.eof() || cin.bad()) {
       cout << "Closing stdin thread." << endl;
       break;
     } else {
-      if ('t' == key) {
+      if (0 == key.compare("t")) {
         do_metrics = !do_metrics;
         if (do_metrics) {
           cout << "Enter t to stop trace." << endl;
@@ -435,9 +438,30 @@ void *input_loop(void *m) {
         }
         metrics_screen.toggle_print(do_metrics);
       } else
-      if ('q' == key) {
+      if (0 == key.compare("q")) {
         running = false;
       }  
+    else if (0 == key.compare("mbms")) {
+      show_mbms = true;
+    } else if (key.find("mbms_service_start") != string::npos) {
+
+      char *dup = strdup(key.c_str());
+      strtok(dup, " ");
+      char *s = strtok(NULL, " ");
+      if(NULL == s) {
+        cout << "Usage: mbms_service_start <service_id> <port_number>" << endl;
+        continue;
+      }
+      serv = atoi(s);
+      char* p = strtok(NULL, " ");
+      if(NULL == p) {
+        cout << "Usage: mbms_service_start <service_id> <port_number>" << endl;
+        continue;
+      }
+      port = atoi(p);
+      mbms_service_start = true;
+      free(dup);
+    }
    }
   }
   return NULL;
@@ -493,15 +517,27 @@ int main(int argc, char *argv[])
   }
   int cnt=0;
   while (running) {
+    if(mbms_service_start) {
+      mbms_service_start = false;
+      ue->mbms_service_start(serv, port);
+    }
+    if(show_mbms) {
+      show_mbms = false;
+      ue->print_mbms();
+    }
+    sleep(1);
     if (args.expert.print_buffer_state) {
       cnt++;
       if (cnt==10) {
         cnt=0;
         ue->print_pool();
       }
-    }
+    } else {
+      while (!ue->attach() && running) {
         sleep(1);
       }
+    }
+  }
   pthread_cancel(input);
   metricshub.stop();
   ue->stop();

srsue/src/ue.cc

@@ -203,8 +203,7 @@ bool ue::init(all_args_t *args_)
 
   gw.set_netmask(args->expert.ip_netmask);
   
-  rrc.init(&phy, &mac, &rlc, &pdcp, &nas, &usim, &mac, &rrc_log);
+  rrc.init(&phy, &mac, &rlc, &pdcp, &nas, &usim, &gw, &mac, &rrc_log);
-
   // Get current band from provided EARFCN
   args->rrc.supported_bands[0] = srslte_band_get_band(args->rf.dl_earfcn);
   args->rrc.nof_supported_bands = 1;
@@ -315,9 +314,19 @@ bool ue::get_metrics(ue_metrics_t &m)
   return false;
 }
 
+
 void ue::radio_overflow() {
   phy.radio_overflow();
 }
+void ue::print_mbms()
+{
+  rrc.print_mbms();
+}
+
+void ue::mbms_service_start(uint32_t serv, uint32_t port)
+{
+  rrc.mbms_service_start(serv, port);
+}
 
 void ue::rf_msg(srslte_rf_error_t error)
 {

srsue/src/upper/gw.cc

@@ -59,6 +59,19 @@ void gw::init(pdcp_interface_gw *pdcp_, nas_interface_gw *nas_, srslte::log *gw_
   gettimeofday(&metrics_time[1], NULL);
   dl_tput_bytes = 0;
   ul_tput_bytes = 0;
+  // MBSFN
+  mbsfn_sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
+  if (mbsfn_sock_fd < 0) {
+    gw_log->error("Failed to create MBSFN sink socket\n");
+  }
+  if (fcntl(mbsfn_sock_fd, F_SETFL, O_NONBLOCK)) {
+    gw_log->error("Failed to set non-blocking MBSFN sink socket\n");
+  }
+
+  mbsfn_sock_addr.sin_family      = AF_INET;
+  mbsfn_sock_addr.sin_addr.s_addr =inet_addr("127.0.0.1");
+
+  bzero(mbsfn_ports, SRSLTE_N_MCH_LCIDS*sizeof(uint32_t));
 }
 
 void gw::stop()
@@ -83,14 +96,15 @@ void gw::stop()
 
       current_ip_addr = 0;
     }
-
     // TODO: tear down TUN device?
   }
+  if (mbsfn_sock_fd) {
+    close(mbsfn_sock_fd);
+  }
 }
 
 void gw::get_metrics(gw_metrics_t &m)
 {
-  
   gettimeofday(&metrics_time[2], NULL);
   get_time_interval(metrics_time);
   double secs = (double) metrics_time[0].tv_sec+metrics_time[0].tv_usec*1e-6;
@@ -105,7 +119,8 @@ void gw::get_metrics(gw_metrics_t &m)
   ul_tput_bytes = 0;
 }
 
-void gw::set_netmask(std::string netmask) {
+void gw::set_netmask(std::string netmask)
+{
   default_netmask = false;
   this->netmask = netmask;
 }
@@ -116,8 +131,7 @@ void gw::set_netmask(std::string netmask) {
 *******************************************************************************/
 void gw::write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu)
 {
-  gw_log->info_hex(pdu->msg, pdu->N_bytes, "RX PDU");
+  gw_log->info_hex(pdu->msg, pdu->N_bytes, "RX PDU. Stack latency: %ld us\n", pdu->get_latency_us());
-  gw_log->info("RX PDU. Stack latency: %ld us\n", pdu->get_latency_us());
   dl_tput_bytes += pdu->N_bytes;
   if(!if_up)
   {
@@ -132,6 +146,48 @@ void gw::write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu)
   pool->deallocate(pdu);
 }
 
+void gw::write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *pdu)
+{
+  if(pdu->N_bytes>2)
+  {
+    gw_log->info_hex(pdu->msg, pdu->N_bytes, "RX MCH PDU. Stack latency: %ld us\n", pdu->get_latency_us());
+    dl_tput_bytes += pdu->N_bytes;
+
+    //Hack to drop initial 2 bytes
+    pdu->msg +=2;
+    pdu->N_bytes-=2;
+    struct in_addr dst_addr;
+    memcpy(&dst_addr.s_addr, &pdu->msg[16],4);
+    gw_log->console("gw\n");
+    gw_log->console("Destination IP: %s\n",inet_ntoa(dst_addr));
+    srslte_vec_fprint_b(stdout,&pdu->msg[0], pdu->N_bytes);
+    if(!if_up)
+    {
+      gw_log->warning("TUN/TAP not up - dropping gw RX message\n");
+    }else{
+      int n = write(tun_fd, pdu->msg, pdu->N_bytes); 
+      if(n > 0 && (pdu->N_bytes != (uint32_t)n))
+      {
+        gw_log->warning("DL TUN/TAP write failure\n");
+      }
+    }
+    /*
+    // Strip IP/UDP header
+    pdu->msg += 28;
+    pdu->N_bytes -= 28;
+
+    if(mbsfn_sock_fd) {
+      if(lcid > 0 && lcid < SRSLTE_N_MCH_LCIDS) {
+        mbsfn_sock_addr.sin_port = htons(mbsfn_ports[lcid]);
+        if(sendto(mbsfn_sock_fd, pdu->msg, pdu->N_bytes, MSG_EOR, (struct sockaddr*)&mbsfn_sock_addr, sizeof(struct sockaddr_in))<0) {
+          gw_log->error("Failed to send MCH PDU to port %d\n", mbsfn_ports[lcid]);
+        }
+      }
+    }*/
+  }
+  pool->deallocate(pdu);
+}
+
 /*******************************************************************************
   NAS interface
 *******************************************************************************/
@@ -234,6 +290,19 @@ srslte::error_t gw::init_if(char *err_str)
   return(srslte::ERROR_NONE);
 }
 
+
+/*******************************************************************************
+  RRC interface
+*******************************************************************************/
+void gw::add_mch_port(uint32_t lcid, uint32_t port)
+{
+  if(lcid > 0 && lcid < SRSLTE_N_MCH_LCIDS) {
+    mbsfn_ports[lcid] = port;
+  }
+}
+
+
+
 /********************/
 /*    GW Receive    */
 /********************/

srsue/src/upper/rrc.cc

@@ -80,6 +80,63 @@ void rrc::liblte_rrc_log(char *str) {
     printf("[ASN]: %s\n", str);
   }
 }
+void rrc::print_mbms()
+{
+  if(rrc_log) {
+    if(serving_cell->has_mcch) {
+      LIBLTE_RRC_MCCH_MSG_STRUCT msg;
+      memcpy(&msg, &serving_cell->mcch, sizeof(LIBLTE_RRC_MCCH_MSG_STRUCT));
+      std::stringstream ss;
+      for(uint32_t i=0;i<msg.pmch_infolist_r9_size; i++){
+        ss << "PMCH: " << i << std::endl;
+        LIBLTE_RRC_PMCH_INFO_R9_STRUCT *pmch = &msg.pmch_infolist_r9[i];
+        for(uint32_t j=0;j<pmch->mbms_sessioninfolist_r9_size; j++) {
+          LIBLTE_RRC_MBMS_SESSION_INFO_R9_STRUCT *sess = &pmch->mbms_sessioninfolist_r9[j];
+          ss << "  Service ID: " << sess->tmgi_r9.serviceid_r9;
+          if(sess->sessionid_r9_present) {
+            ss << ", Session ID: " << (uint32_t)sess->sessionid_r9;
+          }
+          if(sess->tmgi_r9.plmn_id_explicit) {
+            std::string tmp;
+            if(mcc_to_string(sess->tmgi_r9.plmn_id_r9.mcc, &tmp)) {
+              ss << ", MCC: " << tmp;
+            }
+            if(mnc_to_string(sess->tmgi_r9.plmn_id_r9.mnc, &tmp)) {
+              ss << ", MNC: " << tmp;
+            }
+          } else {
+            ss << ", PLMN index: " << (uint32_t)sess->tmgi_r9.plmn_index_r9;
+          }
+          ss << ", LCID: " << (uint32_t)sess->logicalchannelid_r9;
+          ss << std::endl;
+        }
+      }
+      //rrc_log->console(ss.str());
+      std::cout << ss.str();
+    } else {
+      rrc_log->console("MCCH not available for current cell\n");
+    }
+  }
+}
+
+void rrc::mbms_service_start(uint32_t serv, uint32_t port)
+{
+  rrc_log->console("MBMS service start requested. Service id:%d, port: %d\n", serv, port);
+  if(serving_cell->has_mcch) {
+    LIBLTE_RRC_MCCH_MSG_STRUCT msg;
+    memcpy(&msg, &serving_cell->mcch, sizeof(LIBLTE_RRC_MCCH_MSG_STRUCT));
+    for(uint32_t i=0;i<msg.pmch_infolist_r9_size; i++){
+      LIBLTE_RRC_PMCH_INFO_R9_STRUCT *pmch = &msg.pmch_infolist_r9[i];
+      for(uint32_t j=0;j<pmch->mbms_sessioninfolist_r9_size; j++) {
+        LIBLTE_RRC_MBMS_SESSION_INFO_R9_STRUCT *sess = &pmch->mbms_sessioninfolist_r9[j];
+        if(serv == sess->tmgi_r9.serviceid_r9) {
+          add_mrb(sess->logicalchannelid_r9, port);
+        }
+      }
+    }
+  }
+}
+
 
 void rrc::init(phy_interface_rrc *phy_,
                mac_interface_rrc *mac_,
@@ -87,6 +144,7 @@ void rrc::init(phy_interface_rrc *phy_,
                pdcp_interface_rrc *pdcp_,
                nas_interface_rrc *nas_,
                usim_interface_rrc *usim_,
+               gw_interface_rrc *gw_,
                mac_interface_timers *mac_timers_,
                srslte::log *rrc_log_) {
   pool = byte_buffer_pool::get_instance();
@@ -96,6 +154,7 @@ void rrc::init(phy_interface_rrc *phy_,
   pdcp = pdcp_;
   nas = nas_;
   usim = usim_;
+  gw = gw_;
   rrc_log = rrc_log_;
 
   // Use MAC timers
@@ -474,6 +533,7 @@ bool rrc::configure_serving_cell() {
     rrc_log->error("Trying to configure Cell while not camping on it\n");
     return false;
   }
+  serving_cell->has_mcch = false;
   // Apply configurations if already retrieved SIB2
   if (serving_cell->has_sib2()) {
     apply_sib2_configs(serving_cell->sib2ptr());
@@ -490,6 +550,9 @@ bool rrc::configure_serving_cell() {
       }
     } else {
       rrc_log->info("Cell has SIB%d\n", i+1);
+      if(i+1 == 13){
+        apply_sib13_configs(serving_cell->sib13ptr());
+      }
     }
   }
   return true;
@@ -1763,7 +1826,23 @@ void rrc::process_pcch(byte_buffer_t *pdu) {
 }
 
 
+void rrc::write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *pdu)
+{
+  if (pdu->N_bytes > 0 && pdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BITS) {
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "MCH message received %d bytes on lcid:%d\n", pdu->N_bytes, lcid);
+    rrc_log->info("MCH message Stack latency: %ld us\n", pdu->get_latency_us());
+    //TODO: handle MCCH notifications and update MCCH
+    if(0 == lcid && !serving_cell->has_mcch) {
+      srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+      bit_buf.N_bits = pdu->N_bytes * 8;
+      liblte_rrc_unpack_mcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &serving_cell->mcch);
+      serving_cell->has_mcch = true;
+      phy->set_config_mbsfn_mcch(&serving_cell->mcch);
+    }
 
+    pool->deallocate(pdu);
+  }
+}
 
 
 
@@ -2139,6 +2218,9 @@ void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
 //    memcpy(&cfg.mbsfn_subfr_cnfg_list[i], &sib2->mbsfn_subfr_cnfg_list[i], sizeof(LIBLTE_RRC_MBSFN_SUBFRAME_CONFIG_STRUCT));
 //  }
   
+    // Set MBSFN configs
+  phy->set_config_mbsfn_sib2(sib2);
+
   mac->set_config(&cfg);
 
   rrc_log->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
@@ -2202,6 +2284,12 @@ void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
 
 }
 
+void rrc::apply_sib13_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT *sib13)
+{
+  phy->set_config_mbsfn_sib13(&serving_cell->sib13);
+  add_mrb(0, 0); // Add MRB0
+}
+
 // Go through all information elements and apply defaults (9.2.4) if not defined
 void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults) {
   // Get current configuration
@@ -2589,6 +2677,14 @@ void rrc::release_drb(uint8_t lcid) {
   // TODO
 }
 
+void rrc::add_mrb(uint32_t lcid, uint32_t port)
+{
+  gw->add_mch_port(lcid, port);
+  rlc->add_bearer_mrb(lcid);
+  mac->mch_start_rx(lcid);
+  rrc_log->info("Added MRB bearer for lcid:%d\n", lcid);
+}
+
 // PHY CONFIG DEDICATED Defaults (3GPP 36.331 v10 9.2.4)
 void rrc::set_phy_default_pucch_srs() {
 

srsue/test/mac/mac_test.cc

@@ -418,6 +418,7 @@ public:
   }
   
   void write_pdu_pcch(uint8_t *payload, uint32_t nof_bytes) {}
+  void write_pdu_mch(uint32_t lcid, uint8_t *payload, uint32_t nof_bytes) {}
   
 private:
   LIBLTE_BIT_MSG_STRUCT  bit_msg; 

srsue/test/upper/nas_test.cc

@@ -74,6 +74,7 @@ public:
       void write_pdu_bcch_bch(byte_buffer_t *pdu) {}
       void write_pdu_bcch_dlsch(byte_buffer_t *pdu) {}
       void write_pdu_pcch(byte_buffer_t *pdu) {}
+      void write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *sdu) {}
       std::string get_rb_name(uint32_t lcid) { return std::string("lcid"); }
 };
 
@@ -123,6 +124,7 @@ class gw_dummy : public gw_interface_nas, public gw_interface_pdcp
 {
   error_t setup_if_addr(uint32_t ip_addr, char *err_str) { return ERROR_NONE; }
   void write_pdu(uint32_t lcid, byte_buffer_t *pdu) {}
+  void write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *sdu) {}
 };
 
 }