parent
81f6c8f2e9
commit
ab646c3a47
@ -1,8 +1,191 @@ |
||||
#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() |
||||
{ |
||||
printf("Hello, World!\n"); |
||||
#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; |
||||
} |
||||
|
||||
Loading…
Reference in new issue