1 // SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause
2 /*
3 * Copyright (C) 2021 Microchip
4 */
5
6 #include <dt-bindings/clock/at91.h>
7 #include <kernel/panic.h>
8 #include <malloc.h>
9 #include <string.h>
10 #include <types_ext.h>
11
12 #include "at91_clk.h"
13
pmc_clk_get_by_id(struct pmc_clk * clks,unsigned int nclk,unsigned int id)14 static struct clk *pmc_clk_get_by_id(struct pmc_clk *clks, unsigned int nclk,
15 unsigned int id)
16 {
17 unsigned int i = 0;
18
19 for (i = 0; i < nclk; i++) {
20 if (clks[i].clk && clks[i].id == id)
21 return clks[i].clk;
22 }
23
24 return NULL;
25 }
26
pmc_clk_get_by_name(struct pmc_clk * clks,unsigned int nclk,const char * name)27 struct clk *pmc_clk_get_by_name(struct pmc_clk *clks, unsigned int nclk,
28 const char *name)
29 {
30 unsigned int i = 0;
31
32 for (i = 0; i < nclk; i++)
33 if (strcmp(clks[i].clk->name, name) == 0)
34 return clks[i].clk;
35
36 return NULL;
37 }
38
clk_dt_pmc_get(struct dt_driver_phandle_args * clkspec,void * data,TEE_Result * res)39 struct clk *clk_dt_pmc_get(struct dt_driver_phandle_args *clkspec, void *data,
40 TEE_Result *res)
41 {
42 unsigned int type = clkspec->args[0];
43 unsigned int idx = clkspec->args[1];
44 struct pmc_data *pmc_data = data;
45 struct pmc_clk *clks = NULL;
46 struct clk *clk = NULL;
47 unsigned int nclk = 0;
48 *res = TEE_ERROR_GENERIC;
49
50 if (clkspec->args_count != 2) {
51 *res = TEE_ERROR_BAD_PARAMETERS;
52 return NULL;
53 }
54
55 switch (type) {
56 case PMC_TYPE_CORE:
57 nclk = pmc_data->ncore;
58 clks = pmc_data->chws;
59 break;
60 case PMC_TYPE_SYSTEM:
61 nclk = pmc_data->nsystem;
62 clks = pmc_data->shws;
63 break;
64 case PMC_TYPE_PERIPHERAL:
65 nclk = pmc_data->nperiph;
66 clks = pmc_data->phws;
67 break;
68 case PMC_TYPE_GCK:
69 nclk = pmc_data->ngck;
70 clks = pmc_data->ghws;
71 break;
72 case PMC_TYPE_PROGRAMMABLE:
73 nclk = pmc_data->npck;
74 clks = pmc_data->pchws;
75 break;
76 default:
77 return NULL;
78 }
79
80 clk = pmc_clk_get_by_id(clks, nclk, idx);
81 if (clk)
82 *res = TEE_SUCCESS;
83
84 return clk;
85 }
86
pmc_data_allocate(unsigned int ncore,unsigned int nsystem,unsigned int nperiph,unsigned int ngck,unsigned int npck)87 struct pmc_data *pmc_data_allocate(unsigned int ncore, unsigned int nsystem,
88 unsigned int nperiph, unsigned int ngck,
89 unsigned int npck)
90 {
91 unsigned int num_clks = ncore + nsystem + nperiph + ngck + npck;
92 unsigned int alloc_size = sizeof(struct pmc_data) +
93 num_clks * sizeof(struct pmc_clk);
94 struct pmc_data *pmc_data = NULL;
95
96 pmc_data = calloc(1, alloc_size);
97 if (!pmc_data)
98 return NULL;
99
100 pmc_data->ncore = ncore;
101 pmc_data->chws = pmc_data->hwtable;
102
103 pmc_data->nsystem = nsystem;
104 pmc_data->shws = pmc_data->chws + ncore;
105
106 pmc_data->nperiph = nperiph;
107 pmc_data->phws = pmc_data->shws + nsystem;
108
109 pmc_data->ngck = ngck;
110 pmc_data->ghws = pmc_data->phws + nperiph;
111
112 pmc_data->npck = npck;
113 pmc_data->pchws = pmc_data->ghws + ngck;
114
115 return pmc_data;
116 }
117