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); } }