Chameleon

Chameleon Svn Source Tree

Root/branches/valv/i386/libsaio/device_inject.c

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

Archive Download this file

Revision: 665