param.c

Hermit-at-1.1.3,一款bootloader

#include <target/herrno.h>
#include <target/flash.h>
#include <target/str.h>
#include <target/io.h>
#include <target/mem.h>
#include <target/buffer.h>

#include "../arch/memregions.h"

#include <target/param.h>

#if defined(DEBUG)
#define DEBUG_INFO(args...) hprintf(args)
#else
#define DEBUG_INFO(args...)
#endif

static char _param_buf[HERMIT_PARAM_SIZE + 3];
static char *param_buf;

static u32
get_current_param_start_offset(void)
{
	u32 offset;
	u32 next;

	for (offset = PARAM_BOOT_START;
	     offset < PARAM_BOOT_START + PARAM_BOOT_SIZE;
	     offset += next) {
		next = *(u32 *)offset;
		if (next == 0xffffffff) {
			offset += sizeof(u32);
			break;
		}

		if (next == 0x0 || next & 0x3 || next >= PARAM_BOOT_SIZE) {
			/* broken parameter */
			flash_erase(HERMIT_PARAM_START);
			return -1;
		}		
	}
	if (offset >= (PARAM_BOOT_START + PARAM_BOOT_SIZE))
		return sizeof(u32);

	return (offset - PARAM_BOOT_START);
}

static u32
get_next_param_start_offset(void)
{
	u32 offset;
	u32 next;

	for (offset = PARAM_BOOT_START + get_current_param_start_offset();
	     offset < PARAM_BOOT_START + PARAM_BOOT_SIZE;
	     offset += sizeof(u32)) {
	  next = *(u32 *)offset;
	  if (next == 0xffffffff) {
	  	offset += sizeof(u32);
		break;
	  }
	}
	if (offset >= (PARAM_BOOT_START + PARAM_BOOT_SIZE))
		return -1;

	return (offset - PARAM_BOOT_START);
}

void
get_param(char **param, int count)
{
	u32 start_offset = get_current_param_start_offset();
	char *ptr = (char *)(PARAM_BOOT_START + start_offset);
	int i;

	for (i=0; i<count; i++) {
		param[i] = ptr;
		ptr += strlen(ptr);
		*(ptr++) = 0x00;/* NULL */
	}
}

int
get_param_count(void)
{
	u32 start_offset = get_current_param_start_offset();
	u8 *ptr;
	int detect = 1;
	int count = 0;

	DEBUG_INFO("start_offset: %p(%d)\n",
		   start_offset, start_offset);

	for (ptr = (u8 *)(PARAM_BOOT_START + start_offset);
	     ptr < (u8 *)(PARAM_BOOT_START + PARAM_BOOT_SIZE);
	     ptr++) {
		switch (*ptr) {
		case 0x00:
			detect = 1;
			break;
		case 0xff:
			return count;
		default:
			if (detect) {
				count++;
				detect = 0;
			}
			break;
		}
	}

	return -1;
}

int
check_param(char *label)
{
  int param_count = get_param_count();
  int i;
  {
    char *param[param_count];
    get_param(param, param_count);
    for (i=0; i<param_count; i++) {
      if (strncmp(param[i], label, strlen(label)) == 0)
	return 1;
    }
  }
  return 0;
}

void
param_partial_write(u32 start, u32 size, u8* buf, u32 erase)
{
	/* Load all parameter */
	flash_copy_to_dram(HERMIT_PARAM_START, 
			   (u32)param_buf, 
			   HERMIT_PARAM_SIZE);

	if (erase) {
		flash_erase(HERMIT_PARAM_START);
		if (erase & ERASE_PARAM_BOOT) {
			memset(&param_buf[PARAM_BOOT_START - 
					  HERMIT_PARAM_START],
			       0xff,
			       PARAM_BOOT_SIZE);
		}
		if (erase & ERASE_PARAM_REGION) {
			memset(&param_buf[PARAM_REGION_START - 
					  HERMIT_PARAM_START],
			       0xff,
			       PARAM_REGION_SIZE);
		}
	}
	memcpy(&param_buf[start], buf, size);

	if (erase) {
		flash_program((u32)param_buf,
			      HERMIT_PARAM_START,
			      HERMIT_PARAM_SIZE);
	} else {
		flash_program((u32)&param_buf[start & ~1],
			      HERMIT_PARAM_START + (start & ~1),
			      (size + (start & 1) + 1) & ~1);
	} 
}

static void
param_init(void)
{
	static int init = 0;
	if (init == 0) {
		param_buf = (char *)((u32)_param_buf & ~0x3);
		init = 1;
	}
}

/* DEBUG CODE */
#if 0
static int
param_cmdfunc(int argc, char *argv[])
{
	int count;
	int i;

	param_init();

	count = get_param_count();
	DEBUG_INFO("Param Count: %d\n", count);

	{
		char *param[count];
		get_param(param, count);
		for (i=0; i<count; i++)
		  hprintf("%d: %s\n", i + 1, param[i]);
	}

	return 0;
}
#endif

