Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 2658