1 /* 2 * Copyright (C) 2018 Microhip / Atmel Corporation 3 * Wenyou.Yang <wenyou.yang@microchip.com> 4 * 5 * SPDX-License-Identifier: GPL-2.0+ 6 */ 7 8 #include <common.h> 9 #include <clk-uclass.h> 10 #include <dm/device.h> 11 #include <linux/io.h> 12 #include <mach/at91_pmc.h> 13 #include "pmc.h" 14 15 DECLARE_GLOBAL_DATA_PTR; 16 17 #define AT91_USB_CLK_SOURCE_MAX 2 18 #define AT91_USB_CLK_MAX_DIV 15 19 20 struct at91_usb_clk_priv { 21 u32 num_clksource; 22 }; 23 24 static ulong at91_usb_clk_get_rate(struct clk *clk) 25 { 26 struct pmc_platdata *plat = dev_get_platdata(clk->dev); 27 struct at91_pmc *pmc = plat->reg_base; 28 struct clk source; 29 u32 tmp, usbdiv; 30 u8 source_index; 31 int ret; 32 33 tmp = readl(&pmc->pcr); 34 source_index = (tmp >> AT91_PMC_USB_USBS_OFFSET) & 35 AT91_PMC_USB_USBS_MASK; 36 usbdiv = (tmp >> AT91_PMC_USB_DIV_OFFSET) & AT91_PMC_USB_DIV_MASK; 37 38 ret = clk_get_by_index(clk->dev, source_index, &source); 39 if (ret) 40 return 0; 41 42 return clk_get_rate(&source) / (usbdiv + 1); 43 } 44 45 static ulong at91_usb_clk_set_rate(struct clk *clk, ulong rate) 46 { 47 struct pmc_platdata *plat = dev_get_platdata(clk->dev); 48 struct at91_pmc *pmc = plat->reg_base; 49 struct at91_usb_clk_priv *priv = dev_get_priv(clk->dev); 50 struct clk source, best_source; 51 ulong tmp_rate, best_rate = rate, source_rate; 52 int tmp_diff, best_diff = -1; 53 u32 div, best_div = 0; 54 u8 best_source_index = 0; 55 u8 i; 56 u32 tmp; 57 int ret; 58 59 for (i = 0; i < priv->num_clksource; i++) { 60 ret = clk_get_by_index(clk->dev, i, &source); 61 if (ret) 62 return ret; 63 64 source_rate = clk_get_rate(&source); 65 if (IS_ERR_VALUE(source_rate)) 66 return source_rate; 67 68 for (div = 1; div < AT91_USB_CLK_MAX_DIV + 2; div++) { 69 tmp_rate = DIV_ROUND_CLOSEST(source_rate, div); 70 tmp_diff = abs(rate - tmp_rate); 71 72 if (best_diff < 0 || best_diff > tmp_diff) { 73 best_rate = tmp_rate; 74 best_diff = tmp_diff; 75 76 best_div = div - 1; 77 best_source = source; 78 best_source_index = i; 79 } 80 81 if (!best_diff || tmp_rate < rate) 82 break; 83 } 84 85 if (!best_diff) 86 break; 87 } 88 89 debug("AT91 USB: best sourc: %s, best_rate = %ld, best_div = %d\n", 90 best_source.dev->name, best_rate, best_div); 91 92 ret = clk_enable(&best_source); 93 if (ret) 94 return ret; 95 96 tmp = AT91_PMC_USB_USBS_(best_source_index) | 97 AT91_PMC_USB_DIV_(best_div); 98 writel(tmp, &pmc->usb); 99 100 return 0; 101 } 102 103 static struct clk_ops at91_usb_clk_ops = { 104 .get_rate = at91_usb_clk_get_rate, 105 .set_rate = at91_usb_clk_set_rate, 106 }; 107 108 static int at91_usb_clk_ofdata_to_platdata(struct udevice *dev) 109 { 110 struct at91_usb_clk_priv *priv = dev_get_priv(dev); 111 u32 cells[AT91_USB_CLK_SOURCE_MAX]; 112 u32 num_clksource; 113 114 num_clksource = fdtdec_get_int_array_count(gd->fdt_blob, 115 dev_of_offset(dev), 116 "clocks", cells, 117 AT91_USB_CLK_SOURCE_MAX); 118 119 if (!num_clksource) 120 return -1; 121 122 priv->num_clksource = num_clksource; 123 124 return 0; 125 } 126 127 static int at91_usb_clk_probe(struct udevice *dev) 128 { 129 return at91_pmc_core_probe(dev); 130 } 131 132 static const struct udevice_id at91_usb_clk_match[] = { 133 { .compatible = "atmel,at91sam9x5-clk-usb" }, 134 {} 135 }; 136 137 U_BOOT_DRIVER(at91_usb_clk) = { 138 .name = "at91-usb-clk", 139 .id = UCLASS_CLK, 140 .of_match = at91_usb_clk_match, 141 .probe = at91_usb_clk_probe, 142 .ofdata_to_platdata = at91_usb_clk_ofdata_to_platdata, 143 .priv_auto_alloc_size = sizeof(struct at91_usb_clk_priv), 144 .platdata_auto_alloc_size = sizeof(struct pmc_platdata), 145 .ops = &at91_usb_clk_ops, 146 }; 147