restore_ifs.c

qnx powerpc MPC8245的 BSP源文件

			{
				if(debug_flag > RIFS_DEBUG_LEVEL)	
				{
					kprintf("WARNING: Checksum failed on image!\n");
				}
				// Checksum failed - IFS is corrupt
				status = -1;
			}
		}
		else
		{
			if(debug_flag > RIFS_DEBUG_LEVEL)	
			{
				kprintf("WARNING: Skipped image checksum verification\n");
			}
		}
	}
	else
	{
		// No IFS or invalid restore data
		status = -1;
	}
	
	// Restore info is not initialized until after we have checked the old data
	rifs_init(rifs_info);
	
	if((status == -1) && (debug_flag > RIFS_DEBUG_LEVEL))
	{
		kprintf("Restore IFS failed - Reload entire IFS.\n");
	}
	
	return(status);
}

// Save writeable data section of ELF executables
int rifs_save_elf32data(paddr32_t addr, union image_dirent *dir, int numboot)
{
	Elf32_Phdr 				*phdr;
	
	// Make sure the number of bootable executables isn't greater than the max	
	if(numboot >= RIFS_MAX_BOOTABLE)
		return(-1);
		
	// Read the ELF header	
	if((phdr = rifs_readelf(addr)))
	{
		if(debug_flag > RIFS_DEBUG_LEVEL)	
		{
			kprintf("Found procnto Elf header\n");
			kprintf("bootable exec data: offset %x, size %x\n", phdr->p_offset, phdr->p_filesz );
		}
		// Increment the number of bootable images found and save the related info
		rifs_info->numboot++;
		rifs_info->elfinfo[numboot].offset = dir->file.offset + phdr->p_offset;
		rifs_info->elfinfo[numboot].size = phdr->p_filesz;
	
		// If the image is compressed, save the data 
		if(shdr->flags1 & STARTUP_HDR_FLAGS1_COMPRESS_MASK)
		{
			if(debug_flag > RIFS_DEBUG_LEVEL)	
			{
				kprintf("Compressed image, store data\n");
			}
		
			// Allocate storage space. 
			// NOTE: We assume that the address will be the same everytime.
			// This should OK since the alloc_ram/find_ram algorithm is deterministic.
			rifs_info->elfinfo[numboot].data = alloc_ram(NULL_PADDR, rifs_info->elfinfo[numboot].size, sizeof(uint64_t));
			// Save a copy of the data	
			// NOTE: Use copy_memory to support mini-drivers
			copy_memory(rifs_info->elfinfo[numboot].data, 
					shdr->image_paddr + shdr->startup_size + rifs_info->elfinfo[numboot].offset, 
					rifs_info->elfinfo[numboot].size);
		}
	}
	else
	{
		if(debug_flag > RIFS_DEBUG_LEVEL)	
		{
			kprintf("Invalid ELF header\n");
		}
		// Error reading ELF header
		return(-1);
	}
	
	return(0);
}

// Calculate the checksum for the restore info and save the IFS's size
void rifs_set_cksum(struct image_header *ifs_hdr)
{
	if(debug_flag > RIFS_DEBUG_LEVEL)	
	{
		kprintf("Calculate restore info checksum\n");
	}
	// Save the image size to be used for the image checksum
	rifs_info->image_size = ifs_hdr->image_size;
	
	// Calculate the restore checksum such that a checksum will result in 0
	rifs_info->cksum = 0xFFFFFFFF & (0x100000000ULL - rifs_checksum(rifs_info, sizeof(struct restore_ifs_info)));
}

