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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
|
// SPDX-License-Identifier: GPL-2.0-or-later
/*
* Copyright (c) 2023 Daniel Golle <daniel@makrotopia.org>
*/
/* UBI NVMEM provider */
#include "ubi.h"
#include <linux/nvmem-provider.h>
#include <asm/div64.h>
/* List of all NVMEM devices */
static LIST_HEAD(nvmem_devices);
static DEFINE_MUTEX(devices_mutex);
struct ubi_nvmem {
struct nvmem_device *nvmem;
int ubi_num;
int vol_id;
int usable_leb_size;
struct list_head list;
};
static int ubi_nvmem_reg_read(void *priv, unsigned int from,
void *val, size_t bytes)
{
size_t to_read, bytes_left = bytes;
struct ubi_nvmem *unv = priv;
struct ubi_volume_desc *desc;
uint32_t offs;
uint64_t lnum = from;
int err = 0;
desc = ubi_open_volume(unv->ubi_num, unv->vol_id, UBI_READONLY);
if (IS_ERR(desc))
return PTR_ERR(desc);
offs = do_div(lnum, unv->usable_leb_size);
while (bytes_left) {
to_read = unv->usable_leb_size - offs;
if (to_read > bytes_left)
to_read = bytes_left;
err = ubi_read(desc, lnum, val, offs, to_read);
if (err)
break;
lnum += 1;
offs = 0;
bytes_left -= to_read;
val += to_read;
}
ubi_close_volume(desc);
if (err)
return err;
return bytes_left == 0 ? 0 : -EIO;
}
static int ubi_nvmem_add(struct ubi_volume_info *vi)
{
struct device_node *np = dev_of_node(vi->dev);
struct nvmem_config config = {};
struct ubi_nvmem *unv;
int ret;
if (!np)
return 0;
if (!of_get_child_by_name(np, "nvmem-layout"))
return 0;
if (WARN_ON_ONCE(vi->usable_leb_size <= 0) ||
WARN_ON_ONCE(vi->size <= 0))
return -EINVAL;
unv = kzalloc(sizeof(struct ubi_nvmem), GFP_KERNEL);
if (!unv)
return -ENOMEM;
config.id = NVMEM_DEVID_NONE;
config.dev = vi->dev;
config.name = dev_name(vi->dev);
config.owner = THIS_MODULE;
config.priv = unv;
config.reg_read = ubi_nvmem_reg_read;
config.size = vi->usable_leb_size * vi->size;
config.word_size = 1;
config.stride = 1;
config.read_only = true;
config.root_only = true;
config.ignore_wp = true;
config.of_node = np;
unv->ubi_num = vi->ubi_num;
unv->vol_id = vi->vol_id;
unv->usable_leb_size = vi->usable_leb_size;
unv->nvmem = nvmem_register(&config);
if (IS_ERR(unv->nvmem)) {
ret = dev_err_probe(vi->dev, PTR_ERR(unv->nvmem),
"Failed to register NVMEM device\n");
kfree(unv);
return ret;
}
mutex_lock(&devices_mutex);
list_add_tail(&unv->list, &nvmem_devices);
mutex_unlock(&devices_mutex);
return 0;
}
static void ubi_nvmem_remove(struct ubi_volume_info *vi)
{
struct ubi_nvmem *unv_c, *unv = NULL;
mutex_lock(&devices_mutex);
list_for_each_entry(unv_c, &nvmem_devices, list)
if (unv_c->ubi_num == vi->ubi_num && unv_c->vol_id == vi->vol_id) {
unv = unv_c;
break;
}
if (!unv) {
mutex_unlock(&devices_mutex);
return;
}
list_del(&unv->list);
mutex_unlock(&devices_mutex);
nvmem_unregister(unv->nvmem);
kfree(unv);
}
/**
* nvmem_notify - UBI notification handler.
* @nb: registered notifier block
* @l: notification type
* @ns_ptr: pointer to the &struct ubi_notification object
*/
static int nvmem_notify(struct notifier_block *nb, unsigned long l,
void *ns_ptr)
{
struct ubi_notification *nt = ns_ptr;
switch (l) {
case UBI_VOLUME_RESIZED:
ubi_nvmem_remove(&nt->vi);
fallthrough;
case UBI_VOLUME_ADDED:
ubi_nvmem_add(&nt->vi);
break;
case UBI_VOLUME_SHUTDOWN:
ubi_nvmem_remove(&nt->vi);
break;
default:
break;
}
return NOTIFY_OK;
}
static struct notifier_block nvmem_notifier = {
.notifier_call = nvmem_notify,
};
static int __init ubi_nvmem_init(void)
{
return ubi_register_volume_notifier(&nvmem_notifier, 0);
}
static void __exit ubi_nvmem_exit(void)
{
struct ubi_nvmem *unv, *tmp;
mutex_lock(&devices_mutex);
list_for_each_entry_safe(unv, tmp, &nvmem_devices, list) {
nvmem_unregister(unv->nvmem);
list_del(&unv->list);
kfree(unv);
}
mutex_unlock(&devices_mutex);
ubi_unregister_volume_notifier(&nvmem_notifier);
}
module_init(ubi_nvmem_init);
module_exit(ubi_nvmem_exit);
MODULE_DESCRIPTION("NVMEM layer over UBI volumes");
MODULE_AUTHOR("Daniel Golle");
MODULE_LICENSE("GPL");
|
