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

Archive Download this file

Revision: 1977