Chameleon

Chameleon Svn Source Tree

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

Archive Download this file

Revision: 2457