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

Archive Download this file

Revision: 2118