0.3.3.1/src/device.c

/*
 *  Copyright (C) 2005  Anders Gavare.  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright  
 *     notice, this list of conditions and the following disclaimer in the 
 *     documentation and/or other materials provided with the distribution.
 *  3. The name of the author may not be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 *  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE   
 *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 *  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 *  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 *  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 *  SUCH DAMAGE.
 *
 *
 *  $Id: device.c,v 1.13 2005/02/26 17:37:25 debug Exp $
 *
 *  Device registry framework.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "device.h"
#include "misc.h"


static struct device_entry *device_entries = NULL;
static int device_entries_sorted = 0;
static int n_device_entries = 0;
static int device_exit_on_error = 1;


/*
 *  device_entry_compar():
 *
 *  Internal function, used by sort_entries().
 */
static int device_entry_compar(const void *a, const void *b)
{
        struct device_entry *pa = (struct device_entry *) a;
        struct device_entry *pb = (struct device_entry *) b;

        return strcmp(pa->name, pb->name);
}


/*
 *  sort_entries():
 *
 *  Internal function. Sorts the device_entries array in alphabetic order.
 */
static void sort_entries(void)
{
        qsort(device_entries, n_device_entries, sizeof(struct device_entry),
            device_entry_compar);

        device_entries_sorted = 1;
}


/*
 *  device_register():
 *
 *  Registers a device. The device is added to the end of the device_entries
 *  array, and the sorted flag is set to zero.
 *
 *  NOTE: It would be a bad thing if two devices had the same name. However,
 *        that isn't checked here, it is up to the caller!
 *
 *  Return value is 1 if the device was registered, 0 otherwise.
 */
int device_register(char *name, int (*initf)(struct devinit *))
{
        device_entries = realloc(device_entries, sizeof(struct device_entry)
            * (n_device_entries + 1));
        if (device_entries == NULL) {
                fprintf(stderr, "device_register(): out of memory\n");
                exit(1);
        }

        memset(&device_entries[n_device_entries], 0,
            sizeof(struct device_entry));

        device_entries[n_device_entries].name = strdup(name);
        device_entries[n_device_entries].initf = initf;

        device_entries_sorted = 0;
        n_device_entries ++;
        return 1;
}


/*
 *  device_lookup():
 *
 *  Lookup a device name by scanning the device_entries array (as a binary
 *  search tree).
 *
 *  Return value is a pointer to the device_entry on success, or a NULL pointer
 *  if there was no such device.
 */
struct device_entry *device_lookup(char *name)
{
        int i, step, r, do_return = 0;

        if (name == NULL) {
                fprintf(stderr, "device_lookup(): NULL ptr\n");
                exit(1);
        }

        if (!device_entries_sorted)
                sort_entries();

        if (n_device_entries == 0)
                return NULL;

        i = n_device_entries / 2;
        step = i/2 + 1;

        for (;;) {
                if (i < 0)
                        i = 0;
                if (i >= n_device_entries)
                        i = n_device_entries - 1;

                /*  printf("device_lookup(): i=%i step=%i\n", i, step);
                    printf("  name='%s', '%s'\n", name,
                    device_entries[i].name);  */

                r = strcmp(name, device_entries[i].name);

                if (r < 0) {
                        /*  Go left:  */
                        i -= step;
                        if (step == 0)
                                i --;
                } else if (r > 0) {
                        /*  Go right:  */
                        i += step;
                        if (step == 0)
                                i ++;
                } else {
                        /*  Found it!  */
                        return &device_entries[i];
                }

                if (do_return)
                        return NULL;

                if (step == 0)
                        do_return = 1;

                step /= 2;
        }
}


/*
 *  device_unregister():
 *
 *  Unregisters a device.
 *
 *  Return value is 1 if a device was unregistered, 0 otherwise.
 */
int device_unregister(char *name)
{
        size_t i;
        struct device_entry *p = device_lookup(name);

        if (p == NULL) {
                fatal("device_unregister(): no such device (\"%s\")\n", name);
                return 0;
        }

        i = (size_t)p - (size_t)device_entries;
        i /= sizeof(struct device_entry);

        free(device_entries[i].name);
        device_entries[i].name = NULL;

        if (i == n_device_entries-1) {
                /*  Do nothing if we're removing the last array element.  */
        } else {
                /*  Remove array element i by copying the last element
                    to i's position:  */
                device_entries[i] = device_entries[n_device_entries-1];

                /*  The array is not sorted anymore:  */
                device_entries_sorted = 0;
        }

        n_device_entries --;

        /*  TODO: realloc?  */
        return 1;
}


/*
 *  device_add():
 *
 *  Add a device to a machine.
 *
 *      "kn210 addr=0x12340000"   adds a kn210 device at a specific address.
 */
