parent
27a2489212
commit
ed87982a8e
@ -0,0 +1,88 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// modbus.cc is part of TestModbus-Client.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include "modbus.h"
|
||||
|
||||
#define PACK_INT8(__dstptr, __srci8) \
|
||||
memcpy(__dstptr, &__srci8, 1); \
|
||||
__dstptr += 1
|
||||
|
||||
#define UNPACK_INT8(__dsti8, __srcptr) \
|
||||
memcpy(&__dsti8, __srcptr, 1); \
|
||||
__srcptr += 1
|
||||
|
||||
#define PACK_INT16(__tmpi16, __dstptr, __srci16) \
|
||||
__tmpi16 = htons((uint16_t)__srci16); \
|
||||
memcpy(__dstptr, &__tmpi16, 2); \
|
||||
__dstptr += 2
|
||||
|
||||
#define UNPACK_INT16(__tmpi16, __dsti16, __srcptr) \
|
||||
memcpy(&__tmpi16, __srcptr, 2); \
|
||||
__srcptr += 2; \
|
||||
__dsti16 = ntohs((uint16_t)__tmpi16)
|
||||
|
||||
/*
|
||||
* packs the header into the buffer data and returns the lenght of the header
|
||||
* returns NULL on error or points to end of data
|
||||
*/
|
||||
char * modbustcp_pkt_header (char *destbuffer, int bufsize, modbustcp_header *srcmbh) {
|
||||
uint16_t tmp;
|
||||
|
||||
// destination memory valid
|
||||
if (destbuffer == NULL) {
|
||||
errno = EFAULT;
|
||||
return NULL;
|
||||
}
|
||||
if (bufsize < 12) {
|
||||
errno = EOVERFLOW;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
char *pos = destbuffer;
|
||||
|
||||
PACK_INT16(tmp, pos, srcmbh->transid);
|
||||
PACK_INT16(tmp, pos, srcmbh->protid);
|
||||
PACK_INT16(tmp, pos, srcmbh->length);
|
||||
PACK_INT8(pos, srcmbh->uid);
|
||||
PACK_INT8(pos, srcmbh->fc);
|
||||
PACK_INT16(tmp, pos, srcmbh->offset);
|
||||
PACK_INT16(tmp, pos, srcmbh->number);
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* unpacks the buffer into the header
|
||||
* returns NULL on error or points to end of data
|
||||
*/
|
||||
char * modbustcp_unpkt_header (modbustcp_header *destmbh, char *srcbuffer, int bufsize) {
|
||||
uint16_t tmp;
|
||||
|
||||
// source memory valid
|
||||
if (srcbuffer == NULL) {
|
||||
errno = EFAULT;
|
||||
return NULL;
|
||||
}
|
||||
if (bufsize < 12) {
|
||||
errno = EOVERFLOW;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
char *src = srcbuffer;
|
||||
|
||||
UNPACK_INT16(tmp, destmbh->transid, src);
|
||||
UNPACK_INT16(tmp, destmbh->protid, src);
|
||||
UNPACK_INT16(tmp, destmbh->length, src);
|
||||
UNPACK_INT8(destmbh->uid, src);
|
||||
UNPACK_INT8(destmbh->fc, src);
|
||||
UNPACK_INT16(tmp, destmbh->offset, src);
|
||||
UNPACK_INT16(tmp, destmbh->number, src);
|
||||
|
||||
return src;
|
||||
}
|
||||
@ -0,0 +1,33 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// modbus.h is part of TestModbus-Client.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef _MODBUS_H_
|
||||
#define _MODBUS_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
/*
|
||||
* this file will only contain the encoding and decoding functions
|
||||
*/
|
||||
|
||||
struct {
|
||||
uint16_t transid; // transaction id
|
||||
uint16_t protid; // protocol id
|
||||
uint16_t length; // number of bytes followed
|
||||
uint8_t uid; // unit/slave id
|
||||
uint8_t fc; // function code
|
||||
uint16_t offset; // first register to read
|
||||
uint16_t number; // number of registers to read
|
||||
} typedef modbustcp_header;
|
||||
|
||||
|
||||
char * modbustcp_pkt_header (char *destbuffer, int bufsize, modbustcp_header *srcmbh);
|
||||
char * modbustcp_unpkt_header (modbustcp_header *destmbh, char *srcbuffer, int bufsize);
|
||||
|
||||
|
||||
#endif
|
||||
Loading…
Reference in new issue