Commit bbc1928d authored by Henrik Brix Andersen's avatar Henrik Brix Andersen Committed by Carles Cufi
Browse files

canbus: canopen: add program download support 



Add program download (firmware update) support according to the CAN in
Automation (CiA) 302-3 draft standard proposal v4.1.0.

The implementation supports a Zephyr specific vendor command allowing
external confirmation of a newly booted firmware image. If this is not
desired, the application can confirm the image by other means.

Signed-off-by: default avatarHenrik Brix Andersen <hebad@vestas.com>
parent 28d5c0bf

include/canbus/canopen.h

+22 −0
Original line number Diff line number Diff line
@@ -93,6 +93,19 @@ int canopen_storage_save(enum canopen_storage storage); 
 */

int canopen_storage_erase(enum canopen_storage storage);



/**

 * @brief Attach CANopen object dictionary program download handlers.

 *

 * Attach CANopen program download functions to object dictionary

 * indexes 0x1F50, 0x1F51, 0x1F56, and 0x1F57. This function must be

 * called after calling CANopenNode `CO_init()`.

 *

 * @param nmt CANopenNode NMT object

 * @param sdo CANopenNode SDO server object

 * @param em  CANopenNode Emergency object

 */

void canopen_program_download_attach(CO_NMT_t *nmt, CO_SDO_t *sdo, CO_EM_t *em);



/**

 * @typedef canopen_led_callback_t

 * @brief CANopen LED indicator callback function signature.
@@ -119,6 +132,15 @@ void canopen_leds_init(CO_NMT_t *nmt, 
		       canopen_led_callback_t green_cb, void *green_arg,

		       canopen_led_callback_t red_cb, void *red_arg);



/**

 * @brief Indicate CANopen program download in progress

 *

 * Indicate that a CANopen program download is in progress.

 *

 * @param in_progress true if program download is in progress, false otherwise

 */

void canopen_leds_program_download(bool in_progress);



#ifdef __cplusplus

}

#endif

subsys/canbus/canopen/CMakeLists.txt

+1 −0
Original line number Diff line number Diff line
@@ -5,4 +5,5 @@ zephyr_library_sources_ifdef(CONFIG_CANOPEN CO_driver.c) 
zephyr_library_sources_ifdef(CONFIG_CANOPEN_SYNC_THREAD canopen_sync.c)

zephyr_library_sources_ifdef(CONFIG_CANOPEN_STORAGE canopen_storage.c)

zephyr_library_sources_if_kconfig(canopen_leds.c)

zephyr_library_sources_ifdef(CONFIG_CANOPEN_PROGRAM_DOWNLOAD canopen_program.c)

zephyr_include_directories(.)

subsys/canbus/canopen/Kconfig

+9 −0
Original line number Diff line number Diff line
@@ -104,4 +104,13 @@ config CANOPEN_SYNC_THREAD_PRIORITY 
	  Priority level of the internal thread which processes

	  CANopen SYNC RPDOs and TPDOs.



config CANOPEN_PROGRAM_DOWNLOAD

	bool "CANopen program download"

	depends on BOOTLOADER_MCUBOOT

	select IMG_MANAGER

	default y

	help

	  Enable support for program download over CANopen according

	  to the CiA 303-2 (draft) specification.



endif # CANOPEN

subsys/canbus/canopen/canopen_leds.c

+12 −1
Original line number Diff line number Diff line
@@ -15,6 +15,7 @@ struct canopen_leds_state { 
	void *red_arg;

	bool green : 1;

	bool red : 1;

	bool program_download : 1;

};



static struct canopen_leds_state canopen_leds;
@@ -28,7 +29,12 @@ static void canopen_leds_update(struct k_timer *timer_id) 


	CO_NMT_blinkingProcess50ms(canopen_leds.nmt);



	if (canopen_leds.program_download) {

		green = LED_TRIPLE_FLASH(canopen_leds.nmt);

	} else {

		green = LED_GREEN_RUN(canopen_leds.nmt);

	}



	red = LED_RED_ERROR(canopen_leds.nmt);



#ifdef CONFIG_CANOPEN_LEDS_BICOLOR
@@ -89,3 +95,8 @@ void canopen_leds_init(CO_NMT_t *nmt, 
		k_timer_start(&canopen_leds_timer, K_MSEC(50), K_MSEC(50));

	}

}



void canopen_leds_program_download(bool in_progress)

{

	canopen_leds.program_download = in_progress;

}

subsys/canbus/canopen/canopen_program.c