void *device_add(struct machine *machine, char *name_and_params)
{
        struct device_entry *p;
        struct devinit devinit;
        char *s2, *s3;
        size_t len;

        memset(&devinit, 0, sizeof(struct devinit));
        devinit.machine = machine;

        /*  Default values:  */
        devinit.addr_mult = 1;

        /*  Get the device name first:  */
        s2 = name_and_params;
        while (s2[0] != ',' && s2[0] != ' ' && s2[0] != '\0')
                s2 ++;

        len = (size_t)s2 - (size_t)name_and_params;
        devinit.name = malloc(len + 1);
        if (devinit.name == NULL) {
                fprintf(stderr, "device_add(): out of memory\n");
                exit(1);
        }
        memcpy(devinit.name, name_and_params, len);
        devinit.name[len] = '\0';

        p = device_lookup(devinit.name);
        if (p == NULL) {
                fatal("no such device (\"%s\")\n", devinit.name);
                if (device_exit_on_error)
                        exit(1);
                else
                        goto return_fail;
        }

        /*  Get params from name_and_params:  */
        while (*s2 != '\0') {
                /*  Skip spaces, commas, and semicolons:  */
                while (*s2 == ' ' || *s2 == ',' || *s2 == ';')
                        s2 ++;

                if (*s2 == '\0')
                        break;

                /*  s2 now points to the next param. eg "addr=1234"  */

                /*  Get a word (until there is a '=' sign):  */
                s3 = s2;
                while (*s3 != '=' && *s3 != '\0')
                        s3 ++;
                if (s3 == s2) {
                        fatal("weird param: %s\n", s2);
                        if (device_exit_on_error)
                                exit(1);
                        else
                                goto return_fail;
                }
                s3 ++;
                /*  s3 now points to the parameter value ("1234")  */

                if (strncmp(s2, "addr=", 5) == 0) {
                        devinit.addr = mystrtoull(s3, NULL, 0);
                } else if (strncmp(s2, "len=", 4) == 0) {
                        devinit.len = mystrtoull(s3, NULL, 0);
                } else if (strncmp(s2, "addr_mult=", 10) == 0) {
                        devinit.addr_mult = mystrtoull(s3, NULL, 0);
                } else if (strncmp(s2, "irq=", 4) == 0) {
                        devinit.irq_nr = mystrtoull(s3, NULL, 0);
                } else {
                        fatal("unknown param: %s\n", s2);
                        if (device_exit_on_error)
                                exit(1);
                        else
                                goto return_fail;
                }

                /*  skip to the next param:  */
                s2 = s3;
                while (*s2 != '\0' && *s2 != ' ' && *s2 != ',' && *s2 != ';')
                        s2 ++;
        }


        /*
         *  Call the init function for this device:
         */

        devinit.return_ptr = NULL;

        if (!p->initf(&devinit)) {
                fatal("error adding device (\"%s\")\n", name_and_params);
                if (device_exit_on_error)
                        exit(1);
                else
                        goto return_fail;
        }

        free(devinit.name);
        return devinit.return_ptr;

return_fail:
        free(devinit.name);
        return NULL;
}


/*
 *  device_dumplist():
 *
 *  Dump a list of all registered devices.  (If the list is not sorted when
 *  this function is called, it is implicitly sorted.)
 */
void device_dumplist(void)
{
        int i;

        if (!device_entries_sorted)
                sort_entries();

        for (i=0; i<n_device_entries; i++) {
                debug("  %s", device_entries[i].name);

                /*  TODO: flags?  */

                debug("\n");
        }
}


/*
 *  device_set_exit_on_error():
 */
void device_set_exit_on_error(int exit_on_error)
{
        device_exit_on_error = exit_on_error;
}


/*
 *  device_init():
 *
 *  Initialize the device registry, and call autodev_init() to automatically
 *  add all normal devices (from the devices/ directory).
 *
 *  This function should be called before any other device_*() function is used.
 */
void device_init(void)
{
        device_entries = NULL;
        device_entries_sorted = 0;
        n_device_entries = 0;

        autodev_init();
}

