--- embedtools/src/ub_env.c	2014/01/28 22:50:37	1.1.2.6
+++ embedtools/src/ub_env.c	2014/01/29 13:46:16	1.1.2.14
@@ -4,33 +4,51 @@
 
 env_t *env;
 
-
 static int
 ub_flash_io(const char *csSec, int mode)
 {
-	int f, rlen, ret = 0;
+	int f, l, rlen;
 	const char *str;
-	size_t siz;
+	ssize_t siz, esiz;
+	ait_val_t v;
+	u_int crc;
 
 	FTRACE(4);
 
-	str = cfg_getAttribute(&cfg, csSec, "size");
+	str = cfg_getAttribute(&cfg, csSec, "drive_size");
 	siz = strtol(str, NULL, 0);
 	if (!siz)
 		return -1;
-	str = cfg_getAttribute(&cfg, csSec, "drive");
+	str = cfg_getAttribute(&cfg, csSec, "env_size");
+	if (!str)
+		esiz = siz - sizeof env->env_crc;
+	else
+		esiz = strtol(str, NULL, 0);
+	str = cfg_getAttribute(&cfg, csSec, "drive_name");
 	if (!str) {
 		printf("Error:: drive not found!\n");
 		return -1;
 	}
 
+	cfg_loadAttribute(&cfg, "ube", "lock", &v, UBE_LOCK);
+	l = open(AIT_GET_STR(&v), O_CREAT | O_EXCL | O_WRONLY, 0644);
+	if (l == -1) {
+		printf("Error:: Locked u-boot-env map %s\n", AIT_GET_STR(&v));
+		AIT_FREE_VAL(&v);
+		return -1;
+	}
+
 	f = open(str, mode);
 	if (f == -1) {
 		printf("Error:: Can't access u-boot-env device %s\n", str);
+		close(l);
+		unlink(AIT_GET_STR(&v));
+		AIT_FREE_VAL(&v);
 		return -1;
 	}
 
 	if (mode & O_RDWR) {
+		env->env_crc = crc32(0, (u_char*) env->env_data, esiz);
 		rlen = write(f, env, siz);
 		if (rlen != siz)
 			printf("Error:: written %d bytes != %d\n", rlen, siz);
@@ -45,10 +63,30 @@ ub_flash_io(const char *csSec, int mode)
 	else
 		VERB(3) printf("Readed %d bytes\n", rlen);
 
+	crc = crc32(0, (u_char*) env->env_data, esiz);
+	if (crc != env->env_crc)
+		VERB(1) printf("Warning:: Bad CRC, Flash crc32 0x%x != 0x%x\n", 
+				env->env_crc, crc);
+
 	close(f);
-	return ret;
+	close(l);
+	unlink(AIT_GET_STR(&v));
+	AIT_FREE_VAL(&v);
+	return 0;
 }
 
+static inline const char *
+ub_envmatch(const char *csName, const char *e)
+{
+	while (*csName == *e++)
+		if (*csName++ == '=')
+			return e;
+	if (!*csName && *(e - 1) == '=')
+		return e;
+
+	return NULL;
+}
+
 int
 ub_load(const char *csSec)
 {
@@ -57,7 +95,7 @@ ub_load(const char *csSec)
 
 	FTRACE(4);
 
-	str = cfg_getAttribute(&cfg, csSec, "size");
+	str = cfg_getAttribute(&cfg, csSec, "drive_size");
 	siz = strtol(str, NULL, 0);
 	if (!siz)
 		return -1;
@@ -68,7 +106,7 @@ ub_load(const char *csSec)
 		return -1;
 	}
 
-	return 0;
+	return ub_flash_io(csSec, O_RDONLY);
 }
 
 void
