Changeset 360 for trunk/libfreebob/src/libstreaming/IsoHandler.cpp
- Timestamp:
- 12/30/06 11:49:46 (17 years ago)
- Files:
-
- trunk/libfreebob/src/libstreaming/IsoHandler.cpp (modified) (11 diffs)
Legend:
- Unmodified
- Added
- Removed
- Modified
- Copied
- Moved
trunk/libfreebob/src/libstreaming/IsoHandler.cpp
r341 r360 29 29 #include "IsoHandler.h" 30 30 #include "IsoStream.h" 31 #include "cyclecounter.h" 32 33 #include "libutil/Time.h" 34 #include "libutil/TimeSource.h" 35 #include "libutil/SystemTimeSource.h" 36 31 37 #include <errno.h> 32 38 #include <netinet/in.h> … … 36 42 #include <iostream> 37 43 using namespace std; 44 45 46 #define CC_SLEEP_TIME_AFTER_UPDATE 100 47 #define CC_SLEEP_TIME_AFTER_FAILURE 10 48 #define CC_DLL_COEFF ((0.01)*((float)(CC_SLEEP_TIME_AFTER_UPDATE/1000.0))) 49 50 #define CC_MAX_RATE_ERROR (2/100.0) 51 #define CC_INIT_MAX_TRIES 10 38 52 39 53 … … 79 93 80 94 /* Base class implementation */ 95 IsoHandler::IsoHandler(int port) 96 : TimeSource(), m_handle(0), m_handle_util(0), m_port(port), 97 m_buf_packets(400), m_max_packet_size(1024), m_irq_interval(-1), 98 m_cyclecounter_ticks(0), m_lastmeas_usecs(0), m_ticks_per_usec(24.576), 99 m_ticks_per_usec_dll_err2(0), 100 m_packetcount(0), m_dropped(0), m_Client(0) 101 { 102 InitTime(); 103 m_TimeSource=new FreebobUtil::SystemTimeSource(); 104 } 105 106 IsoHandler::IsoHandler(int port, unsigned int buf_packets, unsigned int max_packet_size, int irq) 107 : TimeSource(), m_handle(0), m_port(port), 108 m_buf_packets(buf_packets), m_max_packet_size( max_packet_size), 109 m_irq_interval(irq), 110 m_cyclecounter_ticks(0), m_lastmeas_usecs(0), m_ticks_per_usec(24.576), 111 m_ticks_per_usec_dll_err2(0), 112 m_packetcount(0), m_dropped(0), m_Client(0) 113 { 114 InitTime(); 115 m_TimeSource=new FreebobUtil::SystemTimeSource(); 116 } 81 117 82 118 IsoHandler::~IsoHandler() { … … 87 123 if(m_handle_util) raw1394_destroy_handle(m_handle_util); 88 124 125 delete m_TimeSource; 89 126 } 90 127 … … 127 164 raw1394_set_bus_reset_handler(m_handle, busreset_handler); 128 165 166 // initialize the local timesource 167 m_TimeSource_NbCycleWraps=0; 168 quadlet_t buf=0; 169 unsigned int new_counter; 170 171 raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util), 172 CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf); 173 174 new_counter= ntohl(buf) & 0xFFFFFFFF; 175 m_TimeSource_LastSecs=CYCLE_COUNTER_GET_SECS(new_counter); 176 129 177 // update the cycle counter value for initial value 130 updateCycleCounter(); 131 132 return true; 178 initCycleCounter(); 179 180 return true; 181 } 182 183 bool 184 IsoHandler::setSyncMaster(FreebobUtil::TimeSource *t) 185 { 186 m_TimeSource=t; 187 188 // update the cycle counter value for initial value 189 initCycleCounter(); 190 191 return true; 133 192 } 134 193 … … 141 200 142 201 /** 143 * Returns the current value of the cycle counter202 * Bus reset handler 144 203 * 145 * @return the current value of the cycle counter204 * @return ? 146 205 */ 147 #define CSR_CYCLE_TIME 0x200 148 #define CSR_REGISTER_BASE 0xfffff0000000ULL 149 150 #define CYCLE_COUNTER_GET_SECS(x) (((x & 0xFE000000) >> 25)) 151 #define CYCLE_COUNTER_GET_CYCLES(x) (((x & 0x01FFF000) >> 12)) 152 #define CYCLE_COUNTER_GET_TICKS(x) (((x & 0x00000FFF))) 153 #define CYCLE_COUNTER_TO_TICKS(x) ((CYCLE_COUNTER_GET_SECS(x) * 24576000) +\ 154 (CYCLE_COUNTER_GET_CYCLES(x) * 3072) +\ 155 (CYCLE_COUNTER_GET_TICKS(x) )) 206 207 int IsoHandler::handleBusReset(unsigned int generation) { 208 debugOutput( DEBUG_LEVEL_VERBOSE, "bus reset...\n"); 209 210 // as busreset can elect a new cycle master, 211 // we need to re-initialize our timing code 212 initCycleCounter(); 213 214 return 0; 215 } 216 217 /** 218 * Returns the current value of the cycle counter (in ticks) 219 * 220 * @return the current value of the cycle counter (in ticks) 221 */ 156 222 157 223 unsigned int IsoHandler::getCycleCounter() { 158 return m_cyclecounter; 159 } 160 161 void IsoHandler::updateCycleCounter() { 224 // calculate the cycle counter based upon the current time 225 // and the estimated tick rate 226 freebob_microsecs_t now=m_TimeSource->getCurrentTimeAsUsecs(); 227 228 // linear interpolation 229 int delta_usecs=now-m_lastmeas_usecs; 230 231 float offset=m_ticks_per_usec * ((float)delta_usecs); 232 233 unsigned int pred_ticks=m_cyclecounter_ticks+(unsigned int)offset; 234 235 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"Get CC: d_usecs=%d, offset=%f, cc_ticks=%lu, pred_ticks=%lu\n", 236 delta_usecs, offset, m_cyclecounter_ticks,pred_ticks 237 ); 238 239 // if we need to wrap, do it 240 if (pred_ticks > TICKS_PER_SECOND * 128) { 241 pred_ticks -= TICKS_PER_SECOND * 128; 242 } 243 244 return pred_ticks; 245 } 246 247 bool IsoHandler::updateCycleCounter() { 162 248 quadlet_t buf=0; 249 250 freebob_microsecs_t prev_usecs=m_lastmeas_usecs; 251 unsigned int prev_ticks=m_cyclecounter_ticks; 252 253 freebob_microsecs_t new_usecs; 254 unsigned int new_ticks; 255 unsigned int new_counter; 256 257 /* To estimate the cycle counter, we implement a 258 DLL based routine, that maps the cycle counter 259 on the system clock. 260 261 For more info, refer to: 262 "Using a DLL to filter time" 263 Fons Adriaensen 264 265 Can be found at: 266 http://users.skynet.be/solaris/linuxaudio/downloads/usingdll.pdf 267 or maybe at: 268 http://www.kokkinizita.net/linuxaudio 269 270 Basically what we do is estimate the next point (T1,CC1_est) 271 based upon the previous point (T0, CC0) and the estimated rate (R). 272 Then we compare our estimation with the measured cycle counter 273 at T1 (=CC1_meas). We then calculate the estimation error on R: 274 err=(CC1_meas-CC0)/(T1-T2) - (CC1_est-CC0)/(T1-T2) 275 and try to minimize this on average (DLL) 276 277 Note that in order to have a contignous mapping, we should 278 update CC0<=CC1_est instead of CC0<=CC1_meas. The measurement 279 serves only to correct the error 'on average'. 280 281 In the code, the following variable names are used: 282 T0=prev_usecs 283 T1=next_usecs 284 285 CC0=prev_ticks 286 CC1_est=est_ticks 287 CC1_meas=meas_ticks 288 289 */ 163 290 164 291 // normally we should be able to use the same handle … … 167 294 raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util), 168 295 CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf); 169 170 m_cyclecounter= ntohl(buf) & 0xFFFFFFFF; 171 172 // debugOutput(DEBUG_LEVEL_VERBOSE,"Updating timestamp: %08X (%2u sec + %2u cycles + %04u ticks)\n", 173 // m_cyclecounter, 174 // CYCLE_COUNTER_GET_SECS(m_cyclecounter), 175 // CYCLE_COUNTER_GET_CYCLES(m_cyclecounter), 176 // CYCLE_COUNTER_GET_TICKS(m_cyclecounter) 177 // ); 178 179 180 usleep(100); 296 new_usecs=m_TimeSource->getCurrentTimeAsUsecs(); 297 298 new_counter= ntohl(buf) & 0xFFFFFFFF; 299 new_ticks=CYCLE_COUNTER_TO_TICKS(new_counter); 300 301 // the difference in system time 302 int delta_usecs=new_usecs-prev_usecs; 303 // this cannot be 0, because m_TimeSource->getCurrentTimeAsUsecs should 304 // never return the same value (maybe in future terrahz processors?) 305 assert(delta_usecs); 306 307 // the measured cycle counter difference 308 unsigned int delta_ticks_meas; 309 if (new_ticks > prev_ticks) { 310 delta_ticks_meas=new_ticks - prev_ticks; 311 } else { // wraparound 312 delta_ticks_meas=CYCLE_COUNTER_UNWRAP_TICKS(new_ticks) - prev_ticks; 313 } 314 315 // the estimated cycle counter difference 316 unsigned int delta_ticks_est=(unsigned int)(m_ticks_per_usec * ((float)delta_usecs)); 317 318 // the measured & estimated rate 319 float rate_meas=((float)delta_ticks_meas/(float)delta_usecs); 320 float rate_est=((float)m_ticks_per_usec); 321 322 // these make sure we don't update when the measurement is 323 // bad. We know the nominal rate, and it can't be that far 324 // off. The thing is that there is a problem in measuring 325 // both usecs and ticks at the same time (no provision in 326 // the kernel. 327 // We know that there are some tolerances on both 328 // the system clock and the firewire clock such that the 329 // actual difference is rather small. So we discard values 330 // that are too far from the nominal rate. 331 // Otherwise the DLL has to have a very low bandwidth, in 332 // order not to be desturbed too much by these bad measurements 333 // resulting in very slow locking. 334 335 if ( (rate_meas < 24.576*(1.0+CC_MAX_RATE_ERROR)) 336 && (rate_meas > 24.576*(1.0-CC_MAX_RATE_ERROR))) { 337 338 #ifdef DEBUG 339 340 int diff=(int)delta_ticks_est; 341 342 // calculate the difference in predicted ticks and 343 // measured ticks 344 diff -= delta_ticks_meas; 345 346 347 if (diff > 24000 || diff < -24000) { // approx +/-1 msec error 348 debugOutput(DEBUG_LEVEL_VERBOSE,"Bad pred (%p): diff=%d, dt_est=%u, dt_meas=%u, d=%dus, err=%fus\n", this, 349 diff, delta_ticks_est, delta_ticks_meas, delta_usecs, (((float)diff)/24.576) 350 ); 351 } else { 352 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"Good pred: diff=%d, dt_est=%u, dt_meas=%u, d=%dus, err=%fus\n", 353 diff, delta_ticks_est, delta_ticks_meas, delta_usecs, (((float)diff)/24.576) 354 ); 355 } 356 #endif 357 // DLL the error to obtain the rate. 358 // (note: the DLL makes the error=0) 359 // only update the DLL if the rate is within 10% of the expected 360 // rate 361 float err=rate_meas-rate_est; 362 363 // 2nd order DLL update 364 // const float w=6.28*0.0001; 365 // const float b=w*1.45; 366 // const float c=w*w; 367 // 368 // m_ticks_per_usec += b*err + m_ticks_per_usec_dll_err2; 369 // m_ticks_per_usec_dll_err2 += c * err; 370 371 // first order DLL update 372 m_ticks_per_usec += CC_DLL_COEFF*err; 373 374 if ( (m_ticks_per_usec > 24.576*(1.0+CC_MAX_RATE_ERROR)) 375 || (m_ticks_per_usec < 24.576*(1.0-CC_MAX_RATE_ERROR))) { 376 debugOutput(DEBUG_LEVEL_VERBOSE, "Warning: DLL ticks/usec near clipping (%8.4f)\n", 377 m_ticks_per_usec); 378 } 379 380 // update the internal values 381 // note: the next cyclecounter point is 382 // the estimated one, not the measured one! 383 m_cyclecounter_ticks += delta_ticks_est; 384 // if we need to wrap, do it 385 if (m_cyclecounter_ticks > TICKS_PER_SECOND * 128) { 386 m_cyclecounter_ticks -= TICKS_PER_SECOND * 128; 387 } 388 389 m_lastmeas_usecs = new_usecs; 390 391 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"U TS: %10u -> %10u, d=%7uus, dt_est=%7u, dt_meas=%7u, erate=%6.4f, mrate=%6f\n", 392 prev_ticks, m_cyclecounter_ticks, delta_usecs, 393 delta_ticks_est, delta_ticks_meas, m_ticks_per_usec, rate_meas 394 ); 395 396 // the estimate is good 397 return true; 398 } else { 399 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"U TS: Not updating, rate out of range (%6.4f)\n", 400 rate_meas 401 ); 402 return false; 403 404 } 405 } 406 407 void IsoHandler::initCycleCounter() { 408 quadlet_t buf=0; 409 410 freebob_microsecs_t prev_usecs; 411 unsigned int prev_ticks; 412 unsigned int prev_counter; 413 414 freebob_microsecs_t new_usecs; 415 unsigned int new_ticks; 416 unsigned int new_counter; 417 418 float rate=0.0; 419 420 unsigned int try_cnt=0; 421 422 // make sure that we start with a decent rate, 423 // meaning that we want two successive (usecs,ticks) 424 // points that make sense. 425 426 while ( (try_cnt++ < CC_INIT_MAX_TRIES) && 427 ( (rate > 24.576*(1.0+CC_MAX_RATE_ERROR)) 428 || (rate < 24.576*(1.0-CC_MAX_RATE_ERROR)))) { 429 430 // normally we should be able to use the same handle 431 // because it is not iterated on by any other stuff 432 // but I'm not sure 433 raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util), 434 CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf); 435 prev_usecs=m_TimeSource->getCurrentTimeAsUsecs(); 436 437 prev_counter= ntohl(buf) & 0xFFFFFFFF; 438 prev_ticks=CYCLE_COUNTER_TO_TICKS(prev_counter); 439 440 usleep(CC_SLEEP_TIME_AFTER_UPDATE); 441 442 // normally we should be able to use the same handle 443 // because it is not iterated on by any other stuff 444 // but I'm not sure 445 raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util), 446 CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf); 447 new_usecs=m_TimeSource->getCurrentTimeAsUsecs(); 448 449 new_counter= ntohl(buf) & 0xFFFFFFFF; 450 new_ticks=CYCLE_COUNTER_TO_TICKS(new_counter); 451 452 unsigned int delta_ticks; 453 454 if (new_ticks > prev_ticks) { 455 delta_ticks=new_ticks - prev_ticks; 456 } else { // wraparound 457 delta_ticks=CYCLE_COUNTER_UNWRAP_TICKS(new_ticks) - prev_ticks; 458 } 459 460 int delta_usecs=new_usecs-prev_usecs; 461 462 // this cannot be 0, because m_TimeSource->getCurrentTimeAsUsecs should 463 // never return the same value (maybe in future terrahz processors?) 464 assert(delta_usecs); 465 466 rate=((float)delta_ticks/(float)delta_usecs); 467 468 // update the internal values 469 m_cyclecounter_ticks=new_ticks; 470 m_lastmeas_usecs=new_usecs; 471 472 debugOutput(DEBUG_LEVEL_VERBOSE,"Try %d: rate=%6.4f\n", 473 try_cnt,rate 474 ); 475 476 } 477 478 // this is not fatal, the DLL will eventually correct this 479 if(try_cnt == CC_INIT_MAX_TRIES) { 480 debugWarning("Failed to properly initialize cycle counter...\n"); 481 } 482 483 // initialize this to the nominal value 484 m_ticks_per_usec = 24.576; 485 m_ticks_per_usec_dll_err2 = 0; 486 181 487 } 182 488 … … 189 495 debugOutputShort( DEBUG_LEVEL_NORMAL, " Handler type : %s\n", 190 496 (this->getType()==EHT_Receive ? "Receive" : "Transmit")); 191 debugOutputShort( DEBUG_LEVEL_NORMAL, " Port, Channel : % d, %d\n",497 debugOutputShort( DEBUG_LEVEL_NORMAL, " Port, Channel : %2d, %2d\n", 192 498 m_port, channel); 193 debugOutputShort( DEBUG_LEVEL_NORMAL, " Packet count : % d (%d dropped)\n\n",499 debugOutputShort( DEBUG_LEVEL_NORMAL, " Packet count : %10d (%5d dropped)\n", 194 500 this->getPacketCount(), this->getDroppedCount()); 501 #ifdef DEBUG 502 unsigned int cc=this->getCycleCounter(); 503 debugOutputShort( DEBUG_LEVEL_NORMAL, " Cycle counter : %10lu (%03us, %04ucycles, %04uticks)\n", 504 cc,TICKS_TO_SECS(cc),TICKS_TO_CYCLES(cc),TICKS_TO_OFFSET(cc)); 505 #endif 506 debugOutputShort( DEBUG_LEVEL_NORMAL, " Ticks/usec : %8.6f (dll2: %8.6e)\n\n", 507 this->getTicksPerUsec(), m_ticks_per_usec_dll_err2); 195 508 196 509 }; … … 235 548 236 549 } 550 551 /* The timesource interface */ 552 freebob_microsecs_t IsoHandler::getCurrentTime() { 553 quadlet_t buf=0; 554 unsigned int new_counter; 555 556 raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util), 557 CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf); 558 559 new_counter= ntohl(buf) & 0xFFFFFFFF; 560 561 // this assumes that it never happens that there are more than 2 562 // minutes between calls 563 if (CYCLE_COUNTER_GET_SECS(new_counter) < m_TimeSource_LastSecs) { 564 m_TimeSource_NbCycleWraps++; 565 } 566 567 freebob_microsecs_t ticks=m_TimeSource_NbCycleWraps * 128 * TICKS_PER_SECOND 568 + CYCLE_COUNTER_TO_TICKS(new_counter); 569 570 m_TimeSource_LastSecs=CYCLE_COUNTER_GET_SECS(new_counter); 571 572 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"Wraps=%4u, LastSecs=%3u, nowSecs=%3u, ticks=%10u\n", 573 m_TimeSource_NbCycleWraps, m_TimeSource_LastSecs, 574 CYCLE_COUNTER_GET_SECS(new_counter), ticks 575 ); 576 577 return ticks; 578 } 579 580 freebob_microsecs_t IsoHandler::getCurrentTimeAsUsecs() { 581 float tmp=getCurrentTime(); 582 float tmp2 = tmp * USECS_PER_TICK; 583 freebob_microsecs_t retval=(freebob_microsecs_t)tmp2; 584 585 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"tmp=%f, tmp2=%f, retval=%u\n", 586 tmp, tmp2,retval 587 ); 588 589 return retval; 590 } 591 592 237 593 238 594 /* Child class implementations */ … … 329 685 int IsoRecvHandler::handleBusReset(unsigned int generation) { 330 686 debugOutput( DEBUG_LEVEL_VERBOSE, "handle bus reset...\n"); 687 331 688 //TODO: implement busreset 689 690 // pass on the busreset signal 691 if(IsoHandler::handleBusReset(generation)) { 692 return -1; 693 } 332 694 return 0; 333 695 } … … 439 801 debugOutput( DEBUG_LEVEL_VERBOSE, "bus reset...\n"); 440 802 //TODO: implement busreset 803 804 // pass on the busreset signal 805 if(IsoHandler::handleBusReset(generation)) { 806 return -1; 807 } 808 441 809 return 0; 442 810 }