1	/*
2	* Copyright (C) 2005 Anders Gavare. All rights reserved.
3	*
4	* Redistribution and use in source and binary forms, with or without
5	* modification, are permitted provided that the following conditions are met:
6	*
7	* 1. Redistributions of source code must retain the above copyright
8	* notice, this list of conditions and the following disclaimer.
9	* 2. Redistributions in binary form must reproduce the above copyright
10	* notice, this list of conditions and the following disclaimer in the
11	* documentation and/or other materials provided with the distribution.
12	* 3. The name of the author may not be used to endorse or promote products
13	* derived from this software without specific prior written permission.
14	*
15	* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16	* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18	* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19	* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20	* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21	* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22	* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24	* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25	* SUCH DAMAGE.
26	*
27	*
28	* $Id: device.c,v 1.13 2005/02/26 17:37:25 debug Exp $
29	*
30	* Device registry framework.
31	*/
32
33	#include <stdio.h>
34	#include <stdlib.h>
35	#include <string.h>
36
37	#include "device.h"
38	#include "misc.h"
39
40
41	static struct device_entry *device_entries = NULL;
42	static int device_entries_sorted = 0;
43	static int n_device_entries = 0;
44	static int device_exit_on_error = 1;
45
46
47	/*
48	* device_entry_compar():
49	*
50	* Internal function, used by sort_entries().
51	*/
52	static int device_entry_compar(const void a, const void b)
53	{
54	struct device_entry pa = (struct device_entry ) a;
55	struct device_entry pb = (struct device_entry ) b;
56
57	return strcmp(pa->name, pb->name);
58	}
59
60
61	/*
62	* sort_entries():
63	*
64	* Internal function. Sorts the device_entries array in alphabetic order.
65	*/
66	static void sort_entries(void)
67	{
68	qsort(device_entries, n_device_entries, sizeof(struct device_entry),
69	device_entry_compar);
70
71	device_entries_sorted = 1;
72	}
73
74
75	/*
76	* device_register():
77	*
78	* Registers a device. The device is added to the end of the device_entries
79	* array, and the sorted flag is set to zero.
80	*
81	* NOTE: It would be a bad thing if two devices had the same name. However,
82	* that isn't checked here, it is up to the caller!
83	*
84	* Return value is 1 if the device was registered, 0 otherwise.
85	*/
86	int device_register(char name, int (initf)(struct devinit *))
87	{
88	device_entries = realloc(device_entries, sizeof(struct device_entry)
89	* (n_device_entries + 1));
90	if (device_entries == NULL) {
91	fprintf(stderr, "device_register(): out of memory\n");
92	exit(1);
93	}
94
95	memset(&device_entries[n_device_entries], 0,
96	sizeof(struct device_entry));
97
98	device_entries[n_device_entries].name = strdup(name);
99	device_entries[n_device_entries].initf = initf;
100
101	device_entries_sorted = 0;
102	n_device_entries ++;
103	return 1;
104	}
105
106
107	/*
108	* device_lookup():
109	*
110	* Lookup a device name by scanning the device_entries array (as a binary
111	* search tree).
112	*
113	* Return value is a pointer to the device_entry on success, or a NULL pointer
114	* if there was no such device.
115	*/
116	struct device_entry device_lookup(char name)
117	{
118	int i, step, r, do_return = 0;
119
120	if (name == NULL) {
121	fprintf(stderr, "device_lookup(): NULL ptr\n");
122	exit(1);
123	}
124
125	if (!device_entries_sorted)
126	sort_entries();
127
128	if (n_device_entries == 0)
129	return NULL;
130
131	i = n_device_entries / 2;
132	step = i/2 + 1;
133
134	for (;;) {
135	if (i < 0)
136	i = 0;
137	if (i >= n_device_entries)
138	i = n_device_entries - 1;
139
140	/* printf("device_lookup(): i=%i step=%i\n", i, step);
141	printf(" name='%s', '%s'\n", name,
142	device_entries[i].name); */
143
144	r = strcmp(name, device_entries[i].name);
145
146	if (r < 0) {
147	/* Go left: */
148	i -= step;
149	if (step == 0)
150	i --;
151	} else if (r > 0) {
152	/* Go right: */
153	i += step;
154	if (step == 0)
155	i ++;
156	} else {
157	/* Found it! */
158	return &device_entries[i];
159	}
160
161	if (do_return)
162	return NULL;
163
164	if (step == 0)
165	do_return = 1;
166
167	step /= 2;
168	}
169	}
170
171
172	/*
173	* device_unregister():
174	*
175	* Unregisters a device.
176	*
177	* Return value is 1 if a device was unregistered, 0 otherwise.
178	*/
179	int device_unregister(char *name)
180	{
181	size_t i;
182	struct device_entry *p = device_lookup(name);
183
184	if (p == NULL) {
185	fatal("device_unregister(): no such device (\"%s\")\n", name);
186	return 0;
187	}
188
189	i = (size_t)p - (size_t)device_entries;
190	i /= sizeof(struct device_entry);
191
192	free(device_entries[i].name);
193	device_entries[i].name = NULL;
194
195	if (i == n_device_entries-1) {
196	/* Do nothing if we're removing the last array element. */
197	} else {
198	/* Remove array element i by copying the last element
199	to i's position: */
200	device_entries[i] = device_entries[n_device_entries-1];
201
202	/* The array is not sorted anymore: */
203	device_entries_sorted = 0;
204	}
205
206	n_device_entries --;
207
208	/* TODO: realloc? */
209	return 1;
210	}
211
212
213	/*
214	* device_add():
215	*
216	* Add a device to a machine.
217	*
218	* "kn210 addr=0x12340000" adds a kn210 device at a specific address.
219	*/
220	void device_add(struct machine machine, char *name_and_params)
221	{
222	struct device_entry *p;
223	struct devinit devinit;
224	char s2, s3;
225	size_t len;
226
227	memset(&devinit, 0, sizeof(struct devinit));
228	devinit.machine = machine;
229
230	/* Default values: */
231	devinit.addr_mult = 1;
232
233	/* Get the device name first: */
234	s2 = name_and_params;
235	while (s2[0] != ',' && s2[0] != ' ' && s2[0] != '\0')
236	s2 ++;
237
238	len = (size_t)s2 - (size_t)name_and_params;
239	devinit.name = malloc(len + 1);
240	if (devinit.name == NULL) {
241	fprintf(stderr, "device_add(): out of memory\n");
242	exit(1);
243	}
244	memcpy(devinit.name, name_and_params, len);
245	devinit.name[len] = '\0';
246
247	p = device_lookup(devinit.name);
248	if (p == NULL) {
249	fatal("no such device (\"%s\")\n", devinit.name);
250	if (device_exit_on_error)
251	exit(1);
252	else
253	goto return_fail;
254	}
255
256	/* Get params from name_and_params: */
257	while (*s2 != '\0') {
258	/* Skip spaces, commas, and semicolons: */
259	while (s2 == ' ' \|\| s2 == ',' \|\| *s2 == ';')
260	s2 ++;
261
262	if (*s2 == '\0')
263	break;
264
265	/* s2 now points to the next param. eg "addr=1234" */
266
267	/* Get a word (until there is a '=' sign): */
268	s3 = s2;
269	while (s3 != '=' && s3 != '\0')
270	s3 ++;
271	if (s3 == s2) {
272	fatal("weird param: %s\n", s2);
273	if (device_exit_on_error)
274	exit(1);
275	else
276	goto return_fail;
277	}
278	s3 ++;
279	/* s3 now points to the parameter value ("1234") */
280
281	if (strncmp(s2, "addr=", 5) == 0) {
282	devinit.addr = mystrtoull(s3, NULL, 0);
283	} else if (strncmp(s2, "len=", 4) == 0) {
284	devinit.len = mystrtoull(s3, NULL, 0);
285	} else if (strncmp(s2, "addr_mult=", 10) == 0) {
286	devinit.addr_mult = mystrtoull(s3, NULL, 0);
287	} else if (strncmp(s2, "irq=", 4) == 0) {
288	devinit.irq_nr = mystrtoull(s3, NULL, 0);
289	} else {
290	fatal("unknown param: %s\n", s2);
291	if (device_exit_on_error)
292	exit(1);
293	else
294	goto return_fail;
295	}
296
297	/* skip to the next param: */
298	s2 = s3;
299	while (s2 != '\0' && s2 != ' ' && s2 != ',' && s2 != ';')
300	s2 ++;
301	}
302
303
304	/*
305	* Call the init function for this device:
306	*/
307
308	devinit.return_ptr = NULL;
309
310	if (!p->initf(&devinit)) {
311	fatal("error adding device (\"%s\")\n", name_and_params);
312	if (device_exit_on_error)
313	exit(1);
314	else
315	goto return_fail;
316	}
317
318	free(devinit.name);
319	return devinit.return_ptr;
320
321	return_fail:
322	free(devinit.name);
323	return NULL;
324	}
325
326
327	/*
328	* device_dumplist():
329	*
330	* Dump a list of all registered devices. (If the list is not sorted when
331	* this function is called, it is implicitly sorted.)
332	*/
333	void device_dumplist(void)
334	{
335	int i;
336
337	if (!device_entries_sorted)
338	sort_entries();
339
340	for (i=0; i<n_device_entries; i++) {
341	debug(" %s", device_entries[i].name);
342
343	/* TODO: flags? */
344
345	debug("\n");
346	}
347	}
348
349
350	/*
351	* device_set_exit_on_error():
352	*/
353	void device_set_exit_on_error(int exit_on_error)
354	{
355	device_exit_on_error = exit_on_error;
356	}
357
358
359	/*
360	* device_init():
361	*
362	* Initialize the device registry, and call autodev_init() to automatically
363	* add all normal devices (from the devices/ directory).
364	*
365	* This function should be called before any other device_*() function is used.
366	*/
367	void device_init(void)
368	{
369	device_entries = NULL;
370	device_entries_sorted = 0;
371	n_device_entries = 0;
372
373	autodev_init();
374	}
375