0 → 100644
+395 −0
Original line number Diff line number Diff line 
/*

 * Copyright (c) 2020 Vestas Wind Systems A/S

 *

 * SPDX-License-Identifier: Apache-2.0

 */



#include <CANopen.h>



#include <canbus/canopen.h>

#include <dfu/flash_img.h>

#include <dfu/mcuboot.h>

#include <sys/crc.h>



#define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL

#include <logging/log.h>

LOG_MODULE_REGISTER(canopen_program);



/* Object dictionary indexes */

#define OD_H1F50_PROGRAM_DATA 0x1F50

#define OD_H1F51_PROGRAM_CTRL 0x1F51

#define OD_H1F56_PROGRAM_SWID 0x1F56

#define OD_H1F57_FLASH_STATUS 0x1F57



/* Common program control commands and status */

#define PROGRAM_CTRL_STOP           0x00

#define PROGRAM_CTRL_START          0x01

#define PROGRAM_CTRL_RESET          0x02

#define PROGRAM_CTRL_CLEAR          0x03

/* Zephyr specific program control and status */

#define PROGRAM_CTRL_ZEPHYR_CONFIRM 0x80



/* Flash status bits */

#define FLASH_STATUS_IN_PROGRESS          BIT(0)

/* Flash common error bits values */

#define FLASH_STATUS_NO_ERROR            (0U << 1U)

#define FLASH_STATUS_NO_VALID_PROGRAM    (1U << 1U)

#define FLASH_STATUS_DATA_FORMAT_UNKNOWN (2U << 1U)

#define FLASH_STATUS_DATA_FORMAT_ERROR   (3U << 1U)

#define FLASH_STATUS_FLASH_NOT_CLEARED   (4U << 1U)

#define FLASH_STATUS_FLASH_WRITE_ERROR   (5U << 1U)

#define FLASH_STATUS_GENERAL_ADDR_ERROR  (6U << 1U)

#define FLASH_STATUS_FLASH_SECURED       (7U << 1U)

#define FLASH_STATUS_UNSPECIFIED_ERROR   (63U << 1)



struct canopen_program_context {

	u32_t flash_status;

	size_t total;

	CO_NMT_t *nmt;

	CO_EM_t *em;

	struct flash_img_context flash_img_ctx;

	u8_t program_status;

	bool flash_written;

};



static struct canopen_program_context ctx;



static void canopen_program_set_status(u32_t status)

{

	ctx.program_status = status;

}



static u32_t canopen_program_get_status(void)

{

	/*

	 * Non-confirmed boot image takes precedence over other

	 * status. This must be checked on every invocation since the

	 * app may be using other means of confirming the image.

	 */

	if (!boot_is_img_confirmed()) {

		return PROGRAM_CTRL_ZEPHYR_CONFIRM;

	}



	return ctx.program_status;

}



static CO_SDO_abortCode_t canopen_odf_1f50(CO_ODF_arg_t *odf_arg)

{

	int err;



	if (odf_arg->subIndex != 1U) {

		return CO_SDO_AB_NONE;

	}



	if (odf_arg->reading) {

		return CO_SDO_AB_WRITEONLY;

	}



	if (canopen_program_get_status() != PROGRAM_CTRL_CLEAR) {

		ctx.flash_status = FLASH_STATUS_FLASH_NOT_CLEARED;

		return CO_SDO_AB_DATA_DEV_STATE;

	}



	if (odf_arg->firstSegment) {

		err = flash_img_init(&ctx.flash_img_ctx);

		if (err) {

			LOG_ERR("failed to initialize flash img (err %d)", err);

			CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,

				       CO_EMC_HARDWARE, err);

			ctx.flash_status = FLASH_STATUS_FLASH_WRITE_ERROR;

			return CO_SDO_AB_HW;

		}

		ctx.flash_status = FLASH_STATUS_IN_PROGRESS;

		if (IS_ENABLED(CONFIG_CANOPEN_LEDS)) {

			canopen_leds_program_download(true);

		}

		ctx.total = odf_arg->dataLengthTotal;

		LOG_DBG("total = %d", ctx.total);

	}



	err = flash_img_buffered_write(&ctx.flash_img_ctx, odf_arg->data,

				       odf_arg->dataLength,

				       odf_arg->lastSegment);

	if (err) {

		CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,

			       CO_EMC_HARDWARE, err);

		ctx.flash_status = FLASH_STATUS_FLASH_WRITE_ERROR;

		canopen_leds_program_download(false);

		return CO_SDO_AB_HW;

	}



	if (odf_arg->lastSegment) {

		/* ctx.total is zero if not provided by download process */

		if (ctx.total != 0 &&

		    ctx.total != flash_img_bytes_written(&ctx.flash_img_ctx)) {

			LOG_WRN("premature end of program download");

			ctx.flash_status = FLASH_STATUS_DATA_FORMAT_ERROR;

		} else {

			LOG_DBG("program downloaded");

			ctx.flash_written = true;

			ctx.flash_status = FLASH_STATUS_NO_ERROR;

		}



		canopen_program_set_status(PROGRAM_CTRL_STOP);

		canopen_leds_program_download(false);

	}



	return CO_SDO_AB_NONE;

}



