File indexing completed on 2025-05-11 08:24:16
0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026
0027
0028
0029 #include <stdio.h>
0030 #include <stdlib.h>
0031 #include <string.h>
0032
0033 #include <drvmgr/drvmgr.h>
0034 #include <drvmgr/drvmgr_confdefs.h>
0035
0036 #include <rtems/sysinit.h>
0037
0038 #include "drvmgr_internal.h"
0039
0040
0041
0042
0043 #ifdef DEBUG
0044 #define DBG(x...) printk(x)
0045 #else
0046 #define DBG(x...)
0047 #endif
0048
0049 struct drvmgr drvmgr = {
0050 .level = 0,
0051 .initializing_objs = 0,
0052 .lock = API_MUTEX_INITIALIZER("_Drvmgr"),
0053 .root_dev = {0},
0054 .root_drv = NULL,
0055
0056 .drivers = LIST_INITIALIZER(struct drvmgr_drv, next),
0057
0058 .buses = {
0059 LIST_INITIALIZER(struct drvmgr_bus, next),
0060 LIST_INITIALIZER(struct drvmgr_bus, next),
0061 LIST_INITIALIZER(struct drvmgr_bus, next),
0062 LIST_INITIALIZER(struct drvmgr_bus, next),
0063 LIST_INITIALIZER(struct drvmgr_bus, next),
0064 },
0065 .buses_inactive = LIST_INITIALIZER(struct drvmgr_bus, next),
0066
0067 .devices = {
0068 LIST_INITIALIZER(struct drvmgr_dev, next),
0069 LIST_INITIALIZER(struct drvmgr_dev, next),
0070 LIST_INITIALIZER(struct drvmgr_dev, next),
0071 LIST_INITIALIZER(struct drvmgr_dev, next),
0072 LIST_INITIALIZER(struct drvmgr_dev, next),
0073 },
0074 .devices_inactive = LIST_INITIALIZER(struct drvmgr_dev, next),
0075 };
0076
0077 static int do_bus_init(
0078 struct drvmgr *mgr,
0079 struct drvmgr_bus *bus,
0080 int level);
0081 static int do_dev_init(
0082 struct drvmgr *mgr,
0083 struct drvmgr_dev *dev,
0084 int level);
0085
0086
0087
0088 void _DRV_Manager_init_level(int level)
0089 {
0090 struct drvmgr *mgr = &drvmgr;
0091
0092 if (mgr->level >= level)
0093 return;
0094
0095
0096 mgr->level = level;
0097
0098
0099 drvmgr_init_update();
0100 }
0101
0102
0103
0104
0105 void _DRV_Manager_initialization(void)
0106 {
0107 drvmgr_drv_reg_func *drvreg;
0108
0109
0110 drvreg = &drvmgr_drivers[0];
0111 while (*drvreg) {
0112
0113 (*drvreg)();
0114 drvreg++;
0115 }
0116 }
0117
0118
0119
0120
0121
0122 void drvmgr_init_update(void)
0123 {
0124 struct drvmgr *mgr = &drvmgr;
0125 struct drvmgr_bus *bus;
0126 struct drvmgr_dev *dev;
0127 int bus_might_been_registered;
0128 int level;
0129
0130
0131
0132
0133 DRVMGR_LOCK_WRITE();
0134 if (mgr->initializing_objs || (mgr->level == 0))
0135 goto out;
0136 mgr->initializing_objs = 1;
0137
0138
0139
0140
0141 for (level = 0; level < mgr->level; level++) {
0142
0143 bus_might_been_registered = 0;
0144
0145
0146
0147 while ((bus = BUS_LIST_HEAD(&mgr->buses[level])) != NULL) {
0148
0149
0150
0151
0152 drvmgr_list_remove_head(&mgr->buses[level]);
0153
0154 DRVMGR_UNLOCK();
0155
0156
0157
0158
0159 do_bus_init(mgr, bus, level+1);
0160
0161 DRVMGR_LOCK_WRITE();
0162 }
0163
0164
0165 while ((dev = DEV_LIST_HEAD(&mgr->devices[level])) != NULL) {
0166
0167
0168 dev = DEV_LIST_HEAD(&mgr->devices[level]);
0169
0170
0171
0172
0173 drvmgr_list_remove_head(&mgr->devices[level]);
0174
0175 DRVMGR_UNLOCK();
0176
0177
0178 do_dev_init(mgr, dev, level+1);
0179
0180 DRVMGR_LOCK_WRITE();
0181
0182 bus_might_been_registered = 1;
0183 }
0184
0185
0186
0187
0188 if (bus_might_been_registered) {
0189 level = -1;
0190 }
0191 }
0192
0193
0194 mgr->initializing_objs = 0;
0195
0196 out:
0197 DRVMGR_UNLOCK();
0198 }
0199
0200
0201 static int do_bus_init(
0202 struct drvmgr *mgr,
0203 struct drvmgr_bus *bus,
0204 int level)
0205 {
0206 int (*init)(struct drvmgr_bus *);
0207
0208
0209
0210
0211 if (bus->dev->state & DEV_STATE_INIT_FAILED) {
0212 bus->state |= BUS_STATE_DEPEND_FAILED;
0213 goto inactivate_out;
0214 }
0215
0216 if (bus->ops && (init = bus->ops->init[level-1])) {
0217
0218 bus->error = init(bus);
0219 if (bus->error != DRVMGR_OK) {
0220
0221
0222
0223
0224
0225
0226
0227
0228 goto inactivate_out;
0229 }
0230 }
0231
0232 DRVMGR_LOCK_WRITE();
0233
0234
0235 bus->level = level;
0236
0237
0238
0239
0240
0241 drvmgr_list_add_tail(&mgr->buses[level], bus);
0242
0243 DRVMGR_UNLOCK();
0244
0245 return 0;
0246
0247 inactivate_out:
0248 DRVMGR_LOCK_WRITE();
0249 bus->state |= BUS_STATE_INIT_FAILED;
0250 bus->state |= BUS_STATE_LIST_INACTIVE;
0251 drvmgr_list_add_head(&mgr->buses_inactive, bus);
0252 DRVMGR_UNLOCK();
0253
0254 DBG("do_bus_init(%d): (DEV: %s) failed\n", level, bus->dev->name);
0255
0256 return 1;
0257 }
0258
0259
0260 static int do_dev_init(
0261 struct drvmgr *mgr,
0262 struct drvmgr_dev *dev,
0263 int level)
0264 {
0265 int (*init)(struct drvmgr_dev *);
0266
0267
0268
0269
0270 if (dev->drv && dev->drv->dev_priv_size && !dev->priv) {
0271 dev->priv = malloc(dev->drv->dev_priv_size);
0272 memset(dev->priv, 0, dev->drv->dev_priv_size);
0273 }
0274
0275
0276
0277
0278 if (dev->parent && (dev->parent->state & BUS_STATE_INIT_FAILED)) {
0279 dev->state |= DEV_STATE_DEPEND_FAILED;
0280 goto inactivate_out;
0281 }
0282
0283
0284 if (dev->drv && (init = dev->drv->ops->init[level-1])) {
0285
0286 dev->error = init(dev);
0287 if (dev->error != DRVMGR_OK) {
0288
0289
0290
0291
0292
0293
0294
0295
0296
0297
0298
0299
0300
0301
0302
0303
0304 goto inactivate_out;
0305 }
0306 }
0307
0308 DRVMGR_LOCK_WRITE();
0309
0310 dev->level = level;
0311
0312
0313
0314
0315 drvmgr_list_add_tail(&mgr->devices[level], dev);
0316 DRVMGR_UNLOCK();
0317
0318 return 0;
0319
0320 inactivate_out:
0321 DRVMGR_LOCK_WRITE();
0322 dev->state |= DEV_STATE_INIT_FAILED;
0323 dev->state |= DEV_STATE_LIST_INACTIVE;
0324 drvmgr_list_add_head(&mgr->devices_inactive, dev);
0325 DRVMGR_UNLOCK();
0326
0327 DBG("do_dev_init(%d): DRV: %s (DEV: %s) failed\n",
0328 level, dev->drv->name, dev->name);
0329
0330 return 1;
0331 }
0332
0333
0334 int drvmgr_root_drv_register(struct drvmgr_drv *drv)
0335 {
0336 struct drvmgr *mgr = &drvmgr;
0337 struct drvmgr_dev *root = &mgr->root_dev;
0338
0339 if (mgr->root_drv) {
0340
0341 return DRVMGR_FAIL;
0342 }
0343
0344
0345 drv->next = NULL;
0346 mgr->root_drv = drv;
0347
0348
0349 root->minor_drv = -1;
0350 root->minor_bus = 0;
0351 root->businfo = mgr;
0352 root->name = "root bus";
0353
0354 root->drv = mgr->root_drv;
0355
0356
0357 drvmgr_dev_register(root);
0358
0359 return DRVMGR_OK;
0360 }
0361
0362
0363 int drvmgr_drv_register(struct drvmgr_drv *drv)
0364 {
0365 struct drvmgr *mgr = &drvmgr;
0366
0367
0368
0369
0370
0371
0372 if (mgr->level > 0)
0373 return -1;
0374
0375 drv->obj_type = DRVMGR_OBJ_DRV;
0376
0377
0378 drvmgr_list_add_head(&mgr->drivers, drv);
0379
0380
0381
0382
0383
0384
0385
0386
0387
0388 return 0;
0389 }
0390
0391
0392
0393
0394
0395
0396
0397 static void drvmgr_insert_dev_into_drv(
0398 struct drvmgr_drv *drv,
0399 struct drvmgr_dev *dev)
0400 {
0401 struct drvmgr_dev *curr, **pprevnext;
0402 int minor;
0403
0404 minor = 0;
0405 pprevnext = &drv->dev;
0406 curr = drv->dev;
0407
0408 while (curr) {
0409 if (minor < curr->minor_drv) {
0410
0411
0412 break;
0413 }
0414 minor++;
0415 pprevnext = &curr->next_in_drv;
0416 curr = curr->next_in_drv;
0417 }
0418 dev->next_in_drv = curr;
0419 *pprevnext = dev;
0420
0421
0422 dev->minor_drv = minor;
0423 drv->dev_cnt++;
0424 }
0425
0426
0427
0428
0429
0430
0431
0432
0433
0434 static void drvmgr_insert_dev_into_bus(
0435 struct drvmgr_bus *bus,
0436 struct drvmgr_dev *dev)
0437 {
0438 struct drvmgr_dev *curr, **pprevnext;
0439 int minor;
0440
0441 minor = 0;
0442 pprevnext = &bus->children;
0443 curr = bus->children;
0444
0445 while (curr) {
0446 if (dev->drv && (dev->drv == curr->drv)) {
0447 if (minor < curr->minor_bus) {
0448
0449
0450 break;
0451 }
0452 minor++;
0453 }
0454 pprevnext = &curr->next_in_bus;
0455 curr = curr->next_in_bus;
0456 }
0457 dev->next_in_bus = curr;
0458 *pprevnext = dev;
0459
0460
0461 if (dev->drv == NULL)
0462 minor = -1;
0463 dev->minor_bus = minor;
0464 bus->dev_cnt++;
0465 }
0466
0467
0468
0469
0470 static struct drvmgr_drv *drvmgr_dev_find_drv(
0471 struct drvmgr_dev *dev)
0472 {
0473 struct drvmgr *mgr = &drvmgr;
0474 struct drvmgr_drv *drv;
0475
0476
0477
0478
0479
0480
0481
0482 for (drv = DRV_LIST_HEAD(&mgr->drivers); drv; drv = drv->next)
0483 if (dev->parent->ops->unite(drv, dev) == 1)
0484 break;
0485
0486 return drv;
0487 }
0488
0489
0490 int drvmgr_dev_register(struct drvmgr_dev *dev)
0491 {
0492 struct drvmgr *mgr = &drvmgr;
0493 struct drvmgr_drv *drv;
0494 struct drvmgr_bus *bus = dev->parent;
0495 struct drvmgr_key *keys;
0496 struct drvmgr_list *init_list = &mgr->devices_inactive;
0497
0498 DBG("DEV_REG: %s at bus \"%s\"\n", dev->name,
0499 bus && bus->dev && bus->dev->name ? bus->dev->name : "UNKNOWN");
0500
0501
0502 if (dev->drv) {
0503 drv = dev->drv;
0504 DBG("CUSTOM ASSOCIATION (%s to %s)\n", dev->name, drv->name);
0505 } else {
0506
0507 dev->drv = drv = drvmgr_dev_find_drv(dev);
0508 }
0509
0510 DRVMGR_LOCK_WRITE();
0511
0512
0513
0514
0515 if (bus)
0516 drvmgr_insert_dev_into_bus(bus, dev);
0517
0518 if (!drv) {
0519
0520
0521
0522 dev->minor_drv = -1;
0523 dev->state |= DEV_STATE_LIST_INACTIVE;
0524 } else {
0525
0526
0527
0528 dev->state |= DEV_STATE_UNITED;
0529
0530
0531
0532
0533
0534
0535
0536
0537
0538
0539
0540
0541
0542
0543 if (drvmgr_keys_get(dev, &keys) == 0 && keys == NULL) {
0544
0545
0546
0547
0548
0549
0550
0551 dev->state |= DEV_STATE_IGNORED;
0552 } else {
0553
0554
0555
0556 drvmgr_insert_dev_into_drv(drv, dev);
0557
0558
0559
0560
0561
0562
0563 init_list = &mgr->devices[0];
0564
0565 DBG("Registered %s (DRV: %s) on %s\n",
0566 dev->name, drv->name,
0567 bus ? bus->dev->name : "NO PARENT");
0568 }
0569 }
0570
0571 drvmgr_list_add_tail(init_list, dev);
0572
0573 DRVMGR_UNLOCK();
0574
0575
0576
0577
0578 if (bus && dev->drv)
0579 drvmgr_init_update();
0580
0581 return 0;
0582 }
0583
0584
0585 int drvmgr_bus_register(struct drvmgr_bus *bus)
0586 {
0587 struct drvmgr *mgr = &drvmgr;
0588 struct drvmgr_bus *bus_up;
0589
0590
0591 bus->depth = 0;
0592 bus_up = bus->dev->parent;
0593 while (bus_up) {
0594 bus->depth++;
0595 bus_up = bus_up->dev->parent;
0596 }
0597
0598 DRVMGR_LOCK_WRITE();
0599
0600
0601 drvmgr_list_add_tail(&mgr->buses[0], bus);
0602
0603 DRVMGR_UNLOCK();
0604
0605
0606 drvmgr_init_update();
0607
0608 return 0;
0609 }
0610
0611
0612 int drvmgr_alloc_dev(struct drvmgr_dev **pdev, int extra)
0613 {
0614 struct drvmgr_dev *dev;
0615 int size;
0616
0617
0618 size = ((sizeof(struct drvmgr_dev) + 3) & ~0x3) + extra;
0619 dev = (struct drvmgr_dev *)calloc(size, 1);
0620 if (!dev) {
0621
0622 rtems_fatal_error_occurred(RTEMS_NO_MEMORY);
0623 }
0624 *pdev = dev;
0625 dev->obj_type = DRVMGR_OBJ_DEV;
0626
0627 return 0;
0628 }
0629
0630
0631 int drvmgr_alloc_bus(struct drvmgr_bus **pbus, int extra)
0632 {
0633 struct drvmgr_bus *bus;
0634 int size;
0635
0636
0637 size = ((sizeof(struct drvmgr_bus) + 3) & ~0x3) + extra;
0638 bus = (struct drvmgr_bus *)calloc(size, 1);
0639 if (!bus) {
0640
0641 rtems_fatal_error_occurred(RTEMS_NO_MEMORY);
0642 }
0643 *pbus = bus;
0644 bus->obj_type = DRVMGR_OBJ_BUS;
0645
0646 return 0;
0647 }
0648
0649
0650 void drvmgr_bus_res_add(struct drvmgr_bus *bus,
0651 struct drvmgr_bus_res *bres)
0652 {
0653
0654
0655
0656
0657
0658 bres->next = bus->reslist;
0659 bus->reslist = bres;
0660 }
0661
0662 #ifdef RTEMS_DRVMGR_STARTUP
0663
0664 RTEMS_SYSINIT_ITEM(
0665 _DRV_Manager_initialization,
0666 RTEMS_SYSINIT_DRVMGR,
0667 RTEMS_SYSINIT_ORDER_MIDDLE
0668 );
0669
0670
0671
0672
0673
0674
0675
0676
0677
0678
0679
0680
0681
0682
0683
0684
0685
0686
0687 static void _DRV_Manager_init_level_1(void)
0688 {
0689 _DRV_Manager_init_level(1);
0690 }
0691
0692 RTEMS_SYSINIT_ITEM(
0693 _DRV_Manager_init_level_1,
0694 RTEMS_SYSINIT_DRVMGR_LEVEL_1,
0695 RTEMS_SYSINIT_ORDER_MIDDLE
0696 );
0697
0698
0699
0700
0701
0702
0703
0704
0705
0706 static void _DRV_Manager_init_level_2(void)
0707 {
0708 _DRV_Manager_init_level(2);
0709 }
0710
0711 RTEMS_SYSINIT_ITEM(
0712 _DRV_Manager_init_level_2,
0713 RTEMS_SYSINIT_DRVMGR_LEVEL_2,
0714 RTEMS_SYSINIT_ORDER_MIDDLE
0715 );
0716
0717
0718
0719
0720
0721
0722
0723 static void _DRV_Manager_init_level_3(void)
0724 {
0725 _DRV_Manager_init_level(3);
0726 }
0727
0728 RTEMS_SYSINIT_ITEM(
0729 _DRV_Manager_init_level_3,
0730 RTEMS_SYSINIT_DRVMGR_LEVEL_3,
0731 RTEMS_SYSINIT_ORDER_MIDDLE
0732 );
0733
0734
0735
0736
0737 static void _DRV_Manager_init_level_4(void)
0738 {
0739 _DRV_Manager_init_level(4);
0740 }
0741
0742 RTEMS_SYSINIT_ITEM(
0743 _DRV_Manager_init_level_4,
0744 RTEMS_SYSINIT_DRVMGR_LEVEL_4,
0745 RTEMS_SYSINIT_ORDER_MIDDLE
0746 );
0747
0748 #endif