@@ -83,17 +121,150 @@ ub_unload()
 const char*
 ub_getenv(const char *csSec, const char *csName)
 {
+	char *e, *nxt;
+	size_t dlen = 0;
 	const char *str = NULL;
 
 	FTRACE(3);
 
+	str = cfg_getAttribute(&cfg, csSec, "env_size");
+	if (!str) {
+		str = cfg_getAttribute(&cfg, csSec, "drive_size");
+		if (!str)
+			return NULL;
+		dlen -= sizeof env->env_crc;
+	}
+	dlen += strtol(str, NULL, 0);
+	if (!dlen)
+		return NULL;
+	else
+		dlen--;
+
+	for (e = env->env_data; *e; e = nxt + 1) {
+		for (nxt = e; *nxt; nxt++)
+			if (nxt >= env->env_data + dlen) {
+				printf("Error:: environment not terminated\n");
+				return NULL;
+			}
+
+		str = ub_envmatch(csName, e);
+		if (str)
+			break;
+	}
+
 	return str;
 }
 
 int
 ub_setenv(const char *csSec, const char *csName, const char *csValue)
 {
+	char *e, *nxt;
+	ssize_t dlen = 0, len;
+	const char *str, *old = NULL;
+
 	FTRACE(3);
+
+	str = cfg_getAttribute(&cfg, csSec, "env_size");
+	if (!str) {
+		str = cfg_getAttribute(&cfg, csSec, "drive_size");
+		if (!str)
+			return -1;
+		dlen -= sizeof env->env_crc;
+	}
+	dlen += strtol(str, NULL, 0);
+	if (!dlen)
+		return -1;
+	else
+		dlen--;
+
+	for (e = env->env_data; *e; e = nxt + 1) {
+		for (nxt = e; *nxt; nxt++)
+			if (nxt >= env->env_data + dlen) {
+				printf("Error:: environment not terminated\n");
+				return -1;
+			}
+
+		old = ub_envmatch(csName, e);
+		if (old)
+			break;
+	}
+
+	/* Delete any existing definition */
+	if (old) {
+		if (!*++nxt)
+			*e = 0;
+		else
+			while (42) {
+				*e = *nxt++;
+				if (!*e && !*nxt)
+					break;
+				e++;
+			}
+		*++e = 0;
+	}
+
+	if (csValue) {
+		/* Append new definition at the end */
+		for (e = env->env_data; *e || *(e + 1); e++);
+		if (e > env->env_data)
+			e++;
+		/* "name" + "=" + "val" +"\0\0"  > u-boot-env size */
+		len = strlen(csName) + 2; /* add '=' for first arg, ' ' for all others */
+		len += strlen(csValue) + 1;
+		if (len > env->env_data + dlen - e) {
+			printf("Error:: Environment overflow!\n");
+			return -1;
+		}
+
+		/* av pair */
+		while ((*e = *csName++))
+			e++;
+		*e = '=';
+		while ((*++e = *csValue++));
+
+		/* end is marked with double '\0' */
+		*++e = 0;
+	}
+
+	if (ub_flash_io(csSec, O_RDWR)) {
+		printf("Error:: Can't write environment to flash!\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+int
+ub_env(const char *csSec)
+{
+	char *e, *nxt;
+	ssize_t dlen = 0;
+	const char *str;
+
+	FTRACE(3);
+
+	str = cfg_getAttribute(&cfg, csSec, "env_size");
+	if (!str) {
+		str = cfg_getAttribute(&cfg, csSec, "drive_size");
+		if (!str)
+			return -1;
+		dlen -= sizeof env->env_crc;
+	}
+	dlen += strtol(str, NULL, 0);
+	if (!dlen)
+		return -1;
+	else
+		dlen--;
+
+	for (e = env->env_data; *e; e = nxt + 1) {
+		for (nxt = e; *nxt; nxt++)
+			if (nxt >= env->env_data + dlen) {
+				printf("Error:: environment not terminated\n");
+				return -1;
+			}
+
+		printf("%s\n", e);
+	}
 
 	return 0;
 }