mirror of https://github.com/PentHertz/srsLTE.git
948 lines
24 KiB
C++
948 lines
24 KiB
C++
/**
|
|
*
|
|
* \section COPYRIGHT
|
|
*
|
|
* Copyright 2013-2015 Software Radio Systems Limited
|
|
*
|
|
* \section LICENSE
|
|
*
|
|
* This file is part of the srsUE library.
|
|
*
|
|
* srsUE is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU Affero General Public License as
|
|
* published by the Free Software Foundation, either version 3 of
|
|
* the License, or (at your option) any later version.
|
|
*
|
|
* srsUE is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU Affero General Public License for more details.
|
|
*
|
|
* A copy of the GNU Affero General Public License can be found in
|
|
* the LICENSE file in the top-level directory of this distribution
|
|
* and at http://www.gnu.org/licenses/.
|
|
*
|
|
*/
|
|
|
|
#include <strings.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "srslte/common/pdu.h"
|
|
#include "srslte/srslte.h"
|
|
|
|
// Table 6.1.3.1-1 Buffer size levels for BSR
|
|
static uint32_t btable[64] = {
|
|
0, 1, 10, 12, 14, 17, 19, 22, 26, 31, 36, 42, 49, 57, 67, 78, 91, 107, 125, 146, 171, 200, 234, 274, 321, 376, 440, 515, 603, 706, 826, 967, 1132,
|
|
1326, 1552, 1817, 2127, 2490, 2915, 3413, 3995, 4667, 5476, 6411, 7505, 8787, 10287, 12043, 14099, 16507, 19325, 22624, 26487, 31009, 36304,
|
|
42502, 49759, 58255, 68201, 79846, 93479, 109439, 128125, 150000};
|
|
|
|
|
|
|
|
namespace srslte {
|
|
|
|
void sch_pdu::fprint(FILE* stream)
|
|
{
|
|
fprintf(stream, "MAC SDU for UL/DL-SCH. ");
|
|
pdu::fprint(stream);
|
|
}
|
|
|
|
void sch_pdu::parse_packet(uint8_t *ptr)
|
|
{
|
|
pdu::parse_packet(ptr);
|
|
|
|
// Correct size for last SDU
|
|
if (nof_subheaders > 0) {
|
|
uint32_t read_len = 0;
|
|
for (int i=0;i<nof_subheaders-1;i++) {
|
|
read_len += subheaders[i].size_plus_header();
|
|
}
|
|
|
|
int n_sub = pdu_len-read_len-1;
|
|
|
|
if (n_sub >= 0) {
|
|
subheaders[nof_subheaders-1].set_payload_size(n_sub);
|
|
} else {
|
|
fprintf(stderr,"Reading MAC PDU: negative payload for last subheader\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
uint8_t* sch_pdu::write_packet() {
|
|
return write_packet(NULL);
|
|
}
|
|
|
|
/* Writes the MAC PDU in the packet, including the MAC headers and CE payload. Section 6.1.2 */
|
|
uint8_t* sch_pdu::write_packet(srslte::log *log_h)
|
|
{
|
|
int init_rem_len=rem_len;
|
|
sch_subh padding;
|
|
padding.set_padding();
|
|
|
|
if (nof_subheaders <= 0 && nof_subheaders < (int)max_subheaders) {
|
|
log_h->error("Trying to write packet with invalid number of subheaders (nof_subheaders=%d).\n", nof_subheaders);
|
|
return NULL;
|
|
}
|
|
|
|
if (init_rem_len < 0) {
|
|
log_h->error("init_rem_len=%d\n", init_rem_len);
|
|
return NULL;
|
|
}
|
|
|
|
/* If last SDU has zero payload, remove it. FIXME: Why happens this?? */
|
|
if (subheaders[nof_subheaders-1].get_payload_size() == 0) {
|
|
del_subh();
|
|
}
|
|
|
|
/* Determine if we are transmitting CEs only. */
|
|
bool ce_only = last_sdu_idx<0?true:false;
|
|
|
|
/* Determine if we will need multi-byte padding or 1/2 bytes padding */
|
|
bool multibyte_padding = false;
|
|
uint32_t onetwo_padding = 0;
|
|
if (rem_len > 2) {
|
|
multibyte_padding = true;
|
|
// Add 1 header for padding
|
|
rem_len--;
|
|
// Add the header for the last SDU
|
|
if (!ce_only) {
|
|
rem_len -= (subheaders[last_sdu_idx].get_header_size(false)-1); // Becuase we were assuming it was the one
|
|
}
|
|
} else if (rem_len > 0) {
|
|
onetwo_padding = rem_len;
|
|
rem_len = 0;
|
|
}
|
|
|
|
|
|
/* Determine the header size and CE payload size */
|
|
uint32_t header_sz = 0;
|
|
uint32_t ce_payload_sz = 0;
|
|
for (int i=0;i<nof_subheaders;i++) {
|
|
header_sz += subheaders[i].get_header_size(!multibyte_padding && i==last_sdu_idx);
|
|
if (!subheaders[i].is_sdu()) {
|
|
ce_payload_sz += subheaders[i].get_payload_size();
|
|
}
|
|
}
|
|
if (multibyte_padding) {
|
|
header_sz += 1;
|
|
} else if (onetwo_padding) {
|
|
header_sz += onetwo_padding;
|
|
}
|
|
if (ce_payload_sz + header_sz >= sdu_offset_start) {
|
|
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 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;
|
|
|
|
// Add single/two byte padding first
|
|
for (uint32_t i=0;i<onetwo_padding;i++) {
|
|
padding.write_subheader(&ptr, false);
|
|
}
|
|
// Then write subheaders for MAC CE
|
|
for (int i=0;i<nof_subheaders;i++) {
|
|
if (!subheaders[i].is_sdu()) {
|
|
subheaders[i].write_subheader(&ptr, ce_only && !multibyte_padding && i==(nof_subheaders-1));
|
|
}
|
|
}
|
|
// Then for SDUs
|
|
if (!ce_only) {
|
|
for (int i=0;i<nof_subheaders;i++) {
|
|
if (subheaders[i].is_sdu()) {
|
|
subheaders[i].write_subheader(&ptr, !multibyte_padding && i==last_sdu_idx);
|
|
}
|
|
}
|
|
}
|
|
// and finally add multi-byte padding
|
|
if (multibyte_padding) {
|
|
sch_subh padding_multi;
|
|
padding_multi.set_padding(rem_len);
|
|
padding_multi.write_subheader(&ptr, true);
|
|
}
|
|
|
|
// Write CE payloads (SDU payloads already in the buffer)
|
|
for (int i=0;i<nof_subheaders;i++) {
|
|
if (!subheaders[i].is_sdu()) {
|
|
subheaders[i].write_payload(&ptr);
|
|
}
|
|
}
|
|
// Set padding to zeros (if any)
|
|
if (rem_len > 0) {
|
|
bzero(&pdu_start_ptr[pdu_len-rem_len], rem_len*sizeof(uint8_t));
|
|
}
|
|
|
|
/* Sanity check and print if error */
|
|
if (log_h) {
|
|
log_h->debug("Wrote PDU: pdu_len=%d, header_and_ce=%d (%d+%d), nof_subh=%d, last_sdu=%d, sdu_len=%d, onepad=%d, multi=%d\n",
|
|
pdu_len, header_sz+ce_payload_sz, header_sz, ce_payload_sz,
|
|
nof_subheaders, last_sdu_idx, total_sdu_len, onetwo_padding, rem_len);
|
|
} else {
|
|
printf("Wrote PDU: pdu_len=%d, header_and_ce=%d (%d+%d), nof_subh=%d, last_sdu=%d, sdu_len=%d, onepad=%d, multi=%d, init_rem_len=%d\n",
|
|
pdu_len, header_sz+ce_payload_sz, header_sz, ce_payload_sz,
|
|
nof_subheaders, last_sdu_idx, total_sdu_len, onetwo_padding, rem_len, init_rem_len);
|
|
}
|
|
|
|
if (rem_len + header_sz + ce_payload_sz + total_sdu_len != pdu_len) {
|
|
printf("\n------------------------------\n");
|
|
for (int i=0;i<nof_subheaders;i++) {
|
|
printf("SUBH %d is_sdu=%d, payload=%d\n", i, subheaders[i].is_sdu(), subheaders[i].get_payload_size());
|
|
}
|
|
printf("Wrote PDU: pdu_len=%d, header_and_ce=%d (%d+%d), nof_subh=%d, last_sdu=%d, sdu_len=%d, onepad=%d, multi=%d, init_rem_len=%d\n",
|
|
pdu_len, header_sz+ce_payload_sz, header_sz, ce_payload_sz,
|
|
nof_subheaders, last_sdu_idx, total_sdu_len, onetwo_padding, rem_len, init_rem_len);
|
|
fprintf(stderr, "Expected PDU len %d bytes but wrote %d\n", pdu_len, rem_len + header_sz + ce_payload_sz + total_sdu_len);
|
|
printf("------------------------------\n");
|
|
|
|
if (log_h) {
|
|
log_h->error("Wrote PDU: pdu_len=%d, header_and_ce=%d (%d+%d), nof_subh=%d, last_sdu=%d, sdu_len=%d, onepad=%d, multi=%d, init_rem_len=%d\n",
|
|
pdu_len, header_sz+ce_payload_sz, header_sz, ce_payload_sz,
|
|
nof_subheaders, last_sdu_idx, total_sdu_len, onetwo_padding, rem_len, init_rem_len);
|
|
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
if ((int)(header_sz + ce_payload_sz) != (int) (ptr - pdu_start_ptr)) {
|
|
fprintf(stderr, "Expected a header and CE payload of %d bytes but wrote %d\n",
|
|
header_sz+ce_payload_sz,(int) (ptr - pdu_start_ptr));
|
|
return NULL;
|
|
}
|
|
|
|
return pdu_start_ptr;
|
|
}
|
|
|
|
int sch_pdu::rem_size() {
|
|
return rem_len;
|
|
}
|
|
|
|
int sch_pdu::get_pdu_len()
|
|
{
|
|
return pdu_len;
|
|
}
|
|
|
|
uint32_t sch_pdu::size_header_sdu(uint32_t nbytes)
|
|
{
|
|
if (nbytes < 128) {
|
|
return 2;
|
|
} else {
|
|
return 3;
|
|
}
|
|
}
|
|
|
|
bool sch_pdu::has_space_ce(uint32_t nbytes, bool var_len)
|
|
{
|
|
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 var_len)
|
|
{
|
|
uint32_t head_len = var_len ? size_header_sdu(nbytes) : 1;
|
|
if (has_space_ce(nbytes)) {
|
|
rem_len -= nbytes + head_len;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool sch_pdu::has_space_sdu(uint32_t nbytes)
|
|
{
|
|
int s = get_sdu_space();
|
|
|
|
if (s < 0) {
|
|
return false;
|
|
} else {
|
|
return (uint32_t)s >= nbytes;
|
|
}
|
|
}
|
|
|
|
bool sch_pdu::update_space_sdu(uint32_t nbytes)
|
|
{
|
|
int init_rem = rem_len;
|
|
if (has_space_sdu(nbytes)) {
|
|
if (last_sdu_idx < 0) {
|
|
rem_len -= (nbytes+1);
|
|
} else {
|
|
rem_len -= (nbytes+1 + (size_header_sdu(subheaders[last_sdu_idx].get_payload_size())-1));
|
|
}
|
|
last_sdu_idx = cur_idx;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
int sch_pdu::get_sdu_space()
|
|
{
|
|
int ret;
|
|
if (last_sdu_idx < 0) {
|
|
ret = rem_len - 1;
|
|
} else {
|
|
ret = rem_len - (size_header_sdu(subheaders[last_sdu_idx].get_payload_size())-1) - 1;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void sch_subh::init()
|
|
{
|
|
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 && 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) {
|
|
nof_bytes = size;
|
|
}
|
|
|
|
uint32_t sch_subh::size_plus_header() {
|
|
if (is_sdu() || is_var_len_ce()) {
|
|
return sch_pdu::size_header_sdu(nof_bytes) + nof_bytes;
|
|
}
|
|
// All others are 1-byte headers
|
|
return 1 + nof_bytes;
|
|
}
|
|
|
|
uint32_t sch_subh::sizeof_ce(uint32_t lcid, bool is_ul)
|
|
{
|
|
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) {
|
|
return (uint16_t) payload[0]<<8 | payload[1];
|
|
} else {
|
|
return (uint16_t) w_payload_ce[0]<<8 | w_payload_ce[1];
|
|
}
|
|
}
|
|
|
|
uint64_t sch_subh::get_con_res_id()
|
|
{
|
|
if (payload) {
|
|
return ((uint64_t) payload[5]) | (((uint64_t) payload[4])<<8) | (((uint64_t) payload[3])<<16) | (((uint64_t) payload[2])<<24) |
|
|
(((uint64_t) payload[1])<<32) | (((uint64_t) payload[0])<<40);
|
|
} else {
|
|
return ((uint64_t) w_payload_ce[5]) | (((uint64_t) w_payload_ce[4])<<8) | (((uint64_t) w_payload_ce[3])<<16) | (((uint64_t) w_payload_ce[2])<<24) |
|
|
(((uint64_t) w_payload_ce[1])<<32) | (((uint64_t) w_payload_ce[0])<<40);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
float sch_subh::get_phr()
|
|
{
|
|
if (payload) {
|
|
return (float) (payload[0]&0x3f) - 23;
|
|
} else {
|
|
return (float) (w_payload_ce[0]&0x3f) - 23;
|
|
}
|
|
}
|
|
|
|
int sch_subh::get_bsr(uint32_t buff_size[4])
|
|
{
|
|
if (payload) {
|
|
uint32_t nonzero_lcg = 0;
|
|
if (ce_type()==LONG_BSR) {
|
|
buff_size[0] = (payload[0]&0xFC) >> 2;
|
|
buff_size[1] = (payload[0]&0x03) << 4 | (payload[1]&0xF0) >> 4;
|
|
buff_size[2] = (payload[1]&0x0F) << 4 | (payload[1]&0xC0) >> 6;
|
|
buff_size[3] = (payload[2]&0x3F);
|
|
} else {
|
|
nonzero_lcg = (payload[0]&0xc0) >> 6;
|
|
buff_size[nonzero_lcg%4] = payload[0]&0x3f;
|
|
}
|
|
for (int i=0;i<4;i++) {
|
|
if (buff_size[i]) {
|
|
if (buff_size[i]<63) {
|
|
buff_size[i] = btable[1+buff_size[i]];
|
|
} else {
|
|
buff_size[i] = btable[63];
|
|
}
|
|
}
|
|
}
|
|
return nonzero_lcg;
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
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) {
|
|
return (uint8_t) payload[0]&0x3f;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
uint32_t sch_subh::get_sdu_lcid()
|
|
{
|
|
return lcid;
|
|
}
|
|
|
|
|
|
uint32_t sch_subh::get_payload_size()
|
|
|
|
{
|
|
return nof_bytes;
|
|
}
|
|
|
|
uint32_t sch_subh::get_header_size(bool is_last) {
|
|
if (!is_last) {
|
|
if (is_sdu()) {
|
|
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 {
|
|
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;
|
|
for (int i=0;i<4;i++) {
|
|
if (buff_size[i]) {
|
|
nonzero_lcg=i;
|
|
}
|
|
}
|
|
uint32_t ce_size = format==LONG_BSR?3:1;
|
|
if (((sch_pdu*)parent)->has_space_ce(ce_size)) {
|
|
if (format==LONG_BSR) {
|
|
w_payload_ce[0] = (buff_size_table(buff_size[0])&0x3f) << 2 | (buff_size_table(buff_size[1])&0xc0)>>6;
|
|
w_payload_ce[1] = (buff_size_table(buff_size[1])&0xf) << 4 | (buff_size_table(buff_size[2])&0xf0)>>4;
|
|
w_payload_ce[2] = (buff_size_table(buff_size[2])&0x3) << 6 | (buff_size_table(buff_size[3])&0x3f);
|
|
} else {
|
|
w_payload_ce[0] = (nonzero_lcg&0x3)<<6 | (buff_size_table(buff_size[nonzero_lcg])&0x3f);
|
|
}
|
|
lcid = format;
|
|
((sch_pdu*)parent)->update_space_ce(ce_size);
|
|
nof_bytes = ce_size;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool sch_subh::set_c_rnti(uint16_t crnti)
|
|
{
|
|
if (((sch_pdu*)parent)->has_space_ce(2)) {
|
|
w_payload_ce[0] = (uint8_t) ((crnti&0xff00)>>8);
|
|
w_payload_ce[1] = (uint8_t) ((crnti&0x00ff));
|
|
lcid = CRNTI;
|
|
((sch_pdu*)parent)->update_space_ce(2);
|
|
nof_bytes = 2;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
bool sch_subh::set_con_res_id(uint64_t con_res_id)
|
|
{
|
|
if (((sch_pdu*)parent)->has_space_ce(6)) {
|
|
w_payload_ce[0] = (uint8_t) ((con_res_id&0xff0000000000)>>40);
|
|
w_payload_ce[1] = (uint8_t) ((con_res_id&0x00ff00000000)>>32);
|
|
w_payload_ce[2] = (uint8_t) ((con_res_id&0x0000ff000000)>>24);
|
|
w_payload_ce[3] = (uint8_t) ((con_res_id&0x000000ff0000)>>16);
|
|
w_payload_ce[4] = (uint8_t) ((con_res_id&0x00000000ff00)>>8);
|
|
w_payload_ce[5] = (uint8_t) ((con_res_id&0x0000000000ff));
|
|
lcid = CON_RES_ID;
|
|
((sch_pdu*)parent)->update_space_ce(6);
|
|
nof_bytes = 6;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
bool sch_subh::set_phr(float phr)
|
|
{
|
|
if (((sch_pdu*)parent)->has_space_ce(1)) {
|
|
w_payload_ce[0] = phr_report_table(phr)&0x3f;
|
|
lcid = PHR_REPORT;
|
|
((sch_pdu*)parent)->update_space_ce(1);
|
|
nof_bytes = 1;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool sch_subh::set_ta_cmd(uint8_t ta_cmd)
|
|
{
|
|
if (((sch_pdu*)parent)->has_space_ce(1)) {
|
|
w_payload_ce[0] = ta_cmd&0x3f;
|
|
lcid = TA_CMD;
|
|
((sch_pdu*)parent)->update_space_ce(1);
|
|
nof_bytes = 1;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
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)) {
|
|
lcid = lcid_;
|
|
|
|
payload = ((sch_pdu*)parent)->get_current_sdu_ptr();
|
|
// Copy data and get final number of bytes written to the MAC PDU
|
|
int sdu_sz = sdu_itf->read_pdu(lcid, payload, requested_bytes);
|
|
|
|
if (sdu_sz < 0) {
|
|
return -1;
|
|
}
|
|
if (sdu_sz == 0) {
|
|
return 0;
|
|
}
|
|
else {
|
|
// Save final number of written bytes
|
|
nof_bytes = sdu_sz;
|
|
|
|
if(nof_bytes > (int32_t)requested_bytes) {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
((sch_pdu*)parent)->add_sdu(nof_bytes);
|
|
((sch_pdu*)parent)->update_space_sdu(nof_bytes);
|
|
return nof_bytes;
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
int sch_subh::set_sdu(uint32_t lcid_, uint32_t nof_bytes_, uint8_t *payload)
|
|
{
|
|
if (((sch_pdu*)parent)->has_space_sdu(nof_bytes_)) {
|
|
lcid = lcid_;
|
|
|
|
memcpy(((sch_pdu*)parent)->get_current_sdu_ptr(), payload, nof_bytes_);
|
|
|
|
((sch_pdu*)parent)->add_sdu(nof_bytes_);
|
|
((sch_pdu*)parent)->update_space_sdu(nof_bytes_);
|
|
nof_bytes = nof_bytes_;
|
|
|
|
return (int) nof_bytes;
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
|
|
// Section 6.2.1
|
|
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() || is_var_len_ce()) {
|
|
// MAC SDU: R/R/E/LCID/F/L subheader
|
|
// 2nd and 3rd octet
|
|
if (!is_last) {
|
|
if (nof_bytes >= 128) {
|
|
*(*ptr) = (uint8_t) 1<<7 | ((nof_bytes & 0x7f00) >> 8);
|
|
*ptr += 1;
|
|
*(*ptr) = (uint8_t) (nof_bytes & 0xff);
|
|
*ptr += 1;
|
|
} else {
|
|
*(*ptr) = (uint8_t) (nof_bytes & 0x7f);
|
|
*ptr += 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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));
|
|
}
|
|
*ptr += nof_bytes;
|
|
}
|
|
|
|
bool sch_subh::read_subheader(uint8_t** ptr)
|
|
{
|
|
// Skip R
|
|
bool e_bit = (bool) (*(*ptr) & 0x20)?true:false;
|
|
lcid = (uint8_t) *(*ptr) & 0x1f;
|
|
*ptr += 1;
|
|
if (is_sdu() || is_var_len_ce()) {
|
|
if (e_bit) {
|
|
F_bit = (bool) (*(*ptr) & 0x80)?true:false;
|
|
nof_bytes = (uint32_t)*(*ptr) & 0x7f;
|
|
*ptr += 1;
|
|
if (F_bit) {
|
|
nof_bytes = nof_bytes<<8 | ((uint32_t) *(*ptr) & 0xff);
|
|
*ptr += 1;
|
|
}
|
|
} else {
|
|
nof_bytes = 0;
|
|
F_bit = 0;
|
|
}
|
|
} else {
|
|
nof_bytes = sizeof_ce(lcid, parent->is_ul());
|
|
}
|
|
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;
|
|
} else if (buffer_size > 150000) {
|
|
return 63;
|
|
} else {
|
|
for (int i=0;i<61;i++) {
|
|
if (buffer_size < btable[i+2]) {
|
|
return 1+i;
|
|
}
|
|
}
|
|
return 62;
|
|
}
|
|
}
|
|
|
|
// Implements Table 9.1.8.4-1 Power headroom report mapping (36.133)
|
|
uint8_t sch_subh::phr_report_table(float phr_value)
|
|
{
|
|
if (phr_value < -23) {
|
|
phr_value = -23;
|
|
}
|
|
if (phr_value > 40) {
|
|
phr_value = 40;
|
|
}
|
|
return (uint8_t) floor(phr_value+23);
|
|
}
|
|
|
|
void rar_pdu::fprint(FILE* stream)
|
|
{
|
|
fprintf(stream, "MAC PDU for RAR. ");
|
|
if (has_backoff_indicator) {
|
|
fprintf(stream, "Backoff Indicator %d. ", backoff_indicator);
|
|
}
|
|
pdu::fprint(stream);
|
|
}
|
|
|
|
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;
|
|
backoff_indicator = bi;
|
|
}
|
|
|
|
// Section 6.1.5
|
|
bool rar_pdu::write_packet(uint8_t* ptr)
|
|
{
|
|
// Write Backoff Indicator, if any
|
|
if (has_backoff_indicator) {
|
|
*(ptr) = backoff_indicator&0xf;
|
|
if (nof_subheaders > 0) {
|
|
*(ptr) = 1<<7;
|
|
}
|
|
ptr++;
|
|
}
|
|
|
|
// Write RAR subheaders
|
|
for (int i=0;i<nof_subheaders;i++) {
|
|
subheaders[i].write_subheader(&ptr, i==nof_subheaders-1);
|
|
}
|
|
// Write payload
|
|
for (int i=0;i<nof_subheaders;i++) {
|
|
subheaders[i].write_payload(&ptr);
|
|
}
|
|
// Set padding to zeros (if any)
|
|
bzero(ptr, rem_len*sizeof(uint8_t));
|
|
|
|
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()
|
|
{
|
|
bzero(grant, sizeof(uint8_t) * RAR_GRANT_LEN);
|
|
ta = 0;
|
|
temp_rnti = 0;
|
|
}
|
|
|
|
uint32_t rar_subh::get_rapid()
|
|
{
|
|
return preamble;
|
|
}
|
|
|
|
void rar_subh::get_sched_grant(uint8_t grant_[RAR_GRANT_LEN])
|
|
{
|
|
memcpy(grant_, grant, sizeof(uint8_t)*RAR_GRANT_LEN);
|
|
}
|
|
|
|
uint32_t rar_subh::get_ta_cmd()
|
|
{
|
|
return ta;
|
|
}
|
|
|
|
uint16_t rar_subh::get_temp_crnti()
|
|
{
|
|
return temp_rnti;
|
|
}
|
|
|
|
void rar_subh::set_rapid(uint32_t rapid)
|
|
{
|
|
preamble = rapid;
|
|
}
|
|
|
|
void rar_subh::set_sched_grant(uint8_t grant_[RAR_GRANT_LEN])
|
|
{
|
|
memcpy(grant, grant_, sizeof(uint8_t)*RAR_GRANT_LEN);
|
|
}
|
|
|
|
void rar_subh::set_ta_cmd(uint32_t ta_)
|
|
{
|
|
ta = ta_;
|
|
}
|
|
|
|
void rar_subh::set_temp_crnti(uint16_t temp_rnti_)
|
|
{
|
|
temp_rnti = temp_rnti_;
|
|
}
|
|
|
|
// Section 6.2.2
|
|
void rar_subh::write_subheader(uint8_t** ptr, bool is_last)
|
|
{
|
|
*(*ptr) = (uint8_t) (!is_last<<7 | 1<<6 | (preamble & 0x3f));
|
|
*ptr += 1;
|
|
}
|
|
// Section 6.2.3
|
|
void rar_subh::write_payload(uint8_t** ptr)
|
|
{
|
|
*(*ptr + 0) = (uint8_t) ((ta&0x7f0)>>4);
|
|
*(*ptr + 1) = (uint8_t) ((ta&0xf) <<4) | (grant[0]<<3) | (grant[1]<<2) | (grant[2]<<1) | grant[3];
|
|
uint8_t *x = &grant[4];
|
|
*(*ptr + 2) = (uint8_t) srslte_bit_pack(&x, 8);
|
|
*(*ptr + 3) = (uint8_t) srslte_bit_pack(&x, 8);
|
|
*(*ptr + 4) = (uint8_t) ((temp_rnti&0xff00) >> 8);
|
|
*(*ptr + 5) = (uint8_t) (temp_rnti&0x00ff);
|
|
*ptr += 6;
|
|
}
|
|
|
|
void rar_subh::read_payload(uint8_t** ptr)
|
|
{
|
|
ta = ((uint32_t) *(*ptr + 0)&0x7f)<<4 | (*(*ptr + 1)&0xf0)>>4;
|
|
grant[0] = *(*ptr + 1)&0x8?1:0;
|
|
grant[1] = *(*ptr + 1)&0x4?1:0;
|
|
grant[2] = *(*ptr + 1)&0x2?1:0;
|
|
grant[3] = *(*ptr + 1)&0x1?1:0;
|
|
uint8_t *x = &grant[4];
|
|
srslte_bit_unpack(*(*ptr+2), &x, 8);
|
|
srslte_bit_unpack(*(*ptr+3), &x, 8);
|
|
temp_rnti = ((uint16_t) *(*ptr + 4))<<8 | *(*ptr + 5);
|
|
*ptr += 6;
|
|
}
|
|
|
|
bool rar_subh::read_subheader(uint8_t** ptr)
|
|
{
|
|
bool e_bit = *(*ptr) & 0x80?true:false;
|
|
bool type = *(*ptr) & 0x40?true:false;
|
|
if (type) {
|
|
preamble = *(*ptr) & 0x3f;
|
|
} else {
|
|
((rar_pdu*)parent)->set_backoff(*(*ptr) & 0xf);
|
|
}
|
|
*ptr += 1;
|
|
return e_bit;
|
|
}
|
|
|
|
}
|