Syscon FW reader [FIX 2]

#include "tools.h"
#include "types.h"

#include <stdio.h>
#include <string.h>
#include <assert.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/stat.h>

u8 *pkg = NULL;
static u64 dec_size;
static u32 meta_offset;
static u32 n_sections;

u16 Get_Soft_ID(int);

static void unpack_firmware(const char *name)
{
    u8 *tmp;
    u8 *decompressed;
    u64 offset;
    u64 size;
    u64 size_real;

    tmp = pkg + meta_offset + 0x80 + 0x30 * 2;


    offset = be64(tmp);
    size = be64(tmp + 8);
    size_real = dec_size - 0x80;

    printf("[FW size] %u Bytes\n", (u32)size);

    if (be32(tmp + 0x2c) == 0x2) {
        decompressed = malloc(size_real);
        memset(decompressed, 0xaa, size_real);

        decompress(pkg + offset, size, decompressed, size_real);

        memcpy_to_file(name, decompressed, size_real);
    } else {
        memcpy_to_file(name, pkg + offset, size);
    }
}


static void decrypt_pkg(void)
{
    int j;

    u16 flags;
    u16 type;
    u32 hdr_len;

    u32 PatchID1;
    u32 PatchID2;
    u16 SoftID;

    struct keylist *k;

    flags    = be16(pkg + 0x08);
    type     = be16(pkg + 0x0a);
    hdr_len  = be64(pkg + 0x10);
    dec_size = be64(pkg + 0x18);
    SoftID   = be16(pkg + 0x28e);
    PatchID1 = be32(pkg + 0x290);
    PatchID2 = be32(pkg + 0x294);

    for(j=0;j<11;j++){
        if(SoftID==Get_Soft_ID(j)){
            printf("[Gen  %02i]", j+1);
            if(j<8) printf("[Phat]\n");
            else    printf("[Slim]\n");

            break;
        }if(j==10){
            printf("[New Gen][Slim]\n");
        }
    }


    printf("[Soft ID] %04X\n", SoftID);
    printf("[PatchID] %08X%08X\n", PatchID1, PatchID2);

    if (type != 3)
        fail("not a .pkg file");

    k = keys_get(KEY_PKG);

    if (k == NULL)
        fail("no key found");

    if (sce_decrypt_header(pkg, k) < 0)
        fail("pkg header decryption failed");

    if (sce_decrypt_data(pkg) < 0)
        fail("pkg data decryption failed");

    meta_offset = be32(pkg + 0x0c);
    n_sections  = be32(pkg + meta_offset + 0x60 + 0xc);

    if (n_sections != 3)
        fail("invalid section count: %d", n_sections);

}

u16 Get_Soft_ID(int i){
    u16 sc_soft_id[11];
    sc_soft_id[ 0] = 0x0B8E;
    sc_soft_id[ 1] = 0x0C16;
    sc_soft_id[ 2] = 0x0D52;
    sc_soft_id[ 3] = 0x0DBF;
    sc_soft_id[ 4] = 0x0E69;
    sc_soft_id[ 5] = 0x0F29;
    sc_soft_id[ 6] = 0x0F38;
    sc_soft_id[ 7] = 0x065D;
    sc_soft_id[ 8] = 0x0832;
    sc_soft_id[ 9] = 0x08C2;
    sc_soft_id[10] = 0x0918;

    return sc_soft_id[i];
}

void readFW(void)
{
    u32 sc_header = 0x1b2d700f ;

    u32 hdr;
    u32 A[8],D[3];

    hdr = be32(pkg);

    A[0]    = be32(pkg + 0x4);
    A[1]    = be32(pkg + 0x8);
    A[2]    = be32(pkg + 0xc);
    A[3]    = be32(pkg + 0x10);
    A[4]    = be32(pkg + 0x14);
    A[5]    = be32(pkg + 0x18);
    A[6]    = be32(pkg + 0x1c);
    A[7]    = be32(pkg + 0x20);

//  This Offset is static.. i don't know what it is...
    D[0]    = be32(pkg + 0x24);
    D[1]    = be32(pkg + 0x28);
    D[2]    = be32(pkg + 0x2c);

    if(hdr!=sc_header)
        printf("[New HDR] %08x \n", hdr);
    else
        printf("[SC  HDR] %08x \n", hdr);

    printf("[CHKSUM?] %08x %08x %08x %08x \n          %08x %08x %08x %08x\n", A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7]);
//  printf("[   B   ] [HEX] %08x\n        [DEC] %u\n", B, B);
    printf("[?static] %08x%08x%08x\n", D[0], D[1], D[2]);


}

int main(int argc, char *argv[])
{
    if (argc == 2) {
        pkg = mmap_file(argv[1]);
        printf("\n[Name FW] %s\n", argv[1]);
        decrypt_pkg();
        unpack_firmware("temp.sc");
        pkg = mmap_file("temp.sc");
        readFW();

        if(remove("temp.sc") != 0)
            fail("Error deleting temp file.");

    } else {
        fail("usage: unpkg syscon_firmware.pkg");
    }


    return 0;
}