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 #define zero_as_one(X) ((X) ? (X) : 1) 409 410 411 412 // Feedback multiplier. 413 414 uint32_t 415 Divider_pll::get_multiplier(Cpm_regs ®s) 416 { 417 return zero_as_one(_multiplier.get_field(regs)); 418 } 419 420 void 421 Divider_pll::set_multiplier(Cpm_regs ®s, uint32_t multiplier) 422 { 423 _multiplier.set_field(regs, multiplier); 424 } 425 426 // Input divider. 427 428 uint32_t 429 Divider_pll::get_input_divider(Cpm_regs ®s) 430 { 431 return zero_as_one(_input_divider.get_field(regs)); 432 } 433 434 void 435 Divider_pll::set_input_divider(Cpm_regs ®s, uint32_t divider) 436 { 437 _input_divider.set_field(regs, divider); 438 } 439 440 // Output dividers. 441 442 uint32_t 443 Divider_pll::get_output_divider(Cpm_regs ®s) 444 { 445 uint32_t d0 = zero_as_one(_output_divider0.get_field(regs)); 446 uint32_t d1 = _output_divider1.is_defined() ? 447 zero_as_one(_output_divider1.get_field(regs)) : 1; 448 449 return d0 * d1; 450 } 451 452 void 453 Divider_pll::set_output_divider(Cpm_regs ®s, uint32_t divider) 454 { 455 uint32_t d0, d1; 456 457 // Assert 1 as a minimum. 458 459 if (!divider) 460 divider = 1; 461 462 // Attempt to set any single divider. 463 464 if (!_output_divider1.is_defined()) 465 { 466 _output_divider0.set_field(regs, divider); 467 return; 468 } 469 470 // For two-divider implementations such as the X1600, divider 0 must be less 471 // than or equal to divider 1. 472 473 if (divider < _output_divider1.get_limit()) 474 { 475 d0 = 1; 476 d1 = divider; 477 } 478 else 479 { 480 d0 = (uint32_t) floor(sqrt(divider)); 481 d1 = divider / d0; 482 } 483 484 _output_divider0.set_field(regs, d0); 485 _output_divider1.set_field(regs, d1); 486 } 487 488 uint64_t 489 Divider_pll::get_frequency(Cpm_regs ®s, uint64_t source_frequency) 490 { 491 return (source_frequency * get_multiplier(regs)) / 492 (get_input_divider(regs) * get_output_divider(regs)); 493 } 494 495 int 496 Divider_pll::set_frequency(Cpm_regs ®s, uint64_t source_frequency, uint64_t frequency) 497 { 498 double intermediate_multiplier, intermediate_input_divider; 499 uint32_t output_min, output_max, output0, output1; 500 uint32_t multiplier, input_divider, output_divider; 501 502 // Define the range for the output dividers using the intermediate frequency 503 // range applying to each chip, this being the result of the multiplier and 504 // input divider. 505 506 output_min = (uint32_t) ceil(_intermediate_min / frequency); 507 output_max = (uint32_t) floor(_intermediate_max / frequency); 508 509 // Distribute the divider across the input and output dividers. 510 511 output_divider = output_min; 512 513 while (output_divider <= output_max) 514 { 515 bool usable_divider; 516 517 // Test divider constraints. 518 519 if (_output_divider1.is_defined()) 520 { 521 output0 = (uint32_t) floor(sqrt(output_divider)); 522 output1 = (uint32_t) floor(output_divider / output0); 523 524 usable_divider = ((output0 * output1 == output_divider) && 525 (output0 <= _output_divider0.get_limit()) && 526 (output1 <= _output_divider1.get_limit())); 527 } 528 else 529 usable_divider = output_divider <= _output_divider0.get_limit(); 530 531 // Apply any usable divider. 532 533 if (usable_divider) 534 { 535 // Calculate the other parameters. Start by working back from the desired 536 // output frequency to obtain an intermediate frequency using the proposed 537 // divider. 538 539 double intermediate_frequency = frequency * output_divider; 540 541 // Calculate the required multiplier and divider. 542 543 get_divider_operands(intermediate_frequency, source_frequency, 544 &intermediate_multiplier, &intermediate_input_divider); 545 546 multiplier = (uint32_t) round(intermediate_multiplier); 547 input_divider = (uint32_t) round(intermediate_input_divider); 548 549 // Attempt to reduce the multiplier and divider to usable values. 550 551 uint32_t multiplier_limit = _multiplier.get_limit(); 552 uint32_t input_divider_limit = _input_divider.get_limit(); 553 554 reduce_divider_operands(&multiplier, &input_divider, 555 multiplier_limit, input_divider_limit); 556 557 if ((multiplier <= multiplier_limit) && (input_divider <= input_divider_limit)) 558 { 559 set_multiplier(regs, multiplier); 560 set_input_divider(regs, input_divider); 561 set_output_divider(regs, output_divider); 562 563 return 1; 564 } 565 } 566 567 output_divider++; 568 } 569 570 return 0; 571 } 572 573 int 574 Divider_pll::get_parameters(Cpm_regs ®s, uint32_t parameters[]) 575 { 576 parameters[0] = get_multiplier(regs); 577 parameters[1] = get_input_divider(regs); 578 parameters[2] = get_output_divider(regs); 579 return 3; 580 } 581 582 int 583 Divider_pll::set_parameters(Cpm_regs ®s, int num_parameters, uint32_t parameters[]) 584 { 585 if (num_parameters > 2) 586 { 587 set_multiplier(regs, parameters[0]); 588 set_input_divider(regs, parameters[1]); 589 set_output_divider(regs, parameters[2]); 590 591 return 3; 592 } 593 594 return 0; 595 } 596 597 598 599 // I2S clock divider. 600 601 uint32_t 602 Divider_i2s::get_multiplier(Cpm_regs ®s) 603 { 604 return _multiplier.get_field(regs); 605 } 606 607 uint32_t 608 Divider_i2s::get_divider_N(Cpm_regs ®s) 609 { 610 return _divider_N.get_field(regs); 611 } 612 613 uint32_t 614 Divider_i2s::get_divider_D(Cpm_regs ®s) 615 { 616 return _divider_D.get_field(regs); 617 } 618 619 uint64_t 620 Divider_i2s::get_frequency(Cpm_regs ®s, uint64_t source_frequency) 621 { 622 /* NOTE: Assuming that this is the formula, given that the manual does not 623 really describe how D is used. */ 624 625 return (source_frequency * get_multiplier(regs)) / 626 (get_divider_N(regs) * get_divider_D(regs)); 627 } 628 629 int 630 Divider_i2s::set_frequency(Cpm_regs ®s, uint64_t source_frequency, uint64_t frequency) 631 { 632 double m, n; 633 634 get_divider_operands(frequency, source_frequency, &m, &n); 635 636 uint32_t multiplier = (uint32_t) round(m); 637 uint32_t divider = (uint32_t) round(n); 638 639 reduce_divider_operands(&multiplier, ÷r, 640 _multiplier.get_limit(), 641 _divider_N.get_limit()); 642 643 // Test for operand within limits and the N >= 2M constraint. 644 645 if ((multiplier <= _multiplier.get_limit()) && (divider <= _divider_N.get_limit()) && 646 (divider >= 2 * multiplier)) 647 { 648 /* NOTE: Setting D to 1. Even though it seems that D might also be used, 649 it does not seem necessary in practice, and the documentation is 650 unclear about its use. */ 651 652 uint32_t parameters[] = {multiplier, divider, 1}; 653 654 set_parameters(regs, 3, parameters); 655 return 1; 656 } 657 658 return 0; 659 } 660 661 int 662 Divider_i2s::get_parameters(Cpm_regs ®s, uint32_t parameters[]) 663 { 664 parameters[0] = get_multiplier(regs); 665 parameters[1] = get_divider_N(regs); 666 parameters[2] = get_divider_D(regs); 667 return 3; 668 } 669 670 int 671 Divider_i2s::set_parameters(Cpm_regs ®s, int num_parameters, uint32_t parameters[]) 672 { 673 if (num_parameters == 1) 674 { 675 // Set automatic N and D value calculation if only one parameter is given. 676 677 _auto_N.set_field(regs, 0); 678 _auto_D.set_field(regs, 0); 679 _multiplier.set_field(regs, parameters[0]); 680 681 return 1; 682 } 683 else if (num_parameters > 1) 684 { 685 // Require N >= 2M, returning otherwise. 686 687 if (parameters[1] < 2 * parameters[0]) 688 return 0; 689 690 // Set automatic D value calculation if only two parameters are given. 691 692 _auto_N.set_field(regs, 1); 693 _auto_D.set_field(regs, (num_parameters == 2) ? 0 : 1); 694 695 _multiplier.set_field(regs, parameters[0]); 696 _divider_N.set_field(regs, parameters[1]); 697 698 // Set D explicitly if given. 699 700 if (num_parameters > 2) 701 _divider_D.set_field(regs, parameters[2]); 702 703 return num_parameters; 704 } 705 706 return 0; 707 } 708 709 710 711 // Clock interface. 712 713 Clock_base::~Clock_base() 714 { 715 } 716 717 718 719 // Null clock. 720 721 int 722 Clock_null::have_clock(Cpm_regs ®s) 723 { 724 (void) regs; 725 return false; 726 } 727 728 void 729 Clock_null::start_clock(Cpm_regs ®s) 730 { 731 (void) regs; 732 } 733 734 void 735 Clock_null::stop_clock(Cpm_regs ®s) 736 { 737 (void) regs; 738 } 739 740 // Output clock frequencies. 741 742 uint64_t 743 Clock_null::get_frequency(Cpm_regs ®s) 744 { 745 (void) regs; 746 return 0; 747 } 748 749 750 751 // Passive clock. 752 753 int 754 Clock_passive::have_clock(Cpm_regs ®s) 755 { 756 (void) regs; 757 return true; 758 } 759 760 void 761 Clock_passive::start_clock(Cpm_regs ®s) 762 { 763 (void) regs; 764 } 765 766 void 767 Clock_passive::stop_clock(Cpm_regs ®s) 768 { 769 (void) regs; 770 } 771 772 // Output clock frequencies. 773 774 uint64_t 775 Clock_passive::get_frequency(Cpm_regs ®s) 776 { 777 (void) regs; 778 return _frequency; 779 } 780 781 782 783 // Clock control. 784 785 int 786 Clock_controlled::have_clock(Cpm_regs ®s) 787 { 788 return _get_control().have_clock(regs); 789 } 790 791 void 792 Clock_controlled::start_clock(Cpm_regs ®s) 793 { 794 _get_control().start_clock(regs); 795 } 796 797 void 798 Clock_controlled::stop_clock(Cpm_regs ®s) 799 { 800 _get_control().stop_clock(regs); 801 } 802 803 804 805 // Active clock interface. 806 807 Clock_active::~Clock_active() 808 { 809 } 810 811 // Clock sources. 812 813 uint8_t 814 Clock_active::get_source(Cpm_regs ®s) 815 { 816 return _source.get_source(regs); 817 } 818 819 void 820 Clock_active::set_source(Cpm_regs ®s, uint8_t source) 821 { 822 _get_control().change_enable(regs); 823 _source.set_source(regs, source); 824 _get_control().wait_busy(regs); 825 _get_control().change_disable(regs); 826 } 827 828 enum Clock_identifiers 829 Clock_active::get_source_clock(Cpm_regs ®s) 830 { 831 return _source.get_source_clock(regs); 832 } 833 834 void 835 Clock_active::set_source_clock(Cpm_regs ®s, enum Clock_identifiers clock) 836 { 837 _source.set_source_clock(regs, clock); 838 } 839 840 // Clock source frequencies. 841 842 uint64_t 843 Clock_active::get_source_frequency(Cpm_regs ®s) 844 { 845 return _source.get_frequency(regs); 846 } 847 848 // Output clock frequencies. 849 850 uint64_t 851 Clock_active::get_frequency(Cpm_regs ®s) 852 { 853 return get_source_frequency(regs); 854 } 855 856 857 858 // Divided clock interface. 859 860 Clock_divided_base::~Clock_divided_base() 861 { 862 } 863 864 // Output clock frequencies. 865 866 uint64_t 867 Clock_divided_base::get_frequency(Cpm_regs ®s) 868 { 869 return _get_divider().get_frequency(regs, get_source_frequency(regs)); 870 } 871 872 int 873 Clock_divided_base::set_frequency(Cpm_regs ®s, uint64_t frequency) 874 { 875 _get_control().change_enable(regs); 876 int result = _get_divider().set_frequency(regs, get_source_frequency(regs), frequency); 877 _get_control().wait_busy(regs); 878 _get_control().change_disable(regs); 879 880 return result; 881 } 882 883 int 884 Clock_divided_base::get_parameters(Cpm_regs ®s, uint32_t parameters[]) 885 { 886 return _get_divider().get_parameters(regs, parameters); 887 } 888 889 int 890 Clock_divided_base::set_parameters(Cpm_regs ®s, int num_parameters, uint32_t parameters[]) 891 { 892 _get_control().change_enable(regs); 893 int n = _get_divider().set_parameters(regs, num_parameters, parameters); 894 _get_control().wait_busy(regs); 895 _get_control().change_disable(regs); 896 897 return n; 898 } 899 900 901 902 // PLL functionality. 903 904 Pll::~Pll() 905 { 906 } 907 908 uint64_t 909 Pll::get_frequency(Cpm_regs ®s) 910 { 911 if (!_control.pll_bypassed(regs)) 912 return _divider.get_frequency(regs, get_source_frequency(regs)); 913 else 914 return get_source_frequency(regs); 915 } 916 917 int 918 Pll::set_frequency(Cpm_regs ®s, uint64_t frequency) 919 { 920 int result = Clock_divided_base::set_frequency(regs, frequency); 921 _control.pll_engage(regs); 922 923 return result; 924 }