BadVPN – Blame information for rev 1
?pathlinks?
Rev | Author | Line No. | Line |
---|---|---|---|
1 | office | 1 | /** |
2 | * @file sys_watch_usb.c |
||
3 | * @author Ambroz Bizjak <ambrop7@gmail.com> |
||
4 | * |
||
5 | * @section LICENSE |
||
6 | * |
||
7 | * Redistribution and use in source and binary forms, with or without |
||
8 | * modification, are permitted provided that the following conditions are met: |
||
9 | * 1. Redistributions of source code must retain the above copyright |
||
10 | * notice, this list of conditions and the following disclaimer. |
||
11 | * 2. Redistributions in binary form must reproduce the above copyright |
||
12 | * notice, this list of conditions and the following disclaimer in the |
||
13 | * documentation and/or other materials provided with the distribution. |
||
14 | * 3. Neither the name of the author nor the |
||
15 | * names of its contributors may be used to endorse or promote products |
||
16 | * derived from this software without specific prior written permission. |
||
17 | * |
||
18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND |
||
19 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
||
20 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
||
21 | * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY |
||
22 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
||
23 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
24 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
||
25 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
||
26 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
||
27 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
||
28 | * |
||
29 | * @section DESCRIPTION |
||
30 | * |
||
31 | * USB device watcher. |
||
32 | * |
||
33 | * Synopsis: sys.watch_usb() |
||
34 | * Description: reports USB device events. Transitions up when an event is detected, and |
||
35 | * goes down waiting for the next event when ->nextevent() is called. |
||
36 | * On startup, "added" events are reported for existing USB devices. |
||
37 | * |
||
38 | * Variables: |
||
39 | * string event_type - what happened with the USB device: "added" or "removed" |
||
40 | * string devname - device node path, e.g. /dev/bus/usb/XXX/YYY |
||
41 | * string vendor_id - vendor ID, e.g. 046d |
||
42 | * string model_id - model ID, e.g. c03e |
||
43 | * |
||
44 | * Synopsis: sys.watch_usb::nextevent() |
||
45 | * Description: makes the watch_usb module transition down in order to report the next event. |
||
46 | */ |
||
47 | |||
48 | #include <inttypes.h> |
||
49 | #include <stdlib.h> |
||
50 | #include <string.h> |
||
51 | #include <stdio.h> |
||
52 | |||
53 | #include <misc/debug.h> |
||
54 | #include <misc/offset.h> |
||
55 | #include <misc/parse_number.h> |
||
56 | #include <structure/LinkedList1.h> |
||
57 | #include <udevmonitor/NCDUdevManager.h> |
||
58 | #include <ncd/modules/event_template.h> |
||
59 | |||
60 | #include <ncd/module_common.h> |
||
61 | |||
62 | #include <generated/blog_channel_ncd_sys_watch_usb.h> |
||
63 | |||
64 | struct device { |
||
65 | char *devname; |
||
66 | char *devpath; |
||
67 | uint16_t vendor_id; |
||
68 | uint16_t model_id; |
||
69 | BStringMap removed_map; |
||
70 | LinkedList1Node devices_list_node; |
||
71 | }; |
||
72 | |||
73 | struct instance { |
||
74 | NCDModuleInst *i; |
||
75 | NCDUdevClient client; |
||
76 | LinkedList1 devices_list; |
||
77 | event_template templ; |
||
78 | }; |
||
79 | |||
80 | static void templ_func_free (struct instance *o, int is_error); |
||
81 | |||
82 | static struct device * find_device_by_devname (struct instance *o, const char *devname) |
||
83 | { |
||
84 | for (LinkedList1Node *list_node = LinkedList1_GetFirst(&o->devices_list); list_node; list_node = LinkedList1Node_Next(list_node)) { |
||
85 | struct device *device = UPPER_OBJECT(list_node, struct device, devices_list_node); |
||
86 | if (!strcmp(device->devname, devname)) { |
||
87 | return device; |
||
88 | } |
||
89 | } |
||
90 | |||
91 | return NULL; |
||
92 | } |
||
93 | |||
94 | static struct device * find_device_by_devpath (struct instance *o, const char *devpath) |
||
95 | { |
||
96 | for (LinkedList1Node *list_node = LinkedList1_GetFirst(&o->devices_list); list_node; list_node = LinkedList1Node_Next(list_node)) { |
||
97 | struct device *device = UPPER_OBJECT(list_node, struct device, devices_list_node); |
||
98 | if (!strcmp(device->devpath, devpath)) { |
||
99 | return device; |
||
100 | } |
||
101 | } |
||
102 | |||
103 | return NULL; |
||
104 | } |
||
105 | |||
106 | static void free_device (struct instance *o, struct device *device, int have_removed_map) |
||
107 | { |
||
108 | // remove from devices list |
||
109 | LinkedList1_Remove(&o->devices_list, &device->devices_list_node); |
||
110 | |||
111 | // free removed map |
||
112 | if (have_removed_map) { |
||
113 | BStringMap_Free(&device->removed_map); |
||
114 | } |
||
115 | |||
116 | // free devpath |
||
117 | free(device->devpath); |
||
118 | |||
119 | // free devname |
||
120 | free(device->devname); |
||
121 | |||
122 | // free structure |
||
123 | free(device); |
||
124 | } |
||
125 | |||
126 | static int make_event_map (struct instance *o, int added, const char *devname, uint16_t vendor_id, uint16_t model_id, BStringMap *out_map) |
||
127 | { |
||
128 | // init map |
||
129 | BStringMap map; |
||
130 | BStringMap_Init(&map); |
||
131 | |||
132 | // set type |
||
133 | if (!BStringMap_Set(&map, "event_type", (added ? "added" : "removed"))) { |
||
134 | ModuleLog(o->i, BLOG_ERROR, "BStringMap_Set failed"); |
||
135 | goto fail1; |
||
136 | } |
||
137 | |||
138 | // set devname |
||
139 | if (!BStringMap_Set(&map, "devname", devname)) { |
||
140 | ModuleLog(o->i, BLOG_ERROR, "BStringMap_Set failed"); |
||
141 | goto fail1; |
||
142 | } |
||
143 | |||
144 | // set vendor ID |
||
145 | char vendor_id_str[5]; |
||
146 | sprintf(vendor_id_str, "%04"PRIx16, vendor_id); |
||
147 | if (!BStringMap_Set(&map, "vendor_id", vendor_id_str)) { |
||
148 | ModuleLog(o->i, BLOG_ERROR, "BStringMap_Set failed"); |
||
149 | goto fail1; |
||
150 | } |
||
151 | |||
152 | // set model ID |
||
153 | char model_id_str[5]; |
||
154 | sprintf(model_id_str, "%04"PRIx16, model_id); |
||
155 | if (!BStringMap_Set(&map, "model_id", model_id_str)) { |
||
156 | ModuleLog(o->i, BLOG_ERROR, "BStringMap_Set failed"); |
||
157 | goto fail1; |
||
158 | } |
||
159 | |||
160 | *out_map = map; |
||
161 | return 1; |
||
162 | |||
163 | fail1: |
||
164 | BStringMap_Free(&map); |
||
165 | return 0; |
||
166 | } |
||
167 | |||
168 | static void queue_event (struct instance *o, BStringMap map) |
||
169 | { |
||
170 | // pass event to template |
||
171 | int was_empty; |
||
172 | event_template_queue(&o->templ, map, &was_empty); |
||
173 | |||
174 | // if event queue was empty, stop receiving udev events |
||
175 | if (was_empty) { |
||
176 | NCDUdevClient_Pause(&o->client); |
||
177 | } |
||
178 | } |
||
179 | |||
180 | static void add_device (struct instance *o, const char *devname, const char *devpath, uint16_t vendor_id, uint16_t model_id) |
||
181 | { |
||
182 | ASSERT(!find_device_by_devname(o, devname)) |
||
183 | ASSERT(!find_device_by_devpath(o, devpath)) |
||
184 | |||
185 | // allocate structure |
||
186 | struct device *device = malloc(sizeof(*device)); |
||
187 | if (!device) { |
||
188 | ModuleLog(o->i, BLOG_ERROR, "malloc failed"); |
||
189 | goto fail0; |
||
190 | } |
||
191 | |||
192 | // init devname |
||
193 | if (!(device->devname = strdup(devname))) { |
||
194 | ModuleLog(o->i, BLOG_ERROR, "strdup failed"); |
||
195 | goto fail1; |
||
196 | } |
||
197 | |||
198 | // init devpath |
||
199 | if (!(device->devpath = strdup(devpath))) { |
||
200 | ModuleLog(o->i, BLOG_ERROR, "strdup failed"); |
||
201 | goto fail2; |
||
202 | } |
||
203 | |||
204 | // set vendor and model ID |
||
205 | device->vendor_id = vendor_id; |
||
206 | device->model_id = model_id; |
||
207 | |||
208 | // init removed map |
||
209 | if (!make_event_map(o, 0, devname, vendor_id, model_id, &device->removed_map)) { |
||
210 | ModuleLog(o->i, BLOG_ERROR, "make_event_map failed"); |
||
211 | goto fail3; |
||
212 | } |
||
213 | |||
214 | // init added map |
||
215 | BStringMap added_map; |
||
216 | if (!make_event_map(o, 1, devname, vendor_id, model_id, &added_map)) { |
||
217 | ModuleLog(o->i, BLOG_ERROR, "make_event_map failed"); |
||
218 | goto fail4; |
||
219 | } |
||
220 | |||
221 | // insert to devices list |
||
222 | LinkedList1_Append(&o->devices_list, &device->devices_list_node); |
||
223 | |||
224 | // queue event |
||
225 | queue_event(o, added_map); |
||
226 | return; |
||
227 | |||
228 | fail4: |
||
229 | BStringMap_Free(&device->removed_map); |
||
230 | fail3: |
||
231 | free(device->devpath); |
||
232 | fail2: |
||
233 | free(device->devname); |
||
234 | fail1: |
||
235 | free(device); |
||
236 | fail0: |
||
237 | ModuleLog(o->i, BLOG_ERROR, "failed to add device %s", devname); |
||
238 | } |
||
239 | |||
240 | static void remove_device (struct instance *o, struct device *device) |
||
241 | { |
||
242 | queue_event(o, device->removed_map); |
||
243 | free_device(o, device, 0); |
||
244 | } |
||
245 | |||
246 | static void next_event (struct instance *o) |
||
247 | { |
||
248 | ASSERT(event_template_is_enabled(&o->templ)) |
||
249 | |||
250 | // order template to finish the current event |
||
251 | int is_empty; |
||
252 | event_template_dequeue(&o->templ, &is_empty); |
||
253 | |||
254 | // if template has no events, continue udev events |
||
255 | if (is_empty) { |
||
256 | NCDUdevClient_Continue(&o->client); |
||
257 | } |
||
258 | } |
||
259 | |||
260 | static void client_handler (struct instance *o, char *devpath, int have_map, BStringMap map) |
||
261 | { |
||
262 | // lookup existing device with this devpath |
||
263 | struct device *ex_device = find_device_by_devpath(o, devpath); |
||
264 | // lookup cache entry |
||
265 | const BStringMap *cache_map = NCDUdevManager_Query(o->i->params->iparams->umanager, devpath); |
||
266 | |||
267 | if (!cache_map) { |
||
268 | if (ex_device) { |
||
269 | remove_device(o, ex_device); |
||
270 | } |
||
271 | goto out; |
||
272 | } |
||
273 | |||
274 | const char *subsystem = BStringMap_Get(cache_map, "SUBSYSTEM"); |
||
275 | const char *devname = BStringMap_Get(cache_map, "DEVNAME"); |
||
276 | const char *devtype = BStringMap_Get(cache_map, "DEVTYPE"); |
||
277 | const char *vendor_id_str = BStringMap_Get(cache_map, "ID_VENDOR_ID"); |
||
278 | const char *model_id_str = BStringMap_Get(cache_map, "ID_MODEL_ID"); |
||
279 | |||
280 | uintmax_t vendor_id; |
||
281 | uintmax_t model_id; |
||
282 | |||
283 | if (!(subsystem && !strcmp(subsystem, "usb") && |
||
284 | devname && |
||
285 | devtype && !strcmp(devtype, "usb_device") && |
||
286 | vendor_id_str && parse_unsigned_hex_integer(MemRef_MakeCstr(vendor_id_str), &vendor_id) && |
||
287 | model_id_str && parse_unsigned_hex_integer(MemRef_MakeCstr(model_id_str), &model_id) |
||
288 | )) { |
||
289 | if (ex_device) { |
||
290 | remove_device(o, ex_device); |
||
291 | } |
||
292 | goto out; |
||
293 | } |
||
294 | |||
295 | if (ex_device && ( |
||
296 | strcmp(ex_device->devname, devname) || |
||
297 | ex_device->vendor_id != vendor_id || ex_device->model_id != model_id |
||
298 | )) { |
||
299 | remove_device(o, ex_device); |
||
300 | ex_device = NULL; |
||
301 | } |
||
302 | |||
303 | if (!ex_device) { |
||
304 | struct device *ex_devname_device = find_device_by_devname(o, devname); |
||
305 | if (ex_devname_device) { |
||
306 | remove_device(o, ex_devname_device); |
||
307 | } |
||
308 | |||
309 | add_device(o, devname, devpath, vendor_id, model_id); |
||
310 | } |
||
311 | |||
312 | out: |
||
313 | free(devpath); |
||
314 | if (have_map) { |
||
315 | BStringMap_Free(&map); |
||
316 | } |
||
317 | } |
||
318 | |||
319 | static void func_new (void *vo, NCDModuleInst *i, const struct NCDModuleInst_new_params *params) |
||
320 | { |
||
321 | struct instance *o = vo; |
||
322 | o->i = i; |
||
323 | |||
324 | // check arguments |
||
325 | if (!NCDVal_ListRead(params->args, 0)) { |
||
326 | ModuleLog(o->i, BLOG_ERROR, "wrong arity"); |
||
327 | goto fail0; |
||
328 | } |
||
329 | |||
330 | // init client |
||
331 | NCDUdevClient_Init(&o->client, o->i->params->iparams->umanager, o, (NCDUdevClient_handler)client_handler); |
||
332 | |||
333 | // init devices list |
||
334 | LinkedList1_Init(&o->devices_list); |
||
335 | |||
336 | event_template_new(&o->templ, o->i, BLOG_CURRENT_CHANNEL, 3, o, (event_template_func_free)templ_func_free); |
||
337 | return; |
||
338 | |||
339 | fail0: |
||
340 | NCDModuleInst_Backend_DeadError(i); |
||
341 | } |
||
342 | |||
343 | static void templ_func_free (struct instance *o, int is_error) |
||
344 | { |
||
345 | // free devices |
||
346 | while (!LinkedList1_IsEmpty(&o->devices_list)) { |
||
347 | struct device *device = UPPER_OBJECT(LinkedList1_GetFirst(&o->devices_list), struct device, devices_list_node); |
||
348 | free_device(o, device, 1); |
||
349 | } |
||
350 | |||
351 | // free client |
||
352 | NCDUdevClient_Free(&o->client); |
||
353 | |||
354 | if (is_error) { |
||
355 | NCDModuleInst_Backend_DeadError(o->i); |
||
356 | } else { |
||
357 | NCDModuleInst_Backend_Dead(o->i); |
||
358 | } |
||
359 | } |
||
360 | |||
361 | static void func_die (void *vo) |
||
362 | { |
||
363 | struct instance *o = vo; |
||
364 | event_template_die(&o->templ); |
||
365 | } |
||
366 | |||
367 | static int func_getvar (void *vo, const char *name, NCDValMem *mem, NCDValRef *out) |
||
368 | { |
||
369 | struct instance *o = vo; |
||
370 | return event_template_getvar(&o->templ, name, mem, out); |
||
371 | } |
||
372 | |||
373 | static void nextevent_func_new (void *unused, NCDModuleInst *i, const struct NCDModuleInst_new_params *params) |
||
374 | { |
||
375 | // check arguments |
||
376 | if (!NCDVal_ListRead(params->args, 0)) { |
||
377 | ModuleLog(i, BLOG_ERROR, "wrong arity"); |
||
378 | goto fail0; |
||
379 | } |
||
380 | |||
381 | // get method object |
||
382 | struct instance *mo = NCDModuleInst_Backend_GetUser((NCDModuleInst *)params->method_user); |
||
383 | |||
384 | // make sure we are currently reporting an event |
||
385 | if (!event_template_is_enabled(&mo->templ)) { |
||
386 | ModuleLog(i, BLOG_ERROR, "not reporting an event"); |
||
387 | goto fail0; |
||
388 | } |
||
389 | |||
390 | // signal up. |
||
391 | // Do it before finishing the event so our process does not advance any further if |
||
392 | // we would be killed the event provider going down. |
||
393 | NCDModuleInst_Backend_Up(i); |
||
394 | |||
395 | // wait for next event |
||
396 | next_event(mo); |
||
397 | return; |
||
398 | |||
399 | fail0: |
||
400 | NCDModuleInst_Backend_DeadError(i); |
||
401 | } |
||
402 | |||
403 | static struct NCDModule modules[] = { |
||
404 | { |
||
405 | .type = "sys.watch_usb", |
||
406 | .func_new2 = func_new, |
||
407 | .func_die = func_die, |
||
408 | .func_getvar = func_getvar, |
||
409 | .alloc_size = sizeof(struct instance) |
||
410 | }, { |
||
411 | .type = "sys.watch_usb::nextevent", |
||
412 | .func_new2 = nextevent_func_new |
||
413 | }, { |
||
414 | .type = NULL |
||
415 | } |
||
416 | }; |
||
417 | |||
418 | const struct NCDModuleGroup ncdmodule_sys_watch_usb = { |
||
419 | .modules = modules |
||
420 | }; |