73 lines
2 KiB
C
73 lines
2 KiB
C
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <ctype.h>
|
|
|
|
#define MSG_HEADER_SIZE 4
|
|
#define MSG_MAX_SIZE 4096
|
|
|
|
typedef struct {
|
|
char type[MSG_HEADER_SIZE + 1];
|
|
int length;
|
|
char* payload;
|
|
} message_t;
|
|
|
|
int msg_create(const char* type, const void* data, int len, message_t* out) {
|
|
if (!type || !out) return -1;
|
|
strncpy(out->type, type, MSG_HEADER_SIZE);
|
|
out->type[MSG_HEADER_SIZE] = '\0';
|
|
out->length = len;
|
|
out->payload = malloc(len);
|
|
if (!out->payload && len > 0) return -1;
|
|
if (data && len > 0) memcpy(out->payload, data, len);
|
|
return 0;
|
|
}
|
|
|
|
int msg_encode(const message_t* msg, char* buf, size_t len) {
|
|
if (!msg || !buf) return -1;
|
|
int pos = 0;
|
|
pos += snprintf(buf + pos, len - pos, "%s|%d|", msg->type, msg->length);
|
|
if (msg->payload && msg->length > 0) {
|
|
memcpy(buf + pos, msg->payload, msg->length);
|
|
pos += msg->length;
|
|
}
|
|
return pos;
|
|
}
|
|
|
|
int msg_decode(const char* buf, size_t len, message_t* out) {
|
|
if (!buf || !out) return -1;
|
|
const char* pipe1 = strchr(buf, '|');
|
|
if (!pipe1) return -1;
|
|
size_t type_len = pipe1 - buf;
|
|
if (type_len > MSG_HEADER_SIZE) type_len = MSG_HEADER_SIZE;
|
|
strncpy(out->type, buf, type_len);
|
|
out->type[type_len] = '\0';
|
|
const char* pipe2 = strchr(pipe1 + 1, '|');
|
|
if (!pipe2) return -1;
|
|
sscanf(pipe1 + 1, "%d", &out->length);
|
|
const char* payload_start = pipe2 + 1;
|
|
size_t payload_len = len - (payload_start - buf);
|
|
if (payload_len > 0 && payload_len <= MSG_MAX_SIZE) {
|
|
out->payload = malloc(payload_len);
|
|
if (!out->payload) return -1;
|
|
memcpy(out->payload, payload_start, payload_len);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void msg_free(message_t* msg) {
|
|
if (msg) {
|
|
free(msg->payload);
|
|
msg->payload = NULL;
|
|
msg->length = 0;
|
|
}
|
|
}
|
|
|
|
int msg_checksum(const message_t* msg) {
|
|
if (!msg) return 0;
|
|
int sum = 0;
|
|
for (int i = 0; i < msg->length; i++) {
|
|
sum += (unsigned char)msg->payload[i];
|
|
}
|
|
return sum & 0xFFFF;
|
|
}
|