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

Archive Download this file

Revision: 2697