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

Archive Download this file

Revision: 4