#!/usr/bin/env python3
"""
Enhanced RISC-V function finder for GSP firmware.
Searches for multiple prologue patterns.
"""

import struct
import json
import os

FWIMAGE_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "gsp-570.144.fwimage.bin")
OUTPUT_DIR = os.path.dirname(os.path.abspath(__file__))
BASE_ADDR = 0x04000000

def read_binary(path):
    with open(path, 'rb') as f:
        return f.read()

def find_functions(data, base_addr):
    functions = []
    
    # Pattern 1: Standard 32-bit prologue
    # addi sp, sp, -imm (opcode=0x13, funct3=0, rd=2, rs1=2)
    # sd ra, offset(sp) (opcode=0x23, funct3=3, rs1=2, rs2=1)
    for i in range(0, len(data) - 8, 2):
        if i + 8 <= len(data):
            inst1 = struct.unpack('<I', data[i:i+4])[0]
            inst2 = struct.unpack('<I', data[i+4:i+8])[0]
            
            # addi sp, sp, -imm
            if (inst1 & 0x707F) == 0x0013:
                rd = (inst1 >> 7) & 0x1F
                rs1 = (inst1 >> 15) & 0x1F
                if rd == 2 and rs1 == 2:
                    imm = (inst1 >> 20) & 0xFFF
                    if imm >= 0x800:
                        imm = imm - 0x1000
                    
                    # sd ra, offset(sp)
                    if (inst2 & 0x707F) == 0x23:
                        funct3 = (inst2 >> 12) & 0x7
                        rs1_2 = (inst2 >> 15) & 0x1F
                        rs2 = (inst2 >> 20) & 0x1F
                        if funct3 == 3 and rs1_2 == 2 and rs2 == 1:
                            addr = base_addr + i
                            functions.append({
                                "address": f"0x{addr:08x}",
                                "offset": i,
                                "stack_size": -imm,
                                "pattern": "std_prologue_addi_sd"
                            })
                            continue
    
    # Pattern 2: Compressed prologue
    # c.addi sp, sp, imm
    # c.sd ra, offset(sp)
    for i in range(0, len(data) - 4, 2):
        if i + 4 <= len(data):
            inst1 = struct.unpack('<H', data[i:i+2])[0]
            inst2 = struct.unpack('<H', data[i+2:i+4])[0]
            
            # c.addi sp, sp, imm
            if (inst1 & 0xE003) == 0x6001:
                rd_prime = (inst1 >> 7) & 0x7
                if rd_prime == 2:
                    # c.sd ra, offset(sp)
                    if (inst2 & 0xE003) == 0xE000:
                        rs1_prime = (inst2 >> 7) & 0x7
                        rs2_prime = (inst2 >> 10) & 0x7
                        if rs1_prime == 2 and rs2_prime == 1:
                            addr = base_addr + i
                            functions.append({
                                "address": f"0x{addr:08x}",
                                "offset": i,
                                "pattern": "compressed_prologue"
                            })
                            continue
    
    # Pattern 3: Standard prologue with s0 save
    # addi sp, sp, -N; sd ra, N-8(sp); sd s0, N-16(sp); addi s0, sp, N
    for i in range(0, len(data) - 16, 2):
        if i + 16 <= len(data):
            inst1 = struct.unpack('<I', data[i:i+4])[0]
            inst2 = struct.unpack('<I', data[i+4:i+8])[0]
            inst3 = struct.unpack('<I', data[i+8:i+12])[0]
            inst4 = struct.unpack('<I', data[i+12:i+16])[0]
            
            # addi sp, sp, -N
            if (inst1 & 0x707F) == 0x0013:
                rd = (inst1 >> 7) & 0x1F
                rs1 = (inst1 >> 15) & 0x1F
                if rd == 2 and rs1 == 2:
                    imm = (inst1 >> 20) & 0xFFF
                    if imm >= 0x800:
                        imm = imm - 0x1000
                    
                    # sd ra, N-8(sp)
                    if (inst2 & 0x707F) == 0x23:
                        funct3 = (inst2 >> 12) & 0x7
                        rs1_2 = (inst2 >> 15) & 0x1F
                        rs2 = (inst2 >> 20) & 0x1F
                        if funct3 == 3 and rs1_2 == 2 and rs2 == 1:
                            # sd s0, N-16(sp)
                            if (inst3 & 0x707F) == 0x23:
                                funct3_3 = (inst3 >> 12) & 0x7
                                rs1_3 = (inst3 >> 15) & 0x1F
                                rs2_3 = (inst3 >> 20) & 0x1F
                                if funct3_3 == 3 and rs1_3 == 2 and rs2_3 == 8:  # s0 = x8
                                    # addi s0, sp, N
                                    if (inst4 & 0x707F) == 0x0013:
                                        rd4 = (inst4 >> 7) & 0x1F
                                        rs1_4 = (inst4 >> 15) & 0x1F
                                        if rd4 == 8 and rs1_4 == 2:
                                            addr = base_addr + i
                                            functions.append({
                                                "address": f"0x{addr:08x}",
                                                "offset": i,
                                                "stack_size": -imm,
                                                "pattern": "full_prologue_ra_s0"
                                            })
                                            continue
    
    # Pattern 4: Just sd ra, offset(sp) followed by code
    # Many functions might just save ra
    for i in range(0, len(data) - 4, 2):
        if i + 4 <= len(data):
            inst = struct.unpack('<I', data[i:i+4])[0]
            
            # sd ra, offset(sp)
            if (inst & 0x707F) == 0x23:
                funct3 = (inst >> 12) & 0x7
                rs1 = (inst >> 15) & 0x1F
                rs2 = (inst >> 20) & 0x1F
                if funct3 == 3 and rs1 == 2 and rs2 == 1:
                    # Check if previous instruction was addi sp, sp (within 8 bytes)
                    has_addi_sp = False
                    stack_size = 0
                    for j in range(max(0, i-8), i, 2):
                        if j >= 0:
                            prev = struct.unpack('<I', data[j:j+4])[0]
                            if (prev & 0x707F) == 0x0013:
                                rd = (prev >> 7) & 0x1F
                                rs1_p = (prev >> 15) & 0x1F
                                if rd == 2 and rs1_p == 2:
                                    imm = (prev >> 20) & 0xFFF
                                    if imm >= 0x800:
                                        imm = imm - 0x1000
                                    has_addi_sp = True
                                    stack_size = -imm
                                    break
                    
                    if has_addi_sp:
                        addr = base_addr + i
                        # Avoid duplicates
                        dup = False
                        for f in functions:
                            if f["offset"] == i:
                                dup = True
                                break
                        if not dup:
                            functions.append({
                                "address": f"0x{addr:08x}",
                                "offset": i,
                                "stack_size": stack_size,
                                "pattern": "sd_ra_with_addi_sp"
                            })
    
    # Pattern 5: Jump/return instructions that might indicate function boundaries
    # ret = jalr zero, 0(ra) = 0x00008067
    # jr ra = jalr zero, 0(ra) (same)
    for i in range(0, len(data) - 4, 2):
        if i + 4 <= len(data):
            inst = struct.unpack('<I', data[i:i+4])[0]
            if inst == 0x00008067:  # ret / jalr zero, 0(ra)
                addr = base_addr + i
                functions.append({
                    "address": f"0x{addr:08x}",
                    "offset": i,
                    "pattern": "return_instruction"
                })
    
    return functions

