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