Chameleon

Chameleon Svn Source Tree

Root/branches/ErmaC/Enoch_Modules/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 onboard_number␉␉= 1; // TODO: increment the onboard-x(+1) value if the duallink option is true for nVidia and ATi (gfx and HDAU)␊
28uint32_t builtin_set␉␉= 0;␊
29DevPropString *string␉␉= NULL;␊
30uint8_t *stringdata␉␉= NULL;␊
31uint32_t stringlength␉␉= 0;␊
32␊
33char *efi_inject_get_devprop_string(uint32_t *len)␊
34{␊
35␉if(string)␊
36␉{␊
37␉␉*len = string->length;␊
38␉␉return devprop_generate_string(string);␊
39␉}␊
40␉verbose("efi_inject_get_devprop_string NULL trying stringdata\n");␊
41␉return NULL;␊
42}␊
43␊
44void setupDeviceProperties(Node *node)␊
45{␊
46 const char *val;␊
47 uint8_t *binStr;␊
48 int cnt, cnt2;␊
49␊
50 static char DEVICE_PROPERTIES_PROP[] = "device-properties";␊
51␊
52 /* Generate devprop string.␊
53 */␊
54 uint32_t strlength;␊
55 char *string = efi_inject_get_devprop_string(&strlength);␊
56␊
57 /* Use the static "device-properties" boot config key contents if available,␊
58 * otheriwse use the generated one.␊
59 */ ␊
60␉if (!getValueForKey(kDeviceProperties, &val, &cnt, &bootInfo->chameleonConfig) && string)␊
61␉{␊
62␉␉val = (const char*)string;␊
63␉␉cnt = strlength * 2;␊
64␉}␊
65␊
66␉if (cnt > 1)␊
67␉{␊
68␉␉binStr = convertHexStr2Binary(val, &cnt2);␊
69␉␉if (cnt2 > 0)␊
70␉␉{␊
71␉␉␉DT__AddProperty(node, DEVICE_PROPERTIES_PROP, cnt2, binStr);␊
72␉␉}␊
73␉}␊
74}␊
75␊
76DevPropString *devprop_create_string(void)␊
77{␊
78␉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␊
91DevPropDevice *devprop_add_device(DevPropString *string, char *path)␊
92{␊
93␉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(DevPropDevice));␊
101␉if (!device)␊
102␉{␊
103␉␉return NULL;␊
104␉}␊
105␉if (strncmp(path, pciroot_string, strlen(pciroot_string)))␊
106␉{␊
107␉␉free(device);␊
108␉␉printf("ERROR parsing device path\n");␊
109␉␉return NULL;␊
110␉}␊
111␊
112␉memset(device, 0, sizeof(DevPropDevice));␊
113␉device->acpi_dev_path._UID = getPciRootUID();␊
114␊
115␉int numpaths = 0;␊
116␉int␉␉x, curr = 0;␊
117␉char␉buff[] = "00";␊
118␊
119␉for (x = 0; x < strlen(path); x++)␊
120␉{␊
121␉␉if (!strncmp(&path[x], pci_device_string, strlen(pci_device_string)))␊
122␉␉{␊
123␉␉␉x+=strlen(pci_device_string);␊
124␉␉␉curr=x;␊
125␉␉␉while(path[++x] != ',');␊
126␉␉␉if(x-curr == 2)␊
127␉␉␉{␊
128␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
129␉␉␉}␊
130␉␉␉else if(x-curr == 1)␊
131␉␉␉{␊
132␉␉␉␉sprintf(buff, "%c", path[curr]);␊
133␉␉␉}␊
134␉␉␉else␊
135␉␉␉{␊
136␉␉␉␉printf("ERROR parsing device path\n");␊
137␉␉␉␉numpaths = 0;␊
138␉␉␉␉break;␊
139␉␉␉}␊
140␉␉␉device->pci_dev_path[numpaths].device =␉ascii_hex_to_int(buff);␊
141␊
142␉␉␉x += 3; // 0x␊
143␉␉␉curr = x;␊
144␉␉␉while(path[++x] != ')');␊
145␉␉␉if(x-curr == 2)␊
146␉␉␉{␊
147␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊
148␉␉␉}␊
149␉␉␉else if(x-curr == 1)␊
150␉␉␉{␊
151␉␉␉␉sprintf(buff, "%c", path[curr]);␊
152␉␉␉}␊
153␉␉␉else␊
154␉␉␉{␊
155␉␉␉␉printf("ERROR parsing device path\n");␊
156␉␉␉␉numpaths = 0;␊
157␉␉␉␉break;␊
158␉␉␉}␊
159␉␉␉device->pci_dev_path[numpaths].function = ascii_hex_to_int(buff); // TODO: find dev from char *path␊
160␊
161␉␉␉numpaths++;␊
162␉␉}␊
163␉}␊
164␉␊
165␉if(!numpaths)␊
166␉{␊
167␉␉free(device);␊
168␉␉return NULL;␊
169␉}␊
170␉␊
171␉device->numentries = 0x00;␊
172␉␊
173␉device->acpi_dev_path.length = 0x0c;␊
174␉device->acpi_dev_path.type = 0x02;␊
175␉device->acpi_dev_path.subtype = 0x01;␊
176␉device->acpi_dev_path._HID = 0xd041030a;␊
177␉␊
178␉device->num_pci_devpaths = numpaths;␊
179␉device->length = 24 + (6*numpaths);␊
180␉␊
181␉int␉␉i;␊
182␉␊
183␉for(i = 0; i < numpaths; i++)␊
184␉{␊
185␉␉device->pci_dev_path[i].length = 0x06;␊
186␉␉device->pci_dev_path[i].type = 0x01;␊
187␉␉device->pci_dev_path[i].subtype = 0x01;␊
188␉}␊
189␉␊
190␉device->path_end.length = 0x04;␊
191␉device->path_end.type = 0x7f;␊
192␉device->path_end.subtype = 0xff;␊
193␉␊
194␉device->string = string;␊
195␉device->data = NULL;␊
196␉string->length += device->length;␊
197␉␊
198␉if(!string->entries)␊
199␉{␊
200␉␉if((string->entries = (DevPropDevice**)malloc(sizeof(device)*DEV_PROP_DEVICE_MAX_ENTRIES))== NULL)␊
201␉␉{␊
202␉␉␉printf("ERROR parsing device path 2\n");␊
203␊
204␉␉␉free(device);␊
205␉␉␉return 0;␊
206␉␉}␊
207␉}␊
208␉␊
209␉string->entries[string->numentries++] = (DevPropDevice*)malloc(sizeof(device));␊
210␉string->entries[string->numentries-1] = device;␊
211␊
212␉return device;␊
213}␊
214␊
215int devprop_add_value(DevPropDevice *device, char *nm, uint8_t *vl, uint32_t len)␊
216{␊
217␊
218␉if(!nm || !vl || !len)␊
219␉␉return 0;␊
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 = (uint32_t)(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));␊
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(DevPropString *string)␊
289{␊
290␉uint32_t len = string->length * 2;␊
291␉int i = 0;␊
292␉uint32_t x = 0;␊
293␊
294␉char *buffer = (char*)malloc(len +1);␊
295␉char *ptr = buffer;␊
296␊
297␉if(!buffer)␊
298␉{␊
299␉␉return NULL;␊
300␉}␊
301␊
302␉sprintf(buffer, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊
303␉␉␉dp_swap16(string->numentries), string->WHAT3);␊
304␉buffer += 24;␊
305␊
306␉while(i < string->numentries)␊
307␉{␊
308␉␉sprintf(buffer, "%08x%04x%04x", dp_swap32(string->entries[i]->length),␊
309␉␉␉␉dp_swap16(string->entries[i]->numentries), string->entries[i]->WHAT2);␊
310␉␉␊
311␉␉buffer += 16;␊
312␉␉sprintf(buffer, "%02x%02x%04x%08x%08x", string->entries[i]->acpi_dev_path.type,␊
313␉␉␉␉string->entries[i]->acpi_dev_path.subtype,␊
314␉␉␉␉dp_swap16(string->entries[i]->acpi_dev_path.length),␊
315␉␉␉␉string->entries[i]->acpi_dev_path._HID,␊
316␉␉␉␉dp_swap32(string->entries[i]->acpi_dev_path._UID));␊
317␊
318␉␉buffer += 24;␊
319␉␉for(x = 0;x < string->entries[i]->num_pci_devpaths; x++)␊
320␉␉{␊
321␉␉␉sprintf(buffer, "%02x%02x%04x%02x%02x", string->entries[i]->pci_dev_path[x].type,␊
322␉␉␉␉␉string->entries[i]->pci_dev_path[x].subtype,␊
323␉␉␉␉␉dp_swap16(string->entries[i]->pci_dev_path[x].length),␊
324␉␉␉␉␉string->entries[i]->pci_dev_path[x].function,␊
325␉␉␉␉␉string->entries[i]->pci_dev_path[x].device);␊
326␉␉␉buffer += 12;␊
327␉␉}␊
328␉␉␊
329␉␉sprintf(buffer, "%02x%02x%04x", string->entries[i]->path_end.type,␊
330␉␉␉␉string->entries[i]->path_end.subtype,␊
331␉␉␉␉dp_swap16(string->entries[i]->path_end.length));␊
332␉␉␊
333␉␉buffer += 8;␊
334␉␉uint8_t *dataptr = string->entries[i]->data;␊
335␉␉for(x = 0; (uint32_t)x < (string->entries[i]->length) - (24 + (6 * string->entries[i]->num_pci_devpaths)) ; x++)␊
336␉␉{␊
337␉␉␉sprintf(buffer, "%02x", *dataptr++);␊
338␉␉␉buffer += 2;␊
339␉␉}␊
340␉␉i++;␊
341␉}␊
342␉return ptr;␊
343}␊
344␊
345void devprop_free_string(DevPropString *string)␊
346{␊
347␊
348␉if(!string)␊
349␉{␊
350␉␉return;␊
351␉}␊
352␊
353␉int i;␊
354␉for(i = 0; i < string->numentries; i++)␊
355␉{␊
356␉␉if(string->entries[i])␊
357␉␉{␊
358␉␉␉if(string->entries[i]->data)␊
359␉␉␉{␊
360␉␉␉␉free(string->entries[i]->data);␊
361␉␉␉␉string->entries[i]->data = NULL;␊
362␉␉␉}␊
363␉␉␉free(string->entries[i]);␊
364␉␉␉string->entries[i] = NULL;␊
365␉␉}␊
366␉}␊
367␊
368␉free(string);␊
369␉string = NULL;␊
370}␊
371␊
372/* ======================================================= */␊
373␊
374/* Converters */␊
375␊
376/*static int hexdigit(int c)␊
377{␊
378␉unsigned d;␊
379 ␊
380␉d = (unsigned)(c - '0');␊
381␉if (d < 10) return d;␊
382␊
383␉d = (unsigned)(c - 'a');␊
384␉if (d < 6) return d+10;␊
385␊
386␉d = (unsigned)(c - 'A');␊
387␉if (d < 6) return d+10;␊
388␉printf("[ERROR] hexdigit input error\n");␊
389␉return -1;␊
390}*/␊
391␊
392/*********************************************************************␊
393 * decodes a sequence of 'len' hexadecimal chars from 'hex' into *␊
394 * an integer. returns -1 in case of error (i.e. badly formed chars) *␊
395 *********************************************************************/␊
396static const long hextable[] = {␊
397 ␉[0 ... 255] = -1, // bit aligned access into this table is considerably␊
398 ␉['0'] = 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, // faster for most modern processors,␊
399 ␉['A'] = 10, 11, 12, 13, 14, 15, // for the space conscious, reduce to␊
400 ␉['a'] = 10, 11, 12, 13, 14, 15 // signed char.␊
401};␊
402␊
403int hex2dec(unsigned const char *hex) { //long... unsigned const char␊
404␉int ret = 0;␊
405␉while (*hex && ret >= 0) {␊
406␉ret = (ret << 4) | hextable[*hex++];␊
407␉}␊
408␉return ret;␊
409}␊
410␊
411/************************************************************************␊
412 * Encodes an integer 'val' into 'len' hexadecimal charaters into 'hex' *␊
413 ************************************************************************/␊
414//char int2hex(int val)␊
415␊
416␊
417/***********************************************************␊
418 * bin2hex creates hexadecimal presentation of binary data *␊
419 ***********************************************************/␊
420char *bin2hex(const uint8_t* bin, char* hex, int len)␊
421{␊
422␉int i;␊
423␉const char *hexc = "0123456789abcdef";␊
424␉for (i = 0; i < len; i++) {␊
425␉␉hex[i * 2] = hexc[(bin[i] >> 4) & 0xf];␊
426␉␉hex[i * 2 + 1] = hexc[bin[i] &0xf];␊
427␉}␊
428␉hex[i * 2] = '\0';␊
429␉return hex;␊
430}␊
431/* */␊
432␊
433/*******************************************************************␊
434 * Decodes a sequence of 'len' hexadecimal chars from 'hex' into *␊
435 * a binary. returns -1 in case of error (i.e. badly formed chars) *␊
436 *******************************************************************/␊
437int hex2bin(const char *hex, uint8_t *bin, int len)␊
438{␊
439␉char␉*p;␊
440␉int␉i;␊
441␉char␉buf[3];␊
442␊
443␉if (hex == NULL || bin == NULL || len <= 0 || strlen(hex) != len * 2)␊
444␉{␊
445␉␉printf("[ERROR] bin2hex input error\n");␊
446␉␉return -1;␊
447␉}␊
448␊
449␉buf[2] = '\0';␊
450␉p = (char *) hex;␊
451␊
452␉for (i = 0; i < len; i++)␊
453␉{␊
454␉␉if (p[0] == '\0' || p[1] == '\0' || !isxdigit(p[0]) || !isxdigit(p[1]))␊
455␉␉{␊
456␉␉␉printf("[ERROR] bin2hex '%s' syntax error\n", hex);␊
457␉␉␉return -2;␊
458␉␉}␊
459␉␉buf[0] = *p++;␊
460␉␉buf[1] = *p++;␊
461␉␉bin[i] = (unsigned char) strtoul(buf, NULL, 16);␊
462␉}␊
463␉return 0;␊
464}␊
465␊
466/* ======================================================= */␊
467␊
468/* a fine place for this code */␊
469int devprop_add_network_template(DevPropDevice *device, uint16_t vendor_id)␊
470{␊
471␉uint8_t builtin = 0x00;␊
472␉if(device)␊
473␉{␊
474␊
475␉␉if((vendor_id != 0x168c) && (builtin_set == 0))␊
476␉␉{␊
477␉␉␉builtin_set = 1;␊
478␉␉␉builtin = 0x01;␊
479␉␉}␊
480␉␉if(!devprop_add_value(device, "device_type", (uint8_t*)"ethernet", 8))␊
481␉␉{␊
482␉␉␉return 0;␊
483␉␉}␊
484␉␉if(!devprop_add_value(device, "built-in", (uint8_t*)&builtin, 1))␊
485␉␉{␊
486␉␉␉return 0;␊
487␉␉}␊
488␉␉devices_number++;␊
489␉␉return 1;␊
490␉}␊
491␉else␊
492␉{␊
493␉␉return 0;␊
494␉}␊
495}␊
496␊
497void set_eth_builtin(pci_dt_t *eth_dev)␊
498{␊
499␉char *devicepath = get_pci_dev_path(eth_dev);␊
500␉DevPropDevice *device = (DevPropDevice*)malloc(sizeof(DevPropDevice));␊
501␊
502␉verbose("LAN Controller [%04x:%04x] :: %s\n", eth_dev->vendor_id, eth_dev->device_id, devicepath);␊
503␊
504␉if (!string)␊
505␉{␊
506␉␉string = devprop_create_string();␊
507␉␉if (!string)␊
508␉␉{␊
509␉␉␉return;␊
510␉␉}␊
511␉}␊
512␊
513␉device = devprop_add_device(string, devicepath);␊
514␉if(device)␊
515␉{␊
516␉␉verbose("Setting up lan keys\n");␊
517␉␉devprop_add_network_template(device, eth_dev->vendor_id);␊
518␉␉stringdata = (uint8_t*)malloc(sizeof(uint8_t) * string->length);␊
519␉␉if(stringdata)␊
520␉␉{␊
521␉␉␉memcpy(stringdata, (uint8_t*)devprop_generate_string(string), string->length);␊
522␉␉␉stringlength = string->length;␊
523␉␉}␊
524␉}␊
525}␊
526

Archive Download this file

Revision: 2238