Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 2051