Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 2266