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