Skip to content

Commit b51b118

Browse files
author
Dong Heng
committed
virtualization: add lesson code "02.module"
1 parent c11e59c commit b51b118

File tree

4 files changed

+409
-0
lines changed

4 files changed

+409
-0
lines changed

‎04.virtualization/02.module/Makefile

+15
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
ifneq ($(KERNELRELEASE), )
2+
3+
obj-m := vhw.o
4+
5+
else
6+
7+
KDIR := /lib/modules/$(shell uname -r)/build
8+
9+
all:
10+
$(MAKE) -C $(KDIR) M=$(shell pwd) modules
11+
12+
clean:
13+
rm *.o *.ko *.mod.c *.order *.symvers .tmp_versions .*.o.cmd .*.ko.cmd -rf
14+
15+
endif

‎04.virtualization/02.module/vhw.c

+311
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,311 @@
1+
#include <linux/net.h>
2+
#include <linux/inet.h>
3+
#include <linux/in.h>
4+
#include <linux/kthread.h>
5+
#include <linux/slab.h>
6+
#include <linux/spinlock.h>
7+
#include <linux/errno.h>
8+
#include <linux/kfifo.h>
9+
#include <linux/semaphore.h>
10+
#include <linux/module.h>
11+
12+
#include "vhw_def.h"
13+
14+
static struct task_struct *main_task, *event_task;
15+
static struct socket *main_socket;
16+
static struct sockaddr_in main_sockaddr;
17+
static LIST_HEAD(irq_list);
18+
static DEFINE_MUTEX(list_mutex);
19+
static DEFINE_SEMAPHORE(report_sem);
20+
static DEFINE_SEMAPHORE(sync_sem);
21+
static DEFINE_KFIFO(data_fifo, struct vhw_event, VHW_FIFO_SIZE);
22+
23+
static int vhw_send_data(const void *buffer, int n)
24+
{
25+
int ret;
26+
struct vhw_event event;
27+
28+
if (!buffer || !n)
29+
return -EINVAL;
30+
31+
if (!main_socket)
32+
return -ENOENT;
33+
34+
event.pbuf = buffer;
35+
event.len = n;
36+
37+
ret = kfifo_in(&data_fifo, &event, sizeof(event));
38+
if (ret <= 0) {
39+
printk("in fifo error %d\n", ret);
40+
return ret;
41+
}
42+
43+
up(&report_sem);
44+
down_interruptible(&sync_sem);
45+
46+
return 0;
47+
}
48+
EXPORT_SYMBOL(vhw_send_data);
49+
50+
static int vhw_set_gpio(int num, bool state)
51+
{
52+
int gpio_event[3] = {GPIO_EVENT_ID, num, state};
53+
54+
return vhw_send_data(gpio_event, sizeof(gpio_event));
55+
}
56+
EXPORT_SYMBOL(vhw_set_gpio);
57+
58+
int vhw_register_irq(int id, void (*func)(int id, int val, void *arg), void *arg)
59+
{
60+
struct vhw_irq *peripheral;
61+
62+
peripheral = kzalloc(sizeof(*peripheral), GFP_KERNEL);
63+
if (!peripheral)
64+
return -ENOMEM;
65+
66+
peripheral->id = id;
67+
peripheral->func = func;
68+
peripheral->arg = arg;
69+
INIT_LIST_HEAD(&peripheral->list);
70+
71+
mutex_lock(&list_mutex);
72+
list_add_tail(&peripheral->list, &irq_list);
73+
mutex_unlock(&list_mutex);
74+
75+
return 0;
76+
}
77+
EXPORT_SYMBOL(vhw_register_irq);
78+
79+
void vhw_unregister_irq(int id)
80+
{
81+
struct vhw_irq *peripheral, *n;
82+
83+
mutex_lock(&list_mutex);
84+
list_for_each_entry_safe(peripheral, n, &irq_list, list) {
85+
if (peripheral->id == id) {
86+
list_del(&peripheral->list);
87+
kfree(peripheral);
88+
break;
89+
}
90+
}
91+
mutex_unlock(&list_mutex);
92+
}
93+
EXPORT_SYMBOL(vhw_unregister_irq);
94+
95+
static int vhw_main_entry(void *p)
96+
{
97+
int ret;
98+
int loop;
99+
struct socket *socket;
100+
struct ip_mreq mreq;
101+
struct sockaddr_in sockaddr;
102+
103+
ret = sock_create(PF_INET, SOCK_DGRAM, 0, &socket);
104+
if (ret)
105+
goto create_fail;
106+
main_socket = socket;
107+
108+
memset(&sockaddr, 0, sizeof(sockaddr));
109+
sockaddr.sin_family = PF_INET;
110+
sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
111+
sockaddr.sin_port = htons(VHW_UDP_PORT);
112+
ret = kernel_bind(socket, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
113+
if (ret)
114+
goto bind_fail;
115+
116+
loop = 0;
117+
ret = kernel_setsockopt(socket, IPPROTO_IP, IP_MULTICAST_LOOP, (char *)&loop, sizeof(loop));
118+
if (ret)
119+
goto setopt_fail1;
120+
121+
mreq.imr_multiaddr.s_addr = in_aton(VHW_GROUP);
122+
mreq.imr_interface.s_addr = htons(INADDR_ANY);
123+
ret = kernel_setsockopt(socket, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq, sizeof(mreq));
124+
if (ret)
125+
goto setopt_fail2;
126+
127+
//iov_len = count = sock_recvmsg.size ?
128+
while (1) {
129+
char buf[128];
130+
struct iovec iov[1] = {
131+
{
132+
.iov_base = buf,
133+
.iov_len = 127
134+
}
135+
};
136+
struct msghdr msg = {
137+
.msg_name = &main_sockaddr,
138+
.msg_namelen = sizeof(main_sockaddr),
139+
.msg_iter = {
140+
.type = ITER_IOVEC,
141+
.iov_offset = 0,
142+
.count = 127,
143+
.iov = iov,
144+
.nr_segs = 0,
145+
},
146+
.msg_control = NULL,
147+
.msg_controllen = 0,
148+
.msg_flags = 0
149+
};
150+
151+
memset(buf, 0, 128);
152+
153+
ret = sock_recvmsg(socket, &msg, 0);
154+
if (ret > 0) {
155+
int num, id, val;
156+
struct vhw_irq *peripheral;
157+
158+
if (ret < 8) {
159+
printk("package length error\n");
160+
continue;
161+
}
162+
163+
ret = sscanf(buf, "%d", &num);
164+
if (ret != 1) {
165+
printk("package payload error\n");
166+
continue;
167+
}
168+
169+
id = num % 10000;
170+
val = num / 10000;
171+
172+
if (id < 0 && id > VHW_IRQ_ID_MAX)
173+
continue;
174+
175+
printk("receive message %s type is %d\n", buf, id);
176+
177+
mutex_lock(&list_mutex);
178+
list_for_each_entry(peripheral, &irq_list, list) {
179+
if (peripheral->id == id) {
180+
peripheral->func(id, val, peripheral->arg);
181+
break;
182+
}
183+
}
184+
mutex_unlock(&list_mutex);
185+
} else if (ret <= 0) {
186+
printk("receive error %d\n", ret);
187+
break;
188+
}
189+
}
190+
191+
setopt_fail2:
192+
printk("enbale multicase error\n");
193+
setopt_fail1:
194+
printk("disable multicase loop back error\n");
195+
bind_fail:
196+
printk("release socket %p\n", socket);
197+
main_socket = NULL;
198+
sock_release(socket);
199+
create_fail:
200+
printk("main thread exit\n");
201+
return -1;
202+
}
203+
204+
static int vhw_send_udp(const void *p, int n)
205+
{
206+
int ret;
207+
struct sockaddr_in sockaddr;
208+
struct iovec iov[1] = {
209+
{
210+
.iov_base = (void *)p,
211+
.iov_len = n
212+
}
213+
};
214+
struct msghdr msg = {
215+
.msg_name = &sockaddr,
216+
.msg_namelen = sizeof(main_sockaddr),
217+
.msg_iter = {
218+
.type = ITER_IOVEC,
219+
.iov_offset = 0,
220+
.count = n,
221+
.iov = iov,
222+
.nr_segs = 0,
223+
},
224+
.msg_control = NULL,
225+
.msg_controllen = 0,
226+
.msg_flags = 0,
227+
.msg_iocb = NULL
228+
};
229+
230+
memset(&sockaddr, 0, sizeof(sockaddr));
231+
sockaddr.sin_family = PF_INET;
232+
sockaddr.sin_addr.s_addr = in_aton(VHW_GROUP);
233+
sockaddr.sin_port = htons(VHW_UDP_PORT);
234+
235+
ret = sock_sendmsg(main_socket, &msg);
236+
if (ret <= 0) {
237+
printk("send message error %d, pbuf is %p, len is %lu, count %lu\n", ret,
238+
iov[0].iov_base, iov[0].iov_len, msg.msg_iter.count);
239+
}
240+
241+
return 0;
242+
}
243+
244+
static int vhw_event_task(void *p)
245+
{
246+
while (1) {
247+
int ret;
248+
struct vhw_event event;
249+
unsigned int len;
250+
251+
ret = down_interruptible(&report_sem);
252+
if (ret) {
253+
printk("down semaphore error %d\n", ret);
254+
break;
255+
}
256+
257+
len = kfifo_out(&data_fifo, &event, sizeof(event));
258+
if (!len) {
259+
printk("no fifo data\n");
260+
break;
261+
}
262+
263+
vhw_send_udp(event.pbuf, event.len);
264+
up(&sync_sem);
265+
}
266+
267+
return 0;
268+
}
269+
270+
__init static int vhw_init(void)
271+
{
272+
down(&report_sem);
273+
down(&sync_sem);
274+
275+
main_task = kthread_create(vhw_main_entry, NULL, "virtual_board%d", 1);
276+
if (!main_task)
277+
goto board_thread_fail;
278+
279+
event_task = kthread_create(vhw_event_task, NULL, "put_board%d", 1);
280+
if (!event_task)
281+
goto put_thread_fail;
282+
283+
wake_up_process(main_task);
284+
wake_up_process(event_task);
285+
286+
printk("VHW initialize OK\n");
287+
288+
return 0;
289+
290+
put_thread_fail:
291+
printk("event thread fail\n");
292+
kthread_stop(main_task);
293+
main_task = NULL;
294+
board_thread_fail:
295+
printk("main thread fail\n");
296+
return -ENOMEM;
297+
}
298+
299+
__exit static void vhw_deinit(void)
300+
{
301+
if (main_socket) {
302+
printk("shutdown socket %p\n", main_socket);
303+
main_socket->ops->shutdown(main_socket, SHUT_RDWR);
304+
}
305+
306+
printk("VHW deinitialize OK\n");
307+
}
308+
309+
module_init(vhw_init);
310+
module_exit(vhw_deinit);
311+
MODULE_LICENSE("GPL");

‎04.virtualization/02.module/vhw.h

+52
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
#ifndef _VHW_H_
2+
#define _VHW_H_
3+
4+
#include "vhw_def.h"
5+
6+
/*
7+
* @bref virtual hardware set UDP data
8+
*
9+
* @param buffer data point
10+
* @param n data size
11+
*
12+
* @return the result
13+
* 0 : OK
14+
* other : fail
15+
*/
16+
int vhw_send_data(const void *buffer, int n);
17+
18+
/*
19+
* @bref virtual hardware set gpio state
20+
*
21+
* @param gpio gpio number
22+
* @param set gpio state
23+
*
24+
* @return the result
25+
* 0 : OK
26+
* other : fail
27+
*/
28+
int vhw_set_gpio(int gpio, bool set);
29+
30+
/*
31+
* @bref virtual hardware register a IRQ
32+
*
33+
* @param id id number
34+
* @param func IRQ callback function
35+
* @arg
36+
*
37+
* @return the result
38+
* 0 : OK
39+
* other : fail
40+
*/
41+
int vhw_register_irq(int id, void (*func)(int id, int val, void *arg), void *arg);
42+
43+
/*
44+
* @bref virtual hardware unregister a IRQ
45+
*
46+
* @param id id number
47+
*
48+
* @return none
49+
*/
50+
void vhw_unregister_irq(int id);
51+
52+
#endif /* _VHW_H_ */

0 commit comments

Comments
 (0)