1 /* 2 * Common clock functionality. 3 * 4 * Copyright (C) 2023 Paul Boddie <paul@boddie.org.uk> 5 * 6 * This program is free software; you can redistribute it and/or 7 * modify it under the terms of the GNU General Public License as 8 * published by the Free Software Foundation; either version 2 of 9 * the License, or (at your option) any later version. 10 * 11 * This program is distributed in the hope that it will be useful, 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 * GNU General Public License for more details. 15 * 16 * You should have received a copy of the GNU General Public License 17 * along with this program; if not, write to the Free Software 18 * Foundation, Inc., 51 Franklin Street, Fifth Floor, 19 * Boston, MA 02110-1301, USA 20 */ 21 22 #include <l4/devices/hw_mmio_register_block.h> 23 24 #include "cpm-common.h" 25 #include <math.h> 26 #include <stdio.h> 27 28 29 30 // Register access. 31 32 Cpm_regs::Cpm_regs(l4_addr_t addr, Clock_base *clocks[], 33 uint32_t exclk_freq) 34 : _clocks(clocks), exclk_freq(exclk_freq) 35 { 36 _regs = new Hw::Mmio_register_block<32>(addr); 37 } 38 39 // Utility methods. 40 41 uint32_t 42 Cpm_regs::get_field(uint32_t reg, uint32_t mask, uint8_t shift) 43 { 44 return (_regs[reg] & (mask << shift)) >> shift; 45 } 46 47 void 48 Cpm_regs::set_field(uint32_t reg, uint32_t mask, uint8_t shift, uint32_t value) 49 { 50 _regs[reg] = (_regs[reg] & (~(mask << shift))) | ((mask & value) << shift); 51 } 52 53 Clock_base * 54 Cpm_regs::get_clock(int num) 55 { 56 return _clocks[num]; 57 } 58 59 60 61 // Field methods. 62 63 uint32_t 64 Field::get_field(Cpm_regs ®s) 65 { 66 if (defined) 67 return regs.get_field(reg, mask, bit); 68 else 69 return 0; 70 } 71 72 void 73 Field::set_field(Cpm_regs ®s, uint32_t value) 74 { 75 if (defined) 76 regs.set_field(reg, mask, bit, value); 77 } 78 79 // Undefined field. 80 81 Field Field::undefined; 82 83 84 85 // Clock sources. 86 87 enum Clock_identifiers 88 Mux::get_input(int num) 89 { 90 if (num < _num_inputs) 91 return _inputs[num]; 92 else 93 return Clock_undefined; 94 } 95 96 // Clock sources. 97 98 uint8_t 99 Source::get_source(Cpm_regs ®s) 100 { 101 if (_source.is_defined()) 102 return _source.get_field(regs); 103 else 104 return 0; 105 } 106 107 void 108 Source::set_source(Cpm_regs ®s, uint8_t source) 109 { 110 if (!_source.is_defined()) 111 return; 112 113 _source.set_field(regs, source); 114 } 115 116 enum Clock_identifiers 117 Source::get_source_clock(Cpm_regs ®s) 118 { 119 return get_input(get_number() == 1 ? 0 : get_source(regs)); 120 } 121 122 void 123 Source::set_source_clock(Cpm_regs ®s, enum Clock_identifiers clock) 124 { 125 for (int source = 0; source < _inputs.get_number(); source++) 126 if (get_input(source) == clock) 127 _source.set_field(regs, source); 128 } 129 130 // Clock source frequencies. 131 132 uint32_t 133 Source::get_frequency(Cpm_regs ®s) 134 { 135 enum Clock_identifiers input = get_source_clock(regs); 136 137 if (input != Clock_undefined) 138 return regs.get_clock(input)->get_frequency(regs); 139 else 140 return 0; 141 } 142 143 144 145 // Clock control. 146 147 Control_base::~Control_base() 148 { 149 } 150 151 void 152 Control_base::change_disable(Cpm_regs ®s) 153 { 154 (void) regs; 155 } 156 157 void 158 Control_base::change_enable(Cpm_regs ®s) 159 { 160 (void) regs; 161 } 162 163 int 164 Control::have_clock(Cpm_regs ®s) 165 { 166 if (_gate.is_defined()) 167 return !_gate.get_field(regs); 168 else 169 return true; 170 } 171 172 void 173 Control::start_clock(Cpm_regs ®s) 174 { 175 if (_gate.is_defined()) 176 _gate.set_field(regs, 0); 177 } 178 179 void 180 Control::stop_clock(Cpm_regs ®s) 181 { 182 if (_gate.is_defined()) 183 _gate.set_field(regs, 1); 184 } 185 186 void 187 Control::wait_busy(Cpm_regs ®s) 188 { 189 if (_busy.is_defined()) 190 while (_busy.get_field(regs)); 191 } 192 193 void 194 Control::change_disable(Cpm_regs ®s) 195 { 196 if (_change_enable.is_defined()) 197 _change_enable.set_field(regs, 0); 198 } 199 200 void 201 Control::change_enable(Cpm_regs ®s) 202 { 203 if (_change_enable.is_defined()) 204 _change_enable.set_field(regs, 1); 205 } 206 207 208 209 // PLL-specific control. 210 211 int 212 Control_pll::have_pll(Cpm_regs ®s) 213 { 214 return _stable.get_field(regs); 215 } 216 217 int 218 Control_pll::pll_enabled(Cpm_regs ®s) 219 { 220 return _enable.get_field(regs); 221 } 222 223 int 224 Control_pll::pll_bypassed(Cpm_regs ®s) 225 { 226 return _bypass.get_field(regs); 227 } 228 229 void 230 Control_pll::pll_bypass(Cpm_regs ®s) 231 { 232 _bypass.set_field(regs, 1); 233 } 234 235 void 236 Control_pll::pll_engage(Cpm_regs ®s) 237 { 238 _bypass.set_field(regs, 0); 239 } 240 241 // Clock control. 242 243 int 244 Control_pll::have_clock(Cpm_regs ®s) 245 { 246 return have_pll(regs) && pll_enabled(regs); 247 } 248 249 void 250 Control_pll::start_clock(Cpm_regs ®s) 251 { 252 _enable.set_field(regs, 1); 253 while (!have_pll(regs)); 254 } 255 256 void 257 Control_pll::stop_clock(Cpm_regs ®s) 258 { 259 _enable.set_field(regs, 0); 260 while (have_pll(regs)); 261 } 262 263 void 264 Control_pll::wait_busy(Cpm_regs ®s) 265 { 266 if (pll_enabled(regs) && !pll_bypassed(regs)) 267 while (!have_pll(regs)); 268 } 269 270 271 272 // Clock dividers. 273 274 Divider_base::~Divider_base() 275 { 276 } 277 278 279 280 uint32_t 281 Divider::get_divider(Cpm_regs ®s) 282 { 283 if (_divider.is_defined()) 284 return _divider.get_field(regs) + 1; 285 else 286 return 1; 287 } 288 289 void 290 Divider::set_divider(Cpm_regs ®s, uint32_t divider) 291 { 292 if (_divider.is_defined()) 293 _divider.set_field(regs, divider - 1); 294 } 295 296 // Output clock frequencies. 297 298 uint32_t 299 Divider::get_frequency(Cpm_regs ®s, uint32_t source_frequency) 300 { 301 return source_frequency / get_divider(regs); 302 } 303 304 int 305 Divider::set_frequency(Cpm_regs ®s, uint32_t source_frequency, uint32_t frequency) 306 { 307 set_divider(regs, (uint32_t) round((double) source_frequency / (double) frequency)); 308 return 1; 309 } 310 311 int 312 Divider::get_parameters(Cpm_regs ®s, uint32_t parameters[]) 313 { 314 parameters[0] = get_divider(regs); 315 return 1; 316 } 317 318 int 319 Divider::set_parameters(Cpm_regs ®s, int num_parameters, uint32_t parameters[]) 320 { 321 if (num_parameters) 322 { 323 set_divider(regs, parameters[0]); 324 return 1; 325 } 326 327 return 0; 328 } 329 330 331 332 // Common divider functionality. 333 334 static int is_integer(double x) 335 { 336 double target = round(x) * 1000; 337 double rounded = floor(x * 1000); 338 339 return (target - 100 < rounded) && (rounded < target + 100); 340 } 341 342 static double getscale_part(double x) 343 { 344 double part = x - floor(x); 345 346 if (part > 0.5) 347 return 1 / (1 - part); 348 else if (part > 0) 349 return 1 / part; 350 else 351 return 1; 352 } 353 354 static double getscale(double x) 355 { 356 double scale = getscale_part(x); 357 358 if (is_integer(scale)) 359 return scale; 360 else 361 return scale * getscale(scale); 362 } 363 364 static void get_divider_operands(double frequency, double source_frequency, 365 double *multiplier, double *divider) 366 { 367 double ratio = frequency / source_frequency; 368 double scale = getscale(ratio); 369 370 *multiplier = scale * ratio; 371 *divider = scale; 372 } 373 374 static void reduce_divider_operands(uint32_t *m, uint32_t *n, uint32_t m_limit, 375 uint32_t n_limit) 376 { 377 while ((*m > m_limit) && (*n > n_limit) && (*m > 1) && (*n > 1)) 378 { 379 *m >>= 1; 380 *n >>= 1; 381 } 382 } 383 384 #define zero_as_one(X) ((X) ? (X) : 1) 385 386 387 388 // Feedback multiplier. 389 390 uint32_t 391 Divider_pll::get_multiplier(Cpm_regs ®s) 392 { 393 return zero_as_one(_multiplier.get_field(regs)); 394 } 395 396 void 397 Divider_pll::set_multiplier(Cpm_regs ®s, uint32_t multiplier) 398 { 399 _multiplier.set_field(regs, multiplier); 400 } 401 402 // Input divider. 403 404 uint32_t 405 Divider_pll::get_input_divider(Cpm_regs ®s) 406 { 407 return zero_as_one(_input_divider.get_field(regs)); 408 } 409 410 void 411 Divider_pll::set_input_divider(Cpm_regs ®s, uint32_t divider) 412 { 413 _input_divider.set_field(regs, divider); 414 } 415 416 // Output dividers. 417 418 uint32_t 419 Divider_pll::get_output_divider(Cpm_regs ®s) 420 { 421 uint32_t d0 = zero_as_one(_output_divider0.get_field(regs)); 422 uint32_t d1 = zero_as_one(_output_divider1.get_field(regs)); 423 424 return d0 * d1; 425 } 426 427 void 428 Divider_pll::set_output_divider(Cpm_regs ®s, uint32_t divider) 429 { 430 // Assert 1 as a minimum. 431 // Divider 0 must be less than or equal to divider 1. 432 433 uint32_t d0, d1; 434 435 if (!divider) 436 divider = 1; 437 438 if (divider < _output_divider1.get_limit()) 439 { 440 d0 = 1; 441 d1 = divider; 442 } 443 else 444 { 445 d0 = (uint32_t) floor(sqrt(divider)); 446 d1 = divider / d0; 447 } 448 449 _output_divider0.set_field(regs, d0); 450 _output_divider1.set_field(regs, d1); 451 } 452 453 uint32_t 454 Divider_pll::get_frequency(Cpm_regs ®s, uint32_t source_frequency) 455 { 456 return (source_frequency * get_multiplier(regs)) / 457 (get_input_divider(regs) * get_output_divider(regs)); 458 } 459 460 int 461 Divider_pll::set_frequency(Cpm_regs ®s, uint32_t source_frequency, uint32_t frequency) 462 { 463 double intermediate_min = 600000000, intermediate_max = 2400000000; 464 double intermediate_multiplier, intermediate_input_divider; 465 uint32_t output_min, output_max, output0, output1; 466 uint32_t multiplier, input_divider, output_divider; 467 468 // Distribute the divider across the input and output divider. For the X1600, 469 // the multiplier and input divider should collectively deliver a frequency in 470 // the range 600-2400 MHz. 471 472 output_min = (uint32_t) ceil(intermediate_min / frequency); 473 output_max = (uint32_t) floor(intermediate_max / frequency); 474 475 output_divider = output_min; 476 477 while (output_divider <= output_max) 478 { 479 // Test divider constraints. 480 481 output0 = (uint32_t) floor(sqrt(output_divider)); 482 output1 = (uint32_t) floor(output_divider / output0); 483 484 if ((output0 * output1 == output_divider) && 485 (output0 <= _output_divider0.get_limit()) && 486 (output1 <= _output_divider1.get_limit())) 487 { 488 // Calculate the other parameters. 489 490 uint32_t intermediate_frequency = frequency * output_divider; 491 492 get_divider_operands(intermediate_frequency, source_frequency, 493 &intermediate_multiplier, &intermediate_input_divider); 494 495 multiplier = (uint32_t) round(intermediate_multiplier); 496 input_divider = (uint32_t) round(intermediate_input_divider); 497 498 uint32_t multiplier_limit = _multiplier.get_limit(); 499 uint32_t input_divider_limit = _input_divider.get_limit(); 500 501 reduce_divider_operands(&multiplier, &input_divider, 502 multiplier_limit, input_divider_limit); 503 504 if ((multiplier <= multiplier_limit) && (input_divider <= input_divider_limit)) 505 { 506 set_multiplier(regs, multiplier); 507 set_input_divider(regs, input_divider); 508 set_output_divider(regs, output_divider); 509 510 return 1; 511 } 512 } 513 514 output_divider++; 515 } 516 517 return 0; 518 } 519 520 int 521 Divider_pll::get_parameters(Cpm_regs ®s, uint32_t parameters[]) 522 { 523 parameters[0] = get_multiplier(regs); 524 parameters[1] = get_input_divider(regs); 525 parameters[2] = get_output_divider(regs); 526 return 3; 527 } 528 529 int 530 Divider_pll::set_parameters(Cpm_regs ®s, int num_parameters, uint32_t parameters[]) 531 { 532 if (num_parameters > 2) 533 { 534 set_multiplier(regs, parameters[0]); 535 set_input_divider(regs, parameters[1]); 536 set_output_divider(regs, parameters[2]); 537 538 return 3; 539 } 540 541 return 0; 542 } 543 544 545 546 // I2S clock divider. 547 548 uint32_t 549 Divider_i2s::get_multiplier(Cpm_regs ®s) 550 { 551 return _multiplier.get_field(regs); 552 } 553 554 uint32_t 555 Divider_i2s::get_divider_N(Cpm_regs ®s) 556 { 557 return _divider_N.get_field(regs); 558 } 559 560 uint32_t 561 Divider_i2s::get_divider_D(Cpm_regs ®s) 562 { 563 return _divider_D.get_field(regs); 564 } 565 566 uint32_t 567 Divider_i2s::get_frequency(Cpm_regs ®s, uint32_t source_frequency) 568 { 569 /* NOTE: Assuming that this is the formula, given that the manual does not 570 really describe how D is used. */ 571 572 return (source_frequency * get_multiplier(regs)) / 573 (get_divider_N(regs) * get_divider_D(regs)); 574 } 575 576 int 577 Divider_i2s::set_frequency(Cpm_regs ®s, uint32_t source_frequency, uint32_t frequency) 578 { 579 double m, n; 580 581 get_divider_operands(frequency, source_frequency, &m, &n); 582 583 uint32_t multiplier = (uint32_t) round(m); 584 uint32_t divider = (uint32_t) round(n); 585 586 reduce_divider_operands(&multiplier, ÷r, 587 _multiplier.get_limit(), 588 _divider_N.get_limit()); 589 590 // Test for operand within limits and the N >= 2M constraint. 591 592 if ((multiplier <= _multiplier.get_limit()) && (divider <= _divider_N.get_limit()) && 593 (divider >= 2 * multiplier)) 594 { 595 /* NOTE: Setting D to 1. Even though it seems that D might also be used, 596 it does not seem necessary in practice, and the documentation is 597 unclear about its use. */ 598 599 uint32_t parameters[] = {multiplier, divider, 1}; 600 601 set_parameters(regs, 3, parameters); 602 return 1; 603 } 604 605 return 0; 606 } 607 608 int 609 Divider_i2s::get_parameters(Cpm_regs ®s, uint32_t parameters[]) 610 { 611 parameters[0] = get_multiplier(regs); 612 parameters[1] = get_divider_N(regs); 613 parameters[2] = get_divider_D(regs); 614 return 3; 615 } 616 617 int 618 Divider_i2s::set_parameters(Cpm_regs ®s, int num_parameters, uint32_t parameters[]) 619 { 620 if (num_parameters == 1) 621 { 622 // Set automatic N and D value calculation if only one parameter is given. 623 624 _auto_N.set_field(regs, 0); 625 _auto_D.set_field(regs, 0); 626 _multiplier.set_field(regs, parameters[0]); 627 628 return 1; 629 } 630 else if (num_parameters > 1) 631 { 632 // Require N >= 2M, returning otherwise. 633 634 if (parameters[1] < 2 * parameters[0]) 635 return 0; 636 637 // Set automatic D value calculation if only two parameters are given. 638 639 _auto_N.set_field(regs, 1); 640 _auto_D.set_field(regs, (num_parameters == 2) ? 0 : 1); 641 642 _multiplier.set_field(regs, parameters[0]); 643 _divider_N.set_field(regs, parameters[1]); 644 645 // Set D explicitly if given. 646 647 if (num_parameters > 2) 648 _divider_D.set_field(regs, parameters[2]); 649 650 return num_parameters; 651 } 652 653 return 0; 654 } 655 656 657 658 // Clock interface. 659 660 Clock_base::~Clock_base() 661 { 662 } 663 664 665 666 // Null clock. 667 668 int 669 Clock_null::have_clock(Cpm_regs ®s) 670 { 671 (void) regs; 672 return false; 673 } 674 675 void 676 Clock_null::start_clock(Cpm_regs ®s) 677 { 678 (void) regs; 679 } 680 681 void 682 Clock_null::stop_clock(Cpm_regs ®s) 683 { 684 (void) regs; 685 } 686 687 // Output clock frequencies. 688 689 uint32_t 690 Clock_null::get_frequency(Cpm_regs ®s) 691 { 692 (void) regs; 693 return 0; 694 } 695 696 697 698 // Passive clock. 699 700 int 701 Clock_passive::have_clock(Cpm_regs ®s) 702 { 703 (void) regs; 704 return true; 705 } 706 707 void 708 Clock_passive::start_clock(Cpm_regs ®s) 709 { 710 (void) regs; 711 } 712 713 void 714 Clock_passive::stop_clock(Cpm_regs ®s) 715 { 716 (void) regs; 717 } 718 719 // Output clock frequencies. 720 721 uint32_t 722 Clock_passive::get_frequency(Cpm_regs ®s) 723 { 724 // NOTE: Return the external clock frequency. 725 726 return regs.exclk_freq; 727 } 728 729 730 731 // Clock control. 732 733 int 734 Clock_controlled::have_clock(Cpm_regs ®s) 735 { 736 return _get_control().have_clock(regs); 737 } 738 739 void 740 Clock_controlled::start_clock(Cpm_regs ®s) 741 { 742 _get_control().start_clock(regs); 743 } 744 745 void 746 Clock_controlled::stop_clock(Cpm_regs ®s) 747 { 748 _get_control().stop_clock(regs); 749 } 750 751 752 753 // Active clock interface. 754 755 Clock_active::~Clock_active() 756 { 757 } 758 759 // Clock sources. 760 761 uint8_t 762 Clock_active::get_source(Cpm_regs ®s) 763 { 764 return _source.get_source(regs); 765 } 766 767 void 768 Clock_active::set_source(Cpm_regs ®s, uint8_t source) 769 { 770 _get_control().change_enable(regs); 771 _source.set_source(regs, source); 772 _get_control().wait_busy(regs); 773 _get_control().change_disable(regs); 774 } 775 776 enum Clock_identifiers 777 Clock_active::get_source_clock(Cpm_regs ®s) 778 { 779 return _source.get_source_clock(regs); 780 } 781 782 void 783 Clock_active::set_source_clock(Cpm_regs ®s, enum Clock_identifiers clock) 784 { 785 _source.set_source_clock(regs, clock); 786 } 787 788 // Clock source frequencies. 789 790 uint32_t 791 Clock_active::get_source_frequency(Cpm_regs ®s) 792 { 793 return _source.get_frequency(regs); 794 } 795 796 // Output clock frequencies. 797 798 uint32_t 799 Clock_active::get_frequency(Cpm_regs ®s) 800 { 801 return get_source_frequency(regs); 802 } 803 804 805 806 // Divided clock interface. 807 808 Clock_divided_base::~Clock_divided_base() 809 { 810 } 811 812 // Output clock frequencies. 813 814 uint32_t 815 Clock_divided_base::get_frequency(Cpm_regs ®s) 816 { 817 return _get_divider().get_frequency(regs, get_source_frequency(regs)); 818 } 819 820 int 821 Clock_divided_base::set_frequency(Cpm_regs ®s, uint32_t frequency) 822 { 823 _get_control().change_enable(regs); 824 int result = _get_divider().set_frequency(regs, get_source_frequency(regs), frequency); 825 _get_control().wait_busy(regs); 826 _get_control().change_disable(regs); 827 828 return result; 829 } 830 831 int 832 Clock_divided_base::get_parameters(Cpm_regs ®s, uint32_t parameters[]) 833 { 834 return _get_divider().get_parameters(regs, parameters); 835 } 836 837 int 838 Clock_divided_base::set_parameters(Cpm_regs ®s, int num_parameters, uint32_t parameters[]) 839 { 840 _get_control().change_enable(regs); 841 int n = _get_divider().set_parameters(regs, num_parameters, parameters); 842 _get_control().wait_busy(regs); 843 _get_control().change_disable(regs); 844 845 return n; 846 } 847 848 849 850 // PLL functionality. 851 852 Pll::~Pll() 853 { 854 } 855 856 uint32_t 857 Pll::get_frequency(Cpm_regs ®s) 858 { 859 if (!_control.pll_bypassed(regs)) 860 return _divider.get_frequency(regs, get_source_frequency(regs)); 861 else 862 return get_source_frequency(regs); 863 } 864 865 int 866 Pll::set_frequency(Cpm_regs ®s, uint32_t frequency) 867 { 868 int result = Clock_divided_base::set_frequency(regs, frequency); 869 _control.pll_engage(regs); 870 871 return result; 872 }