Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 1146