You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
191 lines
4.6 KiB
191 lines
4.6 KiB
#include <stdio.h>
|
|
#include "modbus.h"
|
|
|
|
/* https://gist.github.com/ccbrown/9722406 */
|
|
void hexdump(const void *data, size_t size)
|
|
{
|
|
char ascii[17];
|
|
size_t i, j;
|
|
ascii[16] = '\0';
|
|
for (i = 0; i < size; ++i) {
|
|
printf("%02X ", ((unsigned char *) data)[i]);
|
|
const uint8_t c = ((unsigned char *) data)[i];
|
|
if (c >= ' ' && c <= '~') {
|
|
ascii[i % 16] = (char) c;
|
|
} else {
|
|
ascii[i % 16] = '.';
|
|
}
|
|
if ((i + 1) % 8 == 0 || i + 1 == size) {
|
|
printf(" ");
|
|
if ((i + 1) % 16 == 0) {
|
|
printf("| %s \n", ascii);
|
|
} else if (i + 1 == size) {
|
|
ascii[(i + 1) % 16] = '\0';
|
|
if ((i + 1) % 16 <= 8) {
|
|
printf(" ");
|
|
}
|
|
for (j = (i + 1) % 16; j < 16; ++j) {
|
|
printf(" ");
|
|
}
|
|
printf("| %s \n", ascii);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static ssize_t parsehex(const char *s, uint8_t *out, size_t cap)
|
|
{
|
|
uint8_t buf = 0;
|
|
uint8_t v;
|
|
bool first = true;
|
|
char c;
|
|
size_t sz = 0;
|
|
while (0 != (c = *s++)) {
|
|
if (c >= '0' && c <= '9') {
|
|
v = c - '0';
|
|
} else if (c >= 'a' && c <= 'f') {
|
|
v = 10 + (c - 'a');
|
|
} else if (c >= 'A' && c <= 'F') {
|
|
v = 10 + (c - 'A');
|
|
} else {
|
|
continue;
|
|
}
|
|
|
|
if (first) {
|
|
buf |= (v & 15) << 4;
|
|
first = false;
|
|
} else {
|
|
buf |= (v & 15);
|
|
if (cap == 0) {
|
|
return -1;
|
|
}
|
|
out[sz++] = buf;
|
|
buf = 0;
|
|
first = true;
|
|
cap--;
|
|
}
|
|
}
|
|
return (ssize_t) sz;
|
|
}
|
|
|
|
|
|
ModbusException_t soa(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t slave_id) {
|
|
printf("Start of access: fc%02d, slave %d\n", fcx, slave_id);
|
|
return 0;
|
|
}
|
|
void eoa(ModbusSlave_t *ms) {
|
|
printf("End of access\n");
|
|
}
|
|
ModbusException_t rh(ModbusSlave_t *ms, uint16_t i, uint16_t *out) {
|
|
printf("rh %d\n", i);
|
|
switch (i) {
|
|
case 107:
|
|
*out = 0xAE41;
|
|
break;
|
|
case 108:
|
|
*out = 0x5652;
|
|
break;
|
|
case 109:
|
|
*out = 0x4340;
|
|
break;
|
|
default:
|
|
return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
|
|
}
|
|
return 0;
|
|
}
|
|
ModbusException_t ri(ModbusSlave_t *ms, uint16_t i, uint16_t *out) {
|
|
printf("ri %d\n", i);
|
|
switch (i) {
|
|
case 8:
|
|
*out = 0xa;
|
|
break;
|
|
default:
|
|
return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
|
|
}
|
|
return 0;
|
|
}
|
|
ModbusException_t rc(ModbusSlave_t *ms, uint16_t i, bool *out) {
|
|
uint8_t store[] = {
|
|
0b11001101,
|
|
0b01101011,
|
|
0b10110010,
|
|
0b00001110,
|
|
0b00011011,
|
|
};
|
|
|
|
if (i >= 19 && i <= 55) {
|
|
int pos = i-19;
|
|
*out = 0 != (store[pos/8] & (1 << (pos%8)));
|
|
} else {
|
|
return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
|
|
}
|
|
return 0;
|
|
}
|
|
ModbusException_t rd(ModbusSlave_t *ms, uint16_t i, bool *out) {
|
|
uint8_t store[] = {
|
|
0b10101100,
|
|
0b11011011,
|
|
0b00110101,
|
|
};
|
|
|
|
if (i >= 196 && i <= 217) {
|
|
int pos = i - 196;
|
|
*out = 0 != (store[pos/8] & (1 << (pos%8)));
|
|
} else {
|
|
return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
ModbusException_t wc(ModbusSlave_t *pSlave, uint16_t i, bool b) {
|
|
printf("Write coil %d <- %d\n", i, b);
|
|
return 0;
|
|
}
|
|
ModbusException_t wh(ModbusSlave_t *pSlave, uint16_t i, uint16_t i1) {
|
|
printf("Write reg %d <- %d\n", i, i1);
|
|
return 0;
|
|
}
|
|
|
|
int main()
|
|
{
|
|
#define buflen 1024
|
|
uint8_t buf[buflen];
|
|
uint8_t buf2[buflen];
|
|
|
|
// 11 03 006B 0003 7687
|
|
// 11 04 0008 0001 B298
|
|
// 11 01 0013 0025 0E84
|
|
// 11 02 00C4 0016 BAA9
|
|
// 11 05 00AC FF00 4E8B
|
|
// 11 06 0001 0003 9A9B
|
|
// 11 0F 0013 000A 02 CD01 BF0B
|
|
// 11 10 0001 0002 04 000A 0102 C6F0
|
|
ssize_t pldlen = parsehex("11 03 006B 0003 7687", buf, buflen);
|
|
|
|
ModbusSlave_t ms = {
|
|
.addr = 17,
|
|
.proto = MB_PROTO_RTU,
|
|
.startOfAccess = soa,
|
|
.endOfAccess = eoa,
|
|
.readHolding = rh,
|
|
.readInput = ri,
|
|
.readCoil = rc,
|
|
.readDiscrete = rd,
|
|
.writeCoil = wc,
|
|
.writeHolding = wh,
|
|
};
|
|
|
|
printf("Req:\n");
|
|
hexdump(buf, pldlen);
|
|
|
|
size_t respsize;
|
|
ModbusError_t e = mb_handleRequest(&ms, buf, pldlen, buf2, buflen, &respsize);
|
|
if (e) {
|
|
printf("Err %d\n", e);
|
|
} else {
|
|
printf("Resp:\n");
|
|
hexdump(buf2, respsize);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|