Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 2469