Chameleon

Chameleon Svn Source Tree

Root/trunk/i386/libsaio/device_inject.c

1/*␊
2 *␉Copyright 2009 Jasmin Fazlic All rights reserved.␊
3 */␊
4/*␊
5 *␉Cleaned and merged by iNDi␊
6 */␊
7␊
8#include "config.h"␊
9#include "libsaio.h"␊
10#include "boot.h"␊
11#include "bootstruct.h"␊
12#include "pci.h"␊
13#include "pci_root.h"␊
14#include "device_inject.h"␊
15#include "convert.h"␊
16␊
17#if DEBUG_INJECT␊
18␉#define DBG(x...)␉printf(x)␊
19#else␊
20␉#define DBG(x...)␉msglog(x)␊
21#endif␊
22␊
23uint32_t␉devices_number␉= 1; // nvidia.c␊
24uint32_t␉location_number␉= 1; // networking.c␊
25␊
26DevPropString␉*string␉␉= NULL;␊
27uint8_t␉␉*stringdata␉= NULL;␊
28uint32_t␉stringlength␉= 0;␊
29␊
30char *efi_inject_get_devprop_string(uint32_t *len)␊
31{␊
32␉if(string) {␊
33␉␉*len = string->length;␊
34␉␉return devprop_generate_string(string);␊
35␉}␊
36␉verbose("efi_inject_get_devprop_string NULL trying stringdata\n");␊
37␉return NULL;␊
38}␊
39␊
40void setupDeviceProperties(Node *node)␊
41{␊
42 const char *val;␊
43 uint8_t *binStr;␊
44 int cnt, cnt2;␊
45␊
46 static char DEVICE_PROPERTIES_PROP[] = "device-properties";␊
47␊
48 /* Generate devprop string.␊
49 */␊
50 uint32_t strlength;␊
51 char *string = efi_inject_get_devprop_string(&strlength);␊
52␊
53 /* Use the static "device-properties" boot config key contents if available,␊
54 * otheriwse use the generated one.␊
55 */ ␊
56␉if (!getValueForKey(kDeviceProperties, &val, &cnt, &bootInfo->chameleonConfig) && string)␊
57␉{␊
58␉␉val = (const char *)string;␊
59␉␉cnt = strlength * 2;␊
60␉}␊
61␊
62␉if (cnt > 1)␊
63␉{␊
64␉␉binStr = convertHexStr2Binary(val, &cnt2);␊
65␉␉if (cnt2 > 0)␊
66␉␉{␊
67␉␉␉DT__AddProperty(node, DEVICE_PROPERTIES_PROP, cnt2, binStr);␊
68␉␉␉DBG("Adding device-properties string to DT\n");␊
69␉␉}␊
70␉}␊
71}␊
72␊
73DevPropString *devprop_create_string(void)␊
74{␊
75␉string = (DevPropString *)malloc(sizeof(DevPropString));␊
76␊
77␉if(string == NULL)␊
78␉{␊
79␉␉return NULL;␊
80␉}␊
81␉␊
82␉memset(string, 0, sizeof(DevPropString));␊
83␉string->length = 12;␊
84␉string->WHAT2 = 0x01000000;␊
85␉return string;␊
86}␊
87␊
88DevPropDevice *devprop_add_device(DevPropString *string, char *path)␊
89{␊
90␉DevPropDevice␉*device␉␉␉= NULL;␊
91␉const char␉pciroot_string[]␉= "PciRoot(0x";␊
92␉const char␉pci_device_string[]␉= "Pci(0x";␊
93␊
94␉if (string == NULL || path == NULL)␊
95␉{␊
96␉␉printf("ERROR null device path\n");␊
97␉␉return NULL;␊
98␉}␊
99␊
100␉if (strncmp(path, pciroot_string, strlen(pciroot_string)))␊
101␉{␊
102␉␉printf("ERROR parsing device path\n");␊
103␉␉return NULL;␊
104␉}␊
105␊
106␉if (!(device = malloc(sizeof(DevPropDevice))))␊
107␉{␊
108␉␉printf("ERROR malloc failed\n");␊
109␉␉return NULL;␊
110␉}␊
111␊
112␉memset(device, 0, sizeof(DevPropDevice));␊
113␉device->acpi_dev_path._UID = getPciRootUID();␊
114␊
115␉int numpaths = 0;␊
116␉int␉␉x, curr = 0;␊
117␉char␉buff[] = "00";␊
118␊
119␉for (x = 0; x < strlen(path); x++)␊
120␉{␊
121␉␉if (!strncmp(&path[x], pci_device_string, strlen(pci_device_string)))␊
122␉␉{␊
123␉␉␉x+=strlen(pci_device_string);␊
124␉␉␉curr=x;␊
125␉␉␉while(path[++x] != ',');␊
126␉␉␉if(x-curr == 2)␊
127␉␉␉{␊
128␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
129␉␉␉}␊
130␉␉␉else if(x-curr == 1)␊
131␉␉␉{␊
132␉␉␉␉sprintf(buff, "%c", path[curr]);␊
133␉␉␉}␊
134␉␉␉else␊
135␉␉␉{␊
136␉␉␉␉printf("ERROR parsing device path\n");␊
137␉␉␉␉numpaths = 0;␊
138␉␉␉␉break;␊
139␉␉␉}␊
140␉␉␉device->pci_dev_path[numpaths].device =␉ascii_hex_to_int(buff);␊
141␊
142␉␉␉x += 3; // 0x␊
143␉␉␉curr = x;␊
144␉␉␉while(path[++x] != ')');␊
145␉␉␉if(x-curr == 2)␊
146␉␉␉{␊
147␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
148␉␉␉}␊
149␉␉␉else if(x-curr == 1)␊
150␉␉␉{␊
151␉␉␉␉sprintf(buff, "%c", path[curr]);␊
152␉␉␉}␊
153␉␉␉else␊
154␉␉␉{␊
155␉␉␉␉printf("ERROR parsing device path\n");␊
156␉␉␉␉numpaths = 0;␊
157␉␉␉␉break;␊
158␉␉␉}␊
159␉␉␉device->pci_dev_path[numpaths].function = ascii_hex_to_int(buff); // TODO: find dev from char *path␊
160␊
161␉␉␉numpaths++;␊
162␉␉}␊
163␉}␊
164␉␊
165␉if(!numpaths)␊
166␉{␊
167␉␉free(device);␊
168␉␉return NULL;␊
169␉}␊
170␊
171␉device->numentries = 0x00;␊
172␊
173␉device->acpi_dev_path.length␉= 0x0c;␊
174␉device->acpi_dev_path.type␉= 0x02;␊
175␉device->acpi_dev_path.subtype␉= 0x01;␊
176␉device->acpi_dev_path._HID␉= 0xd041030a; // 0x0a0341d0␊
177␊
178␉device->num_pci_devpaths = numpaths;␊
179␉device->length = 24 + (6*numpaths);␊
180␊
181␉int␉␉i;␊
182␊
183␉for(i = 0; i < numpaths; i++)␊
184␉{␊
185␉␉device->pci_dev_path[i].length = 0x06;␊
186␉␉device->pci_dev_path[i].type = 0x01;␊
187␉␉device->pci_dev_path[i].subtype = 0x01;␊
188␉}␊
189␊
190␉device->path_end.length = 0x04;␊
191␉device->path_end.type = 0x7f;␊
192␉device->path_end.subtype = 0xff;␊
193␊
194␉device->string = string;␊
195␉device->data = NULL;␊
196␊
197␉if(!string->entries)␊
198␉{␊
199␉␉if (!(string->entries = (struct DevPropDevice**) malloc(sizeof(device) * DEV_PROP_DEVICE_MAX_ENTRIES)))␊
200␉␉{␊
201␉␉␉free(device);␊
202␉␉␉return NULL;␊
203␉␉}␊
204␉}␊
205␊
206␉/* FIXME: probably needs bounds checking, as well as error handling in event of malloc failure */␊
207␉string->length += device->length;␊
208␉string->entries[string->numentries++] = (DevPropDevice*)malloc(sizeof(device));␊
209␉string->entries[string->numentries-1] = device;␊
210␊
211␉return device;␊
212}␊
213␊
214int devprop_add_value(DevPropDevice *device, char *nm, uint8_t *vl, uint32_t len)␊
215{␊
216␊
217␉if(!nm || !vl || !len)␊
218␉{␊
219␉␉return 0;␊
220␉}␊
221␉uint32_t length␉= ((strlen(nm) * 2) + len + (2 * sizeof(uint32_t)) + 2);␊
222␉uint8_t *data␉= (uint8_t*)malloc(length);␊
223␊
224␉if(!data)␊
225␉{␊
226␉␉return 0;␊
227␉}␊
228␊
229␉memset(data, 0, length);␊
230␉uint32_t off = 0;␊
231␉data[off+1] = (uint8_t)(((strlen(nm) * 2) + 6) >> 8);␊
232␉data[off] = ((strlen(nm) * 2) + 6) & 0x00FF;␊
233␊
234␉off += 4;␊
235␉uint32_t i=0, l = strlen(nm);␊
236␉for(i = 0 ; i < l ; i++, off += 2)␊
237␉{␊
238␉␉data[off] = *nm++;␊
239␉}␊
240␊
241␉off += 2;␊
242␉l = len;␊
243␉uint32_t *datalength = (uint32_t*)&data[off];␊
244␉*datalength = (uint32_t)(l + 4);␊
245␉off += 4;␊
246␉for(i = 0 ; i < l ; i++, off++)␊
247␉{␊
248␉␉data[off] = *vl++;␊
249␉}␊
250␉␊
251␉uint32_t offset = device->length - (24 + (6 * device->num_pci_devpaths));␊
252␉␊
253␉uint8_t *newdata = (uint8_t*)malloc((length + offset));␊
254␉if(!newdata)␊
255␉{␊
256␉␉return 0;␊
257␉}␊
258␊
259␉if(device->data)␊
260␉{␊
261␉␉if(offset > 1)␊
262␉␉{␊
263␉␉␉memcpy(newdata, device->data, offset);␊
264␉␉}␊
265␉}␊
266␊
267␉memcpy(newdata + offset, data, length);␊
268␊
269␉device->length += length;␊
270␉device->string->length += length;␊
271␉device->numentries++;␊
272␊
273␉if(!device->data)␊
274␉{␊
275␉␉device->data = (uint8_t *)malloc(sizeof(uint8_t));␊
276␉}␊
277␉else␊
278␉{␊
279␉␉free(device->data);␊
280␉}␊
281␊
282␉free(data);␊
283␉device->data = newdata;␊
284␊
285␉return 1;␊
286}␊
287␊
288// devprop_generate_string optimized by cparm␊
289char *devprop_generate_string(DevPropString *string)␊
290{␊
291␉int len = ((string->length * 2) + 1);␊
292␉char *buffer = (char *)malloc(len);␊
293␉char *ptr = buffer;␊
294␊
295␉if(!buffer)␊
296␉{␊
297␉␉return NULL;␊
298␉}␊
299␊
300␉snprintf(buffer, len, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊
301␉␉␉dp_swap16(string->numentries), string->WHAT3);␊
302␉buffer += 24;␊
303␉len -= 24;␊
304␉int i = 0, x = 0;␊
305␊
306␉while(i < string->numentries)␊
307␉{␊
308␉ if (!(i < DEV_PROP_DEVICE_MAX_ENTRIES))␊
309␉␉{␊
310␉ break;␊
311␉ }␊
312␊
313␉ if(!len)␊
314␉␉{␊
315␉ break;␊
316␉ }␊
317␊
318␉␉snprintf(buffer, len, "%08x%04x%04x", dp_swap32(string->entries[i]->length),␊
319␉␉␉␉dp_swap16(string->entries[i]->numentries), string->entries[i]->WHAT2);␊
320␉␉␊
321␉␉buffer += 16;␊
322␉␉len -= 16;␊
323␉␉snprintf(buffer, len, "%02x%02x%04x%08x%08x", string->entries[i]->acpi_dev_path.type,␊
324␉␉␉␉string->entries[i]->acpi_dev_path.subtype,␊
325␉␉␉␉dp_swap16(string->entries[i]->acpi_dev_path.length),␊
326␉␉␉␉string->entries[i]->acpi_dev_path._HID,␊
327␉␉␉␉dp_swap32(string->entries[i]->acpi_dev_path._UID));␊
328␊
329␉␉buffer += 24;␊
330␉␉len -= 24;␊
331␉␉for(x = 0;x < string->entries[i]->num_pci_devpaths; x++)␊
332␉␉{␊
333␉␉␉if(!len)␊
334␉␉␉{␊
335␉␉␉␉break;␊
336␉␉␉}␊
337␉␉␉snprintf(buffer, len, "%02x%02x%04x%02x%02x", string->entries[i]->pci_dev_path[x].type,␊
338␉␉␉␉␉string->entries[i]->pci_dev_path[x].subtype,␊
339␉␉␉␉␉dp_swap16(string->entries[i]->pci_dev_path[x].length),␊
340␉␉␉␉␉string->entries[i]->pci_dev_path[x].function,␊
341␉␉␉␉␉string->entries[i]->pci_dev_path[x].device);␊
342␉␉␉buffer += 12;␊
343␉␉␉len -= 12;␊
344␉␉}␊
345␊
346␉␉if(!len)␊
347␉␉{␊
348␉␉␉break;␊
349␉␉}␊
350␊
351␉␉snprintf(buffer, len, "%02x%02x%04x", string->entries[i]->path_end.type,␊
352␉␉␉␉string->entries[i]->path_end.subtype,␊
353␉␉␉␉dp_swap16(string->entries[i]->path_end.length));␊
354␉␉␊
355␉␉buffer += 8;␊
356␉␉len -= 8;␊
357␉␉uint8_t *dataptr = string->entries[i]->data;␊
358␉␉for(x = 0; (uint32_t)x < (string->entries[i]->length) - (24 + (6 * string->entries[i]->num_pci_devpaths)) ; x++)␊
359␉␉{␊
360␉␉␉if(!len)␊
361␉␉␉{␊
362␉␉␉␉break;␊
363␉␉␉}␊
364␉␉␉snprintf(buffer, len, "%02x", *dataptr++);␊
365␉␉␉buffer += 2;␊
366␉␉␉len -= 2;␊
367␉␉}␊
368␉␉i++;␊
369␉}␊
370␉return ptr;␊
371}␊
372␊
373void devprop_free_string(DevPropString *string)␊
374{␊
375␊
376␉if(!string) {␊
377␉␉return;␊
378␉}␊
379␊
380␉int i;␊
381␉for(i = 0; i < string->numentries; i++)␊
382␉{␊
383␉␉if(string->entries[i])␊
384␉␉{␊
385␉␉␉if(string->entries[i]->data)␊
386␉␉␉{␊
387␉␉␉␉free(string->entries[i]->data);␊
388␉␉␉␉string->entries[i]->data = NULL;␊
389␉␉␉}␊
390␉␉␉free(string->entries[i]);␊
391␉␉␉string->entries[i] = NULL;␊
392␉␉}␊
393␉}␊
394␊
395␉free(string);␊
396␉string = NULL;␊
397}␊
398␊
399/* ======================================================= */␊
400␊
401/* a fine place for this code */␊
402␊
403// (devprop_add_network_template) and (setup_eth_builtin) moved to i386/libsaio/networking.c␊
404

Archive Download this file

Revision: 2858