1 // SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause
2 /*
3 * Copyright (C) 2018 Microchip Technology Inc,
4 * Codrin Ciubotariu <codrin.ciubotariu@microchip.com>
5 */
6
7 #include <io.h>
8 #include <kernel/delay.h>
9 #include <kernel/panic.h>
10 #include <mm/core_memprot.h>
11 #include <sam_sfr.h>
12 #include <types_ext.h>
13
14 #include "at91_clk.h"
15
16 struct clk_i2s_mux {
17 vaddr_t sfr_base;
18 uint8_t bus_id;
19 };
20
clk_i2s_mux_get_parent(struct clk * clk)21 static size_t clk_i2s_mux_get_parent(struct clk *clk)
22 {
23 struct clk_i2s_mux *mux = clk->priv;
24 uint32_t val = io_read32(mux->sfr_base + AT91_SFR_I2SCLKSEL);
25
26 return (val & BIT(mux->bus_id)) >> mux->bus_id;
27 }
28
clk_i2s_mux_set_parent(struct clk * clk,size_t index)29 static TEE_Result clk_i2s_mux_set_parent(struct clk *clk, size_t index)
30 {
31 struct clk_i2s_mux *mux = clk->priv;
32
33 io_clrsetbits32(mux->sfr_base + AT91_SFR_I2SCLKSEL,
34 BIT(mux->bus_id), index << mux->bus_id);
35
36 return TEE_SUCCESS;
37 }
38
39 static const struct clk_ops clk_i2s_mux_ops = {
40 .get_parent = clk_i2s_mux_get_parent,
41 .set_parent = clk_i2s_mux_set_parent,
42 };
43
44 struct clk *
at91_clk_i2s_mux_register(const char * name,struct clk ** parents,unsigned int num_parents,uint8_t bus_id)45 at91_clk_i2s_mux_register(const char *name, struct clk **parents,
46 unsigned int num_parents, uint8_t bus_id)
47 {
48 struct clk_i2s_mux *i2s_ck = NULL;
49 struct clk *clk = NULL;
50
51 clk = clk_alloc(name, &clk_i2s_mux_ops, parents, num_parents);
52 if (!clk)
53 return NULL;
54
55 i2s_ck = calloc(1, sizeof(*i2s_ck));
56 if (!i2s_ck) {
57 clk_free(clk);
58 return NULL;
59 }
60
61 i2s_ck->bus_id = bus_id;
62 i2s_ck->sfr_base = sam_sfr_base();
63
64 clk->priv = i2s_ck;
65
66 if (clk_register(clk)) {
67 clk_free(clk);
68 free(i2s_ck);
69 return NULL;
70 }
71
72 return clk;
73 }
74