source: rtems/cpukit/libdrvmgr/drvmgr.c @ 418149c8

Last change on this file since 418149c8 was 418149c8, checked in by Daniel Hellstrom <daniel@…>, on Feb 1, 2017 at 9:48:38 AM

libdrvmgr: added default BSP init level hook

  • Property mode set to 100644
File size: 18.0 KB
Line 
1/* Driver Manager Interface Implementation.
2 *
3 * COPYRIGHT (c) 2009 Cobham Gaisler AB.
4 *
5 * The license and distribution terms for this file may be
6 * found in the file LICENSE in this distribution or at
7 * http://www.rtems.org/license/LICENSE.
8 */
9
10#include <stdio.h>
11#include <stdlib.h>
12#include <string.h>
13
14#include <drvmgr/drvmgr.h>
15#include <drvmgr/drvmgr_confdefs.h>
16
17#include <rtems/sysinit.h>
18
19#include "drvmgr_internal.h"
20
21/* Enable debugging */
22/*#define DEBUG 1*/
23
24#ifdef DEBUG
25#define DBG(x...) printk(x)
26#else
27#define DBG(x...)
28#endif
29
30struct drvmgr drvmgr = {
31        .level =                0,
32        .initializing_objs =    0,
33        .lock =                 0,
34        .root_dev =             {0},
35        .root_drv =             NULL,
36
37        .drivers =      LIST_INITIALIZER(struct drvmgr_drv, next),
38
39        .buses = {
40                LIST_INITIALIZER(struct drvmgr_bus, next),
41                LIST_INITIALIZER(struct drvmgr_bus, next),
42                LIST_INITIALIZER(struct drvmgr_bus, next),
43                LIST_INITIALIZER(struct drvmgr_bus, next),
44                LIST_INITIALIZER(struct drvmgr_bus, next),
45        },
46        .buses_inactive =       LIST_INITIALIZER(struct drvmgr_bus, next),
47
48        .devices = {
49                LIST_INITIALIZER(struct drvmgr_dev, next),
50                LIST_INITIALIZER(struct drvmgr_dev, next),
51                LIST_INITIALIZER(struct drvmgr_dev, next),
52                LIST_INITIALIZER(struct drvmgr_dev, next),
53                LIST_INITIALIZER(struct drvmgr_dev, next),
54        },
55        .devices_inactive =     LIST_INITIALIZER(struct drvmgr_dev, next),
56};
57
58static int do_bus_init(
59        struct drvmgr *mgr,
60        struct drvmgr_bus *bus,
61        int level);
62static int do_dev_init(
63        struct drvmgr *mgr,
64        struct drvmgr_dev *dev,
65        int level);
66
67/* DRIVER MANAGER */
68
69void bsp_driver_level_hook(int level) __attribute__((weak));
70
71/* default hook does nothing, BSP may override. */
72void bsp_driver_level_hook(int level)
73{
74}
75
76void _DRV_Manager_init_level(int level)
77{
78        struct drvmgr *mgr = &drvmgr;
79
80        if (mgr->level >= level)
81                return;
82
83        /* Set new Level */
84        mgr->level = level;
85
86        /* Initialize buses and devices into this new level */
87        drvmgr_init_update();
88
89        bsp_driver_level_hook(level);
90}
91
92/* Initialize Data structures of the driver manager and call driver
93 * register functions configured by the user.
94 */
95void _DRV_Manager_initialization(void)
96{
97        drvmgr_drv_reg_func *drvreg;
98
99        /* drvmgr is already initialized statically by compiler except
100         * the lock
101         */
102        DRVMGR_LOCK_INIT();
103
104        /* Call driver register functions. */
105        drvreg = &drvmgr_drivers[0];
106        while (*drvreg) {
107                /* Make driver register */
108                (*drvreg)();
109                drvreg++;
110        }
111}
112
113/* Take ready devices and buses into the correct init level step by step.
114 * Once a bus or a device has been registered there is no turning
115 * back - they are taken to the level of the driver manager.
116 */
117void drvmgr_init_update(void)
118{
119        struct drvmgr *mgr = &drvmgr;
120        struct drvmgr_bus *bus;
121        struct drvmgr_dev *dev;
122        int bus_might_been_registered;
123        int level;
124
125        /* "Lock" to make sure we don't use up the stack and that the lists
126         * remain consistent.
127         */
128        DRVMGR_LOCK_WRITE();
129        if (mgr->initializing_objs || (mgr->level == 0))
130                goto out;
131        mgr->initializing_objs = 1;
132
133        /* Take all buses and devices ready into the same stage
134         * as the driver manager global level.
135         */
136        for (level = 0; level < mgr->level; level++) {
137
138                bus_might_been_registered = 0;
139
140                /* Take buses into next level */
141
142                while ((bus = BUS_LIST_HEAD(&mgr->buses[level])) != NULL) {
143
144                        /* Remove first in the list (will be inserted in
145                         * appropriate list by do_bus_init())
146                         */
147                        drvmgr_list_remove_head(&mgr->buses[level]);
148
149                        DRVMGR_UNLOCK();
150
151                        /* Initialize Bus, this will register devices on
152                         * the bus. Take bus into next level.
153                         */
154                        do_bus_init(mgr, bus, level+1);
155
156                        DRVMGR_LOCK_WRITE();
157                }
158
159                /* Take devices into next level */
160                while ((dev = DEV_LIST_HEAD(&mgr->devices[level])) != NULL) {
161
162                        /* Always process first in list */
163                        dev = DEV_LIST_HEAD(&mgr->devices[level]);
164
165                        /* Remove first in the list (will be inserted in
166                         * appropriate list by do_dev_init())
167                         */
168                        drvmgr_list_remove_head(&mgr->devices[level]);
169
170                        DRVMGR_UNLOCK();
171
172                        /* Initialize Device, this may register a new bus */
173                        do_dev_init(mgr, dev, level+1);
174
175                        DRVMGR_LOCK_WRITE();
176
177                        bus_might_been_registered = 1;
178                }
179
180                /* Make sure all buses registered and ready are taken at
181                 * the same time into init level N.
182                 */
183                if (bus_might_been_registered) {
184                        level = -1; /* restart loop */
185                }
186        }
187
188        /* Release bus/device initialization "Lock" */
189        mgr->initializing_objs = 0;
190
191out:
192        DRVMGR_UNLOCK();
193}
194
195/* Take bus into next level */
196static int do_bus_init(
197        struct drvmgr *mgr,
198        struct drvmgr_bus *bus,
199        int level)
200{
201        int (*init)(struct drvmgr_bus *);
202
203        /* If bridge device has failed during initialization, the bus is not
204         * initialized further.
205         */
206        if (bus->dev->state & DEV_STATE_INIT_FAILED) {
207                bus->state |= BUS_STATE_DEPEND_FAILED;
208                goto inactivate_out;
209        }
210
211        if (bus->ops && (init = bus->ops->init[level-1])) {
212                /* Note: This init1 function may register new devices */
213                bus->error = init(bus);
214                if (bus->error != DRVMGR_OK) {
215                        /* An error of some kind during bus initialization.
216                         *
217                         * Child devices and their buses are not inactived
218                         * directly here, instead they will all be catched by
219                         * do_dev_init() and do_bus_init() by checking if
220                         * parent or bridge-device failed. We know that
221                         * initialization will happen later for those devices.
222                         */
223                        goto inactivate_out;
224                }
225        }
226
227        DRVMGR_LOCK_WRITE();
228
229        /* Bus taken into the new level */
230        bus->level = level;
231
232        /* Put bus into list of buses reached level 'level'.
233         * Put at end of bus list so that init[N+1]() calls comes
234         * in the same order as init[N]()
235         */
236        drvmgr_list_add_tail(&mgr->buses[level], bus);
237
238        DRVMGR_UNLOCK();
239
240        return 0;
241
242inactivate_out:
243        DRVMGR_LOCK_WRITE();
244        bus->state |= BUS_STATE_INIT_FAILED;
245        bus->state |= BUS_STATE_LIST_INACTIVE;
246        drvmgr_list_add_head(&mgr->buses_inactive, bus);
247        DRVMGR_UNLOCK();
248
249        DBG("do_bus_init(%d): (DEV: %s) failed\n", level, bus->dev->name);
250
251        return 1;
252}
253
254/* Take device to initialization level 1 */
255static int do_dev_init(
256        struct drvmgr *mgr,
257        struct drvmgr_dev *dev,
258        int level)
259{
260        int (*init)(struct drvmgr_dev *);
261
262        /* Try to allocate Private Device Structure for driver if driver
263         * requests for this feature.
264         */
265        if (dev->drv && dev->drv->dev_priv_size && !dev->priv) {
266                dev->priv = malloc(dev->drv->dev_priv_size);
267                memset(dev->priv, 0, dev->drv->dev_priv_size);
268        }
269
270        /* If parent bus has failed during initialization,
271         * the device is not initialized further.
272         */
273        if (dev->parent && (dev->parent->state & BUS_STATE_INIT_FAILED)) {
274                dev->state |= DEV_STATE_DEPEND_FAILED;
275                goto inactivate_out;
276        }
277
278        /* Call Driver's Init Routine */
279        if (dev->drv && (init = dev->drv->ops->init[level-1])) {
280                /* Note: This init function may register new devices */
281                dev->error = init(dev);
282                if (dev->error != DRVMGR_OK) {
283                        /* An error of some kind has occured in the
284                         * driver/device, the failed device is put into the
285                         * inactive list, this way Init2,3 and/or 4 will not
286                         * be called for this device.
287                         *
288                         * The device is not removed from the bus (not
289                         * unregistered). The driver can be used to find
290                         * device information and debugging for example even
291                         * if device initialization failed.
292                         *
293                         * Child buses and their devices are not inactived
294                         * directly here, instead they will all be catched by
295                         * do_dev_init() and do_bus_init() by checking if
296                         * parent or bridge-device failed. We know that
297                         * initialization will happen later for those devices.
298                         */
299                        goto inactivate_out;
300                }
301        }
302
303        DRVMGR_LOCK_WRITE();
304        /* Dev taken into new level */
305        dev->level = level;
306
307        /* Put at end of device list so that init[N+1]() calls comes
308         * in the same order as init[N]()
309         */
310        drvmgr_list_add_tail(&mgr->devices[level], dev);
311        DRVMGR_UNLOCK();
312
313        return 0;
314
315inactivate_out:
316        DRVMGR_LOCK_WRITE();
317        dev->state |= DEV_STATE_INIT_FAILED;
318        dev->state |= DEV_STATE_LIST_INACTIVE;
319        drvmgr_list_add_head(&mgr->devices_inactive, dev);
320        DRVMGR_UNLOCK();
321
322        DBG("do_dev_init(%d): DRV: %s (DEV: %s) failed\n",
323                level, dev->drv->name, dev->name);
324
325        return 1; /* Failed to take device into requested level */
326}
327
328/* Register Root device driver */
329int drvmgr_root_drv_register(struct drvmgr_drv *drv)
330{
331        struct drvmgr *mgr = &drvmgr;
332        struct drvmgr_dev *root = &mgr->root_dev;
333
334        if (mgr->root_drv) {
335                /* Only possible to register root device once */
336                return DRVMGR_FAIL;
337        }
338
339        /* Set root device driver */
340        drv->next = NULL;
341        mgr->root_drv = drv;
342
343        /* Init root device non-NULL fields */
344        root->minor_drv = -1;
345        root->minor_bus = 0;
346        root->businfo = mgr;
347        root->name = "root bus";
348        /* Custom Driver association */
349        root->drv = mgr->root_drv;
350
351        /* This registers the root device and a bus */
352        drvmgr_dev_register(root);
353
354        return DRVMGR_OK;
355}
356
357/* Register a driver */
358int drvmgr_drv_register(struct drvmgr_drv *drv)
359{
360        struct drvmgr *mgr = &drvmgr;
361
362        /* All drivers must have been registered before start of init,
363         * because the manager does not scan all existing devices to find
364         * suitable hardware for this driver, and it is not protected with
365         * a lock therefore.
366         */
367        if (mgr->level > 0)
368                return -1;
369
370        drv->obj_type = DRVMGR_OBJ_DRV;
371
372        /* Put driver into list of registered drivers */
373        drvmgr_list_add_head(&mgr->drivers, drv);
374
375        /* TODO: we could scan for devices that this new driver has support
376         *       for. However, at this stage we assume that all drivers are
377         *       registered before devices are registered.
378         *
379         * LOCK: From the same assumsion locking the driver list is not needed
380         *       either.
381         */
382
383        return 0;
384}
385
386/* Insert a device into a driver's device list and assign a driver minor number
387 * to the device.
388 *
389 * The devices are ordered by their minor number (sorted linked list of devices)
390 * the minor number is found by looking for a gap or at the end.
391 */
392static void drvmgr_insert_dev_into_drv(
393        struct drvmgr_drv *drv,
394        struct drvmgr_dev *dev)
395{
396        struct drvmgr_dev *curr, **pprevnext;
397        int minor;
398
399        minor = 0;
400        pprevnext = &drv->dev;
401        curr = drv->dev;
402
403        while (curr) {
404                if (minor < curr->minor_drv) {
405                        /* Found a gap. Insert new device between prev
406                         * and curr. */
407                        break;
408                }
409                minor++;
410                pprevnext = &curr->next_in_drv;
411                curr = curr->next_in_drv;
412        }
413        dev->next_in_drv = curr;
414        *pprevnext = dev;
415
416        /* Set minor */
417        dev->minor_drv = minor;
418        drv->dev_cnt++;
419}
420
421/* Insert a device into a bus device list and assign a bus minor number to the
422 * device.
423 *
424 * The devices are ordered by their minor number (sorted linked list of devices)
425 * and by their registeration order if not using the same driver.
426 *
427 * The minor number is found by looking for a gap or at the end.
428 */
429static void drvmgr_insert_dev_into_bus(
430        struct drvmgr_bus *bus,
431        struct drvmgr_dev *dev)
432{
433        struct drvmgr_dev *curr, **pprevnext;
434        int minor;
435
436        minor = 0;
437        pprevnext = &bus->children;
438        curr = bus->children;
439
440        while (curr) {
441                if (dev->drv && (dev->drv == curr->drv)) {
442                        if (minor < curr->minor_bus) {
443                                /* Found a gap. Insert new device between prev
444                                 * and curr. */
445                                break;
446                        }
447                        minor++;
448                }
449                pprevnext = &curr->next_in_bus;
450                curr = curr->next_in_bus;
451        }
452        dev->next_in_bus = curr;
453        *pprevnext = dev;
454
455        /* Set minor. Devices without driver are given -1 */
456        if (dev->drv == NULL)
457                minor = -1;
458        dev->minor_bus = minor;
459        bus->dev_cnt++;
460}
461
462/* Try to find a driver for a device (unite a device with driver).
463 * a device with a driver
464 */
465static struct drvmgr_drv *drvmgr_dev_find_drv(
466                struct drvmgr_dev *dev)
467{
468        struct drvmgr *mgr = &drvmgr;
469        struct drvmgr_drv *drv;
470
471        /* NOTE: No locking is needed here since Driver list is supposed to be
472         *       initialized once during startup, we treat it as a static
473         *       read-only list
474         */
475
476        /* Try to find a driver that can handle this device */
477        for (drv = DRV_LIST_HEAD(&mgr->drivers); drv; drv = drv->next)
478                if (dev->parent->ops->unite(drv, dev) == 1)
479                        break;
480
481        return drv;
482}
483
484/* Register a device */
485int drvmgr_dev_register(struct drvmgr_dev *dev)
486{
487        struct drvmgr *mgr = &drvmgr;
488        struct drvmgr_drv *drv;
489        struct drvmgr_bus *bus = dev->parent;
490        struct drvmgr_key *keys;
491        struct drvmgr_list *init_list = &mgr->devices_inactive;
492
493        DBG("DEV_REG: %s at bus \"%s\"\n", dev->name,
494                bus && bus->dev && bus->dev->name ? bus->dev->name : "UNKNOWN");
495
496        /* Custom driver assocation? */
497        if (dev->drv) {
498                drv = dev->drv;
499                DBG("CUSTOM ASSOCIATION (%s to %s)\n", dev->name, drv->name);
500        } else {
501                /* Try to find a driver that can handle this device */
502                dev->drv = drv = drvmgr_dev_find_drv(dev);
503        }
504
505        DRVMGR_LOCK_WRITE();
506
507        /* Assign Bus Minor number and put into bus device list
508         * unless root device.
509         */
510        if (bus)
511                drvmgr_insert_dev_into_bus(bus, dev);
512
513        if (!drv) {
514                /* No driver found that can handle this device, put into
515                 * inactive list
516                 */
517                dev->minor_drv = -1;
518                dev->state |= DEV_STATE_LIST_INACTIVE;
519        } else {
520                /* United device with driver.
521                 * Put the device on the registered device list
522                 */
523                dev->state |= DEV_STATE_UNITED;
524
525                /* Check if user want to skip this core. This is not a
526                 * normal request, however in a multi-processor system
527                 * the two(or more) RTEMS instances must not use the same
528                 * devices in a system, not reporting a device to
529                 * it's driver will effectively accomplish this. In a
530                 * non Plug & Play system one can easily avoid this
531                 * problem by not report the core, but in a Plug & Play
532                 * system the bus driver will report all found cores.
533                 *
534                 * To stop the two RTEMS instances from using the same
535                 * device the user can simply define a resource entry
536                 * for a certain device but set the keys field to NULL.
537                 */
538                if (drvmgr_keys_get(dev, &keys) == 0 && keys == NULL) {
539                        /* Found Driver resource entry point
540                         * for this device, it was NULL, this
541                         * indicates to skip the core.
542                         *
543                         * We put it into the inactive list
544                         * marking it as ignored.
545                         */
546                        dev->state |= DEV_STATE_IGNORED;
547                } else {
548                        /* Assign Driver Minor number and put into driver's
549                         * device list
550                         */
551                        drvmgr_insert_dev_into_drv(drv, dev);
552
553                        /* Just register device, it will be initialized
554                         * later together with bus.
555                         *
556                         * At the end of the list (breadth first search)
557                         */
558                        init_list = &mgr->devices[0];
559
560                        DBG("Registered %s (DRV: %s) on %s\n",
561                                dev->name, drv->name,
562                                bus ? bus->dev->name : "NO PARENT");
563                }
564        }
565
566        drvmgr_list_add_tail(init_list, dev);
567
568        DRVMGR_UNLOCK();
569
570        /* Trigger Device initialization if not root device and
571         * has a driver
572         */
573        if (bus && dev->drv)
574                drvmgr_init_update();
575
576        return 0;
577}
578
579/* Register a bus */
580int drvmgr_bus_register(struct drvmgr_bus *bus)
581{
582        struct drvmgr *mgr = &drvmgr;
583        struct drvmgr_bus *bus_up;
584
585        /* Get bus architecture depth - the distance from root bus */
586        bus->depth = 0;
587        bus_up = bus->dev->parent;
588        while (bus_up) {
589                bus->depth++;
590                bus_up = bus_up->dev->parent;
591        }
592
593        DRVMGR_LOCK_WRITE();
594
595        /* Put driver into list of found buses */
596        drvmgr_list_add_tail(&mgr->buses[0], bus);
597
598        DRVMGR_UNLOCK();
599
600        /* Take bus into level1 and so on */
601        drvmgr_init_update();
602
603        return 0;
604}
605
606/* Allocate memory for a Device structure */
607int drvmgr_alloc_dev(struct drvmgr_dev **pdev, int extra)
608{
609        struct drvmgr_dev *dev;
610        int size;
611
612        /* The extra memory "service" is aligned to 4 bytes boundary. */
613        size = ((sizeof(struct drvmgr_dev) + 3) & ~0x3) + extra;
614        dev = (struct drvmgr_dev *)calloc(size, 1);
615        if (!dev) {
616                /* Failed to allocate device structure - critical error */
617                rtems_fatal_error_occurred(RTEMS_NO_MEMORY);
618        }
619        *pdev = dev;
620        dev->obj_type = DRVMGR_OBJ_DEV;
621
622        return 0;
623}
624
625/* Allocate memory for a Bus structure */
626int drvmgr_alloc_bus(struct drvmgr_bus **pbus, int extra)
627{
628        struct drvmgr_bus *bus;
629        int size;
630
631        /* The extra memory "service" is aligned to 4 bytes boundary. */
632        size = ((sizeof(struct drvmgr_bus) + 3) & ~0x3) + extra;
633        bus = (struct drvmgr_bus *)calloc(size, 1);
634        if (!bus) {
635                /* Failed to allocate device structure - critical error */
636                rtems_fatal_error_occurred(RTEMS_NO_MEMORY);
637        }
638        *pbus = bus;
639        bus->obj_type = DRVMGR_OBJ_BUS;
640
641        return 0;
642}
643
644/* Add driver resources to a bus instance */
645void drvmgr_bus_res_add(struct drvmgr_bus *bus,
646                                struct drvmgr_bus_res *bres)
647{
648        /* insert first in bus resource list. Locking isn't needed since
649         * resources can only be added before resource requests are made.
650         * When bus has been registered resources are considered a read-only
651         * tree.
652         */
653        bres->next = bus->reslist;
654        bus->reslist = bres;
655}
656
657#ifdef RTEMS_DRVMGR_STARTUP
658
659RTEMS_SYSINIT_ITEM(
660  _DRV_Manager_initialization,
661  RTEMS_SYSINIT_DRVMGR,
662  RTEMS_SYSINIT_ORDER_MIDDLE
663);
664
665/* BSPs has already registered their "root bus" driver in the
666 * bsp_predriver hook or so.
667 *
668 * Init Drivers to Level 1, constraints:
669 *   - Interrupts and system clock timer does not work.
670 *   - malloc() work, however other memory services may not
671 *     have been initialized yet.
672 *   - initializes most basic stuff
673 *
674 * Typical setup in Level 1:
675 *   - Find most devices in system, do PCI scan and configuration.
676 *   - Reset hardware if needed.
677 *   - Install IRQ driver
678 *   - Install Timer driver
679 *   - Install console driver and debug printk()
680 *   - Install extra memory.
681 */
682static void _DRV_Manager_init_level_1(void)
683{
684  _DRV_Manager_init_level(1);
685}
686
687RTEMS_SYSINIT_ITEM(
688  _DRV_Manager_init_level_1,
689  RTEMS_SYSINIT_DRVMGR_LEVEL_1,
690  RTEMS_SYSINIT_ORDER_MIDDLE
691);
692
693/* Init Drivers to Level 2, constraints:
694 *  - Interrupts can be registered and enabled.
695 *  - System Clock is running
696 *  - Console may be used.
697 *
698 * This is typically where drivers are initialized
699 * for the first time.
700 */
701static void _DRV_Manager_init_level_2(void)
702{
703  _DRV_Manager_init_level(2);
704}
705
706RTEMS_SYSINIT_ITEM(
707  _DRV_Manager_init_level_2,
708  RTEMS_SYSINIT_DRVMGR_LEVEL_2,
709  RTEMS_SYSINIT_ORDER_MIDDLE
710);
711
712/* Init Drivers to Level 3
713 *
714 * This is typically where normal drivers are initialized
715 * for the second time, they may depend on other drivers
716 * API inited in level 2
717 */
718static void _DRV_Manager_init_level_3(void)
719{
720  _DRV_Manager_init_level(3);
721}
722
723RTEMS_SYSINIT_ITEM(
724  _DRV_Manager_init_level_3,
725  RTEMS_SYSINIT_DRVMGR_LEVEL_3,
726  RTEMS_SYSINIT_ORDER_MIDDLE
727);
728
729/* Init Drivers to Level 4,
730 * Init drivers that depend on services initialized in Level 3
731 */
732static void _DRV_Manager_init_level_4(void)
733{
734  _DRV_Manager_init_level(4);
735}
736
737RTEMS_SYSINIT_ITEM(
738  _DRV_Manager_init_level_4,
739  RTEMS_SYSINIT_DRVMGR_LEVEL_4,
740  RTEMS_SYSINIT_ORDER_MIDDLE
741);
742
743#endif /* RTEMS_DRVMGR_STARTUP */
Note: See TracBrowser for help on using the repository browser.