void crc_writeheader( unsigned char *bitstream, int bit_count )
{
unsigned int crc = 0xffff; /* (jo) init crc16 for error_protection */
int whole_bytes = (bit_count>>3);
int byte;
// Calculate the CRC on the second two bytes of the header
crc = crc_update(bitstream[2], crc, 8);
crc = crc_update(bitstream[3], crc, 8);
// Calculate CRC on whole bytes after CRC
for(byte=6; byte<(whole_bytes+6); byte++)
{
crc = crc_update(bitstream[byte], crc, 8);
}
// Calculate CRC on remaining bits
if(bit_count & 7)
{
crc = crc_update(bitstream[byte], crc, bit_count&7);
}
// Insert the CRC into the 16-bits after the header
bitstream[4] = crc >> 8;
bitstream[5] = crc & 0xFF;
}
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;
}
bool RoboClaw::GetPinFunctions(uint8_t address, uint8_t &S3mode, uint8_t &S4mode, uint8_t &S5mode){
uint8_t crc;
bool valid = false;
uint8_t val1,val2,val3;
uint8_t trys=MAXRETRY;
int16_t data;
do{
flush();
crc_clear();
write(address);
crc_update(address);
write(GETPINFUNCTIONS);
crc_update(GETPINFUNCTIONS);
data = read(timeout);
crc_update(data);
val1=data;
if(data!=-1){
data = read(timeout);
crc_update(data);
val2=data;
}
if(data!=-1){
data = read(timeout);
crc_update(data);
val3=data;
}
if(data!=-1){
uint16_t ccrc;
data = read(timeout);
if(data!=-1){
ccrc = data << 8;
data = read(timeout);
if(data!=-1){
ccrc |= data;
if(crc_get()==ccrc){
S3mode = val1;
S4mode = val2;
S5mode = val3;
return true;
}
}
}
}
}while(trys--);
return false;
}
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;
}
/* 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;
}
uint16_t RoboClaw::Read2(uint8_t address,uint8_t cmd,bool *valid){
uint8_t crc;
if(valid)
*valid = false;
uint16_t value=0;
uint8_t trys=MAXRETRY;
int16_t data;
do{
flush();
crc_clear();
write(address);
crc_update(address);
write(cmd);
crc_update(cmd);
data = read(timeout);
crc_update(data);
value=(uint16_t)data<<8;
if(data!=-1){
data = read(timeout);
crc_update(data);
value|=(uint16_t)data;
}
if(data!=-1){
uint16_t ccrc;
data = read(timeout);
if(data!=-1){
ccrc = data << 8;
data = read(timeout);
if(data!=-1){
ccrc |= data;
if(crc_get()==ccrc){
*valid = true;
return value;
}
}
}
}
}while(trys--);
return false;
}
/* detect if a block is used in a particular pattern */
static int
check_crc(const u_char *S, const u_char *buf, u_int32_t len)
{
u_int32_t crc;
const u_char *c;
crc = 0;
for (c = buf; c < buf + len; c += SSH_BLOCKSIZE) {
if (!CMP(S, c)) {
crc_update(&crc, 1);
crc_update(&crc, 0);
} else {
crc_update(&crc, 0);
crc_update(&crc, 0);
}
}
return crc == 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);
}
// Bypass buffer
BON_INLINE void bon_write_to_writer(bon_w_doc* B, const void* data, bon_size n)
{
if (!B->writer(B->userData, data, n)) {
B->error = BON_ERR_WRITE_ERROR;
}
if (B->flags & BON_W_FLAG_CRC) {
B->crc_inv = crc_update(B->crc_inv, (const uint8_t*)data, n);
}
}
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;
}
bool RoboClaw::ReadVersion(uint8_t address,char *version){
uint8_t data;
uint8_t trys=MAXRETRY;
do{
flush();
data = 0;
crc_clear();
write(address);
crc_update(address);
write(GETVERSION);
crc_update(GETVERSION);
uint8_t i;
for(i=0;i<48;i++){
if(data!=-1){
data=read(timeout);
version[i] = data;
crc_update(version[i]);
if(version[i]==0){
uint16_t ccrc;
data = read(timeout);
if(data!=-1){
ccrc = data << 8;
data = read(timeout);
if(data!=-1){
ccrc |= data;
return crc_get()==ccrc;
}
}
break;
}
}
else{
break;
}
}
}while(trys--);
return false;
}
//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);
}
void crc_update_dw(unsigned long ulData)
{
int iCnt;
iCnt = 3;
do
{
crc_update((unsigned char)(ulData&0xff));
ulData >>= 8U;
} while( --iCnt>=0 );
}
/**
* 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;
}
void bon_w_footer(bon_w_doc* B)
{
if (B->flags & BON_W_FLAG_CRC)
{
// Add contribution of buffered data:
B->crc_inv = crc_update(B->crc_inv, B->buff, B->buff_ix);
uint32_t crc = B->crc_inv ^ 0xffffffff;
uint32_t crc_le = uint32_to_le(crc);
bon_w_raw_uint8 (B, BON_CTRL_FOOTER_CRC);
bon_w_raw_uint32(B, crc_le);
bon_w_raw_uint8 (B, BON_CTRL_FOOTER_CRC);
}
else
{
bon_w_raw_uint8 (B, BON_CTRL_FOOTER);
}
}
TEST_RESULT_T test(TEST_PARAMETER_T *ptTestParam)
{
TEST_RESULT_T tTestResult;
CRCTEST_PARAMETER_T *ptTestParams;
const unsigned char *pucCnt;
const unsigned char *pucEnd;
unsigned long ulCrc32;
systime_init();
uprintf("\f. *** CRC test by [email protected] ***\n");
uprintf("V" VERSION_ALL "\n\n");
/* Switch off SYS led. */
rdy_run_setLEDs(RDYRUN_OFF);
/* Get the test parameter. */
ptTestParams = (CRCTEST_PARAMETER_T*)(ptTestParam->pvInitParams);
/* Build the CRC for the requested area. */
pucCnt = ptTestParams->pucAreaStart;
pucEnd = ptTestParams->pucAreaEnd;
uprintf(". area start: 0x%08x\n", ptTestParams->pucAreaStart);
uprintf(". area end : 0x%08x\n\n", ptTestParams->pucAreaEnd);
uprintf("Calculating CRC32 ...\n");
crc_init_crc32();
while( pucCnt<pucEnd )
{
crc_update( *(pucCnt++) );
}
ulCrc32 = crc_get_crc32();
ptTestParams->ulCrc32 = ulCrc32;
uprintf("CRC32 = 0x%08x\n", ulCrc32);
rdy_run_setLEDs(RDYRUN_GREEN);
tTestResult = TEST_RESULT_OK;
return tTestResult;
}
bool RoboClaw::write_n(uint8_t cnt, ... )
{
uint8_t trys=MAXRETRY;
do{
crc_clear();
//send data with crc
va_list marker;
va_start( marker, cnt ); /* Initialize variable arguments. */
for(uint8_t index=0;index<cnt;index++){
uint8_t data = va_arg(marker, uint16_t);
crc_update(data);
write(data);
}
va_end( marker ); /* Reset variable arguments. */
uint16_t crc = crc_get();
write(crc>>8);
write(crc);
if(read(timeout)==0xFF)
return true;
}while(trys--);
return false;
}
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;
}
//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;
}
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);
}
}
/* detect if a block is used in a particular pattern */
static int check_crc(uchar *S, uchar *buf, uint32 len, uchar *IV)
{
uint32 crc;
uchar *c;
crc = 0;
if (IV && !CMP(S, IV)) {
crc_update(&crc, ONE);
crc_update(&crc, ZERO);
}
for (c = buf; c < buf + len; c += SSH_BLOCKSIZE) {
if (!CMP(S, c)) {
crc_update(&crc, ONE);
crc_update(&crc, ZERO);
} else {
crc_update(&crc, ZERO);
crc_update(&crc, ZERO);
}
}
return (crc == 0);
}
int check_crc(unsigned char *S, unsigned char *buf, word32 len, unsigned char *IV)
{
word32 crc;
unsigned char *c;
crc = 0;
if (IV && !CMP(S, IV)) {
crc_update(&crc, 1);
crc_update(&crc, 0);
}
for (c = buf; c < buf + len; c += SSH_BLOCKSIZE) {
if (!CMP(S, c)) {
crc_update(&crc, 1);
crc_update(&crc, 0);
} else {
crc_update(&crc, 0);
crc_update(&crc, 0);
}
}
return (crc == 0);
}
/* detect if a block is used in a particular pattern */
static int
check_crc(u_char *S, u_char *buf, u_int32_t len,
u_char *IV)
{
u_int32_t crc;
u_char *c;
crc = 0;
if (IV && !CMP(S, IV)) {
crc_update(&crc, 1);
crc_update(&crc, 0);
}
for (c = buf; c < buf + len; c += SSH_BLOCKSIZE) {
if (!CMP(S, c)) {
crc_update(&crc, 1);
crc_update(&crc, 0);
} else {
crc_update(&crc, 0);
crc_update(&crc, 0);
}
}
return (crc == 0);
}
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;
}
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;
}
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;
}
void lux_hal_crc(uint8_t byte){
crc = crc_update(crc, &byte, 1);
}
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) {
uint8_t tmp[2048];
