adding support for embms in common functions and interfaces

lib/include/srslte/common/common.h

@@ -44,6 +44,8 @@
 #define SRSLTE_N_DRB           8
 #define SRSLTE_N_RADIO_BEARERS 11
 
+#define SRSLTE_N_MCH_LCIDS     32
+
 #define HARQ_DELAY_MS   4
 #define MSG3_DELAY_MS   2 // Delay added to HARQ_DELAY_MS
 #define TTI_RX(tti)     (tti>HARQ_DELAY_MS?((tti-HARQ_DELAY_MS)%10240):(10240+tti-HARQ_DELAY_MS))

lib/include/srslte/common/mac_pcap.h

@@ -50,6 +50,7 @@ public:
   void write_dl_sirnti(uint8_t *pdu, uint32_t pdu_len_bytes, bool crc_ok, uint32_t tti);
   void write_dl_bch(uint8_t *pdu, uint32_t pdu_len_bytes, bool crc_ok, uint32_t tti);
   void write_dl_pch(uint8_t *pdu, uint32_t pdu_len_bytes, bool crc_ok, uint32_t tti);
+  void write_dl_mch(uint8_t *pdu, uint32_t pdu_len_bytes, bool crc_ok, uint32_t tti);
   
 private:
   bool enable_write; 

lib/include/srslte/common/pdu.h

@@ -34,10 +34,10 @@
 #include <stdio.h>
 
 /* MAC PDU Packing/Unpacking functions. Section 6 of 36.321 */   
-
+class subh;
 
 namespace srslte {  
-  
+    
 template<class SubH>
 class pdu
 {
@@ -159,11 +159,10 @@ protected:
   uint32_t   total_sdu_len; 
   uint32_t   sdu_offset_start; 
   int        last_sdu_idx;
-  
-private: 
+   
   
   /* Prepares the PDU for parsing or writing by setting the number of subheaders to 0 and the pdu length */
-  void init_(uint8_t *buffer_tx_ptr, uint32_t pdu_len_bytes, bool is_ulsch) {
+  virtual void init_(uint8_t *buffer_tx_ptr, uint32_t pdu_len_bytes, bool is_ulsch) {
     nof_subheaders = 0; 
     pdu_len        = pdu_len_bytes; 
     rem_len        = pdu_len; 
@@ -180,55 +179,81 @@ private:
   }
 };
 
+
+
+typedef enum {
+  SCH_SUBH_TYPE = 0,
+  MCH_SUBH_TYPE = 1,
+  RAR_SUBH_TYPE = 2
+} subh_type;
+
 template<class SubH>
 class subh
 {
-public: 
-      
+public:
+  subh(){}
+  virtual ~subh(){}
+  
   virtual bool read_subheader(uint8_t** ptr)                        = 0;
   virtual void read_payload(uint8_t **ptr)                          = 0;    
   virtual void write_subheader(uint8_t** ptr, bool is_last)         = 0;
   virtual void write_payload(uint8_t **ptr)                         = 0;
   virtual void fprint(FILE *stream)                                 = 0;
-
+ 
   pdu<SubH>* parent; 
-private: 
+private:
   virtual void init() = 0;
 };
 
 
-
 class sch_subh : public subh<sch_subh>
 {
 
 public: 
+
+
+  sch_subh(subh_type type_ = SCH_SUBH_TYPE):type(type_){}
+
+  virtual ~sch_subh(){}
   
   typedef enum {
-    PHR_REPORT = 26,
-    CRNTI      = 27,
-    CON_RES_ID = 28,
-    TRUNC_BSR  = 28,
-    TA_CMD     = 29,
-    SHORT_BSR  = 29,
-    DRX_CMD    = 30,
-    LONG_BSR   = 30,
-    PADDING    = 31,
-    SDU        = 0
-  } cetype; 
+    PHR_REPORT       = 26,
+    CRNTI            = 27,
+    CON_RES_ID       = 28,
+    MTCH_MAX_LCID    = 28,
+    TRUNC_BSR        = 28,
+    TA_CMD           = 29,
+    SHORT_BSR        = 29,
+    DRX_CMD          = 30,
+    LONG_BSR         = 30,
+    MCH_SCHED_INFO   = 30,
+    PADDING          = 31,
+    SDU              = 0
+  } cetype;
+
+  typedef struct {
+    uint8_t  lcid;
+    uint16_t stop_mtch;
+  } mch_sched_elem;
   
   // Size of MAC CEs
   const static int MAC_CE_CONTRES_LEN = 6; 
 
   // Reading functions
   bool     is_sdu();
-  cetype   ce_type();
+  bool     is_var_len_ce();
+  cetype ce_type();
   uint32_t size_plus_header();
   void     set_payload_size(uint32_t size);
-  
-  bool     read_subheader(uint8_t** ptr);
-  void     read_payload(uint8_t **ptr);
+ 
+
+
+  bool read_subheader(uint8_t** ptr);
+  void read_payload(uint8_t **ptr);
+
+
   uint32_t get_sdu_lcid();
-  int get_payload_size();
+  uint32_t get_payload_size();
   uint32_t get_header_size(bool is_last);
   uint8_t* get_sdu_ptr();
   
@@ -237,10 +262,16 @@ public:
   uint8_t  get_ta_cmd();
   float    get_phr();
   int      get_bsr(uint32_t buff_size[4]);
+
+  bool     get_next_mch_sched_info(uint8_t *lcid, uint16_t *mtch_stop);
   
   // Writing functions
-  void     write_subheader(uint8_t** ptr, bool is_last);
-  void     write_payload(uint8_t **ptr);
+
+
+  void write_subheader(uint8_t** ptr, bool is_last);
+  void write_payload(uint8_t **ptr);
+
+
   int      set_sdu(uint32_t lcid, uint32_t nof_bytes, uint8_t *payload);
   int      set_sdu(uint32_t lcid, uint32_t requested_bytes, read_pdu_interface *sdu_itf);
   bool     set_c_rnti(uint16_t crnti);
@@ -251,41 +282,54 @@ public:
   void     set_padding();
   void     set_padding(uint32_t padding_len);
 
-  void     init();
-  void     fprint(FILE *stream);
+  void init();
+  void fprint(FILE *stream);
+  
+  bool     set_next_mch_sched_info(uint8_t lcid, uint16_t mtch_stop);
   
+  
+protected:
+
+static const int MAX_CE_PAYLOAD_LEN = 8;
+  uint32_t  lcid;
+  int       nof_bytes;
+  uint8_t*  payload;
+  uint8_t   w_payload_ce[64];
+  uint8_t   nof_mch_sched_ce;
+  uint8_t   cur_mch_sched_ce;
+  bool      F_bit;
+  subh_type type;
+
+
+
 
 private: 
-  static const int MAX_CE_PAYLOAD_LEN = 8; 
-  uint32_t lcid;
-  uint32_t nof_bytes; 
-  uint8_t* payload; 
-  uint8_t  w_payload_ce[8];
-  bool     F_bit;    
   uint32_t sizeof_ce(uint32_t lcid, bool is_ul);
   static uint8_t buff_size_table(uint32_t buffer_size);
   static uint8_t phr_report_table(float phr_value);
 };
 
+
 class sch_pdu : public pdu<sch_subh>
 {
 public:
   
-  sch_pdu(uint32_t max_subh) : pdu(max_subh) {}
-
+  sch_pdu(uint32_t max_subh): pdu(max_subh) {}
+  
   void      parse_packet(uint8_t *ptr);
   uint8_t*  write_packet();
   uint8_t*  write_packet(srslte::log *log_h);
-  bool      has_space_ce(uint32_t nbytes);  
+  bool      has_space_ce(uint32_t nbytes, bool var_len=false);
   bool      has_space_sdu(uint32_t nbytes);  
   int       get_pdu_len();
   int       rem_size(); 
   int       get_sdu_space();
   
   static uint32_t size_header_sdu(uint32_t nbytes);
-  bool      update_space_ce(uint32_t nbytes);  
-  bool      update_space_sdu(uint32_t nbytes);  
+  bool      update_space_ce(uint32_t nbytes, bool var_len=false);
+  bool      update_space_sdu(uint32_t nbytes);
   void      fprint(FILE *stream);
+
 };
 
 class rar_subh : public subh<rar_subh>
@@ -333,11 +377,56 @@ public:
   bool     write_packet(uint8_t* ptr);
   void     fprint(FILE *stream);
 
-private: 
+private:
   bool       has_backoff_indicator;
   uint8_t    backoff_indicator; 
+
 };
 
