|
@@ -406,300 +406,300 @@ void Planner::init() {
|
406
|
406
|
register const uint8_t* ptab = inv_tab;
|
407
|
407
|
|
408
|
408
|
__asm__ __volatile__(
|
409
|
|
- /* %8:%7:%6 = interval*/
|
410
|
|
- /* r31:r30: MUST be those registers, and they must point to the inv_tab */
|
|
409
|
+ // %8:%7:%6 = interval
|
|
410
|
+ // r31:r30: MUST be those registers, and they must point to the inv_tab
|
411
|
411
|
|
412
|
|
- " clr %13" "\n\t" /* %13 = 0 */
|
|
412
|
+ " clr %13" "\n\t" // %13 = 0
|
413
|
413
|
|
414
|
|
- /* Now we must compute */
|
415
|
|
- /* result = 0xFFFFFF / d */
|
416
|
|
- /* %8:%7:%6 = interval*/
|
417
|
|
- /* %16:%15:%14 = nr */
|
418
|
|
- /* %13 = 0*/
|
|
414
|
+ // Now we must compute
|
|
415
|
+ // result = 0xFFFFFF / d
|
|
416
|
+ // %8:%7:%6 = interval
|
|
417
|
+ // %16:%15:%14 = nr
|
|
418
|
+ // %13 = 0
|
419
|
419
|
|
420
|
|
- /* A plain division of 24x24 bits should take 388 cycles to complete. We will */
|
421
|
|
- /* use Newton-Raphson for the calculation, and will strive to get way less cycles*/
|
422
|
|
- /* for the same result - Using C division, it takes 500cycles to complete .*/
|
|
420
|
+ // A plain division of 24x24 bits should take 388 cycles to complete. We will
|
|
421
|
+ // use Newton-Raphson for the calculation, and will strive to get way less cycles
|
|
422
|
+ // for the same result - Using C division, it takes 500cycles to complete .
|
423
|
423
|
|
424
|
|
- " clr %3" "\n\t" /* idx = 0 */
|
|
424
|
+ " clr %3" "\n\t" // idx = 0
|
425
|
425
|
" mov %14,%6" "\n\t"
|
426
|
426
|
" mov %15,%7" "\n\t"
|
427
|
|
- " mov %16,%8" "\n\t" /* nr = interval */
|
428
|
|
- " tst %16" "\n\t" /* nr & 0xFF0000 == 0 ? */
|
429
|
|
- " brne 2f" "\n\t" /* No, skip this */
|
|
427
|
+ " mov %16,%8" "\n\t" // nr = interval
|
|
428
|
+ " tst %16" "\n\t" // nr & 0xFF0000 == 0 ?
|
|
429
|
+ " brne 2f" "\n\t" // No, skip this
|
430
|
430
|
" mov %16,%15" "\n\t"
|
431
|
|
- " mov %15,%14" "\n\t" /* nr <<= 8, %14 not needed */
|
432
|
|
- " subi %3,-8" "\n\t" /* idx += 8 */
|
433
|
|
- " tst %16" "\n\t" /* nr & 0xFF0000 == 0 ? */
|
434
|
|
- " brne 2f" "\n\t" /* No, skip this */
|
435
|
|
- " mov %16,%15" "\n\t" /* nr <<= 8, %14 not needed */
|
436
|
|
- " clr %15" "\n\t" /* We clear %14 */
|
437
|
|
- " subi %3,-8" "\n\t" /* idx += 8 */
|
438
|
|
-
|
439
|
|
- /* here %16 != 0 and %16:%15 contains at least 9 MSBits, or both %16:%15 are 0 */
|
|
431
|
+ " mov %15,%14" "\n\t" // nr <<= 8, %14 not needed
|
|
432
|
+ " subi %3,-8" "\n\t" // idx += 8
|
|
433
|
+ " tst %16" "\n\t" // nr & 0xFF0000 == 0 ?
|
|
434
|
+ " brne 2f" "\n\t" // No, skip this
|
|
435
|
+ " mov %16,%15" "\n\t" // nr <<= 8, %14 not needed
|
|
436
|
+ " clr %15" "\n\t" // We clear %14
|
|
437
|
+ " subi %3,-8" "\n\t" // idx += 8
|
|
438
|
+
|
|
439
|
+ // here %16 != 0 and %16:%15 contains at least 9 MSBits, or both %16:%15 are 0
|
440
|
440
|
"2:" "\n\t"
|
441
|
|
- " cpi %16,0x10" "\n\t" /* (nr & 0xf00000) == 0 ? */
|
442
|
|
- " brcc 3f" "\n\t" /* No, skip this */
|
443
|
|
- " swap %15" "\n\t" /* Swap nibbles */
|
444
|
|
- " swap %16" "\n\t" /* Swap nibbles. Low nibble is 0 */
|
|
441
|
+ " cpi %16,0x10" "\n\t" // (nr & 0xf00000) == 0 ?
|
|
442
|
+ " brcc 3f" "\n\t" // No, skip this
|
|
443
|
+ " swap %15" "\n\t" // Swap nibbles
|
|
444
|
+ " swap %16" "\n\t" // Swap nibbles. Low nibble is 0
|
445
|
445
|
" mov %14, %15" "\n\t"
|
446
|
|
- " andi %14,0x0f" "\n\t" /* Isolate low nibble */
|
447
|
|
- " andi %15,0xf0" "\n\t" /* Keep proper nibble in %15 */
|
448
|
|
- " or %16, %14" "\n\t" /* %16:%15 <<= 4 */
|
449
|
|
- " subi %3,-4" "\n\t" /* idx += 4 */
|
|
446
|
+ " andi %14,0x0f" "\n\t" // Isolate low nibble
|
|
447
|
+ " andi %15,0xf0" "\n\t" // Keep proper nibble in %15
|
|
448
|
+ " or %16, %14" "\n\t" // %16:%15 <<= 4
|
|
449
|
+ " subi %3,-4" "\n\t" // idx += 4
|
450
|
450
|
|
451
|
451
|
"3:" "\n\t"
|
452
|
|
- " cpi %16,0x40" "\n\t" /* (nr & 0xc00000) == 0 ? */
|
453
|
|
- " brcc 4f" "\n\t" /* No, skip this*/
|
|
452
|
+ " cpi %16,0x40" "\n\t" // (nr & 0xc00000) == 0 ?
|
|
453
|
+ " brcc 4f" "\n\t" // No, skip this
|
454
|
454
|
" add %15,%15" "\n\t"
|
455
|
455
|
" adc %16,%16" "\n\t"
|
456
|
456
|
" add %15,%15" "\n\t"
|
457
|
|
- " adc %16,%16" "\n\t" /* %16:%15 <<= 2 */
|
458
|
|
- " subi %3,-2" "\n\t" /* idx += 2 */
|
|
457
|
+ " adc %16,%16" "\n\t" // %16:%15 <<= 2
|
|
458
|
+ " subi %3,-2" "\n\t" // idx += 2
|
459
|
459
|
|
460
|
460
|
"4:" "\n\t"
|
461
|
|
- " cpi %16,0x80" "\n\t" /* (nr & 0x800000) == 0 ? */
|
462
|
|
- " brcc 5f" "\n\t" /* No, skip this */
|
|
461
|
+ " cpi %16,0x80" "\n\t" // (nr & 0x800000) == 0 ?
|
|
462
|
+ " brcc 5f" "\n\t" // No, skip this
|
463
|
463
|
" add %15,%15" "\n\t"
|
464
|
|
- " adc %16,%16" "\n\t" /* %16:%15 <<= 1 */
|
465
|
|
- " inc %3" "\n\t" /* idx += 1 */
|
|
464
|
+ " adc %16,%16" "\n\t" // %16:%15 <<= 1
|
|
465
|
+ " inc %3" "\n\t" // idx += 1
|
466
|
466
|
|
467
|
|
- /* Now %16:%15 contains its MSBit set to 1, or %16:%15 is == 0. We are now absolutely sure*/
|
468
|
|
- /* we have at least 9 MSBits available to enter the initial estimation table*/
|
|
467
|
+ // Now %16:%15 contains its MSBit set to 1, or %16:%15 is == 0. We are now absolutely sure
|
|
468
|
+ // we have at least 9 MSBits available to enter the initial estimation table
|
469
|
469
|
"5:" "\n\t"
|
470
|
470
|
" add %15,%15" "\n\t"
|
471
|
|
- " adc %16,%16" "\n\t" /* %16:%15 = tidx = (nr <<= 1), we lose the top MSBit (always set to 1, %16 is the index into the inverse table)*/
|
472
|
|
- " add r30,%16" "\n\t" /* Only use top 8 bits */
|
473
|
|
- " adc r31,%13" "\n\t" /* r31:r30 = inv_tab + (tidx) */
|
474
|
|
- " lpm %14, Z" "\n\t" /* %14 = inv_tab[tidx] */
|
475
|
|
- " ldi %15, 1" "\n\t" /* %15 = 1 %15:%14 = inv_tab[tidx] + 256 */
|
476
|
|
-
|
477
|
|
- /* We must scale the approximation to the proper place*/
|
478
|
|
- " clr %16" "\n\t" /* %16 will always be 0 here */
|
479
|
|
- " subi %3,8" "\n\t" /* idx == 8 ? */
|
480
|
|
- " breq 6f" "\n\t" /* yes, no need to scale*/
|
481
|
|
- " brcs 7f" "\n\t" /* If C=1, means idx < 8, result was negative!*/
|
482
|
|
-
|
483
|
|
- /* idx > 8, now %3 = idx - 8. We must perform a left shift. idx range:[1-8]*/
|
484
|
|
- " sbrs %3,0" "\n\t" /* shift by 1bit position?*/
|
485
|
|
- " rjmp 8f" "\n\t" /* No*/
|
|
471
|
+ " adc %16,%16" "\n\t" // %16:%15 = tidx = (nr <<= 1), we lose the top MSBit (always set to 1, %16 is the index into the inverse table)
|
|
472
|
+ " add r30,%16" "\n\t" // Only use top 8 bits
|
|
473
|
+ " adc r31,%13" "\n\t" // r31:r30 = inv_tab + (tidx)
|
|
474
|
+ " lpm %14, Z" "\n\t" // %14 = inv_tab[tidx]
|
|
475
|
+ " ldi %15, 1" "\n\t" // %15 = 1 %15:%14 = inv_tab[tidx] + 256
|
|
476
|
+
|
|
477
|
+ // We must scale the approximation to the proper place
|
|
478
|
+ " clr %16" "\n\t" // %16 will always be 0 here
|
|
479
|
+ " subi %3,8" "\n\t" // idx == 8 ?
|
|
480
|
+ " breq 6f" "\n\t" // yes, no need to scale
|
|
481
|
+ " brcs 7f" "\n\t" // If C=1, means idx < 8, result was negative!
|
|
482
|
+
|
|
483
|
+ // idx > 8, now %3 = idx - 8. We must perform a left shift. idx range:[1-8]
|
|
484
|
+ " sbrs %3,0" "\n\t" // shift by 1bit position?
|
|
485
|
+ " rjmp 8f" "\n\t" // No
|
486
|
486
|
" add %14,%14" "\n\t"
|
487
|
|
- " adc %15,%15" "\n\t" /* %15:16 <<= 1*/
|
|
487
|
+ " adc %15,%15" "\n\t" // %15:16 <<= 1
|
488
|
488
|
"8:" "\n\t"
|
489
|
|
- " sbrs %3,1" "\n\t" /* shift by 2bit position?*/
|
490
|
|
- " rjmp 9f" "\n\t" /* No*/
|
|
489
|
+ " sbrs %3,1" "\n\t" // shift by 2bit position?
|
|
490
|
+ " rjmp 9f" "\n\t" // No
|
491
|
491
|
" add %14,%14" "\n\t"
|
492
|
492
|
" adc %15,%15" "\n\t"
|
493
|
493
|
" add %14,%14" "\n\t"
|
494
|
|
- " adc %15,%15" "\n\t" /* %15:16 <<= 1*/
|
|
494
|
+ " adc %15,%15" "\n\t" // %15:16 <<= 1
|
495
|
495
|
"9:" "\n\t"
|
496
|
|
- " sbrs %3,2" "\n\t" /* shift by 4bits position?*/
|
497
|
|
- " rjmp 16f" "\n\t" /* No*/
|
498
|
|
- " swap %15" "\n\t" /* Swap nibbles. lo nibble of %15 will always be 0*/
|
499
|
|
- " swap %14" "\n\t" /* Swap nibbles*/
|
|
496
|
+ " sbrs %3,2" "\n\t" // shift by 4bits position?
|
|
497
|
+ " rjmp 16f" "\n\t" // No
|
|
498
|
+ " swap %15" "\n\t" // Swap nibbles. lo nibble of %15 will always be 0
|
|
499
|
+ " swap %14" "\n\t" // Swap nibbles
|
500
|
500
|
" mov %12,%14" "\n\t"
|
501
|
|
- " andi %12,0x0f" "\n\t" /* isolate low nibble*/
|
502
|
|
- " andi %14,0xf0" "\n\t" /* and clear it*/
|
503
|
|
- " or %15,%12" "\n\t" /* %15:%16 <<= 4*/
|
|
501
|
+ " andi %12,0x0f" "\n\t" // isolate low nibble
|
|
502
|
+ " andi %14,0xf0" "\n\t" // and clear it
|
|
503
|
+ " or %15,%12" "\n\t" // %15:%16 <<= 4
|
504
|
504
|
"16:" "\n\t"
|
505
|
|
- " sbrs %3,3" "\n\t" /* shift by 8bits position?*/
|
506
|
|
- " rjmp 6f" "\n\t" /* No, we are done */
|
|
505
|
+ " sbrs %3,3" "\n\t" // shift by 8bits position?
|
|
506
|
+ " rjmp 6f" "\n\t" // No, we are done
|
507
|
507
|
" mov %16,%15" "\n\t"
|
508
|
508
|
" mov %15,%14" "\n\t"
|
509
|
509
|
" clr %14" "\n\t"
|
510
|
510
|
" jmp 6f" "\n\t"
|
511
|
511
|
|
512
|
|
- /* idx < 8, now %3 = idx - 8. Get the count of bits */
|
|
512
|
+ // idx < 8, now %3 = idx - 8. Get the count of bits
|
513
|
513
|
"7:" "\n\t"
|
514
|
|
- " neg %3" "\n\t" /* %3 = -idx = count of bits to move right. idx range:[1...8]*/
|
515
|
|
- " sbrs %3,0" "\n\t" /* shift by 1 bit position ?*/
|
516
|
|
- " rjmp 10f" "\n\t" /* No, skip it*/
|
517
|
|
- " asr %15" "\n\t" /* (bit7 is always 0 here)*/
|
|
514
|
+ " neg %3" "\n\t" // %3 = -idx = count of bits to move right. idx range:[1...8]
|
|
515
|
+ " sbrs %3,0" "\n\t" // shift by 1 bit position ?
|
|
516
|
+ " rjmp 10f" "\n\t" // No, skip it
|
|
517
|
+ " asr %15" "\n\t" // (bit7 is always 0 here)
|
518
|
518
|
" ror %14" "\n\t"
|
519
|
519
|
"10:" "\n\t"
|
520
|
|
- " sbrs %3,1" "\n\t" /* shift by 2 bit position ?*/
|
521
|
|
- " rjmp 11f" "\n\t" /* No, skip it*/
|
522
|
|
- " asr %15" "\n\t" /* (bit7 is always 0 here)*/
|
|
520
|
+ " sbrs %3,1" "\n\t" // shift by 2 bit position ?
|
|
521
|
+ " rjmp 11f" "\n\t" // No, skip it
|
|
522
|
+ " asr %15" "\n\t" // (bit7 is always 0 here)
|
523
|
523
|
" ror %14" "\n\t"
|
524
|
|
- " asr %15" "\n\t" /* (bit7 is always 0 here)*/
|
|
524
|
+ " asr %15" "\n\t" // (bit7 is always 0 here)
|
525
|
525
|
" ror %14" "\n\t"
|
526
|
526
|
"11:" "\n\t"
|
527
|
|
- " sbrs %3,2" "\n\t" /* shift by 4 bit position ?*/
|
528
|
|
- " rjmp 12f" "\n\t" /* No, skip it*/
|
529
|
|
- " swap %15" "\n\t" /* Swap nibbles*/
|
530
|
|
- " andi %14, 0xf0" "\n\t" /* Lose the lowest nibble*/
|
531
|
|
- " swap %14" "\n\t" /* Swap nibbles. Upper nibble is 0*/
|
532
|
|
- " or %14,%15" "\n\t" /* Pass nibble from upper byte*/
|
533
|
|
- " andi %15, 0x0f" "\n\t" /* And get rid of that nibble*/
|
|
527
|
+ " sbrs %3,2" "\n\t" // shift by 4 bit position ?
|
|
528
|
+ " rjmp 12f" "\n\t" // No, skip it
|
|
529
|
+ " swap %15" "\n\t" // Swap nibbles
|
|
530
|
+ " andi %14, 0xf0" "\n\t" // Lose the lowest nibble
|
|
531
|
+ " swap %14" "\n\t" // Swap nibbles. Upper nibble is 0
|
|
532
|
+ " or %14,%15" "\n\t" // Pass nibble from upper byte
|
|
533
|
+ " andi %15, 0x0f" "\n\t" // And get rid of that nibble
|
534
|
534
|
"12:" "\n\t"
|
535
|
|
- " sbrs %3,3" "\n\t" /* shift by 8 bit position ?*/
|
536
|
|
- " rjmp 6f" "\n\t" /* No, skip it*/
|
|
535
|
+ " sbrs %3,3" "\n\t" // shift by 8 bit position ?
|
|
536
|
+ " rjmp 6f" "\n\t" // No, skip it
|
537
|
537
|
" mov %14,%15" "\n\t"
|
538
|
538
|
" clr %15" "\n\t"
|
539
|
|
- "6:" "\n\t" /* %16:%15:%14 = initial estimation of 0x1000000 / d*/
|
|
539
|
+ "6:" "\n\t" // %16:%15:%14 = initial estimation of 0x1000000 / d
|
540
|
540
|
|
541
|
|
- /* Now, we must refine the estimation present on %16:%15:%14 using 1 iteration*/
|
542
|
|
- /* of Newton-Raphson. As it has a quadratic convergence, 1 iteration is enough*/
|
543
|
|
- /* to get more than 18bits of precision (the initial table lookup gives 9 bits of*/
|
544
|
|
- /* precision to start from). 18bits of precision is all what is needed here for result */
|
|
541
|
+ // Now, we must refine the estimation present on %16:%15:%14 using 1 iteration
|
|
542
|
+ // of Newton-Raphson. As it has a quadratic convergence, 1 iteration is enough
|
|
543
|
+ // to get more than 18bits of precision (the initial table lookup gives 9 bits of
|
|
544
|
+ // precision to start from). 18bits of precision is all what is needed here for result
|
545
|
545
|
|
546
|
|
- /* %8:%7:%6 = d = interval*/
|
547
|
|
- /* %16:%15:%14 = x = initial estimation of 0x1000000 / d*/
|
548
|
|
- /* %13 = 0*/
|
549
|
|
- /* %3:%2:%1:%0 = working accumulator*/
|
|
546
|
+ // %8:%7:%6 = d = interval
|
|
547
|
+ // %16:%15:%14 = x = initial estimation of 0x1000000 / d
|
|
548
|
+ // %13 = 0
|
|
549
|
+ // %3:%2:%1:%0 = working accumulator
|
550
|
550
|
|
551
|
|
- /* Compute 1<<25 - x*d. Result should never exceed 25 bits and should always be positive*/
|
|
551
|
+ // Compute 1<<25 - x*d. Result should never exceed 25 bits and should always be positive
|
552
|
552
|
" clr %0" "\n\t"
|
553
|
553
|
" clr %1" "\n\t"
|
554
|
554
|
" clr %2" "\n\t"
|
555
|
|
- " ldi %3,2" "\n\t" /* %3:%2:%1:%0 = 0x2000000*/
|
556
|
|
- " mul %6,%14" "\n\t" /* r1:r0 = LO(d) * LO(x)*/
|
|
555
|
+ " ldi %3,2" "\n\t" // %3:%2:%1:%0 = 0x2000000
|
|
556
|
+ " mul %6,%14" "\n\t" // r1:r0 = LO(d) * LO(x)
|
557
|
557
|
" sub %0,r0" "\n\t"
|
558
|
558
|
" sbc %1,r1" "\n\t"
|
559
|
559
|
" sbc %2,%13" "\n\t"
|
560
|
|
- " sbc %3,%13" "\n\t" /* %3:%2:%1:%0 -= LO(d) * LO(x)*/
|
561
|
|
- " mul %7,%14" "\n\t" /* r1:r0 = MI(d) * LO(x)*/
|
|
560
|
+ " sbc %3,%13" "\n\t" // %3:%2:%1:%0 -= LO(d) * LO(x)
|
|
561
|
+ " mul %7,%14" "\n\t" // r1:r0 = MI(d) * LO(x)
|
562
|
562
|
" sub %1,r0" "\n\t"
|
563
|
563
|
" sbc %2,r1" "\n\t"
|
564
|
|
- " sbc %3,%13" "\n\t" /* %3:%2:%1:%0 -= MI(d) * LO(x) << 8*/
|
565
|
|
- " mul %8,%14" "\n\t" /* r1:r0 = HI(d) * LO(x)*/
|
|
564
|
+ " sbc %3,%13" "\n\t" // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
|
|
565
|
+ " mul %8,%14" "\n\t" // r1:r0 = HI(d) * LO(x)
|
566
|
566
|
" sub %2,r0" "\n\t"
|
567
|
|
- " sbc %3,r1" "\n\t" /* %3:%2:%1:%0 -= MIL(d) * LO(x) << 16*/
|
568
|
|
- " mul %6,%15" "\n\t" /* r1:r0 = LO(d) * MI(x)*/
|
|
567
|
+ " sbc %3,r1" "\n\t" // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
|
|
568
|
+ " mul %6,%15" "\n\t" // r1:r0 = LO(d) * MI(x)
|
569
|
569
|
" sub %1,r0" "\n\t"
|
570
|
570
|
" sbc %2,r1" "\n\t"
|
571
|
|
- " sbc %3,%13" "\n\t" /* %3:%2:%1:%0 -= LO(d) * MI(x) << 8*/
|
572
|
|
- " mul %7,%15" "\n\t" /* r1:r0 = MI(d) * MI(x)*/
|
|
571
|
+ " sbc %3,%13" "\n\t" // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
|
|
572
|
+ " mul %7,%15" "\n\t" // r1:r0 = MI(d) * MI(x)
|
573
|
573
|
" sub %2,r0" "\n\t"
|
574
|
|
- " sbc %3,r1" "\n\t" /* %3:%2:%1:%0 -= MI(d) * MI(x) << 16*/
|
575
|
|
- " mul %8,%15" "\n\t" /* r1:r0 = HI(d) * MI(x)*/
|
576
|
|
- " sub %3,r0" "\n\t" /* %3:%2:%1:%0 -= MIL(d) * MI(x) << 24*/
|
577
|
|
- " mul %6,%16" "\n\t" /* r1:r0 = LO(d) * HI(x)*/
|
|
574
|
+ " sbc %3,r1" "\n\t" // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
|
|
575
|
+ " mul %8,%15" "\n\t" // r1:r0 = HI(d) * MI(x)
|
|
576
|
+ " sub %3,r0" "\n\t" // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
|
|
577
|
+ " mul %6,%16" "\n\t" // r1:r0 = LO(d) * HI(x)
|
578
|
578
|
" sub %2,r0" "\n\t"
|
579
|
|
- " sbc %3,r1" "\n\t" /* %3:%2:%1:%0 -= LO(d) * HI(x) << 16*/
|
580
|
|
- " mul %7,%16" "\n\t" /* r1:r0 = MI(d) * HI(x)*/
|
581
|
|
- " sub %3,r0" "\n\t" /* %3:%2:%1:%0 -= MI(d) * HI(x) << 24*/
|
582
|
|
- /* %3:%2:%1:%0 = (1<<25) - x*d [169]*/
|
|
579
|
+ " sbc %3,r1" "\n\t" // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
|
|
580
|
+ " mul %7,%16" "\n\t" // r1:r0 = MI(d) * HI(x)
|
|
581
|
+ " sub %3,r0" "\n\t" // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
|
|
582
|
+ // %3:%2:%1:%0 = (1<<25) - x*d [169]
|
583
|
583
|
|
584
|
|
- /* We need to multiply that result by x, and we are only interested in the top 24bits of that multiply*/
|
|
584
|
+ // We need to multiply that result by x, and we are only interested in the top 24bits of that multiply
|
585
|
585
|
|
586
|
|
- /* %16:%15:%14 = x = initial estimation of 0x1000000 / d*/
|
587
|
|
- /* %3:%2:%1:%0 = (1<<25) - x*d = acc*/
|
588
|
|
- /* %13 = 0 */
|
|
586
|
+ // %16:%15:%14 = x = initial estimation of 0x1000000 / d
|
|
587
|
+ // %3:%2:%1:%0 = (1<<25) - x*d = acc
|
|
588
|
+ // %13 = 0
|
589
|
589
|
|
590
|
|
- /* result = %11:%10:%9:%5:%4*/
|
591
|
|
- " mul %14,%0" "\n\t" /* r1:r0 = LO(x) * LO(acc)*/
|
|
590
|
+ // result = %11:%10:%9:%5:%4
|
|
591
|
+ " mul %14,%0" "\n\t" // r1:r0 = LO(x) * LO(acc)
|
592
|
592
|
" mov %4,r1" "\n\t"
|
593
|
593
|
" clr %5" "\n\t"
|
594
|
594
|
" clr %9" "\n\t"
|
595
|
595
|
" clr %10" "\n\t"
|
596
|
|
- " clr %11" "\n\t" /* %11:%10:%9:%5:%4 = LO(x) * LO(acc) >> 8*/
|
597
|
|
- " mul %15,%0" "\n\t" /* r1:r0 = MI(x) * LO(acc)*/
|
|
596
|
+ " clr %11" "\n\t" // %11:%10:%9:%5:%4 = LO(x) * LO(acc) >> 8
|
|
597
|
+ " mul %15,%0" "\n\t" // r1:r0 = MI(x) * LO(acc)
|
598
|
598
|
" add %4,r0" "\n\t"
|
599
|
599
|
" adc %5,r1" "\n\t"
|
600
|
600
|
" adc %9,%13" "\n\t"
|
601
|
601
|
" adc %10,%13" "\n\t"
|
602
|
|
- " adc %11,%13" "\n\t" /* %11:%10:%9:%5:%4 += MI(x) * LO(acc) */
|
603
|
|
- " mul %16,%0" "\n\t" /* r1:r0 = HI(x) * LO(acc)*/
|
|
602
|
+ " adc %11,%13" "\n\t" // %11:%10:%9:%5:%4 += MI(x) * LO(acc)
|
|
603
|
+ " mul %16,%0" "\n\t" // r1:r0 = HI(x) * LO(acc)
|
604
|
604
|
" add %5,r0" "\n\t"
|
605
|
605
|
" adc %9,r1" "\n\t"
|
606
|
606
|
" adc %10,%13" "\n\t"
|
607
|
|
- " adc %11,%13" "\n\t" /* %11:%10:%9:%5:%4 += MI(x) * LO(acc) << 8*/
|
|
607
|
+ " adc %11,%13" "\n\t" // %11:%10:%9:%5:%4 += MI(x) * LO(acc) << 8
|
608
|
608
|
|
609
|
|
- " mul %14,%1" "\n\t" /* r1:r0 = LO(x) * MIL(acc)*/
|
|
609
|
+ " mul %14,%1" "\n\t" // r1:r0 = LO(x) * MIL(acc)
|
610
|
610
|
" add %4,r0" "\n\t"
|
611
|
611
|
" adc %5,r1" "\n\t"
|
612
|
612
|
" adc %9,%13" "\n\t"
|
613
|
613
|
" adc %10,%13" "\n\t"
|
614
|
|
- " adc %11,%13" "\n\t" /* %11:%10:%9:%5:%4 = LO(x) * MIL(acc)*/
|
615
|
|
- " mul %15,%1" "\n\t" /* r1:r0 = MI(x) * MIL(acc)*/
|
|
614
|
+ " adc %11,%13" "\n\t" // %11:%10:%9:%5:%4 = LO(x) * MIL(acc)
|
|
615
|
+ " mul %15,%1" "\n\t" // r1:r0 = MI(x) * MIL(acc)
|
616
|
616
|
" add %5,r0" "\n\t"
|
617
|
617
|
" adc %9,r1" "\n\t"
|
618
|
618
|
" adc %10,%13" "\n\t"
|
619
|
|
- " adc %11,%13" "\n\t" /* %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 8*/
|
620
|
|
- " mul %16,%1" "\n\t" /* r1:r0 = HI(x) * MIL(acc)*/
|
|
619
|
+ " adc %11,%13" "\n\t" // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 8
|
|
620
|
+ " mul %16,%1" "\n\t" // r1:r0 = HI(x) * MIL(acc)
|
621
|
621
|
" add %9,r0" "\n\t"
|
622
|
622
|
" adc %10,r1" "\n\t"
|
623
|
|
- " adc %11,%13" "\n\t" /* %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 16*/
|
|
623
|
+ " adc %11,%13" "\n\t" // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 16
|
624
|
624
|
|
625
|
|
- " mul %14,%2" "\n\t" /* r1:r0 = LO(x) * MIH(acc)*/
|
|
625
|
+ " mul %14,%2" "\n\t" // r1:r0 = LO(x) * MIH(acc)
|
626
|
626
|
" add %5,r0" "\n\t"
|
627
|
627
|
" adc %9,r1" "\n\t"
|
628
|
628
|
" adc %10,%13" "\n\t"
|
629
|
|
- " adc %11,%13" "\n\t" /* %11:%10:%9:%5:%4 = LO(x) * MIH(acc) << 8*/
|
630
|
|
- " mul %15,%2" "\n\t" /* r1:r0 = MI(x) * MIH(acc)*/
|
|
629
|
+ " adc %11,%13" "\n\t" // %11:%10:%9:%5:%4 = LO(x) * MIH(acc) << 8
|
|
630
|
+ " mul %15,%2" "\n\t" // r1:r0 = MI(x) * MIH(acc)
|
631
|
631
|
" add %9,r0" "\n\t"
|
632
|
632
|
" adc %10,r1" "\n\t"
|
633
|
|
- " adc %11,%13" "\n\t" /* %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 16*/
|
634
|
|
- " mul %16,%2" "\n\t" /* r1:r0 = HI(x) * MIH(acc)*/
|
|
633
|
+ " adc %11,%13" "\n\t" // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 16
|
|
634
|
+ " mul %16,%2" "\n\t" // r1:r0 = HI(x) * MIH(acc)
|
635
|
635
|
" add %10,r0" "\n\t"
|
636
|
|
- " adc %11,r1" "\n\t" /* %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 24*/
|
|
636
|
+ " adc %11,r1" "\n\t" // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 24
|
637
|
637
|
|
638
|
|
- " mul %14,%3" "\n\t" /* r1:r0 = LO(x) * HI(acc)*/
|
|
638
|
+ " mul %14,%3" "\n\t" // r1:r0 = LO(x) * HI(acc)
|
639
|
639
|
" add %9,r0" "\n\t"
|
640
|
640
|
" adc %10,r1" "\n\t"
|
641
|
|
- " adc %11,%13" "\n\t" /* %11:%10:%9:%5:%4 = LO(x) * HI(acc) << 16*/
|
642
|
|
- " mul %15,%3" "\n\t" /* r1:r0 = MI(x) * HI(acc)*/
|
|
641
|
+ " adc %11,%13" "\n\t" // %11:%10:%9:%5:%4 = LO(x) * HI(acc) << 16
|
|
642
|
+ " mul %15,%3" "\n\t" // r1:r0 = MI(x) * HI(acc)
|
643
|
643
|
" add %10,r0" "\n\t"
|
644
|
|
- " adc %11,r1" "\n\t" /* %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 24*/
|
645
|
|
- " mul %16,%3" "\n\t" /* r1:r0 = HI(x) * HI(acc)*/
|
646
|
|
- " add %11,r0" "\n\t" /* %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 32*/
|
|
644
|
+ " adc %11,r1" "\n\t" // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 24
|
|
645
|
+ " mul %16,%3" "\n\t" // r1:r0 = HI(x) * HI(acc)
|
|
646
|
+ " add %11,r0" "\n\t" // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 32
|
647
|
647
|
|
648
|
|
- /* At this point, %11:%10:%9 contains the new estimation of x. */
|
|
648
|
+ // At this point, %11:%10:%9 contains the new estimation of x.
|
649
|
649
|
|
650
|
|
- /* Finally, we must correct the result. Estimate remainder as*/
|
651
|
|
- /* (1<<24) - x*d*/
|
652
|
|
- /* %11:%10:%9 = x*/
|
653
|
|
- /* %8:%7:%6 = d = interval" "\n\t" /* */
|
|
650
|
+ // Finally, we must correct the result. Estimate remainder as
|
|
651
|
+ // (1<<24) - x*d
|
|
652
|
+ // %11:%10:%9 = x
|
|
653
|
+ // %8:%7:%6 = d = interval" "\n\t"
|
654
|
654
|
" ldi %3,1" "\n\t"
|
655
|
655
|
" clr %2" "\n\t"
|
656
|
656
|
" clr %1" "\n\t"
|
657
|
|
- " clr %0" "\n\t" /* %3:%2:%1:%0 = 0x1000000*/
|
658
|
|
- " mul %6,%9" "\n\t" /* r1:r0 = LO(d) * LO(x)*/
|
|
657
|
+ " clr %0" "\n\t" // %3:%2:%1:%0 = 0x1000000
|
|
658
|
+ " mul %6,%9" "\n\t" // r1:r0 = LO(d) * LO(x)
|
659
|
659
|
" sub %0,r0" "\n\t"
|
660
|
660
|
" sbc %1,r1" "\n\t"
|
661
|
661
|
" sbc %2,%13" "\n\t"
|
662
|
|
- " sbc %3,%13" "\n\t" /* %3:%2:%1:%0 -= LO(d) * LO(x)*/
|
663
|
|
- " mul %7,%9" "\n\t" /* r1:r0 = MI(d) * LO(x)*/
|
|
662
|
+ " sbc %3,%13" "\n\t" // %3:%2:%1:%0 -= LO(d) * LO(x)
|
|
663
|
+ " mul %7,%9" "\n\t" // r1:r0 = MI(d) * LO(x)
|
664
|
664
|
" sub %1,r0" "\n\t"
|
665
|
665
|
" sbc %2,r1" "\n\t"
|
666
|
|
- " sbc %3,%13" "\n\t" /* %3:%2:%1:%0 -= MI(d) * LO(x) << 8*/
|
667
|
|
- " mul %8,%9" "\n\t" /* r1:r0 = HI(d) * LO(x)*/
|
|
666
|
+ " sbc %3,%13" "\n\t" // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
|
|
667
|
+ " mul %8,%9" "\n\t" // r1:r0 = HI(d) * LO(x)
|
668
|
668
|
" sub %2,r0" "\n\t"
|
669
|
|
- " sbc %3,r1" "\n\t" /* %3:%2:%1:%0 -= MIL(d) * LO(x) << 16*/
|
670
|
|
- " mul %6,%10" "\n\t" /* r1:r0 = LO(d) * MI(x)*/
|
|
669
|
+ " sbc %3,r1" "\n\t" // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
|
|
670
|
+ " mul %6,%10" "\n\t" // r1:r0 = LO(d) * MI(x)
|
671
|
671
|
" sub %1,r0" "\n\t"
|
672
|
672
|
" sbc %2,r1" "\n\t"
|
673
|
|
- " sbc %3,%13" "\n\t" /* %3:%2:%1:%0 -= LO(d) * MI(x) << 8*/
|
674
|
|
- " mul %7,%10" "\n\t" /* r1:r0 = MI(d) * MI(x)*/
|
|
673
|
+ " sbc %3,%13" "\n\t" // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
|
|
674
|
+ " mul %7,%10" "\n\t" // r1:r0 = MI(d) * MI(x)
|
675
|
675
|
" sub %2,r0" "\n\t"
|
676
|
|
- " sbc %3,r1" "\n\t" /* %3:%2:%1:%0 -= MI(d) * MI(x) << 16*/
|
677
|
|
- " mul %8,%10" "\n\t" /* r1:r0 = HI(d) * MI(x)*/
|
678
|
|
- " sub %3,r0" "\n\t" /* %3:%2:%1:%0 -= MIL(d) * MI(x) << 24*/
|
679
|
|
- " mul %6,%11" "\n\t" /* r1:r0 = LO(d) * HI(x)*/
|
|
676
|
+ " sbc %3,r1" "\n\t" // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
|
|
677
|
+ " mul %8,%10" "\n\t" // r1:r0 = HI(d) * MI(x)
|
|
678
|
+ " sub %3,r0" "\n\t" // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
|
|
679
|
+ " mul %6,%11" "\n\t" // r1:r0 = LO(d) * HI(x)
|
680
|
680
|
" sub %2,r0" "\n\t"
|
681
|
|
- " sbc %3,r1" "\n\t" /* %3:%2:%1:%0 -= LO(d) * HI(x) << 16*/
|
682
|
|
- " mul %7,%11" "\n\t" /* r1:r0 = MI(d) * HI(x)*/
|
683
|
|
- " sub %3,r0" "\n\t" /* %3:%2:%1:%0 -= MI(d) * HI(x) << 24*/
|
684
|
|
- /* %3:%2:%1:%0 = r = (1<<24) - x*d*/
|
685
|
|
- /* %8:%7:%6 = d = interval */
|
|
681
|
+ " sbc %3,r1" "\n\t" // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
|
|
682
|
+ " mul %7,%11" "\n\t" // r1:r0 = MI(d) * HI(x)
|
|
683
|
+ " sub %3,r0" "\n\t" // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
|
|
684
|
+ // %3:%2:%1:%0 = r = (1<<24) - x*d
|
|
685
|
+ // %8:%7:%6 = d = interval
|
686
|
686
|
|
687
|
|
- /* Perform the final correction*/
|
|
687
|
+ // Perform the final correction
|
688
|
688
|
" sub %0,%6" "\n\t"
|
689
|
689
|
" sbc %1,%7" "\n\t"
|
690
|
|
- " sbc %2,%8" "\n\t" /* r -= d*/
|
691
|
|
- " brcs 14f" "\n\t" /* if ( r >= d) */
|
|
690
|
+ " sbc %2,%8" "\n\t" // r -= d
|
|
691
|
+ " brcs 14f" "\n\t" // if ( r >= d)
|
692
|
692
|
|
693
|
|
- /* %11:%10:%9 = x */
|
|
693
|
+ // %11:%10:%9 = x
|
694
|
694
|
" ldi %3,1" "\n\t"
|
695
|
695
|
" add %9,%3" "\n\t"
|
696
|
696
|
" adc %10,%13" "\n\t"
|
697
|
|
- " adc %11,%13" "\n\t" /* x++*/
|
|
697
|
+ " adc %11,%13" "\n\t" // x++
|
698
|
698
|
"14:" "\n\t"
|
699
|
699
|
|
700
|
|
- /* Estimation is done. %11:%10:%9 = x */
|
701
|
|
- " clr __zero_reg__" "\n\t" /* Make C runtime happy */
|
702
|
|
- /* [211 cycles total]*/
|
|
700
|
+ // Estimation is done. %11:%10:%9 = x
|
|
701
|
+ " clr __zero_reg__" "\n\t" // Make C runtime happy
|
|
702
|
+ // [211 cycles total]
|
703
|
703
|
: "=r" (r2),
|
704
|
704
|
"=r" (r3),
|
705
|
705
|
"=r" (r4),
|