Chameleon

Chameleon Svn Source Tree

Root/branches/JrCs/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 "bootstruct.h"␊
10#include "pci.h"␊
11#include "device_inject.h"␊
12#include "convert.h"␊
13␊
14#ifndef DEBUG_INJECT␊
15#define DEBUG_INJECT 0␊
16#endif␊
17␊
18#if DEBUG_INJECT␊
19#define DBG(x...)␉printf(x)␊
20#else␊
21#define DBG(x...)␊
22#endif␊
23␊
24uint32_t devices_number = 1;␊
25uint32_t builtin_set = 0;␊
26struct DevPropString *string = 0;␊
27uint8_t *stringdata = 0;␊
28uint32_t stringlength = 0;␊
29␊
30char *efi_inject_get_devprop_string(uint32_t *len)␊
31{␊
32␉if(string)␊
33␉{␊
34␉␉*len = string->length;␊
35␉␉return devprop_generate_string(string);␊
36␉}␊
37␉printf("efi_inject_get_devprop_string NULL trying stringdata\n");␊
38␉return NULL;␊
39}␊
40␊
41void setupDeviceProperties(Node *node)␊
42{␊
43 const char *val;␊
44 uint8_t *binStr;␊
45 int cnt, cnt2;␊
46␊
47 static char DEVICE_PROPERTIES_PROP[] = "device-properties";␊
48␊
49 /* Generate devprop string.␊
50 */␊
51 uint32_t strlength;␊
52 char *string = efi_inject_get_devprop_string(&strlength);␊
53␊
54 /* Use the static "device-properties" boot config key contents if available,␊
55 * otheriwse use the generated one.␊
56 */ ␊
57 if (!getValueForKey(DEVICE_PROPERTIES_PROP, &val, &cnt, &bootInfo->bootConfig) && string)␊
58 {␊
59 val = (const char*)string;␊
60 cnt = strlength * 2;␊
61 } ␊
62 ␊
63 if (cnt > 1)␊
64 {␊
65 binStr = convertHexStr2Binary(val, &cnt2);␊
66 if (cnt2 > 0) DT__AddProperty(node, DEVICE_PROPERTIES_PROP, cnt2, binStr);␊
67 }␊
68}␊
69␊
70struct DevPropString *devprop_create_string(void)␊
71{␊
72␉string = (struct DevPropString*)malloc(sizeof(struct DevPropString));␊
73␉␊
74␉if(string == NULL)␊
75␉␉return NULL;␊
76␉␊
77␉memset(string, 0, sizeof(struct DevPropString));␊
78␉string->length = 12;␊
79␉string->WHAT2 = 0x01000000;␊
80␉return string;␊
81}␊
82 ␊
83struct DevPropDevice *devprop_add_device(struct DevPropString *string, char *path)␊
84{␊
85␉uint32_t PciRootID = 0;␊
86␉const char *val;␊
87␉int len;␊
88␊
89␉struct DevPropDevice *device = (struct DevPropDevice*)malloc(sizeof(struct DevPropDevice));␊
90␉if(!device || !string || !path) {␊
91␉␉if(device)␊
92␉␉␉free(device);␊
93␉␉return NULL;␊
94␉}␊
95␊
96␉const char pciroot_string[]␉␉= "PciRoot(0x";␊
97␉const char pci_device_string[]␉= "Pci(0x";␊
98␊
99␉if (getValueForKey("PciRoot", &val, &len, &bootInfo->bootConfig))␊
100␉ PciRootID = atoi(val);␊
101␉␊
102␉if(strncmp(path, pciroot_string, strlen(pciroot_string)))␊
103␉{␊
104␉␉printf("ERROR parsing device path\n");␊
105␉␉return NULL;␊
106␉}␊
107␉␊
108␉memset(device, 0, sizeof(struct DevPropDevice));␊
109␉␊
110␉device->acpi_dev_path._UID = PciRootID;␊
111␉␊
112␉int numpaths = 0;␊
113␉int␉␉x, curr = 0;␊
114␉char␉buff[] = "00";␊
115␊
116␉for(x = 0; x < strlen(path); x++)␊
117␉{␊
118␉␉if(!strncmp(&path[x], pci_device_string, strlen(pci_device_string)))␊
119␉␉{␊
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 = strtoul(buff, NULL, 16);␊
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 = strtoul(buff, NULL, 16);␊
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␉return device;␊
192}␊
193␊
194int devprop_add_value(struct DevPropDevice *device, char *nm, uint8_t *vl, uint32_t len)␊
195{␊
196␉␊
197␉if(!nm || !vl || !len)␊
198␉␉return 0;␊
199␉␊
200␉uint32_t length = ((strlen(nm) * 2) + len + (2 * sizeof(uint32_t)) + 2);␊
201␉uint8_t *data = (uint8_t*)malloc(length);␊
202␉{␊
203␉␉if(!data)␊
204␉␉␉return 0;␊
205␉␉␊
206␉␉memset(data, 0, length);␊
207␉␉uint32_t off= 0;␊
208␉␉data[off+1] = ((strlen(nm) * 2) + 6) >> 8;␊
209␉␉data[off] = ((strlen(nm) * 2) + 6) & 0x00FF;␊
210␉␉␊
211␉␉off += 4;␊
212␉␉uint32_t i=0, l = strlen(nm);␊
213␉␉for(i = 0 ; i < l ; i++, off += 2)␊
214␉␉{␊
215␉␉␉data[off] = *nm++;␊
216␉␉}␊
217␉␉␊
218␉␉off += 2;␊
219␉␉l = len;␊
220␉␉uint32_t *datalength = (uint32_t*)&data[off];␊
221␉␉*datalength = l + 4;␊
222␉␉off += 4;␊
223␉␉for(i = 0 ; i < l ; i++, off++)␊
224␉␉{␊
225␉␉␉data[off] = *vl++;␊
226␉␉}␊
227␉}␉␊
228␉␊
229␉uint32_t offset = device->length - (24 + (6 * device->num_pci_devpaths));␊
230␉␊
231␉uint8_t *newdata = (uint8_t*)malloc((length + offset));␊
232␉if(!newdata)␊
233␉␉return 0;␊
234␉if(device->data)␊
235␉␉if(offset > 1)␊
236␉␉␉memcpy(newdata, device->data, offset);␊
237␊
238␉memcpy(newdata + offset, data, length);␊
239␉␊
240␉device->length += length;␊
241␉device->string->length += length;␊
242␉device->numentries++;␊
243␉␊
244␉if(!device->data)␊
245␉␉device->data = (uint8_t*)malloc(sizeof(uint8_t));␊
246␉else␊
247␉␉free(device->data);␊
248␉␊
249␉free(data);␊
250␉device->data = newdata;␊
251␉␊
252␉return 1;␊
253}␊
254␊
255char *devprop_generate_string(struct DevPropString *string)␊
256{␊
257␉char *buffer = (char*)malloc(string->length * 2);␊
258␉char *ptr = buffer;␊
259␉␊
260␉if(!buffer)␊
261␉␉return NULL;␊
262␊
263␉sprintf(buffer, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊
264␉␉␉dp_swap16(string->numentries), string->WHAT3);␊
265␉buffer += 24;␊
266␉int i = 0, x = 0;␊
267␉␊
268␉while(i < string->numentries)␊
269␉{␊
270␉␉sprintf(buffer, "%08x%04x%04x", dp_swap32(string->entries[i]->length),␊
271␉␉␉␉dp_swap16(string->entries[i]->numentries), string->entries[i]->WHAT2);␊
272␉␉␊
273␉␉buffer += 16;␊
274␉␉sprintf(buffer, "%02x%02x%04x%08x%08x", string->entries[i]->acpi_dev_path.type,␊
275␉␉␉␉string->entries[i]->acpi_dev_path.subtype,␊
276␉␉␉␉dp_swap16(string->entries[i]->acpi_dev_path.length),␊
277␉␉␉␉string->entries[i]->acpi_dev_path._HID,␊
278␉␉␉␉dp_swap32(string->entries[i]->acpi_dev_path._UID));␊
279␊
280␉␉buffer += 24;␊
281␉␉for(x=0;x < string->entries[i]->num_pci_devpaths; x++)␊
282␉␉{␊
283␉␉␉sprintf(buffer, "%02x%02x%04x%02x%02x", string->entries[i]->pci_dev_path[x].type,␊
284␉␉␉␉␉string->entries[i]->pci_dev_path[x].subtype,␊
285␉␉␉␉␉dp_swap16(string->entries[i]->pci_dev_path[x].length),␊
286␉␉␉␉␉string->entries[i]->pci_dev_path[x].function,␊
287␉␉␉␉␉string->entries[i]->pci_dev_path[x].device);␊
288␉␉␉buffer += 12;␊
289␉␉}␊
290␉␉␊
291␉␉sprintf(buffer, "%02x%02x%04x", string->entries[i]->path_end.type,␊
292␉␉␉␉string->entries[i]->path_end.subtype,␊
293␉␉␉␉dp_swap16(string->entries[i]->path_end.length));␊
294␉␉␊
295␉␉buffer += 8;␊
296␉␉uint8_t *dataptr = string->entries[i]->data;␊
297␉␉for(x = 0; x < (string->entries[i]->length) - (24 + (6 * string->entries[i]->num_pci_devpaths)) ; x++)␊
298␉␉{␊
299␉␉␉sprintf(buffer, "%02x", *dataptr++);␊
300␉␉␉buffer += 2;␊
301␉␉}␊
302␉␉i++;␊
303␉}␊
304␉return ptr;␊
305}␊
306␊
307void devprop_free_string(struct DevPropString *string)␊
308{␊
309␉if(!string)␊
310␉␉return;␊
311␉␊
312␉int i;␊
313␉for(i = 0; i < string->numentries; i++)␊
314␉{␊
315␉␉if(string->entries[i])␊
316␉␉{␊
317␉␉␉if(string->entries[i]->data)␊
318␉␉␉{␊
319␉␉␉␉free(string->entries[i]->data);␊
320␉␉␉␉string->entries[i]->data = NULL;␊
321␉␉␉}␊
322␉␉␉free(string->entries[i]);␊
323␉␉␉string->entries[i] = NULL;␊
324␉␉}␊
325␉}␊
326␉␊
327␉free(string);␊
328␉string = NULL;␊
329}␊
330␊
331/* a fine place for this code */␊
332␊
333int devprop_add_network_template(struct DevPropDevice *device, uint16_t vendor_id)␊
334{␊
335␉if(!device)␊
336␉␉return 0;␊
337␉uint8_t builtin = 0x0;␊
338␉if((vendor_id != 0x168c) && (builtin_set == 0)) ␊
339␉{␊
340␉␉builtin_set = 1;␊
341␉␉builtin = 0x01;␊
342␉}␊
343␉if(!devprop_add_value(device, "built-in", (uint8_t*)&builtin, 1))␊
344␉␉return 0;␊
345␉devices_number++;␊
346␉return 1;␊
347}␊
348␊
349void set_eth_builtin(pci_dt_t *eth_dev)␊
350{␊
351␉char *devicepath = get_pci_dev_path(eth_dev);␊
352␉struct DevPropDevice *device = (struct DevPropDevice*)malloc(sizeof(struct DevPropDevice));␊
353␊
354␉verbose("LAN Controller [%04x:%04x] :: %s\n", eth_dev->vendor_id, eth_dev->device_id, devicepath);␊
355␊
356␉if (!string)␊
357␉␉string = devprop_create_string();␊
358␊
359␉device = devprop_add_device(string, devicepath);␊
360␉if(device)␊
361␉{␊
362␉␉verbose("Setting up lan keys\n");␊
363␉␉devprop_add_network_template(device, eth_dev->vendor_id);␊
364␉␉stringdata = (uint8_t*)malloc(sizeof(uint8_t) * string->length);␊
365␉␉if(stringdata)␊
366␉␉{␊
367␉␉␉memcpy(stringdata, (uint8_t*)devprop_generate_string(string), string->length);␊
368␉␉␉stringlength = string->length;␊
369␉␉}␊
370␉}␊
371}␊
372

Archive Download this file

Revision: 126