-} // namespace srslte
+class mch_subh : public sch_subh
+{
+
+public:
+    mch_subh():sch_subh(MCH_SUBH_TYPE){}
+ 
+    
+
+
+//  // Size of MAC CEs
+  const static int MAC_CE_CONTRES_LEN = 6;
+      
+  };
+
+class mch_pdu : public sch_pdu
+{
+public:
+
+  mch_pdu(uint32_t max_subh) : sch_pdu(max_subh) {}
+  
+  private:
+  /* Prepares the PDU for parsing or writing by setting the number of subheaders to 0 and the pdu length */
+      
+  virtual void init_(uint8_t *buffer_tx_ptr, uint32_t pdu_len_bytes, bool is_ulsch) {
+    nof_subheaders = 0; 
+    pdu_len        = pdu_len_bytes; 
+    rem_len        = pdu_len; 
+    pdu_is_ul      = is_ulsch; 
+    buffer_tx      = buffer_tx_ptr; 
+    sdu_offset_start = max_subheaders*2 + 13; // Assuming worst-case 2 bytes per sdu subheader + all possible CE
+    total_sdu_len  = 0; 
+    last_sdu_idx   = -1;
+    reset();
+    for (uint32_t i=0;i<max_subheaders;i++) {
+      mch_subh mch_subh1;
+      subheaders[i] = mch_subh1;
+      subheaders[i].parent = this;
+      subheaders[i].init();
+    }
+  }
+ };
+
+} // namespace srsue
+
+
+#endif // MACPDU_H
 
-#endif // SRSLTE_PDU_H

lib/include/srslte/interfaces/enb_interfaces.h

@@ -72,6 +72,7 @@ public:
   virtual int crc_info(uint32_t tti, uint16_t rnti, uint32_t nof_bytes, bool crc_res) = 0; 
   
   virtual int get_dl_sched(uint32_t tti, dl_sched_t *dl_sched_res) = 0;
