Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 141