[2/2] Add SSBL Handler

diff --git a/doc/source/handlers.rst b/doc/source/handlers.rst
index 14d8ab0..6b694af 100644
--- a/doc/source/handlers.rst
+++ b/doc/source/handlers.rst
@@ -583,3 +583,63 @@  Example:
         }
     );
 
+SSBL Handler
+------------
+
+This implements a way to switch two software sets using a duplicated structure saved on the
+flash (currently, only NOR flash is supported). Each of the two structures contains address
+and size of the image to be loaded by a first loader. A field contain the "age", and it is
+incremented after each switch to show which is the active set.
+
+
+.. table:: Structure of SSBL Admin
+
+   +---------------------------------------------------------------+-------------+
+   |  SSBL Magic Number (29 bit)Name                               | Age (3 bit) |
+   +---------------------------------------------------------------+-------------+
+   |                            Image Address Offset                             |
+   +-----------------------------------------------------------------------------+
+   |                            Image Size                                       |
+   +-----------------------------------------------------------------------------+
+
+
+The handler implements a post install script. First, it checks for consistency the two
+structures and find the active reading the first 32 bit value with a magic number and the age.
+It increments the age and saves the new structure in the inactive copy. After a reboot,
+the loader will check it and switch the software set.
+
+::
+
+	scripts: (
+		{
+		        type = "ssblswitch";
+			properties: {
+				device = ["mtdX", "mtdY"];
+				offset = ["0", "0"];
+				imageoffs = ["0x780000",  "0xA40000"];
+				imagesize = ["0x800000", "0x800000"];
+			}
+        }
+
+
+Properties in sw-description are all mandatory. They define where the SSBL Administration data
+are stored for both sets. Each properties is an array of two entries, containing values for each
+of the two SSBL administration.
+
+.. table:: Properties for SSBL handler
+
+   +-------------+----------+----------------------------------------------------+
+   |  Name       |  Type    |  Description                                       |
+   +=============+==========+====================================================+
+   | device      | string   | MTD device where the SSBL Admin Header is stored   | 
+   +-------------+----------+----------------------------------------------------+
+   | offset      | hex      | Offset of SSBL header inside the MTD device        | 
+   +-------------+----------+----------------------------------------------------+
+   | imageoffset | hex      | Offset of the image to be loaded by a bootloader   |
+   |             |          | when this SSBL is set.                             |
+   +-------------+----------+----------------------------------------------------+
+   | imagesize   | hex      | Size of the image to be loaded by a bootloader     |
+   |             |          | when this SSBL is set.                             |
+   +-------------+----------+----------------------------------------------------+
+
+
diff --git a/handlers/Config.in b/handlers/Config.in
index 40d2753..41eac1c 100644
--- a/handlers/Config.in
+++ b/handlers/Config.in
@@ -212,6 +212,17 @@  config BOOTLOADERHANDLER
 	  Enable it to change bootloader environment
 	  during the installation process.
 
+config SSBLSWITCH
+	bool "Second Stage Switcher"
+	depends on MTD
+	default n
+	help
+	  This handler allows to switch between two software set
+	  based on an administration block stored in flash (CFI interface).
+	  It works like the switch of UBI and allow to switch in an atomic
+	  way between two software set. It can be used to reliable update
+	  a second stage bootloader.
+
 config UCFWHANDLER
 	bool "microcontroller firmware update"
 	depends on HAVE_LIBGPIOD
diff --git a/handlers/Makefile b/handlers/Makefile
index 10bd8a4..61e4f76 100644
--- a/handlers/Makefile
+++ b/handlers/Makefile
@@ -17,6 +17,7 @@  obj-$(CONFIG_RAW)	+= raw_handler.o
 obj-$(CONFIG_RDIFFHANDLER) += rdiff_handler.o
 obj-$(CONFIG_REMOTE_HANDLER) += remote_handler.o
 obj-$(CONFIG_SHELLSCRIPTHANDLER) += shell_scripthandler.o
+obj-$(CONFIG_SSBLSWITCH) += ssbl_handler.o
 obj-$(CONFIG_SWUFORWARDER_HANDLER) += swuforward_handler.o swuforward-ws.o
 obj-$(CONFIG_UBIVOL)	+= ubivol_handler.o
 obj-$(CONFIG_UCFWHANDLER)	+= ucfw_handler.o
diff --git a/handlers/ssbl_handler.c b/handlers/ssbl_handler.c
new file mode 100644
index 0000000..659c90d
--- /dev/null
+++ b/handlers/ssbl_handler.c
@@ -0,0 +1,299 @@ 
+/*
+ * (C) Copyright 2019
+ * Stefano Babic, DENX Software Engineering, sbabic@denx.de.
+ *
+ * SPDX-License-Identifier:     GPL-2.0-only
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <errno.h>
+#include <linux/version.h>
+#include <sys/ioctl.h>
+#include <stddef.h>
+
+#include <mtd/mtd-user.h>
+#include "swupdate.h"
+#include "handler.h"
+#include "util.h"
+#include "flash.h"
+
+#if !defined(TESTHOST)
+#define PATH_TO_MTD	"/dev/mtd"
+#else
+#define PATH_TO_MTD	""
+#endif
+
+/*
+ * This is used to write a general function
+ * to parse the properties and passing the type
+ */
+
+typedef enum {
+	INT32_TYPE,
+	STRING_TYPE
+} PROPTYPE;
+
+/*
+ * The administration sector
+ * There are two copies of admin
+ */
+struct ssbl_admin_sector {
+	uint32_t magic_age;
+	uint32_t image_offs;
+	uint32_t image_size;
+};
+
+struct ssbl_priv {
+	char device[MAX_VOLNAME];
+	int32_t admin_offs;
+	uint32_t image_offs;
+	uint32_t image_size;
+	int mtdnum;
+	struct ssbl_admin_sector ssbl;
+};
+
+struct proplist {
+	const char *name;
+	PROPTYPE type;
+	size_t offset;
+};
+
+static struct proplist list[] =  {
+	{"device", STRING_TYPE, offsetof(struct ssbl_priv, device)},
+	{"offset", INT32_TYPE, offsetof(struct ssbl_priv, admin_offs)},
+	{"imageoffs", INT32_TYPE, offsetof(struct ssbl_priv, image_offs)},
+	{"imagesize", INT32_TYPE, offsetof(struct ssbl_priv, image_size)},
+	{ NULL, INT32_TYPE,  0 }
+};
+
+/*
+ * Mask for magic_age
+ */
+#define SSBL_MAGIC	0x1CEEDBEE
+#define get_ssbl_age(t) ((t & 0x07) % 3)
+#define get_ssbl_magic(t) ((t & ~0x07) >> 3)
+
+void ssbl_handler(void);
+
+static inline bool ssbl_verify_magic(struct ssbl_priv *adm)
+{
+	return get_ssbl_magic(adm->ssbl.magic_age) == SSBL_MAGIC;
+}
+static inline int ssbl_get_age(struct ssbl_priv *adm)
+{
+	return get_ssbl_age(adm->ssbl.magic_age);
+}
+
+static bool ssbl_retrieve_property(struct img_type *img, const char *name,
+				   struct ssbl_priv *admins,
+				   uint32_t offset, PROPTYPE type)
+{
+	struct dict_list *proplist;
+	struct dict_list_elem *property;
+	int num = 0;
+	uint32_t *pval;
+	char *p;
+
+	proplist = dict_get_list(&img->properties, name);
+
+	if (!proplist)
+		return false;
+
+	LIST_FOREACH(property, proplist, next) {
+		if (num >= 2) {
+			ERROR("SSBL switches between two structures, too many found (%s)", name);
+			return false;
+		}
+
+		switch (type) {
+		case INT32_TYPE:
+			pval = (uint32_t *) ((size_t)&admins[num] + offset);
+			*pval = strtoul(property->value, NULL, 0);
+			break;
+		case STRING_TYPE:
+			p = (char *)((size_t)&admins[num] + offset);
+			strncpy(p, property->value, MAX_VOLNAME);
+			break;
+		}
+		num++;
+	}
+
+	if (num != 2)
+		return false;
+
+	return true;
+}
+
+/*
+ * Check which SSBL is the standby copy
+ * At least one of the two SSBL Admin block
+ * must contain valid data
+ */
+static int get_inactive_ssbl(struct ssbl_priv *padmins)
+{
+	int i;
+	int age0, age1;
+
+	for (i = 0; i < 2; i++) {
+		if (!ssbl_verify_magic(&padmins[i]))
+			return i;
+	}
+
+	/*
+	 * Both valid, check age
+	 */
+	age0 = ssbl_get_age(&padmins[0]);
+	age1 = ssbl_get_age(&padmins[1]);
+
+	DEBUG("AGES : %s --> %d %s-->%d",
+		padmins[0].device, age0,
+		padmins[1].device, age1);
+
+	if (!age0 && age1 == 3)
+		age0 = 4;
+	if (!age1 && age0 == 3)
+		age1 = 4;
+
+	if (age1 > age0)
+		return 0;
+
+	return 1;
+}
+
+static int inline get_active_ssbl(struct ssbl_priv *padmins) {
+	return get_inactive_ssbl(padmins) == 1 ? 0 : 1;
+}
+
+static int ssbl_swap(struct img_type *img, void *data)
+{
+	script_fn scriptfn;
+	struct ssbl_priv admins[2];
+	struct ssbl_priv *pssbl;
+	struct proplist *entry;
+	int iter, ret;
+	int fd;
+	struct flash_description *flash = get_flash_info();
+	char mtd_device[80];
+
+	if (!data)
+		return -EINVAL;
+
+	scriptfn = *(script_fn *)data;
+
+	/*
+	 * Call only in case of postinstall
+	 */
+	if (scriptfn != POSTINSTALL)
+		return 0;
+
+	memset(admins, 0, 2 * sizeof(struct ssbl_priv));
+
+	entry = &list[0];
+	while (entry->name) {
+		if (!ssbl_retrieve_property(img, entry->name, admins,
+					    entry->offset, entry->type)) {
+			ERROR("Cannot get %s from sw-description", entry->name);
+			return -EINVAL;
+		}
+		entry++;
+	}
+
+	/*
+	 * Retrieve SSBL Admin sectors
+	 */
+	for (iter = 0; iter < 2; iter++) {
+		pssbl = &admins[iter];
+#if !defined(TESTHOST)
+		pssbl->mtdnum = get_mtd_from_device(pssbl->device);
+		if (pssbl->mtdnum < 0) {
+		/* Allow device to be specified by name OR number */
+			pssbl->mtdnum = get_mtd_from_name(pssbl->device);
+		}
+		if (pssbl->mtdnum < 0 || !mtd_dev_present(flash->libmtd,
+							  pssbl->mtdnum)) {
+			ERROR("%s does not exist: partitioning not possible",
+			pssbl->device);
+			return -ENODEV;
+		}
+		snprintf(mtd_device, sizeof(mtd_device),
+			"%s%d", PATH_TO_MTD, pssbl->mtdnum);
+#else
+		snprintf(mtd_device, sizeof(mtd_device), "%s", pssbl->device);
+#endif
+		if ((fd = open(mtd_device, O_RDWR)) < 0) {
+			ERROR( "%s: %s: %s", __func__, mtd_device,
+				strerror(errno));
+			return -ENODEV;
+		}
+
+		ret = read(fd, &pssbl->ssbl, sizeof(struct ssbl_admin_sector));
+		close(fd);
+		if (ret < 0) {
+			ERROR("%s: SSBL cannot be read: %s", mtd_device,
+			       strerror(errno));
+			return -ENODEV;
+		}
+	}
+
+	/*
+	 * Perform the switch:
+	 * - find the inactive copy
+	 * - increment age
+	 * - write to flash
+	 */
+	pssbl = &admins[get_inactive_ssbl(admins)];
+#if !defined(TESTHOST)
+	flash_erase(pssbl->mtdnum);	/* erase inactive copy */
+	snprintf(mtd_device, sizeof(mtd_device), "%s%d", PATH_TO_MTD,
+		 pssbl->mtdnum);
+#else
+	snprintf(mtd_device, sizeof(mtd_device), "%s", pssbl->device);
+#endif
+	pssbl->ssbl.image_size = pssbl->image_size;
+	pssbl->ssbl.image_offs = pssbl->image_offs;
+
+	/*
+	 * Get age from the active copy and increment it
+	 * age is module 3
+	 */
+	pssbl->ssbl.magic_age = 0xFFFFFFF8 |
+		((ssbl_get_age(&admins[get_active_ssbl(admins)]) + 1) % 3);
+
+	/* Write to flash */
+	if ((fd = open(mtd_device, O_RDWR)) < 0) {
+		ERROR( "%s: %s: %s", __func__, mtd_device, strerror(errno));
+		return -ENODEV;
+	}
+	ret = write(fd, &pssbl->ssbl, sizeof(struct ssbl_admin_sector));
+	if (ret != sizeof(struct ssbl_admin_sector)) {
+		ERROR( "Cannot write SSBL admin : %s: %s", mtd_device,
+			strerror(errno));
+		return -EIO;
+	}
+
+	/* Last but not least, write the magic to make the SSBL valid */
+	pssbl->ssbl.magic_age = (pssbl->ssbl.magic_age & 0x07) | (SSBL_MAGIC << 3);
+
+	/* Magic is at the beginning of sector */
+	lseek(fd, 0, SEEK_SET);
+	ret = write(fd, &pssbl->ssbl.magic_age, sizeof(uint32_t));
+	close(fd);
+	if (ret != sizeof(uint32_t)) {
+		ERROR( "Cannot write SSBL admin : %s: %s", mtd_device,
+			strerror(errno));
+		return -EIO;
+	}
+
+	return 0;
+}
+
+__attribute__((constructor))
+void ssbl_handler(void)
+{
+	register_handler("ssblswitch", ssbl_swap,
+				SCRIPT_HANDLER | NO_DATA_HANDLER, NULL);
+}