From: John Hodge <tpg@mutabah.net>
Date: Sun, 13 Oct 2013 02:12:18 +0000 (+0800)
Subject: Modules/UDI - Buffer delete, chained CB support
X-Git-Tag: rel0.15~110
X-Git-Url: https://git.ucc.asn.au/?a=commitdiff_plain;ds=sidebyside;h=a335dd47727c40467c1ff7fc256c9061c0398d05;p=tpg%2Facess2.git

Modules/UDI - Buffer delete, chained CB support
---

diff --git a/KernelLand/Modules/Interfaces/UDI/include/udi_internal.h b/KernelLand/Modules/Interfaces/UDI/include/udi_internal.h
index 83093f9f..62d37afc 100644
--- a/KernelLand/Modules/Interfaces/UDI/include/udi_internal.h
+++ b/KernelLand/Modules/Interfaces/UDI/include/udi_internal.h
@@ -75,6 +75,7 @@ struct sUDI_MetaLang
 	 int	nCbTypes;
 	struct {
 		udi_size_t	Size;
+		udi_size_t	ChainOfs;
 		udi_layout_t	*Layout;
 	} CbTypes[];
 };
diff --git a/KernelLand/Modules/Interfaces/UDI/trans/nsr.c b/KernelLand/Modules/Interfaces/UDI/trans/nsr.c
index e6a39308..616c3650 100644
--- a/KernelLand/Modules/Interfaces/UDI/trans/nsr.c
+++ b/KernelLand/Modules/Interfaces/UDI/trans/nsr.c
@@ -345,6 +345,7 @@ tIPStackBuffer *acessnsr_WaitForPacket(void *Card)
 	tIPStackBuffer	*ret = IPStack_Buffer_CreateBuffer(1);
 	void	*data = malloc( cb->rx_buf->buf_size );
 	udi_buf_read(cb->rx_buf, 0, cb->rx_buf->buf_size, data);
+	Debug_HexDump("NSR WaitForPacket", data, cb->rx_buf->buf_size);
 	IPStack_Buffer_AppendSubBuffer(ret, cb->rx_buf->buf_size, 0, data, _FreeHeapSubBuf, data);
 
 	udi_nd_rx_rdy(cb);
diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/buf.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/buf.c
index 1dda0552..5f8e0a84 100644
--- a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/buf.c
+++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/buf.c
@@ -66,7 +66,10 @@ tUDI_BufSect *UDI_int_BufAddSect(size_t data_len, size_t relofs, tUDI_BufSect **
 	newsect->Length = data_len;
 	newsect->Space = space_atom;
 	newsect->Data = newsect+1;
-	memcpy(newsect->Data, data, data_len);
+	if( !data )
+		memset(newsect->Data, 0xFF, data_len);
+	else
+		memcpy(newsect->Data, data, data_len);
 	
 	newsect->Next = *prevptr;
 	*prevptr = newsect;
@@ -116,6 +119,7 @@ void udi_buf_write(
 		dst_buf = &dst->buf;
 	}
 
+	// Find dst_off segment
 	tUDI_BufSect	**prevptr = &dst->Sections;
 	tUDI_BufSect	*sect = dst->Sections;
 	for( ; sect; prevptr = &sect->Next, sect = sect->Next )
@@ -127,59 +131,122 @@ void udi_buf_write(
 		dst_off -= sect->RelOfs + sect->Length;
 	}
 
-	// Find dst_off segment (or create one if src_len>0)
-	// Copy in src data
-	// or (if src_len==0) delete dst_len bytes	
-	if( dst_len == 0 )
+	// Overwrite MIN(src_len,dst_len) bytes
+	// then delete/append remainder
+	size_t	len = MIN(src_len,dst_len);
+	src_len -= len;
+	dst_len -= len;
+	while( len > 0 )
+	{
+		// Create new section
+		if( !sect || sect->RelOfs > dst_off ) {
+			size_t	newsize = MIN(sect->RelOfs - dst_off, len);
+			sect = UDI_int_BufAddSect(len, dst_off, prevptr, src_mem, path_handle);
+			len -= newsize;
+			src_mem += newsize;
+			prevptr = &sect->Next;
+			sect = sect->Next;
+			dst_off = 0;
+		}
+		if( len == 0 )
+			break;
+		
+		// Update existing section
+		size_t	bytes = MIN(len, sect->Length - dst_off);
+		memcpy(sect->Data + dst_off, src_mem, bytes);
+		len -= bytes;
+		src_mem += bytes;
+		
+		prevptr = &sect->Next;
+		sect = sect->Next;
+	}
+
+	if( dst_len > 0 )
 	{
-		// Insert / Initialise
-		if( src_len > 0 )
+		ASSERT(src_len == 0);
+		// Delete
+		while( dst_len > 0 )
 		{
-			if( !sect || sect->RelOfs > dst_off ) {
-				// Simple: Just add a new section
-				UDI_int_BufAddSect(src_len, dst_off, prevptr, src_mem, path_handle);
-				dst_buf->buf_size += src_len;
+			if( !sect ) {
+				dst_buf->buf_size = dst_off;
+				dst_len = 0;
+			}
+			else if( sect->RelOfs > dst_off ) {
+				size_t	bytes = MIN(dst_len, sect->RelOfs - dst_off);
+				dst_len -= bytes;
+				dst_buf->buf_size -= bytes;
+				sect->RelOfs -= bytes;
 			}
-			else if( sect->RelOfs + sect->Length == dst_off ) {
-				// End of block inserts
-				size_t	avail = sect->Space - sect->Length;
-				if( avail ) {
-					size_t	bytes = MIN(avail, src_len);
-					memcpy(sect->Data + sect->Length, src_mem, bytes);
-					src_mem += bytes;
-					src_len -= bytes;
-				}
-				if( src_len ) {
-					// New block(s)
-					UDI_int_BufAddSect(src_len, 0, prevptr, src_mem, path_handle);
-				}
-				dst_buf->buf_size += src_len;
+			else if( dst_off == 0 && sect->Length <= dst_len ) {
+				// Remove entire section
+				dst_len -= sect->Length;
+				dst_buf->buf_size -= sect->Length;
+				*prevptr = sect->Next;
+				free(sect);
+				
+				// Next block
+				sect = *prevptr;
+			}
+			else if( dst_off + dst_len >= sect->Length ) {
+				// block truncate
+				size_t	bytes = MIN(dst_len, sect->Length - dst_off);
+				dst_len -= bytes;
+				dst_buf->buf_size -= bytes;
+				sect->Length -= bytes;
+				
+				// Next block
+				prevptr = &sect->Next;
+				sect = sect->Next;
 			}
 			else {
-				// Complex: Need to handle mid-section inserts
-				Log_Warning("UDI", "TODO: udi_buf_write - mid-section inserts");
+				// in-block removal (must be last)
+				ASSERTC(dst_off + dst_len, <, sect->Length);
+				size_t	tail = sect->Length - (dst_off + dst_len);
+				memmove(sect->Data+dst_off, sect->Data+dst_off+dst_len, tail); 
+				dst_buf->buf_size -= dst_len;
+				sect->Length -= dst_len;
+				dst_len = 0;
 			}
 		}
-		// no-op
-		else
-		{
-		}
 	}
-	else
+	else if( src_len > 0 )
 	{
-		// Delete
-		if( src_len == 0 )
-		{
-			Log_Warning("UDI", "TODO: udi_buf_write - delete");
+		ASSERT(dst_len == 0);
+		// Insert
+		if( !sect || sect->RelOfs > dst_off ) {
+			// Simple: Just add a new section
+			UDI_int_BufAddSect(src_len, dst_off, prevptr, src_mem, path_handle);
+			dst_buf->buf_size += src_len;
 		}
-		// Overwrite
-		else
-		{
-			Log_Warning("UDI", "TODO: udi_buf_write - overwrite");
+		else if( sect->RelOfs + sect->Length == dst_off ) {
+			// End of block inserts
+			size_t	avail = sect->Space - sect->Length;
+			if( avail ) {
+				size_t	bytes = MIN(avail, src_len);
+				memcpy(sect->Data + sect->Length, src_mem, bytes);
+				src_mem += bytes;
+				src_len -= bytes;
+			}
+			if( src_len ) {
+				// New block(s)
+				UDI_int_BufAddSect(src_len, 0, prevptr, src_mem, path_handle);
+			}
+			dst_buf->buf_size += src_len;
+		}
+		else {
+			// Complex: Need to handle mid-section inserts
+			Log_Warning("UDI", "TODO: udi_buf_write - mid-section inserts");
 		}
 	}
+	else
+	{
+		// No-op
+	}
 	
-	callback(gcb, &dst->buf);
+	// HACK: udi_pio_trans calls this with a NULL cb, so handle that
+	if( callback ) {
+		callback(gcb, &dst->buf);
+	}
 }
 
 void udi_buf_read(
diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/cb.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/cb.c
index 2e0dbfcc..7834cf37 100644
--- a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/cb.c
+++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/cb.c
@@ -135,7 +135,7 @@ void udi_cb_alloc_batch(
 
 	// Get chain offset and buffer offset
 	size_t	buf_ofs = 0;	// TODO: Multiple buffers are to be supported
-	size_t	chain_ofs = 0;
+	size_t	chain_ofs = metalang->CbTypes[cb_init->meta_cb_num].ChainOfs;
 	{
 		udi_layout_t	*layout = metalang->CbTypes[cb_init->meta_cb_num].Layout;
 		if( !layout ) {
@@ -155,19 +155,11 @@ void udi_cb_alloc_batch(
 				}
 				buf_ofs = cur_ofs;
 			}
-			else if( *layout == UDI_DL_CB ) {
-				if( chain_ofs ) {
-					Log_Warning("UDI", "Cb %s:%i has multiple DL_CB entries",
-						metalang->Name, cb_init->meta_cb_num);
-				}
-				chain_ofs = cur_ofs;
-			}
 			else {
 				// No-op	
 			}
 			
 			cur_ofs += _udi_marshal_step(NULL, 0, &layout, NULL);
-			layout ++;
 		}
 	}
 
@@ -179,6 +171,7 @@ void udi_cb_alloc_batch(
 	else {
 		LOG("chain_ofs = %i", chain_ofs);
 	}
+	LOG("buf_ofs = %i", buf_ofs);
 
 	udi_cb_t *first_cb = NULL, *cur_cb;
 	udi_cb_t **prevptr = &first_cb;
@@ -189,7 +182,9 @@ void udi_cb_alloc_batch(
 
 		// Allocate buffer
 		if( with_buf && buf_ofs ) {
-			*(void**)((char*)cur_cb + buf_ofs) =_udi_buf_allocate(NULL, buf_size, path_handle);
+			udi_buf_t	*buf = _udi_buf_allocate(NULL, buf_size, path_handle);
+			LOG("buf +%i = %p", buf_ofs, buf);
+			*(void**)((char*)cur_cb + buf_ofs) = buf;
 		}
 
 		LOG("*%p = %p", prevptr, cur_cb);
diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/mei.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/mei.c
index 338898cf..051df92b 100644
--- a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/mei.c
+++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/mei.c
@@ -67,6 +67,16 @@ void udi_mei_call(udi_cb_t *gcb, udi_mei_init_t *meta_info, udi_index_t meta_ops
 	// Check call type
 	udi_op_t	*const*ops = UDI_int_ChannelPrepForCall(gcb, metalang, meta_ops_num);
 	udi_op_t	*op = ops[vec_idx];
+	
+	// Change ownership of chained cbs
+	size_t	chain_ofs = metalang->CbTypes[cb_type].ChainOfs;
+	if( chain_ofs )
+	{
+		udi_cb_t	*chained_cb = gcb;
+		while( (chained_cb = *(udi_cb_t**)((void*)chained_cb + chain_ofs)) )
+			UDI_int_ChannelFlip(chained_cb);
+	}
+	
 	if( indirect_call )
 	{
 		va_list	args;
diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/meta_mgmt.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/meta_mgmt.c
index 1c13b310..7db637f8 100644
--- a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/meta_mgmt.c
+++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/meta_mgmt.c
@@ -25,9 +25,9 @@ tUDI_MetaLang	cMetaLang_Management = {
 	NULL,
 	3,
 	{
-		{sizeof(udi_enumerate_cb_t), NULL},
-		{sizeof(udi_usage_cb_t), NULL},
-		{sizeof(udi_channel_event_cb_t), NULL},
+		{sizeof(udi_enumerate_cb_t),     0, NULL},
+		{sizeof(udi_usage_cb_t),         0, NULL},
+		{sizeof(udi_channel_event_cb_t), 0, NULL},
 	}
 };
 
diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/meta_bus.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/meta_bus.c
index 5fe1cb98..0c21222b 100644
--- a/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/meta_bus.c
+++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/meta_bus.c
@@ -280,9 +280,9 @@ tUDI_MetaLang	cMetaLang_BusBridge = {
 	5,
 	{
 		{0},	// 0: Empty
-		{sizeof(udi_bus_bind_cb_t), udi_meta_info__bridge__bus_bind_cb},
-		{sizeof(udi_intr_attach_cb_t), udi_meta_info__bridge__intr_attach_cb},
-		{sizeof(udi_intr_detach_cb_t), udi_meta_info__bridge__intr_detach_cb},
-		{sizeof(udi_intr_event_cb_t), NULL}
+		{sizeof(udi_bus_bind_cb_t),    0, udi_meta_info__bridge__bus_bind_cb},
+		{sizeof(udi_intr_attach_cb_t), 0, udi_meta_info__bridge__intr_attach_cb},
+		{sizeof(udi_intr_detach_cb_t), 0, udi_meta_info__bridge__intr_detach_cb},
+		{sizeof(udi_intr_event_cb_t),  0, NULL}
 	}
 };
diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/pio.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/pio.c
index 4d05e45a..635544c1 100644
--- a/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/pio.c
+++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/pio.c
@@ -297,6 +297,7 @@ static inline int _read_mem(udi_cb_t *gcb, udi_buf_t *buf, void *mem_ptr,
 		//LOG("scr %p+%i => %i %x,...", gcb->scratch, ofs, size, val->words[0]);
 		break;
 	case UDI_PIO_BUF:
+		ASSERT(buf);
 		udi_buf_read(buf, ofs, size, val);
 		//LOG("buf %p+%i => %i %x,...", buf, ofs, size, val->words[0]);
 		break;
@@ -318,21 +319,23 @@ static inline int _write_mem(udi_cb_t *gcb, udi_buf_t *buf, void *mem_ptr,
 	switch(op)
 	{
 	case UDI_PIO_DIRECT:
+		//LOG("reg %p = %i %x,...", reg, size, val->words[0]);
 		memcpy(reg, val, size);
 		_zero_upper(sizelog2, reg);
 		break;
 	case UDI_PIO_SCRATCH:
 		ASSERTCR( (ofs & (size-1)), ==, 0, 1);
-		LOG("scr %p+%i = %i %x,...", gcb->scratch, ofs, size, val->words[0]);
+		//LOG("scr %p+%i = %i %x,...", gcb->scratch, ofs, size, val->words[0]);
 		memcpy(gcb->scratch + ofs, val, size);
 		break;
 	case UDI_PIO_BUF:
-		LOG("buf %p+%i = %i %x,...", buf, ofs, size, val->words[0]);
+		ASSERT(buf);
+		//LOG("buf %p+%i = %i %x,...", buf, ofs, size, val->words[0]);
 		udi_buf_write(NULL,NULL, val, size, buf, ofs, size, UDI_NULL_BUF_PATH);
 		break;
 	case UDI_PIO_MEM:
 		ASSERTCR( (ofs & (size-1)), ==, 0, 1);
-		LOG("mem %p+%i = %i %x,...", mem_ptr, ofs, size, val->words[0]);
+		//LOG("mem %p+%i = %i %x,...", mem_ptr, ofs, size, val->words[0]);
 		memcpy(mem_ptr + ofs, val, size);
 		break;
 	}
@@ -376,6 +379,7 @@ void udi_pio_trans(udi_pio_trans_call_t *callback, udi_cb_t *gcb,
 				break;
 			case UDI_PIO_OUT:
 				_read_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, &tmpval);
+				LOG("OUT %x = %i %x", operand, tran_size, tmpval.words[0]);
 				pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
 					operand, tran_size, &tmpval, true);
 				break;
@@ -429,21 +433,22 @@ void udi_pio_trans(udi_pio_trans_call_t *callback, udi_cb_t *gcb,
 				switch(operand)
 				{
 				case UDI_PIO_NZ:
-					LOG("CSKIP NZ R%i", reg_num);
+					LOG("CSKIP NZ R%i (%i %x)", reg_num, tran_size, reg->words[0]);
 					if( !(cnd & 1) )
 						ip ++;
 					break;
 				case UDI_PIO_Z:
-					LOG("CSKIP Z R%i", reg_num);
+					LOG("CSKIP Z R%i (%i %x)", reg_num, tran_size, reg->words[0]);
 					if( cnd & 1 )
 						ip ++;
 					break;
 				case UDI_PIO_NNEG:
-					LOG("CSKIP NNEG R%i", reg_num);
+					LOG("CSKIP NNEG R%i (%i %x)", reg_num, tran_size, reg->words[0]);
 					if( !(cnd & 2) )
 						ip ++;
+					break;
 				case UDI_PIO_NEG:
-					LOG("CSKIP NEG R%i", reg_num);
+					LOG("CSKIP NEG R%i (%i %x)", reg_num, tran_size, reg->words[0]);
 					if( cnd & 2 )
 						ip ++;
 					break;
@@ -484,6 +489,8 @@ void udi_pio_trans(udi_pio_trans_call_t *callback, udi_cb_t *gcb,
 				_operation_xor(tran_size, reg, &registers[operand]);
 				break;
 			case UDI_PIO_ADD:
+				LOG("ADD R%i += R%i", reg_num, operand);
+				ASSERTC(operand, <, 8);
 				_operation_add(tran_size, reg, &registers[operand]);
 				break;
 			case UDI_PIO_ADD_IMM:
@@ -495,9 +502,12 @@ void udi_pio_trans(udi_pio_trans_call_t *callback, udi_cb_t *gcb,
 				else {
 					_zero_upper(UDI_PIO_4BYTE, &tmpval);
 				}
+				LOG("ADD R%i += 0x%x", reg_num, tmpval.words[0]);
 				_operation_add(tran_size, reg, &tmpval);
 				break;
 			case UDI_PIO_SUB:
+				LOG("SUB R%i -= R%i", reg_num, operand);
+				ASSERTC(operand, <, 8);
 				_operation_sub(tran_size, reg, &registers[operand]);
 				break;
 			default:
diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/udi_nic.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/udi_nic.c
index 5dfd7d50..47089e64 100644
--- a/KernelLand/Modules/Interfaces/UDI/udi_lib/udi_nic.c
+++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/udi_nic.c
@@ -248,12 +248,12 @@ tUDI_MetaLang	cMetaLang_NIC = {
 	8,
 	{
 		{0},
-		{sizeof(udi_nic_cb_t), _NIC_STD_cb_layout},
-		{sizeof(udi_nic_bind_cb_t), _NIC_BIND_cb_layout},
-		{sizeof(udi_nic_ctrl_cb_t), _NIC_CTRL_cb_layout},
-		{sizeof(udi_nic_status_cb_t), _NIC_STATUS_cb_layout},
-		{sizeof(udi_nic_info_cb_t), _NIC_INFO_cb_layout},
-		{sizeof(udi_nic_tx_cb_t), _NIC_TX_cb_layout},
-		{sizeof(udi_nic_rx_cb_t), _NIC_RX_cb_layout},
+		{sizeof(udi_nic_cb_t),        0, _NIC_STD_cb_layout},
+		{sizeof(udi_nic_bind_cb_t),   0, _NIC_BIND_cb_layout},
+		{sizeof(udi_nic_ctrl_cb_t),   0, _NIC_CTRL_cb_layout},
+		{sizeof(udi_nic_status_cb_t), 0, _NIC_STATUS_cb_layout},
+		{sizeof(udi_nic_info_cb_t),   0, _NIC_INFO_cb_layout},
+		{sizeof(udi_nic_tx_cb_t),     offsetof(udi_nic_tx_cb_t, chain), _NIC_TX_cb_layout},
+		{sizeof(udi_nic_rx_cb_t),     offsetof(udi_nic_rx_cb_t, chain), _NIC_RX_cb_layout},
 	}
 };