Chameleon

Chameleon Svn Source Tree

Root/branches/cparm/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 "libsaio.h"␊
9#include "bootstruct.h"␊
10#include "pci.h"␊
11#include "pci_root.h"␊
12#include "device_inject.h"␊
13#include "convert.h"␊
14#include "platform.h"␊
15␊
16#ifndef DEBUG_INJECT␊
17#define DEBUG_INJECT 0␊
18#endif␊
19␊
20#if DEBUG_INJECT␊
21#define DBG(x...)␉printf(x)␊
22#else␊
23#define DBG(x...)␊
24#endif␊
25␊
26char *efi_inject_get_devprop_string(uint32_t *len)␊
27{␊
28 struct DevPropString *string = (struct DevPropString *)(uint32_t)get_env(envEFIString);␊
29␊
30␉if(string)␊
31␉{␊
32␉␉*len = string->length;␊
33␉␉return devprop_generate_string(string);␊
34␉}␊
35␉verbose("efi_inject_get_devprop_string NULL trying stringdata\n");␊
36␉return NULL;␊
37}␊
38␊
39void setupDeviceProperties(Node *node)␊
40{␊
41␉const char *val;␊
42␉uint8_t *binStr;␊
43 uint8_t *kbinStr;␊
44␊
45␉int cnt, cnt2;␊
46␉␊
47␉static char DEVICE_PROPERTIES_PROP[] = "device-properties";␊
48␉␊
49␉/* Generate devprop string.␊
50␉ */␊
51␉uint32_t strlength;␊
52␉char *string = efi_inject_get_devprop_string(&strlength);␊
53␉␊
54␉/* Use the static "device-properties" boot config key contents if available,␊
55␉ * otheriwse use the generated one.␊
56␉ */ ␊
57␉if (!getValueForKey(kDeviceProperties, &val, &cnt, DEFAULT_BOOT_CONFIG) && string)␊
58␉{␊
59␉␉val = (const char*)string;␊
60␉␉cnt = strlength * 2;␊
61␉} ␊
62 ␊
63␉if (cnt > 1)␊
64␉{␊
65␉␉binStr = convertHexStr2Binary(val, &cnt2); ␊
66 ␊
67 if (cnt2 > 0)␊
68 {␊
69 kbinStr = (uint8_t*)AllocateKernelMemory(cnt2); ␊
70 bcopy(binStr,kbinStr,cnt2); ␊
71 DT__AddProperty(node, DEVICE_PROPERTIES_PROP, cnt2, kbinStr);␊
72 }␊
73␉}␊
74}␊
75␊
76struct DevPropString *devprop_create_string(void)␊
77{␊
78␉struct DevPropString *string = (struct DevPropString*)malloc(sizeof(struct DevPropString));␊
79␉␊
80␉if(string == NULL)␊
81␉{␊
82␉␉return NULL;␊
83␉}␊
84␉␊
85␉memset(string, 0, sizeof(struct DevPropString));␊
86␉string->length = 12;␊
87␉string->WHAT2 = 0x01000000;␊
88␉return string;␊
89}␊
90␊
91struct DevPropDevice *devprop_make_device(pci_dt_t *pci_dt)␊
92{␊
93␉struct DevPropDevice␉*device;␊
94 int numpaths = 0;␊
95␊
96␉pci_dt_t␉*current;␊
97␉pci_dt_t␉*end;␊
98␉␊
99␉end = root_pci_dev;␊
100 ␊
101␉device = malloc(sizeof(struct DevPropDevice));␊
102␉if (!device) {␊
103 return NULL;␊
104 }␊
105 memset(device, 0, sizeof(struct DevPropDevice));␊
106␊
107␉device->acpi_dev_path._UID = getPciRootUID();␊
108␉while (end != pci_dt)␊
109␉{␊
110␉␉current = pci_dt;␊
111␉␉while (current->parent != end)␊
112␉␉␉current = current->parent;␉␉␉␊
113␉␉end = current;␊
114␊
115␉␉{␊
116 device->pci_dev_path[numpaths].device =␉(uint8_t)current->dev.bits.dev;␊
117␉␉␉device->pci_dev_path[numpaths].function = (uint8_t)current->dev.bits.func;␊
118 numpaths++;␊
119␉␉} ␊
120␉␉␊
121␉}␊
122␉␉␉␊
123␉if(!numpaths)␊
124␉{␊
125 printf("ERROR parsing device path\n");␊
126 free(device);␊
127␉␉return NULL;␊
128␉}␊
129␉␊
130␉device->numentries = 0x00;␊
131␉␊
132␉device->acpi_dev_path.length = 0x0c;␊
133␉device->acpi_dev_path.type = 0x02;␊
134␉device->acpi_dev_path.subtype = 0x01;␊
135␉device->acpi_dev_path._HID = 0xd041030a;␊
136␉␊
137␉device->num_pci_devpaths = numpaths;␊
138␉device->length = 24 + (6*numpaths);␊
139␉␊
140␉int␉␉i; ␊
141␉␊
142␉for(i = 0; i < numpaths; i++)␊
143␉{␊
144␉␉device->pci_dev_path[i].length = 0x06;␊
145␉␉device->pci_dev_path[i].type = 0x01;␊
146␉␉device->pci_dev_path[i].subtype = 0x01;␊
147␉}␊
148␉␊
149␉device->path_end.length = 0x04;␊
150␉device->path_end.type = 0x7f;␊
151␉device->path_end.subtype = 0xff;␊
152␉␊
153␉device->data = NULL;␊
154␉␉␉␊
155␉return device;␊
156}␊
157␊
158struct DevPropDevice *devprop_add_device(struct DevPropString *string, pci_dt_t * pci_dt)␊
159{␊
160␉struct DevPropDevice␉*device;␊
161␉␉␊
162␉if (string == NULL || pci_dt == NULL) {␊
163␉␉return NULL;␊
164␉}␊
165␉device = devprop_make_device(pci_dt);␊
166␉if (!device) {␊
167 return NULL;␊
168 }␊
169␉␉␊
170␉device->string = string;␊
171␉string->length += device->length;␊
172␉␊
173␉if(!string->entries)␊
174␉{␊
175␉␉if((string->entries = (struct DevPropDevice*)malloc(sizeof(struct DevPropDevice) * MAX_STRING_NUM_ENTRIES))== NULL)␊
176␉␉{␊
177 printf("ERROR parsing device path 2\n");␊
178 ␊
179 free(device);␊
180␉␉␉return NULL;␊
181␉␉}␊
182␉}␊
183␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊
184␉␊
185␉if ((string->numentries+1) < MAX_STRING_NUM_ENTRIES)␊
186 {␊
187␉␉string->numentries++;␊
188␉␉␊
189 }␊
190 else␊
191 {␊
192 free(string->entries);␊
193 free(device);␊
194 return NULL;␊
195 }␉␊
196 ␊
197␉string_entries_arrey[string->numentries-1] = device;␊
198␉␊
199␉return device;␊
200}␊
201␊
202#if 0␊
203struct DevPropDevice *devprop_add_device(struct DevPropString *string, char *path)␊
204{␊
205␉struct DevPropDevice␉*device;␊
206␉const char␉␉pciroot_string[] = "PciRoot(0x";␊
207␉const char␉␉pci_device_string[] = "Pci(0x";␊
208␉␊
209␉if (string == NULL || path == NULL) {␊
210␉␉return NULL;␊
211␉}␊
212␉device = malloc(sizeof(struct DevPropDevice));␊
213␉if (!device) {␊
214 return NULL;␊
215 }␊
216␉if (strncmp(path, pciroot_string, strlen(pciroot_string))) {␊
217 free(device);␊
218␉␉printf("ERROR parsing device path 1\n");␊
219␉␉return NULL;␊
220␉}␊
221␉␊
222␉memset(device, 0, sizeof(struct DevPropDevice));␊
223␉device->acpi_dev_path._UID = getPciRootUID();␊
224␉␊
225␉int numpaths = 0;␊
226␉int␉␉x, curr = 0, w = 0;␊
227 ␊
228␉char␉buff[16];␊
229␉␊
230 ␊
231␉for (x = 0; x < strlen(path); x++) ␊
232␉{␊
233 ␊
234␉␉if (!strncmp(&path[x], pci_device_string, strlen(pci_device_string)))␊
235␉␉{␊
236␉␉␉x+=strlen(pci_device_string);␊
237␉␉␉curr=x;␊
238␉␉␉while(path[++x] != ',');␊
239 ␊
240 w = x-curr;␊
241 ␊
242 if ((w > 4) || /*(w > sizeof(buff)) ||*/ (w == 0)) {␊
243 printf("ERROR parsing device path 2\n");␊
244 break;␊
245 }␊
246 ␊
247 snprintf(buff, x-curr, "%s",&path[curr]);␊
248␉␉␉␊
249␉␉␉device->pci_dev_path[numpaths].device =␉(uint8_t)strtoul(buff, NULL, 16);␊
250␉␉␉␊
251 bzero(buff, sizeof(buff));␊
252 ␊
253␉␉␉x += 3; // 0x␊
254␉␉␉curr = x;␊
255␉␉␉while(path[++x] != ')');␊
256 ␊
257 w = x-curr;␊
258 ␊
259 if ((w > 4) || /*(w > sizeof(buff)) ||*/ (w == 0)) {␊
260 printf("ERROR parsing device path 3\n");␊
261 break;␊
262 }␊
263 ␊
264 snprintf(buff, x-curr, "%s",&path[curr]);␊
265 ␊
266␉␉␉device->pci_dev_path[numpaths].function = (uint8_t)strtoul(buff, NULL, 16); // TODO: find dev from char *path␊
267␉␉␉␊
268␉␉␉numpaths++;␊
269␉␉}␊
270␉}␊
271␉␊
272␉if(!numpaths)␊
273␉{␊
274 printf("ERROR parsing device path 4\n");␊
275 free(device);␊
276␉␉return NULL;␊
277␉}␊
278␉␊
279␉device->numentries = 0x00;␊
280␉␊
281␉device->acpi_dev_path.length = 0x0c;␊
282␉device->acpi_dev_path.type = 0x02;␊
283␉device->acpi_dev_path.subtype = 0x01;␊
284␉device->acpi_dev_path._HID = 0xd041030a;␊
285␉␊
286␉device->num_pci_devpaths = numpaths;␊
287␉device->length = 24 + (6*numpaths);␊
288␉␊
289␉int␉␉i; ␊
290␉␊
291␉for(i = 0; i < numpaths; i++)␊
292␉{␊
293␉␉device->pci_dev_path[i].length = 0x06;␊
294␉␉device->pci_dev_path[i].type = 0x01;␊
295␉␉device->pci_dev_path[i].subtype = 0x01;␊
296␉}␊
297␉␊
298␉device->path_end.length = 0x04;␊
299␉device->path_end.type = 0x7f;␊
300␉device->path_end.subtype = 0xff;␊
301␉␊
302␉device->string = string;␊
303␉device->data = NULL;␊
304␉string->length += device->length;␊
305␉␊
306␉if(!string->entries)␊
307␉{␊
308␉␉if((string->entries = (struct DevPropDevice*)malloc(sizeof(struct DevPropDevice) * MAX_STRING_NUM_ENTRIES))== NULL)␊
309␉␉{␊
310 printf("ERROR parsing device path 6\n");␊
311 ␊
312 free(device);␊
313␉␉␉return NULL;␊
314␉␉}␊
315␉}␊
316␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊
317␉␊
318␉if ((string->numentries+1) < MAX_STRING_NUM_ENTRIES)␊
319 {␊
320␉␉string->numentries++;␊
321␉␉␊
322 }␊
323 else␊
324 {␊
325 free(string->entries);␊
326 free(device);␊
327 return NULL;␊
328 }␉␊
329 ␊
330␉string_entries_arrey[string->numentries-1] = device;␊
331␉␊
332␉return device;␊
333}␊
334#endif␊
335␊
336int devprop_add_value(struct DevPropDevice *device, char *nm, uint8_t *vl, uint32_t len)␊
337{␊
338␉␊
339␉if(!nm || !vl || !len)␊
340␉{␊
341␉␉return 0;␊
342␉}␊
343␉␊
344␉uint32_t length = ((strlen(nm) * 2) + len + (2 * sizeof(uint32_t)) + 2);␊
345␉uint8_t *data = (uint8_t*)malloc(length);␊
346␉{␊
347␉␉if(!data)␊
348␉␉{␊
349␉␉␉return 0;␊
350␉␉}␊
351␉␉␊
352␉␉memset(data, 0, length);␊
353␉␉uint32_t off= 0;␊
354␉␉data[off+1] = ((strlen(nm) * 2) + 6) >> 8;␊
355␉␉data[off] = ((strlen(nm) * 2) + 6) & 0x00FF;␊
356␉␉␊
357␉␉off += 4;␊
358␉␉uint32_t i=0, l = strlen(nm);␊
359␉␉for(i = 0 ; i < l ; i++, off += 2)␊
360␉␉{␊
361␉␉␉data[off] = *nm++;␊
362␉␉}␊
363␉␉␊
364␉␉off += 2;␊
365␉␉l = len;␊
366␉␉uint32_t *datalength = (uint32_t*)&data[off];␊
367␉␉*datalength = l + 4;␊
368␉␉off += 4;␊
369␉␉for(i = 0 ; i < l ; i++, off++)␊
370␉␉{␊
371␉␉␉data[off] = *vl++;␊
372␉␉}␊
373␉}␉␊
374␉␊
375␉uint32_t offset = device->length - (24 + (6 * device->num_pci_devpaths));␊
376␉␊
377␉uint8_t *newdata = (uint8_t*)malloc((length + offset));␊
378␉if(!newdata)␊
379␉{␊
380␉␉return 0;␊
381␉}␊
382␉if(device->data)␊
383␉{␊
384␉␉if(offset > 1)␊
385␉␉{␊
386␉␉␉memcpy(newdata, device->data, offset);␊
387␉␉}␊
388␉}␊
389␉␊
390␉memcpy(newdata + offset, data, length);␊
391␉␊
392␉device->length += length;␊
393␉device->string->length += length;␊
394␉device->numentries++;␊
395␉␊
396␉if(!device->data)␊
397␉{␊
398␉␉//device->data = (uint8_t*)malloc(sizeof(uint8_t)); //IMHO this is useless␊
399␉}␊
400␉else␊
401␉{␊
402␉␉free(device->data);␊
403␉}␊
404␉␊
405␉free(data);␊
406␉device->data = newdata;␊
407␉␊
408␉return 1;␊
409}␊
410␊
411char *devprop_generate_string(struct DevPropString *string)␊
412{␊
413␉char *buffer = (char*)malloc(string->length * 2);␊
414␉char *ptr = buffer;␊
415␉␊
416␉if(!buffer)␊
417␉{␊
418␉␉return NULL;␊
419␉}␊
420␉␊
421␉sprintf(buffer, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊
422␉␉␉dp_swap16(string->numentries), string->WHAT3);␊
423␉buffer += 24;␊
424␉int i = 0, x = 0;␊
425␉␊
426␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊
427␊
428␉while(i < string->numentries)␊
429␉{␊
430␉␉if (!(i<MAX_STRING_NUM_ENTRIES)) ␊
431 {␊
432 continue;␊
433 }␊
434␉␉sprintf(buffer, "%08x%04x%04x", dp_swap32(string_entries_arrey[i]->length),␊
435␉␉␉␉dp_swap16(string_entries_arrey[i]->numentries), string_entries_arrey[i]->WHAT2);␊
436␉␉␊
437␉␉buffer += 16;␊
438␉␉sprintf(buffer, "%02x%02x%04x%08x%08x", string_entries_arrey[i]->acpi_dev_path.type,␊
439␉␉␉␉string_entries_arrey[i]->acpi_dev_path.subtype,␊
440␉␉␉␉dp_swap16(string_entries_arrey[i]->acpi_dev_path.length),␊
441␉␉␉␉string_entries_arrey[i]->acpi_dev_path._HID,␊
442␉␉␉␉dp_swap32(string_entries_arrey[i]->acpi_dev_path._UID));␊
443␉␉␊
444␉␉buffer += 24;␊
445␉␉for(x=0;x < string_entries_arrey[i]->num_pci_devpaths; x++)␊
446␉␉{␊
447␉␉␉sprintf(buffer, "%02x%02x%04x%02x%02x", string_entries_arrey[i]->pci_dev_path[x].type,␊
448␉␉␉␉␉string_entries_arrey[i]->pci_dev_path[x].subtype,␊
449␉␉␉␉␉dp_swap16(string_entries_arrey[i]->pci_dev_path[x].length),␊
450␉␉␉␉␉string_entries_arrey[i]->pci_dev_path[x].function,␊
451␉␉␉␉␉string_entries_arrey[i]->pci_dev_path[x].device);␊
452␉␉␉buffer += 12;␊
453␉␉}␊
454␉␉␊
455␉␉sprintf(buffer, "%02x%02x%04x", string_entries_arrey[i]->path_end.type,␊
456␉␉␉␉string_entries_arrey[i]->path_end.subtype,␊
457␉␉␉␉dp_swap16(string_entries_arrey[i]->path_end.length));␊
458␉␉␊
459␉␉buffer += 8;␊
460␉␉uint8_t *dataptr = string_entries_arrey[i]->data;␊
461␉␉for(x = 0; (uint32_t)x < (string_entries_arrey[i]->length) - (24 + (6 * string_entries_arrey[i]->num_pci_devpaths)) ; x++)␊
462␉␉{␊
463␉␉␉sprintf(buffer, "%02x", *dataptr++);␊
464␉␉␉buffer += 2;␊
465␉␉}␊
466␉␉i++;␊
467␉}␊
468␉return ptr;␊
469}␊
470␊
471void devprop_free_string(struct DevPropString *string)␊
472{␊
473␉if(!string)␊
474␉{␊
475␉␉return;␊
476␉}␊
477␉␊
478␉int i;␊
479␉␊
480␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊
481␉␊
482␉for(i = 0; i < string->numentries; i++)␊
483␉{␊
484 if (!(i<MAX_STRING_NUM_ENTRIES)) ␊
485 {␊
486 continue;␊
487 }␊
488␉␉if(string_entries_arrey[i])␊
489␉␉{␊
490␉␉␉if(string_entries_arrey[i]->data)␊
491␉␉␉{␊
492␉␉␉␉free(string_entries_arrey[i]->data);␊
493␉␉␉␉string_entries_arrey[i]->data = NULL;␊
494␉␉␉}␊
495␉␉}␊
496␉}␊
497 ␊
498 free(string->entries);␊
499 string->entries = NULL;␊
500 ␊
501␉free(string);␊
502␉string = NULL;␊
503}

Archive Download this file

Revision: 2006