int eps_slave_hk2(struct command_context *ctx) {
printf("Requesting EPS HK2 data\r\n");
eps_hk_t * chkparam;
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = EPS_PORT_HK;
frame->data[1] = 0;
frame->len = 2;
frame->len_rx = 2 + (uint8_t) sizeof(eps_hk_t);
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
if (i2c_receive(0, &frame, 20) != E_NO_ERR)
return CMD_ERROR_FAIL;
chkparam = (eps_hk_t *)&frame->data[2];
eps_hk_unpack(chkparam);
eps_hk_print(chkparam);
csp_buffer_free(frame);
return CMD_ERROR_NONE;
}
int eps_slave_ping(struct command_context *ctx) {
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = CSP_PING; // Ping port
frame->data[1] = 0x55;
frame->len = 2;
frame->len_rx = 3;
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
if (i2c_receive(0, &frame, 20) == E_NO_ERR) {
printf("Received a reply from EPS!\r\n");
} else {
printf("No reply from EPS\r\n");
}
csp_buffer_free(frame);
return CMD_ERROR_NONE;
}
void csp_ping_noreply(uint8_t node) {
/* Prepare data */
csp_packet_t * packet;
packet = csp_buffer_get(1);
/* Check malloc */
if (packet == NULL)
return;
/* Open connection */
csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, 0, 0);
if (conn == NULL) {
csp_buffer_free(packet);
return;
}
packet->data[0] = 0x55;
packet->length = 1;
printf("Ping ignore reply node %u.\r\n", node);
/* Try to send frame */
if (!csp_send(conn, packet, 0))
csp_buffer_free(packet);
csp_close(conn);
}
int csp_sfp_recv(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout) {
unsigned int last_byte = 0;
csp_packet_t * packet;
while((packet = csp_read(conn, timeout)) != NULL) {
/* Check that SFP header is present */
if ((packet->id.flags & CSP_FFRAG) == 0) {
csp_debug(CSP_ERROR, "Missing SFP header\r\n");
return -1;
}
/* Read SFP header */
sfp_header_t * sfp_header = csp_sfp_header_remove(packet);
sfp_header->offset = csp_ntoh32(sfp_header->offset);
sfp_header->totalsize = csp_ntoh32(sfp_header->totalsize);
csp_debug(CSP_PROTOCOL, "SFP fragment %u/%u\r\n", sfp_header->offset + packet->length, sfp_header->totalsize);
if (sfp_header->offset > last_byte + 1) {
csp_debug(CSP_ERROR, "SFP missing %u bytes\r\n", sfp_header->offset - last_byte);
csp_buffer_free(packet);
return -1;
} else {
last_byte = sfp_header->offset + packet->length;
}
/* Allocate memory */
if (*dataout == NULL)
*dataout = csp_malloc(sfp_header->totalsize);
*datasize = sfp_header->totalsize;
/* Copy data to output */
if (*dataout != NULL)
memcpy(*dataout + sfp_header->offset, packet->data, packet->length);
if (sfp_header->offset + packet->length >= sfp_header->totalsize) {
csp_debug(CSP_PROTOCOL, "SFP complete\r\n");
csp_buffer_free(packet);
return 0;
} else {
csp_buffer_free(packet);
}
}
return -1;
}
int eps_slave_output(struct command_context *ctx) {
char * args = command_args(ctx);
unsigned int outputs;
printf("console args: %s\r\n", args);
if (sscanf(args, "%X", &outputs) != 1)
return CMD_ERROR_SYNTAX;
printf("Outputs 0x%02X\r\n", outputs);
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = EPS_PORT_SET_OUTPUT; // Ping port
frame->data[1] = outputs;
frame->len = 2;
frame->len_rx = 0;
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
return CMD_ERROR_NONE;
}
int eps_slave_single_output(struct command_context *ctx) {
char * args = command_args(ctx);
unsigned int channel;
unsigned int mode;
int delay;
printf("Input channel, mode (0=off, 1=on), and delay\r\n");
if (sscanf(args, "%u %u %d", &channel, &mode, &delay) != 3)
return CMD_ERROR_SYNTAX;
printf("Channel %d is set to %d with delay %d\r\n", channel, mode, delay);
eps_output_set_single_req eps_switch;
eps_switch.channel = (uint8_t)channel;
eps_switch.mode = (uint8_t)mode;
eps_switch.delay = csp_hton16((int16_t)delay);
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = EPS_PORT_SET_SINGLE_OUTPUT; // Ping port
memcpy(&frame->data[1], &eps_switch, sizeof(eps_switch));
frame->len = 1 + sizeof(eps_switch);
frame->len_rx = 0;
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
return CMD_ERROR_NONE;
}
static int csp_tiradio_tx (csp_iface_t *interface,
csp_packet_t *packet, uint32_t timeout) {
int ret = CSP_ERR_NONE;
int txbufin = packet->length + CSP_HEADER_LENGTH;
uint8_t *txbuf = csp_malloc(txbufin);
csp_tiradio_driver_handle_t *driver = interface->driver;
if (txbuf == NULL)
return CSP_ERR_NOMEM;
if (driver->module_id < NUM_TIRADIO_MODULES)
latest_csp_transfer_id[driver->module_id] = packet->id;
/* Save the outgoing id in the buffer */
packet->id.ext = csp_hton32(packet->id.ext);
memcpy(txbuf, &packet->id.ext, txbufin);
/* The packet goes straight to the radio. */
if (driver->putstr(driver->module_id,txbuf, txbufin) != 0) {
interface->tx_error++;
ret = CSP_ERR_TIMEDOUT;
} else {
csp_buffer_free(packet);
}
csp_free(txbuf);
return ret;
}
int eps_slave_volt(struct command_context *ctx) {
char * args = command_args(ctx);
unsigned int pv1, pv2, pv3;
if (sscanf(args, "%u %u %u", &pv1, &pv2, &pv3) != 3)
return CMD_ERROR_SYNTAX;
printf("PV1: %04d PV2: %04d PV3: %04d\r\n", pv1, pv2, pv3);
uint16_t pvolt[3];
pvolt[0] = csp_hton16(pv1);
pvolt[1] = csp_hton16(pv2);
pvolt[2] = csp_hton16(pv3);
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = EPS_PORT_SET_VBOOST; // Ping port
memcpy(&frame->data[1], &pvolt, 3 * sizeof(uint16_t));
frame->len = 1 + 3 * sizeof(uint16_t);
frame->len_rx = 0;
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
return CMD_ERROR_NONE;
}
int eps_slave_ppt_mode(struct command_context *ctx) {
char * args = command_args(ctx);
unsigned int mode;
printf("EPS_PPT_MODE_OFF = 0, EPS_PPT_MODE_AUTO = 1, EPS_PPT_MODE_FIXED= 2\r\n");
if (sscanf(args, "%d", &mode) != 1)
return CMD_ERROR_SYNTAX;
printf("Mode %d\r\n", mode);
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = EPS_PORT_SET_PPTMODE; // Ping port
frame->data[1] = (uint8_t)mode;
frame->len = 2;
frame->len_rx = 0;
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
return CMD_ERROR_NONE;
}
int csp_fifo_tx(csp_iface_t *ifc, csp_packet_t *packet, uint32_t timeout) {
/* Write packet to fifo */
if (write(tx_channel, &packet->length, packet->length + sizeof(uint32_t) + sizeof(uint16_t)) < 0)
printf("Failed to write frame\r\n");
csp_buffer_free(packet);
return CSP_ERR_NONE;
}
/**
* Context: Task only
*/
int i2c_master_transaction(int handle, uint8_t addr, void * txbuf, size_t txlen, void * rxbuf, size_t rxlen, uint16_t timeout) {
if (handle >= pca9665_device_count)
return E_NO_DEVICE;
if (!device[handle].is_initialised)
return E_NO_DEVICE;
if ((txlen > I2C_MTU - 10) || (rxlen > I2C_MTU - 10))
return E_INVALID_BUF_SIZE;
i2c_frame_t * frame = csp_buffer_get(I2C_MTU);
if (frame == NULL)
return E_NO_BUFFER;
xSemaphoreTake(i2c_lock, 10 * configTICK_RATE_HZ);
frame->dest = addr;
memcpy(&frame->data[0], txbuf, txlen);
frame->len = txlen;
frame->len_rx = rxlen;
if (i2c_send(handle, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
xSemaphoreGive(i2c_lock);
return E_TIMEOUT;
}
if (rxlen == 0) {
xSemaphoreGive(i2c_lock);
return E_NO_ERR;
}
if (i2c_receive(handle, &frame, timeout) != E_NO_ERR) {
xSemaphoreGive(i2c_lock);
return E_TIMEOUT;
}
memcpy(rxbuf, &frame->data[0], rxlen);
csp_buffer_free(frame);
xSemaphoreGive(i2c_lock);
return E_NO_ERR;
}
/**
* Use an existing connection to perform a transaction,
* This is only possible if the next packet is on the same port and destination!
* @param conn pointer to connection structure
* @param timeout timeout in ms
* @param outbuf pointer to outgoing data buffer
* @param outlen length of request to send
* @param inbuf pointer to incoming data buffer
* @param inlen length of expected reply, -1 for unknown size (note inbuf MUST be large enough)
* @return
*/
int csp_transaction_persistent(csp_conn_t * conn, unsigned int timeout, void * outbuf, int outlen, void * inbuf, int inlen) {
/* Stupid way to implement max() but more portable than macros */
int size = outlen;
if (inlen > outlen)
size = inlen;
csp_packet_t * packet = csp_buffer_get(size);
if (packet == NULL)
return 0;
/* Copy the request */
if (outlen > 0 && outbuf != NULL)
memcpy(packet->data, outbuf, outlen);
packet->length = outlen;
if (!csp_send(conn, packet, timeout)) {
printf("Send failed\r\n");
csp_buffer_free(packet);
return 0;
}
/* If no reply is expected, return now */
if (inlen == 0)
return 1;
packet = csp_read(conn, timeout);
if (packet == NULL) {
printf("Read failed\r\n");
return 0;
}
if ((inlen != -1) && (packet->length != inlen)) {
printf("Reply length %u expected %u\r\n", packet->length, inlen);
csp_buffer_free(packet);
return 0;
}
memcpy(inbuf, packet->data, packet->length);
int length = packet->length;
csp_buffer_free(packet);
return length;
}
int csp_ping(uint8_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) {
int i;
uint32_t start, time, status = 0;
/* Counter */
start = csp_get_ms();
/* Open connection */
csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, timeout, conn_options);
if (conn == NULL)
return -1;
/* Prepare data */
csp_packet_t * packet;
packet = csp_buffer_get(size);
if (packet == NULL)
goto out;
/* Set data to increasing numbers */
packet->length = size;
for (i = 0; i < size; i++)
packet->data[i] = i;
/* Try to send frame */
if (!csp_send(conn, packet, 0))
goto out;
/* Read incoming frame */
packet = csp_read(conn, timeout);
if (packet == NULL)
goto out;
/* Ensure that the data was actually echoed */
for (i = 0; i < size; i++)
if (packet->data[i] != i)
goto out;
status = 1;
out:
/* Clean up */
if (packet != NULL)
csp_buffer_free(packet);
csp_close(conn);
/* We have a reply */
time = (csp_get_ms() - start);
if (status) {
return time;
} else {
return -1;
}
}
int csp_fifo_tx(csp_packet_t *packet, uint32_t timeout) {
printf("csp_fifo_tx tid: %lu\n", GetCurrentThreadId());
DWORD expectedSent = packet->length + sizeof(uint32_t) + sizeof(uint16_t);
DWORD actualSent;
/* Write packet to fifo */
if( !WriteFile(pipe, &packet->length, expectedSent, &actualSent, NULL)
|| actualSent != expectedSent ) {
printError();
}
csp_buffer_free(packet);
return CSP_ERR_NONE;
}
int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen) {
int size = (inlen > outlen) ? inlen : outlen;
csp_packet_t * packet = csp_buffer_get(size);
if (packet == NULL)
return 0;
/* Copy the request */
if (outlen > 0 && outbuf != NULL)
memcpy(packet->data, outbuf, outlen);
packet->length = outlen;
if (!csp_send(conn, packet, timeout)) {
csp_buffer_free(packet);
return 0;
}
/* If no reply is expected, return now */
if (inlen == 0)
{
return 1;
}
packet = csp_read(conn, timeout);
if (packet == NULL)
return 0;
if ((inlen != -1) && ((int)packet->length != inlen)) {
csp_log_error("Reply length %u expected %u\r\n", packet->length, inlen);
csp_buffer_free(packet);
return 0;
}
memcpy(inbuf, packet->data, packet->length);
int length = packet->length;
csp_buffer_free(packet);
return length;
}
int send_test_packet(void) {
csp_packet_t * packet = csp_buffer_get(100);
char * str = "Hello World\r\n";
sprintf((char *) packet->data, str);
packet->length = strlen(str) + 1;
if (csp_sendto(CSP_PRIO_NORM, NODE_OBC, OBC_PORT_CADCS_HK, 0, CSP_O_CRC32 | CSP_O_HMAC | CSP_O_XTEA, packet, 0) < 0) {
//printf("Error sending packet\r\n");
csp_buffer_free(packet);
}
return 0;
}
void csp_server(void *p) {
(void) p;
/* Create socket without any socket options */
csp_socket_t *sock = csp_socket(CSP_SO_NONE);
/* Bind all ports to socket */
csp_bind(sock, CSP_ANY);
/* Create 10 connections backlog queue */
csp_listen(sock, 10);
/* Pointer to current connection and packet */
csp_conn_t *conn;
csp_packet_t *packet;
/* Process incoming connections */
while (1) {
/* Wait for connection, 10000 ms timeout */
if ((conn = csp_accept(sock, 10000)) == NULL)
continue;
/* Read packets. Timout is 100 ms */
while ((packet = csp_read(conn, 100)) != NULL) {
switch (csp_conn_dport(conn)) {
case MY_PORT:
/* Process packet here */
blink(K_LED_GREEN);
csp_buffer_free(packet);
break;
default:
/* Let the service handler reply pings, buffer use, etc. */
#ifdef TARGET_LIKE_MSP430
blink(K_LED_GREEN);
blink(K_LED_GREEN);
#else
blink(K_LED_BLUE);
#endif
csp_service_handler(conn, packet);
break;
}
}
/* Close current connection, and handle next */
csp_close(conn);
}
}
void csp_new_packet(csp_packet_t * packet, csp_iface_t * interface, CSP_BASE_TYPE * pxTaskWoken) {
int result, fifo;
if (packet == NULL) {
csp_log_warn("csp_new packet called with NULL packet\r\n");
return;
} else if (interface == NULL) {
csp_log_warn("csp_new packet called with NULL interface\r\n");
if (pxTaskWoken == NULL)
csp_buffer_free(packet);
else
csp_buffer_free_isr(packet);
return;
}
csp_route_queue_t queue_element;
queue_element.interface = interface;
queue_element.packet = packet;
fifo = csp_route_get_fifo(packet->id.pri);
result = csp_route_enqueue(router_input_fifo[fifo], &queue_element, 0, pxTaskWoken);
if (result != CSP_ERR_NONE) {
csp_log_warn("ERROR: Routing input FIFO is FULL. Dropping packet.\r\n");
interface->drop++;
if (pxTaskWoken == NULL)
csp_buffer_free(packet);
else
csp_buffer_free_isr(packet);
} else {
interface->rx++;
interface->rxbytes += packet->length;
}
}
int eps_slave_hardreset(struct command_context *ctx) {
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = EPS_PORT_HARDRESET;
frame->len = 1;
frame->len_rx = 0;
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
return CMD_ERROR_NONE;
}
void csp_promisc_add(csp_packet_t * packet, csp_queue_handle_t queue) {
if (csp_promisc_enabled == 0)
return;
if (queue != NULL) {
/* Make a copy of the message and queue it to the promiscuous task */
csp_packet_t *packet_copy = csp_buffer_clone(packet);
if (packet_copy != NULL) {
if (csp_queue_enqueue(queue, &packet_copy, 0) != CSP_QUEUE_OK) {
csp_log_error("Promiscuous mode input queue full\r\n");
csp_buffer_free(packet_copy);
}
}
}
}
void gps_rx_cb (void * arg, uint8_t *buf, int len, void *pxTaskWoken)
{
gps_cfg_t *gps_cfg = (gps_cfg_t *) arg;
DEBUG("GPS_RX: %c\n", *buf);
if (*buf == '\n' || *buf == '\r') {
gps_fix_t *fix = gps_incr_fix();
int len = gps_buf_cur + 1;
int result = nmea_parse(gps_buf, len, fix);
memset(gps_buf, '\0', len);
gps_buf_cur = 0;
if (result != NMEA_OK) {
DEBUG("error parsing NMEA: %d\n", result);
return;
}
// Send location fix
if (NULL != gps_cfg->conn) {
DEBUG("Parsed NMEA sentence, sending message\n");
DEBUG("NMEA %02d:%02d:%02d.%03d\n", fix->hour, fix->minute,
fix->seconds, fix->milliseconds);
csp_packet_t * msg;
msg = csp_buffer_get(sizeof(gps_fix_t));
memcpy(msg->data, fix, sizeof(gps_fix_t));
//*((gps_fix_t*)msg->data) = *fix;
msg->length = sizeof(gps_fix_t);
if (!csp_send(gps_cfg->conn, msg, 1000)) {
DEBUG("csp_send failed\n");
csp_buffer_free(msg);
}
}
return;
}
gps_buf[gps_buf_cur++] = *buf;
DEBUG("GPS_BUF: %.*s\n", gps_buf_cur, gps_buf);
if (gps_buf_cur > GPS_BUFSIZE - 1) {
DEBUG("Too much data for GPS buffer, discarding");
gps_buf_cur = 0;
}
}
void csp_tiradio_rx (csp_iface_t *interface,
uint8_t *buf, int len, void *xTaskWoken) {
csp_packet_t *packet;
csp_tiradio_driver_handle_t* handle =
(csp_tiradio_driver_handle_t*) (interface->driver);
if (len < CSP_HEADER_LENGTH) {
csp_log_warn("Length less than minimum expected! Size %u,"
" expected %u; dropping message\r\n", len,
CSP_HEADER_LENGTH);
return;
}
packet = csp_buffer_get(interface->mtu);
if (packet != NULL) {
memcpy(&packet->id.ext, buf, len);
packet->length = len;
if (packet->length >= CSP_HEADER_LENGTH &&
packet->length <= interface->mtu + CSP_HEADER_LENGTH) {
/* Strip CSP header off the length field*/
packet->length -= CSP_HEADER_LENGTH;
/* Convert the packet from network to host order */
packet->id.ext = csp_ntoh32(packet->id.ext);
csp_new_packet(packet, interface, xTaskWoken);
if (handle->module_id < NUM_TIRADIO_MODULES)
latest_csp_transfer_id[handle->module_id] = packet->id;
}
else {
interface->frame++;
csp_buffer_free(packet);
}
}
else {
interface->frame++;
}
}
int csp_sfp_send(csp_conn_t * conn, void * data, int totalsize, int mtu, uint32_t timeout) {
int count = 0;
while(count < totalsize) {
/* Allocate packet */
csp_packet_t * packet = csp_buffer_get(mtu);
if (packet == NULL)
return -1;
/* Calculate sending size */
int size = totalsize - count;
if (size > mtu)
size = mtu;
/* Print debug */
csp_debug(CSP_PROTOCOL, "Sending SFP at %x size %u\r\n", data + count, size);
/* Copy data */
memcpy(packet->data, data + count, size);
packet->length = size;
/* Set fragment flag */
conn->idout.flags |= CSP_FFRAG;
/* Add SFP header */
sfp_header_t * sfp_header = csp_sfp_header_add(packet);
sfp_header->totalsize = csp_hton32(totalsize);
sfp_header->offset = csp_hton32(count);
/* Send data */
if (!csp_send(conn, packet, timeout)) {
csp_buffer_free(packet);
return -1;
}
/* Increment count */
count += size;
}
return 0;
}
unsigned WINAPI task_server(void * parameters) {
/* Create socket without any socket options */
csp_socket_t *sock = csp_socket(CSP_SO_NONE);
/* Bind all ports to socket */
csp_bind(sock, CSP_ANY);
/* Create 10 connections backlog queue */
csp_listen(sock, 10);
/* Pointer to current connection and packet */
csp_conn_t *conn;
csp_packet_t *packet;
/* Process incoming connections */
while (1) {
/* Wait for connection, 10000 ms timeout */
if ((conn = csp_accept(sock, 10000)) == NULL)
continue;
/* Read packets. Timout is 100 ms */
while ((packet = csp_read(conn, 100)) != NULL) {
switch (csp_conn_dport(conn)) {
case MY_PORT:
/* Process packet here */
printf("Packet received on MY_PORT: %s\r\n", (char *) packet->data);
csp_buffer_free(packet);
default:
/* Let the service handler reply pings, buffer use, etc. */
csp_service_handler(conn, packet);
break;
}
}
/* Close current connection, and handle next */
csp_close(conn);
}
return 0;
}
int cmd_eps_slave_config_restore(struct command_context *ctx) {
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = EPS_PORT_CONFIG_CMD;
frame->data[1] = 1;
frame->len = 2;
frame->len_rx = 0;
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
return CMD_ERROR_NONE;
}
void csp_ps(uint8_t node, uint32_t timeout) {
/* Open connection */
csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PS, 0, 0);
if (conn == NULL)
return;
/* Prepare data */
csp_packet_t * packet;
packet = csp_buffer_get(95);
/* Check malloc */
if (packet == NULL)
goto out;
packet->data[0] = 0x55;
packet->length = 1;
printf("PS node %u: ", node);
/* Try to send frame */
if (!csp_send(conn, packet, 0))
goto out;
/* Read incoming frame */
packet = csp_read(conn, timeout);
if (packet == NULL) {
printf(" Timeout!\r\n");
goto out;
}
/* We have a reply */
printf("PS Length %u\r\n", packet->length);
printf("%s\r\n", packet->data);
/* Clean up */
out:
if (packet != NULL)
csp_buffer_free(packet);
csp_close(conn);
}
/** pbuf_free
* Free buffer element and associated CSP packet buffer element.
* @param buf Buffer element to free
* @return 0 on success, -1 on error.
*/
static int pbuf_free(pbuf_element_t *buf, CSP_BASE_TYPE *task_woken) {
int32_t refcount;
/* Lock packet buffer */
if (task_woken == NULL)
CSP_ENTER_CRITICAL(pbuf_sem);
/* Free CSP packet */
if (buf->packet != NULL) {
/* first check to see if the buffer was already free. This could happen
if the top level timeout occurred already. If so, no need to free it
again */
refcount = csp_buffer_get_refcount(buf->packet);
if (refcount > 0) {
if (task_woken == NULL) {
csp_buffer_free(buf->packet);
} else {
csp_buffer_free_isr(buf->packet);
}
}
buf->packet = NULL;
}
/* Mark buffer element free */
buf->state = BUF_FREE;
buf->rx_count = 0;
buf->tx_count = 0;
buf->cfpid = 0;
buf->last_used = 0;
buf->remain = 0;
/* Unlock packet buffer */
if (task_woken == NULL)
CSP_EXIT_CRITICAL(pbuf_sem);
return CSP_ERR_NONE;
}
int eps_slave_reset_persistent(struct command_context *ctx) {
uint8_t magic = 0x42;
i2c_frame_t * frame;
frame = csp_buffer_get(I2C_MTU);
frame->dest = slave_node;
frame->data[0] = EPS_PORT_RESET_COUNTERS;
frame->data[1] = (uint8_t) magic;
frame->len = 2;
frame->len_rx = 0;
frame->retries = 0;
if (i2c_send(0, frame, 0) != E_NO_ERR) {
csp_buffer_free(frame);
return CMD_ERROR_NOMEM;
}
return CMD_ERROR_NONE;
}
void csp_udp_new_packet(csp_conn_t * conn, csp_packet_t * packet) {
/* Enqueue */
if (csp_conn_enqueue_packet(conn, packet) < 0) {
csp_log_error("Connection buffer queue full!\r\n");
csp_buffer_free(packet);
return;
}
/* Try to queue up the new connection pointer */
if (conn->socket != NULL) {
if (csp_queue_enqueue(conn->socket, &conn, 0) != CSP_QUEUE_OK) {
csp_log_warn("Warning socket connection queue full\r\n");
csp_close(conn);
return;
}
/* Ensure that this connection will not be posted to this socket again */
conn->socket = NULL;
}
}
int csp_conn_flush_rx_queue(csp_conn_t * conn) {
csp_packet_t * packet;
int prio;
/* Flush packet queues */
for (prio = 0; prio < CSP_RX_QUEUES; prio++) {
while (csp_queue_dequeue(conn->rx_queue[prio], &packet, 0) == CSP_QUEUE_OK)
if (packet != NULL)
csp_buffer_free(packet);
}
/* Flush event queue */
#ifdef CSP_USE_QOS
int event;
while (csp_queue_dequeue(conn->rx_event, &event, 0) == CSP_QUEUE_OK);
#endif
return CSP_ERR_NONE;
}