/src/cmp_tool/lib/rdcu_compress/rmap.c
Line | Count | Source (jump to first uncovered line) |
1 | | /** |
2 | | * @file rmap.c |
3 | | * @author Armin Luntzer (armin.luntzer@univie.ac.at) |
4 | | * @date 2018 |
5 | | * |
6 | | * @copyright GPLv2 |
7 | | * This program is free software; you can redistribute it and/or modify it |
8 | | * under the terms and conditions of the GNU General Public License, |
9 | | * version 2, as published by the Free Software Foundation. |
10 | | * |
11 | | * This program is distributed in the hope it will be useful, but WITHOUT |
12 | | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
13 | | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for |
14 | | * more details. |
15 | | * |
16 | | * @brief rmap command/reply helper functions |
17 | | * |
18 | | * @note the extended address byte is always set to 0x0 |
19 | | */ |
20 | | |
21 | | |
22 | | #include <stdint.h> |
23 | | #include <string.h> |
24 | | #include <stdlib.h> |
25 | | |
26 | | #include "../common/cmp_debug.h" |
27 | | #include "../common/leon_inttypes.h" |
28 | | #include "rmap.h" |
29 | | |
30 | | |
31 | | /** |
32 | | * @brief valiidates a command code |
33 | | * |
34 | | * @param cmd the command code |
35 | | * |
36 | | * @returns 0 on success, error otherwise |
37 | | */ |
38 | | |
39 | | static int rmap_validate_cmd_code(uint8_t cmd) |
40 | 0 | { |
41 | 0 | switch (cmd) { |
42 | 0 | case RMAP_READ_ADDR_SINGLE: |
43 | 0 | case RMAP_READ_ADDR_INC: |
44 | 0 | case RMAP_READ_MODIFY_WRITE_ADDR_INC: |
45 | 0 | case RMAP_WRITE_ADDR_SINGLE: |
46 | 0 | case RMAP_WRITE_ADDR_INC: |
47 | 0 | case RMAP_WRITE_ADDR_SINGLE_REPLY: |
48 | 0 | case RMAP_WRITE_ADDR_INC_REPLY: |
49 | 0 | case RMAP_WRITE_ADDR_SINGLE_VERIFY: |
50 | 0 | case RMAP_WRITE_ADDR_INC_VERIFY: |
51 | 0 | case RMAP_WRITE_ADDR_SINGLE_VERIFY_REPLY: |
52 | 0 | case RMAP_WRITE_ADDR_INC_VERIFY_REPLY: |
53 | 0 | return 0; |
54 | 0 | default: |
55 | 0 | return -1; |
56 | 0 | } |
57 | 0 | } |
58 | | |
59 | | |
60 | | /** |
61 | | * @brief get the minimum header size given the RMAP instruction |
62 | | * |
63 | | * @param pkt a struct rmap_pkt |
64 | | * |
65 | | * @returns header size or -1 on error |
66 | | */ |
67 | | |
68 | | static int rmap_get_min_hdr_size(struct rmap_pkt *pkt) |
69 | 0 | { |
70 | | |
71 | |
|
72 | 0 | switch (pkt->ri.cmd) { |
73 | 0 | case RMAP_READ_ADDR_SINGLE: |
74 | 0 | case RMAP_READ_ADDR_INC: |
75 | 0 | case RMAP_READ_MODIFY_WRITE_ADDR_INC: |
76 | |
|
77 | 0 | if (pkt->ri.cmd_resp) |
78 | 0 | return RMAP_HDR_MIN_SIZE_READ_CMD; |
79 | | |
80 | 0 | return RMAP_HDR_MIN_SIZE_READ_REP; |
81 | | |
82 | 0 | case RMAP_WRITE_ADDR_SINGLE: |
83 | 0 | case RMAP_WRITE_ADDR_INC: |
84 | 0 | case RMAP_WRITE_ADDR_SINGLE_REPLY: |
85 | 0 | case RMAP_WRITE_ADDR_INC_REPLY: |
86 | 0 | case RMAP_WRITE_ADDR_SINGLE_VERIFY: |
87 | 0 | case RMAP_WRITE_ADDR_INC_VERIFY: |
88 | 0 | case RMAP_WRITE_ADDR_SINGLE_VERIFY_REPLY: |
89 | 0 | case RMAP_WRITE_ADDR_INC_VERIFY_REPLY: |
90 | |
|
91 | 0 | if (pkt->ri.cmd_resp) |
92 | 0 | return RMAP_HDR_MIN_SIZE_WRITE_CMD; |
93 | | |
94 | 0 | return RMAP_HDR_MIN_SIZE_WRITE_REP; |
95 | | |
96 | 0 | default: |
97 | 0 | return -1; |
98 | 0 | } |
99 | 0 | } |
100 | | |
101 | | /** |
102 | | * @brief calculate the CRC8 of a given buffer |
103 | | * |
104 | | * @param buf the buffer containing the data |
105 | | * @param len the length of the buffer |
106 | | * |
107 | | * @returns the CRC8 |
108 | | */ |
109 | | |
110 | | uint8_t rmap_crc8(const uint8_t *buf, const size_t len) |
111 | 0 | { |
112 | 0 | size_t i; |
113 | |
|
114 | 0 | uint8_t crc8 = 0; |
115 | | |
116 | | /* crc8 lookup table from ECSS‐E‐ST‐50‐52C A.3 */ |
117 | 0 | const uint8_t crc8_lt[256] = { |
118 | 0 | 0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75, |
119 | 0 | 0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b, |
120 | 0 | 0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69, |
121 | 0 | 0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67, |
122 | 0 | 0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d, |
123 | 0 | 0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43, |
124 | 0 | 0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51, |
125 | 0 | 0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f, |
126 | 0 | 0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05, |
127 | 0 | 0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b, |
128 | 0 | 0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19, |
129 | 0 | 0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17, |
130 | 0 | 0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d, |
131 | 0 | 0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33, |
132 | 0 | 0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21, |
133 | 0 | 0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f, |
134 | 0 | 0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95, |
135 | 0 | 0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b, |
136 | 0 | 0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89, |
137 | 0 | 0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87, |
138 | 0 | 0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad, |
139 | 0 | 0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3, |
140 | 0 | 0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1, |
141 | 0 | 0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf, |
142 | 0 | 0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5, |
143 | 0 | 0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb, |
144 | 0 | 0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9, |
145 | 0 | 0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7, |
146 | 0 | 0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd, |
147 | 0 | 0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3, |
148 | 0 | 0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1, |
149 | 0 | 0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf, |
150 | 0 | }; |
151 | | |
152 | | |
153 | |
|
154 | 0 | if (!buf) |
155 | 0 | return 0; |
156 | | |
157 | | |
158 | 0 | for (i = 0; i < len; i++) |
159 | 0 | crc8 = crc8_lt[crc8 ^ buf[i]]; |
160 | |
|
161 | 0 | return crc8; |
162 | 0 | } |
163 | | |
164 | | |
165 | | /** |
166 | | * @brief create an RMAP packet and set defaults |
167 | | * |
168 | | * |
169 | | * @note initialises protocol id to 1 and all others to 0 |
170 | | * |
171 | | * @returns a struct rmap_pkt or NULL on error |
172 | | */ |
173 | | |
174 | | struct rmap_pkt *rmap_create_packet(void) |
175 | 0 | { |
176 | 0 | struct rmap_pkt *pkt; |
177 | | |
178 | |
|
179 | 0 | pkt = (struct rmap_pkt *)calloc(1, sizeof(struct rmap_pkt)); |
180 | 0 | if (pkt) |
181 | 0 | pkt->proto_id = RMAP_PROTOCOL_ID; |
182 | |
|
183 | 0 | return pkt; |
184 | 0 | } |
185 | | |
186 | | |
187 | | /** |
188 | | * @brief completely destroys an RMAP packet |
189 | | * |
190 | | * @param pkt a struct rmap_pkt |
191 | | * |
192 | | * @note this will attempt to deallocate any pointer references assigned by the |
193 | | * user |
194 | | * @warning use with care |
195 | | */ |
196 | | |
197 | | void rmap_erase_packet(struct rmap_pkt *pkt) |
198 | 0 | { |
199 | 0 | free(pkt->path); |
200 | 0 | free(pkt->rpath); |
201 | 0 | free(pkt->data); |
202 | 0 | free(pkt); |
203 | 0 | } |
204 | | |
205 | | /** |
206 | | * @brief set the destination (target) logical address |
207 | | * |
208 | | * @param pkt a struct rmap_pkt |
209 | | * @param addr the destination logical address |
210 | | */ |
211 | | |
212 | | void rmap_set_dst(struct rmap_pkt *pkt, uint8_t addr) |
213 | 0 | { |
214 | 0 | if (pkt) |
215 | 0 | pkt->dst = addr; |
216 | 0 | } |
217 | | |
218 | | |
219 | | /** |
220 | | * @brief set the source (initiator) logical address |
221 | | * |
222 | | * @param pkt a struct rmap_pkt |
223 | | * @param addr the source logical address |
224 | | */ |
225 | | |
226 | | void rmap_set_src(struct rmap_pkt *pkt, uint8_t addr) |
227 | 0 | { |
228 | 0 | if (pkt) |
229 | 0 | pkt->src = addr; |
230 | 0 | } |
231 | | |
232 | | |
233 | | /** |
234 | | * @brief set the command authorisation key |
235 | | * |
236 | | * @param pkt a struct rmap_pkt |
237 | | * @param key the authorisation key |
238 | | */ |
239 | | |
240 | | void rmap_set_key(struct rmap_pkt *pkt, uint8_t key) |
241 | 0 | { |
242 | 0 | if (pkt) |
243 | 0 | pkt->key = key; |
244 | 0 | } |
245 | | |
246 | | |
247 | | /** |
248 | | * @brief set the reply address path |
249 | | * |
250 | | * @param pkt a struct rmap_pkt |
251 | | * @param rpath the reply path |
252 | | * @param len the number of elements in the reply path (multiple of 4) |
253 | | * |
254 | | * @note see ECSS‐E‐ST‐50‐52C 5.1.6 for return path rules |
255 | | * |
256 | | * @returns 0 on success, -1 on error |
257 | | */ |
258 | | |
259 | | int rmap_set_reply_path(struct rmap_pkt *pkt, const uint8_t *rpath, uint8_t len) |
260 | 0 | { |
261 | 0 | if (!pkt) |
262 | 0 | return -1; |
263 | | |
264 | 0 | if (!rpath && len) |
265 | 0 | return -1; |
266 | | |
267 | 0 | if (len > RMAP_MAX_REPLY_PATH_LEN) |
268 | 0 | return -1; |
269 | | |
270 | 0 | if (len & 0x3) |
271 | 0 | return -1; |
272 | | |
273 | 0 | pkt->rpath_len = len; |
274 | |
|
275 | 0 | if (len) { |
276 | 0 | pkt->rpath = (uint8_t *)malloc(pkt->rpath_len); |
277 | 0 | if (!pkt->rpath) |
278 | 0 | return -1; |
279 | | |
280 | 0 | memcpy(pkt->rpath, rpath, pkt->rpath_len); |
281 | 0 | } |
282 | | |
283 | | /* number of 32 bit words needed to contain the path */ |
284 | 0 | pkt->ri.reply_addr_len = len >> 2; |
285 | |
|
286 | 0 | return 0; |
287 | 0 | } |
288 | | |
289 | | |
290 | | /** |
291 | | * @brief set the destination address path |
292 | | * |
293 | | * @param pkt a struct rmap_pkt |
294 | | * @param path the destination path |
295 | | * @param len the number of elements in the destination path |
296 | | * |
297 | | * @note see ECSS‐E‐ST‐50‐52C 5.1.6 for return path rules |
298 | | * |
299 | | * @returns 0 on success, -1 on error |
300 | | */ |
301 | | |
302 | | int rmap_set_dest_path(struct rmap_pkt *pkt, const uint8_t *path, uint8_t len) |
303 | 0 | { |
304 | 0 | if (!pkt) |
305 | 0 | return -1; |
306 | | |
307 | 0 | if (!path && len) |
308 | 0 | return -1; |
309 | | |
310 | 0 | if (len > RMAP_MAX_PATH_LEN) |
311 | 0 | return -1; |
312 | | |
313 | 0 | pkt->path_len = len; |
314 | |
|
315 | 0 | if (!len) |
316 | 0 | return 0; |
317 | | |
318 | 0 | pkt->path = (uint8_t *)malloc(pkt->path_len); |
319 | 0 | if (!pkt->path) |
320 | 0 | return -1; |
321 | | |
322 | 0 | memcpy(pkt->path, path, pkt->path_len); |
323 | |
|
324 | 0 | return 0; |
325 | 0 | } |
326 | | |
327 | | |
328 | | /** |
329 | | * @brief set an RMAP command |
330 | | * |
331 | | * @param pkt a struct rmap_pkt |
332 | | * @param cmd the selected command |
333 | | * |
334 | | * @returns -1 on error |
335 | | */ |
336 | | |
337 | | int rmap_set_cmd(struct rmap_pkt *pkt, uint8_t cmd) |
338 | 0 | { |
339 | 0 | if (!pkt) |
340 | 0 | return -1; |
341 | | |
342 | 0 | if (rmap_validate_cmd_code(cmd)) |
343 | 0 | return -1; |
344 | | |
345 | | |
346 | 0 | pkt->ri.cmd = cmd & 0xF; |
347 | 0 | pkt->ri.cmd_resp = 1; |
348 | |
|
349 | 0 | return 0; |
350 | 0 | } |
351 | | |
352 | | |
353 | | /** |
354 | | * @brief set an RMAP transaction identifier |
355 | | * |
356 | | * @param pkt a struct rmap_pkt |
357 | | * @param id the transaction identifier |
358 | | */ |
359 | | |
360 | | void rmap_set_tr_id(struct rmap_pkt *pkt, uint16_t id) |
361 | 0 | { |
362 | 0 | if (!pkt) |
363 | 0 | return; |
364 | | |
365 | 0 | pkt->tr_id = id; |
366 | 0 | } |
367 | | |
368 | | |
369 | | /** |
370 | | * @brief set a data address |
371 | | * |
372 | | * @param pkt a struct rmap_pkt |
373 | | * @param addr the address |
374 | | */ |
375 | | |
376 | | void rmap_set_data_addr(struct rmap_pkt *pkt, uint32_t addr) |
377 | 0 | { |
378 | 0 | if (!pkt) |
379 | 0 | return; |
380 | | |
381 | 0 | pkt->addr = addr; |
382 | 0 | } |
383 | | |
384 | | /** |
385 | | * @brief set an RMAP command |
386 | | * |
387 | | * @param pkt a struct rmap_pkt |
388 | | * @param len the data length (in bytes) |
389 | | * |
390 | | * @returns -1 on error |
391 | | * |
392 | | * @note the length is at most 2^24-1 bytes |
393 | | * @note if the RMAP command is of 'SINGLE' type, only multiples of 4 |
394 | | * will result in successful execution of the command (at least |
395 | | * with the GRSPW2 core) |
396 | | */ |
397 | | |
398 | | int rmap_set_data_len(struct rmap_pkt *pkt, uint32_t len) |
399 | 0 | { |
400 | 0 | if (!pkt) |
401 | 0 | return -1; |
402 | | |
403 | 0 | if (len > RMAP_MAX_DATA_LEN) |
404 | 0 | return -1; |
405 | | |
406 | 0 | pkt->data_len = len; |
407 | |
|
408 | 0 | return 0; |
409 | 0 | } |
410 | | |
411 | | |
412 | | /** |
413 | | * @brief build an rmap header |
414 | | * |
415 | | * @param pkt a struct rmap_pkt |
416 | | * @param hdr the header buffer; if NULL, the function returns the needed size |
417 | | * |
418 | | * @returns -1 on error, size of header otherwise |
419 | | */ |
420 | | |
421 | | int rmap_build_hdr(struct rmap_pkt *pkt, uint8_t *hdr) |
422 | 0 | { |
423 | 0 | int i; |
424 | 0 | int n = 0; |
425 | | |
426 | |
|
427 | 0 | if (!pkt) |
428 | 0 | return -1; |
429 | | |
430 | 0 | if (!hdr) { |
431 | 0 | n = rmap_get_min_hdr_size(pkt); |
432 | 0 | n += pkt->path_len; |
433 | 0 | n += pkt->rpath_len; |
434 | 0 | return n; |
435 | 0 | } |
436 | | |
437 | | |
438 | 0 | for (i = 0; i < pkt->path_len; i++) |
439 | 0 | hdr[n++] = pkt->path[i]; /* routing path to target */ |
440 | |
|
441 | 0 | hdr[n++] = pkt->dst; /* target logical address */ |
442 | 0 | hdr[n++] = pkt->proto_id; /* protocol id */ |
443 | 0 | hdr[n++] = pkt->instruction; /* instruction */ |
444 | 0 | hdr[n++] = pkt->key; /* key/status */ |
445 | |
|
446 | 0 | for (i = 0; i < pkt->rpath_len; i++) |
447 | 0 | hdr[n++] = pkt->rpath[i]; /* return path to source */ |
448 | |
|
449 | 0 | hdr[n++] = pkt->src; /* source logical address */ |
450 | 0 | hdr[n++] = (uint8_t)(pkt->tr_id >> 8); /* MSB of transaction id */ |
451 | 0 | hdr[n++] = (uint8_t)pkt->tr_id; /* LSB of transaction id */ |
452 | | |
453 | | |
454 | | /* commands have a data address */ |
455 | 0 | if (pkt->ri.cmd_resp) { |
456 | 0 | hdr[n++] = 0x0; /* extended address field (unused) */ |
457 | 0 | hdr[n++] = (uint8_t)(pkt->addr >> 24); /* data addr MSB */ |
458 | 0 | hdr[n++] = (uint8_t)(pkt->addr >> 16); |
459 | 0 | hdr[n++] = (uint8_t)(pkt->addr >> 8); |
460 | 0 | hdr[n++] = (uint8_t)pkt->addr; /* data addr LSB */ |
461 | 0 | } else if (!pkt->ri.cmd_resp && pkt->ri.cmd & RMAP_CMD_BIT_WRITE) { |
462 | | /* all headers have data length unless they are a write reply */ |
463 | 0 | return n; |
464 | 0 | } else { |
465 | 0 | hdr[n++] = 0x0; /* on other replies, this is a reserved field */ |
466 | 0 | } |
467 | | |
468 | 0 | hdr[n++] = (uint8_t)(pkt->data_len >> 16); /* data len MSB */ |
469 | 0 | hdr[n++] = (uint8_t)(pkt->data_len >> 8); |
470 | 0 | hdr[n++] = (uint8_t)pkt->data_len; /* data len LSB */ |
471 | |
|
472 | 0 | return n; |
473 | 0 | } |
474 | | |
475 | | |
476 | | /** |
477 | | * @brief create an rmap packet from a buffer |
478 | | * |
479 | | * @param buf the buffer, with the target path stripped away, i.e. |
480 | | * starting with [logical address], [protocol id], ... |
481 | | * @param len the data length of the buffer (in bytes) |
482 | | * |
483 | | * @returns an rmap packet, containing the decoded buffer including any data, |
484 | | * NULL on error |
485 | | */ |
486 | | |
487 | | struct rmap_pkt *rmap_pkt_from_buffer(uint8_t *buf, uint32_t len) |
488 | 0 | { |
489 | 0 | size_t n = 0; |
490 | 0 | size_t i; |
491 | 0 | int min_hdr_size; |
492 | |
|
493 | 0 | struct rmap_pkt *pkt = NULL; |
494 | | |
495 | |
|
496 | 0 | if (!buf) |
497 | 0 | goto error; |
498 | | |
499 | 0 | if (len < RMAP_HDR_MIN_SIZE_WRITE_REP) { |
500 | 0 | debug_print("buffer len is smaller than the smallest RMAP packet"); |
501 | 0 | goto error; |
502 | 0 | } |
503 | | |
504 | 0 | if (buf[RMAP_PROTOCOL_ID] != RMAP_PROTOCOL_ID) { |
505 | 0 | debug_print("Not an RMAP packet, got %x but expected %x", |
506 | 0 | buf[RMAP_PROTOCOL_ID], RMAP_PROTOCOL_ID); |
507 | 0 | goto error; |
508 | 0 | } |
509 | | |
510 | 0 | pkt = rmap_create_packet(); |
511 | 0 | if (!pkt) { |
512 | 0 | debug_print("Error creating packet"); |
513 | 0 | goto error; |
514 | 0 | } |
515 | | |
516 | 0 | pkt->dst = buf[RMAP_DEST_ADDRESS]; |
517 | 0 | pkt->proto_id = buf[RMAP_PROTOCOL_ID]; |
518 | 0 | pkt->instruction = buf[RMAP_INSTRUCTION]; |
519 | 0 | pkt->key = buf[RMAP_CMD_DESTKEY]; |
520 | |
|
521 | 0 | min_hdr_size = rmap_get_min_hdr_size(pkt); |
522 | 0 | if (min_hdr_size < 0) |
523 | 0 | goto error; |
524 | | |
525 | 0 | if (len < (uint32_t)min_hdr_size) { |
526 | 0 | debug_print("buffer len is smaller than the contained RMAP packet"); |
527 | 0 | goto error; |
528 | 0 | } |
529 | | |
530 | | |
531 | 0 | if (pkt->ri.cmd_resp) { |
532 | 0 | pkt->rpath_len = (pkt->ri.reply_addr_len << 2) & 0xFF; |
533 | 0 | if (len < (uint32_t)min_hdr_size + pkt->rpath_len) { |
534 | 0 | debug_print("buffer is smaller than the contained RMAP packet"); |
535 | 0 | goto error; |
536 | 0 | } |
537 | | |
538 | 0 | pkt->rpath = (uint8_t *)malloc(pkt->rpath_len); |
539 | 0 | if (!pkt->rpath) |
540 | 0 | goto error; |
541 | | |
542 | 0 | for (i = 0; i < pkt->rpath_len; i++) |
543 | 0 | pkt->rpath[i] = buf[RMAP_REPLY_ADDR_START + i]; |
544 | |
|
545 | 0 | n = pkt->rpath_len; /* rpath skip */ |
546 | 0 | } |
547 | | |
548 | 0 | pkt->src = buf[RMAP_SRC_ADDR + n]; |
549 | 0 | pkt->tr_id = (((uint16_t)buf[RMAP_TRANS_ID_BYTE0 + n] << 8) & 0xFF00) | |
550 | 0 | (uint16_t)buf[RMAP_TRANS_ID_BYTE1 + n]; |
551 | | |
552 | | /* commands have a data address */ |
553 | 0 | if (pkt->ri.cmd_resp) { |
554 | 0 | pkt->addr = ((uint32_t)buf[RMAP_ADDR_BYTE0 + n] << 24) | |
555 | 0 | ((uint32_t)buf[RMAP_ADDR_BYTE1 + n] << 16) | |
556 | 0 | ((uint32_t)buf[RMAP_ADDR_BYTE2 + n] << 8) | |
557 | 0 | (uint32_t)buf[RMAP_ADDR_BYTE3 + n]; |
558 | 0 | n += 4; /* addr skip, extended byte is incorporated in define */ |
559 | 0 | } |
560 | | |
561 | | /* all headers have data length unless they are a write reply */ |
562 | 0 | if (!(!pkt->ri.cmd_resp && (pkt->ri.cmd & (RMAP_CMD_BIT_WRITE)))) { |
563 | 0 | pkt->data_len = ((uint32_t)buf[RMAP_DATALEN_BYTE0 + n] << 16) | |
564 | 0 | ((uint32_t)buf[RMAP_DATALEN_BYTE1 + n] << 8) | |
565 | 0 | (uint32_t)buf[RMAP_DATALEN_BYTE2 + n]; |
566 | 0 | } |
567 | |
|
568 | 0 | pkt->hdr_crc = buf[RMAP_HEADER_CRC]; |
569 | |
|
570 | 0 | if (pkt->data_len) { |
571 | 0 | size_t pkt_size = RMAP_DATA_START + n + pkt->data_len + 1; /* +1 for data CRC */ |
572 | |
|
573 | 0 | if (len < pkt_size) { |
574 | 0 | debug_print("buffer len is smaller than the contained RMAP packet; buf len: %" PRIu32 " bytes vs RMAP: %lu bytes needed", |
575 | 0 | len, pkt_size); |
576 | 0 | goto error; |
577 | 0 | } |
578 | 0 | if (len > pkt_size) |
579 | 0 | debug_print("warning: the buffer is larger than the included RMAP packet"); |
580 | |
|
581 | 0 | pkt->data = (uint8_t *)malloc(pkt->data_len); |
582 | 0 | if (!pkt->data) |
583 | 0 | goto error; |
584 | | |
585 | 0 | for (i = 0; i < pkt->data_len; i++) |
586 | 0 | pkt->data[i] = buf[RMAP_DATA_START + n + i]; |
587 | | |
588 | | /* final byte is data crc */ |
589 | 0 | pkt->data_crc = buf[RMAP_DATA_START + n + i]; |
590 | 0 | } |
591 | | |
592 | | |
593 | 0 | return pkt; |
594 | | |
595 | 0 | error: |
596 | 0 | if (pkt) { |
597 | 0 | free(pkt->data); |
598 | 0 | free(pkt->rpath); |
599 | 0 | free(pkt); |
600 | 0 | } |
601 | |
|
602 | 0 | return NULL; |
603 | 0 | } |
604 | | |
605 | | |
606 | | |
607 | | /**** UNFINISHED INFO STUFF BELOW ******/ |
608 | | |
609 | | __extension__ |
610 | | static int rmap_check_status(uint8_t status) |
611 | 0 | { |
612 | 0 | debug_print("Status: "); |
613 | |
|
614 | 0 | switch (status) { |
615 | 0 | case RMAP_STATUS_SUCCESS: |
616 | 0 | debug_print("Command executed successfully"); |
617 | 0 | break; |
618 | 0 | case RMAP_STATUS_GENERAL_ERROR: |
619 | 0 | debug_print("General error code"); |
620 | 0 | break; |
621 | 0 | case RMAP_STATUS_UNUSED_TYPE_OR_CODE: |
622 | 0 | debug_print("Unused RMAP Packet Type or Command Code"); |
623 | 0 | break; |
624 | 0 | case RMAP_STATUS_INVALID_KEY: |
625 | 0 | debug_print("Invalid key"); |
626 | 0 | break; |
627 | 0 | case RMAP_STATUS_INVALID_DATA_CRC: |
628 | 0 | debug_print("Invalid Data CRC"); |
629 | 0 | break; |
630 | 0 | case RMAP_STATUS_EARLY_EOP: |
631 | 0 | debug_print("Early EOP"); |
632 | 0 | break; |
633 | 0 | case RMAP_STATUS_TOO_MUCH_DATA: |
634 | 0 | debug_print("Too much data"); |
635 | 0 | break; |
636 | 0 | case RMAP_STATUS_EEP: |
637 | 0 | debug_print("EEP"); |
638 | 0 | break; |
639 | 0 | case RMAP_STATUS_RESERVED: |
640 | 0 | debug_print("Reserved"); |
641 | 0 | break; |
642 | 0 | case RMAP_STATUS_VERIFY_BUFFER_OVERRRUN: |
643 | 0 | debug_print("Verify buffer overrun"); |
644 | 0 | break; |
645 | 0 | case RMAP_STATUS_CMD_NOT_IMPL_OR_AUTH: |
646 | 0 | debug_print("RMAP Command not implemented or not authorised"); |
647 | 0 | break; |
648 | 0 | case RMAP_STATUS_RMW_DATA_LEN_ERROR: |
649 | 0 | debug_print("RMW Data Length error"); |
650 | 0 | break; |
651 | 0 | case RMAP_STATUS_INVALID_TARGET_LOGICAL_ADDR: |
652 | 0 | debug_print("Invalid Target Logical Address"); |
653 | 0 | break; |
654 | 0 | default: |
655 | 0 | debug_print("Reserved unused error code %d", status); |
656 | 0 | break; |
657 | 0 | } |
658 | | |
659 | 0 | return status; |
660 | 0 | } |
661 | | |
662 | | |
663 | | |
664 | | static void rmap_process_read_reply(uint8_t *pkt) |
665 | 0 | { |
666 | 0 | uint32_t i; |
667 | |
|
668 | 0 | uint32_t len = 0; |
669 | | |
670 | |
|
671 | 0 | len |= ((uint32_t)pkt[RMAP_DATALEN_BYTE0]) << 16; |
672 | 0 | len |= ((uint32_t)pkt[RMAP_DATALEN_BYTE1]) << 8; |
673 | 0 | len |= ((uint32_t)pkt[RMAP_DATALEN_BYTE2]) << 0; |
674 | |
|
675 | | #if defined(__sparc__) |
676 | | debug_print("Data length is %lu bytes:", len); |
677 | | #else |
678 | 0 | debug_print("Data length is %u bytes:", len); |
679 | 0 | #endif |
680 | | |
681 | |
|
682 | 0 | for (i = 0; i < len; i++) |
683 | 0 | debug_print("%02x:", pkt[RMAP_DATA_START + i]); |
684 | 0 | } |
685 | | |
686 | | |
687 | | |
688 | | |
689 | | static void rmap_parse_cmd_pkt(uint8_t *pkt) |
690 | 0 | { |
691 | 0 | (void)pkt; |
692 | 0 | debug_print("rmap_parse_cmd_pkt() not implemented"); |
693 | 0 | } |
694 | | |
695 | | |
696 | | static void rmap_parse_reply_pkt(uint8_t *pkt) |
697 | 0 | { |
698 | 0 | struct rmap_instruction *ri; |
699 | | |
700 | |
|
701 | 0 | ri = (struct rmap_instruction *)&pkt[RMAP_INSTRUCTION]; |
702 | |
|
703 | 0 | debug_print("Instruction: "); |
704 | |
|
705 | 0 | switch (ri->cmd) { |
706 | | |
707 | 0 | case RMAP_READ_ADDR_SINGLE: |
708 | 0 | debug_print("Read single address"); |
709 | 0 | rmap_process_read_reply(pkt); |
710 | 0 | break; |
711 | 0 | case RMAP_READ_ADDR_INC: |
712 | 0 | debug_print("Read incrementing address"); |
713 | 0 | rmap_process_read_reply(pkt); |
714 | 0 | break; |
715 | 0 | case RMAP_READ_MODIFY_WRITE_ADDR_INC: |
716 | 0 | debug_print("RMW incrementing address verify reply"); |
717 | 0 | break; |
718 | 0 | case RMAP_WRITE_ADDR_INC_VERIFY_REPLY: |
719 | 0 | debug_print("Write incrementing address verify reply"); |
720 | 0 | break; |
721 | 0 | case RMAP_WRITE_ADDR_INC_REPLY: |
722 | 0 | debug_print("Write incrementing address reply"); |
723 | 0 | break; |
724 | 0 | default: |
725 | 0 | debug_print("decoding of instruction 0x%02X not implemented", |
726 | 0 | ri->cmd); |
727 | 0 | break; |
728 | 0 | } |
729 | 0 | } |
730 | | |
731 | | |
732 | | /** |
733 | | * parse an RMAP packet: |
734 | | * |
735 | | * expected format: [logical address] [protocol id] ... |
736 | | */ |
737 | | |
738 | | void rmap_parse_pkt(uint8_t *pkt) |
739 | 0 | { |
740 | 0 | struct rmap_instruction *ri; |
741 | |
|
742 | 0 | if (pkt[RMAP_PROTOCOL_ID] != RMAP_PROTOCOL_ID) { |
743 | 0 | debug_print("Not an RMAP packet, got %x but expected %x", |
744 | 0 | pkt[RMAP_PROTOCOL_ID], RMAP_PROTOCOL_ID); |
745 | 0 | return; |
746 | 0 | } |
747 | | |
748 | | |
749 | 0 | ri = (struct rmap_instruction *)&pkt[RMAP_INSTRUCTION]; |
750 | |
|
751 | 0 | if (ri->cmd_resp) { |
752 | 0 | debug_print("This is a command packet"); |
753 | 0 | if (!rmap_check_status(pkt[RMAP_REPLY_STATUS])) |
754 | 0 | rmap_parse_cmd_pkt(pkt); |
755 | 0 | } else { |
756 | 0 | debug_print("This is a reply packet"); |
757 | 0 | if (!rmap_check_status(pkt[RMAP_REPLY_STATUS])) |
758 | 0 | rmap_parse_reply_pkt(pkt); |
759 | 0 | } |
760 | 0 | } |
761 | | |