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; }
void csp_rtable_clear(void) { for (csp_rtable_t * i = rtable; (i);) { void * freeme = i; i = i->next; csp_free(freeme); } rtable = NULL; /* Set loopback up again */ csp_rtable_set(csp_get_address(), CSP_ID_HOST_SIZE, &csp_if_lo, CSP_NODE_MAC); }
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_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_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; }
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); } }