Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 2299