Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 2679