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 csp_buffer_init(int buf_count, int buf_size) {
#ifndef CSP_BUFFER_STATIC
/* Remember size */
count = buf_count;
size = buf_size;
/* Allocate main memory */
csp_buffer_p = csp_malloc(count * size);
if (csp_buffer_p == NULL)
return CSP_ERR_NOMEM;
/* Allocate housekeeping memory */
csp_buffer_list = (csp_buffer_state_t *) csp_malloc(count * sizeof(csp_buffer_state_t));
if (csp_buffer_list == NULL) {
csp_free(csp_buffer_p);
return CSP_ERR_NOMEM;
}
#endif
#if defined(CSP_POSIX) || defined(CSP_WINDOWS)
/* Initialize critical lock */
if (csp_bin_sem_create(&csp_critical_lock) != CSP_SEMAPHORE_OK) {
csp_debug(CSP_ERROR, "No more memory for buffer semaphore\r\n");
if (csp_buffer_list)
csp_free(csp_buffer_list);
if (csp_buffer_p)
csp_free(csp_buffer_p);
return CSP_ERR_NOMEM;
}
#endif
/* Clear housekeeping memory = all free mem */
memset(csp_buffer_list, 0, count * sizeof(csp_buffer_state_t));
return CSP_ERR_NONE;
}
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 csp_buffer_init(int buf_count, int buf_size) {
unsigned int i;
csp_skbf_t * buf;
count = buf_count;
size = buf_size;
unsigned int skbfsize = (sizeof(csp_skbf_t) + size);
skbfsize = CSP_BUFFER_ALIGN * ((skbfsize + CSP_BUFFER_ALIGN - 1) / CSP_BUFFER_ALIGN);
unsigned int poolsize = count * skbfsize;
csp_buffer_pool = csp_malloc(poolsize);
if (csp_buffer_pool == NULL)
goto fail_malloc;
csp_buffers = csp_queue_create(count, sizeof(void *));
if (!csp_buffers)
goto fail_queue;
if (CSP_INIT_CRITICAL(csp_critical_lock) != CSP_ERR_NONE)
goto fail_critical;
memset(csp_buffer_pool, 0, poolsize);
for (i = 0; i < count; i++) {
/* We have already taken care of pointer alignment since
* skbfsize is an integer multiple of sizeof(int *)
* but the explicit cast to a void * is still necessary
* to tell the compiler so.
*/
buf = (void *) &csp_buffer_pool[i * skbfsize];
buf->refcount = 0;
buf->skbf_addr = buf;
csp_queue_enqueue(csp_buffers, &buf, 0);
}
return CSP_ERR_NONE;
fail_critical:
csp_queue_remove(csp_buffers);
fail_queue:
csp_free(csp_buffer_pool);
fail_malloc:
return CSP_ERR_NOMEM;
}
int csp_rtable_set(uint8_t _address, uint8_t _netmask, csp_iface_t *ifc, uint8_t mac) {
if (ifc == NULL)
return CSP_ERR_INVAL;
/* Set default route in the old way */
int address, netmask;
if (_address == CSP_DEFAULT_ROUTE) {
netmask = 0;
address = 0;
} else {
netmask = _netmask;
address = _address;
}
/* Fist see if the entry exists */
csp_rtable_t * entry = csp_rtable_find(address, netmask, 1);
/* If not, create a new one */
if (!entry) {
entry = csp_malloc(sizeof(csp_rtable_t));
if (entry == NULL)
return CSP_ERR_NOMEM;
entry->next = NULL;
/* Add entry to linked-list */
if (rtable == NULL) {
/* This is the first interface to be added */
rtable = entry;
} else {
/* One or more interfaces were already added */
csp_rtable_t * i = rtable;
while (i->next)
i = i->next;
i->next = entry;
}
}
/* Fill in the data */
entry->address = address;
entry->netmask = netmask;
entry->interface = ifc;
entry->mac = mac;
return CSP_ERR_NONE;
}
int csp_buffer_init(int buf_count, int buf_size) {
unsigned int i;
void *element;
count = buf_count;
size = buf_size;
csp_buffer_list = csp_malloc(count * size);
if (csp_buffer_list == NULL)
goto fail_malloc;
csp_buffers = csp_queue_create(count, sizeof(void *));
if (!csp_buffers)
goto fail_queue;
if (CSP_INIT_CRITICAL(csp_critical_lock) != CSP_ERR_NONE)
goto fail_critical;
memset(csp_buffer_list, 0, count * size);
for (i = 0; i < count; i++) {
element = csp_buffer_list + i * size;
csp_queue_enqueue(csp_buffers, &element, 0);
}
return CSP_ERR_NONE;
fail_critical:
csp_queue_remove(csp_buffers);
fail_queue:
csp_free(csp_buffer_list);
fail_malloc:
return CSP_ERR_NOMEM;
}
/** csp_socket
* Create CSP socket endpoint
* @param opts Socket options
* @return Pointer to socket on success, NULL on failure
*/
csp_socket_t * csp_socket(uint32_t opts) {
/* Validate socket options */
if ((opts & CSP_SO_RDPREQ) && !CSP_USE_RDP) {
csp_debug(CSP_ERROR, "Attempt to create socket that requires RDP, but CSP was compiled without RDP support\r\n");
return NULL;
} else if ((opts & CSP_SO_XTEAREQ) && !CSP_ENABLE_XTEA) {
csp_debug(CSP_ERROR, "Attempt to create socket that requires XTEA, but CSP was compiled without XTEA support\r\n");
return NULL;
} else if ((opts & CSP_SO_HMACREQ) && !CSP_ENABLE_HMAC) {
csp_debug(CSP_ERROR, "Attempt to create socket that requires XTEA, but CSP was compiled without XTEA support\r\n");
return NULL;
}
/* Use CSP buffers instead? */
csp_socket_t * sock = csp_malloc(sizeof(csp_socket_t));
if (sock != NULL) {
sock->conn_queue = NULL;
sock->opts = opts;
}
return sock;
}
void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) {
switch (csp_conn_dport(conn)) {
case CSP_CMP:
/* Pass to CMP handler */
if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) {
csp_buffer_free(packet);
return;
}
break;
case CSP_PING:
/* A ping means, just echo the packet, so no changes */
csp_log_info("SERVICE: Ping received");
break;
case CSP_PS: {
/* Sanity check on request */
if ((packet->length != 1) || (packet->data[0] != 0x55)) {
/* Sanity check failed */
csp_buffer_free(packet);
/* Clear the packet, it has been freed */
packet = NULL;
break;
}
/* Start by allocating just the right amount of memory */
int task_list_size = csp_sys_tasklist_size();
char * pslist = csp_malloc(task_list_size);
/* Check for malloc fail */
if (pslist == NULL) {
/* Send out the data */
strcpy((char *)packet->data, "Not enough memory");
packet->length = strlen((char *)packet->data);
/* Break and let the default handling send packet */
break;
}
/* Retrieve the tasklist */
csp_sys_tasklist(pslist);
int pslen = strnlen(pslist, task_list_size);
/* Split the potentially very long string into packets */
int i = 0;
while(i < pslen) {
/* Allocate packet buffer, if need be */
if (packet == NULL)
packet = csp_buffer_get(CSP_RPS_MTU);
if (packet == NULL)
break;
/* Calculate length, either full MTU or the remainder */
packet->length = (pslen - i > CSP_RPS_MTU) ? CSP_RPS_MTU : (pslen - i);
/* Send out the data */
memcpy(packet->data, &pslist[i], packet->length);
i += packet->length;
if (!csp_send(conn, packet, 0))
csp_buffer_free(packet);
/* Clear the packet reference when sent */
packet = NULL;
}
csp_free(pslist);
break;
}
case CSP_MEMFREE: {
uint32_t total = csp_sys_memfree();
total = csp_hton32(total);
memcpy(packet->data, &total, sizeof(total));
packet->length = sizeof(total);
break;
}
case CSP_REBOOT: {
uint32_t magic_word;
memcpy(&magic_word, packet->data, sizeof(magic_word));
magic_word = csp_ntoh32(magic_word);
/* If the magic word is valid, reboot */
if (magic_word == CSP_REBOOT_MAGIC) {
csp_sys_reboot();
} else if (magic_word == CSP_REBOOT_SHUTDOWN_MAGIC) {
csp_sys_shutdown();
}
csp_buffer_free(packet);
return;
}
case CSP_BUF_FREE: {
uint32_t size = csp_buffer_remaining();
size = csp_hton32(size);
memcpy(packet->data, &size, sizeof(size));
packet->length = sizeof(size);
break;
}
case CSP_UPTIME: {
uint32_t time = csp_get_s();
time = csp_hton32(time);
memcpy(packet->data, &time, sizeof(time));
packet->length = sizeof(time);
break;
}
default:
csp_buffer_free(packet);
return;
}
if (packet != NULL) {
if (!csp_send(conn, packet, 0))
csp_buffer_free(packet);
}
}