+  virtual int get_mch_sched(bool is_mcch, dl_sched_t *dl_sched_res) = 0;
   virtual int get_ul_sched(uint32_t tti, ul_sched_t *ul_sched_res) = 0;
   
   // Radio-Link status 
@@ -95,6 +96,20 @@ public:
 class phy_interface_rrc
 {
 public:
+    
+   typedef struct {
+    LIBLTE_RRC_MBSFN_SUBFRAME_CONFIG_STRUCT     mbsfn_subfr_cnfg;
+    LIBLTE_RRC_MBSFN_NOTIFICATION_CONFIG_STRUCT mbsfn_notification_cnfg;
+    LIBLTE_RRC_MBSFN_AREA_INFO_STRUCT           mbsfn_area_info;
+    LIBLTE_RRC_MCCH_MSG_STRUCT                  mcch;
+  } phy_cfg_mbsfn_t;
+  
+  typedef struct {
+    phy_cfg_mbsfn_t  mbsfn;
+  } phy_rrc_cfg_t; 
+  
+  
+  virtual void configure_mbsfn(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2, LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT *sib13, LIBLTE_RRC_MCCH_MSG_STRUCT mcch) = 0;
   virtual void set_conf_dedicated_ack(uint16_t rnti, bool rrc_completed) = 0;
   virtual void set_config_dedicated(uint16_t rnti, LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT* dedicated) = 0;
   
@@ -116,7 +131,7 @@ public:
   virtual int bearer_ue_rem(uint16_t rnti, uint32_t lc_id) = 0; 
   virtual int set_dl_ant_info(uint16_t rnti, LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT *dl_ant_info) = 0;
   virtual void phy_config_enabled(uint16_t rnti, bool enabled) = 0;
-
+  virtual void write_mcch(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2, LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT *sib13, LIBLTE_RRC_MCCH_MSG_STRUCT *mcch) = 0;
 };
 
 class mac_interface_rlc 
@@ -164,6 +179,7 @@ public:
   virtual void rem_user(uint16_t rnti) = 0; 
   virtual void add_bearer(uint16_t rnti, uint32_t lcid) = 0;
   virtual void add_bearer(uint16_t rnti, uint32_t lcid, srslte::srslte_rlc_config_t cnfg) = 0;
+  virtual void add_bearer_mrb(uint16_t rnti, uint32_t lcid) = 0;
 };
 
 // PDCP interface for GTPU

lib/include/srslte/interfaces/sched_interface.h

@@ -129,6 +129,22 @@ public:
     uint32_t nbytes;
   } dl_sched_pdu_t; 
   
+  
+  typedef struct {
+      uint32_t lcid;
+      uint32_t lcid_buffer_size;
+      uint32_t stop;
+      uint8_t *mtch_payload;
+  } dl_mtch_sched_t;
+  
+  typedef struct {
+    dl_sched_pdu_t pdu[20];
+    dl_mtch_sched_t mtch_sched[8];
+    uint32_t num_mtch_sched;
+    uint8_t *mcch_payload;
+    uint32_t current_sf_allocation_num;
+  } dl_pdu_mch_t; 
+ 
   typedef struct {
     uint32_t              rnti; 
     srslte_dci_format_t   dci_format;

lib/include/srslte/interfaces/ue_interfaces.h

@@ -101,11 +101,19 @@ public:
   virtual srslte::error_t setup_if_addr(uint32_t ip_addr, char *err_str) = 0;
 };
 
+// GW interface for RRC
+class gw_interface_rrc
+{
+public:
+  virtual void add_mch_port(uint32_t lcid, uint32_t port) = 0;
+};
+
 // GW interface for PDCP
 class gw_interface_pdcp
 {
 public:
   virtual void write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
+  virtual void write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
 };
 
 // NAS interface for RRC
@@ -198,6 +206,7 @@ public:
   virtual void write_pdu_bcch_bch(srslte::byte_buffer_t *pdu) = 0;
   virtual void write_pdu_bcch_dlsch(srslte::byte_buffer_t *pdu) = 0;
   virtual void write_pdu_pcch(srslte::byte_buffer_t *pdu) = 0;
+  virtual void write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
   virtual std::string get_rb_name(uint32_t lcid) = 0;
 };
 
@@ -247,6 +256,7 @@ public:
   virtual void write_pdu_bcch_bch(srslte::byte_buffer_t *sdu) = 0;
   virtual void write_pdu_bcch_dlsch(srslte::byte_buffer_t *sdu) = 0;
   virtual void write_pdu_pcch(srslte::byte_buffer_t *sdu) = 0;
+  virtual void write_pdu_mch(uint32_t lcid, srslte::byte_buffer_t *sdu) = 0;
 };
 
 // RLC interface for RRC
@@ -257,6 +267,7 @@ public:
   virtual void reestablish() = 0;
   virtual void add_bearer(uint32_t lcid) = 0;
   virtual void add_bearer(uint32_t lcid, srslte::srslte_rlc_config_t cnfg) = 0;
+  virtual void add_bearer_mrb(uint32_t lcid) = 0;
 };
 
 // RLC interface for PDCP
