streamtools/newpacket.cpp

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