static inline CO_SDO_abortCode_t canopen_program_cmd_stop(void)

{

	if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {

		return CO_SDO_AB_DATA_DEV_STATE;

	}



	LOG_DBG("program stopped");

	canopen_program_set_status(PROGRAM_CTRL_STOP);



	return CO_SDO_AB_NONE;

}



static inline CO_SDO_abortCode_t canopen_program_cmd_start(void)

{

	int err;



	if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {

		return CO_SDO_AB_DATA_DEV_STATE;

	}



	if (ctx.flash_written) {

		LOG_DBG("requesting upgrade and reset");



		err = boot_request_upgrade(BOOT_UPGRADE_TEST);

		if (err) {

			LOG_ERR("failed to request upgrade (err %d)", err);

			CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,

				       CO_EMC_HARDWARE, err);

			return CO_SDO_AB_HW;

		}



		ctx.nmt->resetCommand = CO_RESET_APP;

	} else {

		LOG_DBG("program started");

		canopen_program_set_status(PROGRAM_CTRL_START);

	}



	return CO_SDO_AB_NONE;

}



static inline CO_SDO_abortCode_t canopen_program_cmd_clear(void)

{

	int err;



	if (canopen_program_get_status() != PROGRAM_CTRL_STOP) {

		return CO_SDO_AB_DATA_DEV_STATE;

	}



	if (!IS_ENABLED(CONFIG_IMG_ERASE_PROGRESSIVELY)) {

		LOG_DBG("erasing flash area");



		err = boot_erase_img_bank(DT_FLASH_AREA_IMAGE_1_ID);

		if (err) {

			LOG_ERR("failed to erase image bank (err %d)", err);

			CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,

				       CO_EMC_HARDWARE, err);

			return CO_SDO_AB_HW;

		}

	}



	LOG_DBG("program cleared");

	canopen_program_set_status(PROGRAM_CTRL_CLEAR);

	ctx.flash_status = FLASH_STATUS_NO_ERROR;

	ctx.flash_written = false;



	return CO_SDO_AB_NONE;

}



static inline CO_SDO_abortCode_t canopen_program_cmd_confirm(void)

{

	int err;



	if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {

		err = boot_write_img_confirmed();

		if (err) {

			LOG_ERR("failed to confirm image (err %d)", err);

			CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,

				       CO_EMC_HARDWARE, err);

			return CO_SDO_AB_HW;

		}



		LOG_DBG("program confirmed");

		canopen_program_set_status(PROGRAM_CTRL_START);

	}



	return CO_SDO_AB_NONE;

}



static CO_SDO_abortCode_t canopen_odf_1f51(CO_ODF_arg_t *odf_arg)

{

	CO_SDO_abortCode_t ab;

	u8_t cmd;



	if (odf_arg->subIndex != 1U) {

		return CO_SDO_AB_NONE;

	}



	if (odf_arg->reading) {

		odf_arg->data[0] = canopen_program_get_status();

		return CO_SDO_AB_NONE;

	}



	if (CO_NMT_getInternalState(ctx.nmt) != CO_NMT_PRE_OPERATIONAL) {

		LOG_DBG("not in pre-operational state");

		return CO_SDO_AB_DATA_DEV_STATE;

	};



	/* Preserve old value */

	cmd = odf_arg->data[0];

	memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(u8_t));



	LOG_DBG("program status = %d, cmd = %d", canopen_program_get_status(),

		cmd);



	switch (cmd) {

	case PROGRAM_CTRL_STOP:

		ab = canopen_program_cmd_stop();

		break;

	case PROGRAM_CTRL_START:

		ab = canopen_program_cmd_start();

		break;

	case PROGRAM_CTRL_CLEAR:

		ab = canopen_program_cmd_clear();

		break;

	case PROGRAM_CTRL_ZEPHYR_CONFIRM:

		ab = canopen_program_cmd_confirm();

		break;

	case PROGRAM_CTRL_RESET:

		/* Fallthrough */

	default:

		LOG_DBG("unsupported command '%d'", cmd);

		ab = CO_SDO_AB_INVALID_VALUE;

	}



	return ab;

}



