source: libbsdport/bsd_eth_drivers/libbsdport/devicet.c @ a8bf95d

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