src/core/controllerframe.cpp

   1 #include "core/controllerframe.hpp"
   2
   3 #define SYSTEM_BYTES 5
   4
   5 namespace
   6 {
   7         std::set<porttype_info*>& porttypes()
   8         {
   9                 static std::set<porttype_info*> p;
  10                 return p;
  11         }
  12 }
  13
  14 const porttype_info& porttype_info::lookup(porttype_t p) throw(std::runtime_error)
  15 {
  16         for(auto i : porttypes())
  17                 if(p == i->value)
  18                         return *i;
  19         throw std::runtime_error("Bad port type");
  20 }
  21
  22 const porttype_info& porttype_info::lookup(const std::string& p) throw(std::runtime_error)
  23 {
  24         for(auto i : porttypes())
  25                 if(p == i->name)
  26                         return *i;
  27         throw std::runtime_error("Bad port type");
  28 }
  29
  30 porttype_info::~porttype_info() throw()
  31 {
  32         porttypes().erase(this);
  33 }
  34
  35 porttype_info::porttype_info(porttype_t ptype, const std::string& pname, size_t psize) throw(std::bad_alloc)
  36 {
  37         value = ptype;
  38         name = pname;
  39         storage_size = psize;
  40         porttypes().insert(this);
  41 }
  42
  43 pollcounter_vector::pollcounter_vector() throw()
  44 {
  45         clear();
  46 }
  47
  48 void pollcounter_vector::clear() throw()
  49 {
  50         system_flag = false;
  51         memset(ctrs, 0, sizeof(ctrs));
  52 }
  53
  54 void pollcounter_vector::set_all_DRDY() throw()
  55 {
  56         for(size_t i = 0; i < MAX_BUTTONS ; i++)
  57                 ctrs[i] |= 0x80000000UL;
  58 }
  59
  60 #define INDEXOF(pid, ctrl) ((pid) * MAX_CONTROLS_PER_CONTROLLER + (ctrl))
  61
  62 void pollcounter_vector::clear_DRDY(unsigned pid, unsigned ctrl) throw()
  63 {
  64         ctrs[INDEXOF(pid, ctrl)] &= 0x7FFFFFFFUL;
  65 }
  66
  67 bool pollcounter_vector::get_DRDY(unsigned pid, unsigned ctrl) throw()
  68 {
  69         return ((ctrs[INDEXOF(pid, ctrl)] & 0x80000000UL) != 0);
  70 }
  71
  72 bool pollcounter_vector::has_polled() throw()
  73 {
  74         uint32_t res = system_flag ? 1 : 0;
  75         for(size_t i = 0; i < MAX_BUTTONS ; i++)
  76                 res |= ctrs[i];
  77         return ((res & 0x7FFFFFFFUL) != 0);
  78 }
  79
  80 uint32_t pollcounter_vector::get_polls(unsigned pid, unsigned ctrl) throw()
  81 {
  82         return ctrs[INDEXOF(pid, ctrl)] & 0x7FFFFFFFUL;
  83 }
  84
  85 uint32_t pollcounter_vector::increment_polls(unsigned pid, unsigned ctrl) throw()
  86 {
  87         size_t i = INDEXOF(pid, ctrl);
  88         uint32_t x = ctrs[i] & 0x7FFFFFFFUL;
  89         ++ctrs[i];
  90         return x;
  91 }
  92
  93 void pollcounter_vector::set_system() throw()
  94 {
  95         system_flag = true;
  96 }
  97
  98 bool pollcounter_vector::get_system() throw()
  99 {
 100         return system_flag;
 101 }
 102
 103 uint32_t pollcounter_vector::max_polls() throw()
 104 {
 105         uint32_t max = system_flag ? 1 : 0;
 106         for(unsigned i = 0; i < MAX_BUTTONS; i++) {
 107                 uint32_t tmp = ctrs[i] & 0x7FFFFFFFUL;
 108                 max = (max < tmp) ? tmp : max;
 109         }
 110         return max;
 111 }
 112
 113 void pollcounter_vector::save_state(std::vector<uint32_t>& mem) throw(std::bad_alloc)
 114 {
 115         mem.resize(4 + MAX_BUTTONS );
 116         //Compatiblity fun.
 117         mem[0] = 0x80000000UL;
 118         mem[1] = system_flag ? 1 : 0x80000000UL;
 119         mem[2] = system_flag ? 1 : 0x80000000UL;
 120         mem[3] = system_flag ? 1 : 0x80000000UL;
 121         for(size_t i = 0; i < MAX_BUTTONS ; i++)
 122                 mem[4 + i] = ctrs[i];
 123 }
 124
 125 void pollcounter_vector::load_state(const std::vector<uint32_t>& mem) throw()
 126 {
 127         system_flag = (mem[1] | mem[2] | mem[3]) & 0x7FFFFFFFUL;
 128         for(size_t i = 0; i < MAX_BUTTONS ; i++)
 129                 ctrs[i] = mem[i + 4];
 130 }
 131
 132 bool pollcounter_vector::check(const std::vector<uint32_t>& mem) throw()
 133 {
 134         return (mem.size() == MAX_BUTTONS  + 4);
 135 }
 136
 137
 138 controller_frame::controller_frame(porttype_t p1, porttype_t p2) throw(std::runtime_error)
 139 {
 140         memset(memory, 0, sizeof(memory));
 141         backing = memory;
 142         types[0] = p1;
 143         types[1] = p2;
 144         set_types(types);
 145 }
 146
 147 controller_frame::controller_frame(unsigned char* mem, porttype_t p1, porttype_t p2) throw(std::runtime_error)
 148 {
 149         if(!mem)
 150                 throw std::runtime_error("NULL backing memory not allowed");
 151         memset(memory, 0, sizeof(memory));
 152         backing = mem;
 153         types[0] = p1;
 154         types[1] = p2;
 155         set_types(types);
 156 }
 157
 158 controller_frame::controller_frame(const controller_frame& obj) throw()
 159 {
 160         memset(memory, 0, sizeof(memory));
 161         backing = memory;
 162         set_types(obj.types);
 163         memcpy(backing, obj.backing, totalsize);
 164 }
 165
 166 controller_frame& controller_frame::operator=(const controller_frame& obj) throw(std::runtime_error)
 167 {
 168         set_types(obj.types);
 169         memcpy(backing, obj.backing, totalsize);
 170 }
 171
 172 void controller_frame::set_types(const porttype_t* tarr)
 173 {
 174         for(unsigned i = 0; i < MAX_PORTS; i++) {
 175                 if(memory != backing && types[i] != tarr[i])
 176                         throw std::runtime_error("Controller_frame: Type mismatch");
 177                 if(!porttype_info::lookup(tarr[i]).legal(i))
 178                         throw std::runtime_error("Illegal port type for port index");
 179         }
 180         size_t offset = SYSTEM_BYTES;
 181         for(unsigned i = 0; i < MAX_PORTS; i++) {
 182                 offsets[i] = offset;
 183                 types[i] = tarr[i];
 184                 pinfo[i] = &porttype_info::lookup(tarr[i]);
 185                 offset += pinfo[i]->storage_size;
 186         }
 187         totalsize = offset;
 188 }
 189
 190 size_t controller_frame_vector::walk_helper(size_t frame, bool sflag) throw()
 191 {
 192         size_t ret = sflag ? frame : 0;
 193         if(frame >= frames)
 194                 return ret;
 195         frame++;
 196         ret++;
 197         size_t page = frame / frames_per_page;
 198         size_t offset = frame_size * (frame % frames_per_page);
 199         size_t index = frame % frames_per_page;
 200         if(cache_page_num != page) {
 201                 cache_page = &pages[page];
 202                 cache_page_num = page;
 203         }
 204         while(frame < frames) {
 205                 if(index == frames_per_page) {
 206                         page++;
 207                         cache_page = &pages[page];
 208                         cache_page_num = page;
 209                 }
 210                 if(controller_frame::sync(cache_page->content + offset))
 211                         break;
 212                 index++;
 213                 offset += frame_size;
 214                 frame++;
 215                 ret++;
 216         }
 217         return ret;
 218 }
 219
 220 size_t controller_frame_vector::count_frames() throw()
 221 {
 222         size_t ret = 0;
 223         if(!frames)
 224                 return 0;
 225         cache_page_num = 0;
 226         cache_page = &pages[0];
 227         size_t offset = 0;
 228         size_t index = 0;
 229         for(size_t i = 0; i < frames; i++) {
 230                 if(index == frames_per_page) {
 231                         cache_page_num++;
 232                         cache_page = &cache_page[cache_page_num];
 233                         index = 0;
 234                         offset = 0;
 235                 }
 236                 if(controller_frame::sync(cache_page->content + offset))
 237                         ret++;
 238                 index++;
 239                 offset += frame_size;
 240
 241         }
 242         return ret;
 243 }
 244
 245 void controller_frame_vector::clear(enum porttype_t p1, enum porttype_t p2) throw(std::runtime_error)
 246 {
 247         controller_frame check(p1, p2);
 248         frame_size = check.size();
 249         frames_per_page = CONTROLLER_PAGE_SIZE / frame_size;
 250         frames = 0;
 251         types[0] = p1;
 252         types[1] = p2;
 253         clear_cache();
 254         pages.clear();
 255 }
 256
 257 controller_frame_vector::~controller_frame_vector() throw()
 258 {
 259         pages.clear();
 260         cache_page = NULL;
 261 }
 262
 263 controller_frame_vector::controller_frame_vector(enum porttype_t p1, enum porttype_t p2) throw(std::runtime_error)
 264 {
 265         clear(p1, p2);
 266 }
 267
 268 void controller_frame_vector::append(controller_frame frame) throw(std::bad_alloc, std::runtime_error)
 269 {
 270         controller_frame check(types[0], types[1]);
 271         if(!check.types_match(frame))
 272                 throw std::runtime_error("controller_frame_vector::append: Type mismatch");
 273         if(frames % frames_per_page == 0) {
 274                 //Create new page.
 275                 pages[frames / frames_per_page];
 276         }
 277         //Write the entry.
 278         size_t page = frames / frames_per_page;
 279         size_t offset = frame_size * (frames % frames_per_page);
 280         cache_page_num = page;
 281         cache_page = &pages[page];
 282         controller_frame(cache_page->content + offset, types[0], types[1]) = frame;
 283         frames++;
 284 }
 285
 286 controller_frame_vector::controller_frame_vector(const controller_frame_vector& vector) throw(std::bad_alloc)
 287 {
 288         clear(vector.types[0], vector.types[1]);
 289         *this = vector;
 290 }
 291
 292 controller_frame_vector& controller_frame_vector::operator=(const controller_frame_vector& v)
 293         throw(std::bad_alloc)
 294 {
 295         if(this == &v)
 296                 return *this;
 297         resize(v.frames);
 298         clear_cache();
 299
 300         //Copy the fields.
 301         frame_size = v.frame_size;
 302         frames_per_page = v.frames_per_page;
 303         for(size_t i = 0; i < MAX_PORTS; i++)
 304                 types[i] = v.types[i];
 305
 306         //This can't fail anymore. Copy the raw page contents.
 307         size_t pagecount = (frames + frames_per_page - 1) / frames_per_page;
 308         for(size_t i = 0; i < pagecount; i++) {
 309                 page& pg = pages[i];
 310                 const page& pg2 = v.pages.find(i)->second;
 311                 pg = pg2;
 312         }
 313
 314         return *this;
 315 }
 316
 317 size_t controller_frame::system_serialize(const unsigned char* buffer, char* textbuf)
 318 {
 319         char tmp[128];
 320         if(buffer[1] || buffer[2] || buffer[3] || buffer[4])
 321                 sprintf(tmp, "%c%c %i %i", ((buffer[0] & 1) ? 'F' : '.'), ((buffer[0] & 2) ? 'R' : '.'),
 322                         unserialize_short(buffer + 1), unserialize_short(buffer + 3));
 323         else
 324                 sprintf(tmp, "%c%c", ((buffer[0] & 1) ? 'F' : '.'), ((buffer[0] & 2) ? 'R' : '.'));
 325         size_t len = strlen(tmp);
 326         memcpy(textbuf, tmp, len);
 327         return len;
 328 }
 329
 330 size_t controller_frame::system_deserialize(unsigned char* buffer, const char* textbuf)
 331 {
 332         buffer[0] = 0;
 333         size_t idx = 0;
 334         if(read_button_value(textbuf, idx))
 335                 buffer[0] |= 1;
 336         if(read_button_value(textbuf, idx))
 337                 buffer[0] |= 2;
 338         serialize_short(buffer + 1, read_axis_value(textbuf, idx));
 339         serialize_short(buffer + 3, read_axis_value(textbuf, idx));
 340         skip_rest_of_field(textbuf, idx, false);
 341         return idx;
 342 }
 343
 344 short read_axis_value(const char* buf, size_t& idx) throw()
 345 {
 346                 char ch;
 347                 //Skip ws.
 348                 while(is_nonterminator(buf[idx])) {
 349                         char ch = buf[idx];
 350                         if(ch != ' ' && ch != '\t')
 351                                 break;
 352                         idx++;
 353                 }
 354                 //Read the sign if any.
 355                 if(!is_nonterminator(buf[idx]))
 356                         return 0;
 357                 bool negative = false;
 358                 if(ch == '-') {
 359                         negative = true;
 360                         idx++;
 361                 }
 362                 if(ch == '+')
 363                         idx++;
 364
 365                 //Read numeric value.
 366                 int numval = 0;
 367                 while(!is_nonterminator(buf[idx]) && isdigit(static_cast<unsigned char>(ch = buf[idx]))) {
 368                         numval = numval * 10 + (ch - '0');
 369                         idx++;
 370                 }
 371                 if(negative)
 372                         numval = -numval;
 373
 374                 return static_cast<short>(numval);
 375 }
 376
 377 void controller_frame_vector::resize(size_t newsize) throw(std::bad_alloc)
 378 {
 379         clear_cache();
 380         if(newsize == 0) {
 381                 clear();
 382         } else if(newsize < frames) {
 383                 //Shrink movie.
 384                 size_t current_pages = (frames + frames_per_page - 1) / frames_per_page;
 385                 size_t pages_needed = (newsize + frames_per_page - 1) / frames_per_page;
 386                 for(size_t i = pages_needed; i < current_pages; i++)
 387                         pages.erase(i);
 388                 //Now zeroize the excess memory.
 389                 size_t offset = frame_size * (newsize % frames_per_page);
 390                 memset(pages[pages_needed - 1].content + offset, 0, CONTROLLER_PAGE_SIZE - offset);
 391                 frames = newsize;
 392         } else if(newsize > frames) {
 393                 //Enlarge movie.
 394                 size_t current_pages = (frames + frames_per_page - 1) / frames_per_page;
 395                 size_t pages_needed = (newsize + frames_per_page - 1) / frames_per_page;
 396                 //Create the needed pages.
 397                 for(size_t i = current_pages; i < pages_needed; i++) {
 398                         try {
 399                                 pages[i];
 400                         } catch(...) {
 401                                 for(size_t i = current_pages; i < pages_needed; i++)
 402                                         if(pages.count(i))
 403                                                 pages.erase(i);
 404                                 throw;
 405                         }
 406                 }
 407                 frames = newsize;
 408         }
 409 }
 410
 411 controller_frame::controller_frame() throw()
 412 {
 413         memset(memory, 0, sizeof(memory));
 414         backing = memory;
 415         for(unsigned i = 0; i < MAX_PORTS; i++) {
 416                 offsets[i] = SYSTEM_BYTES;
 417                 types[i] = PT_INVALID;
 418                 pinfo[i] = NULL;
 419         }
 420         totalsize = SYSTEM_BYTES;
 421 }
 422
 423 void controller_frame::set_port_type(unsigned port, porttype_t ptype) throw(std::runtime_error)
 424 {
 425         char tmp[MAXIMUM_CONTROLLER_FRAME_SIZE] = {0};
 426         if(!porttype_info::lookup(ptype).legal(port))
 427                 throw std::runtime_error("Illegal port type for port index");
 428         if(memory != backing)
 429                 throw std::runtime_error("Can't set port type on non-dedicated controller frame");
 430         if(port >= MAX_PORTS)
 431                 return;
 432         const porttype_info* newpinfo[MAX_PORTS];
 433         size_t newoffsets[MAX_PORTS];
 434         size_t offset = SYSTEM_BYTES;
 435         for(size_t i = 0; i < MAX_PORTS; i++) {
 436                 if(i != port)
 437                         newpinfo[i] = pinfo[i];
 438                 else
 439                         newpinfo[i] = &porttype_info::lookup(ptype);
 440                 newoffsets[i] = offset;
 441                 if(newpinfo[i])
 442                         offset += newpinfo[i]->storage_size;
 443                 if(i != port && newpinfo[i] && newpinfo[i]->storage_size)
 444                         memcpy(tmp + newoffsets[i], backing + offsets[i], newpinfo[i]->storage_size);
 445         }
 446         memcpy(memory, tmp, MAXIMUM_CONTROLLER_FRAME_SIZE);
 447         types[port] = ptype;
 448         pinfo[port] = newpinfo[port];
 449 }