Chameleon

Chameleon Svn Source Tree

Root/branches/rekursor/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␊
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␊
42/* XXX AsereBLN replace by strtoul */␊
43uint32_t ascii_hex_to_int(char *buff) ␊
44{␊
45␉uint32_t␉value = 0, i, digit;␊
46␉for(i = 0; i < strlen(buff); i++)␊
47␉{␊
48␉␉if (buff[i] >= 48 && buff[i] <= 57)␉␉␉// '0' through '9'␊
49␉␉␉digit = buff[i] - 48;␉␊
50␉␉else if (buff[i] >= 65 && buff[i] <= 70)␉// 'A' through 'F'␊
51␉␉␉digit = buff[i] - 55;␊
52␉␉else if (buff[i] >= 97 && buff[i] <= 102)␉// 'a' through 'f'␊
53␉␉␉digit = buff[i] - 87;␊
54␉␉else␊
55␉␉␉return value;␊
56␉␉␊
57␉␉value = digit + 16 * value;␊
58␉}␊
59␉return␉value;␊
60}␊
61␊
62void *convertHexStr2Binary(const char *hexStr, int *outLength)␊
63{␊
64 int len;␊
65 char hexNibble;␊
66 char hexByte[2];␊
67 uint8_t binChar;␊
68 uint8_t *binStr;␊
69 int hexStrIdx, binStrIdx, hexNibbleIdx;␊
70␊
71 len = strlen(hexStr);␊
72 if (len > 1)␊
73 {␊
74 // the resulting binary will be the half size of the input hex string␊
75 binStr = malloc(len / 2);␊
76 binStrIdx = 0;␊
77 hexNibbleIdx = 0;␊
78 for (hexStrIdx = 0; hexStrIdx < len; hexStrIdx++)␊
79 {␊
80 hexNibble = hexStr[hexStrIdx];␊
81 ␊
82 // ignore all chars except valid hex numbers␊
83 if (hexNibble >= '0' && hexNibble <= '9'␊
84 || hexNibble >= 'A' && hexNibble <= 'F'␊
85 || hexNibble >= 'a' && hexNibble <= 'f')␊
86 {␊
87 hexByte[hexNibbleIdx++] = hexNibble;␊
88 ␊
89 // found both two nibbles, convert to binary␊
90 if (hexNibbleIdx == 2)␊
91 {␊
92 binChar = 0;␊
93 ␊
94 for (hexNibbleIdx = 0; hexNibbleIdx < sizeof(hexByte); hexNibbleIdx++)␊
95 {␊
96 if (hexNibbleIdx > 0) binChar = binChar << 4;␊
97 ␊
98 if (hexByte[hexNibbleIdx] <= '9') binChar += hexByte[hexNibbleIdx] - '0';␊
99 else if (hexByte[hexNibbleIdx] <= 'F') binChar += hexByte[hexNibbleIdx] - ('A' - 10);␊
100 else if (hexByte[hexNibbleIdx] <= 'f') binChar += hexByte[hexNibbleIdx] - ('a' - 10);␊
101 }␊
102 ␊
103 binStr[binStrIdx++] = binChar;␉␉␉␉␉␉␊
104 hexNibbleIdx = 0;␊
105 }␊
106 }␊
107 }␊
108 *outLength = binStrIdx;␊
109 return binStr;␊
110 }␊
111 else␊
112 {␊
113 *outLength = 0;␊
114 return NULL;␊
115 }␊
116}␊
117␊
118void setupDeviceProperties(Node *node)␊
119{␊
120 const char *val;␊
121 uint8_t *binStr;␊
122 int cnt, cnt2;␊
123␊
124 static char DEVICE_PROPERTIES_PROP[] = "device-properties";␊
125␊
126 /* Generate devprop string.␊
127 */␊
128 uint32_t strlength;␊
129 char *string = efi_inject_get_devprop_string(&strlength);␊
130␊
131 /* Use the static "device-properties" boot config key contents if available,␊
132 * otheriwse use the generated one.␊
133 */ ␊
134 if (!getValueForKey(kDeviceProperties, &val, &cnt, &bootInfo->bootConfig) && string)␊
135 {␊
136 val = (const char*)string;␊
137 cnt = strlength * 2;␊
138 } ␊
139 ␊
140 if (cnt > 1)␊
141 {␊
142 binStr = convertHexStr2Binary(val, &cnt2);␊
143 if (cnt2 > 0) DT__AddProperty(node, DEVICE_PROPERTIES_PROP, cnt2, binStr);␊
144 }␊
145}␊
146␊
147uint16_t dp_swap16(uint16_t toswap)␊
148{␊
149␉return (((toswap & 0x00FF) << 8) | ((toswap & 0xFF00) >> 8));␊
150}␊
151␊
152uint32_t dp_swap32(uint32_t toswap)␊
153{␊
154␉return ((toswap & 0x000000FF) << 24) |␊
155␉␉␉((toswap & 0x0000FF00) << 8 ) |␊
156␉␉␉((toswap & 0x00FF0000) >> 8 ) |␊
157␉␉␉((toswap & 0xFF000000) >> 24);␊
158}␉␊
159␊
160struct DevPropString *devprop_create_string(void)␊
161{␊
162␉string = (struct DevPropString*)malloc(sizeof(struct DevPropString));␊
163␉␊
164␉if(string == NULL)␊
165␉␉return NULL;␊
166␉␊
167␉memset(string, 0, sizeof(struct DevPropString));␊
168␉string->length = 12;␊
169␉string->WHAT2 = 0x01000000;␊
170␉return string;␊
171}␊
172 ␊
173struct DevPropDevice *devprop_add_device(struct DevPropString *string, char *path)␊
174{␊
175␉struct DevPropDevice␉*device;␊
176␉const char␉␉pciroot_string[] = "PciRoot(0x";␊
177␉const char␉␉pci_device_string[] = "Pci(0x";␊
178␊
179␉if (string == NULL || path == NULL) {␊
180␉␉return NULL;␊
181␉}␊
182␉device = malloc(sizeof(struct DevPropDevice));␊
183␊
184␉if (strncmp(path, pciroot_string, strlen(pciroot_string))) {␊
185␉␉printf("ERROR parsing device path\n");␊
186␉␉return NULL;␊
187␉}␊
188␊
189␉memset(device, 0, sizeof(struct DevPropDevice));␊
190␉device->acpi_dev_path._UID = getPciRootUID();␊
191␊
192␉int numpaths = 0;␊
193␉int␉␉x, curr = 0;␊
194␉char␉buff[] = "00";␊
195␊
196␉for (x = 0; x < strlen(path); x++) {␊
197␉␉if (!strncmp(&path[x], pci_device_string, strlen(pci_device_string))) {␊
198␉␉␉x+=strlen(pci_device_string);␊
199␉␉␉curr=x;␊
200␉␉␉while(path[++x] != ',');␊
201␉␉␉if(x-curr == 2)␊
202␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
203␉␉␉else if(x-curr == 1)␊
204␉␉␉␉sprintf(buff, "%c", path[curr]);␊
205␉␉␉else ␊
206␉␉␉{␊
207␉␉␉␉printf("ERROR parsing device path\n");␊
208␉␉␉␉numpaths = 0;␊
209␉␉␉␉break;␊
210␉␉␉}␊
211␉␉␉device->pci_dev_path[numpaths].device =␉ascii_hex_to_int(buff);␊
212␉␉␉␊
213␉␉␉x += 3; // 0x␊
214␉␉␉curr = x;␊
215␉␉␉while(path[++x] != ')');␊
216␉␉␉if(x-curr == 2)␊
217␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
218␉␉␉else if(x-curr == 1)␊
219␉␉␉␉sprintf(buff, "%c", path[curr]);␊
220␉␉␉else␊
221␉␉␉{␊
222␉␉␉␉printf("ERROR parsing device path\n");␊
223␉␉␉␉numpaths = 0;␊
224␉␉␉␉break;␊
225␉␉␉}␊
226␉␉␉device->pci_dev_path[numpaths].function = ascii_hex_to_int(buff); // TODO: find dev from char *path␊
227␉␉␉␊
228␉␉␉numpaths++;␊
229␉␉}␊
230␉}␊
231␉␊
232␉if(!numpaths)␊
233␉␉return NULL;␊
234␉␊
235␉device->numentries = 0x00;␊
236␉␊
237␉device->acpi_dev_path.length = 0x0c;␊
238␉device->acpi_dev_path.type = 0x02;␊
239␉device->acpi_dev_path.subtype = 0x01;␊
240␉device->acpi_dev_path._HID = 0xd041030a;␊
241␉␊
242␉device->num_pci_devpaths = numpaths;␊
243␉device->length = 24 + (6*numpaths);␊
244␉␊
245␉int␉␉i; ␊
246␉␊
247␉for(i = 0; i < numpaths; i++)␊
248␉{␊
249␉␉device->pci_dev_path[i].length = 0x06;␊
250␉␉device->pci_dev_path[i].type = 0x01;␊
251␉␉device->pci_dev_path[i].subtype = 0x01;␊
252␉}␊
253␉␊
254␉device->path_end.length = 0x04;␊
255␉device->path_end.type = 0x7f;␊
256␉device->path_end.subtype = 0xff;␊
257␉␊
258␉device->string = string;␊
259␉device->data = NULL;␊
260␉string->length += device->length;␊
261␉␊
262␉if(!string->entries)␊
263␉␉if((string->entries = (struct DevPropDevice**)malloc(sizeof(device)))== NULL)␊
264␉␉␉return 0;␊
265␉␊
266␉string->entries[string->numentries++] = (struct DevPropDevice*)malloc(sizeof(device));␊
267␉string->entries[string->numentries-1] = device;␊
268␉␊
269␉return device;␊
270}␊
271␊
272int devprop_add_value(struct DevPropDevice *device, char *nm, uint8_t *vl, uint32_t len)␊
273{␊
274␉␊
275␉if(!nm || !vl || !len)␊
276␉␉return 0;␊
277␉␊
278␉uint32_t length = ((strlen(nm) * 2) + len + (2 * sizeof(uint32_t)) + 2);␊
279␉uint8_t *data = (uint8_t*)malloc(length);␊
280␉{␊
281␉␉if(!data)␊
282␉␉␉return 0;␊
283␉␉␊
284␉␉memset(data, 0, length);␊
285␉␉uint32_t off= 0;␊
286␉␉data[off+1] = ((strlen(nm) * 2) + 6) >> 8;␊
287␉␉data[off] = ((strlen(nm) * 2) + 6) & 0x00FF;␊
288␉␉␊
289␉␉off += 4;␊
290␉␉uint32_t i=0, l = strlen(nm);␊
291␉␉for(i = 0 ; i < l ; i++, off += 2)␊
292␉␉{␊
293␉␉␉data[off] = *nm++;␊
294␉␉}␊
295␉␉␊
296␉␉off += 2;␊
297␉␉l = len;␊
298␉␉uint32_t *datalength = (uint32_t*)&data[off];␊
299␉␉*datalength = l + 4;␊
300␉␉off += 4;␊
301␉␉for(i = 0 ; i < l ; i++, off++)␊
302␉␉{␊
303␉␉␉data[off] = *vl++;␊
304␉␉}␊
305␉}␉␊
306␉␊
307␉uint32_t offset = device->length - (24 + (6 * device->num_pci_devpaths));␊
308␉␊
309␉uint8_t *newdata = (uint8_t*)malloc((length + offset));␊
310␉if(!newdata)␊
311␉␉return 0;␊
312␉if(device->data)␊
313␉␉if(offset > 1)␊
314␉␉␉memcpy(newdata, device->data, offset);␊
315␊
316␉memcpy(newdata + offset, data, length);␊
317␉␊
318␉device->length += length;␊
319␉device->string->length += length;␊
320␉device->numentries++;␊
321␉␊
322␉if(!device->data)␊
323␉␉device->data = (uint8_t*)malloc(sizeof(uint8_t));␊
324␉else␊
325␉␉free(device->data);␊
326␉␊
327␉free(data);␊
328␉device->data = newdata;␊
329␉␊
330␉return 1;␊
331}␊
332␊
333char *devprop_generate_string(struct DevPropString *string)␊
334{␊
335␉char *buffer = (char*)malloc(string->length * 2);␊
336␉char *ptr = buffer;␊
337␉␊
338␉if(!buffer)␊
339␉␉return NULL;␊
340␊
341␉sprintf(buffer, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊
342␉␉␉dp_swap16(string->numentries), string->WHAT3);␊
343␉buffer += 24;␊
344␉int i = 0, x = 0;␊
345␉␊
346␉while(i < string->numentries)␊
347␉{␊
348␉␉sprintf(buffer, "%08x%04x%04x", dp_swap32(string->entries[i]->length),␊
349␉␉␉␉dp_swap16(string->entries[i]->numentries), string->entries[i]->WHAT2);␊
350␉␉␊
351␉␉buffer += 16;␊
352␉␉sprintf(buffer, "%02x%02x%04x%08x%08x", string->entries[i]->acpi_dev_path.type,␊
353␉␉␉␉string->entries[i]->acpi_dev_path.subtype,␊
354␉␉␉␉dp_swap16(string->entries[i]->acpi_dev_path.length),␊
355␉␉␉␉string->entries[i]->acpi_dev_path._HID,␊
356␉␉␉␉dp_swap32(string->entries[i]->acpi_dev_path._UID));␊
357␊
358␉␉buffer += 24;␊
359␉␉for(x=0;x < string->entries[i]->num_pci_devpaths; x++)␊
360␉␉{␊
361␉␉␉sprintf(buffer, "%02x%02x%04x%02x%02x", string->entries[i]->pci_dev_path[x].type,␊
362␉␉␉␉␉string->entries[i]->pci_dev_path[x].subtype,␊
363␉␉␉␉␉dp_swap16(string->entries[i]->pci_dev_path[x].length),␊
364␉␉␉␉␉string->entries[i]->pci_dev_path[x].function,␊
365␉␉␉␉␉string->entries[i]->pci_dev_path[x].device);␊
366␉␉␉buffer += 12;␊
367␉␉}␊
368␉␉␊
369␉␉sprintf(buffer, "%02x%02x%04x", string->entries[i]->path_end.type,␊
370␉␉␉␉string->entries[i]->path_end.subtype,␊
371␉␉␉␉dp_swap16(string->entries[i]->path_end.length));␊
372␉␉␊
373␉␉buffer += 8;␊
374␉␉uint8_t *dataptr = string->entries[i]->data;␊
375␉␉for(x = 0; x < (string->entries[i]->length) - (24 + (6 * string->entries[i]->num_pci_devpaths)) ; x++)␊
376␉␉{␊
377␉␉␉sprintf(buffer, "%02x", *dataptr++);␊
378␉␉␉buffer += 2;␊
379␉␉}␊
380␉␉i++;␊
381␉}␊
382␉return ptr;␊
383}␊
384␊
385void devprop_free_string(struct DevPropString *string)␊
386{␊
387␉if(!string)␊
388␉␉return;␊
389␉␊
390␉int i;␊
391␉for(i = 0; i < string->numentries; i++)␊
392␉{␊
393␉␉if(string->entries[i])␊
394␉␉{␊
395␉␉␉if(string->entries[i]->data)␊
396␉␉␉{␊
397␉␉␉␉free(string->entries[i]->data);␊
398␉␉␉␉string->entries[i]->data = NULL;␊
399␉␉␉}␊
400␉␉␉free(string->entries[i]);␊
401␉␉␉string->entries[i] = NULL;␊
402␉␉}␊
403␉}␊
404␉␊
405␉free(string);␊
406␉string = NULL;␊
407}␊
408␊
409/* a fine place for this code */␊
410␊
411int devprop_add_network_template(struct DevPropDevice *device, uint16_t vendor_id)␊
412{␊
413␉if(!device)␊
414␉␉return 0;␊
415␉uint8_t builtin = 0x0;␊
416␉if((vendor_id != 0x168c) && (builtin_set == 0)) ␊
417␉{␊
418␉␉builtin_set = 1;␊
419␉␉builtin = 0x01;␊
420␉}␊
421␉if(!devprop_add_value(device, "built-in", (uint8_t*)&builtin, 1))␊
422␉␉return 0;␊
423␉devices_number++;␊
424␉return 1;␊
425}␊
426␊
427void set_eth_builtin(pci_dt_t *eth_dev)␊
428{␊
429␉char *devicepath = get_pci_dev_path(eth_dev);␊
430␉struct DevPropDevice *device = (struct DevPropDevice*)malloc(sizeof(struct DevPropDevice));␊
431␊
432␉verbose("LAN Controller [%04x:%04x] :: %s\n", eth_dev->vendor_id, eth_dev->device_id, devicepath);␊
433␊
434␉if (!string)␊
435␉␉string = devprop_create_string();␊
436␊
437␉device = devprop_add_device(string, devicepath);␊
438␉if(device)␊
439␉{␊
440␉␉verbose("Setting up lan keys\n");␊
441␉␉devprop_add_network_template(device, eth_dev->vendor_id);␊
442␉␉stringdata = (uint8_t*)malloc(sizeof(uint8_t) * string->length);␊
443␉␉if(stringdata)␊
444␉␉{␊
445␉␉␉memcpy(stringdata, (uint8_t*)devprop_generate_string(string), string->length);␊
446␉␉␉stringlength = string->length;␊
447␉␉}␊
448␉}␊
449}␊
450

Archive Download this file

Revision: 11