Chameleon

Chameleon Svn Source Tree

Root/branches/cparm/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 "pci_root.h"␊
12#include "device_inject.h"␊
13#include "convert.h"␊
14␊
15#ifndef DEBUG_INJECT␊
16#define DEBUG_INJECT 0␊
17#endif␊
18␊
19#if DEBUG_INJECT␊
20#define DBG(x...)␉printf(x)␊
21#else␊
22#define DBG(x...)␊
23#endif␊
24␊
25uint32_t devices_number = 1;␊
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␉verbose("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(kDeviceProperties, &val, &cnt, DEFAULT_BOOT_CONFIG) && 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␉{␊
76␉␉return NULL;␊
77␉}␊
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␉if (!device) {␊
96 return NULL;␊
97 }␊
98␉if (strncmp(path, pciroot_string, strlen(pciroot_string))) {␊
99 free(device);␊
100␉␉printf("ERROR parsing device path\n");␊
101␉␉return NULL;␊
102␉}␊
103␉␊
104␉memset(device, 0, sizeof(struct DevPropDevice));␊
105␉device->acpi_dev_path._UID = getPciRootUID();␊
106␉␊
107␉int numpaths = 0;␊
108␉int␉␉x, curr = 0;␊
109␉char␉buff[] = "00";␊
110␉␊
111␉for (x = 0; x < strlen(path); x++) ␊
112␉{␊
113␉␉if (!strncmp(&path[x], pci_device_string, strlen(pci_device_string)))␊
114␉␉{␊
115␉␉␉x+=strlen(pci_device_string);␊
116␉␉␉curr=x;␊
117␉␉␉while(path[++x] != ',');␊
118␉␉␉if(x-curr == 2)␊
119␉␉␉{␊
120␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
121␉␉␉}␊
122␉␉␉else if(x-curr == 1)␊
123␉␉␉{␊
124␉␉␉␉sprintf(buff, "%c", path[curr]);␊
125␉␉␉}␊
126␉␉␉else ␊
127␉␉␉{␊
128␉␉␉␉printf("ERROR parsing device path\n");␊
129␉␉␉␉numpaths = 0;␊
130␉␉␉␉break;␊
131␉␉␉}␊
132␉␉␉device->pci_dev_path[numpaths].device =␉(uint8_t)strtoul(buff, NULL, 16);␊
133␉␉␉␊
134␉␉␉x += 3; // 0x␊
135␉␉␉curr = x;␊
136␉␉␉while(path[++x] != ')');␊
137␉␉␉if(x-curr == 2)␊
138␉␉␉{␊
139␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
140␉␉␉}␊
141␉␉␉else if(x-curr == 1)␊
142␉␉␉{␊
143␉␉␉␉sprintf(buff, "%c", path[curr]);␊
144␉␉␉}␊
145␉␉␉else␊
146␉␉␉{␊
147␉␉␉␉printf("ERROR parsing device path\n");␊
148␉␉␉␉numpaths = 0;␊
149␉␉␉␉break;␊
150␉␉␉}␊
151␉␉␉device->pci_dev_path[numpaths].function = (uint8_t)strtoul(buff, NULL, 16); // TODO: find dev from char *path␊
152␉␉␉␊
153␉␉␉numpaths++;␊
154␉␉}␊
155␉}␊
156␉␊
157␉if(!numpaths)␊
158␉{␊
159 free(device);␊
160␉␉return NULL;␊
161␉}␊
162␉␊
163␉device->numentries = 0x00;␊
164␉␊
165␉device->acpi_dev_path.length = 0x0c;␊
166␉device->acpi_dev_path.type = 0x02;␊
167␉device->acpi_dev_path.subtype = 0x01;␊
168␉device->acpi_dev_path._HID = 0xd041030a;␊
169␉␊
170␉device->num_pci_devpaths = numpaths;␊
171␉device->length = 24 + (6*numpaths);␊
172␉␊
173␉int␉␉i; ␊
174␉␊
175␉for(i = 0; i < numpaths; i++)␊
176␉{␊
177␉␉device->pci_dev_path[i].length = 0x06;␊
178␉␉device->pci_dev_path[i].type = 0x01;␊
179␉␉device->pci_dev_path[i].subtype = 0x01;␊
180␉}␊
181␉␊
182␉device->path_end.length = 0x04;␊
183␉device->path_end.type = 0x7f;␊
184␉device->path_end.subtype = 0xff;␊
185␉␊
186␉device->string = string;␊
187␉device->data = NULL;␊
188␉string->length += device->length;␊
189␉␊
190␉if(!string->entries)␊
191␉{␊
192␉␉if((string->entries = (struct DevPropDevice*)malloc(sizeof(struct DevPropDevice) * MAX_STRING_NUM_ENTRIES))== NULL)␊
193␉␉{␊
194 free(device);␊
195␉␉␉return NULL;␊
196␉␉}␊
197␉}␊
198␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊
199␉␊
200␉string->numentries++;␊
201 ␊
202␉string_entries_arrey[string->numentries-1] = device;␊
203␉␊
204␉return device;␊
205}␊
206␊
207int devprop_add_value(struct DevPropDevice *device, char *nm, uint8_t *vl, uint32_t len)␊
208{␊
209␉␊
210␉if(!nm || !vl || !len)␊
211␉{␊
212␉␉return 0;␊
213␉}␊
214␉␊
215␉uint32_t length = ((strlen(nm) * 2) + len + (2 * sizeof(uint32_t)) + 2);␊
216␉uint8_t *data = (uint8_t*)malloc(length);␊
217␉{␊
218␉␉if(!data)␊
219␉␉{␊
220␉␉␉return 0;␊
221␉␉}␊
222␉␉␊
223␉␉memset(data, 0, length);␊
224␉␉uint32_t off= 0;␊
225␉␉data[off+1] = ((strlen(nm) * 2) + 6) >> 8;␊
226␉␉data[off] = ((strlen(nm) * 2) + 6) & 0x00FF;␊
227␉␉␊
228␉␉off += 4;␊
229␉␉uint32_t i=0, l = strlen(nm);␊
230␉␉for(i = 0 ; i < l ; i++, off += 2)␊
231␉␉{␊
232␉␉␉data[off] = *nm++;␊
233␉␉}␊
234␉␉␊
235␉␉off += 2;␊
236␉␉l = len;␊
237␉␉uint32_t *datalength = (uint32_t*)&data[off];␊
238␉␉*datalength = l + 4;␊
239␉␉off += 4;␊
240␉␉for(i = 0 ; i < l ; i++, off++)␊
241␉␉{␊
242␉␉␉data[off] = *vl++;␊
243␉␉}␊
244␉}␉␊
245␉␊
246␉uint32_t offset = device->length - (24 + (6 * device->num_pci_devpaths));␊
247␉␊
248␉uint8_t *newdata = (uint8_t*)malloc((length + offset));␊
249␉if(!newdata)␊
250␉{␊
251␉␉return 0;␊
252␉}␊
253␉if(device->data)␊
254␉{␊
255␉␉if(offset > 1)␊
256␉␉{␊
257␉␉␉memcpy(newdata, device->data, offset);␊
258␉␉}␊
259␉}␊
260␉␊
261␉memcpy(newdata + offset, data, length);␊
262␉␊
263␉device->length += length;␊
264␉device->string->length += length;␊
265␉device->numentries++;␊
266␉␊
267␉if(!device->data)␊
268␉{␊
269␉␉//device->data = (uint8_t*)malloc(sizeof(uint8_t)); //IMHO this is useless␊
270␉}␊
271␉else␊
272␉{␊
273␉␉free(device->data);␊
274␉}␊
275␉␊
276␉free(data);␊
277␉device->data = newdata;␊
278␉␊
279␉return 1;␊
280}␊
281␊
282char *devprop_generate_string(struct DevPropString *string)␊
283{␊
284␉char *buffer = (char*)malloc(string->length * 2);␊
285␉char *ptr = buffer;␊
286␉␊
287␉if(!buffer)␊
288␉{␊
289␉␉return NULL;␊
290␉}␊
291␉␊
292␉sprintf(buffer, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊
293␉␉␉dp_swap16(string->numentries), string->WHAT3);␊
294␉buffer += 24;␊
295␉int i = 0, x = 0;␊
296␉␊
297␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊
298␊
299␉while(i < string->numentries)␊
300␉{␊
301␉␉sprintf(buffer, "%08x%04x%04x", dp_swap32(string_entries_arrey[i]->length),␊
302␉␉␉␉dp_swap16(string_entries_arrey[i]->numentries), string_entries_arrey[i]->WHAT2);␊
303␉␉␊
304␉␉buffer += 16;␊
305␉␉sprintf(buffer, "%02x%02x%04x%08x%08x", string_entries_arrey[i]->acpi_dev_path.type,␊
306␉␉␉␉string_entries_arrey[i]->acpi_dev_path.subtype,␊
307␉␉␉␉dp_swap16(string_entries_arrey[i]->acpi_dev_path.length),␊
308␉␉␉␉string_entries_arrey[i]->acpi_dev_path._HID,␊
309␉␉␉␉dp_swap32(string_entries_arrey[i]->acpi_dev_path._UID));␊
310␉␉␊
311␉␉buffer += 24;␊
312␉␉for(x=0;x < string_entries_arrey[i]->num_pci_devpaths; x++)␊
313␉␉{␊
314␉␉␉sprintf(buffer, "%02x%02x%04x%02x%02x", string_entries_arrey[i]->pci_dev_path[x].type,␊
315␉␉␉␉␉string_entries_arrey[i]->pci_dev_path[x].subtype,␊
316␉␉␉␉␉dp_swap16(string_entries_arrey[i]->pci_dev_path[x].length),␊
317␉␉␉␉␉string_entries_arrey[i]->pci_dev_path[x].function,␊
318␉␉␉␉␉string_entries_arrey[i]->pci_dev_path[x].device);␊
319␉␉␉buffer += 12;␊
320␉␉}␊
321␉␉␊
322␉␉sprintf(buffer, "%02x%02x%04x", string_entries_arrey[i]->path_end.type,␊
323␉␉␉␉string_entries_arrey[i]->path_end.subtype,␊
324␉␉␉␉dp_swap16(string_entries_arrey[i]->path_end.length));␊
325␉␉␊
326␉␉buffer += 8;␊
327␉␉uint8_t *dataptr = string_entries_arrey[i]->data;␊
328␉␉for(x = 0; (uint32_t)x < (string_entries_arrey[i]->length) - (24 + (6 * string_entries_arrey[i]->num_pci_devpaths)) ; x++)␊
329␉␉{␊
330␉␉␉sprintf(buffer, "%02x", *dataptr++);␊
331␉␉␉buffer += 2;␊
332␉␉}␊
333␉␉i++;␊
334␉}␊
335␉return ptr;␊
336}␊
337␊
338void devprop_free_string(struct DevPropString *string)␊
339{␊
340␉if(!string)␊
341␉{␊
342␉␉return;␊
343␉}␊
344␉␊
345␉int i;␊
346␉␊
347␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊
348␊
349␉for(i = 0; i < string->numentries; i++)␊
350␉{␊
351␉␉if(string_entries_arrey[i])␊
352␉␉{␊
353␉␉␉if(string_entries_arrey[i]->data)␊
354␉␉␉{␊
355␉␉␉␉free(string_entries_arrey[i]->data);␊
356␉␉␉␉string_entries_arrey[i]->data = NULL;␊
357␉␉␉}␊
358␉␉␉free(string_entries_arrey[i]);␊
359␉␉␉string_entries_arrey[i] = NULL;␊
360␉␉}␊
361␉}␊
362␉␊
363␉free(string);␊
364␉string = NULL;␊
365}

Archive Download this file

Revision: 1972