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
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
|
// SPDX-License-Identifier: GPL-2.0
/*
* Google LWIS Test Device Driver
*
* Copyright (c) 2022 Google, LLC
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*/
#define pr_fmt(fmt) KBUILD_MODNAME "-test-dev: " fmt
#include "lwis_device_test.h"
#include <linux/module.h>
#include <linux/platform_device.h>
#include "lwis_commands.h"
#include "lwis_device.h"
#include "lwis_init.h"
#include "lwis_platform.h"
#ifdef CONFIG_OF
#include "lwis_dt.h"
#endif
#define LWIS_DRIVER_NAME "lwis-test"
static int lwis_test_device_enable(struct lwis_device *lwis_dev);
static int lwis_test_device_disable(struct lwis_device *lwis_dev);
static int lwis_test_register_io(struct lwis_device *lwis_dev, struct lwis_io_entry *entry,
int access_size);
static struct lwis_device_subclass_operations test_vops = {
.register_io = lwis_test_register_io,
.register_io_barrier = NULL,
.device_enable = lwis_test_device_enable,
.device_disable = lwis_test_device_disable,
.event_enable = NULL,
.event_flags_updated = NULL,
.close = NULL,
};
static struct lwis_event_subscribe_operations test_subscribe_ops = {
.subscribe_event = NULL,
.unsubscribe_event = NULL,
.notify_event_subscriber = NULL,
.release = NULL,
};
static int lwis_test_device_enable(struct lwis_device *lwis_dev)
{
return 0;
}
static int lwis_test_device_disable(struct lwis_device *lwis_dev)
{
return 0;
}
static int lwis_test_register_io(struct lwis_device *lwis_dev, struct lwis_io_entry *entry,
int access_size)
{
struct lwis_test_device *test_dev =
container_of(lwis_dev, struct lwis_test_device, base_dev);
struct lwis_io_entry_rw_batch *rw_batch;
int i;
uint64_t reg_value;
if (!entry) {
dev_err(test_dev->base_dev.dev, "IO entry is NULL.\n");
return -EINVAL;
}
lwis_save_register_io_info(lwis_dev, entry, access_size);
if (entry->type == LWIS_IO_ENTRY_READ) {
if (entry->rw.offset >= SCRATCH_TEST_DEV_MEMORY_SIZE) {
dev_err(test_dev->base_dev.dev, "Offset (%llu) must be < %d\n",
entry->rw.offset, SCRATCH_TEST_DEV_MEMORY_SIZE);
return -EINVAL;
}
entry->rw.val = test_dev->scratch_mem[entry->rw.offset];
} else if (entry->type == LWIS_IO_ENTRY_READ_BATCH) {
rw_batch = &entry->rw_batch;
if (rw_batch->offset >= SCRATCH_TEST_DEV_MEMORY_SIZE ||
SCRATCH_TEST_DEV_MEMORY_SIZE - rw_batch->offset < rw_batch->size_in_bytes) {
dev_err(test_dev->base_dev.dev,
"Read range[offset(%llu) + size_in_bytes(%zu)] exceeds scratch memory (%d)\n",
rw_batch->offset, rw_batch->size_in_bytes,
SCRATCH_TEST_DEV_MEMORY_SIZE);
return -EINVAL;
}
for (i = 0; i < rw_batch->size_in_bytes; ++i) {
rw_batch->buf[i] = test_dev->scratch_mem[rw_batch->offset + i];
}
} else if (entry->type == LWIS_IO_ENTRY_WRITE) {
if (entry->rw.offset >= SCRATCH_TEST_DEV_MEMORY_SIZE) {
dev_err(test_dev->base_dev.dev, "Offset (%llu) must be < %d\n",
entry->rw.offset, SCRATCH_TEST_DEV_MEMORY_SIZE);
return -EINVAL;
}
test_dev->scratch_mem[entry->rw.offset] = entry->rw.val;
} else if (entry->type == LWIS_IO_ENTRY_WRITE_BATCH) {
rw_batch = &entry->rw_batch;
if (rw_batch->offset >= SCRATCH_TEST_DEV_MEMORY_SIZE ||
SCRATCH_TEST_DEV_MEMORY_SIZE - rw_batch->offset < rw_batch->size_in_bytes) {
dev_err(test_dev->base_dev.dev,
"Write range[offset(%llu) + size_in_bytes(%zu)] exceeds scratch memory (%d)\n",
rw_batch->offset, rw_batch->size_in_bytes,
SCRATCH_TEST_DEV_MEMORY_SIZE);
return -EINVAL;
}
for (i = 0; i < rw_batch->size_in_bytes; ++i) {
test_dev->scratch_mem[rw_batch->offset + i] = rw_batch->buf[i];
}
} else if (entry->type == LWIS_IO_ENTRY_MODIFY) {
if (entry->mod.offset >= SCRATCH_TEST_DEV_MEMORY_SIZE) {
dev_err(test_dev->base_dev.dev, "Offset (%llu) must be < %d\n",
entry->mod.offset, SCRATCH_TEST_DEV_MEMORY_SIZE);
return -EINVAL;
}
reg_value = test_dev->scratch_mem[entry->mod.offset];
reg_value &= ~entry->mod.val_mask;
reg_value |= entry->mod.val_mask & entry->mod.val;
test_dev->scratch_mem[entry->rw.offset] = reg_value;
} else {
dev_err(test_dev->base_dev.dev, "Invalid IO entry type: %d\n", entry->type);
return -EINVAL;
}
return 0;
}
static int lwis_test_device_setup(struct lwis_test_device *test_dev)
{
int ret = 0;
#ifdef CONFIG_OF
/* Parse device tree for device configurations */
ret = lwis_test_device_parse_dt(test_dev);
if (ret) {
dev_err(test_dev->base_dev.dev, "Failed to parse device tree\n");
}
#else
/* Non-device-tree init: Save for future implementation */
ret = -ENOSYS;
#endif
return ret;
}
static int lwis_test_device_probe(struct platform_device *plat_dev)
{
int ret = 0;
struct lwis_test_device *test_dev;
struct device *dev = &plat_dev->dev;
/* Allocate test device specific data construct */
test_dev = devm_kzalloc(dev, sizeof(struct lwis_test_device), GFP_KERNEL);
if (!test_dev) {
dev_err(dev, "Failed to allocate TEST device structure\n");
return -ENOMEM;
}
test_dev->base_dev.type = DEVICE_TYPE_TEST;
test_dev->base_dev.vops = test_vops;
test_dev->base_dev.subscribe_ops = test_subscribe_ops;
/* Call the base device probe function */
ret = lwis_base_probe(&test_dev->base_dev, plat_dev);
if (ret) {
dev_err(dev, "TEST device: Error in lwis base probe: %d\n", ret);
goto error_probe;
}
/* Call TEST device specific setup function */
ret = lwis_test_device_setup(test_dev);
if (ret) {
dev_err(test_dev->base_dev.dev, "Error in TEST device initialization\n");
lwis_base_unprobe(&test_dev->base_dev);
goto error_probe;
}
dev_info(test_dev->base_dev.dev, "TEST Device Probe: Success\n");
return 0;
error_probe:
return ret;
}
#ifdef CONFIG_OF
static const struct of_device_id lwis_id_match[] = {
{ .compatible = LWIS_TEST_DEVICE_COMPAT },
{},
};
// MODULE_DEVICE_TABLE(of, lwis_id_match);
static struct platform_driver lwis_driver = {
.probe = lwis_test_device_probe,
.driver = {
.name = LWIS_DRIVER_NAME,
.owner = THIS_MODULE,
.of_match_table = lwis_id_match,
},
};
#else /* CONFIG_OF not defined */
static struct platform_device_id lwis_driver_id[] = {
{
.name = LWIS_DRIVER_NAME,
.driver_data = 0,
},
{},
};
MODULE_DEVICE_TABLE(platform, lwis_driver_id);
static struct platform_driver lwis_driver = { .probe = lwis_test_device_probe,
.id_table = lwis_driver_id,
.driver = {
.name = LWIS_DRIVER_NAME,
.owner = THIS_MODULE,
} };
#endif /* CONFIG_OF */
/*
* lwis_test_device_init: Init function that will be called by the kernel
* initialization routines.
*/
int __init lwis_test_device_init(void)
{
int ret = 0;
pr_info("TEST device initialization\n");
ret = platform_driver_register(&lwis_driver);
if (ret)
pr_err("platform_driver_register failed: %d\n", ret);
return ret;
}
int lwis_test_device_deinit(void)
{
platform_driver_unregister(&lwis_driver);
return 0;
}
|
