1 /*- 2 * SPDX-License-Identifier: BSD-2-Clause 3 * 4 * Copyright (c) 2023 Beckhoff Automation GmbH & Co. KG 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions 8 * are met: 9 * 1. Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 2. Redistributions in binary form must reproduce the above copyright 12 * notice, this list of conditions and the following disclaimer in the 13 * documentation and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 25 * SUCH DAMAGE. 26 */ 27 28 #include <sys/cdefs.h> 29 30 #include <sys/param.h> 31 #include <sys/systm.h> 32 #include <sys/bus.h> 33 34 #include <dev/clk/clk.h> 35 36 #include <dev/clk/xilinx/zynqmp_clk_div.h> 37 38 #include "clkdev_if.h" 39 #include "zynqmp_firmware_if.h" 40 41 #define DIV_ROUND_CLOSEST(n, d) (((n) + (d) / 2) / (d)) 42 43 struct zynqmp_clk_div_softc { 44 device_t firmware; 45 enum zynqmp_clk_div_type type; 46 uint32_t id; 47 }; 48 49 static int 50 zynqmp_clk_div_init(struct clknode *clk, device_t dev) 51 { 52 53 clknode_init_parent_idx(clk, 0); 54 return (0); 55 } 56 57 static int 58 zynqmp_clk_div_recalc(struct clknode *clk, uint64_t *freq) 59 { 60 struct zynqmp_clk_div_softc *sc; 61 uint32_t div; 62 int rv; 63 64 sc = clknode_get_softc(clk); 65 rv = ZYNQMP_FIRMWARE_CLOCK_GETDIVIDER(sc->firmware, sc->id, &div); 66 if (rv != 0) { 67 printf("%s: Error while getting divider for %s\n", 68 __func__, 69 clknode_get_name(clk)); 70 return (EINVAL); 71 } 72 73 if (sc->type == CLK_DIV_TYPE_DIV0) 74 div &= 0xFFFF; 75 else 76 div = div >> 16; 77 *freq = howmany((unsigned long long)*freq, div + 1); 78 return (0); 79 } 80 81 static int 82 zynqmp_clk_div_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout, 83 int flags, int *stop) 84 { 85 struct zynqmp_clk_div_softc *sc; 86 uint32_t div; 87 int rv; 88 89 sc = clknode_get_softc(clk); 90 91 div = DIV_ROUND_CLOSEST(fparent, *fout); 92 if (sc->type == CLK_DIV_TYPE_DIV0) { 93 div &= 0xFFFF; 94 div |= 0xFFFF << 16; 95 } else { 96 div <<= 16; 97 div |= 0xFFFF; 98 } 99 100 rv = ZYNQMP_FIRMWARE_CLOCK_SETDIVIDER(sc->firmware, sc->id, div); 101 if (rv != 0) { 102 printf("%s: Error while setting divider for %s\n", 103 __func__, 104 clknode_get_name(clk)); 105 return (EINVAL); 106 } 107 108 return (rv); 109 } 110 111 static clknode_method_t zynqmp_clk_div_clknode_methods[] = { 112 /* Device interface */ 113 CLKNODEMETHOD(clknode_init, zynqmp_clk_div_init), 114 CLKNODEMETHOD(clknode_recalc_freq, zynqmp_clk_div_recalc), 115 CLKNODEMETHOD(clknode_set_freq, zynqmp_clk_div_set_freq), 116 CLKNODEMETHOD_END 117 }; 118 119 DEFINE_CLASS_1(zynqmp_clk_div_clknode, zynqmp_clk_div_clknode_class, 120 zynqmp_clk_div_clknode_methods, sizeof(struct zynqmp_clk_div_softc), clknode_class); 121 122 int 123 zynqmp_clk_div_register(struct clkdom *clkdom, device_t fw, struct clknode_init_def *clkdef, enum zynqmp_clk_div_type type) 124 { 125 struct clknode *clk; 126 struct zynqmp_clk_div_softc *sc; 127 uint32_t fw_clk_id; 128 129 fw_clk_id = clkdef->id - 1; 130 clkdef->id = 0; 131 clk = clknode_create(clkdom, &zynqmp_clk_div_clknode_class, clkdef); 132 if (clk == NULL) 133 return (1); 134 sc = clknode_get_softc(clk); 135 sc->id = fw_clk_id; 136 sc->firmware = fw; 137 sc->type = type; 138 clknode_register(clkdom, clk); 139 return (0); 140 } 141