@@ -291,6 +302,7 @@ public:
   virtual void write_pdu_bcch_bch(uint8_t *payload, uint32_t nof_bytes) = 0;
   virtual void write_pdu_bcch_dlsch(uint8_t *payload, uint32_t nof_bytes) = 0;
   virtual void write_pdu_pcch(uint8_t *payload, uint32_t nof_bytes) = 0;
+  virtual void write_pdu_mch(uint32_t lcid, uint8_t *payload, uint32_t nof_bytes) = 0;
 };
 
 
@@ -327,6 +339,11 @@ public:
 class mac_interface_phy
 {
 public:
+  typedef struct {
+      uint32_t nof_mbsfn_services;
+  } mac_phy_cfg_mbsfn_t; 
+    
+    
     
   typedef struct {
     uint32_t    pid;    
@@ -379,21 +396,35 @@ public:
   /* Indicate reception of UL grant + HARQ information throught PHICH in the same TTI. */
   virtual void new_grant_ul_ack(mac_grant_t grant, bool ack, tb_action_ul_t *action) = 0;
 
+  /* Obtain action for a new MCH subframe. */
+  virtual void new_mch_dl(srslte_ra_dl_grant_t phy_grant, tb_action_dl_t *action) = 0;
+
   /* Indicate reception of HARQ information only through PHICH.   */
   virtual void harq_recv(uint32_t tti, bool ack, tb_action_ul_t *action) = 0;
   
   /* Indicate reception of DL grant. */ 
   virtual void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action) = 0;
   
-  /* Indicate successfull decoding of PDSCH TB. */
+  /* Indicate successful decoding of PDSCH TB. */
   virtual void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid) = 0;
   
-  /* Indicate successfull decoding of BCH TB through PBCH */
+  /* Indicate successful decoding of BCH TB through PBCH */
   virtual void bch_decoded_ok(uint8_t *payload, uint32_t len) = 0;  
   
-  /* Indicate successfull decoding of PCH TB through PDSCH */
+  /* Indicate successful decoding of PCH TB through PDSCH */
   virtual void pch_decoded_ok(uint32_t len) = 0;  
 
+  /* Indicate successful decoding of MCH TB through PMCH */
+  virtual void mch_decoded_ok(uint32_t len) = 0;
+  
+  /* Communicate the number of mbsfn services available  */
+  virtual void set_mbsfn_config(uint32_t nof_mbsfn_services) = 0;
+  
+  /* Function called every start of a subframe (TTI). Warning, this function is called 
+   * from a high priority thread and should terminate asap 
+   */
+
+
 };
 
 /* Interface RRC -> MAC shared between different RATs */
@@ -439,6 +470,9 @@ public:
   /* RRC configures a logical channel */
   virtual void    setup_lcid(uint32_t lcid, uint32_t lcg, uint32_t priority, int PBR_x_tti, uint32_t BSD) = 0;
 
+  /* Instructs the MAC to start receiving an MCH */
+  virtual void    mch_start_rx(uint32_t lcid) = 0;
+
   virtual uint32_t get_current_tti() = 0;
 
   virtual void set_config(mac_cfg_t *mac_cfg) = 0;
@@ -547,6 +581,9 @@ public:
   virtual void pdcch_dl_search(srslte_rnti_type_t rnti_type, uint16_t rnti, int tti_start = -1, int tti_end = -1) = 0;
   virtual void pdcch_ul_search_reset() = 0;
   virtual void pdcch_dl_search_reset() = 0;
+  
+  virtual void set_mch_period_stop(uint32_t stop) = 0;
+  
 };
 
 class phy_interface_rrc
@@ -564,10 +601,19 @@ public:
     LIBLTE_RRC_TDD_CONFIG_STRUCT                tdd_cnfg;
     LIBLTE_RRC_ANTENNA_PORTS_COUNT_ENUM         ant_info;      
   } phy_cfg_common_t; 
+
+  typedef struct {
+    LIBLTE_RRC_MBSFN_SUBFRAME_CONFIG_STRUCT     mbsfn_subfr_cnfg;
+    LIBLTE_RRC_MBSFN_NOTIFICATION_CONFIG_STRUCT mbsfn_notification_cnfg;
+    LIBLTE_RRC_MBSFN_AREA_INFO_STRUCT           mbsfn_area_info;
+    LIBLTE_RRC_MCCH_MSG_STRUCT                  mcch;
+  } phy_cfg_mbsfn_t;
+
   
   typedef struct {
     LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT dedicated;
     phy_cfg_common_t                            common; 
+    phy_cfg_mbsfn_t                             mbsfn;
     bool                                        enable_64qam; 
   } phy_cfg_t; 
 
@@ -581,6 +627,9 @@ public:
   virtual void set_config_common(phy_cfg_common_t *common) = 0; 
   virtual void set_config_tdd(LIBLTE_RRC_TDD_CONFIG_STRUCT *tdd) = 0; 
   virtual void set_config_64qam_en(bool enable) = 0;
+  virtual void set_config_mbsfn_sib2(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) = 0;
+  virtual void set_config_mbsfn_sib13(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT *sib13) = 0;
+  virtual void set_config_mbsfn_mcch(LIBLTE_RRC_MCCH_MSG_STRUCT *mcch) = 0;
 
   /* Measurements interface */
   virtual void meas_reset() = 0;

