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

Archive Download this file

Revision: 2662