static int 
setparam_cmdfunc(int argc, char **argv)
{
	u32 current_offset = get_current_param_start_offset();
	u32 next_offset = get_next_param_start_offset();
	u8 *ptr = dlbuf;
	u32 args_len;
	int erase = 0;
	int i;

	param_init();

	DEBUG_INFO("Current Start Offset: %p\n", current_offset);
	DEBUG_INFO("Next Start Offset: %p\n", next_offset);

	for (i=1; i<argc; i++) {
		strcpy(ptr, argv[i]);
		ptr += strlen(argv[i]);
		*(ptr++) = 0x00; /* NULL */
	}
	for (; (u32)ptr%4;) {
		*(ptr++) = 0xff; /* Padding */
	}

	args_len = (u32)(ptr - dlbuf);

	if (next_offset == -1 ||
	    next_offset + args_len > PARAM_BOOT_SIZE) {
		erase = ERASE_PARAM_BOOT;
		next_offset = sizeof(u32);
	} else {
		/* Set Jump Record */
		*(u32 *)(ptr) = next_offset - current_offset;
		param_partial_write(current_offset - sizeof(u32),
				    sizeof(u32),
				    ptr,
				    0);
	}

	/* Program New Param */
	param_partial_write(next_offset,
			    args_len,
			    dlbuf,
			    erase);

	return 0;
}

int 
sethermit_param(int argc, char **argv, char *label)
{
	int param_count;
	int count = 0;
	int c_argc = 1;
	int label_len = strlen(label);
	int i;
	int setenv = 0, clearenv = 0;

	param_init();

	if (strcmp(label, "setenv") == 0)
		setenv = 1;
	else if (strcmp(label, "clearenv") == 0)
		clearenv = 1;

	param_count = get_param_count();

	{
	  char *param[param_count];
	  get_param(param, param_count);

	  /* Print Option */
	  if (setenv || clearenv) {
	    for (i=0; i<param_count; i++) {
	      if (param[i][0] != '@'){
		if (argc < 2 && setenv) {
		  hprintf("%d: %s\n", (count++) + 1, param[i]);
		} else {
		  count++;
		}
	      }
	    }
	  } else {
	    for (i=0; i<param_count; i++) {
	      if (strncmp(param[i], label, label_len) == 0) {
		if (argc < 2) {
		  /* label_len + "1" is '=' */
		  hprintf("%s: %s\n",
			  &label[1], &param[i][label_len + 1]);
		} else {
		  count = 1;
		}
		break;
	      }
	    }
	  }
	  
	  /* Renew Option */
	  if (argc > 1 || clearenv) {
	    char *c_argv[(param_count - count) + argc];
	    char device[64];
	    c_argv[0] = ""; /* set dummy */
	    
	    if (setenv | clearenv) {
	      /* Set other param */
	      for (i=0; i<param_count; i++)
		if (param[i][0] == '@')
		  c_argv[c_argc++] = param[i];
	      if (setenv) {
		/* Set new param */
		for (i=1; i<argc; i++)
		  c_argv[c_argc++] = argv[i];
	      }
	    } else {
	      /* Set other param */
	      for (i=0; i<param_count; i++)
		if (strncmp(param[i], label, label_len) != 0)
		  c_argv[c_argc++] = param[i];
	      /* Set new label param */
	      strcpy(device, label);
	      device[label_len] = '=';
	      for (i=0; i<strlen(argv[1]); i++)
		device[label_len + i + 1] = argv[1][i];
	      device[label_len + i + 1] = 0x00; /* NULL */
	      c_argv[c_argc++] = device;
	    }
	    
#if defined(DEBUG)
	    DEBUG_INFO("NEW_PARAMETER:\n");
	    for (i=1; i<c_argc; i++)
	      DEBUG_INFO("%d: %s\n", i, c_argv[i]);
#endif

	    setparam_cmdfunc(c_argc, c_argv);
	  }
	}
	
	return 0;
}

static int 
setenv_cmdfunc(int argc, char **argv)
{
	return sethermit_param(argc, argv, "setenv");
}

static int 
clearenv_cmdfunc(int argc, char **argv)
{
	return sethermit_param(argc, argv, "clearenv");
}

/* DEBUG CODE */
#if 0
const command_t param_command =
	{ "param", "", "display hermit parameters", &param_cmdfunc };

const command_t setparam_command =
	{ "setparam", "<hermit param>", "set hermit parameters",
	  &setparam_cmdfunc };
#endif

const command_t setenv_command =
	{ "setenv", "<linux options>", "set linux boot options", 
	  &setenv_cmdfunc };

const command_t clearenv_command =
	{ "clearenv", "", "clear linux boot options", &clearenv_cmdfunc };

COMMAND(setenv_command);
COMMAND(clearenv_command);