common/cmd_hnvram.c - uboot/qsr1000 - Git at Google

 /*
  * cmd_hnvram.c -- makes hnvram contents available in u-boot.
  *      Loads contents of hnvram from spi flash and saves it
  *      to environment variables named HNV_<name>.
  *
  * Copyright (C) 2015 Google Inc.
  * Author: Chris Gibson <cgibson@google.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation; either version 2 of the License, or
  * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  * General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */

 #include <command.h>
 #include <common.h>
 #include <malloc.h>
 #include <spi_flash.h>
 #include <asm/byteorder.h>

 /* local debug macro */
 #undef HNVRAM_DEBUG

 #ifdef HNVRAM_DEBUG
 #define DEBUG(fmt, args...)  printf(fmt, ##args)
 #else
 #define DEBUG(fmt, args...)
 #endif  /* HNVRAM_DEBUG */

 #define CONFIG_SF_DEFAULT_MODE SPI_MODE_3


 // Flash erase block size
 #define HNVRAM_BLOCKSIZE 0x00010000

 // Location in DRAM where hnvram variables are stored during boot
 // DRAM is 128 MB [0x80000000 - 0x88000000]
 // Store hnvram at 120 - 122 MB
 #define HNVRAM_DRAM_OFFSET 0x87800000

 // Where in flash does hnvram partition start
 #define HNVRAM_MTD_OFFSET 0x00200000

 // Total hnvram is 2MB with 4 partitions
 #define MAX_HNVRAM_SIZE     0x00200000

 #define HNVRAM_RO_SIZE      0x00100000
 #define HNVRAM_RW_SIZE      0x00040000
 #define HNVRAM_RWB_SIZE     0x00020000
 #define HNVRAM_RAW_FS_SIZE  0x00020000

 // B1 and B2 corresponding to RW_OFFSET and RWB_OFFSET
 #define HNVRAM_B1_OFFSET  (HNVRAM_RO_SIZE)
 #define HNVRAM_B2_OFFSET  (HNVRAM_RO_SIZE + HNVRAM_RW_SIZE)

 #define CMD_RET_SUCCESS 0
 #define CMD_RET_FAILURE 1
 #define CMD_RET_USAGE -1

 // These keys are stored in binary format for historical reasons
 const char *hnvram_binary_keys[] = {
         "LOADER_VERSION",
         "HDCP_KEY",
         "DTCP_KEY",
 };

 static void *xmalloc(size_t size)
 {
         void *p = NULL;
         if (!(p = malloc(size))) {
                 printf("error: memory not allocated\n");
                 return 0;
         }
         memset(p, 0, size);
         return p;
 }

 int read_u8(const char **p)
 {
         int v = *(const unsigned char *)(*p);
         *p += 1;
         return v;
 }

 int read_s32_be(const char **p)
 {
         const unsigned char *vp = (const unsigned char *)*p;
         *p += 4;
         return (vp[0]<<24) + (vp[1]<<16) + (vp[2]<<8) + vp[3];
 }

 int read_u16_le(const char **p)
 {
         const unsigned char *up = (const unsigned char *)(*p);
         *p += 2;
         return up[0] + (up[1] << 8);
 }

 int is_hnvram_binary(const char *name, int namelen)
 {
         int i;
         for (i = 0; i < ARRAY_SIZE(hnvram_binary_keys); i++) {
                 const char *k = hnvram_binary_keys[i];
                 if ((int)strlen(k) == namelen && strncmp(k, name, namelen) == 0) {
                         return 1;
                 }
         }
         return 0;
 }

 char *encode_hex(const char *s, int len)
 {
         char *optr, *out = xmalloc(len * 2 + 1);
         for (optr = out; len > 0; len--) {
                 sprintf(optr, "%02x", read_u8(&s));
                 optr += 2;
         }
         return out;
 }

 char *encode_macaddr(const char *mac)
 {
         int i;
         char *out = xmalloc(6 * 2 + 5 + 2);
         for (i = 0; i < 6; i++) {
                 sprintf(out + i * 3, "%02X:", read_u8(&mac));
         }
         out[6*2 + 5] = '\0';
         return out;
 }

 static void _copy_setenv(const char *name, int namelen,
                 const char *val, int vallen)
 {
         char *n, *v;
         if (namelen + vallen < 128) {
                 n = xmalloc(4 + namelen + 1);
                 v = xmalloc(vallen + 1);
                 memcpy(n, "HNV_", 4);
                 memcpy(n + 4, name, namelen);
                 memcpy(v, val, vallen);
                 n[namelen+4] = 0;
                 v[vallen] = 0;
                 setenv(n, v);
                 free(n);
                 free(v);
         } else {
                 DEBUG("ignoring oversized val: %.15s, vallen: %d\n", val, vallen);
         }
 }

 static int _parse_hnvram(const char *buf, int len)
 {
         // An hnvram structure. Format is a tag-length-value sequence of:
         //    [1 byte]   type (1 for notdone, 0 for done)
         //    [4 bytes]  record length
         //    [1 byte]   key length
         //    [x bytes]  key
         //    [4 bytes]  value length
         //    [y bytes]  value
         int rectype, reclen, namelen, vallen;
         int done = 0;
         const char *name, *val, *p = buf;
         while (p - buf <= len + 11) {
                 rectype = read_u8(&p);
                 if (rectype == 0x00) {
                         printf("done processing hnvram block!\n");
                         done = 1;
                         break;
                 }
                 if (rectype != 0x01) {
                         printf("error: hnvram invalid rectype %x\n", rectype);
                         return -1;
                 }

                 reclen = read_s32_be(&p);
                 if (reclen <= 6 || (p - buf) + reclen >= len) {
                         printf("error: hnvram invalid reclen %d\n", reclen);
                         return -1;
                 }
                 namelen = read_u8(&p);
                 if (namelen < 1 || (p - buf) + namelen >= len) {
                         printf("error: hnvram invalid namelen %d\n", namelen);
                         return -1;
                 }
                 name = p;
                 p += namelen;
                 vallen = read_s32_be(&p);
                 if (vallen < 0 || (p - buf) + vallen >= len) {
                         printf("error: hnvram invalid vallen %d\n", vallen);
                         return -1;
                 }
                 val = p;
                 p += vallen;
                 if (vallen == 6 && namelen >= 8 &&
                                 strncmp("MAC_ADDR", name, 8) == 0) {
                         char *macstr = encode_macaddr(val);
                         _copy_setenv(name, namelen, macstr, strlen(macstr));
                         free(macstr);
                 } else if (is_hnvram_binary(name, namelen)) {
                         char *hexstr = encode_hex(val, vallen);
                         _copy_setenv(name, namelen, hexstr, strlen(hexstr));
                         free(hexstr);
                 } else {
                         _copy_setenv(name, namelen, val, vallen);
                 }
         }
         if (!done) {
                 printf("error: failed to find final hnvram record?\n");
                 return -1;
         }
         return 0;
 }

 #if defined(CONFIG_CMD_HNVRAM)
 int do_hnvram_load(void) {
         char command[60];
         sprintf(command, "spi_flash read 0x%x 0x%x 0x%x", HNVRAM_MTD_OFFSET, HNVRAM_DRAM_OFFSET, MAX_HNVRAM_SIZE);
         int ret = run_command(command, 0);

         if (ret) {
           printf("failed reading from spi-flash at addr: %d\n", HNVRAM_MTD_OFFSET);
           return ret;
         }

         char *buf = HNVRAM_DRAM_OFFSET;

         // Next step: Seek to different parts of the buffer that contain the
         // first, second, and third sections of hnvram.
         if (_parse_hnvram(buf, HNVRAM_BLOCKSIZE) != 0) {
                 printf("failed parsing hnvram at offset: 0x%p\n", buf);
                 return -1;
         }
         if (_parse_hnvram(buf+HNVRAM_B1_OFFSET, HNVRAM_BLOCKSIZE) != 0) {
                 printf("failed parsing hnvram at offset: 0x%p\n", buf+HNVRAM_B1_OFFSET);
                 return -1;
         }
         if (_parse_hnvram(buf+HNVRAM_B2_OFFSET, HNVRAM_BLOCKSIZE) != 0) {
                 printf("failed parsing hnvram at offset: 0x%p\n", buf+HNVRAM_B2_OFFSET);
                 return -1;
         }

         return CMD_RET_SUCCESS;
 }

 int do_hnvram_init(char *val) {
         // Erase entire partition
         char command[60];
         sprintf(command, "spi_flash erase 0x%x 0x%x", HNVRAM_MTD_OFFSET, MAX_HNVRAM_SIZE);
         int ret = run_command(command, 0);

         if (ret) {
           printf("failed to erase hnvram partition\n");
           return ret;
         }

         char key[] = "PLATFORM_NAME";
         int val_len = strlen(val);
         // size variable should fit in one byte
         if (val_len < 1 || 255 < val_len) {
           printf("Intial value for %s has bad size: %d", key, val_len);
           return CMD_RET_FAILURE;
         }
         // +5 to count key length (1) and record length (4) bytes
         int rec_len = sizeof(key) + val_len + 5;

         // Use 2MB of (hopefully) free DRAM
         char *buf = HNVRAM_DRAM_OFFSET;
         memset(buf, 0x00, MAX_HNVRAM_SIZE);

         // Write platform_name to RW section
         int i = HNVRAM_B1_OFFSET;

         // Record start
         buf[i++] = 0x01;

         // Record length
         int nbyte_order = htonl(rec_len); // hnvram expects big endian
         memcpy(buf + i, &nbyte_order, sizeof(nbyte_order));
         i += sizeof(nbyte_order);

         // Key length
         buf[i++] = sizeof(key);

         // Key
         memcpy(buf + i, &key, sizeof(key));
         i += sizeof(key);

         // Val length
         nbyte_order = htonl(val_len);
         memcpy(buf + i, &nbyte_order, sizeof(nbyte_order));
         i += sizeof(nbyte_order);

         // Val
         memcpy(buf + i, val, val_len);

         sprintf(command, "spi_flash write 0x%x 0x%p 0x%x", HNVRAM_MTD_OFFSET, buf, MAX_HNVRAM_SIZE);
         ret = run_command(command, 0);

         if (ret) {
           printf("failed to write init to hnvram partition\n");
           return ret;
         }

         // Load platform_name into environment variables
         do_hnvram_load();

         return CMD_RET_SUCCESS;
 }

 static int do_hnvram_cmd(cmd_tbl_t *cmdtp, int flag, int argc,
                 char * const argv[]) {
         if (argc == 2 && strcmp(argv[1], "load") == 0) {
           return do_hnvram_load();
         } else if (argc == 2 && strcmp(argv[1], "init") == 0) {
           return do_hnvram_init("GFRG240");
         } else if (argc == 3 && strcmp(argv[1], "init") == 0) {
           return do_hnvram_init(argv[2]);
         } else {
           printf("hnvram - incorrect usage. Please use one of:\n");
           printf("hnvram load\n");
           printf("hnvram init <model>\n");
           return CMD_RET_USAGE;
         }
 }

 U_BOOT_CMD(
         hnvram, 3, 0, do_hnvram_cmd,
         "HNVRAM key-val storage on flash",
         "\n"
         "hnvram load            - reads all variables from flash partition and"
         "                         places into environment vars as HNV_<name>"
         "hnvram init [model]    - zeros hnvram and writes PLATFORM_NAME=<model>"
         "                         Uses GFRG240 by default if no arg given"
         );
 #endif  /* CONFIG_CMD_HNVRAM */
	/*
	* cmd_hnvram.c -- makes hnvram contents available in u-boot.
	* Loads contents of hnvram from spi flash and saves it
	* to environment variables named HNV_<name>.
	*
	* Copyright (C) 2015 Google Inc.
	* Author: Chris Gibson <cgibson@google.com>
	*
	* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License as published by
	* the Free Software Foundation; either version 2 of the License, or
	* (at your option) any later version.
	*
	* This program is distributed in the hope that it will be useful,
	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	* General Public License for more details.
	*
	* You should have received a copy of the GNU General Public License
	* along with this program; if not, write to the Free Software
	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
	*/

	#include <command.h>
	#include <common.h>
	#include <malloc.h>
	#include <spi_flash.h>
	#include <asm/byteorder.h>

	/* local debug macro */
	#undef HNVRAM_DEBUG

	#ifdef HNVRAM_DEBUG
	#define DEBUG(fmt, args...) printf(fmt, ##args)
	#else
	#define DEBUG(fmt, args...)
	#endif /* HNVRAM_DEBUG */

	#define CONFIG_SF_DEFAULT_MODE SPI_MODE_3


	// Flash erase block size
	#define HNVRAM_BLOCKSIZE 0x00010000

	// Location in DRAM where hnvram variables are stored during boot
	// DRAM is 128 MB [0x80000000 - 0x88000000]
	// Store hnvram at 120 - 122 MB
	#define HNVRAM_DRAM_OFFSET 0x87800000

	// Where in flash does hnvram partition start
	#define HNVRAM_MTD_OFFSET 0x00200000

	// Total hnvram is 2MB with 4 partitions
	#define MAX_HNVRAM_SIZE 0x00200000

	#define HNVRAM_RO_SIZE 0x00100000
	#define HNVRAM_RW_SIZE 0x00040000
	#define HNVRAM_RWB_SIZE 0x00020000
	#define HNVRAM_RAW_FS_SIZE 0x00020000

	// B1 and B2 corresponding to RW_OFFSET and RWB_OFFSET
	#define HNVRAM_B1_OFFSET (HNVRAM_RO_SIZE)
	#define HNVRAM_B2_OFFSET (HNVRAM_RO_SIZE + HNVRAM_RW_SIZE)

	#define CMD_RET_SUCCESS 0
	#define CMD_RET_FAILURE 1
	#define CMD_RET_USAGE -1

	// These keys are stored in binary format for historical reasons
	const char *hnvram_binary_keys[] = {
	"LOADER_VERSION",
	"HDCP_KEY",
	"DTCP_KEY",
	};

	static void *xmalloc(size_t size)
	{
	void *p = NULL;
	if (!(p = malloc(size))) {
	printf("error: memory not allocated\n");
	return 0;
	}
	memset(p, 0, size);
	return p;
	}

	int read_u8(const char **p)
	{
	int v = (const unsigned char )(*p);
	*p += 1;
	return v;
	}

	int read_s32_be(const char **p)
	{
	const unsigned char vp = (const unsigned char )*p;
	*p += 4;
	return (vp[0]<<24) + (vp[1]<<16) + (vp[2]<<8) + vp[3];
	}

	int read_u16_le(const char **p)
	{
	const unsigned char up = (const unsigned char )(*p);
	*p += 2;
	return up[0] + (up[1] << 8);
	}

	int is_hnvram_binary(const char *name, int namelen)
	{
	int i;
	for (i = 0; i < ARRAY_SIZE(hnvram_binary_keys); i++) {
	const char *k = hnvram_binary_keys[i];
	if ((int)strlen(k) == namelen && strncmp(k, name, namelen) == 0) {
	return 1;
	}
	}
	return 0;
	}

	char encode_hex(const char s, int len)
	{
	char optr, out = xmalloc(len * 2 + 1);
	for (optr = out; len > 0; len--) {
	sprintf(optr, "%02x", read_u8(&s));
	optr += 2;
	}
	return out;
	}

	char encode_macaddr(const char mac)
	{
	int i;
	char out = xmalloc(6 2 + 5 + 2);
	for (i = 0; i < 6; i++) {
	sprintf(out + i * 3, "%02X:", read_u8(&mac));
	}
	out[6*2 + 5] = '\0';
	return out;
	}

	static void _copy_setenv(const char *name, int namelen,
	const char *val, int vallen)
	{
	char n, v;
	if (namelen + vallen < 128) {
	n = xmalloc(4 + namelen + 1);
	v = xmalloc(vallen + 1);
	memcpy(n, "HNV_", 4);
	memcpy(n + 4, name, namelen);
	memcpy(v, val, vallen);
	n[namelen+4] = 0;
	v[vallen] = 0;
	setenv(n, v);
	free(n);
	free(v);
	} else {
	DEBUG("ignoring oversized val: %.15s, vallen: %d\n", val, vallen);
	}
	}

	static int _parse_hnvram(const char *buf, int len)
	{
	// An hnvram structure. Format is a tag-length-value sequence of:
	// [1 byte] type (1 for notdone, 0 for done)
	// [4 bytes] record length
	// [1 byte] key length
	// [x bytes] key
	// [4 bytes] value length
	// [y bytes] value
	int rectype, reclen, namelen, vallen;
	int done = 0;
	const char name, val, *p = buf;
	while (p - buf <= len + 11) {
	rectype = read_u8(&p);
	if (rectype == 0x00) {
	printf("done processing hnvram block!\n");
	done = 1;
	break;
	}
	if (rectype != 0x01) {
	printf("error: hnvram invalid rectype %x\n", rectype);
	return -1;
	}

	reclen = read_s32_be(&p);
	if (reclen <= 6 \|\| (p - buf) + reclen >= len) {
	printf("error: hnvram invalid reclen %d\n", reclen);
	return -1;
	}
	namelen = read_u8(&p);
	if (namelen < 1 \|\| (p - buf) + namelen >= len) {
	printf("error: hnvram invalid namelen %d\n", namelen);
	return -1;
	}
	name = p;
	p += namelen;
	vallen = read_s32_be(&p);
	if (vallen < 0 \|\| (p - buf) + vallen >= len) {
	printf("error: hnvram invalid vallen %d\n", vallen);
	return -1;
	}
	val = p;
	p += vallen;
	if (vallen == 6 && namelen >= 8 &&
	strncmp("MAC_ADDR", name, 8) == 0) {
	char *macstr = encode_macaddr(val);
	_copy_setenv(name, namelen, macstr, strlen(macstr));
	free(macstr);
	} else if (is_hnvram_binary(name, namelen)) {
	char *hexstr = encode_hex(val, vallen);
	_copy_setenv(name, namelen, hexstr, strlen(hexstr));
	free(hexstr);
	} else {
	_copy_setenv(name, namelen, val, vallen);
	}
	}
	if (!done) {
	printf("error: failed to find final hnvram record?\n");
	return -1;
	}
	return 0;
	}

	#if defined(CONFIG_CMD_HNVRAM)
	int do_hnvram_load(void) {
	char command[60];
	sprintf(command, "spi_flash read 0x%x 0x%x 0x%x", HNVRAM_MTD_OFFSET, HNVRAM_DRAM_OFFSET, MAX_HNVRAM_SIZE);
	int ret = run_command(command, 0);

	if (ret) {
	printf("failed reading from spi-flash at addr: %d\n", HNVRAM_MTD_OFFSET);
	return ret;
	}

	char *buf = HNVRAM_DRAM_OFFSET;

	// Next step: Seek to different parts of the buffer that contain the
	// first, second, and third sections of hnvram.
	if (_parse_hnvram(buf, HNVRAM_BLOCKSIZE) != 0) {
	printf("failed parsing hnvram at offset: 0x%p\n", buf);
	return -1;
	}
	if (_parse_hnvram(buf+HNVRAM_B1_OFFSET, HNVRAM_BLOCKSIZE) != 0) {
	printf("failed parsing hnvram at offset: 0x%p\n", buf+HNVRAM_B1_OFFSET);
	return -1;
	}
	if (_parse_hnvram(buf+HNVRAM_B2_OFFSET, HNVRAM_BLOCKSIZE) != 0) {
	printf("failed parsing hnvram at offset: 0x%p\n", buf+HNVRAM_B2_OFFSET);
	return -1;
	}

	return CMD_RET_SUCCESS;
	}

	int do_hnvram_init(char *val) {
	// Erase entire partition
	char command[60];
	sprintf(command, "spi_flash erase 0x%x 0x%x", HNVRAM_MTD_OFFSET, MAX_HNVRAM_SIZE);
	int ret = run_command(command, 0);

	if (ret) {
	printf("failed to erase hnvram partition\n");
	return ret;
	}

	char key[] = "PLATFORM_NAME";
	int val_len = strlen(val);
	// size variable should fit in one byte
	if (val_len < 1 \|\| 255 < val_len) {
	printf("Intial value for %s has bad size: %d", key, val_len);
	return CMD_RET_FAILURE;
	}
	// +5 to count key length (1) and record length (4) bytes
	int rec_len = sizeof(key) + val_len + 5;

	// Use 2MB of (hopefully) free DRAM
	char *buf = HNVRAM_DRAM_OFFSET;
	memset(buf, 0x00, MAX_HNVRAM_SIZE);

	// Write platform_name to RW section
	int i = HNVRAM_B1_OFFSET;

	// Record start
	buf[i++] = 0x01;

	// Record length
	int nbyte_order = htonl(rec_len); // hnvram expects big endian
	memcpy(buf + i, &nbyte_order, sizeof(nbyte_order));
	i += sizeof(nbyte_order);

	// Key length
	buf[i++] = sizeof(key);

	// Key
	memcpy(buf + i, &key, sizeof(key));
	i += sizeof(key);

	// Val length
	nbyte_order = htonl(val_len);
	memcpy(buf + i, &nbyte_order, sizeof(nbyte_order));
	i += sizeof(nbyte_order);

	// Val
	memcpy(buf + i, val, val_len);

	sprintf(command, "spi_flash write 0x%x 0x%p 0x%x", HNVRAM_MTD_OFFSET, buf, MAX_HNVRAM_SIZE);
	ret = run_command(command, 0);

	if (ret) {
	printf("failed to write init to hnvram partition\n");
	return ret;
	}

	// Load platform_name into environment variables
	do_hnvram_load();

	return CMD_RET_SUCCESS;
	}

	static int do_hnvram_cmd(cmd_tbl_t *cmdtp, int flag, int argc,
	char * const argv[]) {
	if (argc == 2 && strcmp(argv[1], "load") == 0) {
	return do_hnvram_load();
	} else if (argc == 2 && strcmp(argv[1], "init") == 0) {
	return do_hnvram_init("GFRG240");
	} else if (argc == 3 && strcmp(argv[1], "init") == 0) {
	return do_hnvram_init(argv[2]);
	} else {
	printf("hnvram - incorrect usage. Please use one of:\n");
	printf("hnvram load\n");
	printf("hnvram init <model>\n");
	return CMD_RET_USAGE;
	}
	}

	U_BOOT_CMD(
	hnvram, 3, 0, do_hnvram_cmd,
	"HNVRAM key-val storage on flash",
	"\n"
	"hnvram load - reads all variables from flash partition and"
	" places into environment vars as HNV_<name>"
	"hnvram init [model] - zeros hnvram and writes PLATFORM_NAME=<model>"
	" Uses GFRG240 by default if no arg given"
	);
	#endif /* CONFIG_CMD_HNVRAM */