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 | ␊ |
26 | uint32_t devices_number␉␉= 1;␊ |
27 | DevPropString *string␉␉= NULL;␊ |
28 | uint8_t *stringdata␉␉= NULL;␊ |
29 | uint32_t stringlength␉␉= 0;␊ |
30 | ␊ |
31 | char *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 | ␊ |
41 | void 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 | ␊ |
70 | DevPropString *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 | ␊ |
84 | DevPropDevice *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 | ␊ |
190 | int 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␊ |
254 | char *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 | ␊ |
338 | void 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 | *******************************************************************/␊ |
368 | int 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 | |