81 lines
2.7 KiB
C
81 lines
2.7 KiB
C
#include "set_devboard_v1_stm32f407.h"
|
|
|
|
#include <string.h>
|
|
|
|
static bool transmit(set_devboard_v1_stm32f407_t *port,
|
|
const pcan_frame_t *frame)
|
|
{
|
|
CAN_TxHeaderTypeDef header;
|
|
uint32_t mailbox = 0U;
|
|
uint8_t data[8] = {0U};
|
|
|
|
memset(&header, 0, sizeof(header));
|
|
header.ExtId = frame->id & 0x1FFFFFFFUL;
|
|
header.IDE = CAN_ID_EXT;
|
|
header.RTR = ((frame->flags & PCAN_FLAG_RTR) != 0U) ?
|
|
CAN_RTR_REMOTE : CAN_RTR_DATA;
|
|
header.DLC = frame->dlc;
|
|
header.TransmitGlobalTime = DISABLE;
|
|
memcpy(data, frame->data, frame->dlc);
|
|
if (HAL_CAN_AddTxMessage(port->can, &header, data, &mailbox) != HAL_OK) {
|
|
port->tx_errors++;
|
|
return false;
|
|
}
|
|
port->tx_frames++;
|
|
return true;
|
|
}
|
|
|
|
bool set_devboard_v1_stm32f407_init(
|
|
set_devboard_v1_stm32f407_t *port, CAN_HandleTypeDef *can,
|
|
uint8_t device_type, uint8_t device_id,
|
|
pcan_modbus_read_fn read, pcan_modbus_write_fn write, void *register_user,
|
|
set_devboard_v1_unhandled_fn unhandled, void *unhandled_user)
|
|
{
|
|
if ((port == NULL) || (can == NULL)) {
|
|
return false;
|
|
}
|
|
memset(port, 0, sizeof(*port));
|
|
port->can = can;
|
|
port->unhandled = unhandled;
|
|
port->unhandled_user = unhandled_user;
|
|
return pcan_modbus_server_init(&port->server, device_type, device_id,
|
|
read, write, register_user);
|
|
}
|
|
|
|
size_t set_devboard_v1_stm32f407_poll(set_devboard_v1_stm32f407_t *port)
|
|
{
|
|
size_t handled = 0U;
|
|
if ((port == NULL) || (port->can == NULL)) {
|
|
return 0U;
|
|
}
|
|
while (HAL_CAN_GetRxFifoFillLevel(port->can, CAN_RX_FIFO0) != 0U) {
|
|
CAN_RxHeaderTypeDef header;
|
|
uint8_t data[8] = {0U};
|
|
pcan_frame_t request = {0};
|
|
pcan_frame_t response;
|
|
pcan_modbus_result_t result;
|
|
|
|
if (HAL_CAN_GetRxMessage(port->can, CAN_RX_FIFO0, &header, data) != HAL_OK) {
|
|
break;
|
|
}
|
|
port->rx_frames++;
|
|
request.id = (header.IDE == CAN_ID_EXT) ? header.ExtId : header.StdId;
|
|
request.flags = (header.IDE == CAN_ID_EXT) ? PCAN_FLAG_IDE : 0U;
|
|
if (header.RTR == CAN_RTR_REMOTE) {
|
|
request.flags |= PCAN_FLAG_RTR;
|
|
}
|
|
request.dlc = (header.DLC > 8U) ? 8U : (uint8_t)header.DLC;
|
|
memcpy(request.data, data, request.dlc);
|
|
|
|
result = pcan_modbus_server_handle(&port->server, &request, &response);
|
|
if (result == PCAN_MODBUS_RESPONSE) {
|
|
(void)transmit(port, &response);
|
|
handled++;
|
|
} else if ((result == PCAN_MODBUS_NOT_FOR_US) &&
|
|
(port->unhandled != NULL)) {
|
|
port->unhandled(port->unhandled_user, &header, data);
|
|
}
|
|
}
|
|
return handled;
|
|
}
|