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 "device_inject.h"␊ |
12 | #include "convert.h"␊ |
13 | #include "platform.h"␊ |
14 | #include "modules.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...)␊ |
24 | #endif␊ |
25 | ␊ |
26 | static char *efi_inject_get_devprop_string(uint32_t *len);␊ |
27 | void setupDeviceProperties_hook(void* arg1, void* arg2, void* arg3, void* arg4, void* arg5, void* arg6);␊ |
28 | static EFI_STATUS setupDeviceProperties(Node *node);␊ |
29 | ␊ |
30 | struct DevPropString *devprop_create_string(void)␊ |
31 | {␊ |
32 | ␉struct DevPropString *string = (struct DevPropString*)malloc(sizeof(struct DevPropString));␊ |
33 | ␉␊ |
34 | ␉if(string == NULL)␊ |
35 | ␉{␊ |
36 | ␉␉return NULL;␊ |
37 | ␉}␊ |
38 | ␉␊ |
39 | ␉memset(string, 0, sizeof(struct DevPropString));␊ |
40 | ␉string->length = 12;␊ |
41 | ␉string->WHAT2 = 0x01000000;␊ |
42 | ␉return string;␊ |
43 | }␊ |
44 | ␊ |
45 | struct DevPropDevice *devprop_make_device(pci_dt_t *pci_dt)␊ |
46 | {␊ |
47 | ␉struct DevPropDevice␉*device;␊ |
48 | int numpaths = 0;␊ |
49 | ␊ |
50 | ␉pci_dt_t␉*current;␊ |
51 | ␉pci_dt_t␉*end;␊ |
52 | ␉␊ |
53 | ␉end = (pci_dt_t *)(uint32_t)get_env(envPCIRootDev);␊ |
54 | ␊ |
55 | ␉device = malloc(sizeof(struct DevPropDevice));␊ |
56 | ␉if (!device) {␊ |
57 | return NULL;␊ |
58 | }␊ |
59 | memset(device, 0, sizeof(struct DevPropDevice));␊ |
60 | ␊ |
61 | ␉device->acpi_dev_path._UID = getPciRootUID();␊ |
62 | ␉while (end != pci_dt)␊ |
63 | ␉{␊ |
64 | ␉␉current = pci_dt;␊ |
65 | ␉␉while (current->parent != end)␊ |
66 | ␉␉␉current = current->parent;␉␉␉␊ |
67 | ␉␉end = current;␊ |
68 | ␊ |
69 | ␉␉{␊ |
70 | device->pci_dev_path[numpaths].device =␉(uint8_t)current->dev.bits.dev;␊ |
71 | ␉␉␉device->pci_dev_path[numpaths].function = (uint8_t)current->dev.bits.func;␊ |
72 | numpaths++;␊ |
73 | ␉␉} ␊ |
74 | ␉␉␊ |
75 | ␉}␊ |
76 | ␉␉␉␊ |
77 | ␉if(!numpaths)␊ |
78 | ␉{␊ |
79 | printf("ERROR parsing device path\n");␊ |
80 | free(device);␊ |
81 | ␉␉return NULL;␊ |
82 | ␉}␊ |
83 | ␉␊ |
84 | ␉device->numentries = 0x00;␊ |
85 | ␉␊ |
86 | ␉device->acpi_dev_path.length = 0x0c;␊ |
87 | ␉device->acpi_dev_path.type = 0x02;␊ |
88 | ␉device->acpi_dev_path.subtype = 0x01;␊ |
89 | ␉device->acpi_dev_path._HID = 0xd041030a;␊ |
90 | ␉␊ |
91 | ␉device->num_pci_devpaths = numpaths;␊ |
92 | ␉device->length = 24 + (6*numpaths);␊ |
93 | ␉␊ |
94 | ␉int␉␉i; ␊ |
95 | ␉␊ |
96 | ␉for(i = 0; i < numpaths; i++)␊ |
97 | ␉{␊ |
98 | ␉␉device->pci_dev_path[i].length = 0x06;␊ |
99 | ␉␉device->pci_dev_path[i].type = 0x01;␊ |
100 | ␉␉device->pci_dev_path[i].subtype = 0x01;␊ |
101 | ␉}␊ |
102 | ␉␊ |
103 | ␉device->path_end.length = 0x04;␊ |
104 | ␉device->path_end.type = 0x7f;␊ |
105 | ␉device->path_end.subtype = 0xff;␊ |
106 | ␉␊ |
107 | ␉device->data = NULL;␊ |
108 | ␉␉␉␊ |
109 | ␉return device;␊ |
110 | }␊ |
111 | ␊ |
112 | struct DevPropDevice *devprop_add_device(struct DevPropString *string, pci_dt_t * pci_dt)␊ |
113 | {␊ |
114 | ␉struct DevPropDevice␉*device;␊ |
115 | ␉␉␊ |
116 | ␉if (string == NULL || pci_dt == NULL) {␊ |
117 | ␉␉return NULL;␊ |
118 | ␉}␊ |
119 | ␉device = devprop_make_device(pci_dt);␊ |
120 | ␉if (!device) {␊ |
121 | return NULL;␊ |
122 | }␊ |
123 | ␉␉␊ |
124 | ␉device->string = string;␊ |
125 | ␉string->length += device->length;␊ |
126 | ␉␊ |
127 | ␉if(!string->entries)␊ |
128 | ␉{␊ |
129 | ␉␉if((string->entries = (struct DevPropDevice*)malloc(sizeof(struct DevPropDevice) * MAX_STRING_NUM_ENTRIES))== NULL)␊ |
130 | ␉␉{␊ |
131 | printf("ERROR parsing device path 2\n");␊ |
132 | ␊ |
133 | free(device);␊ |
134 | ␉␉␉return NULL;␊ |
135 | ␉␉}␊ |
136 | ␉}␊ |
137 | ␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊ |
138 | ␉␊ |
139 | ␉if ((string->numentries+1) < MAX_STRING_NUM_ENTRIES)␊ |
140 | {␊ |
141 | ␉␉string->numentries++;␊ |
142 | ␉␉␊ |
143 | }␊ |
144 | else␊ |
145 | {␊ |
146 | free(string->entries);␊ |
147 | free(device);␊ |
148 | return NULL;␊ |
149 | }␉␊ |
150 | ␊ |
151 | ␉string_entries_arrey[string->numentries-1] = device;␊ |
152 | ␉␊ |
153 | ␉return device;␊ |
154 | }␊ |
155 | ␊ |
156 | int devprop_add_value(struct DevPropDevice *device, char *nm, uint8_t *vl, uint32_t len)␊ |
157 | {␊ |
158 | ␉␊ |
159 | ␉if(!nm || !vl || !len)␊ |
160 | ␉{␊ |
161 | ␉␉return 0;␊ |
162 | ␉}␊ |
163 | ␉␊ |
164 | ␉uint32_t length = ((strlen(nm) * 2) + len + (2 * sizeof(uint32_t)) + 2);␊ |
165 | ␉uint8_t *data = (uint8_t*)malloc(length);␊ |
166 | ␉␊ |
167 | ␉if(!data)␊ |
168 | {␊ |
169 | return 0;␊ |
170 | }␊ |
171 | ␊ |
172 | memset(data, 0, length);␊ |
173 | uint32_t off= 0;␊ |
174 | data[off+1] = ((strlen(nm) * 2) + 6) >> 8;␊ |
175 | data[off] = ((strlen(nm) * 2) + 6) & 0x00FF;␊ |
176 | ␊ |
177 | off += 4;␊ |
178 | uint32_t i=0, l = strlen(nm);␊ |
179 | for(i = 0 ; i < l ; i++, off += 2)␊ |
180 | {␊ |
181 | data[off] = *nm++;␊ |
182 | }␊ |
183 | ␊ |
184 | off += 2;␊ |
185 | l = len;␊ |
186 | uint32_t *datalength = (uint32_t*)&data[off];␊ |
187 | *datalength = l + 4;␊ |
188 | off += 4;␊ |
189 | for(i = 0 ; i < l ; i++, off++)␊ |
190 | {␊ |
191 | data[off] = *vl++;␊ |
192 | }␊ |
193 | ␉␊ |
194 | ␉uint32_t offset = device->length - (24 + (6 * device->num_pci_devpaths));␊ |
195 | ␉␊ |
196 | ␉uint8_t *newdata = (uint8_t*)malloc((length + offset));␊ |
197 | ␉if(!newdata)␊ |
198 | ␉{␊ |
199 | ␉␉return 0;␊ |
200 | ␉}␊ |
201 | ␉if(device->data)␊ |
202 | ␉{␊ |
203 | ␉␉if(offset > 1)␊ |
204 | ␉␉{␊ |
205 | ␉␉␉memcpy(newdata, device->data, offset);␊ |
206 | ␉␉}␊ |
207 | ␉}␊ |
208 | ␉␊ |
209 | ␉memcpy(newdata + offset, data, length);␊ |
210 | ␉␊ |
211 | ␉device->length += length;␊ |
212 | ␉device->string->length += length;␊ |
213 | ␉device->numentries++;␊ |
214 | ␉␊ |
215 | ␉if(device->data)␉␊ |
216 | ␉{␊ |
217 | ␉␉free(device->data);␊ |
218 | ␉}␊ |
219 | ␉␊ |
220 | ␉free(data);␊ |
221 | ␉device->data = newdata;␊ |
222 | ␉␊ |
223 | ␉return 1;␊ |
224 | }␊ |
225 | ␊ |
226 | char *devprop_generate_string(struct DevPropString *string)␊ |
227 | {␊ |
228 | int len = string->length * 2;␊ |
229 | ␉char *buffer = (char*)malloc(len);␊ |
230 | ␉char *ptr = buffer;␊ |
231 | ␉␊ |
232 | ␉if(!buffer)␊ |
233 | ␉{␊ |
234 | ␉␉return NULL;␊ |
235 | ␉}␊ |
236 | ␉␊ |
237 | ␉snprintf(buffer, len, "%08x%08x%04x%04x", dp_swap32(string->length), string->WHAT2,␊ |
238 | dp_swap16(string->numentries), string->WHAT3);␊ |
239 | ␉buffer += 24;␊ |
240 | len -= 24;␊ |
241 | ␉int i = 0, x = 0;␊ |
242 | ␉␊ |
243 | ␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊ |
244 | ␊ |
245 | ␉while(i < string->numentries)␊ |
246 | ␉{␊ |
247 | ␉␉if (!(i<MAX_STRING_NUM_ENTRIES))␊ |
248 | {␊ |
249 | break;␊ |
250 | }␊ |
251 | if (!len) {␊ |
252 | break;␊ |
253 | }␊ |
254 | ␉␉snprintf(buffer, len, "%08x%04x%04x", dp_swap32(string_entries_arrey[i]->length),␊ |
255 | dp_swap16(string_entries_arrey[i]->numentries), string_entries_arrey[i]->WHAT2);␊ |
256 | ␉␉␊ |
257 | ␉␉buffer += 16;␊ |
258 | len -= 16;␊ |
259 | ␉␉snprintf(buffer, len,"%02x%02x%04x%08x%08x", string_entries_arrey[i]->acpi_dev_path.type,␊ |
260 | string_entries_arrey[i]->acpi_dev_path.subtype,␊ |
261 | dp_swap16(string_entries_arrey[i]->acpi_dev_path.length),␊ |
262 | string_entries_arrey[i]->acpi_dev_path._HID,␊ |
263 | dp_swap32(string_entries_arrey[i]->acpi_dev_path._UID));␊ |
264 | ␉␉␊ |
265 | ␉␉buffer += 24;␊ |
266 | len -= 24;␊ |
267 | ␉␉for(x=0;x < string_entries_arrey[i]->num_pci_devpaths; x++)␊ |
268 | ␉␉{␊ |
269 | if (!len) {␊ |
270 | break;␊ |
271 | }␊ |
272 | ␉␉␉snprintf(buffer, len,"%02x%02x%04x%02x%02x", string_entries_arrey[i]->pci_dev_path[x].type,␊ |
273 | string_entries_arrey[i]->pci_dev_path[x].subtype,␊ |
274 | dp_swap16(string_entries_arrey[i]->pci_dev_path[x].length),␊ |
275 | string_entries_arrey[i]->pci_dev_path[x].function,␊ |
276 | string_entries_arrey[i]->pci_dev_path[x].device);␊ |
277 | ␉␉␉buffer += 12;␊ |
278 | len -= 12;␊ |
279 | ␉␉}␊ |
280 | ␉␉if (!len) {␊ |
281 | break;␊ |
282 | }␊ |
283 | ␉␉snprintf(buffer, len,"%02x%02x%04x", string_entries_arrey[i]->path_end.type,␊ |
284 | string_entries_arrey[i]->path_end.subtype,␊ |
285 | dp_swap16(string_entries_arrey[i]->path_end.length));␊ |
286 | ␉␉␊ |
287 | ␉␉buffer += 8;␊ |
288 | len -= 8;␊ |
289 | ␉␉uint8_t *dataptr = string_entries_arrey[i]->data;␊ |
290 | ␉␉for(x = 0; (uint32_t)x < (string_entries_arrey[i]->length) - (24 + (6 * string_entries_arrey[i]->num_pci_devpaths)) ; x++)␊ |
291 | ␉␉{␊ |
292 | if (!len) {␊ |
293 | break;␊ |
294 | }␊ |
295 | ␉␉␉snprintf(buffer, len, "%02x", *dataptr++);␊ |
296 | ␉␉␉buffer += 2;␊ |
297 | len -= 2;␊ |
298 | ␉␉}␊ |
299 | ␉␉i++;␊ |
300 | ␉}␊ |
301 | ␉return ptr;␊ |
302 | }␊ |
303 | ␊ |
304 | void devprop_free_string(struct DevPropString *string)␊ |
305 | {␊ |
306 | ␉if(!string)␊ |
307 | ␉{␊ |
308 | ␉␉return;␊ |
309 | ␉}␊ |
310 | ␉␊ |
311 | ␉int i;␊ |
312 | ␉␊ |
313 | ␉struct DevPropDevice **string_entries_arrey = (struct DevPropDevice **) string->entries;␊ |
314 | ␉␊ |
315 | ␉for(i = 0; i < string->numentries; i++)␊ |
316 | ␉{␊ |
317 | if (!(i<MAX_STRING_NUM_ENTRIES))␊ |
318 | {␊ |
319 | break;␊ |
320 | }␊ |
321 | ␉␉if(string_entries_arrey[i])␊ |
322 | ␉␉{␊ |
323 | ␉␉␉if(string_entries_arrey[i]->data)␊ |
324 | ␉␉␉{␊ |
325 | ␉␉␉␉free(string_entries_arrey[i]->data);␊ |
326 | ␉␉␉␉string_entries_arrey[i]->data = NULL;␊ |
327 | ␉␉␉}␊ |
328 | ␉␉}␊ |
329 | ␉}␊ |
330 | ␊ |
331 | free(string->entries);␊ |
332 | string->entries = NULL;␊ |
333 | ␊ |
334 | ␉free(string);␊ |
335 | ␉string = NULL;␊ |
336 | }␊ |
337 | ␊ |
338 | static char *efi_inject_get_devprop_string(uint32_t *len)␊ |
339 | {␊ |
340 | struct DevPropString *string = (struct DevPropString *)(uint32_t)get_env(envEFIString);␊ |
341 | ␊ |
342 | ␉if(string)␊ |
343 | ␉{␊ |
344 | ␉␉*len = string->length;␊ |
345 | ␉␉return devprop_generate_string(string);␊ |
346 | ␉}␊ |
347 | ␉return NULL;␊ |
348 | }␊ |
349 | ␊ |
350 | static EFI_STATUS setupDeviceProperties(Node *node)␊ |
351 | {␊ |
352 | ␉const char *val;␊ |
353 | ␉uint8_t *binStr;␊ |
354 | uint8_t *kbinStr;␊ |
355 | EFI_STATUS ret = EFI_DEVICE_ERROR;␊ |
356 | ␉int cnt = 0, cnt2 = 0;␊ |
357 | ␉␊ |
358 | ␉static char DEVICE_PROPERTIES_PROP[] = "device-properties";␊ |
359 | ␉␊ |
360 | ␉/* Generate devprop string.␊ |
361 | ␉ */␊ |
362 | ␉uint32_t strlength = 0;␊ |
363 | ␉char *string = efi_inject_get_devprop_string(&strlength);␊ |
364 | ␉if (string == NULL) {␊ |
365 | ␉␉DBG("efi_inject_get_devprop_string NULL trying stringdata\n");␊ |
366 | ␉␉return EFI_NO_MEDIA;␊ |
367 | ␉}␉␊ |
368 | ␊ |
369 | ␉val = (const char*)string;␊ |
370 | ␉cnt = strlength * 2;␉␊ |
371 | ␊ |
372 | ␉if (cnt > 1)␊ |
373 | ␉{␊ |
374 | ␉␉binStr = convertHexStr2Binary(val, &cnt2);␊ |
375 | if (binStr) ␊ |
376 | ␉␉{␊ |
377 | ␉␉␉if (cnt2 > 0)␊ |
378 | ␉␉␉{␊ |
379 | ␉␉␉␉kbinStr = (uint8_t*)AllocateKernelMemory(cnt2);␊ |
380 | ␉␉␉␉if (kbinStr) ␊ |
381 | ␉␉␉␉{␊ |
382 | ␉␉␉␉␉bcopy(binStr,kbinStr,cnt2);␊ |
383 | ␉␉␉␉␉DT__AddProperty(node, DEVICE_PROPERTIES_PROP, cnt2, kbinStr);␊ |
384 | ␉␉␉␉␉ret = EFI_SUCCESS;␊ |
385 | ␉␉␉␉}␊ |
386 | ␉␉␉}␊ |
387 | ␉␉␉free(binStr);␊ |
388 | ␉␉}␊ |
389 | ␊ |
390 | ␉}␊ |
391 | ␉return ret;␊ |
392 | ␊ |
393 | }␊ |
394 | ␊ |
395 | void setupDeviceProperties_hook(void* arg1, void* arg2, void* arg3, void* arg4, void* arg5, void* arg6)␊ |
396 | {␊ |
397 | ␉Node *node = (Node *)arg1;␊ |
398 | ␉EFI_STATUS *ret = (EFI_STATUS*)arg2;␊ |
399 | ␉*ret = setupDeviceProperties(node);␊ |
400 | }␊ |
401 | ␊ |
402 | void register_device_inject(void)␊ |
403 | {␊ |
404 | ␉register_one_callback("setupDeviceProperties", &setupDeviceProperties_hook);␊ |
405 | } |