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

Archive Download this file

Revision: 2057