Now peer address has meaning for all types of frames when working wit…

…h queue: #26
lexus2k · Nov 28, 2021 · ad7f0ae · ad7f0ae
1 parent 1e1f8f6
commit ad7f0ae
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 10 deletions.
diff --git a/src/proto/fd/tiny_fd.c b/src/proto/fd/tiny_fd.c
@@ -714,18 +714,18 @@ static uint8_t *tiny_fd_get_next_frame_to_send(tiny_fd_handle_t handle, int *len
 {
     uint8_t *data = NULL;
     uint8_t peer = 0; // TODO: Maybe cycle for all slave stations ???
+    uint8_t address = __peer_to_address_field( handle, peer );
     // Tx data available
     tiny_mutex_lock(&handle->frames.mutex);
-    tiny_fd_frame_info_t *ptr = tiny_fd_queue_get_next( &handle->s_queue, TINY_FD_QUEUE_S_FRAME | TINY_FD_QUEUE_U_FRAME, 0, 0 );
+    tiny_fd_frame_info_t *ptr = tiny_fd_queue_get_next( &handle->s_queue, TINY_FD_QUEUE_S_FRAME | TINY_FD_QUEUE_U_FRAME, address, 0 );
     if ( ptr != NULL )
     {
         // clear queue only, when send is done, so for now, use pointer data for sending only
         data = (uint8_t *)&ptr->header;
         *len = ptr->len + sizeof(tiny_frame_header_t);
         if ( (data[1] & HDLC_S_FRAME_MASK) == HDLC_S_FRAME_BITS )
         {
-            uint8_t s_frame_peer = 0; // TODO: Get from the packet
-            handle->peers[s_frame_peer].sent_nr = ptr->header.control >> 5;
+            handle->peers[peer].sent_nr = ptr->header.control >> 5;
         }
 
 #if TINY_FD_DEBUG
@@ -740,7 +740,7 @@ static uint8_t *tiny_fd_get_next_frame_to_send(tiny_fd_handle_t handle, int *len
         }
 #endif
     }
-    else if ( (ptr = tiny_fd_queue_get_next( &handle->frames.i_queue, TINY_FD_QUEUE_I_FRAME, __peer_to_address_field(handle, peer), handle->peers[peer].next_ns )) != NULL &&
+    else if ( (ptr = tiny_fd_queue_get_next( &handle->frames.i_queue, TINY_FD_QUEUE_I_FRAME, address, handle->peers[peer].next_ns )) != NULL &&
               ( handle->peers[peer].state == TINY_FD_STATE_CONNECTED_ABM || handle->peers[peer].state == TINY_FD_STATE_DISCONNECTING ) )
     {
         data = (uint8_t *)&ptr->header;

diff --git a/src/proto/fd/tiny_fd_frames.c b/src/proto/fd/tiny_fd_frames.c
@@ -88,16 +88,25 @@ tiny_fd_frame_info_t *tiny_fd_queue_get_next(tiny_fd_queue_t *queue, int type, u
     {
         if ( queue->frames[index]->type & type )
         {
-            if ( type != TINY_FD_QUEUE_I_FRAME )
+            if ( queue->frames[index]->type == TINY_FD_QUEUE_FREE )
             {
                 ptr = queue->frames[index];
                 break;
             }
-            // Check for I-frame for the frame number
-            if ( ( ( queue->frames[index]->header.control >> 1 ) & 0x07 )  == arg && (address & 0xFC) == (queue->frames[index]->header.address & 0xFC) )
+            // Check address for all frames
+            if ( (address & 0xFC) == (queue->frames[index]->header.address & 0xFC) )
             {
-                ptr = queue->frames[index];
-                break;
+                if ( queue->frames[index]->type != TINY_FD_QUEUE_I_FRAME )
+                {
+                    ptr = queue->frames[index];
+                    break;
+                }
+                // Check for I-frame for the frame number
+                if ( ( ( queue->frames[index]->header.control >> 1 ) & 0x07 )  == arg )
+                {
+                    ptr = queue->frames[index];
+                    break;
+                }
             }
         }
         index++;
@@ -124,7 +133,7 @@ void tiny_fd_queue_free_by_header(tiny_fd_queue_t *queue, const void *header)
             queue->lookup_index = i + 1;
             if ( queue->lookup_index >= queue->size )
             {
-                 queue->lookup_index -= queue->size;
+                queue->lookup_index -= queue->size;
             }
             break;
         }