1 /*
2 * Copyright (c) 2006-2018, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date Author Notes
8 * 2016-12-28 Bernard first version
9 * 2018-03-09 Bernard Add protection for pt->triggered.
10 */
11 #include <stdint.h>
12
13 #include <rthw.h>
14 #include <rtdevice.h>
15 #include <rtthread.h>
16
17 #include <dfs.h>
18 #include <dfs_file.h>
19 #include <dfs_posix.h>
20 #include <dfs_poll.h>
21
22 struct rt_poll_node;
23
24 struct rt_poll_table
25 {
26 rt_pollreq_t req;
27 rt_uint32_t triggered; /* the waited thread whether triggered */
28 rt_thread_t polling_thread;
29 struct rt_poll_node *nodes;
30 };
31
32 struct rt_poll_node
33 {
34 struct rt_wqueue_node wqn;
35 struct rt_poll_table *pt;
36 struct rt_poll_node *next;
37 };
38
__wqueue_pollwake(struct rt_wqueue_node * wait,void * key)39 static int __wqueue_pollwake(struct rt_wqueue_node *wait, void *key)
40 {
41 struct rt_poll_node *pn;
42
43 if (key && !((rt_ubase_t)key & wait->key))
44 return -1;
45
46 pn = rt_container_of(wait, struct rt_poll_node, wqn);
47 pn->pt->triggered = 1;
48
49 return __wqueue_default_wake(wait, key);
50 }
51
_poll_add(rt_wqueue_t * wq,rt_pollreq_t * req)52 static void _poll_add(rt_wqueue_t *wq, rt_pollreq_t *req)
53 {
54 struct rt_poll_table *pt;
55 struct rt_poll_node *node;
56
57 node = rt_malloc(sizeof(struct rt_poll_node));
58 if (node == RT_NULL)
59 return;
60
61 pt = rt_container_of(req, struct rt_poll_table, req);
62
63 node->wqn.key = req->_key;
64 rt_list_init(&(node->wqn.list));
65 node->wqn.polling_thread = pt->polling_thread;
66 node->wqn.wakeup = __wqueue_pollwake;
67 node->next = pt->nodes;
68 node->pt = pt;
69 pt->nodes = node;
70 rt_wqueue_add(wq, &node->wqn);
71 }
72
poll_table_init(struct rt_poll_table * pt)73 static void poll_table_init(struct rt_poll_table *pt)
74 {
75 pt->req._proc = _poll_add;
76 pt->triggered = 0;
77 pt->nodes = RT_NULL;
78 pt->polling_thread = rt_thread_self();
79 }
80
poll_wait_timeout(struct rt_poll_table * pt,int msec)81 static int poll_wait_timeout(struct rt_poll_table *pt, int msec)
82 {
83 rt_int32_t timeout;
84 int ret = 0;
85 struct rt_thread *thread;
86 rt_base_t level;
87
88 thread = pt->polling_thread;
89
90 timeout = rt_tick_from_millisecond(msec);
91
92 level = rt_hw_interrupt_disable();
93
94 if (timeout != 0 && !pt->triggered)
95 {
96 rt_thread_suspend(thread);
97 if (timeout > 0)
98 {
99 rt_timer_control(&(thread->thread_timer),
100 RT_TIMER_CTRL_SET_TIME,
101 &timeout);
102 rt_timer_start(&(thread->thread_timer));
103 }
104
105 rt_hw_interrupt_enable(level);
106
107 rt_schedule();
108
109 level = rt_hw_interrupt_disable();
110 }
111
112 ret = !pt->triggered;
113 rt_hw_interrupt_enable(level);
114
115 return ret;
116 }
117
do_pollfd(struct pollfd * pollfd,rt_pollreq_t * req)118 static int do_pollfd(struct pollfd *pollfd, rt_pollreq_t *req)
119 {
120 int mask = 0;
121 int fd;
122
123 fd = pollfd->fd;
124
125 if (fd >= 0)
126 {
127 struct dfs_fd *f = fd_get(fd);
128 mask = POLLNVAL;
129
130 if (f)
131 {
132 mask = POLLMASK_DEFAULT;
133 if (f->fops->poll)
134 {
135 req->_key = pollfd->events | POLLERR| POLLHUP;
136
137 mask = f->fops->poll(f, req);
138 }
139 /* Mask out unneeded events. */
140 mask &= pollfd->events | POLLERR | POLLHUP;
141 fd_put(f);
142 }
143 }
144 pollfd->revents = mask;
145
146 return mask;
147 }
148
poll_do(struct pollfd * fds,nfds_t nfds,struct rt_poll_table * pt,int msec)149 static int poll_do(struct pollfd *fds, nfds_t nfds, struct rt_poll_table *pt, int msec)
150 {
151 int num;
152 int istimeout = 0;
153 int n;
154 struct pollfd *pf;
155
156 if (msec == 0)
157 {
158 pt->req._proc = RT_NULL;
159 istimeout = 1;
160 }
161
162 while (1)
163 {
164 pf = fds;
165 num = 0;
166
167 for (n = 0; n < nfds; n ++)
168 {
169 if (do_pollfd(pf, &pt->req))
170 {
171 num ++;
172 pt->req._proc = RT_NULL;
173 }
174 pf ++;
175 }
176
177 pt->req._proc = RT_NULL;
178
179 if (num || istimeout)
180 break;
181
182 if (poll_wait_timeout(pt, msec))
183 istimeout = 1;
184 }
185
186 return num;
187 }
188
poll_teardown(struct rt_poll_table * pt)189 static void poll_teardown(struct rt_poll_table *pt)
190 {
191 struct rt_poll_node *node, *next;
192
193 next = pt->nodes;
194 while (next)
195 {
196 node = next;
197 rt_wqueue_remove(&node->wqn);
198 next = node->next;
199 rt_free(node);
200 }
201 }
202
poll(struct pollfd * fds,nfds_t nfds,int timeout)203 int poll(struct pollfd *fds, nfds_t nfds, int timeout)
204 {
205 int num;
206 struct rt_poll_table table;
207
208 poll_table_init(&table);
209
210 num = poll_do(fds, nfds, &table, timeout);
211
212 poll_teardown(&table);
213
214 return num;
215 }
216
217