lib/src/common/mac_pcap.cc

@@ -94,6 +94,10 @@ void mac_pcap::write_dl_pch(uint8_t* pdu, uint32_t pdu_len_bytes, bool crc_ok, u
 {
   pack_and_write(pdu, pdu_len_bytes, 0, crc_ok, tti, SRSLTE_PRNTI, DIRECTION_DOWNLINK, P_RNTI);
 }
+void mac_pcap::write_dl_mch(uint8_t* pdu, uint32_t pdu_len_bytes, bool crc_ok, uint32_t tti)
+{
+  pack_and_write(pdu, pdu_len_bytes, 0, crc_ok, tti, SRSLTE_MRNTI, DIRECTION_DOWNLINK, M_RNTI);
+}
 void mac_pcap::write_dl_sirnti(uint8_t* pdu, uint32_t pdu_len_bytes, bool crc_ok, uint32_t tti)
 {
   pack_and_write(pdu, pdu_len_bytes, 0, crc_ok, tti, SRSLTE_SIRNTI, DIRECTION_DOWNLINK, SI_RNTI);

lib/src/common/pdu.cc

@@ -40,60 +40,15 @@ static uint32_t btable[64] = {
 
 
 namespace srslte {
-   
-    
+
 void sch_pdu::fprint(FILE* stream)
 {
   fprintf(stream, "MAC SDU for UL/DL-SCH. ");
   pdu::fprint(stream);
 }
 
-void sch_subh::fprint(FILE* stream)
-{
-  if (is_sdu()) {
-    fprintf(stream, "SDU LCHID=%d, SDU nof_bytes=%d\n", lcid, nof_bytes);
-  } else {
-    if (parent->is_ul()) {
-      switch(lcid) {
-        case CRNTI:
-          fprintf(stream, "C-RNTI CE\n");
-          break;
-        case PHR_REPORT:
-          fprintf(stream, "PHR\n");
-          break;
-        case TRUNC_BSR:
-          fprintf(stream, "Truncated BSR CE\n");
-          break;
-        case SHORT_BSR:
-          fprintf(stream, "Short BSR CE\n");
-          break;
-        case LONG_BSR:
-          fprintf(stream, "Long BSR CE\n");
-          break;
-        case PADDING:
-          fprintf(stream, "PADDING\n");
-      }
-    } else {
-      switch(lcid) {
-        case CON_RES_ID:
-          fprintf(stream, "Contention Resolution ID CE: 0x%lx\n", get_con_res_id());
-          break;
-        case TA_CMD:
-          fprintf(stream, "Time Advance Command CE: %d\n", get_ta_cmd());
-          break;
-        case DRX_CMD:
-          fprintf(stream, "DRX Command CE: Not implemented\n");
-          break;
-        case PADDING:
-          fprintf(stream, "PADDING\n");
-      }      
-    }
-  }
-}
-
 void sch_pdu::parse_packet(uint8_t *ptr)
 {
-
   pdu::parse_packet(ptr);
 
   // Correct size for last SDU 
@@ -173,12 +128,12 @@ uint8_t* sch_pdu::write_packet(srslte::log *log_h)
     header_sz += onetwo_padding;
   }
   if (ce_payload_sz + header_sz >= sdu_offset_start) {
-    fprintf(stderr, "Writting PDU: header sz + ce_payload_sz >= sdu_offset_start (%d>=%d). pdu_len=%d, total_sdu_len=%d\n", 
+    fprintf(stderr, "Writing PDU: header sz + ce_payload_sz >= sdu_offset_start (%d>=%d). pdu_len=%d, total_sdu_len=%d\n",
             header_sz + ce_payload_sz, sdu_offset_start, pdu_len, total_sdu_len);
     return NULL; 
   }
 
-  /* Start writting header and CE payload before the start of the SDU payload */
+  /* Start writing header and CE payload before the start of the SDU payload */
   uint8_t *ptr = &buffer_tx[sdu_offset_start-header_sz-ce_payload_sz];
   uint8_t *pdu_start_ptr = ptr; 
   
@@ -277,19 +232,22 @@ uint32_t sch_pdu::size_header_sdu(uint32_t nbytes)
     return 3; 
   }
 }
-bool sch_pdu::has_space_ce(uint32_t nbytes)
+
+bool sch_pdu::has_space_ce(uint32_t nbytes, bool var_len)
 {
-  if (rem_len >= nbytes + 1) {
+  uint32_t head_len = var_len ? size_header_sdu(nbytes) : 1;
+  if (rem_len >= nbytes + head_len) {
     return true; 
   } else {
     return false; 
   }
 }
 
-bool sch_pdu::update_space_ce(uint32_t nbytes)
+bool sch_pdu::update_space_ce(uint32_t nbytes, bool var_len)
 {
+  uint32_t head_len = var_len ? size_header_sdu(nbytes) : 1;
   if (has_space_ce(nbytes)) {
-    rem_len      -= nbytes + 1; 
+    rem_len      -= nbytes + head_len;
     return true; 
   } else {
     return false; 
@@ -336,18 +294,22 @@ int sch_pdu::get_sdu_space()
 
 void sch_subh::init()
 {
-  lcid            = 0; 
-  nof_bytes       = 0; 
-  payload         = NULL;
+  lcid             = 0;
+  nof_bytes        = 0;
+  payload          = NULL;
+  nof_mch_sched_ce = 0;
+  cur_mch_sched_ce = 0;
 }
 
 sch_subh::cetype sch_subh::ce_type()
 {
-  if (lcid >= PHR_REPORT) {
-    return (cetype) lcid;
-  } else {
-    return SDU;
+  if (lcid >= PHR_REPORT && type == SCH_SUBH_TYPE) {
+    return (cetype)lcid;
+  }
+  if(lcid >= MCH_SCHED_INFO && type == MCH_SUBH_TYPE) {
+    return (cetype)lcid;
   }
+  return (cetype)SDU;
 }
 
 void sch_subh::set_payload_size(uint32_t size) {
@@ -355,48 +317,65 @@ void sch_subh::set_payload_size(uint32_t size) {
 }
 
 uint32_t sch_subh::size_plus_header() {
-  if (is_sdu()) {
+  if (is_sdu() || is_var_len_ce()) {
     return sch_pdu::size_header_sdu(nof_bytes) + nof_bytes; 
-  } else {
-    return nof_bytes + 1;
   }
+  // All others are 1-byte headers
+  return 1 + nof_bytes;
 }
 
 uint32_t sch_subh::sizeof_ce(uint32_t lcid, bool is_ul)
 {
-  if (is_ul) {
-    switch(lcid) {
-      case PHR_REPORT: 
-        return 1; 
-      case CRNTI: 
-        return 2;
-      case TRUNC_BSR: 
-        return 1;
-      case SHORT_BSR: 
-        return 1; 
-      case LONG_BSR: 
-        return 3; 
-      case PADDING: 
-        return 0; 
-    }      
-  } else {
-    switch(lcid) {
-      case CON_RES_ID: 
-        return 6;
-      case TA_CMD: 
-        return 1; 
-      case DRX_CMD:
-        return 0; 
-      case PADDING: 
-        return 0; 
-    }  
+  if (type == SCH_SUBH_TYPE) {
+    if (is_ul) {
+      switch(lcid) {
+        case PHR_REPORT:
+          return 1;
+        case CRNTI:
+          return 2;
+        case TRUNC_BSR:
+          return 1;
+        case SHORT_BSR:
+          return 1;
+        case LONG_BSR:
+          return 3;
+        case PADDING:
+          return 0;
+      }
+    } else {
+      switch(lcid) {
+        case CON_RES_ID:
+          return 6;
+        case TA_CMD:
+          return 1;
+        case DRX_CMD:
+          return 0;
+        case PADDING:
+          return 0;
+      }
+    }
+  }
+  if (type == MCH_SUBH_TYPE) {
+    switch (lcid) {
+    case MCH_SCHED_INFO:
+      return nof_mch_sched_ce*2;
+    case PADDING:
+      return 0;
+    }
   }
   return 0;
 }
+
 bool sch_subh::is_sdu()
 {
   return ce_type() == SDU;
 }
+
+bool sch_subh::is_var_len_ce()
+{
+  return (MCH_SCHED_INFO == ce_type()) && (MCH_SUBH_TYPE == type);
+}
+
 uint16_t sch_subh::get_c_rnti()
 {
   if (payload) {
@@ -453,6 +432,21 @@ int sch_subh::get_bsr(uint32_t buff_size[4])
   }
 }
 
+bool sch_subh::get_next_mch_sched_info(uint8_t *lcid_, uint16_t *mtch_stop)
+{
+  if(payload) {
+    nof_mch_sched_ce = nof_bytes/2;
+    if(cur_mch_sched_ce < nof_mch_sched_ce) {
+      *lcid_ = (payload[cur_mch_sched_ce*2]&0xF8) >> 3;
+      *mtch_stop = ((uint16_t)(payload[cur_mch_sched_ce*2]&0x07)) << 8;
+      *mtch_stop += payload[cur_mch_sched_ce*2+1];
+      cur_mch_sched_ce++;
+      return true;
+    }
+  }
+  return false;
+}
+
 uint8_t sch_subh::get_ta_cmd()
 {
   if (payload) {
@@ -461,42 +455,49 @@ uint8_t sch_subh::get_ta_cmd()
     return 0;
   }
 }
+
 uint32_t sch_subh::get_sdu_lcid()
 {
   return lcid;
 }
-int sch_subh::get_payload_size()
+
+
+uint32_t sch_subh::get_payload_size()
+
 {
   return nof_bytes;
 }
+
 uint32_t sch_subh::get_header_size(bool is_last) {
   if (!is_last) {
-    // For all subheaders, size can be 1, 2 or 3 bytes
     if (is_sdu()) {
-      return sch_pdu::size_header_sdu(get_payload_size());
-    } else {
-      return 1; 
+      return sch_pdu::size_header_sdu(nof_bytes);
     }
+    if (lcid == MCH_SCHED_INFO && type == MCH_SUBH_TYPE) {
+      return sch_pdu::size_header_sdu(nof_bytes);
+    }
+    return 1; // All others are 1-byte
   } else {
-    // Last subheader (CE or SDU) has always 1 byte header      
-    return 1; 
+    return 1; // Last subheader (CE or SDU) has always 1 byte header
   }
 }
+
 uint8_t* sch_subh::get_sdu_ptr()
 {
   return payload;
 }
+
 void sch_subh::set_padding(uint32_t padding_len)
 {
   lcid = PADDING;
   nof_bytes = padding_len; 
 }
+
 void sch_subh::set_padding()
 {
   set_padding(0);
 }
 
-
 bool sch_subh::set_bsr(uint32_t buff_size[4], sch_subh::cetype format)
 {
   uint32_t nonzero_lcg=0;
@@ -579,6 +580,20 @@ bool sch_subh::set_ta_cmd(uint8_t ta_cmd)
   }
 }
 
+bool sch_subh::set_next_mch_sched_info(uint8_t lcid_, uint16_t mtch_stop)
+{
+  if (((sch_pdu*)parent)->has_space_ce(2, true)) {
+    w_payload_ce[nof_mch_sched_ce*2] = (lcid_&0x1F) << 3 | (uint8_t) (mtch_stop&0x0700)>>8 ;
+    w_payload_ce[nof_mch_sched_ce*2+1] = (uint8_t) (mtch_stop&0xff);
+    nof_mch_sched_ce++;
+    lcid = MCH_SCHED_INFO;
+    ((sch_pdu*)parent)->update_space_ce(2, true);
+    nof_bytes += 2;
+    return true;
+  }
+  return false;
+}
+
 int sch_subh::set_sdu(uint32_t lcid_, uint32_t requested_bytes, read_pdu_interface *sdu_itf)
 {
   if (((sch_pdu*)parent)->has_space_sdu(requested_bytes)) {
@@ -634,7 +649,7 @@ void sch_subh::write_subheader(uint8_t** ptr, bool is_last)
 {
   *(*ptr)   = (uint8_t) (is_last?0:(1<<5)) | ((uint8_t) lcid & 0x1f);
   *ptr += 1;
-  if (is_sdu()) {
+  if (is_sdu() || is_var_len_ce()) {
     // MAC SDU: R/R/E/LCID/F/L subheader
     // 2nd and 3rd octet
     if (!is_last) {
@@ -656,8 +671,8 @@ void sch_subh::write_payload(uint8_t** ptr)
   if (is_sdu()) {
     // SDU is written directly during subheader creation
   } else {
-    nof_bytes = sizeof_ce(lcid, parent->is_ul()); 
-    memcpy(*ptr, w_payload_ce, nof_bytes*sizeof(uint8_t));    
+    nof_bytes = sizeof_ce(lcid, parent->is_ul());
+    memcpy(*ptr, w_payload_ce, nof_bytes*sizeof(uint8_t));
   }
   *ptr += nof_bytes;
 }
@@ -668,7 +683,7 @@ bool sch_subh::read_subheader(uint8_t** ptr)
   bool e_bit    = (bool)    (*(*ptr) & 0x20)?true:false;
   lcid          = (uint8_t) *(*ptr) & 0x1f;
   *ptr += 1;
-  if (is_sdu()) {
+  if (is_sdu() || is_var_len_ce()) {
     if (e_bit) {
       F_bit     = (bool)    (*(*ptr) & 0x80)?true:false;
       nof_bytes = (uint32_t)*(*ptr) & 0x7f;
@@ -686,12 +701,64 @@ bool sch_subh::read_subheader(uint8_t** ptr)
   }
   return e_bit;
 }
+
 void sch_subh::read_payload(uint8_t** ptr)
 {
   payload = *ptr; 
   *ptr += nof_bytes;
 }
 
+void sch_subh::fprint(FILE* stream)
+{
+  if (is_sdu()) {
+    fprintf(stream, "SDU LCHID=%d, SDU nof_bytes=%d\n", lcid, nof_bytes);
+  } else if (type == SCH_SUBH_TYPE) {
+    if (parent->is_ul()) {
+      switch(lcid) {
+        case CRNTI:
+          fprintf(stream, "C-RNTI CE\n");
+          break;
+        case PHR_REPORT:
+          fprintf(stream, "PHR\n");
+          break;
+        case TRUNC_BSR:
+          fprintf(stream, "Truncated BSR CE\n");
+          break;
+        case SHORT_BSR:
+          fprintf(stream, "Short BSR CE\n");
+          break;
+        case LONG_BSR:
+          fprintf(stream, "Long BSR CE\n");
+          break;
+        case PADDING:
+          fprintf(stream, "PADDING\n");
+      }
+    } else {
+      switch(lcid) {
+        case CON_RES_ID:
+          fprintf(stream, "Contention Resolution ID CE: 0x%lx\n", get_con_res_id());
+          break;
+        case TA_CMD:
+          fprintf(stream, "Time Advance Command CE: %d\n", get_ta_cmd());
+          break;
+        case DRX_CMD:
+          fprintf(stream, "DRX Command CE: Not implemented\n");
+          break;
+        case PADDING:
+          fprintf(stream, "PADDING\n");
+      }
+    }
+  } else if (type == MCH_SUBH_TYPE) {
+    switch(lcid) {
+      case MCH_SCHED_INFO:
+        fprintf(stream, "MCH Scheduling Info CE\n");
+        break;
+      case PADDING:
+        fprintf(stream, "PADDING\n");
+    }
+  }
+}
+
 uint8_t sch_subh::buff_size_table(uint32_t buffer_size) {
   if (buffer_size == 0) {
     return 0; 
@@ -734,26 +801,22 @@ void rar_pdu::fprint(FILE* stream)
   pdu::fprint(stream);  
 } 
 
-
-void rar_subh::fprint(FILE* stream)
-{
-  fprintf(stream, "RAPID: %d, Temp C-RNTI: %d, TA: %d, UL Grant: ", preamble, temp_rnti, ta);
-  srslte_vec_fprint_hex(stream, grant, 20);
-}
-
 rar_pdu::rar_pdu(uint32_t max_rars_) : pdu(max_rars_)
 {
   backoff_indicator = 0; 
   has_backoff_indicator = false; 
 }
+
 uint8_t rar_pdu::get_backoff()
 {
   return backoff_indicator;
 }
+
 bool rar_pdu::has_backoff()
 {
   return has_backoff_indicator;
 }
+
 void rar_pdu::set_backoff(uint8_t bi)
 {
   has_backoff_indicator = true; 
@@ -786,7 +849,11 @@ bool rar_pdu::write_packet(uint8_t* ptr)
   return true; 
 }
 
-
+void rar_subh::fprint(FILE* stream)
+{
+  fprintf(stream, "RAPID: %d, Temp C-RNTI: %d, TA: %d, UL Grant: ", preamble, temp_rnti, ta);
+  srslte_vec_fprint_hex(stream, grant, 20);
+}
 
 void rar_subh::init()
 {
@@ -873,150 +940,3 @@ bool rar_subh::read_subheader(uint8_t** ptr)
 }
     
 }
-
-
-
-//int main()
-//{
-//  /* Test 1st message: CCCH + Short BSR + PHR */
-//  uint8_t buffer[10240];
-//  uint8_t ccch_payload[6] = {0x10, 0x20, 0x30, 0x40, 0x50, 0x60};
-//  uint32_t bsr_st[4] = {1, 2, 3, 4};
-//  srsue::sch_pdu pdu(10);
-//  uint8_t *ptr;
-  
-//  printf("------- CCCH + Short BSR + PHR no padding ----------\n");
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 11, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(0,6,ccch_payload);
-//  pdu.new_subh();
-//  pdu.get()->set_phr(10);
-//  pdu.new_subh();
-//  pdu.get()->set_bsr(bsr_st, srsue::sch_subh::SHORT_BSR);
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-  
-//  /* Test single SDU: SDU 15 + 1 byte header */
-//  printf("------- Single SDU no padding ----------\n");
-//  uint8_t dlsch_payload[15] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 16, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(1, 15, dlsch_payload);
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-  
-//  /* Test multiple SDU + multiword padding: SDU 8 + SDU 2 byte*/
-//  printf("------- Multiple SDU + multiword padding ----------\n");
-//  uint8_t dlsch_payload1[8] = {1,2,3,4,5,6,7,8};
-//  uint8_t dlsch_payload2[2] = {0xA, 0xB};
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 18, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(2, 8, dlsch_payload1);
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(3, 2, dlsch_payload2);
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-  
-//  printf("------- Multiple SDU + 2word padding ----------\n");
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 15, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(2, 8, dlsch_payload1);
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(3, 2, dlsch_payload2);
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-  
-//  printf("------- Multiple SDU + 1word padding ----------\n");
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 14, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(2, 8, dlsch_payload1);
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(3, 2, dlsch_payload2);
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-
-//  printf("------- Multiple SDU + 0word padding ----------\n");
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 13, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(2, 8, dlsch_payload1);
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(3, 2, dlsch_payload2);
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-
-//  printf("------- Multiple SDU + no space ----------\n");
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 12, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(2, 8, dlsch_payload1);
-//  pdu.new_subh();
-//  if (pdu.get()->set_sdu(3, 2, dlsch_payload2) < 0) {
-//    pdu.del_subh();
-//  }
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-
-//  /* CE only */
-//  printf("------- CE only ----------\n");
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 125, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_phr(15);
-//  pdu.new_subh();
-//  pdu.get()->set_bsr(bsr_st, srsue::sch_subh::SHORT_BSR);
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-  
-//  /* Another test */
-//  printf("------- Another test ----------\n");
-//  uint8_t dlsch_payload3[602];
-//  bzero(buffer, 10240);
-//  pdu.init_tx(buffer, 75, true);
-//  printf("Available space: %d\n", pdu.rem_size());
-//  pdu.new_subh();
-//  pdu.get()->set_bsr(bsr_st, srsue::sch_subh::SHORT_BSR);
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(3, 2, dlsch_payload3);
-//  pdu.new_subh();
-//  pdu.get()->set_sdu(3, 66, dlsch_payload3);
-//  pdu.new_subh();
-//  printf("Remaining space: %d\n", pdu.rem_size());
-//  ptr = pdu.write_packet();
-//  //srslte_vec_fprint_byte(stdout, ptr, pdu.get_pdu_len());
-//  printf("\n");
-  
-//  return 0;
-//}
-
-
-