1 /* 2 * ZynqMP clock driver 3 * 4 * Copyright (C) 2016 Xilinx, Inc. 5 * 6 * SPDX-License-Identifier: GPL-2.0+ 7 */ 8 9 #include <common.h> 10 #include <linux/bitops.h> 11 #include <clk-uclass.h> 12 #include <dm/device.h> 13 #include <clk.h> 14 15 #define ZYNQMP_GEM0_REF_CTRL 0xFF5E0050 16 #define ZYNQMP_IOPLL_CTRL 0xFF5E0020 17 #define ZYNQMP_RPLL_CTRL 0xFF5E0030 18 #define ZYNQMP_DPLL_CTRL 0xFD1A002C 19 #define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013 20 #define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013 21 #define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013 22 #define ZYNQMP_SIP_SVC_MMIO_READ 0xC2000014 23 #define ZYNQMP_DIV_MAX_VAL 0x3F 24 #define ZYNQMP_DIV1_SHFT 8 25 #define ZYNQMP_DIV1_SHFT 8 26 #define ZYNQMP_DIV2_SHFT 16 27 #define ZYNQMP_DIV_MASK 0x3F 28 #define ZYNQMP_PLL_CTRL_FBDIV_MASK 0x7F 29 #define ZYNQMP_PLL_CTRL_FBDIV_SHFT 8 30 #define ZYNQMP_GEM_REF_CTRL_SRC_MASK 0x7 31 #define ZYNQMP_GEM0_CLK_ID 45 32 #define ZYNQMP_GEM1_CLK_ID 46 33 #define ZYNQMP_GEM2_CLK_ID 47 34 #define ZYNQMP_GEM3_CLK_ID 48 35 36 static unsigned long pss_ref_clk; 37 38 static int zynqmp_calculate_divisors(unsigned long req_rate, 39 unsigned long parent_rate, 40 u32 *div1, u32 *div2) 41 { 42 u32 req_div = 1; 43 u32 i; 44 45 /* 46 * calculate two divisors to get 47 * required rate and each divisor 48 * should be less than 63 49 */ 50 req_div = DIV_ROUND_UP(parent_rate, req_rate); 51 52 for (i = 1; i <= req_div; i++) { 53 if ((req_div % i) == 0) { 54 *div1 = req_div / i; 55 *div2 = i; 56 if ((*div1 < ZYNQMP_DIV_MAX_VAL) && 57 (*div2 < ZYNQMP_DIV_MAX_VAL)) 58 return 0; 59 } 60 } 61 62 return -1; 63 } 64 65 static int zynqmp_get_periph_id(unsigned long id) 66 { 67 int periph_id; 68 69 switch (id) { 70 case ZYNQMP_GEM0_CLK_ID: 71 periph_id = 0; 72 break; 73 case ZYNQMP_GEM1_CLK_ID: 74 periph_id = 1; 75 break; 76 case ZYNQMP_GEM2_CLK_ID: 77 periph_id = 2; 78 break; 79 case ZYNQMP_GEM3_CLK_ID: 80 periph_id = 3; 81 break; 82 default: 83 printf("%s, Invalid clock id:%ld\n", __func__, id); 84 return -EINVAL; 85 } 86 87 return periph_id; 88 } 89 90 static int zynqmp_set_clk(unsigned long id, u32 div1, u32 div2) 91 { 92 struct pt_regs regs; 93 ulong reg; 94 u32 mask, value; 95 96 id = zynqmp_get_periph_id(id); 97 if (id < 0) 98 return -EINVAL; 99 100 reg = (ulong)((u32 *)ZYNQMP_GEM0_REF_CTRL + id); 101 mask = (ZYNQMP_DIV_MASK << ZYNQMP_DIV1_SHFT) | 102 (ZYNQMP_DIV_MASK << ZYNQMP_DIV2_SHFT); 103 value = (div1 << ZYNQMP_DIV1_SHFT) | (div2 << ZYNQMP_DIV2_SHFT); 104 105 debug("%s: reg:0x%lx, mask:0x%x, value:0x%x\n", __func__, reg, mask, 106 value); 107 108 regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_WRITE; 109 regs.regs[1] = ((u64)mask << 32) | reg; 110 regs.regs[2] = value; 111 regs.regs[3] = 0; 112 113 smc_call(®s); 114 115 return regs.regs[0]; 116 } 117 118 static unsigned long zynqmp_clk_get_rate(struct clk *clk) 119 { 120 struct pt_regs regs; 121 ulong reg; 122 unsigned long value; 123 int id; 124 125 id = zynqmp_get_periph_id(clk->id); 126 if (id < 0) 127 return -EINVAL; 128 129 reg = (ulong)((u32 *)ZYNQMP_GEM0_REF_CTRL + id); 130 131 regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_READ; 132 regs.regs[1] = reg; 133 regs.regs[2] = 0; 134 regs.regs[3] = 0; 135 136 smc_call(®s); 137 138 value = upper_32_bits(regs.regs[0]); 139 140 value &= ZYNQMP_GEM_REF_CTRL_SRC_MASK; 141 142 switch (value) { 143 case 0: 144 regs.regs[1] = ZYNQMP_IOPLL_CTRL; 145 break; 146 case 2: 147 regs.regs[1] = ZYNQMP_RPLL_CTRL; 148 break; 149 case 3: 150 regs.regs[1] = ZYNQMP_DPLL_CTRL; 151 break; 152 default: 153 return -EINVAL; 154 } 155 156 regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_READ; 157 regs.regs[2] = 0; 158 regs.regs[3] = 0; 159 160 smc_call(®s); 161 162 value = upper_32_bits(regs.regs[0]) & 163 (ZYNQMP_PLL_CTRL_FBDIV_MASK << 164 ZYNQMP_PLL_CTRL_FBDIV_SHFT); 165 value >>= ZYNQMP_PLL_CTRL_FBDIV_SHFT; 166 value *= pss_ref_clk; 167 168 return value; 169 } 170 171 static ulong zynqmp_clk_set_rate(struct clk *clk, unsigned long clk_rate) 172 { 173 int ret; 174 u32 div1 = 0; 175 u32 div2 = 0; 176 unsigned long input_clk; 177 178 input_clk = zynqmp_clk_get_rate(clk); 179 if (IS_ERR_VALUE(input_clk)) { 180 dev_err(dev, "failed to get input_clk\n"); 181 return -EINVAL; 182 } 183 184 debug("%s: i/p CLK %ld, clk_rate:0x%ld\n", __func__, input_clk, 185 clk_rate); 186 187 ret = zynqmp_calculate_divisors(clk_rate, input_clk, &div1, &div2); 188 if (ret) { 189 dev_err(dev, "failed to proper divisors\n"); 190 return -EINVAL; 191 } 192 193 debug("%s: Div1:%d, Div2:%d\n", __func__, div1, div2); 194 195 ret = zynqmp_set_clk(clk->id, div1, div2); 196 if (ret) { 197 dev_err(dev, "failed to set gem clk\n"); 198 return -EINVAL; 199 } 200 201 return 0; 202 } 203 204 static int zynqmp_clk_probe(struct udevice *dev) 205 { 206 struct clk clk; 207 int ret; 208 209 debug("%s\n", __func__); 210 ret = clk_get_by_name(dev, "pss_ref_clk", &clk); 211 if (ret < 0) { 212 dev_err(dev, "failed to get pss_ref_clk\n"); 213 return ret; 214 } 215 216 pss_ref_clk = clk_get_rate(&clk); 217 if (IS_ERR_VALUE(pss_ref_clk)) { 218 dev_err(dev, "failed to get rate pss_ref_clk\n"); 219 return -EINVAL; 220 } 221 222 return 0; 223 } 224 225 static struct clk_ops zynqmp_clk_ops = { 226 .set_rate = zynqmp_clk_set_rate, 227 .get_rate = zynqmp_clk_get_rate, 228 }; 229 230 static const struct udevice_id zynqmp_clk_ids[] = { 231 { .compatible = "xlnx,zynqmp-clkc" }, 232 { } 233 }; 234 235 U_BOOT_DRIVER(zynqmp_clk) = { 236 .name = "zynqmp-clk", 237 .id = UCLASS_CLK, 238 .of_match = zynqmp_clk_ids, 239 .probe = zynqmp_clk_probe, 240 .ops = &zynqmp_clk_ops, 241 }; 242