#ifdef CONFIG_BOOTLOADER_MCUBOOT

static CO_SDO_abortCode_t canopen_odf_1f56(CO_ODF_arg_t *odf_arg)

{

	const struct flash_area *flash_area;

	struct mcuboot_img_header header;

	off_t offset = 0;

	u32_t crc = 0;

	size_t size;

	u8_t fa_id;

	u32_t data;

	u32_t len;

	int err;



	if (odf_arg->subIndex != 1U) {

		return CO_SDO_AB_NONE;

	}



	if (!odf_arg->reading) {

		/* Preserve old value */

		memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(u32_t));

		return CO_SDO_AB_READONLY;

	}



	/*

	 * Calculate the CRC32 of the image that is running or will be

	 * started upon receiveing the next 'start' command.

	 */

	if (ctx.flash_written) {

		fa_id = DT_FLASH_AREA_IMAGE_1_ID;

	} else {

		fa_id = DT_FLASH_AREA_IMAGE_0_ID;

	}



	err = boot_read_bank_header(fa_id, &header, sizeof(header));

	if (err) {

		LOG_WRN("failed to read bank header (err %d)", err);

		CO_setUint32(odf_arg->data, 0U);

		return CO_SDO_AB_NONE;

	}



	if (header.mcuboot_version != 1) {

		LOG_WRN("unsupported mcuboot header version %d",

			header.mcuboot_version);

		CO_setUint32(odf_arg->data, 0U);

		return CO_SDO_AB_NONE;

	}

	len = header.h.v1.image_size;



	err = flash_area_open(fa_id, &flash_area);

	if (err) {

		LOG_ERR("failed to open flash area (err %d)", err);

		CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,

			       CO_EMC_HARDWARE, err);

		return CO_SDO_AB_HW;

	}



	while (len) {

		size = (len >= sizeof(data)) ? sizeof(data) : len;

		err = flash_area_read(flash_area, offset, &data, size);

		if (err) {

			LOG_ERR("failed to read flash (err %d)", err);

			CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,

				       CO_EMC_HARDWARE, err);

			flash_area_close(flash_area);

			return CO_SDO_AB_HW;

		}



		crc = crc32_ieee_update(crc, (u8_t *)&data, size);

		len -= size;

		offset += size;

	}



	flash_area_close(flash_area);



	CO_setUint32(odf_arg->data, crc);



	return CO_SDO_AB_NONE;

}

#endif /* CONFIG_BOOTLOADER_MCUBOOT */



static CO_SDO_abortCode_t canopen_odf_1f57(CO_ODF_arg_t *odf_arg)

{

	if (odf_arg->subIndex != 1U) {

		return CO_SDO_AB_NONE;

	}



	if (!odf_arg->reading) {

		/* Preserve old value */

		memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(u32_t));

		return CO_SDO_AB_READONLY;

	}



	CO_setUint32(odf_arg->data, ctx.flash_status);



	return CO_SDO_AB_NONE;

}



void canopen_program_download_attach(CO_NMT_t *nmt, CO_SDO_t *sdo, CO_EM_t *em)

{

	canopen_program_set_status(PROGRAM_CTRL_START);

	ctx.flash_status = FLASH_STATUS_NO_ERROR;

	ctx.flash_written = false;

	ctx.nmt = nmt;

	ctx.em = em;



	CO_OD_configure(sdo, OD_H1F50_PROGRAM_DATA, canopen_odf_1f50,

			NULL, 0U, 0U);



	CO_OD_configure(sdo, OD_H1F51_PROGRAM_CTRL, canopen_odf_1f51,

			NULL, 0U, 0U);



	if (IS_ENABLED(CONFIG_BOOTLOADER_MCUBOOT)) {

		CO_OD_configure(sdo, OD_H1F56_PROGRAM_SWID, canopen_odf_1f56,

				NULL, 0U, 0U);

	}



	CO_OD_configure(sdo, OD_H1F57_FLASH_STATUS, canopen_odf_1f57,

			NULL, 0U, 0U);

}

CRA Git | Maintained and supported by SUSTech CRA and CCSE