88 lines
3.2 KiB
C
88 lines
3.2 KiB
C
#include "set_trends.h"
|
|
#include "pcan_id.h"
|
|
|
|
static uint16_t get16(const uint8_t *p)
|
|
{
|
|
return (uint16_t)((uint16_t)p[0] | ((uint16_t)p[1] << 8));
|
|
}
|
|
|
|
static uint32_t get32(const uint8_t *p)
|
|
{
|
|
return (uint32_t)p[0] | ((uint32_t)p[1] << 8) |
|
|
((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
|
|
}
|
|
|
|
static void put16(uint8_t *p, uint16_t value)
|
|
{
|
|
p[0] = (uint8_t)value;
|
|
p[1] = (uint8_t)(value >> 8);
|
|
}
|
|
|
|
int32_t set_trend_word_value(uint16_t word, uint8_t is_signed)
|
|
{
|
|
return (is_signed != 0U && word >= 0x8000U) ? (int32_t)word - 65536 : (int32_t)word;
|
|
}
|
|
|
|
int32_t set_trend_can_value(
|
|
uint8_t source, uint32_t address, uint8_t device_type, uint8_t device,
|
|
uint8_t byte_offset, uint8_t extended, uint8_t is_signed,
|
|
uint32_t can_id, uint8_t flags, const uint8_t *data, size_t size)
|
|
{
|
|
size_t offset;
|
|
const uint8_t ide = (uint8_t)(flags & 1U);
|
|
if (data == NULL || size < 2U || size > 8U || (flags & 0x0EU) != 0U ||
|
|
can_id > (ide != 0U ? 0x1FFFFFFFUL : 0x7FFUL)) return SET_TREND_NO_VALUE;
|
|
if (source == SET_TREND_CAN_GAS) {
|
|
pcan_id_t id;
|
|
if (ide == 0U || (size & 1U) != 0U || address > 0xFFFFUL ||
|
|
device_type > 7U || device > 15U) return SET_TREND_NO_VALUE;
|
|
pcan_id_unpack(can_id, &id);
|
|
if (id.msg_type != PCAN_MSG_GAS || id.route != PCAN_ROUTE_FROM_DEVICE ||
|
|
id.device_type != device_type || id.device_id != device ||
|
|
address < id.msg_body) return SET_TREND_NO_VALUE;
|
|
offset = (size_t)(address - id.msg_body) * 2U;
|
|
} else if (source == SET_TREND_CAN_RAW) {
|
|
if (ide != (extended != 0U ? 1U : 0U) || can_id != address || byte_offset > 6U)
|
|
return SET_TREND_NO_VALUE;
|
|
offset = byte_offset;
|
|
} else return SET_TREND_NO_VALUE;
|
|
if (offset + 2U > size) return SET_TREND_NO_VALUE;
|
|
return set_trend_word_value(get16(data + offset), is_signed);
|
|
}
|
|
|
|
size_t set_trend_watch_request(uint16_t period_ms, const uint16_t *addresses,
|
|
size_t count, uint8_t *output, size_t capacity)
|
|
{
|
|
size_t i;
|
|
if (count > SET_TREND_WATCH_MAX || output == NULL || capacity < 4U + count * 2U ||
|
|
(count > 0U && addresses == NULL)) return 0U;
|
|
put16(output, period_ms);
|
|
put16(output + 2U, (uint16_t)count);
|
|
for (i = 0U; i < count; ++i) put16(output + 4U + i * 2U, addresses[i]);
|
|
return 4U + count * 2U;
|
|
}
|
|
|
|
int set_trend_watch_ack(const uint8_t *payload, size_t size, uint16_t period_ms, size_t count)
|
|
{
|
|
return payload != NULL && size == 4U && count <= SET_TREND_WATCH_MAX &&
|
|
get16(payload) == period_ms && get16(payload + 2U) == count;
|
|
}
|
|
|
|
int set_trend_watch_values(const uint8_t *payload, size_t size, uint16_t *words, size_t capacity)
|
|
{
|
|
return set_trend_watch_decode(payload, size, NULL, words, capacity);
|
|
}
|
|
|
|
int set_trend_watch_decode(const uint8_t *payload, size_t size, uint32_t *timestamp_ms,
|
|
uint16_t *words, size_t capacity)
|
|
{
|
|
size_t i, count;
|
|
if (payload == NULL || size < 6U) return -1;
|
|
count = get16(payload + 4U);
|
|
if (count > SET_TREND_WATCH_MAX || size != 6U + count * 2U || count > capacity ||
|
|
(count > 0U && words == NULL)) return -1;
|
|
for (i = 0U; i < count; ++i) words[i] = get16(payload + 6U + i * 2U);
|
|
if (timestamp_ms != NULL) *timestamp_ms = get32(payload);
|
|
return (int)count;
|
|
}
|