def main():
    print(f"Reading firmware binary: {FWIMAGE_PATH}")
    data = read_binary(FWIMAGE_PATH)
    print(f"Binary size: {len(data)} bytes")
    
    print("Searching for function prologues...")
    functions = find_functions(data, BASE_ADDR)
    
    print(f"Found {len(functions)} potential functions")
    
    # Deduplicate by address
    seen = set()
    unique = []
    for f in functions:
        if f["address"] not in seen:
            seen.add(f["address"])
            unique.append(f)
    
    # Sort by address
    unique.sort(key=lambda x: x["offset"])
    
    print(f"Unique functions: {len(unique)}")
    
    # Save results
    with open(f"{OUTPUT_DIR}/found_functions_v2.json", "w") as f:
        json.dump(unique, f, indent=2)
    
    with open(f"{OUTPUT_DIR}/function_addresses_v2.txt", "w") as f:
        for func in unique:
            ss = func.get("stack_size", "?")
            f.write(f"{func['address']}  stack_size={ss}  pattern={func['pattern']}\n")
    
    print(f"Results saved to {OUTPUT_DIR}/found_functions_v2.json")
    
    # Show all
    for func in unique:
        ss = func.get("stack_size", "?")
        print(f"  {func['address']}  stack_size={ss}  pattern={func['pattern']}")

if __name__ == "__main__":
    main()