summaryrefslogtreecommitdiffstats
path: root/bsd_eth_drivers/libbsdport/devicet.c
blob: 48ddd9a8ed6fe87043fa1e01503f45acce3f679a (plain) (blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
#define DEVICET_EXTERN_INLINE

#include "devicet.h"
#include <rtems/rtems_bsdnet.h>
#include <sys/malloc.h>
#include <assert.h>
#include <ctype.h>
#include <string.h>
#include <stdio.h>
#include <rtems/pci.h>
#include <rtems/error.h>
#include <sys/bus.h>
#include "libbsdport_api.h"

#define  DEBUG

extern void real_libc_free(void*);

static STAILQ_HEAD(devq_t, device) devq = STAILQ_HEAD_INITIALIZER(devq);

static device_t
devalloc(driver_t *dr)
{
void     *m;
device_t rval;
int      l = sizeof(*rval) + dr->softc_size + DEVICE_SOFTC_ALIGNMENT -1;

	if ( !(m = malloc( l, M_DEVBUF, M_NOWAIT )) )
		return 0;

	memset(m, 0, l);

	rval = (device_t)(((uintptr_t)m + (DEVICE_SOFTC_ALIGNMENT-1)) & ~(DEVICE_SOFTC_ALIGNMENT-1));
	rval->rawmem = m;
	rval->type   = dr->type;
	rval->name   = dr->name;
	rval->drv    = dr; 

	return rval;
}

static void
devclean(device_t dev)
{
	assert( !dev->attached );
	memset(device_get_softc(dev), 0, dev->drv->softc_size);
	real_libc_free(dev->desc);
	dev->desc = 0;
	dev->unit = 0;
	dev->nameunit[0]=0;
	memset( &dev->bushdr, 0, sizeof(dev->bushdr));
}

static void
devfree(device_t dev)
{
	/* paranoia */
	devclean(dev);
	dev->drv = 0;
	free(dev->rawmem, M_DEVBUF);
}

static int
devattach(device_t dev, int unit, struct rtems_bsdnet_ifconfig *cfg)
{
int error;

#ifdef DEBUG
	printf("Now attaching %s%d: (0x%x:%x.%x)\n",
		dev->name, unit, 
		dev->bushdr.pci.bus, dev->bushdr.pci.dev, dev->bushdr.pci.fun);
#endif

	dev->unit     = unit;
	dev->ifconfig = cfg;
	sprintf(dev->nameunit,"%s%d",dev->drv->name,unit);

	/* Try to attach */
	if ( (error = dev->drv->methods->attach(dev)) ) {
		fprintf(stderr,"Attaching '%s%d' failed: %s", dev->drv->name, unit, strerror(error));
		return error;
	}
	/* Successfully attached new device */
	dev->attached = 1;
	cfg->name     = (char*)device_get_nameunit(dev);
	STAILQ_INSERT_TAIL(&devq, dev, list);
	return 0;
}

static int
devequal(device_t a, device_t b)
{
	if ( a->type != b->type )
		return 0;
	switch ( a->type ) {
		case DEV_TYPE_PCI:
			return  a->bushdr.pci.bus == b->bushdr.pci.bus
			     && a->bushdr.pci.dev == b->bushdr.pci.dev
			     && a->bushdr.pci.fun == b->bushdr.pci.fun;

		default:
			rtems_panic("devequal: Unsupported device type %i\n", a->type);
	}
	return 0;
}

/* Check if a particular device is already listed */
static device_t
devattached(device_t dev)
{
struct device *ldev;
	STAILQ_FOREACH(ldev, &devq, list) {
		if ( devequal(ldev, dev) )
			return ldev;
	}
	return 0;
}


int
device_printf(device_t dev, const char *fmt, ...)
{
int rval;
va_list ap;
	rval  = fprintf(stdout,"%s:",device_get_nameunit(dev));
	va_start(ap, fmt);
	rval += vfprintf(stdout,fmt,ap);
	va_end(ap);
	return rval;
}

static uint32_t
get_pci_triple(const char *drvnam)
{
unsigned b,d,f;
	if ( drvnam && 3 == sscanf(drvnam,"%i:%i.%i",&b,&d,&f) )
		return (b<<8) | PCI_DEVFN(d,f);
	return -1;
}

static void
get_name_unit(const char *drvnam, char *nm, int *punit)
{
int l = strlen(drvnam);
int i;
	if ( l > 0 ) {
		for ( i=l-1; i>=0 && isdigit(drvnam[i]); i-- )
			/* nothing else to do */;
		if ( 1 != sscanf(drvnam+i,"%d",punit) )
			*punit = 0; /* wildcard */
		strncpy(nm, drvnam, i+1);
		nm[i+1]=0;
	} else {
		/* wildcards */
		*nm    = 0;
		*punit = 0;
	}
}

static int
matches(driver_t *dr, const char *pat)
{
	if ( 0 == *pat || '*' == *pat )
		return 1;
	return !strcmp(pat, dr->name);
}

static int
pci_slot_empty(int b, int d, int f)
{
uint16_t id;
	pci_read_config_word(b,d,f,PCI_VENDOR_ID,&id);
	return ( 0xffff == id );
}

static int
pci_is_ether(int b, int d, int f)
{
uint16_t dclass;
	if ( pci_slot_empty(b,d,f) )
		return 0;
	pci_read_config_word(b,d,f,PCI_CLASS_DEVICE, &dclass);
	return PCI_CLASS_NETWORK_ETHERNET == dclass;
}

/* this catches the case of an unpopulated slot (returning 0) */
static int
pci_num_functions(int b, int d)
{
uint8_t h;
	if ( pci_slot_empty(b,d,0) )
		return 0;
	pci_read_config_byte(b,d,0,PCI_HEADER_TYPE,&h);
	return (h & 0x80) ? PCI_MAX_FUNCTIONS : 1; /* multifunction device ? */
}

int
libbsdport_netdriver_dump(FILE *f)
{
struct device *ldev;
int           ndevs;
unsigned      w;

	if ( !f )
		f = stdout;

	ndevs = 0;
	fprintf(f, "PCI Network device information:\n");
	rtems_bsdnet_semaphore_obtain();
	STAILQ_FOREACH(ldev, &devq, list) {
		/* ASSUME LIST ELEMENTS DO NOT DISAPPEAR
		 * so we can release the lock while printing...
		 */
		rtems_bsdnet_semaphore_release();
		w=fprintf(f,"%-6s -- (0x%x:%x.%x)",
				device_get_nameunit(ldev),
				ldev->bushdr.pci.bus,
				ldev->bushdr.pci.dev,
				ldev->bushdr.pci.fun);
		for ( ; w < 24 ; w++)
			fputc(' ',f);
		if ( ldev->desc )
			fprintf(f," %s",ldev->desc);
		fputc('\n',f);


		ndevs++;
		rtems_bsdnet_semaphore_obtain();
	}
	rtems_bsdnet_semaphore_release();
	return ndevs;
}

#define UNITMATCH(wanted, unit, bdfunit) \
	((wanted) < 0 ? ((wanted) & 0xffff) == (bdfunit) : (wanted) == (unit))

int
libbsdport_netdriver_attach(struct rtems_bsdnet_ifconfig *cfg, int attaching)
{
char     nm[20]; /* copy of the name */
int      unit,thisunit,wantedunit;
int      i,b,d,f;
int      prob;
driver_t *dr;
device_t dev   = 0;
device_t tmpdev;
int      error = 0;
int      bdfunit;

		if ( !attaching )
			return ENOTSUP;

		if ( (wantedunit = get_pci_triple(cfg->name)) < 0 ) {
			get_name_unit(cfg->name, nm, &wantedunit);
		} else {
			wantedunit |= 1<<31;
			nm[0]=0;
		}
#ifdef DEBUG
		printf("Wanted unit is 0x%x, pattern '%s'\n", wantedunit, nm);
#endif

		unit = 0;
		for ( i=0; (dr=libbsdport_netdriver_table[i]); i++ ) {
			/* Find matching driver */
#ifdef DEBUG
			printf("Trying driver '%s' ...", dr->name);
#endif
			if ( matches(dr, nm) ) {
#ifdef DEBUG
			printf("MATCH\n");
#endif

				assert( dr->methods );

				thisunit = 0;

				if ( DEV_TYPE_PCI != dr->type ) {
					fprintf(stderr,"Non-PCI driver '%s' not supported; skipping\n", dr->name);
					continue;
				}

				dev = devalloc(dr);
				for ( b=0; b<pci_bus_count(); b++)
					for ( d=0; d<PCI_MAX_DEVICES; d++ ) {
						for ( f=0; f<pci_num_functions(b,d); f++ ) {
							if ( ! pci_is_ether(b,d,f) )
								continue;

							dev->bushdr.pci.bus = b;
							dev->bushdr.pci.dev = d;
							dev->bushdr.pci.fun = f;

							bdfunit = (b<<8) | PCI_DEVFN(d,f);

#ifdef DEBUG
							printf("Probing PCI 0x%x:%x.%x\n",
								bdfunit>>8, PCI_SLOT(bdfunit), PCI_FUNC(bdfunit));
#endif

							/* has this device been attached already ? */
							if ( (tmpdev = devattached(dev)) ) {
								if ( dev->drv == tmpdev->drv )
									thisunit++;
								unit++;
								if ( UNITMATCH(wantedunit, unit, bdfunit) ) {
									fprintf(stderr,"Device '%s' has already been attached\n", device_get_nameunit(dev));
									error = EBUSY;
									goto bail;
								}
							} else {
								switch ( ( prob = dr->methods->probe(dev) ) ) {
										/* LOW_PRIORITY currently unsupported; list preferred drivers first */
									case BUS_PROBE_LOW_PRIORITY:
									case BUS_PROBE_DEFAULT:
										/* accepted */
										thisunit++;
										unit++;
										/* wanted unit == 0 means next avail.
										 * unit is acceptable.
										 */
#ifdef DEBUG
										printf("->SUCCESS\n");
#endif
										if ( 0 == wantedunit || UNITMATCH(wantedunit, unit, bdfunit) ) {
												error = devattach(dev, thisunit, cfg);
												if ( !error )
													dev = 0; /* is now on list */
												goto bail;
										}
										break;

									default:
#ifdef DEBUG
										printf("->FAILED\n");
#endif
										/* probe failed */
										break;
								}
							}
							devclean(dev);
						} /* for all functions */
					} /* for all busses + slots */
				devfree(dev); dev = 0;
			} /* matching driver */
#ifdef DEBUG
			else printf("NO MATCH\n");
#endif
		} /* for all drivers */

		/* Nothing found */
		error = ENODEV;
bail:
	if (dev)
		devfree(dev);
	return error;
}

device_t
libbsdport_netdriver_get_dev(const char *name)
{
struct device *ldev;

	if ( !name )
		return 0;

	rtems_bsdnet_semaphore_obtain();
		STAILQ_FOREACH(ldev, &devq, list) {
			if ( !strcmp(name, device_get_nameunit(ldev)) )
				break;
		}
	rtems_bsdnet_semaphore_release();
	return ldev;
}