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 "bootstruct.h"␊ |
10 | #include "pci.h"␊ |
11 | #include "pci_root.h"␊ |
12 | #include "device_inject.h"␊ |
13 | #include "convert.h"␊ |
14 | ␊ |
15 | #ifndef DEBUG_INJECT␊ |
16 | #define DEBUG_INJECT 0␊ |
17 | #endif␊ |
18 | ␊ |
19 | #if DEBUG_INJECT␊ |
20 | #define DBG(x...)␉printf(x)␊ |
21 | #else␊ |
22 | #define DBG(x...)␊ |
23 | #endif␊ |
24 | ␊ |
25 | uint32_t devices_number = 1;␊ |
26 | struct DevPropString *string = 0;␊ |
27 | uint8_t *stringdata = 0;␊ |
28 | uint32_t stringlength = 0;␊ |
29 | ␊ |
30 | char *efi_inject_get_devprop_string(uint32_t *len)␊ |
31 | {␊ |
32 | ␉if(string)␊ |
33 | ␉{␊ |
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, DEFAULT_BOOT_CONFIG) && string)␊ |
58 | ␉{␊ |
59 | ␉␉val = (const char*)string;␊ |
60 | ␉␉cnt = strlength * 2;␊ |
61 | ␉} ␊ |
62 | ␊ |
63 | ␉if (cnt > 1)␊ |
64 | ␉{␊ |
65 | ␉␉binStr = convertHexStr2Binary(val, &cnt2);␊ |
66 | ␉␉if (cnt2 > 0) DT__AddProperty(node, DEVICE_PROPERTIES_PROP, cnt2, binStr);␊ |
67 | ␉}␊ |
68 | }␊ |
69 | ␊ |
70 | struct DevPropString *devprop_create_string(void)␊ |
71 | {␊ |
72 | ␉string = (struct DevPropString*)malloc(sizeof(struct DevPropString));␊ |
73 | ␉␊ |
74 | ␉if(string == NULL)␊ |
75 | ␉{␊ |
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 | ␊ |
85 | struct DevPropDevice *devprop_add_device(struct DevPropString *string, char *path)␊ |
86 | {␊ |
87 | ␉struct DevPropDevice␉*device;␊ |
88 | ␉const char␉␉pciroot_string[] = "PciRoot(0x";␊ |
89 | ␉const char␉␉pci_device_string[] = "Pci(0x";␊ |
90 | ␉␊ |
91 | ␉if (string == NULL || path == NULL) {␊ |
92 | ␉␉return NULL;␊ |
93 | ␉}␊ |
94 | ␉device = malloc(sizeof(struct DevPropDevice));␊ |
95 | ␉if (!device) {␊ |
96 | return NULL;␊ |
97 | }␊ |
98 | ␉if (strncmp(path, pciroot_string, strlen(pciroot_string))) {␊ |
99 | free(device);␊ |
100 | ␉␉printf("ERROR parsing device path\n");␊ |
101 | ␉␉return NULL;␊ |
102 | ␉}␊ |
103 | ␉␊ |
104 | ␉memset(device, 0, sizeof(struct 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 | ␉{␊ |
113 | ␉␉if (!strncmp(&path[x], pci_device_string, strlen(pci_device_string)))␊ |
114 | ␉␉{␊ |
115 | ␉␉␉x+=strlen(pci_device_string);␊ |
116 | ␉␉␉curr=x;␊ |
117 | ␉␉␉while(path[++x] != ',');␊ |
118 | ␉␉␉if(x-curr == 2)␊ |
119 | ␉␉␉{␊ |
120 | ␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊ |
121 | ␉␉␉}␊ |
122 | ␉␉␉else if(x-curr == 1)␊ |
123 | ␉␉␉{␊ |
124 | ␉␉␉␉sprintf(buff, "%c", path[curr]);␊ |
125 | ␉␉␉}␊ |
126 | ␉␉␉else ␊ |
127 | ␉␉␉{␊ |
128 | ␉␉␉␉printf("ERROR parsing device path\n");␊ |
129 | ␉␉␉␉numpaths = 0;␊ |
130 | ␉␉␉␉break;␊ |
131 | ␉␉␉}␊ |
132 | ␉␉␉device->pci_dev_path[numpaths].device =␉ascii_hex_to_int(buff);␊ |
133 | ␉␉␉␊ |
134 | ␉␉␉x += 3; // 0x␊ |
135 | ␉␉␉curr = x;␊ |
136 | ␉␉␉while(path[++x] != ')');␊ |
137 | ␉␉␉if(x-curr == 2)␊ |
138 | ␉␉␉{␊ |
139 | ␉␉␉␉sprintf(buff, "%c%c", path[curr], path[curr+1]);␊ |
140 | ␉␉␉}␊ |
141 | ␉␉␉else if(x-curr == 1)␊ |
142 | ␉␉␉{␊ |
143 | ␉␉␉␉sprintf(buff, "%c", path[curr]);␊ |
144 | ␉␉␉}␊ |
145 | ␉␉␉else␊ |
146 | ␉␉␉{␊ |
147 | ␉␉␉␉printf("ERROR parsing device path\n");␊ |
148 | ␉␉␉␉numpaths = 0;␊ |
149 | ␉␉␉␉break;␊ |
150 | ␉␉␉}␊ |
151 | ␉␉␉device->pci_dev_path[numpaths].function = ascii_hex_to_int(buff); // TODO: find dev from char *path␊ |
152 | ␉␉␉␊ |
153 | ␉␉␉numpaths++;␊ |
154 | ␉␉}␊ |
155 | ␉}␊ |
156 | ␉␊ |
157 | ␉if(!numpaths)␊ |
158 | ␉{␊ |
159 | free(device);␊ |
160 | ␉␉return NULL;␊ |
161 | ␉}␊ |
162 | ␉␊ |
163 | ␉device->numentries = 0x00;␊ |
164 | ␉␊ |
165 | ␉device->acpi_dev_path.length = 0x0c;␊ |
166 | ␉device->acpi_dev_path.type = 0x02;␊ |
167 | ␉device->acpi_dev_path.subtype = 0x01;␊ |
168 | ␉device->acpi_dev_path._HID = 0xd041030a;␊ |
169 | ␉␊ |
170 | ␉device->num_pci_devpaths = numpaths;␊ |
171 | ␉device->length = 24 + (6*numpaths);␊ |
172 | ␉␊ |
173 | ␉int␉␉i; ␊ |
174 | ␉␊ |
175 | ␉for(i = 0; i < numpaths; i++)␊ |
176 | ␉{␊ |
177 | ␉␉device->pci_dev_path[i].length = 0x06;␊ |
178 | ␉␉device->pci_dev_path[i].type = 0x01;␊ |
179 | ␉␉device->pci_dev_path[i].subtype = 0x01;␊ |
180 | ␉}␊ |
181 | ␉␊ |
182 | ␉device->path_end.length = 0x04;␊ |
183 | ␉device->path_end.type = 0x7f;␊ |
184 | ␉device->path_end.subtype = 0xff;␊ |
185 | ␉␊ |
186 | ␉device->string = string;␊ |
187 | ␉device->data = NULL;␊ |
188 | ␉string->length += device->length;␊ |
189 | ␉␊ |
190 | ␉if(!string->entries)␊ |
191 | ␉{␊ |
192 | ␉␉if((string->entries = (struct DevPropDevice**)malloc(sizeof(device)))== NULL)␊ |
193 | ␉␉{␊ |
194 | free(device);␊ |
195 | ␉␉␉return NULL;␊ |
196 | ␉␉}␊ |
197 | ␉}␊ |
198 | ␉␊ |
199 | if((string->entries[string->numentries++] = (struct DevPropDevice*)malloc(sizeof(device)))== NULL)␊ |
200 | {␊ |
201 | free(device);␊ |
202 | free(string->entries);␊ |
203 | return NULL;␊ |
204 | }␊ |
205 | ␉string->entries[string->numentries-1] = device;␊ |
206 | ␉␊ |
207 | ␉return device;␊ |
208 | }␊ |
209 | ␊ |
210 | int devprop_add_value(struct DevPropDevice *device, char *nm, uint8_t *vl, uint32_t len)␊ |
211 | {␊ |
212 | ␉␊ |
213 | ␉if(!nm || !vl || !len)␊ |
214 | ␉{␊ |
215 | ␉␉return 0;␊ |
216 | ␉}␊ |
217 | ␉␊ |
218 | ␉uint32_t length = ((strlen(nm) * 2) + len + (2 * sizeof(uint32_t)) + 2);␊ |
219 | ␉uint8_t *data = (uint8_t*)malloc(length);␊ |
220 | ␉{␊ |
221 | ␉␉if(!data)␊ |
222 | ␉␉{␊ |
223 | ␉␉␉return 0;␊ |
224 | ␉␉}␊ |
225 | ␉␉␊ |
226 | ␉␉memset(data, 0, length);␊ |
227 | ␉␉uint32_t off= 0;␊ |
228 | ␉␉data[off+1] = ((strlen(nm) * 2) + 6) >> 8;␊ |
229 | ␉␉data[off] = ((strlen(nm) * 2) + 6) & 0x00FF;␊ |
230 | ␉␉␊ |
231 | ␉␉off += 4;␊ |
232 | ␉␉uint32_t i=0, l = strlen(nm);␊ |
233 | ␉␉for(i = 0 ; i < l ; i++, off += 2)␊ |
234 | ␉␉{␊ |
235 | ␉␉␉data[off] = *nm++;␊ |
236 | ␉␉}␊ |
237 | ␉␉␊ |
238 | ␉␉off += 2;␊ |
239 | ␉␉l = len;␊ |
240 | ␉␉uint32_t *datalength = (uint32_t*)&data[off];␊ |
241 | ␉␉*datalength = l + 4;␊ |
242 | ␉␉off += 4;␊ |
243 | ␉␉for(i = 0 ; i < l ; i++, off++)␊ |
244 | ␉␉{␊ |
245 | ␉␉␉data[off] = *vl++;␊ |
246 | ␉␉}␊ |
247 | ␉}␉␊ |
248 | ␉␊ |
249 | ␉uint32_t offset = device->length - (24 + (6 * device->num_pci_devpaths));␊ |
250 | ␉␊ |
251 | ␉uint8_t *newdata = (uint8_t*)malloc((length + offset));␊ |
252 | ␉if(!newdata)␊ |
253 | ␉{␊ |
254 | ␉␉return 0;␊ |
255 | ␉}␊ |
256 | ␉if(device->data)␊ |
257 | ␉{␊ |
258 | ␉␉if(offset > 1)␊ |
259 | ␉␉{␊ |
260 | ␉␉␉memcpy(newdata, device->data, offset);␊ |
261 | ␉␉}␊ |
262 | ␉}␊ |
263 | ␉␊ |
264 | ␉memcpy(newdata + offset, data, length);␊ |
265 | ␉␊ |
266 | ␉device->length += length;␊ |
267 | ␉device->string->length += length;␊ |
268 | ␉device->numentries++;␊ |
269 | ␉␊ |
270 | ␉if(!device->data)␊ |
271 | ␉{␊ |
272 | ␉␉//device->data = (uint8_t*)malloc(sizeof(uint8_t)); //IMHO this is useless␊ |
273 | ␉}␊ |
274 | ␉else␊ |
275 | ␉{␊ |
276 | ␉␉free(device->data);␊ |
277 | ␉}␊ |
278 | ␉␊ |
279 | ␉free(data);␊ |
280 | ␉device->data = newdata;␊ |
281 | ␉␊ |
282 | ␉return 1;␊ |
283 | }␊ |
284 | ␊ |
285 | char *devprop_generate_string(struct DevPropString *string)␊ |
286 | {␊ |
287 | ␉char *buffer = (char*)malloc(string->length * 2);␊ |
288 | ␉char *ptr = buffer;␊ |
289 | ␉␊ |
290 | ␉if(!buffer)␊ |
291 | ␉{␊ |
292 | ␉␉return NULL;␊ |
293 | ␉}␊ |
294 | ␉␊ |
295 | ␉sprintf(buffer, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊ |
296 | ␉␉␉dp_swap16(string->numentries), string->WHAT3);␊ |
297 | ␉buffer += 24;␊ |
298 | ␉int i = 0, x = 0;␊ |
299 | ␉␊ |
300 | ␉while(i < string->numentries)␊ |
301 | ␉{␊ |
302 | ␉␉sprintf(buffer, "%08x%04x%04x", dp_swap32(string->entries[i]->length),␊ |
303 | ␉␉␉␉dp_swap16(string->entries[i]->numentries), string->entries[i]->WHAT2);␊ |
304 | ␉␉␊ |
305 | ␉␉buffer += 16;␊ |
306 | ␉␉sprintf(buffer, "%02x%02x%04x%08x%08x", string->entries[i]->acpi_dev_path.type,␊ |
307 | ␉␉␉␉string->entries[i]->acpi_dev_path.subtype,␊ |
308 | ␉␉␉␉dp_swap16(string->entries[i]->acpi_dev_path.length),␊ |
309 | ␉␉␉␉string->entries[i]->acpi_dev_path._HID,␊ |
310 | ␉␉␉␉dp_swap32(string->entries[i]->acpi_dev_path._UID));␊ |
311 | ␉␉␊ |
312 | ␉␉buffer += 24;␊ |
313 | ␉␉for(x=0;x < string->entries[i]->num_pci_devpaths; x++)␊ |
314 | ␉␉{␊ |
315 | ␉␉␉sprintf(buffer, "%02x%02x%04x%02x%02x", string->entries[i]->pci_dev_path[x].type,␊ |
316 | ␉␉␉␉␉string->entries[i]->pci_dev_path[x].subtype,␊ |
317 | ␉␉␉␉␉dp_swap16(string->entries[i]->pci_dev_path[x].length),␊ |
318 | ␉␉␉␉␉string->entries[i]->pci_dev_path[x].function,␊ |
319 | ␉␉␉␉␉string->entries[i]->pci_dev_path[x].device);␊ |
320 | ␉␉␉buffer += 12;␊ |
321 | ␉␉}␊ |
322 | ␉␉␊ |
323 | ␉␉sprintf(buffer, "%02x%02x%04x", string->entries[i]->path_end.type,␊ |
324 | ␉␉␉␉string->entries[i]->path_end.subtype,␊ |
325 | ␉␉␉␉dp_swap16(string->entries[i]->path_end.length));␊ |
326 | ␉␉␊ |
327 | ␉␉buffer += 8;␊ |
328 | ␉␉uint8_t *dataptr = string->entries[i]->data;␊ |
329 | ␉␉for(x = 0; (uint32_t)x < (string->entries[i]->length) - (24 + (6 * string->entries[i]->num_pci_devpaths)) ; x++)␊ |
330 | ␉␉{␊ |
331 | ␉␉␉sprintf(buffer, "%02x", *dataptr++);␊ |
332 | ␉␉␉buffer += 2;␊ |
333 | ␉␉}␊ |
334 | ␉␉i++;␊ |
335 | ␉}␊ |
336 | ␉return ptr;␊ |
337 | }␊ |
338 | ␊ |
339 | void devprop_free_string(struct DevPropString *string)␊ |
340 | {␊ |
341 | ␉if(!string)␊ |
342 | ␉{␊ |
343 | ␉␉return;␊ |
344 | ␉}␊ |
345 | ␉␊ |
346 | ␉int i;␊ |
347 | ␉for(i = 0; i < string->numentries; i++)␊ |
348 | ␉{␊ |
349 | ␉␉if(string->entries[i])␊ |
350 | ␉␉{␊ |
351 | ␉␉␉if(string->entries[i]->data)␊ |
352 | ␉␉␉{␊ |
353 | ␉␉␉␉free(string->entries[i]->data);␊ |
354 | ␉␉␉␉string->entries[i]->data = NULL;␊ |
355 | ␉␉␉}␊ |
356 | ␉␉␉free(string->entries[i]);␊ |
357 | ␉␉␉string->entries[i] = NULL;␊ |
358 | ␉␉}␊ |
359 | ␉}␊ |
360 | ␉␊ |
361 | ␉free(string);␊ |
362 | ␉string = NULL;␊ |
363 | } |