Chameleon

Chameleon Svn Source Tree

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

Source at commit 1146 created 12 years 10 months ago.
By azimutz, Sync with trunk (r1145). Add nVidia dev id's, 0DF4 for "GeForce GT 450M" (issue 99) and 1251 for "GeForce GTX 560M" (thanks to oSxFr33k for testing).
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;␊
27struct DevPropString *string = 0;␊
28uint8_t *stringdata = 0;␊
29uint32_t stringlength = 0;␊
30␊
31char *efi_inject_get_devprop_string(uint32_t *len)␊
32{␊
33␉if(string)␊
34␉{␊
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->bootConfig) && 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) DT__AddProperty(node, DEVICE_PROPERTIES_PROP, cnt2, binStr);␊
68␉}␊
69}␊
70␊
71struct DevPropString *devprop_create_string(void)␊
72{␊
73␉string = (struct DevPropString*)malloc(sizeof(struct DevPropString));␊
74␉␊
75␉if(string == NULL)␊
76␉{␊
77␉␉return NULL;␊
78␉}␊
79␉␊
80␉memset(string, 0, sizeof(struct DevPropString));␊
81␉string->length = 12;␊
82␉string->WHAT2 = 0x01000000;␊
83␉return string;␊
84}␊
85␊
86struct DevPropDevice *devprop_add_device(struct DevPropString *string, char *path)␊
87{␊
88␉struct DevPropDevice␉*device;␊
89␉const char␉␉pciroot_string[] = "PciRoot(0x";␊
90␉const char␉␉pci_device_string[] = "Pci(0x";␊
91␉␊
92␉if (string == NULL || path == NULL) {␊
93␉␉return NULL;␊
94␉}␊
95␉device = malloc(sizeof(struct DevPropDevice));␊
96␉␊
97␉if (strncmp(path, pciroot_string, strlen(pciroot_string))) {␊
98␉␉printf("ERROR parsing device path\n");␊
99␉␉return NULL;␊
100␉}␊
101␉␊
102␉memset(device, 0, sizeof(struct DevPropDevice));␊
103␉device->acpi_dev_path._UID = getPciRootUID();␊
104␉␊
105␉int numpaths = 0;␊
106␉int␉␉x, curr = 0;␊
107␉char␉buff[] = "00";␊
108␉␊
109␉for (x = 0; x < strlen(path); x++) ␊
110␉{␊
111␉␉if (!strncmp(&path[x], pci_device_string, strlen(pci_device_string)))␊
112␉␉{␊
113␉␉␉x+=strlen(pci_device_string);␊
114␉␉␉curr=x;␊
115␉␉␉while(path[++x] != ',');␊
116␉␉␉if(x-curr == 2)␊
117␉␉␉{␊
118␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
119␉␉␉}␊
120␉␉␉else if(x-curr == 1)␊
121␉␉␉{␊
122␉␉␉␉sprintf(buff, "%c", path[curr]);␊
123␉␉␉}␊
124␉␉␉else ␊
125␉␉␉{␊
126␉␉␉␉printf("ERROR parsing device path\n");␊
127␉␉␉␉numpaths = 0;␊
128␉␉␉␉break;␊
129␉␉␉}␊
130␉␉␉device->pci_dev_path[numpaths].device =␉ascii_hex_to_int(buff);␊
131␉␉␉␊
132␉␉␉x += 3; // 0x␊
133␉␉␉curr = x;␊
134␉␉␉while(path[++x] != ')');␊
135␉␉␉if(x-curr == 2)␊
136␉␉␉{␊
137␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
138␉␉␉}␊
139␉␉␉else if(x-curr == 1)␊
140␉␉␉{␊
141␉␉␉␉sprintf(buff, "%c", path[curr]);␊
142␉␉␉}␊
143␉␉␉else␊
144␉␉␉{␊
145␉␉␉␉printf("ERROR parsing device path\n");␊
146␉␉␉␉numpaths = 0;␊
147␉␉␉␉break;␊
148␉␉␉}␊
149␉␉␉device->pci_dev_path[numpaths].function = ascii_hex_to_int(buff); // TODO: find dev from char *path␊
150␉␉␉␊
151␉␉␉numpaths++;␊
152␉␉}␊
153␉}␊
154␉␊
155␉if(!numpaths)␊
156␉{␊
157␉␉return NULL;␊
158␉}␊
159␉␊
160␉device->numentries = 0x00;␊
161␉␊
162␉device->acpi_dev_path.length = 0x0c;␊
163␉device->acpi_dev_path.type = 0x02;␊
164␉device->acpi_dev_path.subtype = 0x01;␊
165␉device->acpi_dev_path._HID = 0xd041030a;␊
166␉␊
167␉device->num_pci_devpaths = numpaths;␊
168␉device->length = 24 + (6*numpaths);␊
169␉␊
170␉int␉␉i; ␊
171␉␊
172␉for(i = 0; i < numpaths; i++)␊
173␉{␊
174␉␉device->pci_dev_path[i].length = 0x06;␊
175␉␉device->pci_dev_path[i].type = 0x01;␊
176␉␉device->pci_dev_path[i].subtype = 0x01;␊
177␉}␊
178␉␊
179␉device->path_end.length = 0x04;␊
180␉device->path_end.type = 0x7f;␊
181␉device->path_end.subtype = 0xff;␊
182␉␊
183␉device->string = string;␊
184␉device->data = NULL;␊
185␉string->length += device->length;␊
186␉␊
187␉if(!string->entries)␊
188␉{␊
189␉␉if((string->entries = (struct DevPropDevice**)malloc(sizeof(device)))== NULL)␊
190␉␉{␊
191␉␉␉return 0;␊
192␉␉}␊
193␉}␊
194␉␊
195␉string->entries[string->numentries++] = (struct DevPropDevice*)malloc(sizeof(device));␊
196␉string->entries[string->numentries-1] = 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␉{␊
206␉␉return 0;␊
207␉}␊
208␉␊
209␉uint32_t length = ((strlen(nm) * 2) + len + (2 * sizeof(uint32_t)) + 2);␊
210␉uint8_t *data = (uint8_t*)malloc(length);␊
211␉{␊
212␉␉if(!data)␊
213␉␉{␊
214␉␉␉return 0;␊
215␉␉}␊
216␉␉␊
217␉␉memset(data, 0, length);␊
218␉␉uint32_t off= 0;␊
219␉␉data[off+1] = ((strlen(nm) * 2) + 6) >> 8;␊
220␉␉data[off] = ((strlen(nm) * 2) + 6) & 0x00FF;␊
221␉␉␊
222␉␉off += 4;␊
223␉␉uint32_t i=0, l = strlen(nm);␊
224␉␉for(i = 0 ; i < l ; i++, off += 2)␊
225␉␉{␊
226␉␉␉data[off] = *nm++;␊
227␉␉}␊
228␉␉␊
229␉␉off += 2;␊
230␉␉l = len;␊
231␉␉uint32_t *datalength = (uint32_t*)&data[off];␊
232␉␉*datalength = l + 4;␊
233␉␉off += 4;␊
234␉␉for(i = 0 ; i < l ; i++, off++)␊
235␉␉{␊
236␉␉␉data[off] = *vl++;␊
237␉␉}␊
238␉}␉␊
239␉␊
240␉uint32_t offset = device->length - (24 + (6 * device->num_pci_devpaths));␊
241␉␊
242␉uint8_t *newdata = (uint8_t*)malloc((length + offset));␊
243␉if(!newdata)␊
244␉{␊
245␉␉return 0;␊
246␉}␊
247␉if(device->data)␊
248␉{␊
249␉␉if(offset > 1)␊
250␉␉{␊
251␉␉␉memcpy(newdata, device->data, offset);␊
252␉␉}␊
253␉}␊
254␉␊
255␉memcpy(newdata + offset, data, length);␊
256␉␊
257␉device->length += length;␊
258␉device->string->length += length;␊
259␉device->numentries++;␊
260␉␊
261␉if(!device->data)␊
262␉{␊
263␉␉device->data = (uint8_t*)malloc(sizeof(uint8_t));␊
264␉}␊
265␉else␊
266␉{␊
267␉␉free(device->data);␊
268␉}␊
269␉␊
270␉free(data);␊
271␉device->data = newdata;␊
272␉␊
273␉return 1;␊
274}␊
275␊
276char *devprop_generate_string(struct DevPropString *string)␊
277{␊
278␉char *buffer = (char*)malloc(string->length * 2);␊
279␉char *ptr = buffer;␊
280␉␊
281␉if(!buffer)␊
282␉{␊
283␉␉return NULL;␊
284␉}␊
285␉␊
286␉sprintf(buffer, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊
287␉␉␉dp_swap16(string->numentries), string->WHAT3);␊
288␉buffer += 24;␊
289␉int i = 0, x = 0;␊
290␉␊
291␉while(i < string->numentries)␊
292␉{␊
293␉␉sprintf(buffer, "%08x%04x%04x", dp_swap32(string->entries[i]->length),␊
294␉␉␉␉dp_swap16(string->entries[i]->numentries), string->entries[i]->WHAT2);␊
295␉␉␊
296␉␉buffer += 16;␊
297␉␉sprintf(buffer, "%02x%02x%04x%08x%08x", string->entries[i]->acpi_dev_path.type,␊
298␉␉␉␉string->entries[i]->acpi_dev_path.subtype,␊
299␉␉␉␉dp_swap16(string->entries[i]->acpi_dev_path.length),␊
300␉␉␉␉string->entries[i]->acpi_dev_path._HID,␊
301␉␉␉␉dp_swap32(string->entries[i]->acpi_dev_path._UID));␊
302␉␉␊
303␉␉buffer += 24;␊
304␉␉for(x=0;x < string->entries[i]->num_pci_devpaths; x++)␊
305␉␉{␊
306␉␉␉sprintf(buffer, "%02x%02x%04x%02x%02x", string->entries[i]->pci_dev_path[x].type,␊
307␉␉␉␉␉string->entries[i]->pci_dev_path[x].subtype,␊
308␉␉␉␉␉dp_swap16(string->entries[i]->pci_dev_path[x].length),␊
309␉␉␉␉␉string->entries[i]->pci_dev_path[x].function,␊
310␉␉␉␉␉string->entries[i]->pci_dev_path[x].device);␊
311␉␉␉buffer += 12;␊
312␉␉}␊
313␉␉␊
314␉␉sprintf(buffer, "%02x%02x%04x", string->entries[i]->path_end.type,␊
315␉␉␉␉string->entries[i]->path_end.subtype,␊
316␉␉␉␉dp_swap16(string->entries[i]->path_end.length));␊
317␉␉␊
318␉␉buffer += 8;␊
319␉␉uint8_t *dataptr = string->entries[i]->data;␊
320␉␉for(x = 0; x < (string->entries[i]->length) - (24 + (6 * string->entries[i]->num_pci_devpaths)) ; x++)␊
321␉␉{␊
322␉␉␉sprintf(buffer, "%02x", *dataptr++);␊
323␉␉␉buffer += 2;␊
324␉␉}␊
325␉␉i++;␊
326␉}␊
327␉return ptr;␊
328}␊
329␊
330void devprop_free_string(struct DevPropString *string)␊
331{␊
332␉if(!string)␊
333␉{␊
334␉␉return;␊
335␉}␊
336␉␊
337␉int i;␊
338␉for(i = 0; i < string->numentries; i++)␊
339␉{␊
340␉␉if(string->entries[i])␊
341␉␉{␊
342␉␉␉if(string->entries[i]->data)␊
343␉␉␉{␊
344␉␉␉␉free(string->entries[i]->data);␊
345␉␉␉␉string->entries[i]->data = NULL;␊
346␉␉␉}␊
347␉␉␉free(string->entries[i]);␊
348␉␉␉string->entries[i] = NULL;␊
349␉␉}␊
350␉}␊
351␉␊
352␉free(string);␊
353␉string = NULL;␊
354}

Archive Download this file

Revision: 1146