ProtoCore v0.0.2
Deterministic, zero-heap network stack for embedded targets
Loading...
Searching...
No Matches
iec60870.cpp
Go to the documentation of this file.
1// Copyright (C) 2026 Douglas Quigg (dstroy0) <dquigg123@gmail.com>
2// SPDX-License-Identifier: AGPL-3.0-or-later
3
4/**
5 * @file iec60870.cpp
6 * @brief IEC 60870-5-101 / -104 telecontrol codec (pure, host-tested).
7 */
8
9#include "services/energy/iec60870/iec60870.h"
10
11#if PC_ENABLE_IEC60870
12
13#include <string.h>
14
15// --- -104 APCI ---
16
17size_t pc_iec104_build_i(uint8_t *buf, size_t cap, uint16_t ns, uint16_t nr, const uint8_t *asdu, size_t asdu_len)
18{
19 if (!buf || (asdu_len && !asdu) || asdu_len > 249) // APDU length octet maxes at 253 (= 4 + 249)
20 {
21 return 0;
22 }
23 size_t total = IEC104_APCI_LEN + asdu_len;
24 if (cap < total)
25 {
26 return 0;
27 }
28 buf[0] = IEC_START_104;
29 buf[1] = (uint8_t)(4 + asdu_len); // APDU length: 4 control octets + ASDU
30 buf[2] = (uint8_t)((ns << 1) & 0xFEu); // I-format: bit0 of octet 1 is 0
31 buf[3] = (uint8_t)((ns >> 7) & 0xFFu);
32 buf[4] = (uint8_t)((nr << 1) & 0xFEu);
33 buf[5] = (uint8_t)((nr >> 7) & 0xFFu);
34 if (asdu_len)
35 {
36 memcpy(buf + 6, asdu, asdu_len);
37 }
38 return total;
39}
40
41size_t pc_iec104_build_s(uint8_t *buf, size_t cap, uint16_t nr)
42{
43 if (!buf || cap < IEC104_APCI_LEN)
44 {
45 return 0;
46 }
47 buf[0] = IEC_START_104;
48 buf[1] = 4;
49 buf[2] = 0x01; // S-format
50 buf[3] = 0x00;
51 buf[4] = (uint8_t)((nr << 1) & 0xFEu);
52 buf[5] = (uint8_t)((nr >> 7) & 0xFFu);
53 return IEC104_APCI_LEN;
54}
55
56size_t pc_iec104_build_u(uint8_t *buf, size_t cap, uint8_t u_cmd)
57{
58 if (!buf || cap < IEC104_APCI_LEN)
59 {
60 return 0;
61 }
62 buf[0] = IEC_START_104;
63 buf[1] = 4;
64 buf[2] = u_cmd; // U-format: bits 0-1 are 11 in every defined command
65 buf[3] = 0x00;
66 buf[4] = 0x00;
67 buf[5] = 0x00;
68 return IEC104_APCI_LEN;
69}
70
71bool pc_iec104_parse(const uint8_t *buf, size_t len, Iec104Apci *out, size_t *consumed)
72{
73 if (!buf || !out || len < 2 || buf[0] != IEC_START_104)
74 {
75 return false;
76 }
77 uint8_t L = buf[1];
78 if (L < 4)
79 {
80 return false;
81 }
82 size_t total = (size_t)2 + L;
83 if (len < total)
84 {
85 return false;
86 }
87 uint8_t c0 = buf[2];
88 out->ns = out->nr = 0;
89 out->u_cmd = 0;
90 out->asdu = nullptr;
91 out->asdu_len = 0;
92 if ((c0 & 0x01u) == 0) // I-format
93 {
94 out->format = Iec104Format::IEC104_I;
95 out->ns = (uint16_t)((buf[2] >> 1) | ((uint16_t)buf[3] << 7));
96 out->nr = (uint16_t)((buf[4] >> 1) | ((uint16_t)buf[5] << 7));
97 out->asdu_len = (size_t)(L - 4);
98 out->asdu = out->asdu_len ? buf + 6 : nullptr;
99 }
100 else if ((c0 & 0x03u) == 0x01u) // S-format
101 {
102 out->format = Iec104Format::IEC104_S;
103 out->nr = (uint16_t)((buf[4] >> 1) | ((uint16_t)buf[5] << 7));
104 }
105 else // U-format (bits 0-1 == 11)
106 {
107 out->format = Iec104Format::IEC104_U;
108 out->u_cmd = c0;
109 }
110 if (consumed)
111 {
112 *consumed = total;
113 }
114 return true;
115}
116
117// --- ASDU header + IOA ---
118
119size_t pc_iec_asdu_build_header(uint8_t *buf, size_t cap, const IecAsduHeader *h)
120{
121 if (!buf || !h || cap < 6)
122 {
123 return 0;
124 }
125 buf[0] = h->type_id;
126 buf[1] = (uint8_t)((h->sq ? 0x80u : 0u) | (h->count & 0x7Fu)); // variable structure qualifier
127 buf[2] = (uint8_t)((h->test ? 0x80u : 0u) | (h->negative ? 0x40u : 0u) | (h->cot & 0x3Fu));
128 buf[3] = h->orig_addr;
129 buf[4] = (uint8_t)(h->common_addr & 0xFFu); // common address, little-endian
130 buf[5] = (uint8_t)((h->common_addr >> 8) & 0xFFu);
131 return 6;
132}
133
134bool pc_iec_asdu_parse_header(const uint8_t *buf, size_t len, IecAsduHeader *out, size_t *consumed)
135{
136 if (!buf || !out || len < 6)
137 {
138 return false;
139 }
140 out->type_id = buf[0];
141 out->sq = (buf[1] & 0x80u) != 0;
142 out->count = (uint8_t)(buf[1] & 0x7Fu);
143 out->test = (buf[2] & 0x80u) != 0;
144 out->negative = (buf[2] & 0x40u) != 0;
145 out->cot = (uint8_t)(buf[2] & 0x3Fu);
146 out->orig_addr = buf[3];
147 out->common_addr = (uint16_t)(buf[4] | ((uint16_t)buf[5] << 8));
148 if (consumed)
149 {
150 *consumed = 6;
151 }
152 return true;
153}
154
155size_t pc_iec_put_ioa(uint8_t *buf, size_t cap, uint32_t ioa)
156{
157 if (!buf || cap < 3)
158 {
159 return 0;
160 }
161 buf[0] = (uint8_t)(ioa & 0xFFu);
162 buf[1] = (uint8_t)((ioa >> 8) & 0xFFu);
163 buf[2] = (uint8_t)((ioa >> 16) & 0xFFu);
164 return 3;
165}
166
167uint32_t pc_iec_get_ioa(const uint8_t *p)
168{
169 return (uint32_t)p[0] | ((uint32_t)p[1] << 8) | ((uint32_t)p[2] << 16);
170}
171
172// --- typed information objects ---
173
174size_t pc_iec_io_build_sp(uint8_t *buf, size_t cap, uint32_t ioa, bool on, uint8_t quality)
175{
176 if (!buf || cap < 4)
177 {
178 return 0;
179 }
180 pc_iec_put_ioa(buf, cap, ioa);
181 buf[3] = (uint8_t)((on ? 0x01u : 0u) | (quality & 0xF0u)); // SIQ: SPI (bit 0) + quality (bits 4..7)
182 return 4;
183}
184
185bool pc_iec_io_parse_sp(const uint8_t *buf, size_t len, uint32_t *ioa, bool *on, uint8_t *quality)
186{
187 if (!buf || len < 4)
188 {
189 return false;
190 }
191 if (ioa)
192 {
193 *ioa = pc_iec_get_ioa(buf);
194 }
195 if (on)
196 {
197 *on = (buf[3] & 0x01u) != 0;
198 }
199 if (quality)
200 {
201 *quality = (uint8_t)(buf[3] & 0xF0u);
202 }
203 return true;
204}
205
206size_t pc_iec_io_build_float(uint8_t *buf, size_t cap, uint32_t ioa, float value, uint8_t qds)
207{
208 if (!buf || cap < 8)
209 {
210 return 0;
211 }
212 pc_iec_put_ioa(buf, cap, ioa);
213 uint32_t bits;
214 memcpy(&bits, &value, 4); // the IEEE-754 bit pattern, written little-endian (endian-safe)
215 buf[3] = (uint8_t)bits;
216 buf[4] = (uint8_t)(bits >> 8);
217 buf[5] = (uint8_t)(bits >> 16);
218 buf[6] = (uint8_t)(bits >> 24);
219 buf[7] = qds;
220 return 8;
221}
222
223bool pc_iec_io_parse_float(const uint8_t *buf, size_t len, uint32_t *ioa, float *value, uint8_t *qds)
224{
225 if (!buf || len < 8)
226 {
227 return false;
228 }
229 if (ioa)
230 {
231 *ioa = pc_iec_get_ioa(buf);
232 }
233 if (value)
234 {
235 uint32_t bits =
236 (uint32_t)buf[3] | ((uint32_t)buf[4] << 8) | ((uint32_t)buf[5] << 16) | ((uint32_t)buf[6] << 24);
237 memcpy(value, &bits, 4);
238 }
239 if (qds)
240 {
241 *qds = buf[7];
242 }
243 return true;
244}
245
246size_t pc_iec_io_build_scaled(uint8_t *buf, size_t cap, uint32_t ioa, int16_t value, uint8_t qds)
247{
248 if (!buf || cap < 6)
249 {
250 return 0;
251 }
252 pc_iec_put_ioa(buf, cap, ioa);
253 uint16_t u = (uint16_t)value; // two's-complement wire form, little-endian
254 buf[3] = (uint8_t)u;
255 buf[4] = (uint8_t)(u >> 8);
256 buf[5] = qds;
257 return 6;
258}
259
260bool pc_iec_io_parse_scaled(const uint8_t *buf, size_t len, uint32_t *ioa, int16_t *value, uint8_t *qds)
261{
262 if (!buf || len < 6)
263 {
264 return false;
265 }
266 if (ioa)
267 {
268 *ioa = pc_iec_get_ioa(buf);
269 }
270 if (value)
271 {
272 *value = (int16_t)((uint16_t)buf[3] | ((uint16_t)buf[4] << 8));
273 }
274 if (qds)
275 {
276 *qds = buf[5];
277 }
278 return true;
279}
280
281size_t pc_iec_io_build_normalized(uint8_t *buf, size_t cap, uint32_t ioa, float value, uint8_t qds)
282{
283 if (!buf || cap < 6)
284 {
285 return 0;
286 }
287 pc_iec_put_ioa(buf, cap, ioa);
288 int32_t nva = (int32_t)(value * 32768.0f); // normalized fraction -> signed 16-bit, clamped to the field
289 if (nva > 32767)
290 {
291 nva = 32767;
292 }
293 if (nva < -32768)
294 {
295 nva = -32768;
296 }
297 uint16_t u = (uint16_t)(int16_t)nva;
298 buf[3] = (uint8_t)u;
299 buf[4] = (uint8_t)(u >> 8);
300 buf[5] = qds;
301 return 6;
302}
303
304bool pc_iec_io_parse_normalized(const uint8_t *buf, size_t len, uint32_t *ioa, float *value, uint8_t *qds)
305{
306 if (!buf || len < 6)
307 {
308 return false;
309 }
310 if (ioa)
311 {
312 *ioa = pc_iec_get_ioa(buf);
313 }
314 if (value)
315 {
316 int16_t nva = (int16_t)((uint16_t)buf[3] | ((uint16_t)buf[4] << 8));
317 *value = (float)nva / 32768.0f;
318 }
319 if (qds)
320 {
321 *qds = buf[5];
322 }
323 return true;
324}
325
326size_t pc_iec_io_build_counter(uint8_t *buf, size_t cap, uint32_t ioa, int32_t value, uint8_t seq)
327{
328 if (!buf || cap < 8)
329 {
330 return 0;
331 }
332 pc_iec_put_ioa(buf, cap, ioa);
333 uint32_t u = (uint32_t)value; // two's-complement wire form, little-endian
334 buf[3] = (uint8_t)u;
335 buf[4] = (uint8_t)(u >> 8);
336 buf[5] = (uint8_t)(u >> 16);
337 buf[6] = (uint8_t)(u >> 24);
338 buf[7] = seq; // sequence notation: SQ (bits 0..4) + CY / CA / IV
339 return 8;
340}
341
342bool pc_iec_io_parse_counter(const uint8_t *buf, size_t len, uint32_t *ioa, int32_t *value, uint8_t *seq)
343{
344 if (!buf || len < 8)
345 {
346 return false;
347 }
348 if (ioa)
349 {
350 *ioa = pc_iec_get_ioa(buf);
351 }
352 if (value)
353 {
354 *value =
355 (int32_t)((uint32_t)buf[3] | ((uint32_t)buf[4] << 8) | ((uint32_t)buf[5] << 16) | ((uint32_t)buf[6] << 24));
356 }
357 if (seq)
358 {
359 *seq = buf[7];
360 }
361 return true;
362}
363
364size_t pc_iec_io_build_sc(uint8_t *buf, size_t cap, uint32_t ioa, bool on, bool select)
365{
366 if (!buf || cap < 4)
367 {
368 return 0;
369 }
370 pc_iec_put_ioa(buf, cap, ioa);
371 buf[3] = (uint8_t)((on ? IEC_SCO_ON : 0u) | (select ? IEC_SCO_SE : 0u)); // SCO: SCS (bit 0) + S/E (bit 7)
372 return 4;
373}
374
375bool pc_iec_io_parse_sc(const uint8_t *buf, size_t len, uint32_t *ioa, bool *on, bool *select)
376{
377 if (!buf || len < 4)
378 {
379 return false;
380 }
381 if (ioa)
382 {
383 *ioa = pc_iec_get_ioa(buf);
384 }
385 if (on)
386 {
387 *on = (buf[3] & IEC_SCO_ON) != 0;
388 }
389 if (select)
390 {
391 *select = (buf[3] & IEC_SCO_SE) != 0;
392 }
393 return true;
394}
395
396size_t pc_iec_io_build_dp(uint8_t *buf, size_t cap, uint32_t ioa, uint8_t dpi, uint8_t quality)
397{
398 if (!buf || cap < 4)
399 {
400 return 0;
401 }
402 pc_iec_put_ioa(buf, cap, ioa);
403 buf[3] = (uint8_t)((dpi & IEC_DP_MASK) | (quality & 0xF0u)); // DIQ: DPI (bits 0..1) + quality (bits 4..7)
404 return 4;
405}
406
407bool pc_iec_io_parse_dp(const uint8_t *buf, size_t len, uint32_t *ioa, uint8_t *dpi, uint8_t *quality)
408{
409 if (!buf || len < 4)
410 {
411 return false;
412 }
413 if (ioa)
414 {
415 *ioa = pc_iec_get_ioa(buf);
416 }
417 if (dpi)
418 {
419 *dpi = (uint8_t)(buf[3] & IEC_DP_MASK);
420 }
421 if (quality)
422 {
423 *quality = (uint8_t)(buf[3] & 0xF0u);
424 }
425 return true;
426}
427
428size_t pc_iec_io_build_dc(uint8_t *buf, size_t cap, uint32_t ioa, uint8_t dcs, uint8_t qu, bool select)
429{
430 if (!buf || cap < 4)
431 {
432 return 0;
433 }
434 pc_iec_put_ioa(buf, cap, ioa);
435 // DCO: DCS (bits 0..1) + QU (bits 2..6) + S/E (bit 7).
436 buf[3] = (uint8_t)((dcs & IEC_DP_MASK) | ((qu & IEC_DCO_QU_MASK) << IEC_DCO_QU_SHIFT) | (select ? IEC_DCO_SE : 0u));
437 return 4;
438}
439
440bool pc_iec_io_parse_dc(const uint8_t *buf, size_t len, uint32_t *ioa, uint8_t *dcs, uint8_t *qu, bool *select)
441{
442 if (!buf || len < 4)
443 {
444 return false;
445 }
446 if (ioa)
447 {
448 *ioa = pc_iec_get_ioa(buf);
449 }
450 if (dcs)
451 {
452 *dcs = (uint8_t)(buf[3] & IEC_DP_MASK);
453 }
454 if (qu)
455 {
456 *qu = (uint8_t)((buf[3] >> IEC_DCO_QU_SHIFT) & IEC_DCO_QU_MASK);
457 }
458 if (select)
459 {
460 *select = (buf[3] & IEC_DCO_SE) != 0;
461 }
462 return true;
463}
464
465// --- -101 FT1.2 link frames ---
466
467static uint8_t sum8(const uint8_t *p, size_t n)
468{
469 uint8_t s = 0;
470 for (size_t i = 0; i < n; i++)
471 {
472 s = (uint8_t)(s + p[i]);
473 }
474 return s;
475}
476
477size_t pc_iec101_build_fixed(uint8_t *buf, size_t cap, uint8_t control, uint8_t addr)
478{
479 if (!buf || cap < 5)
480 {
481 return 0;
482 }
483 buf[0] = IEC_START_FIXED;
484 buf[1] = control;
485 buf[2] = addr;
486 buf[3] = (uint8_t)(control + addr); // checksum over control + address
487 buf[4] = IEC_STOP;
488 return 5;
489}
490
491size_t pc_iec101_build_variable(uint8_t *buf, size_t cap, uint8_t control, uint8_t addr, const uint8_t *asdu,
492 uint8_t asdu_len)
493{
494 if (!buf || (asdu_len && !asdu) || asdu_len > 253)
495 {
496 return 0;
497 }
498 uint8_t L = (uint8_t)(2 + asdu_len); // L counts control + address + ASDU
499 size_t total = (size_t)6 + L;
500 if (cap < total)
501 {
502 return 0;
503 }
504 buf[0] = IEC_START_104; // 0x68
505 buf[1] = L;
506 buf[2] = L;
507 buf[3] = IEC_START_104;
508 buf[4] = control;
509 buf[5] = addr;
510 if (asdu_len)
511 {
512 memcpy(buf + 6, asdu, asdu_len);
513 }
514 buf[4 + L] = sum8(buf + 4, L); // checksum over control..end of ASDU
515 buf[5 + L] = IEC_STOP;
516 return total;
517}
518
519bool pc_iec101_parse(const uint8_t *buf, size_t len, Iec101Frame *out, size_t *consumed)
520{
521 if (!buf || !out || len < 1)
522 {
523 return false;
524 }
525 out->fixed = false;
526 out->control = out->addr = 0;
527 out->asdu = nullptr;
528 out->asdu_len = 0;
529
530 if (buf[0] == IEC_START_FIXED)
531 {
532 if (len < 5 || buf[4] != IEC_STOP || buf[3] != (uint8_t)(buf[1] + buf[2]))
533 {
534 return false;
535 }
536 out->fixed = true;
537 out->control = buf[1];
538 out->addr = buf[2];
539 if (consumed)
540 {
541 *consumed = 5;
542 }
543 return true;
544 }
545 if (buf[0] == IEC_START_104)
546 {
547 if (len < 4)
548 {
549 return false;
550 }
551 uint8_t L = buf[1];
552 if (L < 2 || buf[2] != L || buf[3] != IEC_START_104)
553 {
554 return false;
555 }
556 size_t total = (size_t)6 + L;
557 if (len < total || buf[5 + L] != IEC_STOP || sum8(buf + 4, L) != buf[4 + L])
558 {
559 return false;
560 }
561 out->control = buf[4];
562 out->addr = buf[5];
563 out->asdu_len = (uint8_t)(L - 2);
564 out->asdu = out->asdu_len ? buf + 6 : nullptr;
565 if (consumed)
566 {
567 *consumed = total;
568 }
569 return true;
570 }
571 return false;
572}
573
574#endif // PC_ENABLE_IEC60870
iec60870.h
IEC 60870-5-101 / -104 telecontrol (SCADA) codec (PC_ENABLE_IEC60870).