source: libbsdport/bsd_eth_drivers/libbsdport/devicet.c

Last change on this file was 9d32a19, checked in by Till Straumann <strauman@…>, on Jul 23, 2010 at 8:55:04 AM

2010-07-23 Till Straumann <Till.Straumann@…>

  • libbsdport/devicet.c: Initialize libbspExt.
  • Property mode set to 100644
File size: 8.7 KB
Line 
1#define DEVICET_EXTERN_INLINE
2
3#ifdef HAVE_CONFIG_H
4#include <config.h>
5#endif
6
7#include "devicet.h"
8#include <rtems/rtems_bsdnet.h>
9#include <sys/malloc.h>
10#include <assert.h>
11#include <ctype.h>
12#include <string.h>
13#include <stdio.h>
14#include <rtems/pci.h>
15#include <rtems/error.h>
16#include <sys/bus.h>
17#include "libbsdport_api.h"
18
19#ifdef HAVE_LIBBSPEXT
20#include <bsp/bspExt.h>
21#endif
22
23#define  DEBUG 0
24int libbsdportAttachVerbose = DEBUG;
25
26extern void real_libc_free(void*);
27
28static STAILQ_HEAD(devq_t, device) devq = STAILQ_HEAD_INITIALIZER(devq);
29
30static device_t
31devalloc(driver_t *dr)
32{
33void     *m;
34device_t rval;
35int      l = sizeof(*rval) + dr->softc_size + DEVICE_SOFTC_ALIGNMENT -1;
36
37        if ( !(m = malloc( l, M_DEVBUF, M_NOWAIT )) )
38                return 0;
39
40        memset(m, 0, l);
41
42        rval = (device_t)(((uintptr_t)m + (DEVICE_SOFTC_ALIGNMENT-1)) & ~(DEVICE_SOFTC_ALIGNMENT-1));
43        rval->rawmem = m;
44        rval->type   = dr->type;
45        rval->name   = dr->name;
46        rval->drv    = dr; 
47
48        return rval;
49}
50
51static void
52devclean(device_t dev)
53{
54        assert( !dev->attached );
55        memset(device_get_softc(dev), 0, dev->drv->softc_size);
56        real_libc_free(dev->desc);
57        dev->desc = 0;
58        dev->unit = 0;
59        dev->nameunit[0]=0;
60        memset( &dev->bushdr, 0, sizeof(dev->bushdr));
61}
62
63static void
64devfree(device_t dev)
65{
66        /* paranoia */
67        devclean(dev);
68        dev->drv = 0;
69        free(dev->rawmem, M_DEVBUF);
70}
71
72static int
73devattach(device_t dev, int unit, struct rtems_bsdnet_ifconfig *cfg)
74{
75int error;
76
77        if ( libbsdportAttachVerbose ) {
78                printf("Now attaching %s%d: (0x%x:%x.%x)\n",
79                        dev->name, unit, 
80                        dev->bushdr.pci.bus, dev->bushdr.pci.dev, dev->bushdr.pci.fun);
81        }
82
83        dev->unit     = unit;
84        dev->ifconfig = cfg;
85        sprintf(dev->nameunit,"%s%d",dev->drv->name,unit);
86
87        /* Try to attach */
88        if ( (error = dev->drv->methods->attach(dev)) ) {
89                fprintf(stderr,"Attaching '%s%d' failed: %s", dev->drv->name, unit, strerror(error));
90                return error;
91        }
92        /* Successfully attached new device */
93        dev->attached = 1;
94        cfg->name     = (char*)device_get_nameunit(dev);
95        STAILQ_INSERT_TAIL(&devq, dev, list);
96        return 0;
97}
98
99static int
100devequal(device_t a, device_t b)
101{
102        if ( a->type != b->type )
103                return 0;
104        switch ( a->type ) {
105                case DEV_TYPE_PCI:
106                        return  a->bushdr.pci.bus == b->bushdr.pci.bus
107                             && a->bushdr.pci.dev == b->bushdr.pci.dev
108                             && a->bushdr.pci.fun == b->bushdr.pci.fun;
109
110                default:
111                        rtems_panic("devequal: Unsupported device type %i\n", a->type);
112        }
113        return 0;
114}
115
116/* Check if a particular device is already listed */
117static device_t
118devattached(device_t dev)
119{
120struct device *ldev;
121        STAILQ_FOREACH(ldev, &devq, list) {
122                if ( devequal(ldev, dev) )
123                        return ldev;
124        }
125        return 0;
126}
127
128
129int
130device_printf(device_t dev, const char *fmt, ...)
131{
132int rval;
133va_list ap;
134        rval  = fprintf(stdout,"%s:",device_get_nameunit(dev));
135        va_start(ap, fmt);
136        rval += vfprintf(stdout,fmt,ap);
137        va_end(ap);
138        return rval;
139}
140
141static uint32_t
142get_pci_triple(const char *drvnam)
143{
144unsigned b,d,f;
145        if ( drvnam && 3 == sscanf(drvnam,"%i:%i.%i",&b,&d,&f) )
146                return (b<<8) | PCI_DEVFN(d,f);
147        return -1;
148}
149
150static void
151get_name_unit(const char *drvnam, char *nm, int *punit)
152{
153int l = strlen(drvnam);
154int i;
155        if ( l > 0 ) {
156                for ( i=l-1; i>=0 && isdigit(((unsigned char)drvnam[i])); i-- )
157                        /* nothing else to do */;
158                if ( 1 != sscanf(drvnam+i,"%d",punit) )
159                        *punit = 0; /* wildcard */
160                strncpy(nm, drvnam, i+1);
161                nm[i+1]=0;
162        } else {
163                /* wildcards */
164                *nm    = 0;
165                *punit = 0;
166        }
167}
168
169static int
170matches(driver_t *dr, const char *pat)
171{
172        if ( 0 == *pat || '*' == *pat )
173                return 1;
174        return !strcmp(pat, dr->name);
175}
176
177static int
178pci_slot_empty(int b, int d, int f)
179{
180uint16_t id;
181        pci_read_config_word(b,d,f,PCI_VENDOR_ID,&id);
182        return ( 0xffff == id );
183}
184
185static int
186pci_is_ether(int b, int d, int f)
187{
188uint16_t dclass;
189        if ( pci_slot_empty(b,d,f) )
190                return 0;
191        pci_read_config_word(b,d,f,PCI_CLASS_DEVICE, &dclass);
192        return PCI_CLASS_NETWORK_ETHERNET == dclass;
193}
194
195/* this catches the case of an unpopulated slot (returning 0) */
196static int
197pci_num_functions(int b, int d)
198{
199uint8_t h;
200        if ( pci_slot_empty(b,d,0) )
201                return 0;
202        pci_read_config_byte(b,d,0,PCI_HEADER_TYPE,&h);
203        return (h & 0x80) ? PCI_MAX_FUNCTIONS : 1; /* multifunction device ? */
204}
205
206int
207libbsdport_netdriver_dump(FILE *f)
208{
209struct device *ldev;
210int           ndevs;
211unsigned      w;
212
213        if ( !f )
214                f = stdout;
215
216        ndevs = 0;
217        fprintf(f, "PCI Network device information:\n");
218        rtems_bsdnet_semaphore_obtain();
219        STAILQ_FOREACH(ldev, &devq, list) {
220                /* ASSUME LIST ELEMENTS DO NOT DISAPPEAR
221                 * so we can release the lock while printing...
222                 */
223                rtems_bsdnet_semaphore_release();
224                w=fprintf(f,"%-6s -- (0x%x:%x.%x)",
225                                device_get_nameunit(ldev),
226                                ldev->bushdr.pci.bus,
227                                ldev->bushdr.pci.dev,
228                                ldev->bushdr.pci.fun);
229                for ( ; w < 24 ; w++)
230                        fputc(' ',f);
231                if ( ldev->desc )
232                        fprintf(f," %s",ldev->desc);
233                fputc('\n',f);
234
235
236                ndevs++;
237                rtems_bsdnet_semaphore_obtain();
238        }
239        rtems_bsdnet_semaphore_release();
240        return ndevs;
241}
242
243#define UNITMATCH(wanted, unit, bdfunit) \
244        ((wanted) < 0 ? ((wanted) & 0xffff) == (bdfunit) : (wanted) == (unit))
245
246int
247libbsdport_netdriver_attach(struct rtems_bsdnet_ifconfig *cfg, int attaching)
248{
249char     nm[20]; /* copy of the name */
250int      unit,thisunit,wantedunit;
251int      i,b,d,f;
252int      prob;
253driver_t *dr;
254device_t dev   = 0;
255device_t tmpdev;
256int      error = 0;
257int      bdfunit;
258
259int      n_bus;
260
261                if ( !attaching )
262                        return ENOTSUP;
263
264#ifdef HAVE_LIBBSPEXT
265                bspExtInit();
266#endif
267
268                if ( (wantedunit = get_pci_triple(cfg->name)) < 0 ) {
269                        get_name_unit(cfg->name, nm, &wantedunit);
270                } else {
271                        wantedunit |= 1<<31;
272                        nm[0]=0;
273                }
274                if ( libbsdportAttachVerbose )
275                        printf("Wanted unit is 0x%x, pattern '%s'\n", wantedunit, nm);
276
277                n_bus = pci_bus_count();
278#ifdef __i386__
279                /* ugliest of all hacks -- RTEMS routine is currently (4.9)
280         * still broken; it reports the (0-based) highest bus number
281         * instead of the count.
282         */
283                n_bus++;
284#endif
285
286                unit = 0;
287                for ( i=0; (dr=libbsdport_netdriver_table[i]); i++ ) {
288
289                        /* unused slot ? */
290                        if ( 0 == dr->name && 0 == dr->methods )
291                                continue;
292
293                        /* Find matching driver */
294                        if ( libbsdportAttachVerbose )
295                                printf("Trying driver '%s' ...", dr->name);
296
297                        if ( matches(dr, nm) ) {
298
299                                if ( libbsdportAttachVerbose )
300                                        printf("MATCH\n");
301
302                                assert( dr->methods );
303
304                                thisunit = 0;
305
306                                if ( DEV_TYPE_PCI != dr->type ) {
307                                        fprintf(stderr,"Non-PCI driver '%s' not supported; skipping\n", dr->name);
308                                        continue;
309                                }
310
311                                dev = devalloc(dr);
312                                for ( b=0; b<n_bus; b++ )
313                                        for ( d=0; d<PCI_MAX_DEVICES; d++ ) {
314                                                for ( f=0; f<pci_num_functions(b,d); f++ ) {
315                                                        if ( ! pci_is_ether(b,d,f) )
316                                                                continue;
317
318                                                        dev->bushdr.pci.bus = b;
319                                                        dev->bushdr.pci.dev = d;
320                                                        dev->bushdr.pci.fun = f;
321
322                                                        bdfunit = (b<<8) | PCI_DEVFN(d,f);
323
324                                                        if ( libbsdportAttachVerbose ) {
325                                                                printf("Probing PCI 0x%x:%x.%x\n",
326                                                                                bdfunit>>8, PCI_SLOT(bdfunit), PCI_FUNC(bdfunit));
327                                                        }
328
329                                                        /* has this device been attached already ? */
330                                                        if ( (tmpdev = devattached(dev)) ) {
331                                                                if ( dev->drv == tmpdev->drv )
332                                                                        thisunit++;
333                                                                unit++;
334                                                                if ( UNITMATCH(wantedunit, unit, bdfunit) ) {
335                                                                        fprintf(stderr,"Device '%s' has already been attached\n", device_get_nameunit(dev));
336                                                                        error = EBUSY;
337                                                                        goto bail;
338                                                                }
339                                                        } else {
340                                                                switch ( ( prob = dr->methods->probe(dev) ) ) {
341                                                                                /* LOW_PRIORITY currently unsupported; list preferred drivers first */
342                                                                        case BUS_PROBE_LOW_PRIORITY:
343                                                                        case BUS_PROBE_DEFAULT:
344                                                                                /* accepted */
345                                                                                thisunit++;
346                                                                                unit++;
347                                                                                /* wanted unit == 0 means next avail.
348                                                                                 * unit is acceptable.
349                                                                                 */
350                                                                                if ( libbsdportAttachVerbose )
351                                                                                        printf("->SUCCESS\n");
352
353                                                                                if ( 0 == wantedunit || UNITMATCH(wantedunit, unit, bdfunit) ) {
354                                                                                                error = devattach(dev, thisunit, cfg);
355                                                                                                if ( !error )
356                                                                                                        dev = 0; /* is now on list */
357                                                                                                goto bail;
358                                                                                }
359                                                                                break;
360
361                                                                        default:
362                                                                                if ( libbsdportAttachVerbose )
363                                                                                        printf("->FAILED\n");
364                                                                                /* probe failed */
365                                                                                break;
366                                                                }
367                                                        }
368                                                        devclean(dev);
369                                                } /* for all functions */
370                                        } /* for all busses + slots */
371                                devfree(dev); dev = 0;
372                        } /* matching driver */
373                        else
374                        {
375                                if ( libbsdportAttachVerbose )
376                                        printf("NO MATCH\n");
377                        }
378                } /* for all drivers */
379
380                /* Nothing found */
381                error = ENODEV;
382bail:
383        if (dev)
384                devfree(dev);
385        return error;
386}
387
388device_t
389libbsdport_netdriver_get_dev(const char *name)
390{
391struct device *ldev;
392
393        if ( !name )
394                return 0;
395
396        rtems_bsdnet_semaphore_obtain();
397                STAILQ_FOREACH(ldev, &devq, list) {
398                        if ( !strcmp(name, device_get_nameunit(ldev)) )
399                                break;
400                }
401        rtems_bsdnet_semaphore_release();
402        return ldev;
403}
Note: See TracBrowser for help on using the repository browser.