/* Recalculate CRC. */
int journal_update_crc(int fd)
{
if (fcntl(fd, F_GETFL) < 0) {
return KNOT_EINVAL;
}
char buf[4096];
ssize_t rb = 0;
crc_t crc = crc_init();
if (lseek(fd, MAGIC_LENGTH + sizeof(crc_t), SEEK_SET) < 0) {
return KNOT_ERROR;
}
while((rb = read(fd, buf, sizeof(buf))) > 0) {
crc = crc_update(crc, (const unsigned char *)buf, rb);
}
if (lseek(fd, MAGIC_LENGTH, SEEK_SET) < 0) {
return KNOT_ERROR;
}
if (!sfwrite(&crc, sizeof(crc_t), fd)) {
dbg_journal("journal: couldn't write CRC to fd=%d\n", fd);
return KNOT_ERROR;
}
return KNOT_EOK;
}
int read_header(ch16_header* header, uint32_t size, uint8_t* data)
{
if(header->magic != 0x36314843)
{
fprintf(stderr,"Invalid magic number\n");
fprintf(stderr,"Found: 0x%x, Expected: 0x%x\n",
header->magic, 0x36314843);
return 0;
}
if(header->reserved != 0)
{
fprintf(stderr,"Reserved not 0\n");
return 0;
}
if(header->rom_size != size - sizeof(ch16_header))
{
fprintf(stderr,"Incorrect size reported\n");
fprintf(stderr,"Found: 0x%x, Expected: 0x%x\n",
header->rom_size, (uint32_t)(size - sizeof(ch16_header)));
return 0;
}
crc_t crc = crc_init();
crc = crc_update(crc,data,size-sizeof(ch16_header));
crc = crc_finalize(crc);
if(header->crc32_sum != crc)
{
fprintf(stderr,"Incorrect CRC32 checksum\n");
fprintf(stderr,"Found: 0x%x, Expected: 0x%x\n",
header->crc32_sum, crc);
return 0;
}
return 1;
}
SDC_ERRORCode sdc_check_message(FIL* df, DWORD ofs) {
SDC_ERRORCode sdc_ret;
DWORD saved_ofs = sdc_fp_index;
crc_t crcd, crcd_calc;
uint8_t rd[sizeof(GENERIC_message)];
unsigned int bytes_read;
sdc_ret = sdc_set_fp_index(df, ofs) ;
if(sdc_ret != SDC_OK) { return sdc_ret; }
sdc_ret = sdc_f_read(df, rd, sizeof(GENERIC_message), &bytes_read);
if(sdc_ret != SDC_OK) { return sdc_ret; }
sdc_ret = sdc_f_read(df, &crcd, sizeof(crc_t), &bytes_read);
if(sdc_ret != SDC_OK) { return sdc_ret; }
// calc checksum
crcd_calc = crc_init();
crcd_calc = crc_update(crcd_calc, (const unsigned char*) &rd, sizeof(GENERIC_message));
crcd_calc = crc_finalize(crcd_calc);
if(crcd != crcd_calc) {
SDCDEBUG("%s: No valid checksum in data. Data: %u\tvs.\tCalc: %u\r\n", __func__, crcd, crcd_calc);
sdc_ret = sdc_set_fp_index(df, saved_ofs) ;
if(sdc_ret != SDC_OK) { return sdc_ret; }
return SDC_CHECKSUM_ERROR;
}
sdc_ret = sdc_set_fp_index(df, saved_ofs) ;
if(sdc_ret != SDC_OK) { return sdc_ret; }
return SDC_OK;
}
void GazeboMavlinkInterface::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
component_ID = 0;
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* no idea which numbers should be here*/
buf[2] = 100;
buf[3] = 0;
buf[4] = component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));
if (len <= 0) {
printf("Failed sending mavlink message");
}
}
/**
* Send the header of the message - everything before the data block.
*/
static void send_msg_header(uint8_t addr, uint8_t cmd, uint16_t datalen)
{
crc_init();
buffer_send(addr, true);
buffer_send(cmd, true);
buffer_send(READ_U16_BYTE(datalen, 0), true);
buffer_send(READ_U16_BYTE(datalen, 1), true);
}
crc_t ChecksummedPacket::computeChecksum() const
{
crc_t c = crc_init();
c = crc_update(c, reinterpret_cast<const unsigned char *>(&packet), sizeof(Packet));
c = crc_update(c, reinterpret_cast<const unsigned char *>(&order), sizeof(uint64_t));
c = crc_finalize(c);
return c;
}
int main() {
crc_init();
register_cmd_handler(LOOPBACK_CMD, cmd_loopback_handler);
register_cmd_handler(ERROR_CMD, cmd_error_handler);
//test_loopback(TEST_REPETITIONS);
test_multi_section_loopback(TEST_REPETITIONS);
//test_extra_bytes(TEST_REPETITIONS);
return 0;
}
//credits to iceman
uint32_t CRC8Maxim(uint8_t *buff, size_t size) {
crc_t crc;
crc_init(&crc, 9, 0x8c, 0x00, 0x00);
crc_clear(&crc);
for (size_t i=0; i < size; ++i)
crc_update(&crc, buff[i], 8);
return crc_finish(&crc);
}
int recieveint_rs485(int *value)
{
totalBytesRead += ecrobot_read_rs485(raw, totalBytesRead, (INT_SIZE+1) - totalBytesRead);
if (totalBytesRead != (INT_SIZE+1))
return 0;
crc_t crcRecieved = (crc_t)raw[0];
U8 rawInt[INT_SIZE];
rawInt[0] = raw[1];
rawInt[1] = raw[2];
rawInt[2] = raw[3];
rawInt[3] = raw[4];
crc_t crc;
crc = crc_init();
crc = crc_update(crc, (unsigned char *)rawInt, INT_SIZE);
crc = crc_finalize(crc);
if (crc != crcRecieved)
{
print_str(0,0,"CRC errors = ");
display_int(++crc_errors, 0);
display_update();
//We reboot the network interface
ecrobot_term_rs485();
ecrobot_init_rs485(NETWORK_SPEED);
totalBytesRead = 0;
return 0;
}
// We convert the raw bytes to an int
*value = 0;
for (int i = INT_SIZE-1; i > -1 ; i--)
{
int temp = 0;
temp += rawInt[i];
temp <<= (8 * i);
*value += temp;
}
totalBytesRead = 0;
// print_clear_line(5);
// display_update();
// display_goto_xy(0,5);
// display_int(*value, 0);
// display_update();
return 1;
}
//credits to iceman
uint32_t CRC8Legic(uint8_t *buff, size_t size) {
// Poly 0x63, reversed poly 0xC6, Init 0x55, Final 0x00
crc_t crc;
crc_init(&crc, 8, 0xC6, 0x55, 0);
crc_clear(&crc);
for ( int i = 0; i < size; ++i)
crc_update(&crc, buff[i], 8);
return SwapBits(crc_finish(&crc), 8);
}
/**
* Read current packet from current file. Pad the packet with PAD_CHAR if the
* file has less than PACKET_SIZE bytes left to read.
*
* @return the number of bytes read from the file
*/
size_t Xmodem::readPacket() {
size_t bytesRead = fileRead(_packet, PACKET_SIZE);
for (size_t i = bytesRead; i < PACKET_SIZE; i++) {
_packet[i] = PAD_CHAR;
}
_crc = crc_init();
_crc = crc_update(_crc, _packet, PACKET_SIZE);
_crc = crc_finalize(_crc);
return bytesRead;
}
static int clear_metadata(struct md *md, const char *device)
{
struct era_superblock *sb;
int supported = 0;
int valid = 0;
sb = md_block(md, MD_NOCRC, 0, 0);
if (!sb)
return -1;
if (le32toh(sb->magic) == SUPERBLOCK_MAGIC)
{
uint32_t csum;
csum = crc_update(crc_init(), &sb->flags,
MD_BLOCK_SIZE - sizeof(uint32_t));
csum ^= SUPERBLOCK_CSUM_XOR;
if (le32toh(sb->csum) == csum)
{
valid++;
if (le32toh(sb->version) >= MIN_ERA_VERSION &&
le32toh(sb->version) <= MIN_ERA_VERSION)
supported++;
}
}
if (!force && memcmp(sb, empty_block, MD_BLOCK_SIZE))
{
char *what;
if (valid)
{
if (supported)
what = "valid era superblock";
else
what = "unsupported era superblock";
}
else
what = "existing data";
error(0, "%s found on %s", what, device);
return -1;
}
if (md_write(md, 0, empty_block))
return -1;
return 0;
}
static int decode_resync(bitstream_t *bs)
{
uint_16 sync_word;
//int i = 0;
/* Make sure we sync'ed */
sync_word = bitstream_get(bs,16);
if(sync_word == 0x0b77 )
{
crc_init();
return 1;
}
return 0;
}
__interrupt void USCI0RX_ISR(void)
{
uint8_t rx_read;
// Handle a UART Rx Interrupt
if (IFG2 & UCA0RXIFG) {
// Read in from UART peripheral and echo (for debug... will be removed)
rx_read = UCA0RXBUF;
switch ( uart_dev.state ) {
case IDLE:
if (rx_read == UART_MAGIC_FRAME_START) {
uart_dev.state = MESSAGE;
vector_uint8_clear(&uart_dev.rxbuf);
crc_init(&(uart_dev.rxcrc));
}
break;
case MESSAGE:
if (rx_read == UART_MAGIC_ESCAPE) {
uart_dev.state = ESCAPE;
crc_add_byte( &(uart_dev.rxcrc), rx_read);
} else if (rx_read == UART_MAGIC_FRAME_END) {
if ( crc_check(uart_dev.rxcrc) || ignore_crc ) {
// Remove CRC
uart_dev.rxbuf.end -= 2;
// Find command
switch (vector_uint8_get(&uart_dev.rxbuf, 0)) {
case UART_COMMANDS_WR_REG: // Write Register command
// rxbuf should be 4 long (command + addr + data)
if (uart_dev.rxbuf.end == 4) {
uint8_t addr = vector_uint8_get(&uart_dev.rxbuf, 1);
if (addr == 0x00) {
settings_reg = (uint16_t)(((uint16_t)vector_uint8_get(&uart_dev.rxbuf, 2) << 8) | vector_uint8_get(&uart_dev.rxbuf, 3));
uart_dev_send_ack(&uart_dev);
} else if ((addr & 0xf0) == 0x10) {
// AD5504
ad5504_value_reg[addr & 0x0f] = (uint16_t)(((uint16_t)vector_uint8_get(&uart_dev.rxbuf, 2) << 8) | vector_uint8_get(&uart_dev.rxbuf, 3));
AD5504_send(&ad5504, (ad5504_addresses[addr&0x0f] & 0xff), ad5504_value_reg[addr&0x0f], AD5504_WRITE, ad5504_addresses[addr&0x0f]>>8);
uart_dev_send_ack(&uart_dev);
} else if ((addr & 0xf0) == 0x20) {
// DAC7512
dac7512_value_reg[addr & 0x0f] = (uint16_t)(((uint16_t)vector_uint8_get(&uart_dev.rxbuf, 2) << 8) | vector_uint8_get(&uart_dev.rxbuf, 3));
if ((dac7512_counter_reg[0] == 0) && ((addr & 0x0f) == 0)) {
DAC7512_send(&dac7512, dac7512_value_reg[addr & 0x0f], (addr>>2)&0x3, addr&0x3);
}
uart_dev_send_ack(&uart_dev);
} else if ((addr & 0xf0) == 0x30) {
int main(int argc, char **argv) {
int i;
char *data;
unsigned int crc_word, expected_word;
crc_t crc_p;
parse_args(argc, argv);
data = malloc(sizeof(char) * (num_bits + crc_length * 2));
if (!data) {
perror("malloc");
exit(-1);
}
if (!seed) {
seed = time(NULL);
}
srand(seed);
// Generate data
for (i = 0; i < num_bits; i++) {
data[i] = rand() % 2;
}
//Initialize CRC params and tables
if (crc_init(&crc_p, crc_poly, crc_length)) {
exit(-1);
}
// generate CRC word
crc_word = crc_checksum(&crc_p, data, num_bits);
free(data);
// check if generated word is as expected
if (get_expected_word(num_bits, crc_length, crc_poly, seed,
&expected_word)) {
fprintf(stderr, "Test parameters not defined in test_results.h\n");
exit(-1);
}
exit(expected_word != crc_word);
}
void build_header(ch16_header* header, uint8_t spec_ver, uint32_t rom_size, uint16_t start_addr, uint8_t* data)
{
if(header == NULL)
{
fprintf(stderr,"Null pointer exception ***(build_header)");
exit(1);
}
// Magic number 'CH16'
header->magic = 0x36314843;
// Reserved, always 0
header->reserved = 0x00;
// Spec version
header->spec_ver = spec_ver;
// Rom size
header->rom_size = rom_size;
// Start address
header->start_addr = start_addr;
// Calculate CRC
crc_t crc = crc_init();
crc = crc_update(crc,data,rom_size);
crc = crc_finalize(crc);
header->crc32_sum = crc;
}
void send_buffered_ints_rs485() {
if (buffer_count_elements(&sendBuffer) > 0) {
struct Candy* candy = buffer_dequeue(&sendBuffer);
// We convert the int to an array of 4 bytes
int value = candy->rpmTicksStamp;
U8 buffer[INT_SIZE];
buffer[0] = (U8)value;
value >>= 8;
buffer[1] = (U8)value;
value >>= 8;
buffer[2] = (U8)value;
value >>= 8;
buffer[3] = (U8)value;
// We calculate the checksum for the 4 bytes
crc_t crc;
crc = crc_init();
crc = crc_update(crc, (unsigned char *)buffer, INT_SIZE);
crc = crc_finalize(crc);
// We pack all the bytes in one array and sends it
U8 toSend[INT_SIZE + 1];
toSend[0] = (U8)crc;
toSend[1] = buffer[0];
toSend[2] = buffer[1];
toSend[3] = buffer[2];
toSend[4] = buffer[3];
ecrobot_send_rs485(toSend, 0, INT_SIZE+1);
}
}
//Build a string and check its CRC with a predefined value.
//The string will go through ascii characters 32-126, which are typable on the keyboard
//Once every possibility has been used, the string expands in size (by shifting the null character over)
int main(void)
{
char str[100];
time_t t0, t1;
crc_t crc;
int i, j, k, l;
double timediff;
t0 = time(0);
for(i=1; i<100; i++) {
str[i] = '\0';
for(j=0; j<i; j++) {
str[j] = 32;
}
while((unsigned char)str[i-1] <= 127) {
//check, increment any thats above the ascii table
for(j=0;j<i-1;j++) {
if((unsigned char)str[j] >= 127) {
str[j] = 32;
str[j+1] = str[j+1] + 1;
}
}
crc = crc_init();
crc = crc_update(crc, (unsigned char *)str, i);
crc = crc_finalize(crc);
if((unsigned long) 0xe6e5c283 == crc) { //CRC('123456789') = 0xcbf43926, CRC('b6e98880913adcb24108621903b25f76') = 0xe6e5c283, put in the one you are looking for
t1 = time(0);
timediff = difftime(t1,t0);
printf("time = %f seconds, string = %s value = 0x%lx\n", timediff, str, (unsigned long)crc);
system("PAUSE");
return;
}
str[0] = str[0] + 1;
}
}
system("PAUSE");
return 0;
}
int program_map_section_read(program_map_section_t *pms, uint8_t *buf, size_t buf_size, uint32_t payload_unit_start_indicator,
psi_table_buffer_t *pmtBuffer)
{
LOG_DEBUG ("program_map_section_read -- entering");
if (pms == NULL || buf == NULL)
{
SAFE_REPORT_TS_ERR(-1);
return 0;
}
bs_t *b = NULL;
if (!payload_unit_start_indicator && pmtBuffer->buffer == NULL)
{
// this TS packet is not start of table, and we have no cached table data
LOG_WARN ("program_map_section_read: payload_unit_start_indicator not set and no cached data");
return 0;
}
if (payload_unit_start_indicator)
{
uint8_t payloadStartPtr = buf[0];
buf += (payloadStartPtr + 1);
buf_size -= (payloadStartPtr + 1);
LOG_DEBUG_ARGS ("program_map_section_read: payloadStartPtr = %d", payloadStartPtr);
}
// check for pmt spanning multiple TS packets
if (pmtBuffer->buffer != NULL)
{
LOG_DEBUG_ARGS ("program_map_section_read: pmtBuffer detected: pmtBufferAllocSz = %d, pmtBufferUsedSz = %d", pmtBuffer->bufferAllocSz, pmtBuffer->bufferUsedSz);
size_t numBytesToCopy = buf_size;
if (buf_size > (pmtBuffer->bufferAllocSz - pmtBuffer->bufferUsedSz))
{
numBytesToCopy = pmtBuffer->bufferAllocSz - pmtBuffer->bufferUsedSz;
}
LOG_DEBUG_ARGS ("program_map_section_read: copying %d bytes to pmtBuffer", numBytesToCopy);
memcpy (pmtBuffer->buffer + pmtBuffer->bufferUsedSz, buf, numBytesToCopy);
pmtBuffer->bufferUsedSz += numBytesToCopy;
if (pmtBuffer->bufferUsedSz < pmtBuffer->bufferAllocSz)
{
LOG_DEBUG ("program_map_section_read: pmtBuffer not yet full -- returning");
return 0;
}
b = bs_new(pmtBuffer->buffer, pmtBuffer->bufferUsedSz);
}
else
{
b = bs_new(buf, buf_size);
}
pms->table_id = bs_read_u8(b);
if (pms->table_id != TS_program_map_section)
{
LOG_ERROR_ARGS("Table ID in PMT is 0x%02X instead of expected 0x%02X", pms->table_id, TS_program_map_section);
reportAddErrorLogArgs("Table ID in PMT is 0x%02X instead of expected 0x%02X", pms->table_id, TS_program_map_section);
SAFE_REPORT_TS_ERR(-40);
resetPSITableBuffer(pmtBuffer);
bs_free (b);
return 0;
}
pms->section_syntax_indicator = bs_read_u1(b);
if (!pms->section_syntax_indicator)
{
LOG_ERROR("section_syntax_indicator not set in PMT");
reportAddErrorLog("section_syntax_indicator not set in PMT");
SAFE_REPORT_TS_ERR(-41);
resetPSITableBuffer(pmtBuffer);
bs_free (b);
return 0;
}
bs_skip_u(b, 3);
pms->section_length = bs_read_u(b, 12);
if (pms->section_length > MAX_SECTION_LEN)
{
LOG_ERROR_ARGS("PMT section length is 0x%02X, larger than maximum allowed 0x%02X",
pms->section_length, MAX_SECTION_LEN);
reportAddErrorLogArgs("PMT section length is 0x%02X, larger than maximum allowed 0x%02X",
pms->section_length, MAX_SECTION_LEN);
SAFE_REPORT_TS_ERR(-42);
resetPSITableBuffer(pmtBuffer);
bs_free (b);
return 0;
}
if (pms->section_length > bs_bytes_left(b))
{
LOG_DEBUG ("program_map_section_read: Detected section spans more than one TS packet -- allocating buffer");
if (pmtBuffer->buffer != NULL)
{
// should never get here
LOG_ERROR ("program_map_section_read: unexpected pmtBufffer");
reportAddErrorLog ("program_map_section_read: unexpected pmtBufffer");
resetPSITableBuffer(pmtBuffer);
}
pmtBuffer->bufferAllocSz = pms->section_length + 3;
pmtBuffer->buffer = (uint8_t *)calloc (pms->section_length + 3, 1);
memcpy (pmtBuffer->buffer, buf, buf_size);
pmtBuffer->bufferUsedSz = buf_size;
bs_free (b);
return 0;
}
int section_start = bs_pos(b);
// bytes 0,1
pms->program_number = bs_read_u16(b);
// byte 2;
bs_skip_u(b, 2);
pms->version_number = bs_read_u(b, 5);
pms->current_next_indicator = bs_read_u1(b);
if (!pms->current_next_indicator) LOG_WARN("This PMT is not yet applicable/n");
// bytes 3,4
pms->section_number = bs_read_u8(b);
pms->last_section_number = bs_read_u8(b);
if (pms->section_number != 0 || pms->last_section_number != 0)
{
LOG_ERROR("Multi-section PMT is not allowed/n");
reportAddErrorLog("Multi-section PMT is not allowed/n");
SAFE_REPORT_TS_ERR(-43);
resetPSITableBuffer(pmtBuffer);
bs_free (b);
return 0;
}
bs_skip_u(b, 3);
pms->PCR_PID = bs_read_u(b, 13);
if (pms->PCR_PID < GENERAL_PURPOSE_PID_MIN || pms->PCR_PID > GENERAL_PURPOSE_PID_MAX)
{
LOG_ERROR_ARGS("PCR PID has invalid value 0x%02X", pms->PCR_PID);
reportAddErrorLogArgs("PCR PID has invalid value 0x%02X", pms->PCR_PID);
SAFE_REPORT_TS_ERR(-44);
resetPSITableBuffer(pmtBuffer);
bs_free (b);
return 0;
}
// printf ("PCR PID = %d\n", pms->PCR_PID);
bs_skip_u(b, 4);
pms->program_info_length = bs_read_u(b, 12);
if (pms->program_info_length > MAX_PROGRAM_INFO_LEN)
{
LOG_ERROR_ARGS("PMT program info length is 0x%02X, larger than maximum allowed 0x%02X",
pms->program_info_length, MAX_PROGRAM_INFO_LEN);
reportAddErrorLogArgs("PMT program info length is 0x%02X, larger than maximum allowed 0x%02X",
pms->program_info_length, MAX_PROGRAM_INFO_LEN);
SAFE_REPORT_TS_ERR(-45);
resetPSITableBuffer(pmtBuffer);
bs_free (b);
return 0;
}
read_descriptor_loop(pms->descriptors, b, pms->program_info_length);
while (!bs_eof(b) && pms->section_length - (bs_pos(b) - section_start) > 4) // account for CRC
{
elementary_stream_info_t *es = es_info_new();
es_info_read(es, b);
vqarray_add(pms->es_info, es);
}
pms->CRC_32 = bs_read_u32(b);
// check CRC
crc_t pas_crc = crc_init();
pas_crc = crc_update(pas_crc, b->start, bs_pos(b) - 4);
pas_crc = crc_finalize(pas_crc);
if (pas_crc != pms->CRC_32)
{
LOG_ERROR_ARGS("PMT CRC_32 specified as 0x%08X, but calculated as 0x%08X", pms->CRC_32, pas_crc);
reportAddErrorLogArgs("PMT CRC_32 specified as 0x%08X, but calculated as 0x%08X", pms->CRC_32, pas_crc);
SAFE_REPORT_TS_ERR(-46);
resetPSITableBuffer(pmtBuffer);
bs_free (b);
return 0;
}
else
{
// LOG_DEBUG("PMT CRC_32 checked successfully");
}
int bytes_read = bs_pos(b);
bs_free(b);
resetPSITableBuffer(pmtBuffer);
return bytes_read;
}
void lux_hal_reset_crc(){
crc = crc_init();
}
int program_association_section_read(program_association_section_t *pas, uint8_t *buf, size_t buf_len, uint32_t payload_unit_start_indicator,
psi_table_buffer_t *patBuffer)
{
vqarray_t *programs;
int num_programs = 0;
if (pas == NULL || buf == NULL)
{
SAFE_REPORT_TS_ERR(-1);
return 0;
}
bs_t *b = NULL;
if (!payload_unit_start_indicator && patBuffer->buffer == NULL)
{
// this TS packet is not start of table, and we have no cached table data
LOG_WARN ("program_association_section_read: payload_unit_start_indicator not set and no cached data");
return 0;
}
if (payload_unit_start_indicator)
{
uint8_t payloadStartPtr = buf[0];
buf += (payloadStartPtr + 1);
buf_len -= (payloadStartPtr + 1);
LOG_DEBUG_ARGS ("program_association_section_read: payloadStartPtr = %d", payloadStartPtr);
}
// check for pat spanning multiple TS packets
if (patBuffer->buffer != NULL)
{
LOG_DEBUG_ARGS ("program_association_section_read: patBuffer detected: patBufferAllocSz = %d, patBufferUsedSz = %d",
patBuffer->bufferAllocSz, patBuffer->bufferUsedSz);
size_t numBytesToCopy = buf_len;
if (buf_len > (patBuffer->bufferAllocSz - patBuffer->bufferUsedSz))
{
numBytesToCopy = patBuffer->bufferAllocSz - patBuffer->bufferUsedSz;
}
LOG_DEBUG_ARGS ("program_association_section_read: copying %d bytes to patBuffer", numBytesToCopy);
memcpy (patBuffer->buffer + patBuffer->bufferUsedSz, buf, numBytesToCopy);
patBuffer->bufferUsedSz += numBytesToCopy;
if (patBuffer->bufferUsedSz < patBuffer->bufferAllocSz)
{
LOG_DEBUG ("program_association_section_read: patBuffer not yet full -- returning");
return 0;
}
b = bs_new(patBuffer->buffer, patBuffer->bufferUsedSz);
}
else
{
b = bs_new(buf, buf_len);
}
pas->table_id = bs_read_u8(b);
if (pas->table_id != program_association_section)
{
LOG_ERROR_ARGS("Table ID in PAT is 0x%02X instead of expected 0x%02X",
pas->table_id, program_association_section);
reportAddErrorLogArgs("Table ID in PAT is 0x%02X instead of expected 0x%02X",
pas->table_id, program_association_section);
SAFE_REPORT_TS_ERR(-30);
resetPSITableBuffer(patBuffer);
bs_free (b);
return 0;
}
// read byte 0
pas->section_syntax_indicator = bs_read_u1(b);
if (!pas->section_syntax_indicator)
{
LOG_ERROR("section_syntax_indicator not set in PAT");
reportAddErrorLog("section_syntax_indicator not set in PAT");
SAFE_REPORT_TS_ERR(-31);
resetPSITableBuffer(patBuffer);
bs_free (b);
return 0;
}
bs_skip_u(b, 3); // TODO read the zero bit, check it to be zero
pas->section_length = bs_read_u(b, 12);
if (pas->section_length > MAX_SECTION_LEN)
{
LOG_ERROR_ARGS("PAT section length is 0x%02X, larger than maximum allowed 0x%02X",
pas->section_length, MAX_SECTION_LEN);
reportAddErrorLogArgs("PAT section length is 0x%02X, larger than maximum allowed 0x%02X",
pas->section_length, MAX_SECTION_LEN);
SAFE_REPORT_TS_ERR(-32);
resetPSITableBuffer(patBuffer);
bs_free (b);
return 0;
}
if (pas->section_length > bs_bytes_left(b))
{
LOG_DEBUG ("program_association_section_read: Detected section spans more than one TS packet -- allocating buffer");
if (patBuffer->buffer != NULL)
{
// should never get here
LOG_ERROR ("program_association_section_read: unexpected patBufffer");
reportAddErrorLog ("program_association_section_read: unexpected patBufffer");
resetPSITableBuffer(patBuffer);
}
patBuffer->bufferAllocSz = pas->section_length + 3;
patBuffer->buffer = (uint8_t *)calloc (pas->section_length + 3, 1);
memcpy (patBuffer->buffer, buf, buf_len);
patBuffer->bufferUsedSz = buf_len;
bs_free (b);
return 0;
}
// read bytes 1,2
pas->transport_stream_id = bs_read_u16(b);
// read bytes 3,4
bs_skip_u(b, 2);
pas->version_number = bs_read_u(b, 5);
pas->current_next_indicator = bs_read_u1(b);
if (!pas->current_next_indicator) LOG_WARN("This PAT is not yet applicable/n");
// read byte 5
pas->section_number = bs_read_u8(b);
pas->last_section_number = bs_read_u8(b);
if (pas->section_number != 0 || pas->last_section_number != 0) LOG_WARN("Multi-section PAT is not supported yet/n");
// read bytes 6,7
num_programs = (pas->section_length - 5 - 4) / 4; // Programs listed in the PAT
// explanation: section_length gives us the length from the end of section_length
// we used 5 bytes for the mandatory section fields, and will use another 4 bytes for CRC
// the remaining bytes contain program information, which is 4 bytes per iteration
// It's much shorter in C :-)
// Read the program loop, but ignore the NIT PID "program"
programs = vqarray_new();
for (uint32_t i = 0; i < num_programs; i++)
{
program_info_t *prog = malloc(sizeof(program_info_t));
prog->program_number = bs_read_u16(b);
if (prog->program_number == 0) { // Skip the NIT PID program (not a real program)
free(prog);
bs_skip_u(b, 16);
continue;
}
bs_skip_u(b, 3);
prog->program_map_PID = bs_read_u(b, 13);
vqarray_add(programs, (vqarray_elem_t*)prog);
}
// This is our true number of programs
pas->_num_programs = vqarray_length(programs);
if (pas->_num_programs > 1) LOG_WARN_ARGS("%zd programs found, but only SPTS is fully supported. Patches are welcome.", pas->_num_programs);
// Copy form our vqarray into the native array
pas->programs = malloc(pas->_num_programs * sizeof(program_info_t));
for (uint32_t i = 0; i < pas->_num_programs; i++)
{
program_info_t* prog = (program_info_t*)vqarray_pop(programs);
pas->programs[i] = *prog;
free(prog);
}
vqarray_free(programs);
pas->CRC_32 = bs_read_u32(b);
// check CRC
crc_t pas_crc = crc_init();
pas_crc = crc_update(pas_crc, buf, bs_pos(b) - 4);
pas_crc = crc_finalize(pas_crc);
if (pas_crc != pas->CRC_32)
{
LOG_ERROR_ARGS("PAT CRC_32 specified as 0x%08X, but calculated as 0x%08X", pas->CRC_32, pas_crc);
reportAddErrorLogArgs("PAT CRC_32 specified as 0x%08X, but calculated as 0x%08X", pas->CRC_32, pas_crc);
SAFE_REPORT_TS_ERR(-33);
resetPSITableBuffer(patBuffer);
bs_free (b);
return 0;
}
else
{
// LOG_DEBUG("PAT CRC_32 checked successfully");
// don't enable unless you want to see this every ~100ms
}
bs_free(b);
resetPSITableBuffer(patBuffer);
return 1;
}
uint8_t * ptr = tmp;
int n;
memcpy(ptr, &packet->destination, sizeof packet->destination);
ptr += sizeof packet->destination;
memcpy(ptr, &packet->command, sizeof packet->command);
ptr += sizeof packet->command;
memcpy(ptr, &packet->index, sizeof packet->index);
ptr += sizeof packet->index;
memcpy(ptr, &packet->payload, packet->payload_length);
ptr += packet->payload_length;
crc_t crc = crc_init();
crc = crc_update(crc, tmp, ptr - tmp);
crc = crc_finalize(crc);
packet->crc = crc;
memcpy(ptr, &crc, sizeof packet->crc);
ptr += sizeof packet->crc;
n = cobs_encode(tmp, ptr - tmp, buffer);
if(n < 0) return n;
//buffer[n++] = 0; // Double null bytes
buffer[n++] = 0;
return n; // success
}
static int unframe(uint8_t * raw_data, int raw_len, struct lux_packet * packet) {
// send actuator controls message to Pixhawk
void send_outputs_mavlink(const uint16_t *pwm, const unsigned num_pwm)
{
// Fill up to number of outputs.
mavlink_actuator_control_target_t controls_message;
for (unsigned i = 0; i < num_pwm && i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; ++i) {
controls_message.controls[i] = pwm[i];
}
// And the rest with NAN.
for (unsigned i = _outputs.noutputs; (i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS)
&& (i < actuator_controls_s::NUM_ACTUATOR_CONTROLS); ++i) {
controls_message.controls[i] = NAN;
}
controls_message.time_usec = _controls.timestamp;
const uint8_t msgid = MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET;
const uint8_t component_ID = 0;
const uint8_t payload_len = mavlink_message_lengths[msgid];
const unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
// TODO FIXME: no idea which numbers should be here.
buf[2] = 100;
buf[3] = 0;
buf[4] = component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], (const void *)&controls_message, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
int ret = ::write(_fd, &buf[0], packet_len);
//static unsigned counter = 0;
//if (counter++ % 250 == 0) {
// PX4_INFO("send motor controls %d bytes %.2f %.2f %.2f %.2f",
// ret,
// controls_message.controls[0],
// controls_message.controls[1],
// controls_message.controls[2],
// controls_message.controls[3]);
//}
if (ret < 1) {
PX4_WARN("Failed sending rc mavlink message, ret: %d, errno: %d", ret, errno);
}
}
void
Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
unsigned buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
_last_write_try_time = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (buf_free < packet_len) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* use mavlink's internal counter for the TX seq */
buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;
buf[3] = mavlink_system.sysid;
buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
/* send message to UART */
ssize_t ret = write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
pthread_mutex_unlock(&_send_mutex);
}
conditional_access_section_t* conditional_access_section_read(uint8_t* buf, size_t buf_len)
{
g_return_val_if_fail(buf, NULL);
if (!buf_len) {
g_critical("Buffer for program association section is empty.");
return NULL;
}
uint8_t offset = buf[0] + 1;
if (offset > buf_len) {
g_critical("Invalid pointer field %"PRIu8" in PAT", offset - 1);
return NULL;
}
conditional_access_section_t* cas = conditional_access_section_new();
bitreader_new_stack(b, buf + offset, buf_len - offset);
if (!section_header_read((mpeg2ts_section_t*)cas, b)) {
goto fail;
}
if (cas->table_id != TABLE_ID_CONDITIONAL_ACCESS_SECTION) {
g_critical("Table ID in CAT is 0x%02X instead of expected 0x%02X",
cas->table_id, TABLE_ID_CONDITIONAL_ACCESS_SECTION);
goto fail;
}
// 18-bits of reserved value
bitreader_read_uint16(b);
bitreader_skip_bits(b, 2);
cas->version_number = bitreader_read_bits(b, 5);
cas->current_next_indicator = bitreader_read_bit(b);
cas->section_number = bitreader_read_uint8(b);
cas->last_section_number = bitreader_read_uint8(b);
if (cas->section_number != 0 || cas->last_section_number != 0) {
g_warning("Multi-section CAT is not supported yet");
}
if (cas->section_length < 9) {
g_critical("Invalid CAT section length, %"PRIu16" is not long enough to hold required data.",
cas->section_length);
goto fail;
}
if (!read_descriptors(b, cas->section_length - 5 - 4, &cas->descriptors, &cas->descriptors_len)) {
goto fail;
}
// explanation: section_length gives us the length from the end of section_length
// we used 5 bytes for the mandatory section fields, and will use another 4 bytes for CRC
// the remaining bytes contain descriptors, most probably only one
cas->crc_32 = bitreader_read_uint32(b);
if (b->error || cas->section_length + 3 - 4 > b->len) {
g_critical("Invalid Program Map Section length.");
goto fail;
}
// check CRC
crc_t crc = crc_init();
crc = crc_update(crc, b->data, cas->section_length + 3 - 4);
crc = crc_finalize(crc);
if (crc != cas->crc_32) {
g_critical("CAT CRC_32 should be 0x%08X, but calculated as 0x%08X", cas->crc_32, crc);
goto fail;
}
cleanup:
return cas;
fail:
conditional_access_section_unref(cas);
cas = NULL;
goto cleanup;
}
program_map_section_t* program_map_section_read(uint8_t* buf, size_t buf_len)
{
g_return_val_if_fail(buf, NULL);
if (!buf_len) {
g_critical("Buffer for program map section is empty.");
return NULL;
}
uint8_t offset = buf[0] + 1;
if (offset > buf_len) {
g_critical("Invalid pointer field %"PRIu8" in PMT", offset - 1);
return NULL;
}
program_map_section_t* pms = program_map_section_new();
bitreader_new_stack(b, buf + offset, buf_len - offset);
GPtrArray* es_info = NULL;
if (!section_header_read((mpeg2ts_section_t*)pms, b)) {
goto fail;
}
if (pms->table_id != TABLE_ID_PROGRAM_MAP_SECTION) {
g_critical("Table ID in PMT is 0x%02X instead of expected 0x%02X", pms->table_id,
TABLE_ID_PROGRAM_MAP_SECTION);
goto fail;
}
pms->program_number = bitreader_read_uint16(b);
// reserved
bitreader_skip_bits(b, 2);
pms->version_number = bitreader_read_bits(b, 5);
pms->current_next_indicator = bitreader_read_bit(b);
pms->section_number = bitreader_read_uint8(b);
pms->last_section_number = bitreader_read_uint8(b);
if (pms->section_number != 0 || pms->last_section_number != 0) {
g_critical("Multi-section PMT is not allowed");
}
// reserved
bitreader_skip_bits(b, 3);
pms->pcr_pid = bitreader_read_bits(b, 13);
if (pms->pcr_pid < GENERAL_PURPOSE_PID_MIN || pms->pcr_pid > GENERAL_PURPOSE_PID_MAX) {
g_critical("PCR PID has invalid value 0x%02X", pms->pcr_pid);
goto fail;
}
// reserved
bitreader_skip_bits(b, 4);
uint16_t program_info_length = bitreader_read_bits(b, 12);
if (program_info_length > MAX_PROGRAM_INFO_LEN) {
g_critical("PMT program info length is 0x%02X, larger than maximum allowed 0x%02X",
program_info_length, MAX_PROGRAM_INFO_LEN);
goto fail;
}
if (!read_descriptors(b, program_info_length, &pms->descriptors, &pms->descriptors_len)) {
goto fail;
}
es_info = g_ptr_array_new();
while (bitreader_bytes_left(b) > 4) { // account for CRC
elementary_stream_info_t* es = es_info_read(b);
if (!es) {
goto fail;
}
g_ptr_array_add(es_info, es);
}
if (bitreader_bytes_left(b) != 4) {
g_critical("CRC missing in PMT");
goto fail;
}
pms->es_info_len = es_info->len;
pms->es_info = (elementary_stream_info_t**)g_ptr_array_free(es_info, false);
es_info = NULL;
pms->crc_32 = bitreader_read_uint32(b);
if (b->error || pms->section_length + 3 - 4 > b->len) {
g_critical("Invalid Program Map Section length.");
goto fail;
}
// check CRC
crc_t pms_crc = crc_init();
pms_crc = crc_update(pms_crc, b->data, pms->section_length + 3 - 4);
pms_crc = crc_finalize(pms_crc);
if (pms_crc != pms->crc_32) {
g_critical("PMT CRC_32 should be 0x%08X, but calculated as 0x%08X", pms->crc_32, pms_crc);
goto fail;
}
cleanup:
return pms;
fail:
program_map_section_unref(pms);
if (es_info) {
g_ptr_array_set_free_func(es_info, (GDestroyNotify)es_info_free);
g_ptr_array_free(es_info, true);
}
pms = NULL;
goto cleanup;
}
program_association_section_t* program_association_section_read(uint8_t* buf, size_t buf_len)
{
g_return_val_if_fail(buf, NULL);
if (!buf_len) {
g_critical("Buffer for program association section is empty.");
return NULL;
}
uint8_t offset = buf[0] + 1;
if (offset > buf_len) {
g_critical("Invalid pointer field %"PRIu8" in PAT", offset - 1);
return NULL;
}
program_association_section_t* pas = program_association_section_new();
bitreader_new_stack(b, buf + offset, buf_len - offset);
if (!section_header_read((mpeg2ts_section_t*)pas, b)) {
goto fail;
}
if (pas->table_id != TABLE_ID_PROGRAM_ASSOCIATION_SECTION) {
g_critical("Table ID in PAT is 0x%02X instead of expected 0x%02X", pas->table_id,
TABLE_ID_PROGRAM_ASSOCIATION_SECTION);
goto fail;
}
pas->transport_stream_id = bitreader_read_uint16(b);
// Reserved bits
bitreader_skip_bits(b, 2);
pas->version_number = bitreader_read_bits(b, 5);
pas->current_next_indicator = bitreader_read_bit(b);
pas->section_number = bitreader_read_uint8(b);
pas->last_section_number = bitreader_read_uint8(b);
if(pas->section_number != 0 || pas->last_section_number != 0) {
g_warning("Multi-section PAT is not supported yet");
}
// section_length gives us the length from the end of section_length
// we used 5 bytes for the mandatory section fields, and will use another 4 bytes for CRC
// the remaining bytes contain program information, which is 4 bytes per iteration
if (pas->section_length < 9) {
g_critical("Invalid PAT, section_length of %"PRIu16" is not long enoough to hold require data.",
pas->section_length);
goto fail;
}
pas->num_programs = (pas->section_length - 5 - 4) / 4;
pas->programs = malloc(pas->num_programs * sizeof(program_info_t));
for (size_t i = 0; i < pas->num_programs; ++i) {
pas->programs[i].program_number = bitreader_read_uint16(b);
bitreader_skip_bits(b, 3); // reserved
pas->programs[i].program_map_pid = bitreader_read_bits(b, 13);
}
pas->crc_32 = bitreader_read_uint32(b);
if (b->error || pas->section_length + 3 - 4 > b->len) {
g_critical("Invalid Program Association Section length.");
goto fail;
}
// check CRC
crc_t pas_crc = crc_init();
pas_crc = crc_update(pas_crc, b->data, pas->section_length + 3 - 4);
pas_crc = crc_finalize(pas_crc);
if (pas_crc != pas->crc_32) {
g_critical("PAT CRC_32 should be 0x%08X, but calculated as 0x%08X", pas->crc_32, pas_crc);
goto fail;
}
cleanup:
return pas;
fail:
program_association_section_unref(pas);
pas = NULL;
goto cleanup;
}
int conditional_access_section_read(conditional_access_section_t *cas, uint8_t *buf, size_t buf_len, uint32_t payload_unit_start_indicator,
psi_table_buffer_t *catBuffer)
{
if (cas == NULL || buf == NULL)
{
SAFE_REPORT_TS_ERR(-1);
return 0;
}
bs_t *b = NULL;
if (!payload_unit_start_indicator && catBuffer->buffer == NULL)
{
// this TS packet is not start of table, and we have no cached table data
LOG_WARN ("conditional_access_section_read: payload_unit_start_indicator not set and no cached data");
return 0;
}
if (payload_unit_start_indicator)
{
uint8_t payloadStartPtr = buf[0];
buf += (payloadStartPtr + 1);
buf_len -= (payloadStartPtr + 1);
LOG_DEBUG_ARGS ("conditional_access_section_read: payloadStartPtr = %d", payloadStartPtr);
}
// check for pat spanning multiple TS packets
if (catBuffer->buffer != NULL)
{
LOG_DEBUG_ARGS ("conditional_access_section_read: catBuffer detected: catBufferAllocSz = %d, catBufferUsedSz = %d",
catBuffer->bufferAllocSz, catBuffer->bufferUsedSz);
size_t numBytesToCopy = buf_len;
if (buf_len > (catBuffer->bufferAllocSz - catBuffer->bufferUsedSz))
{
numBytesToCopy = catBuffer->bufferAllocSz - catBuffer->bufferUsedSz;
}
LOG_DEBUG_ARGS ("conditional_access_section_read: copying %d bytes to catBuffer", numBytesToCopy);
memcpy (catBuffer->buffer + catBuffer->bufferUsedSz, buf, numBytesToCopy);
catBuffer->bufferUsedSz += numBytesToCopy;
if (catBuffer->bufferUsedSz < catBuffer->bufferAllocSz)
{
LOG_DEBUG ("conditional_access_section_read: catBuffer not yet full -- returning");
return 0;
}
b = bs_new(catBuffer->buffer, catBuffer->bufferUsedSz);
}
else
{
b = bs_new(buf, buf_len);
}
cas->table_id = bs_read_u8(b);
if (cas->table_id != conditional_access_section)
{
LOG_ERROR_ARGS("Table ID in CAT is 0x%02X instead of expected 0x%02X",
cas->table_id, conditional_access_section);
reportAddErrorLogArgs("Table ID in CAT is 0x%02X instead of expected 0x%02X",
cas->table_id, conditional_access_section);
SAFE_REPORT_TS_ERR(-30);
resetPSITableBuffer(catBuffer);
bs_free (b);
return 0;
}
// read byte 0
cas->section_syntax_indicator = bs_read_u1(b);
if (!cas->section_syntax_indicator)
{
LOG_ERROR("section_syntax_indicator not set in CAT");
reportAddErrorLog("section_syntax_indicator not set in CAT");
SAFE_REPORT_TS_ERR(-31);
resetPSITableBuffer(catBuffer);
bs_free (b);
return 0;
}
bs_skip_u(b, 3); // TODO read the zero bit, check it to be zero
cas->section_length = bs_read_u(b, 12);
if (cas->section_length > 1021) // max CAT length
{
LOG_ERROR_ARGS("CAT section length is 0x%02X, larger than maximum allowed 0x%02X",
cas->section_length, MAX_SECTION_LEN);
reportAddErrorLogArgs("CAT section length is 0x%02X, larger than maximum allowed 0x%02X",
cas->section_length, MAX_SECTION_LEN);
SAFE_REPORT_TS_ERR(-32);
resetPSITableBuffer(catBuffer);
bs_free (b);
return 0;
}
if (cas->section_length > bs_bytes_left(b))
{
LOG_DEBUG ("conditional_access_section_read: Detected section spans more than one TS packet -- allocating buffer");
if (catBuffer->buffer != NULL)
{
// should never get here
LOG_ERROR ("conditional_access_section_read: unexpected catBufffer");
reportAddErrorLog ("conditional_access_section_read: unexpected catBufffer");
resetPSITableBuffer(catBuffer);
}
catBuffer->bufferAllocSz = cas->section_length + 3;
catBuffer->buffer = (uint8_t *)calloc (cas->section_length + 3, 1);
memcpy (catBuffer->buffer, buf, buf_len);
catBuffer->bufferUsedSz = buf_len;
bs_free (b);
return 0;
}
// read bytes 1-2
bs_read_u16(b);
// read bytes 3,4
bs_skip_u(b, 2);
cas->version_number = bs_read_u(b, 5);
cas->current_next_indicator = bs_read_u1(b);
if (!cas->current_next_indicator) LOG_WARN("This CAT is not yet applicable/n");
// read byte 5
cas->section_number = bs_read_u8(b);
cas->last_section_number = bs_read_u8(b);
if (cas->section_number != 0 || cas->last_section_number != 0) LOG_WARN("Multi-section CAT is not supported yet/n");
// read bytes 6,7
read_descriptor_loop(cas->descriptors, b, cas->section_length - 5 - 4 );
// explanation: section_length gives us the length from the end of section_length
// we used 5 bytes for the mandatory section fields, and will use another 4 bytes for CRC
// the remaining bytes contain descriptors, most probably only one
// again, it's much shorter in C :-)
cas->CRC_32 = bs_read_u32(b);
// check CRC
crc_t cas_crc = crc_init();
cas_crc = crc_update(cas_crc, buf, bs_pos(b) - 4);
cas_crc = crc_finalize(cas_crc);
if (cas_crc != cas->CRC_32)
{
LOG_ERROR_ARGS("CAT CRC_32 specified as 0x%08X, but calculated as 0x%08X", cas->CRC_32, cas_crc);
reportAddErrorLogArgs("CAT CRC_32 specified as 0x%08X, but calculated as 0x%08X", cas->CRC_32, cas_crc);
SAFE_REPORT_TS_ERR(-33);
resetPSITableBuffer(catBuffer);
bs_free (b);
return 0;
}
bs_free(b);
resetPSITableBuffer(catBuffer);
return 1;
}
/** Initializes the PBCH transmitter and receiver.
* At the receiver, the field nof_ports in the cell structure indicates the
* maximum number of BS transmitter ports to look for.
*/
int pbch_init(pbch_t *q, lte_cell_t cell) {
int ret = LIBLTE_ERROR_INVALID_INPUTS;
if (q != NULL &&
lte_cell_isvalid(&cell))
{
ret = LIBLTE_ERROR;
bzero(q, sizeof(pbch_t));
q->cell = cell;
if (modem_table_lte(&q->mod, LTE_QPSK, true)) {
goto clean;
}
demod_soft_init(&q->demod);
demod_soft_table_set(&q->demod, &q->mod);
demod_soft_alg_set(&q->demod, APPROX);
if (sequence_pbch(&q->seq_pbch, q->cell.cp, q->cell.id)) {
goto clean;
}
uint32_t poly[3] = { 0x6D, 0x4F, 0x57 };
if (viterbi_init(&q->decoder, viterbi_37, poly, 40, true)) {
goto clean;
}
if (crc_init(&q->crc, LTE_CRC16, 16)) {
goto clean;
}
q->encoder.K = 7;
q->encoder.R = 3;
q->encoder.tail_biting = true;
memcpy(q->encoder.poly, poly, 3 * sizeof(int));
q->nof_symbols = (CP_ISNORM(q->cell.cp)) ? PBCH_RE_CPNORM : PBCH_RE_CPEXT;
q->pbch_d = malloc(sizeof(cf_t) * q->nof_symbols);
if (!q->pbch_d) {
goto clean;
}
int i;
for (i = 0; i < q->cell.nof_ports; i++) {
q->ce[i] = malloc(sizeof(cf_t) * q->nof_symbols);
if (!q->ce[i]) {
goto clean;
}
q->pbch_x[i] = malloc(sizeof(cf_t) * q->nof_symbols);
if (!q->pbch_x[i]) {
goto clean;
}
q->pbch_symbols[i] = malloc(sizeof(cf_t) * q->nof_symbols);
if (!q->pbch_symbols[i]) {
goto clean;
}
}
q->pbch_llr = malloc(sizeof(float) * q->nof_symbols * 4 * 2);
if (!q->pbch_llr) {
goto clean;
}
q->temp = malloc(sizeof(float) * q->nof_symbols * 4 * 2);
if (!q->temp) {
goto clean;
}
q->pbch_rm_f = malloc(sizeof(float) * 120);
if (!q->pbch_rm_f) {
goto clean;
}
q->pbch_rm_b = malloc(sizeof(float) * q->nof_symbols * 4 * 2);
if (!q->pbch_rm_b) {
goto clean;
}
q->data = malloc(sizeof(char) * 40);
if (!q->data) {
goto clean;
}
q->data_enc = malloc(sizeof(char) * 120);
if (!q->data_enc) {
goto clean;
}
ret = LIBLTE_SUCCESS;
}
clean:
if (ret == LIBLTE_ERROR) {
pbch_free(q);
}
return ret;
}
int main(int argc,char *argv[])
{
vm_instance_t *vm;
#ifdef PROFILE
atexit(profiler_savestat);
#endif
printf("Cisco Router Simulation Platform (version %s)\n",sw_version);
printf("Copyright (c) 2005-2007 Christophe Fillot.\n");
printf("Build date: %s %s\n\n",__DATE__,__TIME__);
/* Register platforms */
register_default_platforms();
/* Initialize timers */
timer_init();
/* Initialize object registry */
registry_init();
/* Initialize ATM module (for HEC checksums) */
atm_init();
/* Initialize CRC functions */
crc_init();
/* Initialize NetIO code */
netio_rxl_init();
/* Initialize NetIO packet filters */
netio_filter_load_all();
/* Initialize VTTY code */
vtty_init();
/* Parse standard command line */
if (!run_hypervisor(argc,argv))
parse_std_cmd_line(argc,argv);
/* Create general log file */
create_log_file();
/* Periodic tasks initialization */
if (ptask_init(0) == -1)
exit(EXIT_FAILURE);
/* Create instruction lookup tables */
mips64_jit_create_ilt();
mips64_exec_create_ilt();
ppc32_jit_create_ilt();
ppc32_exec_create_ilt();
setup_signals();
if (!hypervisor_mode) {
/* Initialize the default instance */
vm = vm_acquire("default");
assert(vm != NULL);
if (vm->platform->init_instance(vm) == -1) {
fprintf(stderr,"Unable to initialize router instance.\n");
exit(EXIT_FAILURE);
}
/* Start GDB server before the image to allow debugging from
the begining of it's execution */
if (vm->gdb_server_running)
{
/* Stop main CPU */
vm_suspend(vm);
// cpu_stop(vm->boot_cpu);
if (gdb_server_start_listener(vm) < 0) {
fprintf(stderr,"GDB server unable to create TCP sockets.\n");
exit(EXIT_FAILURE);
}
}
#if (DEBUG_INSN_PERF_CNT > 0) || (DEBUG_BLOCK_PERF_CNT > 0)
{
m_uint32_t counter,prev = 0,delta;
while(vm->status == VM_STATUS_RUNNING) {
counter = cpu_get_perf_counter(vm->boot_cpu);
delta = counter - prev;
prev = counter;
printf("delta = %u\n",delta);
sleep(1);
}
}
#else
/* Start instance monitoring */
vm_monitor(vm);
#endif
// FIXME: remove this kludge
if (vm->gdb_server_running)
{
//while (vm->gdb_conn->active)
// usleep(1000000);
gdb_server_close_control_sockets();
}
/* Free resources used by instance */
vm_release(vm);
} else {
hypervisor_tcp_server(hypervisor_ip_address,hypervisor_tcp_port);
}
dynamips_reset();
close_log_file();
return(0);
}
