streamtools/newpacket.cpp

   1 #include "newpacket.hpp"
   2 #include <cstdio>
   3 #include <iostream>
   4 #include <climits>
   5 #include <cstdlib>
   6 #include <cstring>
   7 #include <stdexcept>
   8 #include <sstream>
   9
  10 namespace
  11 {
  12         void handle_input_error(FILE* rcs)
  13         {
  14                 if(ferror(rcs))
  15                         throw std::runtime_error("Error reading input stream");
  16                 else if(feof(rcs))
  17                         throw std::runtime_error("Unexpected end of file reading input stream");
  18                 else
  19                         throw std::runtime_error("Unexpected short read reading input stream");
  20         }
  21
  22         bool read_input(FILE* rcs, unsigned char* buffer, size_t amount, int blank_ok)
  23         {
  24                 int r = fread(buffer, 1, amount, rcs);
  25                 if(r == (int)amount)
  26                         return true;
  27                 if(r == 0 && blank_ok)
  28                         return false;
  29                 handle_input_error(rcs);
  30                 return false;   //Notreached.
  31         }
  32
  33         channel& lookup_channel(std::vector<channel>& chantab, uint16_t chan)
  34         {
  35                 if(chan == 0xFFFF)
  36                         std::runtime_error("lookup_channel: Illegal channel 0xFFFF");
  37                 for(std::vector<channel>::iterator i = chantab.begin(); i != chantab.end(); ++i)
  38                         if(i->c_channel == chan)
  39                                 return *i;
  40                 std::stringstream str;
  41                 str << "lookup_channel: Channel " << chan << " not found";
  42                 throw std::runtime_error(str.str());
  43         }
  44
  45 #define CHANNEL_BITMASK_SIZE ((65536 + CHAR_BIT - 1) / CHAR_BIT)
  46
  47         void check_segment_table(const std::vector<channel>& table)
  48         {
  49                 unsigned char channelbits[CHANNEL_BITMASK_SIZE];
  50                 if(table.size() == 0)
  51                         throw std::runtime_error("check_segment_table: Zero channels in segment not allowed");
  52                 if(table.size() > 0xFFFF)
  53                         throw std::runtime_error("check_segment_table: Too many channels in segment (max 65535)");
  54                 for(size_t i = 0; i < CHANNEL_BITMASK_SIZE; i++)
  55                         channelbits[i] = 0;
  56                 for(std::vector<channel>::const_iterator i = table.begin(); i != table.end(); ++i) {
  57                         uint16_t num = i->c_channel;
  58                         if(num == 0xFFFF)
  59                                 throw std::runtime_error("Fatal: check_segment_table: Illegal channel 0xFFFF");
  60                         if(channelbits[num / CHAR_BIT] & (1 << (num % CHAR_BIT))) {
  61                                 std::stringstream str;
  62                                 str << "check_segment_table: Duplicate channel " << num;
  63                                 throw std::runtime_error(str.str());
  64                         }
  65                         channelbits[num / CHAR_BIT] |= (1 << (num % CHAR_BIT));
  66                         if(i->c_channel_name.length() > 0xFFFF) {
  67                                 std::stringstream str;
  68                                 str << "check_segment_Table: Channel name too long (" << i->c_channel_name.length()
  69                                         << " bytes, max 65535)";
  70                                 throw std::runtime_error(str.str());
  71                         }
  72                 }
  73         }
  74
  75 #define CHAN_MAXNAME 65535
  76
  77         void read_segment_table_entry(read_channel& rc, FILE* rc_stream, channel& chan)
  78         {
  79                 unsigned char hdr[6 + CHAN_MAXNAME];
  80                 read_input(rc_stream, hdr, 6, 0);
  81                 size_t namelen = ((uint16_t)hdr[4] << 8) | (uint16_t)hdr[5];
  82                 read_input(rc_stream, hdr + 6, namelen, 0);
  83                 chan.c_channel = ((uint16_t)hdr[0] << 8) | (uint16_t)hdr[1];
  84                 chan.c_type = ((uint16_t)hdr[2] << 8) | (uint16_t)hdr[3];
  85                 chan.c_channel_name.resize(namelen);
  86                 for(size_t i = 0; i < namelen; i++)
  87                         chan.c_channel_name[i] = hdr[6 + i];
  88                 chan.c_channel_perm = rc.number_for_channel(chan.c_channel_name);
  89         }
  90
  91         void read_segment_table(std::vector<channel>& chantab, FILE* rc_stream, read_channel& rc)
  92         {
  93                 unsigned char x[2];
  94                 uint16_t i;
  95
  96                 read_input(rc_stream, x, 2, 0);
  97                 uint16_t chans = ((uint16_t)x[0] << 8) | (uint16_t)x[1];
  98                 if(chans == 0)
  99                         throw std::runtime_error("read_segment_table: 0 channel segments not allowed");
 100
 101                 chantab.resize(chans);
 102                 for(i = 0; i < chans; i++)
 103                         read_segment_table_entry(rc, rc_stream, chantab[i]);
 104                 check_segment_table(chantab);
 105         }
 106
 107
 108 #define SPECIAL_TIMESKIP 0
 109 #define SPECIAL_TIMESKIP_STR "\xFF\xFF\xFF\xFF"
 110 #define SPECIAL_NEWSEGMENT 1
 111 #define SPECIAL_NEWSEGMENT_STR "JPCRRMULTIDUMP"
 112
 113         struct special_entry
 114         {
 115                 int se_num;
 116                 const char* se_spec;
 117         } specials[] = {
 118                 {SPECIAL_TIMESKIP, SPECIAL_TIMESKIP_STR},
 119                 {SPECIAL_NEWSEGMENT, SPECIAL_NEWSEGMENT_STR},
 120                 {-1, NULL}
 121         };
 122
 123 #define MAXSPECIALLEN 4096
 124
 125         static int read_special(FILE* rcs)
 126         {
 127                 char buf[MAXSPECIALLEN];
 128                 size_t readcount = 0;
 129                 while(1) {
 130                         unsigned char ch;
 131                         read_input(rcs, &ch, 1, 0);
 132                         buf[readcount++] = (char)ch;
 133                         struct special_entry* e = specials;
 134                         int matched = 0;
 135                         while(e->se_spec) {
 136                                 if(readcount < strlen(e->se_spec) && !strncmp(buf, e->se_spec, readcount))
 137                                         matched = 1;
 138                                 if(readcount == strlen(e->se_spec) && !strncmp(buf, e->se_spec, readcount))
 139                                         return e->se_num;
 140                                 e++;
 141                         }
 142                         if(!matched)
 143                                 break;
 144                 }
 145                 throw std::runtime_error("read_special: Bad special");
 146         }
 147 }
 148
 149 read_channel::~read_channel()
 150 {
 151         fclose(rc_stream);
 152 }
 153
 154 read_channel::read_channel(const std::string& filename)
 155 {
 156         rc_stream = fopen(filename.c_str(), "rb");
 157         if(!rc_stream) {
 158                 std::stringstream str;
 159                 str << "read_channel: Can't open '" << filename << "' for reading";
 160                 throw std::runtime_error(str.str());
 161         }
 162         rc_last_timestamp = 0;
 163         rc_eof_flag = false;
 164         rc_segmenttable_coming = false;
 165         rc_next_permchan = 0;
 166 }
 167
 168 uint32_t read_channel::number_for_channel(const std::string& name)
 169 {
 170         if(rc_permchans.count(name))
 171                 return rc_permchans[name];
 172         return rc_permchans[name] = rc_next_permchan++;
 173 }
 174
 175 struct packet* read_channel::read()
 176 {
 177         struct packet* ret = NULL;
 178         unsigned char packetheader[7];
 179
 180         if(rc_segmenttable_coming) {
 181                 //This is the segment channel table.
 182                 read_segment_table(rc_channels, rc_stream, *this);
 183                 rc_segmenttable_coming = 0;
 184                 goto restart;
 185         }
 186
 187         if(rc_eof_flag)
 188                 return NULL;
 189
 190         //Read the channel number.
 191         if(!read_input(rc_stream, packetheader, 2, 1)) {
 192                 rc_eof_flag = 1;
 193                 return NULL;    //Stream ends.
 194         }
 195         if(packetheader[0] == 0xFF && packetheader[1] == 0xFF) {
 196                 //Special.
 197                 int specialtype = read_special(rc_stream);
 198                 if(specialtype == SPECIAL_TIMESKIP)
 199                         rc_last_timestamp += 0xFFFFFFFFU;
 200                 else if(specialtype == SPECIAL_NEWSEGMENT)
 201                         rc_segmenttable_coming = 1;
 202
 203                 //If we don't have channel table nor channel table won't be next, then the magic is bad (not
 204                 //a valid dump).
 205                 if(!rc_channels.size() && !rc_segmenttable_coming)
 206                         throw std::runtime_error("read_channel: Bad magic");
 207         } else {
 208                 //The first element must be of special type (segment start).
 209                 if(!rc_channels.size())
 210                         throw std::runtime_error("read_channel: Bad magic");
 211                 uint16_t chan = ((uint16_t)packetheader[0] << 8) | (uint16_t)packetheader[1];
 212                 struct channel& c = lookup_channel(rc_channels, chan);
 213                 //Read next 5 bytes of header (that far is safe).
 214                 read_input(rc_stream, packetheader + 2, 5, 0);
 215                 size_t payload = 0;
 216                 ret = new packet();
 217                 unsigned char tmp;
 218
 219                 try {
 220                         do {
 221                                 read_input(rc_stream, &tmp, 1, 0);
 222                                 if(((size_t)0 - 1) / 128 <= payload)
 223                                         throw std::runtime_error("read_channel: Packet payload too large");
 224                                 payload = 128 * payload + (tmp & 0x7F);
 225                         } while(tmp & 0x80);
 226
 227                         ret->rp_payload.resize(payload);
 228                         read_input(rc_stream, &ret->rp_payload[0], payload, 0);
 229                         ret->rp_channel = chan;
 230                         ret->rp_channel_perm = c.c_channel_perm;
 231                         ret->rp_major = c.c_type;
 232                         ret->rp_minor = packetheader[6];
 233                         uint32_t timedelta = ((uint32_t)packetheader[2] << 24) | ((uint32_t)packetheader[3] << 16) |
 234                                 ((uint32_t)packetheader[4] << 8) | ((uint32_t)packetheader[5]);
 235                         ret->rp_timestamp = (rc_last_timestamp += timedelta);
 236                         ret->rp_channel_name = c.c_channel_name;
 237                 } catch(...) {
 238                         delete ret;
 239                         throw;
 240                 }
 241         }
 242
 243         //Try again if return value would be NULL.
 244 restart:
 245         if(!ret)
 246                 return this->read();
 247         else
 248                 return ret;
 249 }
 250
 251 write_channel::write_channel(const std::string& filename)
 252 {
 253         wc_stream = fopen(filename.c_str(), "wb");
 254         if(!wc_stream) {
 255                 std::stringstream str;
 256                 str << "write_channel: Can't open '" << filename << "' for writing";
 257                 throw std::runtime_error(str.str());
 258         }
 259         wc_last_timestamp = 0;
 260 }
 261
 262 write_channel::~write_channel()
 263 {
 264         fclose(wc_stream);
 265 }
 266
 267 void write_channel::start_segment(const std::vector<channel>& channels)
 268 {
 269         unsigned char x[] = {0xFF, 0xFF, 'J', 'P', 'C', 'R', 'R', 'M', 'U', 'L', 'T', 'I', 'D', 'U', 'M', 'P'};
 270         unsigned char chanbuf[6 + 65535];       //Any channel entry fits in this.
 271
 272         check_segment_table(channels);
 273
 274         //Write new segment start.
 275         if(fwrite(x, 16, 1, wc_stream) < 1)
 276                 throw std::runtime_error("write_channel: Error writing output stream");
 277
 278         //Write new segment channel table.
 279         chanbuf[0] = (unsigned char)(channels.size() >> 8);
 280         chanbuf[1] = (unsigned char)(channels.size());
 281         if(fwrite(chanbuf, 2, 1, wc_stream) < 1)
 282                 throw std::runtime_error("write_channel: Error writing output stream (channel count)");
 283
 284         for(std::vector<channel>::const_iterator i = channels.begin(); i != channels.end(); ++i) {
 285                 size_t len;
 286
 287                 chanbuf[0] = (unsigned char)(i->c_channel >> 8);
 288                 chanbuf[1] = (unsigned char)(i->c_channel);
 289                 chanbuf[2] = (unsigned char)(i->c_type >> 8);
 290                 chanbuf[3] = (unsigned char)(i->c_type);
 291                 chanbuf[4] = (unsigned char)(i->c_channel_name.length() >> 8);
 292                 chanbuf[5] = (unsigned char)(i->c_channel_name.length());
 293                 for(size_t j = 0; j < i->c_channel_name.length(); j++)
 294                         chanbuf[6 + j] = i->c_channel_name[j];
 295                 len = 6 + i->c_channel_name.length();
 296                 if(fwrite(chanbuf, len, 1, wc_stream) < 1)
 297                         throw std::runtime_error("write_channel: Error writing output stream (channel entry)");
 298         }
 299         wc_channels = channels;
 300 }
 301
 302 void write_channel::write(struct packet& p)
 303 {
 304         unsigned char packetheaders[17];
 305         if(!wc_channels.size())
 306                 throw std::runtime_error("write_channel: Attempt to write outside segment");
 307         if(p.rp_timestamp < wc_last_timestamp) {
 308                 std::stringstream str;
 309                 str << "write_channel: Non-monotonic timestream (" << p.rp_timestamp << "<" << wc_last_timestamp
 310                         << ")";
 311                 throw std::runtime_error(str.str());
 312         }
 313         uint64_t deltatime = p.rp_timestamp - wc_last_timestamp;
 314         while(deltatime > 0xFFFFFFFFULL) {
 315                 //Timeskip.
 316                 packetheaders[0] = 0xFF;
 317                 packetheaders[1] = 0xFF;
 318                 packetheaders[2] = 0xFF;
 319                 packetheaders[3] = 0xFF;
 320                 packetheaders[4] = 0xFF;
 321                 packetheaders[5] = 0xFF;
 322                 if(fwrite(packetheaders, 6, 1, wc_stream) < 1)
 323                         throw std::runtime_error("write_channel: Error writing output stream (delay)");
 324                 deltatime -= 0xFFFFFFFFU;
 325         }
 326         lookup_channel(wc_channels, p.rp_channel);
 327         size_t hdrlen = 7, counter;
 328         uint64_t tmplen = p.rp_payload.size();
 329
 330         //Compose the actual headers.
 331         packetheaders[0] = (unsigned char)(p.rp_channel >> 8);
 332         packetheaders[1] = (unsigned char)(p.rp_channel);
 333         packetheaders[2] = (unsigned char)(deltatime >> 24);
 334         packetheaders[3] = (unsigned char)(deltatime >> 16);
 335         packetheaders[4] = (unsigned char)(deltatime >> 8);
 336         packetheaders[5] = (unsigned char)(deltatime);
 337         packetheaders[6] = (unsigned char)(p.rp_minor);
 338         bool wflag = false;
 339         for(counter = 9; counter <= 9; counter--) {
 340                 unsigned shift = 7 * counter;
 341                 unsigned char bias = shift ? 0x80 : 0x00;
 342                 if(tmplen >= (1ULL << shift) || !shift || wflag) {
 343                         packetheaders[hdrlen++] = bias + ((tmplen >> shift) & 0x7F);
 344                         tmplen &= ((1ULL << shift) - 1);
 345                         wflag = true;
 346                 }
 347         }
 348
 349         if(hdrlen && fwrite(packetheaders, hdrlen, 1, wc_stream) < 1)
 350                 throw std::runtime_error("write_channel: Error writing output stream (packet headers)");
 351         //Write the actual payload.
 352         if(p.rp_payload.size() && fwrite(&p.rp_payload[0], p.rp_payload.size(), 1, wc_stream) < 1)
 353                 throw std::runtime_error("write_channel: Error writing output stream (payload)");
 354         wc_last_timestamp = p.rp_timestamp;
 355 }