1 /*
2 * Copyright (c) 2020 Vestas Wind Systems A/S
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <CANopen.h>
8
9 #include <canopennode.h>
10 #include <zephyr/dfu/flash_img.h>
11 #include <zephyr/dfu/mcuboot.h>
12 #include <zephyr/storage/flash_map.h>
13 #include <zephyr/sys/crc.h>
14
15 #define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL
16 #include <zephyr/logging/log.h>
17 LOG_MODULE_REGISTER(canopen_program);
18
19 /* Object dictionary indexes */
20 #define OD_H1F50_PROGRAM_DATA 0x1F50
21 #define OD_H1F51_PROGRAM_CTRL 0x1F51
22 #define OD_H1F56_PROGRAM_SWID 0x1F56
23 #define OD_H1F57_FLASH_STATUS 0x1F57
24
25 /* Common program control commands and status */
26 #define PROGRAM_CTRL_STOP 0x00
27 #define PROGRAM_CTRL_START 0x01
28 #define PROGRAM_CTRL_RESET 0x02
29 #define PROGRAM_CTRL_CLEAR 0x03
30 /* Zephyr specific program control and status */
31 #define PROGRAM_CTRL_ZEPHYR_CONFIRM 0x80
32
33 /* Flash status bits */
34 #define FLASH_STATUS_IN_PROGRESS BIT(0)
35 /* Flash common error bits values */
36 #define FLASH_STATUS_NO_ERROR (0U << 1U)
37 #define FLASH_STATUS_NO_VALID_PROGRAM (1U << 1U)
38 #define FLASH_STATUS_DATA_FORMAT_UNKNOWN (2U << 1U)
39 #define FLASH_STATUS_DATA_FORMAT_ERROR (3U << 1U)
40 #define FLASH_STATUS_FLASH_NOT_CLEARED (4U << 1U)
41 #define FLASH_STATUS_FLASH_WRITE_ERROR (5U << 1U)
42 #define FLASH_STATUS_GENERAL_ADDR_ERROR (6U << 1U)
43 #define FLASH_STATUS_FLASH_SECURED (7U << 1U)
44 #define FLASH_STATUS_UNSPECIFIED_ERROR (63U << 1)
45
46 struct canopen_program_context {
47 uint32_t flash_status;
48 size_t total;
49 CO_NMT_t *nmt;
50 CO_EM_t *em;
51 struct flash_img_context flash_img_ctx;
52 uint8_t program_status;
53 bool flash_written;
54 };
55
56 static struct canopen_program_context ctx;
57
canopen_program_set_status(uint32_t status)58 static void canopen_program_set_status(uint32_t status)
59 {
60 ctx.program_status = status;
61 }
62
canopen_program_get_status(void)63 static uint32_t canopen_program_get_status(void)
64 {
65 /*
66 * Non-confirmed boot image takes precedence over other
67 * status. This must be checked on every invocation since the
68 * app may be using other means of confirming the image.
69 */
70 if (!boot_is_img_confirmed()) {
71 return PROGRAM_CTRL_ZEPHYR_CONFIRM;
72 }
73
74 return ctx.program_status;
75 }
76
canopen_odf_1f50(CO_ODF_arg_t * odf_arg)77 static CO_SDO_abortCode_t canopen_odf_1f50(CO_ODF_arg_t *odf_arg)
78 {
79 int err;
80
81 if (odf_arg->subIndex != 1U) {
82 return CO_SDO_AB_NONE;
83 }
84
85 if (odf_arg->reading) {
86 return CO_SDO_AB_WRITEONLY;
87 }
88
89 if (canopen_program_get_status() != PROGRAM_CTRL_CLEAR) {
90 ctx.flash_status = FLASH_STATUS_FLASH_NOT_CLEARED;
91 return CO_SDO_AB_DATA_DEV_STATE;
92 }
93
94 if (odf_arg->firstSegment) {
95 err = flash_img_init(&ctx.flash_img_ctx);
96 if (err) {
97 LOG_ERR("failed to initialize flash img (err %d)", err);
98 CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
99 CO_EMC_HARDWARE, err);
100 ctx.flash_status = FLASH_STATUS_FLASH_WRITE_ERROR;
101 return CO_SDO_AB_HW;
102 }
103 ctx.flash_status = FLASH_STATUS_IN_PROGRESS;
104 if (IS_ENABLED(CONFIG_CANOPENNODE_LEDS)) {
105 canopen_leds_program_download(true);
106 }
107 ctx.total = odf_arg->dataLengthTotal;
108 LOG_DBG("total = %d", ctx.total);
109 }
110
111 err = flash_img_buffered_write(&ctx.flash_img_ctx, odf_arg->data,
112 odf_arg->dataLength,
113 odf_arg->lastSegment);
114 if (err) {
115 CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
116 CO_EMC_HARDWARE, err);
117 ctx.flash_status = FLASH_STATUS_FLASH_WRITE_ERROR;
118 canopen_leds_program_download(false);
119 return CO_SDO_AB_HW;
120 }
121
122 if (odf_arg->lastSegment) {
123 /* ctx.total is zero if not provided by download process */
124 if (ctx.total != 0 &&
125 ctx.total != flash_img_bytes_written(&ctx.flash_img_ctx)) {
126 LOG_WRN("premature end of program download");
127 ctx.flash_status = FLASH_STATUS_DATA_FORMAT_ERROR;
128 } else {
129 LOG_DBG("program downloaded");
130 ctx.flash_written = true;
131 ctx.flash_status = FLASH_STATUS_NO_ERROR;
132 }
133
134 canopen_program_set_status(PROGRAM_CTRL_STOP);
135 canopen_leds_program_download(false);
136 }
137
138 return CO_SDO_AB_NONE;
139 }
140
canopen_program_cmd_stop(void)141 static inline CO_SDO_abortCode_t canopen_program_cmd_stop(void)
142 {
143 if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {
144 return CO_SDO_AB_DATA_DEV_STATE;
145 }
146
147 LOG_DBG("program stopped");
148 canopen_program_set_status(PROGRAM_CTRL_STOP);
149
150 return CO_SDO_AB_NONE;
151 }
152
canopen_program_cmd_start(void)153 static inline CO_SDO_abortCode_t canopen_program_cmd_start(void)
154 {
155 int err;
156
157 if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {
158 return CO_SDO_AB_DATA_DEV_STATE;
159 }
160
161 if (ctx.flash_written) {
162 LOG_DBG("requesting upgrade and reset");
163
164 err = boot_request_upgrade(BOOT_UPGRADE_TEST);
165 if (err) {
166 LOG_ERR("failed to request upgrade (err %d)", err);
167 CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
168 CO_EMC_HARDWARE, err);
169 return CO_SDO_AB_HW;
170 }
171
172 ctx.nmt->resetCommand = CO_RESET_APP;
173 } else {
174 LOG_DBG("program started");
175 canopen_program_set_status(PROGRAM_CTRL_START);
176 }
177
178 return CO_SDO_AB_NONE;
179 }
180
canopen_program_cmd_clear(void)181 static inline CO_SDO_abortCode_t canopen_program_cmd_clear(void)
182 {
183 int err;
184
185 if (canopen_program_get_status() != PROGRAM_CTRL_STOP) {
186 return CO_SDO_AB_DATA_DEV_STATE;
187 }
188
189 if (!IS_ENABLED(CONFIG_IMG_ERASE_PROGRESSIVELY)) {
190 LOG_DBG("erasing flash area");
191
192 err = boot_erase_img_bank(FIXED_PARTITION_ID(slot1_partition));
193 if (err) {
194 LOG_ERR("failed to erase image bank (err %d)", err);
195 CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
196 CO_EMC_HARDWARE, err);
197 return CO_SDO_AB_HW;
198 }
199 }
200
201 LOG_DBG("program cleared");
202 canopen_program_set_status(PROGRAM_CTRL_CLEAR);
203 ctx.flash_status = FLASH_STATUS_NO_ERROR;
204 ctx.flash_written = false;
205
206 return CO_SDO_AB_NONE;
207 }
208
canopen_program_cmd_confirm(void)209 static inline CO_SDO_abortCode_t canopen_program_cmd_confirm(void)
210 {
211 int err;
212
213 if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {
214 err = boot_write_img_confirmed();
215 if (err) {
216 LOG_ERR("failed to confirm image (err %d)", err);
217 CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
218 CO_EMC_HARDWARE, err);
219 return CO_SDO_AB_HW;
220 }
221
222 LOG_DBG("program confirmed");
223 canopen_program_set_status(PROGRAM_CTRL_START);
224 }
225
226 return CO_SDO_AB_NONE;
227 }
228
canopen_odf_1f51(CO_ODF_arg_t * odf_arg)229 static CO_SDO_abortCode_t canopen_odf_1f51(CO_ODF_arg_t *odf_arg)
230 {
231 CO_SDO_abortCode_t ab;
232 uint8_t cmd;
233
234 if (odf_arg->subIndex != 1U) {
235 return CO_SDO_AB_NONE;
236 }
237
238 if (odf_arg->reading) {
239 odf_arg->data[0] = canopen_program_get_status();
240 return CO_SDO_AB_NONE;
241 }
242
243 if (CO_NMT_getInternalState(ctx.nmt) != CO_NMT_PRE_OPERATIONAL) {
244 LOG_DBG("not in pre-operational state");
245 return CO_SDO_AB_DATA_DEV_STATE;
246 }
247
248 /* Preserve old value */
249 cmd = odf_arg->data[0];
250 memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(uint8_t));
251
252 LOG_DBG("program status = %d, cmd = %d", canopen_program_get_status(),
253 cmd);
254
255 switch (cmd) {
256 case PROGRAM_CTRL_STOP:
257 ab = canopen_program_cmd_stop();
258 break;
259 case PROGRAM_CTRL_START:
260 ab = canopen_program_cmd_start();
261 break;
262 case PROGRAM_CTRL_CLEAR:
263 ab = canopen_program_cmd_clear();
264 break;
265 case PROGRAM_CTRL_ZEPHYR_CONFIRM:
266 ab = canopen_program_cmd_confirm();
267 break;
268 case PROGRAM_CTRL_RESET:
269 __fallthrough;
270 default:
271 LOG_DBG("unsupported command '%d'", cmd);
272 ab = CO_SDO_AB_INVALID_VALUE;
273 }
274
275 return ab;
276 }
277
278 #ifdef CONFIG_BOOTLOADER_MCUBOOT
279 /** @brief Calculate crc for region in flash
280 *
281 * @param flash_area Flash area to read from, must be open
282 * @offset Offset to read from
283 * @size Number of bytes to include in calculation
284 * @pcrc Pointer to uint32_t where crc will be written if return value is 0
285 *
286 * @return 0 if successful, negative errno on failure
287 */
flash_crc(const struct flash_area * flash_area,off_t offset,size_t size,uint32_t * pcrc)288 static int flash_crc(const struct flash_area *flash_area,
289 off_t offset, size_t size, uint32_t *pcrc)
290 {
291 uint32_t crc = 0;
292 uint8_t buffer[32];
293
294 while (size > 0) {
295 size_t len = MIN(size, sizeof(buffer));
296
297 int err = flash_area_read(flash_area, offset, buffer, len);
298
299 if (err) {
300 return err;
301 }
302
303 crc = crc32_ieee_update(crc, buffer, len);
304
305 offset += len;
306 size -= len;
307 }
308
309 *pcrc = crc;
310
311 return 0;
312 }
313
canopen_odf_1f56(CO_ODF_arg_t * odf_arg)314 static CO_SDO_abortCode_t canopen_odf_1f56(CO_ODF_arg_t *odf_arg)
315 {
316 const struct flash_area *flash_area;
317 struct mcuboot_img_header header;
318 off_t offset = 0;
319 uint32_t crc = 0;
320 uint8_t fa_id;
321 uint32_t len;
322 int err;
323
324 if (odf_arg->subIndex != 1U) {
325 return CO_SDO_AB_NONE;
326 }
327
328 if (!odf_arg->reading) {
329 /* Preserve old value */
330 memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(uint32_t));
331 return CO_SDO_AB_READONLY;
332 }
333
334 /* Reading from flash and calculating crc can take 100ms or more, and
335 * this function is called with the can od lock taken.
336 *
337 * Release the lock before performing time consuming work, and reacquire
338 * before return.
339 */
340 CO_UNLOCK_OD();
341
342 /*
343 * Calculate the CRC32 of the image that is running or will be
344 * started upon receiveing the next 'start' command.
345 */
346 if (ctx.flash_written) {
347 fa_id = FIXED_PARTITION_ID(slot1_partition);
348 } else {
349 fa_id = FIXED_PARTITION_ID(slot0_partition);
350 }
351
352 err = boot_read_bank_header(fa_id, &header, sizeof(header));
353 if (err) {
354 LOG_WRN("failed to read bank header (err %d)", err);
355 CO_setUint32(odf_arg->data, 0U);
356
357 CO_LOCK_OD();
358 return CO_SDO_AB_NONE;
359 }
360
361 if (header.mcuboot_version != 1) {
362 LOG_WRN("unsupported mcuboot header version %d",
363 header.mcuboot_version);
364 CO_setUint32(odf_arg->data, 0U);
365
366 CO_LOCK_OD();
367 return CO_SDO_AB_NONE;
368 }
369 len = header.h.v1.image_size;
370
371 err = flash_area_open(fa_id, &flash_area);
372 if (err) {
373 LOG_ERR("failed to open flash area (err %d)", err);
374 CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
375 CO_EMC_HARDWARE, err);
376
377 CO_LOCK_OD();
378 return CO_SDO_AB_HW;
379 }
380
381 err = flash_crc(flash_area, offset, len, &crc);
382
383 flash_area_close(flash_area);
384
385 if (err) {
386 LOG_ERR("failed to read flash (err %d)", err);
387 CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
388 CO_EMC_HARDWARE, err);
389
390 CO_LOCK_OD();
391 return CO_SDO_AB_HW;
392 }
393
394 CO_setUint32(odf_arg->data, crc);
395
396 CO_LOCK_OD();
397 return CO_SDO_AB_NONE;
398 }
399 #endif /* CONFIG_BOOTLOADER_MCUBOOT */
400
canopen_odf_1f57(CO_ODF_arg_t * odf_arg)401 static CO_SDO_abortCode_t canopen_odf_1f57(CO_ODF_arg_t *odf_arg)
402 {
403 if (odf_arg->subIndex != 1U) {
404 return CO_SDO_AB_NONE;
405 }
406
407 if (!odf_arg->reading) {
408 /* Preserve old value */
409 memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(uint32_t));
410 return CO_SDO_AB_READONLY;
411 }
412
413 CO_setUint32(odf_arg->data, ctx.flash_status);
414
415 return CO_SDO_AB_NONE;
416 }
417
canopen_program_download_attach(CO_NMT_t * nmt,CO_SDO_t * sdo,CO_EM_t * em)418 void canopen_program_download_attach(CO_NMT_t *nmt, CO_SDO_t *sdo, CO_EM_t *em)
419 {
420 canopen_program_set_status(PROGRAM_CTRL_START);
421 ctx.flash_status = FLASH_STATUS_NO_ERROR;
422 ctx.flash_written = false;
423 ctx.nmt = nmt;
424 ctx.em = em;
425
426 CO_OD_configure(sdo, OD_H1F50_PROGRAM_DATA, canopen_odf_1f50,
427 NULL, 0U, 0U);
428
429 CO_OD_configure(sdo, OD_H1F51_PROGRAM_CTRL, canopen_odf_1f51,
430 NULL, 0U, 0U);
431
432 if (IS_ENABLED(CONFIG_BOOTLOADER_MCUBOOT)) {
433 CO_OD_configure(sdo, OD_H1F56_PROGRAM_SWID, canopen_odf_1f56,
434 NULL, 0U, 0U);
435 }
436
437 CO_OD_configure(sdo, OD_H1F57_FLASH_STATUS, canopen_odf_1f57,
438 NULL, 0U, 0U);
439 }
440
