Chameleon

Chameleon Svn Source Tree

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

Source at commit 553 created 13 years 6 months ago.
By meklort, device_number is used in multiple files, set it in the header file
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: 553