Index: trunk/i386/libsaio/acpi_patcher.c =================================================================== --- trunk/i386/libsaio/acpi_patcher.c (revision 242) +++ trunk/i386/libsaio/acpi_patcher.c (revision 243) @@ -189,17 +189,20 @@ uint8_t acpi_cpu_count = 0; char* acpi_cpu_name[32]; -void find_acpi_cpu_names(unsigned char* dsdt, int length) +void get_acpi_cpu_names(unsigned char* dsdt, uint32_t length) { - int i; + uint32_t i; for (i=0; i> 6) + 1, j; + uint32_t offset = i + 3 + (dsdt[i+2] >> 6); + bool add_name = true; + uint8_t j; + for (j=0; j<4; j++) { char c = dsdt[offset+j]; @@ -207,17 +210,18 @@ if (!aml_isvalidchar(c)) { add_name = false; - verbose("Invalid characters found in ProcessorOP!\n"); + verbose("Invalid character found in ProcessorOP 0x%x!\n", c); break; } } - if (add_name && dsdt[offset+5] < 32 ) + if (add_name) { - acpi_cpu_name[acpi_cpu_count] = malloc(5); + acpi_cpu_name[acpi_cpu_count] = malloc(4); memcpy(acpi_cpu_name[acpi_cpu_count], dsdt+offset, 4); + i = offset + 5; - verbose("Found %c%c%c%c (from DSDT)\n", acpi_cpu_name[acpi_cpu_count][0], acpi_cpu_name[acpi_cpu_count][1], acpi_cpu_name[acpi_cpu_count][2], acpi_cpu_name[acpi_cpu_count][3]); + verbose("Found ACPI CPU: %c%c%c%c\n", acpi_cpu_name[acpi_cpu_count][0], acpi_cpu_name[acpi_cpu_count][1], acpi_cpu_name[acpi_cpu_count][2], acpi_cpu_name[acpi_cpu_count][3]); if (++acpi_cpu_count == 32) return; } @@ -236,163 +240,122 @@ 0x31, 0x03, 0x10, 0x20 /* 1.._ */ }; - char chunk_name_body[] = + char cstate_resource_template[] = { - 0x5C, 0x5F, 0x50, 0x52, 0x5F, 0x08, 0x43, 0x53, /* \_PR_.CS */ - 0x54, 0x5F /* T_ */ + 0x11, 0x14, 0x0A, 0x11, 0x82, 0x0C, 0x00, 0x7F, + 0x01, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x79, 0x00 }; + + if (Platform.CPU.Vendor != 0x756E6547) { + verbose ("Not an Intel platform: C-States will not be generated !!!\n"); + return NULL; + } - char chunk_c1[] = - { - 0x12, 0x1C, 0x04, 0x11, 0x14, 0x0A, 0x11, 0x82, - 0x0C, 0x00, 0x7F, 0x01, 0x02, 0x01, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x79, 0x00, - 0x01, 0x01, 0x0B, 0xE8, 0x03 - }; - - char chunk_c2[] = - { - 0x12, 0x1E, 0x04, 0x11, 0x14, 0x0A, 0x11, 0x82, - 0x0C, 0x00, 0x7F, 0x01, 0x02, 0x01, 0x10, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x79, 0x00, - 0x0A, 0x02, 0x0A, 0x40, 0x0B, 0xF4, 0x01 - }; - - char chunk_c3[] = - { - 0x12, 0x1F, 0x04, 0x11, 0x14, 0x0A, 0x11, 0x82, - 0x0C, 0x00, 0x7F, 0x01, 0x02, 0x01, 0x20, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x79, 0x00, - 0x0A, 0x03, 0x0B, 0x60, 0x03, 0x0B, 0x5E, 0x01 - }; - - char chunk_alias[] = - { - 0x10, 0x14, 0x5C, 0x2E, 0x5F, 0x50, 0x52, 0x5F, /* ..\._PR_ */ - 0x43, 0x50, 0x55, 0x30, 0x06, 0x43, 0x53, 0x54, /* CPU0.CST */ - 0x5F, 0x5F, 0x43, 0x53, 0x54 /* __CST */ - }; - if (fadt == NULL) { - verbose ("FACP not exists: C-States not generated !!!\n"); + verbose ("FACP not exists: C-States will not be generated !!!\n"); return NULL; } struct acpi_2_dsdt* dsdt = (void*)fadt->DSDT; if (dsdt == NULL) { - verbose ("DSDT not found: C-States not generated !!!\n"); + verbose ("DSDT not found: C-States will not be generated !!!\n"); return NULL; } if (acpi_cpu_count == 0) - find_acpi_cpu_names((void*)dsdt, dsdt->Length); + get_acpi_cpu_names((void*)dsdt, dsdt->Length); - if (acpi_cpu_count > 0) { - bool c2_enabled = fadt->C2_Latency < 100, c3_enabled = fadt->C3_Latency < 1000; + if (acpi_cpu_count > 0) + { + bool c2_enabled = fadt->C2_Latency < 100; + bool c3_enabled = fadt->C3_Latency < 1000; + bool c4_enabled = false; - // Setup C2 Latency - if (c2_enabled) - chunk_c2[27] = fadt->C2_Latency & 0xff; + getBoolForKey(kEnableC4States, &c4_enabled, &bootInfo->bootConfig); + + unsigned char cstates_count = 1 + (c2_enabled ? 1 : 0) + (c3_enabled ? 1 : 0); - // Setup C3 Latency - if (c3_enabled) { - chunk_c3[27] = fadt->C3_Latency & 0xff; - chunk_c3[28] = (fadt->C3_Latency >> 8) & 0xff; - } + struct aml_chunk* root = aml_create_node(NULL); + aml_add_buffer(root, ssdt_header, sizeof(ssdt_header)); // SSDT header + struct aml_chunk* scop = aml_add_scope(root, "\\_PR_"); + struct aml_chunk* name = aml_add_name(scop, "CST_"); + struct aml_chunk* pack = aml_add_package(name); + aml_add_byte(pack, cstates_count); - // Generating SSDT - uint32_t package_length = - 4 + - sizeof(chunk_c1) + - c2_enabled * sizeof(chunk_c2) + - c3_enabled * sizeof(chunk_c3); - - if (package_length > 0x3f) - package_length++; - - uint32_t name_length = - 1 + - sizeof(chunk_name_body) + - 1 + package_length; - - if (name_length > 0x3f) - name_length++; - - uint32_t ssdt_size = - sizeof(ssdt_header) + - 1 + name_length + - acpi_cpu_count * sizeof(chunk_alias); - - struct acpi_2_ssdt *ssdt = (void*)AllocateKernelMemory(ssdt_size); - int fd = openmem((char*)ssdt, ssdt_size); + struct aml_chunk* tmpl = aml_add_package(pack); + cstate_resource_template[11] = 0x00; // C1 + aml_add_buffer(tmpl, cstate_resource_template, sizeof(cstate_resource_template)); + aml_add_byte(tmpl, 0x01); // C1 + aml_add_byte(tmpl, 0x01); // Latency + aml_add_word(tmpl, 0x03e8); // Power - // Header - write(fd, ssdt_header, sizeof(ssdt_header)); + // C2 + if (c2_enabled) + { + tmpl = aml_add_package(pack); + cstate_resource_template[11] = 0x10; // C2 + aml_add_buffer(tmpl, cstate_resource_template, sizeof(cstate_resource_template)); + aml_add_byte(tmpl, 0x02); // C2 + aml_add_byte(tmpl, fadt->C2_Latency); + aml_add_word(tmpl, 0x01f4); // Power + } + // C4 + if (c4_enabled) + { + tmpl = aml_add_package(pack); + cstate_resource_template[11] = 0x30; // C4 + aml_add_buffer(tmpl, cstate_resource_template, sizeof(cstate_resource_template)); + aml_add_byte(tmpl, 0x04); // C4 + aml_add_word(tmpl, fadt->C3_Latency / 2); // TODO: right latency for C4 + aml_add_byte(tmpl, 0xfa); // Power + } + else + // C3 + if (c3_enabled) + { + tmpl = aml_add_package(pack); + cstate_resource_template[11] = 0x20; // C3 + aml_add_buffer(tmpl, cstate_resource_template, sizeof(cstate_resource_template)); + aml_add_byte(tmpl, 0x03); // C3 + aml_add_word(tmpl, fadt->C3_Latency); + aml_add_word(tmpl, 0x015e); // Power + } + + + // Aliaces + int i; + for (i = 0; i < acpi_cpu_count; i++) + { + char name[9]; + sprintf(name, "_PR_%c%c%c%c", acpi_cpu_name[i][0], acpi_cpu_name[i][1], acpi_cpu_name[i][2], acpi_cpu_name[i][3]); + + scop = aml_add_scope(root, name); + aml_add_alias(scop, "CST_", "_CST"); + } - // Scope (\_PR) { Name (CST - writebyte(fd, 0x10); // id - if (name_length > 0x3f) - { - writebyte(fd, 0x40 | (name_length & 0xf)); // lo half-byte - writebyte(fd, name_length >> 4); // hi byte - } - else - { - writebyte(fd, name_length); // length - } - write(fd, chunk_name_body, sizeof(chunk_name_body)); + aml_calculate_size(root); - //Package (0x04) { 0x03, - writebyte(fd, 0x12); // id - if (package_length > 0x3f) - { - writebyte(fd, 0x40 | (package_length & 0xf)); // lo half-byte - writebyte(fd, package_length >> 4); // hi byte - } - else - { - writebyte(fd, package_length); // length - } - uint8_t cstates_count = 1 + c2_enabled + c3_enabled; - writebyte(fd, cstates_count + 1); - writebyte(fd, 0x0A); // first entry - number of c-states - writebyte(fd, cstates_count); + struct acpi_2_ssdt *ssdt = (struct acpi_2_ssdt *)AllocateKernelMemory(root->Size); + + aml_write_node(root, (void*)ssdt, 0); - // C1 - write(fd, chunk_c1, sizeof(chunk_c1)); - - // C2 - if (c2_enabled) - write(fd, chunk_c2, sizeof(chunk_c2)); - - // C3 - if (c3_enabled) - write(fd, chunk_c3, sizeof(chunk_c3)); - - // Write aliases - int i; - for (i = 0; i < acpi_cpu_count; i++) { - int j; - for (j = 0; j < 4; j++) - chunk_alias[8+j] = acpi_cpu_name[i][j]; - write(fd, chunk_alias, sizeof(chunk_alias)); - } - - close(fd); - - ssdt->Length = ssdt_size; + ssdt->Length = root->Size; ssdt->Checksum = 0; ssdt->Checksum = 256 - checksum8(ssdt, ssdt->Length); - //dumpPhysAddr("C-States SSDT content: ", ssdt, ssdt_size); + aml_destroy_node(root); + + //dumpPhysAddr("C-States SSDT content: ", ssdt, ssdt->Length); verbose ("SSDT with CPU C-States generated successfully\n"); return ssdt; } - else { - verbose ("DSDT CPUs not found: C-States not generated !!!\n"); + else + { + verbose ("ACPI CPUs not found: C-States not generated !!!\n"); } return NULL; @@ -409,19 +372,6 @@ 0x31, 0x03, 0x10, 0x20, /* 1.._ */ }; - char chunk_name_body[] = - { - 0x5C, 0x5F, 0x50, 0x52, 0x5F, 0x08, 0x50, 0x53, /* \_PR_.PS */ - 0x53, 0x5F /* S_ */ - }; - - char chunk_alias[] = - { - 0x10, 0x14, 0x5C, 0x2E, 0x5F, 0x50, 0x52, 0x5F, /* ..\._PR_ */ - 0x43, 0x50, 0x55, 0x30, 0x06, 0x50, 0x53, 0x53, /* CPU0.PSS */ - 0x5F, 0x5F, 0x50, 0x53, 0x53 /* __PSS */ - }; - if (Platform.CPU.Vendor != 0x756E6547) { verbose ("Not an Intel platform: P-States will not be generated !!!\n"); return NULL; @@ -433,7 +383,7 @@ } if (acpi_cpu_count == 0) - find_acpi_cpu_names((void*)dsdt, dsdt->Length); + get_acpi_cpu_names((void*)dsdt, dsdt->Length); if (acpi_cpu_count > 0) { @@ -442,9 +392,6 @@ uint8_t p_states_count; // Retrieving P-States, ported from code by superhai (c) - - - switch (Platform.CPU.Family) { case 0x06: { @@ -453,10 +400,6 @@ case 0x0F: // Intel Core (65nm) case 0x17: // Intel Core (45nm) case 0x1C: // Intel Atom (45nm) - case 0x1A: // Intel Core i7 LGA1366 (45nm) - case 0x1E: // Intel Core i5, i7 LGA1156 (45nm) - case 0x25: // Intel Core i3, i5, i7 LGA1156 (32nm) - case 0x2C: // Intel Core i7 LGA1366 (32nm) 6 Core if (rdmsr64(MSR_IA32_EXT_CONFIG) & (1 << 27)) { wrmsr64(MSR_IA32_EXT_CONFIG, (rdmsr64(MSR_IA32_EXT_CONFIG) | (1 << 28))); @@ -464,6 +407,14 @@ cpu_dynamic_fsb = rdmsr64(MSR_IA32_EXT_CONFIG) & (1 << 28); } break; + case 0x1A: // Intel Core i7 LGA1366 (45nm) + case 0x1E: // Intel Core i5, i7 LGA1156 (45nm) + case 0x1F: + case 0x25: // Intel Core i3, i5, i7 LGA1156 (32nm) + case 0x2C: // Intel Core i7 LGA1366 (32nm) 6 Core + case 0x2F: + cpu_dynamic_fsb = rdmsr64(MSR_IA32_PERF_STATUS) & (1 << 15); + break; } } } @@ -478,33 +429,39 @@ if (minimum.FID == 0) { + uint64_t msr; uint8_t i; // Probe for lowest fid for (i = maximum.FID; i >= 0x6; i--) { - wrmsr64(MSR_IA32_PERF_CONTROL, (rdmsr64(MSR_IA32_PERF_CONTROL) & 0xFFFFFFFFFFFF0000ULL) | (i << 8) | minimum.VID); + msr = rdmsr64(MSR_IA32_PERF_CONTROL); + wrmsr64(MSR_IA32_PERF_CONTROL, (msr & 0xFFFFFFFFFFFF0000ULL) | (i << 8) | minimum.VID); intel_waitforsts(); minimum.FID = (rdmsr64(MSR_IA32_PERF_STATUS) >> 8) & 0x1F; delay(1); } - wrmsr64(MSR_IA32_PERF_CONTROL, (rdmsr64(MSR_IA32_PERF_CONTROL) & 0xFFFFFFFFFFFF0000ULL) | (maximum.FID << 8) | maximum.VID); + msr = rdmsr64(MSR_IA32_PERF_CONTROL); + wrmsr64(MSR_IA32_PERF_CONTROL, (msr & 0xFFFFFFFFFFFF0000ULL) | (maximum.FID << 8) | maximum.VID); intel_waitforsts(); } if (minimum.VID == maximum.VID) { + uint64_t msr; uint8_t i; // Probe for lowest vid for (i = maximum.VID; i > 0xA; i--) { - wrmsr64(MSR_IA32_PERF_CONTROL, (rdmsr64(MSR_IA32_PERF_CONTROL) & 0xFFFFFFFFFFFF0000ULL) | (minimum.FID << 8) | i); + msr = rdmsr64(MSR_IA32_PERF_CONTROL); + wrmsr64(MSR_IA32_PERF_CONTROL, (msr & 0xFFFFFFFFFFFF0000ULL) | (minimum.FID << 8) | i); intel_waitforsts(); minimum.VID = rdmsr64(MSR_IA32_PERF_STATUS) & 0x3F; delay(1); } - wrmsr64(MSR_IA32_PERF_CONTROL, (rdmsr64(MSR_IA32_PERF_CONTROL) & 0xFFFFFFFFFFFF0000ULL) | (maximum.FID << 8) | maximum.VID); + msr = rdmsr64(MSR_IA32_PERF_CONTROL); + wrmsr64(MSR_IA32_PERF_CONTROL, (msr & 0xFFFFFFFFFFFF0000ULL) | (maximum.FID << 8) | maximum.VID); intel_waitforsts(); } @@ -569,84 +526,57 @@ if (p_states_count > 0) { - uint32_t i, pss_entries_size = 33 * p_states_count, pss_package_length = pss_entries_size + 2; + int i; - if (pss_package_length > 0x3f) pss_package_length++; // for chunks > 0x3f bytes length have 2 bytes encoding + struct aml_chunk* root = aml_create_node(NULL); + aml_add_buffer(root, ssdt_header, sizeof(ssdt_header)); // SSDT header + struct aml_chunk* scop = aml_add_scope(root, "\\_PR_"); + struct aml_chunk* name = aml_add_name(scop, "PSS_"); + struct aml_chunk* pack = aml_add_package(name); - uint32_t pss_name_length = (1 /* id=0x12 */ + pss_package_length) + (1 + 10); - - if (pss_name_length > 0x3f) pss_name_length++; - - uint32_t ssdt_size = 36 + (1 /* id=0x10 */ + pss_name_length) + acpi_cpu_count * sizeof(chunk_alias); - - struct acpi_2_ssdt *ssdt = (void*)AllocateKernelMemory(ssdt_size); - int fd = openmem((char*)ssdt, ssdt_size); - - // write header - write(fd, ssdt_header, sizeof(ssdt_header)); - - // write Scope (\_PR) { Name (PSS, ... - writebyte(fd, 0x10); // id - if (pss_name_length > 0x3f) + for (i = 0; i < p_states_count; i++) + { + struct aml_chunk* pstt = aml_add_package(pack); + + aml_add_dword(pstt, p_states[i].Frequency); + aml_add_dword(pstt, 0x00000000); // Power + aml_add_dword(pstt, 0x0000000A); // Latency + aml_add_dword(pstt, 0x0000000A); // Latency + aml_add_dword(pstt, p_states[i].Control); + aml_add_dword(pstt, i+1); // Status + } + + // Add aliaces + for (i = 0; i < acpi_cpu_count; i++) { - writebyte(fd, 0x40 | (pss_name_length & 0xf)); // lo half-byte - writebyte(fd, pss_name_length >> 4); // hi byte + char name[9]; + sprintf(name, "_PR_%c%c%c%c", acpi_cpu_name[i][0], acpi_cpu_name[i][1], acpi_cpu_name[i][2], acpi_cpu_name[i][3]); + + scop = aml_add_scope(root, name); + aml_add_alias(scop, "PSS_", "_PSS"); } - else - { - writebyte(fd, pss_name_length); // length - } - write(fd, chunk_name_body, sizeof(chunk_name_body)); - // write Package(p_states_count) { ... - writebyte(fd, 0x12); // id - if (pss_package_length > 0x3f) - { - writebyte(fd, 0x40 | (pss_package_length & 0xf)); // lo half-byte - writebyte(fd, pss_package_length >> 4); // hi byte - } - else - { - writebyte(fd, pss_package_length); // length - } - writebyte(fd, p_states_count); // entries + aml_calculate_size(root); - for (i = 0; i < p_states_count; i++) - { - DBG("P-State: Frequency %d MHz, FID 0x%x, VID 0x%x\n", p_states[i].Frequency, p_states[i].FID, p_states[i].VID); - - writebyte(fd, 0x12); // chunk id - writebyte(fd, 32); // chunk length without id - writebyte(fd, 6); // entries - - writebyte(fd, 0x0C); /* id */ writeint(fd, p_states[i].Frequency); // value - writebyte(fd, 0x0C); /* id */ writeint(fd, 0x00000000); // value - writebyte(fd, 0x0C); /* id */ writeint(fd, 0x0000000A); // value - writebyte(fd, 0x0C); /* id */ writeint(fd, 0x0000000A); // value - writebyte(fd, 0x0C); /* id */ writeint(fd, p_states[i].Control); // value - writebyte(fd, 0x0C); /* id */ writeint(fd, i + 1); // value - } + struct acpi_2_ssdt *ssdt = (struct acpi_2_ssdt *)AllocateKernelMemory(root->Size); - // Write aliases - for (i = 0; i < acpi_cpu_count; i++) { - int j; - for (j = 0; j < 4; j++) - chunk_alias[8+j] = acpi_cpu_name[i][j]; - write(fd, chunk_alias, sizeof(chunk_alias)); - } + aml_write_node(root, (void*)ssdt, 0); - ssdt->Length = ssdt_size; + ssdt->Length = root->Size; ssdt->Checksum = 0; ssdt->Checksum = 256 - checksum8(ssdt, ssdt->Length); - //dumpPhysAddr("P-States SSDT content: ", ssdt, ssdt_size); + aml_destroy_node(root); + + //dumpPhysAddr("P-States SSDT content: ", ssdt, ssdt->Length); + verbose ("SSDT with CPU P-States generated successfully\n"); return ssdt; } } else { - verbose ("DSDT CPUs not found: P-States not generated !!!\n"); + verbose ("ACPI CPUs not found: P-States not generated !!!\n"); } return NULL; @@ -699,7 +629,7 @@ else Platform.Type = (unsigned char) strtoul(value, NULL, 10); } - // Set PM_Profile from System-type if only if user wanted this value to be forced + // Set PM_Profile from System-type if only user wanted this value to be forced if (fadt_mod->PM_Profile != Platform.Type) { if (value) Index: trunk/i386/libsaio/aml_generator.c =================================================================== --- trunk/i386/libsaio/aml_generator.c (revision 242) +++ trunk/i386/libsaio/aml_generator.c (revision 243) @@ -9,20 +9,32 @@ #include "aml_generator.h" -unsigned char aml_get_length_size(long length) +bool aml_add_to_parent(struct aml_chunk* parent, struct aml_chunk* node) { - if (length > 0x3F) - return 2; - else if (length > 0x3FFF) - return 3; - - return 1; -} - -void aml_add_to_parent(struct aml_chunk* parent, struct aml_chunk* node) -{ if (parent && node) { + switch (parent->Type) + { + case AML_CHUNK_NONE: + case AML_CHUNK_BYTE: + case AML_CHUNK_WORD: + case AML_CHUNK_DWORD: + case AML_CHUNK_QWORD: + case AML_CHUNK_ALIAS: + verbose("aml_add_to_parent: Node isn't supports child nodes!"); + return FALSE; + case AML_CHUNK_NAME: + if (parent->First) + { + verbose("aml_add_to_parent: Name node could have only one child node!"); + return FALSE; + } + break; + + default: + break; + } + if (!parent->First) parent->First = node; @@ -30,20 +42,48 @@ parent->Last->Next = node; parent->Last = node; + + return TRUE; } + + return FALSE; } struct aml_chunk* aml_create_node(struct aml_chunk* parent) { - struct aml_chunk* node = (void*)malloc(sizeof(struct aml_chunk)); + struct aml_chunk* node = (struct aml_chunk*)malloc(sizeof(struct aml_chunk)); aml_add_to_parent(parent, node); return node; } -int aml_add_buffer(struct aml_chunk* parent, const char* buffer, unsigned int size) +void aml_destroy_node(struct aml_chunk* node) { + // Delete child nodes + struct aml_chunk* child = node->First; + + while (child) + { + struct aml_chunk* next = child->Next; + + if (child->Buffer) + free(child->Buffer); + + free(child); + + child = next; + } + + // Free node + if (node->Buffer) + free(node->Buffer); + + free(node); +} + +struct aml_chunk* aml_add_buffer(struct aml_chunk* parent, const char* buffer, unsigned int size) +{ struct aml_chunk* node = aml_create_node(parent); if (node) @@ -51,38 +91,29 @@ node->Type = AML_CHUNK_NONE; node->Length = size; node->Buffer = malloc(node->Length); - memcpy(node->Buffer, buffer, size); - - return node->Length; + memcpy(node->Buffer, buffer, node->Length); } - return -1; + return node; } -int aml_add_byte(struct aml_chunk* parent, unsigned char value) +struct aml_chunk* aml_add_byte(struct aml_chunk* parent, unsigned char value) { struct aml_chunk* node = aml_create_node(parent); if (node) { node->Type = AML_CHUNK_BYTE; + node->Length = 1; node->Buffer = malloc(node->Length); - - if (value == 0) - node->Buffer[0] = 0x00; - else if (value == 1) - node->Buffer[0] = 0x01; - else - node->Buffer[0] = value; - - return node->Length; + node->Buffer[0] = value; } - return -1; + return node; } -int aml_add_word(struct aml_chunk* parent, unsigned int value) +struct aml_chunk* aml_add_word(struct aml_chunk* parent, unsigned int value) { struct aml_chunk* node = aml_create_node(parent); @@ -93,14 +124,12 @@ node->Buffer = malloc(node->Length); node->Buffer[0] = value & 0xff; node->Buffer[1] = value >> 8; - - return node->Length; } - return -1; + return node; } -int aml_add_dword(struct aml_chunk* parent, unsigned long value) +struct aml_chunk* aml_add_dword(struct aml_chunk* parent, unsigned long value) { struct aml_chunk* node = aml_create_node(parent); @@ -113,14 +142,12 @@ node->Buffer[1] = (value >> 8) & 0xff; node->Buffer[2] = (value >> 16) & 0xff; node->Buffer[3] = (value >> 24) & 0xff; - - return node->Length; } - return -1; + return node; } -int aml_add_qword(struct aml_chunk* parent, unsigned long long value) +struct aml_chunk* aml_add_qword(struct aml_chunk* parent, unsigned long long value) { struct aml_chunk* node = aml_create_node(parent); @@ -137,62 +164,46 @@ node->Buffer[5] = (value >> 40) & 0xff; node->Buffer[6] = (value >> 48) & 0xff; node->Buffer[7] = (value >> 56) & 0xff; - - return node->Length; } - return -1; + return node; } -int aml_fill_simple_name(char* buffer, const char* name) +unsigned int aml_fill_simple_name(char* buffer, const char* name) { - int i, len = strlen(name), count = 0; - - for (i = 0; i < 4; i++) + if (strlen(name) < 4) { - if (i < len && aml_isvalidchar(name[i])) - { - buffer[count++] = name[i]; - } - else - { - buffer[3-i] = '_'; - } + verbose("aml_fill_simple_name: simple name %s has incorrect lengh! Must be 4", name); + return 0; } + memcpy(buffer, name, 4); return 4; } -int aml_fill_name(struct aml_chunk* node, const char* name) +unsigned int aml_fill_name(struct aml_chunk* node, const char* name) { if (!node) - return -1; + return 0; - int i, len = strlen(name), count = 0; + int len = strlen(name), offset = 0, count = len / 4; - for (i = 0; i < len; i++) + if ((len % 4) > 1 || count == 0) { - if (name[i] == '.') - { - count++; - } - else if (!aml_isvalidchar(name[i])) - { - len = i; - break; - } + verbose("aml_fill_name: pathname %s has incorrect length! Must be 4, 8, 12, 16 etc.", name); + return 0; } - if (count == 0 && len > 0) - count++; + unsigned int root = 0; - int offset = 0; - + if ((len % 4) == 1 && name[0] == '\\') + root++; + if (count == 1) { - node->Length = 4; + node->Length = 4 + root; node->Buffer = malloc(node->Length); - aml_fill_simple_name(node->Buffer, name); + memcpy(node->Buffer, name, 4 + root); return node->Length; } @@ -200,41 +211,37 @@ { node->Length = 2 + 8; node->Buffer = malloc(node->Length); - node->Buffer[offset++] = '\\'; // Root + node->Buffer[offset++] = 0x5c; // Root Char node->Buffer[offset++] = 0x2e; // Double name + memcpy(node->Buffer+offset, name + root, 8); + return node->Length; } - else - { - node->Length = 3 + count*4; - node->Buffer[offset++] = '\\'; // Root - node->Buffer[offset++] = 0x2f; // Multi name - node->Buffer[offset++] = count; // Names count - } - int j = 0; + node->Length = 3 + count*4; + node->Buffer = malloc(node->Length); + node->Buffer[offset++] = 0x5c; // Root Char + node->Buffer[offset++] = 0x2f; // Multi name + node->Buffer[offset++] = count; // Names count + memcpy(node->Buffer+offset, name + root, count*4); - for (i = 0; i < count; i++) + return node->Length; +} + +struct aml_chunk* aml_add_scope(struct aml_chunk* parent, const char* name) +{ + struct aml_chunk* node = aml_create_node(parent); + + if (node) { - offset += aml_fill_simple_name(node->Buffer + offset, name + j); + node->Type = AML_CHUNK_SCOPE; - while (name[j] != '.') - { - if (j < len) - { - j++; - } - else - { - verbose("aml_fill_name: unexpected end of names path!"); - return -1; - } - } + aml_fill_name(node, name); } - return offset; + return node; } -int aml_add_name(struct aml_chunk* parent, const char* name, int count, ...) +struct aml_chunk* aml_add_name(struct aml_chunk* parent, const char* name) { struct aml_chunk* node = aml_create_node(parent); @@ -243,25 +250,249 @@ node->Type = AML_CHUNK_NAME; aml_fill_name(node, name); + } + + return node; +} + +struct aml_chunk* aml_add_package(struct aml_chunk* parent) +{ + struct aml_chunk* node = aml_create_node(parent); + + if (node) + { + node->Type = AML_CHUNK_PACKAGE; - return node->Length; + node->Length = 1; + node->Buffer = malloc(node->Length); } - return -1; + return node; } -int aml_add_scope(struct aml_chunk* parent, const char* name) +struct aml_chunk* aml_add_alias(struct aml_chunk* parent, const char* name1, const char* name2) { struct aml_chunk* node = aml_create_node(parent); if (node) { - node->Type = AML_CHUNK_SCOPE; + node->Type = AML_CHUNK_ALIAS; - aml_fill_name(node, name); + node->Length = 8; + node->Buffer = malloc(node->Length); + aml_fill_simple_name(node->Buffer, name1); + aml_fill_simple_name(node->Buffer+4, name2); + } + + return node; +} + +unsigned char aml_get_size_length(unsigned int size) +{ + if (size + 1 <= 0x3f) + return 1; + else if (size + 2 <= 0x3fff) + return 2; + else if (size + 3 <= 0x3fffff) + return 3; + + return 4; +} + +unsigned int aml_calculate_size(struct aml_chunk* node) +{ + if (node) + { + node->Size = 0; - return node->Length; + // Calculate child nodes size + struct aml_chunk* child = node->First; + unsigned char child_count = 0; + + while (child) + { + child_count++; + + node->Size += aml_calculate_size(child); + + child = child->Next; + } + + switch (node->Type) + { + case AML_CHUNK_NONE: + node->Size += node->Length; + break; + case AML_CHUNK_SCOPE: + node->Size += 1 + node->Length; + node->Size += aml_get_size_length(node->Size); + break; + case AML_CHUNK_PACKAGE: + node->Buffer[0] = child_count; + node->Size += 1 + node->Length; + node->Size += aml_get_size_length(node->Size); + break; + + case AML_CHUNK_BYTE: + if (node->Buffer[0] == 0x0 || node->Buffer[0] == 0x1) + { + node->Size += node->Length; + } + else + { + node->Size += 1 + node->Length; + } + + break; + + case AML_CHUNK_WORD: + case AML_CHUNK_DWORD: + case AML_CHUNK_QWORD: + case AML_CHUNK_ALIAS: + case AML_CHUNK_NAME: + node->Size += 1 + node->Length; + break; + } + + return node->Size; } - return -1; + return 0; +} + +unsigned int aml_write_byte(unsigned char value, char* buffer, unsigned int offset) +{ + buffer[offset++] = value; + + return offset; +} + +unsigned int aml_write_word(unsigned int value, char* buffer, unsigned int offset) +{ + buffer[offset++] = value & 0xff; + buffer[offset++] = value >> 8; + + return offset; +} + +unsigned int aml_write_dword(unsigned long value, char* buffer, unsigned int offset) +{ + buffer[offset++] = value & 0xff; + buffer[offset++] = (value >> 8) & 0xff; + buffer[offset++] = (value >> 16) & 0xff; + buffer[offset++] = (value >> 24) & 0xff; + + return offset; +} + +unsigned int aml_write_qword(unsigned long long value, char* buffer, unsigned int offset) +{ + buffer[offset++] = value & 0xff; + buffer[offset++] = (value >> 8) & 0xff; + buffer[offset++] = (value >> 16) & 0xff; + buffer[offset++] = (value >> 24) & 0xff; + buffer[offset++] = (value >> 32) & 0xff; + buffer[offset++] = (value >> 40) & 0xff; + buffer[offset++] = (value >> 48) & 0xff; + buffer[offset++] = (value >> 56) & 0xff; + + return offset; +} + +unsigned int aml_write_buffer(const char* value, unsigned int size, char* buffer, unsigned int offset) +{ + if (size > 0) + { + memcpy(buffer + offset, value, size); + } + + return offset + size; +} + +unsigned int aml_write_size(unsigned int size, char* buffer, unsigned int offset) +{ + if (size <= 0x3f) + { + buffer[offset++] = size; + } + else if (size <= 0x3fff) + { + buffer[offset++] = 0x40 | (size & 0xf); + buffer[offset++] = (size >> 4) & 0xff; + } + else if (size <= 0x3fffff) + { + buffer[offset++] = 0x80 | (size & 0xf); + buffer[offset++] = (size >> 4) & 0xff; + buffer[offset++] = (size >> 12) & 0xff; + } + else + { + buffer[offset++] = 0xc0 | (size & 0xf); + buffer[offset++] = (size >> 4) & 0xff; + buffer[offset++] = (size >> 12) & 0xff; + buffer[offset++] = (size >> 20) & 0xff; + } + + return offset; +} + +unsigned int aml_write_node(struct aml_chunk* node, char* buffer, unsigned int offset) +{ + if (node && buffer) + { + unsigned int old = offset; + + switch (node->Type) + { + case AML_CHUNK_NONE: + offset = aml_write_buffer(node->Buffer, node->Length, buffer, offset); + break; + + case AML_CHUNK_SCOPE: + case AML_CHUNK_PACKAGE: + offset = aml_write_byte(node->Type, buffer, offset); + offset = aml_write_size(node->Size-1, buffer, offset); + offset = aml_write_buffer(node->Buffer, node->Length, buffer, offset); + break; + + case AML_CHUNK_BYTE: + if (node->Buffer[0] == 0x0 || node->Buffer[0] == 0x1) + { + offset = aml_write_buffer(node->Buffer, node->Length, buffer, offset); + } + else + { + offset = aml_write_byte(node->Type, buffer, offset); + offset = aml_write_buffer(node->Buffer, node->Length, buffer, offset); + } + break; + + case AML_CHUNK_WORD: + case AML_CHUNK_DWORD: + case AML_CHUNK_QWORD: + case AML_CHUNK_ALIAS: + case AML_CHUNK_NAME: + offset = aml_write_byte(node->Type, buffer, offset); + offset = aml_write_buffer(node->Buffer, node->Length, buffer, offset); + break; + + default: + break; + } + + struct aml_chunk* child = node->First; + + while (child) + { + offset = aml_write_node(child, buffer, offset); + + child = child->Next; + } + + if (offset - old != node->Size) + verbose("Node size incorrect: 0x%x\n", node->Type); + } + + return offset; } \ No newline at end of file Index: trunk/i386/libsaio/aml_generator.h =================================================================== --- trunk/i386/libsaio/aml_generator.h (revision 242) +++ trunk/i386/libsaio/aml_generator.h (revision 243) @@ -12,27 +12,27 @@ #include "libsaio.h" -enum aml_chunk_type -{ - AML_CHUNK_NONE = -1, - AML_CHUNK_ZERO = 0x00, - AML_CHUNK_ONE = 0x01, - AML_CHUNK_ALIAS = 0x06, - AML_CHUNK_NAME = 0x08, - AML_CHUNK_BYTE = 0x0A, - AML_CHUNK_WORD = 0x0B, - AML_CHUNK_DWORD = 0x0C, - AML_CHUNK_STRING = 0x0D, - AML_CHUNK_QWORD = 0x0E, - AML_CHUNK_SCOPE = 0x10, - AML_CHUNK_PACKAGE = 0x12, -}; +#define AML_CHUNK_NONE 0xff +#define AML_CHUNK_ZERO 0x00 +#define AML_CHUNK_ONE 0x01 +#define AML_CHUNK_ALIAS 0x06 +#define AML_CHUNK_NAME 0x08 +#define AML_CHUNK_BYTE 0x0A +#define AML_CHUNK_WORD 0x0B +#define AML_CHUNK_DWORD 0x0C +#define AML_CHUNK_STRING 0x0D +#define AML_CHUNK_QWORD 0x0E +#define AML_CHUNK_SCOPE 0x10 +#define AML_CHUNK_PACKAGE 0x12 struct aml_chunk { - enum aml_chunk_type Type; - unsigned long Length; + unsigned char Type; + unsigned int Length; char* Buffer; + + unsigned int Size; + struct aml_chunk* Next; struct aml_chunk* First; struct aml_chunk* Last; @@ -43,4 +43,19 @@ return isupper(c) || isdigit(c) || c == '_'; }; +bool aml_add_to_parent(struct aml_chunk* parent, struct aml_chunk* node); +struct aml_chunk* aml_create_node(struct aml_chunk* parent); +void aml_destroy_node(struct aml_chunk* node); +struct aml_chunk* aml_add_buffer(struct aml_chunk* parent, const char* buffer, unsigned int size); +struct aml_chunk* aml_add_byte(struct aml_chunk* parent, unsigned char value); +struct aml_chunk* aml_add_word(struct aml_chunk* parent, unsigned int value); +struct aml_chunk* aml_add_dword(struct aml_chunk* parent, unsigned long value); +struct aml_chunk* aml_add_qword(struct aml_chunk* parent, unsigned long long value); +struct aml_chunk* aml_add_scope(struct aml_chunk* parent, const char* name); +struct aml_chunk* aml_add_name(struct aml_chunk* parent, const char* name); +struct aml_chunk* aml_add_package(struct aml_chunk* parent); +struct aml_chunk* aml_add_alias(struct aml_chunk* parent, const char* name1, const char* name2); +unsigned int aml_calculate_size(struct aml_chunk* node); +unsigned int aml_write_node(struct aml_chunk* node, char* buffer, unsigned int offset); + #endif /* !__LIBSAIO_AML_GENERATOR_H */ \ No newline at end of file