// Process the ELF header to find the location of the writeable data 
static Elf32_Phdr *rifs_readelf(paddr32_t paddr)
{
	uint8_t			*ptr;
	Elf32_Ehdr		*ehdr;
	Elf32_Phdr		*phdr;
	Elf32_Off		off;
	int				i;
	
	ptr = MAKE_1TO1_PTR(paddr);
	ehdr = (Elf32_Ehdr *)ptr;
	
	// Verify ELF header	
	if(	ehdr->e_ident[EI_MAG0] !=  ELFMAG0 ||
		ehdr->e_ident[EI_MAG1] !=  ELFMAG1 ||
		ehdr->e_ident[EI_MAG2] !=  ELFMAG2 ||
		ehdr->e_ident[EI_MAG3] !=  ELFMAG3 )
	{
		return NULL;
	}
		
	// Search through the ELF header for the writeable data information	
	off = ehdr->e_phoff;
	for(i = 0; i < ehdr->e_phnum; i++)
	{
		// Seek to the next type
		phdr = (Elf32_Phdr *)(ptr + off);
		// Look for PT_LOAD type
		if(phdr->p_type == PT_LOAD)
		{
			// Find PT_LOAD type marked as writeable
			if(phdr->p_flags & PF_W)
			{
				if(debug_flag > RIFS_DEBUG_LEVEL)	
				{
					kprintf("PT_LOAD RW: %x size is %x\n", phdr->p_offset, phdr->p_filesz );
				}
				// Found the match, return it to the caller
				return phdr;
			}
		}
		// Skip to the next type
		off += ehdr->e_phentsize;
	}
	
	return NULL;
}

// Calculate the sum of an array of 4byte numbers
static int rifs_checksum(void *ptr, long len)
{
	int			*data = (int *)ptr;
	long		mdriver_count = 0;
	int 		sum;
	unsigned	max;

	// The checksum may take a while for large images, so we want to poll the mini-driver
	max = (lsp.mdriver.size > 0) ? mdriver_cksum_max : len;
	
	sum = 0;
	while(len > 0)
	{
		sum += *data++;
		len -= 4;
		mdriver_count += 4;
		if(mdriver_count >= max) 
		{
			// Poll the mini-driver when we reach the limit
			mdriver_check();
			mdriver_count = 0;	
		}
	}
	return(sum);
}

// Initialize the restore info data structure
static void rifs_init(struct restore_ifs_info *rifs_info)
{
	char		sig[8] = RIFS_SIGNATURE; 
	int			i;
	
	for(i = 0; i < 8; i++)
	{
		rifs_info->signature[i] = sig[i];
	}
	rifs_info->cksum = 0;
	rifs_info->numboot = 0;
	rifs_info->image_size = 0;
	
	// Zero out bootable executable info	
	for(i = 0; i < RIFS_MAX_BOOTABLE; i++)
	{
		rifs_info->elfinfo[i].offset = 0;
		rifs_info->elfinfo[i].size = 0;
		rifs_info->elfinfo[i].data = 0;
	}
}

// Validate the restore info data structure
static int check_rifs_signature(struct restore_ifs_info *rifs_info)
{
	char	sig[8] = RIFS_SIGNATURE; 
	int		cksum;
	int		i;
	
	// We don't access the restore ifs info pointer until the data structure 
	// is validated with the checksum.
	
	// Verify the signature	
	for(i = 0; i < 8; i++)
	{
		if(sig[i] != *((char *)rifs_info + i))
		{
			if(debug_flag > RIFS_DEBUG_LEVEL)	
			{
				kprintf("INVALID RIFS signature\n");
			}
			// Signature does not match!
			return(-1);
		}
	}
	if(debug_flag > RIFS_DEBUG_LEVEL)	
	{
		kprintf("FOUND valid RIFS signature\n");
	}
	
	// Calculate the checksum on the rifs info data structure	
	cksum = rifs_checksum(rifs_info, sizeof(struct restore_ifs_info));
	if(cksum != 0)
	{
		if(debug_flag > RIFS_DEBUG_LEVEL)	
		{
			kprintf("INVALID RIFS info cksum\n");
		}
		// Invalid checksum, data structure is corrupt
		return(-1);
	}
	
	if(debug_flag > RIFS_DEBUG_LEVEL)	
	{
		kprintf("FOUND valid RIFS info\n");
	}
	// Found valid restore ifs data	
	return(0);
}

// Validate the IFS signature
static int check_ifs_signature(struct image_header	*ifs_hdr)
{
	char	sigifs[7] = IMAGE_SIGNATURE;
	int		i;
	
	// Verify the signature	
	for(i = 0; i < 7; i++)
	{
		if(sigifs[i] != *((char *)ifs_hdr + i))
		{
			if(debug_flag > RIFS_DEBUG_LEVEL)	
			{
				kprintf("INVALID IFS signature\n");
			}
			// Signature does not match!
			return(-1);
		}
	}
	
	if(debug_flag > RIFS_DEBUG_LEVEL)	
	{
		kprintf("FOUND valid IFS signature\n");
	}
	// Found valid IFS signature
	return(0);
}