int cavan_mux_add_link(struct cavan_mux *mux, struct cavan_mux_link *link, u16 port)
{
struct cavan_mux_link **head;
cavan_lock_acquire(&mux->lock);
head = mux->links + (cavan_mux_link_head_index(port));
while ((*head) && (*head)->local_port < port)
{
head = &(*head)->next;
}
if ((*head) && (*head)->local_port == port)
{
pr_red_info("port %d already exists!", port);
cavan_lock_release(&mux->lock);
return -EINVAL;
}
link->local_port = port;
link->next = *head;
*head = link;
cavan_lock_release(&mux->lock);
return 0;
}
ssize_t cavan_mux_link_recv(struct cavan_mux_link *link, void *buff, size_t size)
{
size_t length;
const char *data;
struct cavan_mux_package *package;
struct cavan_mux_package_raw *package_raw;
cavan_lock_acquire(&link->lock);
package_raw = link->package_head;
if (package_raw == NULL) {
cavan_lock_release(&link->lock);
return 0;
}
package = &package_raw->package;
data = package->data + link->hole_size;
length = package->length - link->hole_size;
if (size < length) {
memcpy(buff, data, size);
link->hole_size += size;
} else {
size = length;
memcpy(buff, data, size);
cavan_mux_package_free(link->mux, package);
link->hole_size = 0;
}
cavan_lock_release(&link->lock);
return size;
}
ssize_t cavan_mux_link_send(struct cavan_mux_link *link, const void *buff, size_t size)
{
struct cavan_mux_package *package;
struct cavan_mux *mux = link->mux;
package = cavan_mux_package_alloc(mux, size);
if (package == NULL)
{
pr_red_info("cavan_mux_package_alloc");
return -ENOMEM;
}
cavan_lock_acquire(&link->lock);
package->src_port = link->local_port;
package->dest_port = link->remote_port;
cavan_lock_release(&link->lock);
memcpy(package->data, buff, size);
cavan_mux_append_send_package(mux, package);
return size;
}
int cavan_mux_find_free_port(struct cavan_mux *mux, u16 *pport)
{
int i, j;
int ret = 0;
cavan_lock_acquire(&mux->lock);
for (i = 0; i < NELEM(mux->links); i++)
{
struct cavan_mux_link *head = mux->links[i];
for (j = 0; j < (1 << (sizeof(u16) << 3)) / NELEM(mux->links); j++)
{
u16 port = j * NELEM(mux->links) + i;
if (head && head->local_port == port)
{
head = head->next;
}
else if (port != 0)
{
*pport = port;
goto label_found;
}
}
}
ret = -EBUSY;
label_found:
cavan_lock_release(&mux->lock);
return ret;
}
void cavan_mux_unbind(struct cavan_mux *mux, struct cavan_mux_link *link)
{
struct cavan_mux_link **head;
cavan_lock_acquire(&mux->lock);
head = mux->links + (cavan_mux_link_head_index(link->local_port));
if (link == *head)
{
*head = link->next;
}
else
{
struct cavan_mux_link *prev;
for (prev = *head; prev && prev->next != link; prev = prev->next);
if (prev)
{
prev->next = link->next;
}
}
cavan_lock_release(&mux->lock);
}
ssize_t cavan_mux_append_receive_data(struct cavan_mux *mux, const void *buff, size_t size)
{
int ret;
size_t wrlen, rdlen;
struct cavan_mux_package package;
struct cavan_mux_package *ppackage;
cavan_lock_acquire(&mux->lock);
wrlen = cavan_mem_queue_inqueue(&mux->recv_queue, buff, size);
while (1) {
rdlen = cavan_mem_queue_dequeue_peek(&mux->recv_queue, &package, sizeof(package));
if (rdlen < sizeof(package)) {
goto out_success;
}
if (package.magic == CAVAN_MUX_MAGIC) {
break;
}
cavan_mem_queue_dequeue(&mux->recv_queue, NULL, 1);
}
size = cavan_mux_package_get_whole_length(&package);
if (cavan_mem_queue_get_used_size(&mux->recv_queue) < size) {
goto out_success;
}
ppackage = cavan_mux_package_alloc(mux, package.length);
if (ppackage == NULL) {
pr_red_info("cavan_mux_package_alloc");
ret = -ENOMEM;
goto out_cavan_lock_release;
}
if (cavan_mem_queue_dequeue(&mux->recv_queue, ppackage, size) != size) {
pr_red_info("cavan_mem_queue_dequeue");
ret = -EFAULT;
goto out_cavan_lock_release;
}
ret = cavan_mux_append_receive_package(mux, ppackage);
if (ret < 0) {
pr_red_info("cavan_mux_write_recv_package: %d", ret);
cavan_mux_package_free(mux, ppackage);
}
out_success:
ret = wrlen;
out_cavan_lock_release:
cavan_lock_release(&mux->lock);
return ret;
}
int cavan_mux_bind(struct cavan_mux *mux, struct cavan_mux_link *link, u16 port)
{
int ret;
cavan_lock_acquire(&mux->lock);
if (port == 0) {
ret = cavan_mux_find_free_port(mux, &port);
if (ret < 0) {
cavan_lock_release(&mux->lock);
return ret;
}
}
ret = cavan_mux_add_link(mux, link, port);
cavan_lock_release(&mux->lock);
return ret;
}
struct cavan_mux_link *cavan_mux_find_link(struct cavan_mux *mux, u16 port)
{
struct cavan_mux_link *link;
cavan_lock_acquire(&mux->lock);
for (link = mux->links[cavan_mux_link_head_index(port)]; link && link->local_port != port; link = link->next);
cavan_lock_release(&mux->lock);
return link;
}
void cavan_mux_show_packages(struct cavan_mux *mux)
{
struct cavan_mux_package_raw *package;
cavan_lock_acquire(&mux->lock);
for (package = mux->packages; package; package = package->next) {
println("length = %d", package->length);
}
cavan_lock_release(&mux->lock);
}
void cavan_mux_append_package(struct cavan_mux *mux, struct cavan_mux_package_raw *package)
{
struct cavan_mux_package_raw **head;
cavan_lock_acquire(&mux->lock);
for (head = &mux->packages; *head && (*head)->length < package->length; head = &(*head)->next);
package->next = *head;
*head = package;
cavan_lock_release(&mux->lock);
}
void cavan_mux_append_send_package(struct cavan_mux *mux, struct cavan_mux_package *package)
{
struct cavan_mux_package_raw *package_raw = CAVAN_MUX_PACKAGE_GET_RAW(package);
package->magic = CAVAN_MUX_MAGIC;
cavan_lock_acquire(&mux->lock);
*mux->package_tail = package_raw;
package_raw->next = NULL;
mux->package_tail = &package_raw->next;
cavan_thread_resume(&mux->send_thread);
cavan_lock_release(&mux->lock);
}
int cavan_mux_append_receive_package(struct cavan_mux *mux, struct cavan_mux_package *package)
{
int ret;
struct cavan_mux_link *link;
cavan_lock_acquire(&mux->lock);
link = cavan_mux_find_link(mux, package->dest_port);
if (link == NULL) {
pr_red_info("invalid port %d", package->dest_port);
ret = -EINVAL;
} else {
ret = cavan_mux_link_append_receive_package(link, package);;
}
cavan_lock_release(&mux->lock);
return ret;
}
int cavan_mux_link_append_receive_package(struct cavan_mux_link *link, struct cavan_mux_package *package)
{
struct cavan_mux_package_raw *package_raw = CAVAN_MUX_PACKAGE_GET_RAW(package);
cavan_lock_acquire(&link->lock);
*link->package_tail = package_raw;
package_raw->next = NULL;
link->package_tail = &package_raw->next;
link->remote_port = package->src_port;
if (link->on_received) {
link->on_received(link);
}
cavan_lock_release(&link->lock);
return 0;
}
void cavan_mux_deinit(struct cavan_mux *mux)
{
struct cavan_mux_package_raw *head;
cavan_thread_stop(&mux->recv_thread);
cavan_lock_acquire(&mux->lock);
head = mux->packages;
while (head) {
struct cavan_mux_package_raw *next = head->next;
free(head);
head = next;
}
cavan_lock_release(&mux->lock);
cavan_lock_deinit(&mux->lock);
}
struct cavan_mux_package_raw *cavan_mux_dequeue_package(struct cavan_mux *mux, size_t length)
{
struct cavan_mux_package_raw **head;
struct cavan_mux_package_raw *package;
cavan_lock_acquire(&mux->lock);
head = &mux->packages;
while (*head && (*head)->length < length) {
head = &(*head)->next;
}
package = *head;
if (package) {
*head = package->next;
}
cavan_lock_release(&mux->lock);
return package;
}
