1 /*
2 * Copyright (c) 2019 Vestas Wind Systems A/S
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/settings/settings.h>
8
9 #include <CANopen.h>
10 #include <CO_Emergency.h>
11 #include <CO_SDO.h>
12
13 #include <canopennode.h>
14
15 #define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL
16 #include <zephyr/logging/log.h>
17 LOG_MODULE_REGISTER(canopen_storage);
18
19 /* 's', 'a', 'v', 'e' from LSB to MSB */
20 #define STORE_PARAM_MAGIC 0x65766173UL
21
22 /* 'l', 'o', 'a', 'd' from LSB to MSB */
23 #define RESTORE_PARAM_MAGIC 0x64616F6CUL
24
25 /* Variables for reporting errors through CANopen once the stack is up */
26 static int canopen_storage_rom_error;
27 static int canopen_storage_eeprom_error;
28
canopen_odf_1010(CO_ODF_arg_t * odf_arg)29 static CO_SDO_abortCode_t canopen_odf_1010(CO_ODF_arg_t *odf_arg)
30 {
31 CO_EM_t *em = odf_arg->object;
32 uint32_t value;
33 int err;
34
35 value = CO_getUint32(odf_arg->data);
36
37 if (odf_arg->reading) {
38 return CO_SDO_AB_NONE;
39 }
40
41 /* Preserve old value */
42 memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(uint32_t));
43
44 if (odf_arg->subIndex != 1U) {
45 return CO_SDO_AB_NONE;
46 }
47
48 if (value != STORE_PARAM_MAGIC) {
49 return CO_SDO_AB_DATA_TRANSF;
50 }
51
52 err = canopen_storage_save(CANOPEN_STORAGE_ROM);
53 if (err) {
54 LOG_ERR("failed to save object dictionary ROM entries (err %d)",
55 err);
56 CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
57 err);
58 return CO_SDO_AB_HW;
59 } else {
60 LOG_DBG("saved object dictionary ROM entries");
61 }
62
63 return CO_SDO_AB_NONE;
64 }
65
canopen_odf_1011(CO_ODF_arg_t * odf_arg)66 static CO_SDO_abortCode_t canopen_odf_1011(CO_ODF_arg_t *odf_arg)
67 {
68 CO_EM_t *em = odf_arg->object;
69 bool failed = false;
70 uint32_t value;
71 int err;
72
73 value = CO_getUint32(odf_arg->data);
74
75 if (odf_arg->reading) {
76 return CO_SDO_AB_NONE;
77 }
78
79 /* Preserve old value */
80 memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(uint32_t));
81
82 if (odf_arg->subIndex < 1U) {
83 return CO_SDO_AB_NONE;
84 }
85
86 if (value != RESTORE_PARAM_MAGIC) {
87 return CO_SDO_AB_DATA_TRANSF;
88 }
89
90 err = canopen_storage_erase(CANOPEN_STORAGE_ROM);
91 if (err == -ENOENT) {
92 LOG_DBG("no object dictionary ROM entries to delete");
93 } else if (err) {
94 LOG_ERR("failed to delete object dictionary ROM entries"
95 " (err %d)", err);
96 CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
97 err);
98 failed = true;
99 } else {
100 LOG_DBG("deleted object dictionary ROM entries");
101 }
102
103 #ifdef CONFIG_CANOPENNODE_STORAGE_HANDLER_ERASES_EEPROM
104 err = canopen_storage_erase(CANOPEN_STORAGE_EEPROM);
105 if (err == -ENOENT) {
106 LOG_DBG("no object dictionary EEPROM entries to delete");
107 } else if (err) {
108 LOG_ERR("failed to delete object dictionary EEPROM entries"
109 " (err %d)", err);
110 CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
111 err);
112 failed = true;
113 } else {
114 LOG_DBG("deleted object dictionary EEPROM entries");
115 }
116 #endif
117
118 if (failed) {
119 return CO_SDO_AB_HW;
120 }
121
122 return CO_SDO_AB_NONE;
123 }
124
canopen_settings_set(const char * key,size_t len_rd,settings_read_cb read_cb,void * cb_arg)125 static int canopen_settings_set(const char *key, size_t len_rd,
126 settings_read_cb read_cb, void *cb_arg)
127 {
128 const char *next;
129 int nlen;
130 ssize_t len;
131
132 nlen = settings_name_next(key, &next);
133
134 if (!strncmp(key, "eeprom", nlen)) {
135 struct sCO_OD_EEPROM eeprom;
136
137 len = read_cb(cb_arg, &eeprom, sizeof(eeprom));
138 if (len < 0) {
139 LOG_ERR("failed to restore object dictionary EEPROM"
140 " entries (err %zu)", len);
141 canopen_storage_eeprom_error = len;
142 } else {
143 if ((eeprom.FirstWord == CO_OD_FIRST_LAST_WORD) &&
144 (eeprom.LastWord == CO_OD_FIRST_LAST_WORD)) {
145 memcpy(&CO_OD_EEPROM, &eeprom,
146 sizeof(CO_OD_EEPROM));
147 LOG_DBG("restored object dictionary EEPROM"
148 " entries");
149 } else {
150 LOG_WRN("object dictionary EEPROM entries"
151 " signature mismatch, skipping"
152 " restore");
153 }
154 }
155
156 return 0;
157 } else if (!strncmp(key, "rom", nlen)) {
158 struct sCO_OD_ROM rom;
159
160 len = read_cb(cb_arg, &rom, sizeof(rom));
161 if (len < 0) {
162 LOG_ERR("failed to restore object dictionary ROM"
163 " entries (err %zu)", len);
164 canopen_storage_rom_error = len;
165 } else {
166 if ((rom.FirstWord == CO_OD_FIRST_LAST_WORD) &&
167 (rom.LastWord == CO_OD_FIRST_LAST_WORD)) {
168 memcpy(&CO_OD_ROM, &rom, sizeof(CO_OD_ROM));
169 LOG_DBG("restored object dictionary ROM"
170 " entries");
171 } else {
172 LOG_WRN("object dictionary ROM entries"
173 " signature mismatch, skipping"
174 " restore");
175 }
176 }
177
178 return 0;
179 }
180
181 return 0;
182 }
183
184 SETTINGS_STATIC_HANDLER_DEFINE(canopen, "canopen", NULL,
185 canopen_settings_set, NULL, NULL);
186
canopen_storage_attach(CO_SDO_t * sdo,CO_EM_t * em)187 void canopen_storage_attach(CO_SDO_t *sdo, CO_EM_t *em)
188 {
189 CO_OD_configure(sdo, OD_H1010_STORE_PARAM_FUNC, canopen_odf_1010,
190 em, 0U, 0U);
191 CO_OD_configure(sdo, OD_H1011_REST_PARAM_FUNC, canopen_odf_1011,
192 em, 0U, 0U);
193
194 if (canopen_storage_eeprom_error) {
195 CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
196 canopen_storage_eeprom_error);
197 }
198
199 if (canopen_storage_rom_error) {
200 CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
201 canopen_storage_rom_error);
202 }
203 }
204
canopen_storage_save(enum canopen_storage storage)205 int canopen_storage_save(enum canopen_storage storage)
206 {
207 int ret = 0;
208
209 if (storage == CANOPEN_STORAGE_ROM) {
210 ret = settings_save_one("canopen/rom", &CO_OD_ROM,
211 sizeof(CO_OD_ROM));
212 } else if (storage == CANOPEN_STORAGE_EEPROM) {
213 ret = settings_save_one("canopen/eeprom", &CO_OD_EEPROM,
214 sizeof(CO_OD_EEPROM));
215 }
216
217 return ret;
218 }
219
canopen_storage_erase(enum canopen_storage storage)220 int canopen_storage_erase(enum canopen_storage storage)
221 {
222 int ret = 0;
223
224 if (storage == CANOPEN_STORAGE_ROM) {
225 ret = settings_delete("canopen/rom");
226 } else if (storage == CANOPEN_STORAGE_EEPROM) {
227 ret = settings_delete("canopen/eeprom");
228 }
229
230 return ret;
231 }
232
