Fix integer overflow by checking size against header_size

Note that the problem occurs when data_size is less than header_size
what causes a buffer overflow in &data[i]

Co-Authored-By: D4N <dan.cermak@cgc-instruments.com>