1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
|
/*
* SPDX-License-Identifier: BSD-2-Clause
*
* Copyright (c) 2023 Ventana Micro Systems Inc.
*
* Authors:
* Anup Patel <apatel@ventanamicro.com>
*/
#include <sbi/sbi_error.h>
#include <sbi_utils/regmap/regmap.h>
static SBI_LIST_HEAD(regmap_list);
struct regmap *regmap_find(unsigned int id)
{
struct sbi_dlist *pos;
sbi_list_for_each(pos, &(regmap_list)) {
struct regmap *rmap = to_regmap(pos);
if (rmap->id == id)
return rmap;
}
return NULL;
}
int regmap_add(struct regmap *rmap)
{
if (!rmap)
return SBI_EINVAL;
if (regmap_find(rmap->id))
return SBI_EALREADY;
sbi_list_add(&(rmap->node), &(regmap_list));
return 0;
}
void regmap_remove(struct regmap *rmap)
{
if (!rmap)
return;
sbi_list_del(&(rmap->node));
}
static bool regmap_reg_valid(struct regmap *rmap, unsigned int reg)
{
if ((reg >= rmap->reg_max) ||
(reg & (rmap->reg_stride - 1)))
return false;
return true;
}
static unsigned int regmap_reg_addr(struct regmap *rmap, unsigned int reg)
{
reg += rmap->reg_base;
if (rmap->reg_shift > 0)
reg >>= rmap->reg_shift;
else if (rmap->reg_shift < 0)
reg <<= -(rmap->reg_shift);
return reg;
}
int regmap_read(struct regmap *rmap, unsigned int reg, unsigned int *val)
{
if (!rmap || !regmap_reg_valid(rmap, reg))
return SBI_EINVAL;
if (!rmap->reg_read)
return SBI_ENOSYS;
return rmap->reg_read(rmap, regmap_reg_addr(rmap, reg), val);
}
int regmap_write(struct regmap *rmap, unsigned int reg, unsigned int val)
{
if (!rmap || !regmap_reg_valid(rmap, reg))
return SBI_EINVAL;
if (!rmap->reg_write)
return SBI_ENOSYS;
return rmap->reg_write(rmap, regmap_reg_addr(rmap, reg), val);
}
int regmap_update_bits(struct regmap *rmap, unsigned int reg,
unsigned int mask, unsigned int val)
{
int rc;
unsigned int reg_val;
if (!rmap || !regmap_reg_valid(rmap, reg))
return SBI_EINVAL;
if (rmap->reg_update_bits) {
return rmap->reg_update_bits(rmap, regmap_reg_addr(rmap, reg),
mask, val);
} else if (rmap->reg_read && rmap->reg_write) {
reg = regmap_reg_addr(rmap, reg);
rc = rmap->reg_read(rmap, reg, ®_val);
if (rc)
return rc;
reg_val &= ~mask;
reg_val |= val & mask;
return rmap->reg_write(rmap, reg, reg_val);
}
return SBI_ENOSYS;
}
|
