|
@@ -386,89 +386,89 @@ void Planner::init() {
|
386
|
386
|
// use Newton-Raphson for the calculation, and will strive to get way less cycles
|
387
|
387
|
// for the same result - Using C division, it takes 500cycles to complete .
|
388
|
388
|
|
389
|
|
- A("clr %3") // idx = 0
|
|
389
|
+ A("clr %3") // idx = 0
|
390
|
390
|
A("mov %14,%6")
|
391
|
391
|
A("mov %15,%7")
|
392
|
|
- A("mov %16,%8") // nr = interval
|
393
|
|
- A("tst %16") // nr & 0xFF0000 == 0 ?
|
394
|
|
- A("brne 2f") // No, skip this
|
|
392
|
+ A("mov %16,%8") // nr = interval
|
|
393
|
+ A("tst %16") // nr & 0xFF0000 == 0 ?
|
|
394
|
+ A("brne 2f") // No, skip this
|
395
|
395
|
A("mov %16,%15")
|
396
|
|
- A("mov %15,%14") // nr <<= 8, %14 not needed
|
397
|
|
- A("subi %3,-8") // idx += 8
|
398
|
|
- A("tst %16") // nr & 0xFF0000 == 0 ?
|
399
|
|
- A("brne 2f") // No, skip this
|
400
|
|
- A("mov %16,%15") // nr <<= 8, %14 not needed
|
401
|
|
- A("clr %15") // We clear %14
|
402
|
|
- A("subi %3,-8") // idx += 8
|
|
396
|
+ A("mov %15,%14") // nr <<= 8, %14 not needed
|
|
397
|
+ A("subi %3,-8") // idx += 8
|
|
398
|
+ A("tst %16") // nr & 0xFF0000 == 0 ?
|
|
399
|
+ A("brne 2f") // No, skip this
|
|
400
|
+ A("mov %16,%15") // nr <<= 8, %14 not needed
|
|
401
|
+ A("clr %15") // We clear %14
|
|
402
|
+ A("subi %3,-8") // idx += 8
|
403
|
403
|
|
404
|
404
|
// here %16 != 0 and %16:%15 contains at least 9 MSBits, or both %16:%15 are 0
|
405
|
405
|
L("2")
|
406
|
|
- A("cpi %16,0x10") // (nr & 0xF00000) == 0 ?
|
407
|
|
- A("brcc 3f") // No, skip this
|
408
|
|
- A("swap %15") // Swap nibbles
|
409
|
|
- A("swap %16") // Swap nibbles. Low nibble is 0
|
|
406
|
+ A("cpi %16,0x10") // (nr & 0xF00000) == 0 ?
|
|
407
|
+ A("brcc 3f") // No, skip this
|
|
408
|
+ A("swap %15") // Swap nibbles
|
|
409
|
+ A("swap %16") // Swap nibbles. Low nibble is 0
|
410
|
410
|
A("mov %14, %15")
|
411
|
|
- A("andi %14,0x0F") // Isolate low nibble
|
412
|
|
- A("andi %15,0xF0") // Keep proper nibble in %15
|
413
|
|
- A("or %16, %14") // %16:%15 <<= 4
|
414
|
|
- A("subi %3,-4") // idx += 4
|
|
411
|
+ A("andi %14,0x0F") // Isolate low nibble
|
|
412
|
+ A("andi %15,0xF0") // Keep proper nibble in %15
|
|
413
|
+ A("or %16, %14") // %16:%15 <<= 4
|
|
414
|
+ A("subi %3,-4") // idx += 4
|
415
|
415
|
|
416
|
416
|
L("3")
|
417
|
|
- A("cpi %16,0x40") // (nr & 0xC00000) == 0 ?
|
418
|
|
- A("brcc 4f") // No, skip this
|
|
417
|
+ A("cpi %16,0x40") // (nr & 0xC00000) == 0 ?
|
|
418
|
+ A("brcc 4f") // No, skip this
|
419
|
419
|
A("add %15,%15")
|
420
|
420
|
A("adc %16,%16")
|
421
|
421
|
A("add %15,%15")
|
422
|
|
- A("adc %16,%16") // %16:%15 <<= 2
|
423
|
|
- A("subi %3,-2") // idx += 2
|
|
422
|
+ A("adc %16,%16") // %16:%15 <<= 2
|
|
423
|
+ A("subi %3,-2") // idx += 2
|
424
|
424
|
|
425
|
425
|
L("4")
|
426
|
|
- A("cpi %16,0x80") // (nr & 0x800000) == 0 ?
|
427
|
|
- A("brcc 5f") // No, skip this
|
|
426
|
+ A("cpi %16,0x80") // (nr & 0x800000) == 0 ?
|
|
427
|
+ A("brcc 5f") // No, skip this
|
428
|
428
|
A("add %15,%15")
|
429
|
|
- A("adc %16,%16") // %16:%15 <<= 1
|
430
|
|
- A("inc %3") // idx += 1
|
|
429
|
+ A("adc %16,%16") // %16:%15 <<= 1
|
|
430
|
+ A("inc %3") // idx += 1
|
431
|
431
|
|
432
|
432
|
// Now %16:%15 contains its MSBit set to 1, or %16:%15 is == 0. We are now absolutely sure
|
433
|
433
|
// we have at least 9 MSBits available to enter the initial estimation table
|
434
|
434
|
L("5")
|
435
|
435
|
A("add %15,%15")
|
436
|
|
- A("adc %16,%16") // %16:%15 = tidx = (nr <<= 1), we lose the top MSBit (always set to 1, %16 is the index into the inverse table)
|
437
|
|
- A("add r30,%16") // Only use top 8 bits
|
438
|
|
- A("adc r31,%13") // r31:r30 = inv_tab + (tidx)
|
439
|
|
- A("lpm %14, Z") // %14 = inv_tab[tidx]
|
440
|
|
- A("ldi %15, 1") // %15 = 1 %15:%14 = inv_tab[tidx] + 256
|
|
436
|
+ A("adc %16,%16") // %16:%15 = tidx = (nr <<= 1), we lose the top MSBit (always set to 1, %16 is the index into the inverse table)
|
|
437
|
+ A("add r30,%16") // Only use top 8 bits
|
|
438
|
+ A("adc r31,%13") // r31:r30 = inv_tab + (tidx)
|
|
439
|
+ A("lpm %14, Z") // %14 = inv_tab[tidx]
|
|
440
|
+ A("ldi %15, 1") // %15 = 1 %15:%14 = inv_tab[tidx] + 256
|
441
|
441
|
|
442
|
442
|
// We must scale the approximation to the proper place
|
443
|
|
- A("clr %16") // %16 will always be 0 here
|
444
|
|
- A("subi %3,8") // idx == 8 ?
|
445
|
|
- A("breq 6f") // yes, no need to scale
|
446
|
|
- A("brcs 7f") // If C=1, means idx < 8, result was negative!
|
|
443
|
+ A("clr %16") // %16 will always be 0 here
|
|
444
|
+ A("subi %3,8") // idx == 8 ?
|
|
445
|
+ A("breq 6f") // yes, no need to scale
|
|
446
|
+ A("brcs 7f") // If C=1, means idx < 8, result was negative!
|
447
|
447
|
|
448
|
448
|
// idx > 8, now %3 = idx - 8. We must perform a left shift. idx range:[1-8]
|
449
|
|
- A("sbrs %3,0") // shift by 1bit position?
|
450
|
|
- A("rjmp 8f") // No
|
|
449
|
+ A("sbrs %3,0") // shift by 1bit position?
|
|
450
|
+ A("rjmp 8f") // No
|
451
|
451
|
A("add %14,%14")
|
452
|
|
- A("adc %15,%15") // %15:16 <<= 1
|
|
452
|
+ A("adc %15,%15") // %15:16 <<= 1
|
453
|
453
|
L("8")
|
454
|
|
- A("sbrs %3,1") // shift by 2bit position?
|
455
|
|
- A("rjmp 9f") // No
|
|
454
|
+ A("sbrs %3,1") // shift by 2bit position?
|
|
455
|
+ A("rjmp 9f") // No
|
456
|
456
|
A("add %14,%14")
|
457
|
457
|
A("adc %15,%15")
|
458
|
458
|
A("add %14,%14")
|
459
|
|
- A("adc %15,%15") // %15:16 <<= 1
|
|
459
|
+ A("adc %15,%15") // %15:16 <<= 1
|
460
|
460
|
L("9")
|
461
|
|
- A("sbrs %3,2") // shift by 4bits position?
|
462
|
|
- A("rjmp 16f") // No
|
463
|
|
- A("swap %15") // Swap nibbles. lo nibble of %15 will always be 0
|
464
|
|
- A("swap %14") // Swap nibbles
|
|
461
|
+ A("sbrs %3,2") // shift by 4bits position?
|
|
462
|
+ A("rjmp 16f") // No
|
|
463
|
+ A("swap %15") // Swap nibbles. lo nibble of %15 will always be 0
|
|
464
|
+ A("swap %14") // Swap nibbles
|
465
|
465
|
A("mov %12,%14")
|
466
|
|
- A("andi %12,0x0F") // isolate low nibble
|
467
|
|
- A("andi %14,0xF0") // and clear it
|
468
|
|
- A("or %15,%12") // %15:%16 <<= 4
|
|
466
|
+ A("andi %12,0x0F") // isolate low nibble
|
|
467
|
+ A("andi %14,0xF0") // and clear it
|
|
468
|
+ A("or %15,%12") // %15:%16 <<= 4
|
469
|
469
|
L("16")
|
470
|
|
- A("sbrs %3,3") // shift by 8bits position?
|
471
|
|
- A("rjmp 6f") // No, we are done
|
|
470
|
+ A("sbrs %3,3") // shift by 8bits position?
|
|
471
|
+ A("rjmp 6f") // No, we are done
|
472
|
472
|
A("mov %16,%15")
|
473
|
473
|
A("mov %15,%14")
|
474
|
474
|
A("clr %14")
|
|
@@ -476,32 +476,32 @@ void Planner::init() {
|
476
|
476
|
|
477
|
477
|
// idx < 8, now %3 = idx - 8. Get the count of bits
|
478
|
478
|
L("7")
|
479
|
|
- A("neg %3") // %3 = -idx = count of bits to move right. idx range:[1...8]
|
480
|
|
- A("sbrs %3,0") // shift by 1 bit position ?
|
481
|
|
- A("rjmp 10f") // No, skip it
|
482
|
|
- A("asr %15") // (bit7 is always 0 here)
|
|
479
|
+ A("neg %3") // %3 = -idx = count of bits to move right. idx range:[1...8]
|
|
480
|
+ A("sbrs %3,0") // shift by 1 bit position ?
|
|
481
|
+ A("rjmp 10f") // No, skip it
|
|
482
|
+ A("asr %15") // (bit7 is always 0 here)
|
483
|
483
|
A("ror %14")
|
484
|
484
|
L("10")
|
485
|
|
- A("sbrs %3,1") // shift by 2 bit position ?
|
486
|
|
- A("rjmp 11f") // No, skip it
|
487
|
|
- A("asr %15") // (bit7 is always 0 here)
|
|
485
|
+ A("sbrs %3,1") // shift by 2 bit position ?
|
|
486
|
+ A("rjmp 11f") // No, skip it
|
|
487
|
+ A("asr %15") // (bit7 is always 0 here)
|
488
|
488
|
A("ror %14")
|
489
|
|
- A("asr %15") // (bit7 is always 0 here)
|
|
489
|
+ A("asr %15") // (bit7 is always 0 here)
|
490
|
490
|
A("ror %14")
|
491
|
491
|
L("11")
|
492
|
|
- A("sbrs %3,2") // shift by 4 bit position ?
|
493
|
|
- A("rjmp 12f") // No, skip it
|
494
|
|
- A("swap %15") // Swap nibbles
|
495
|
|
- A("andi %14, 0xF0") // Lose the lowest nibble
|
496
|
|
- A("swap %14") // Swap nibbles. Upper nibble is 0
|
497
|
|
- A("or %14,%15") // Pass nibble from upper byte
|
498
|
|
- A("andi %15, 0x0F") // And get rid of that nibble
|
|
492
|
+ A("sbrs %3,2") // shift by 4 bit position ?
|
|
493
|
+ A("rjmp 12f") // No, skip it
|
|
494
|
+ A("swap %15") // Swap nibbles
|
|
495
|
+ A("andi %14, 0xF0") // Lose the lowest nibble
|
|
496
|
+ A("swap %14") // Swap nibbles. Upper nibble is 0
|
|
497
|
+ A("or %14,%15") // Pass nibble from upper byte
|
|
498
|
+ A("andi %15, 0x0F") // And get rid of that nibble
|
499
|
499
|
L("12")
|
500
|
|
- A("sbrs %3,3") // shift by 8 bit position ?
|
501
|
|
- A("rjmp 6f") // No, skip it
|
|
500
|
+ A("sbrs %3,3") // shift by 8 bit position ?
|
|
501
|
+ A("rjmp 6f") // No, skip it
|
502
|
502
|
A("mov %14,%15")
|
503
|
503
|
A("clr %15")
|
504
|
|
- L("6") // %16:%15:%14 = initial estimation of 0x1000000 / d
|
|
504
|
+ L("6") // %16:%15:%14 = initial estimation of 0x1000000 / d
|
505
|
505
|
|
506
|
506
|
// Now, we must refine the estimation present on %16:%15:%14 using 1 iteration
|
507
|
507
|
// of Newton-Raphson. As it has a quadratic convergence, 1 iteration is enough
|
|
@@ -517,33 +517,33 @@ void Planner::init() {
|
517
|
517
|
A("clr %0")
|
518
|
518
|
A("clr %1")
|
519
|
519
|
A("clr %2")
|
520
|
|
- A("ldi %3,2") // %3:%2:%1:%0 = 0x2000000
|
521
|
|
- A("mul %6,%14") // r1:r0 = LO(d) * LO(x)
|
|
520
|
+ A("ldi %3,2") // %3:%2:%1:%0 = 0x2000000
|
|
521
|
+ A("mul %6,%14") // r1:r0 = LO(d) * LO(x)
|
522
|
522
|
A("sub %0,r0")
|
523
|
523
|
A("sbc %1,r1")
|
524
|
524
|
A("sbc %2,%13")
|
525
|
|
- A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * LO(x)
|
526
|
|
- A("mul %7,%14") // r1:r0 = MI(d) * LO(x)
|
|
525
|
+ A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * LO(x)
|
|
526
|
+ A("mul %7,%14") // r1:r0 = MI(d) * LO(x)
|
527
|
527
|
A("sub %1,r0")
|
528
|
528
|
A("sbc %2,r1" )
|
529
|
|
- A("sbc %3,%13") // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
|
530
|
|
- A("mul %8,%14") // r1:r0 = HI(d) * LO(x)
|
|
529
|
+ A("sbc %3,%13") // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
|
|
530
|
+ A("mul %8,%14") // r1:r0 = HI(d) * LO(x)
|
531
|
531
|
A("sub %2,r0")
|
532
|
|
- A("sbc %3,r1") // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
|
533
|
|
- A("mul %6,%15") // r1:r0 = LO(d) * MI(x)
|
|
532
|
+ A("sbc %3,r1") // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
|
|
533
|
+ A("mul %6,%15") // r1:r0 = LO(d) * MI(x)
|
534
|
534
|
A("sub %1,r0")
|
535
|
535
|
A("sbc %2,r1")
|
536
|
|
- A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
|
537
|
|
- A("mul %7,%15") // r1:r0 = MI(d) * MI(x)
|
|
536
|
+ A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
|
|
537
|
+ A("mul %7,%15") // r1:r0 = MI(d) * MI(x)
|
538
|
538
|
A("sub %2,r0")
|
539
|
|
- A("sbc %3,r1") // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
|
540
|
|
- A("mul %8,%15") // r1:r0 = HI(d) * MI(x)
|
541
|
|
- A("sub %3,r0") // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
|
542
|
|
- A("mul %6,%16") // r1:r0 = LO(d) * HI(x)
|
|
539
|
+ A("sbc %3,r1") // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
|
|
540
|
+ A("mul %8,%15") // r1:r0 = HI(d) * MI(x)
|
|
541
|
+ A("sub %3,r0") // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
|
|
542
|
+ A("mul %6,%16") // r1:r0 = LO(d) * HI(x)
|
543
|
543
|
A("sub %2,r0")
|
544
|
|
- A("sbc %3,r1") // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
|
545
|
|
- A("mul %7,%16") // r1:r0 = MI(d) * HI(x)
|
546
|
|
- A("sub %3,r0") // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
|
|
544
|
+ A("sbc %3,r1") // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
|
|
545
|
+ A("mul %7,%16") // r1:r0 = MI(d) * HI(x)
|
|
546
|
+ A("sub %3,r0") // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
|
547
|
547
|
// %3:%2:%1:%0 = (1<<25) - x*d [169]
|
548
|
548
|
|
549
|
549
|
// We need to multiply that result by x, and we are only interested in the top 24bits of that multiply
|
|
@@ -553,62 +553,62 @@ void Planner::init() {
|
553
|
553
|
// %13 = 0
|
554
|
554
|
|
555
|
555
|
// result = %11:%10:%9:%5:%4
|
556
|
|
- A("mul %14,%0") // r1:r0 = LO(x) * LO(acc)
|
|
556
|
+ A("mul %14,%0") // r1:r0 = LO(x) * LO(acc)
|
557
|
557
|
A("mov %4,r1")
|
558
|
558
|
A("clr %5")
|
559
|
559
|
A("clr %9")
|
560
|
560
|
A("clr %10")
|
561
|
|
- A("clr %11") // %11:%10:%9:%5:%4 = LO(x) * LO(acc) >> 8
|
562
|
|
- A("mul %15,%0") // r1:r0 = MI(x) * LO(acc)
|
|
561
|
+ A("clr %11") // %11:%10:%9:%5:%4 = LO(x) * LO(acc) >> 8
|
|
562
|
+ A("mul %15,%0") // r1:r0 = MI(x) * LO(acc)
|
563
|
563
|
A("add %4,r0")
|
564
|
564
|
A("adc %5,r1")
|
565
|
565
|
A("adc %9,%13")
|
566
|
566
|
A("adc %10,%13")
|
567
|
|
- A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * LO(acc)
|
568
|
|
- A("mul %16,%0") // r1:r0 = HI(x) * LO(acc)
|
|
567
|
+ A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * LO(acc)
|
|
568
|
+ A("mul %16,%0") // r1:r0 = HI(x) * LO(acc)
|
569
|
569
|
A("add %5,r0")
|
570
|
570
|
A("adc %9,r1")
|
571
|
571
|
A("adc %10,%13")
|
572
|
|
- A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * LO(acc) << 8
|
|
572
|
+ A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * LO(acc) << 8
|
573
|
573
|
|
574
|
|
- A("mul %14,%1") // r1:r0 = LO(x) * MIL(acc)
|
|
574
|
+ A("mul %14,%1") // r1:r0 = LO(x) * MIL(acc)
|
575
|
575
|
A("add %4,r0")
|
576
|
576
|
A("adc %5,r1")
|
577
|
577
|
A("adc %9,%13")
|
578
|
578
|
A("adc %10,%13")
|
579
|
|
- A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * MIL(acc)
|
580
|
|
- A("mul %15,%1") // r1:r0 = MI(x) * MIL(acc)
|
|
579
|
+ A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * MIL(acc)
|
|
580
|
+ A("mul %15,%1") // r1:r0 = MI(x) * MIL(acc)
|
581
|
581
|
A("add %5,r0")
|
582
|
582
|
A("adc %9,r1")
|
583
|
583
|
A("adc %10,%13")
|
584
|
|
- A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 8
|
585
|
|
- A("mul %16,%1") // r1:r0 = HI(x) * MIL(acc)
|
|
584
|
+ A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 8
|
|
585
|
+ A("mul %16,%1") // r1:r0 = HI(x) * MIL(acc)
|
586
|
586
|
A("add %9,r0")
|
587
|
587
|
A("adc %10,r1")
|
588
|
|
- A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 16
|
|
588
|
+ A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 16
|
589
|
589
|
|
590
|
|
- A("mul %14,%2") // r1:r0 = LO(x) * MIH(acc)
|
|
590
|
+ A("mul %14,%2") // r1:r0 = LO(x) * MIH(acc)
|
591
|
591
|
A("add %5,r0")
|
592
|
592
|
A("adc %9,r1")
|
593
|
593
|
A("adc %10,%13")
|
594
|
|
- A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * MIH(acc) << 8
|
595
|
|
- A("mul %15,%2") // r1:r0 = MI(x) * MIH(acc)
|
|
594
|
+ A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * MIH(acc) << 8
|
|
595
|
+ A("mul %15,%2") // r1:r0 = MI(x) * MIH(acc)
|
596
|
596
|
A("add %9,r0")
|
597
|
597
|
A("adc %10,r1")
|
598
|
|
- A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 16
|
599
|
|
- A("mul %16,%2") // r1:r0 = HI(x) * MIH(acc)
|
|
598
|
+ A("adc %11,%13") // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 16
|
|
599
|
+ A("mul %16,%2") // r1:r0 = HI(x) * MIH(acc)
|
600
|
600
|
A("add %10,r0")
|
601
|
|
- A("adc %11,r1") // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 24
|
|
601
|
+ A("adc %11,r1") // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 24
|
602
|
602
|
|
603
|
|
- A("mul %14,%3") // r1:r0 = LO(x) * HI(acc)
|
|
603
|
+ A("mul %14,%3") // r1:r0 = LO(x) * HI(acc)
|
604
|
604
|
A("add %9,r0")
|
605
|
605
|
A("adc %10,r1")
|
606
|
|
- A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * HI(acc) << 16
|
607
|
|
- A("mul %15,%3") // r1:r0 = MI(x) * HI(acc)
|
|
606
|
+ A("adc %11,%13") // %11:%10:%9:%5:%4 = LO(x) * HI(acc) << 16
|
|
607
|
+ A("mul %15,%3") // r1:r0 = MI(x) * HI(acc)
|
608
|
608
|
A("add %10,r0")
|
609
|
|
- A("adc %11,r1") // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 24
|
610
|
|
- A("mul %16,%3") // r1:r0 = HI(x) * HI(acc)
|
611
|
|
- A("add %11,r0") // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 32
|
|
609
|
+ A("adc %11,r1") // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 24
|
|
610
|
+ A("mul %16,%3") // r1:r0 = HI(x) * HI(acc)
|
|
611
|
+ A("add %11,r0") // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 32
|
612
|
612
|
|
613
|
613
|
// At this point, %11:%10:%9 contains the new estimation of x.
|
614
|
614
|
|
|
@@ -619,47 +619,47 @@ void Planner::init() {
|
619
|
619
|
A("ldi %3,1")
|
620
|
620
|
A("clr %2")
|
621
|
621
|
A("clr %1")
|
622
|
|
- A("clr %0") // %3:%2:%1:%0 = 0x1000000
|
623
|
|
- A("mul %6,%9") // r1:r0 = LO(d) * LO(x)
|
|
622
|
+ A("clr %0") // %3:%2:%1:%0 = 0x1000000
|
|
623
|
+ A("mul %6,%9") // r1:r0 = LO(d) * LO(x)
|
624
|
624
|
A("sub %0,r0")
|
625
|
625
|
A("sbc %1,r1")
|
626
|
626
|
A("sbc %2,%13")
|
627
|
|
- A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * LO(x)
|
628
|
|
- A("mul %7,%9") // r1:r0 = MI(d) * LO(x)
|
|
627
|
+ A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * LO(x)
|
|
628
|
+ A("mul %7,%9") // r1:r0 = MI(d) * LO(x)
|
629
|
629
|
A("sub %1,r0")
|
630
|
630
|
A("sbc %2,r1")
|
631
|
|
- A("sbc %3,%13") // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
|
632
|
|
- A("mul %8,%9") // r1:r0 = HI(d) * LO(x)
|
|
631
|
+ A("sbc %3,%13") // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
|
|
632
|
+ A("mul %8,%9") // r1:r0 = HI(d) * LO(x)
|
633
|
633
|
A("sub %2,r0")
|
634
|
|
- A("sbc %3,r1") // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
|
635
|
|
- A("mul %6,%10") // r1:r0 = LO(d) * MI(x)
|
|
634
|
+ A("sbc %3,r1") // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
|
|
635
|
+ A("mul %6,%10") // r1:r0 = LO(d) * MI(x)
|
636
|
636
|
A("sub %1,r0")
|
637
|
637
|
A("sbc %2,r1")
|
638
|
|
- A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
|
639
|
|
- A("mul %7,%10") // r1:r0 = MI(d) * MI(x)
|
|
638
|
+ A("sbc %3,%13") // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
|
|
639
|
+ A("mul %7,%10") // r1:r0 = MI(d) * MI(x)
|
640
|
640
|
A("sub %2,r0")
|
641
|
|
- A("sbc %3,r1") // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
|
642
|
|
- A("mul %8,%10") // r1:r0 = HI(d) * MI(x)
|
643
|
|
- A("sub %3,r0") // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
|
644
|
|
- A("mul %6,%11") // r1:r0 = LO(d) * HI(x)
|
|
641
|
+ A("sbc %3,r1") // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
|
|
642
|
+ A("mul %8,%10") // r1:r0 = HI(d) * MI(x)
|
|
643
|
+ A("sub %3,r0") // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
|
|
644
|
+ A("mul %6,%11") // r1:r0 = LO(d) * HI(x)
|
645
|
645
|
A("sub %2,r0")
|
646
|
|
- A("sbc %3,r1") // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
|
647
|
|
- A("mul %7,%11") // r1:r0 = MI(d) * HI(x)
|
648
|
|
- A("sub %3,r0") // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
|
|
646
|
+ A("sbc %3,r1") // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
|
|
647
|
+ A("mul %7,%11") // r1:r0 = MI(d) * HI(x)
|
|
648
|
+ A("sub %3,r0") // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
|
649
|
649
|
// %3:%2:%1:%0 = r = (1<<24) - x*d
|
650
|
650
|
// %8:%7:%6 = d = interval
|
651
|
651
|
|
652
|
652
|
// Perform the final correction
|
653
|
653
|
A("sub %0,%6")
|
654
|
654
|
A("sbc %1,%7")
|
655
|
|
- A("sbc %2,%8") // r -= d
|
656
|
|
- A("brcs 14f") // if ( r >= d)
|
|
655
|
+ A("sbc %2,%8") // r -= d
|
|
656
|
+ A("brcs 14f") // if ( r >= d)
|
657
|
657
|
|
658
|
658
|
// %11:%10:%9 = x
|
659
|
659
|
A("ldi %3,1")
|
660
|
660
|
A("add %9,%3")
|
661
|
661
|
A("adc %10,%13")
|
662
|
|
- A("adc %11,%13") // x++
|
|
662
|
+ A("adc %11,%13") // x++
|
663
|
663
|
L("14")
|
664
|
664
|
|
665
|
665
|
// Estimation is done. %11:%10:%9 = x
|