10 #define MPSC_PBUF_DBG(buffer, ...) do { \  argument
13 		if (buffer) { \
14 			mpsc_state_print(buffer); \
19 static inline void mpsc_state_print(struct mpsc_pbuf_buffer *buffer)  in mpsc_state_print()  argument
23 			buffer->wr_idx, buffer->tmp_wr_idx,  in mpsc_state_print()
24 			buffer->rd_idx, buffer->tmp_rd_idx);  in mpsc_state_print()
28 void mpsc_pbuf_init(struct mpsc_pbuf_buffer *buffer,  in mpsc_pbuf_init()  argument
31 	memset(buffer, 0, offsetof(struct mpsc_pbuf_buffer, buf));  in mpsc_pbuf_init()
32 	buffer->get_wlen = cfg->get_wlen;  in mpsc_pbuf_init()
33 	buffer->notify_drop = cfg->notify_drop;  in mpsc_pbuf_init()
34 	buffer->buf = cfg->buf;  in mpsc_pbuf_init()
35 	buffer->size = cfg->size;  in mpsc_pbuf_init()
36 	buffer->max_usage = 0;  in mpsc_pbuf_init()
37 	buffer->flags = cfg->flags;  in mpsc_pbuf_init()
39 	if (is_power_of_two(buffer->size)) {  in mpsc_pbuf_init()
40 		buffer->flags |= MPSC_PBUF_SIZE_POW2;  in mpsc_pbuf_init()
46 		err = k_sem_init(&buffer->sem, 0, 1);  in mpsc_pbuf_init()
61 static inline bool free_space(struct mpsc_pbuf_buffer *buffer, uint32_t *res)  in free_space()  argument
63 	if (buffer->flags & MPSC_PBUF_FULL) {  in free_space()
68 	if (buffer->rd_idx > buffer->tmp_wr_idx) {  in free_space()
69 		*res =  buffer->rd_idx - buffer->tmp_wr_idx;  in free_space()
72 	*res = buffer->size - buffer->tmp_wr_idx;  in free_space()
86 static inline bool available(struct mpsc_pbuf_buffer *buffer, uint32_t *res)  in available()  argument
88 	if (buffer->flags & MPSC_PBUF_FULL || buffer->tmp_rd_idx > buffer->wr_idx) {  in available()
89 		*res = buffer->size - buffer->tmp_rd_idx;  in available()
93 	*res = (buffer->wr_idx - buffer->tmp_rd_idx);  in available()
98 static inline uint32_t get_usage(struct mpsc_pbuf_buffer *buffer)  in get_usage()  argument
102 	if (free_space(buffer, &f)) {  in get_usage()
103 		f += (buffer->rd_idx - 1);  in get_usage()
106 	return buffer->size - 1 - f;  in get_usage()
109 static inline void max_utilization_update(struct mpsc_pbuf_buffer *buffer)  in max_utilization_update()  argument
111 	if (!(buffer->flags & MPSC_PBUF_MAX_UTILIZATION)) {  in max_utilization_update()
115 	buffer->max_usage = MAX(buffer->max_usage, get_usage(buffer));  in max_utilization_update()
128 static inline uint32_t idx_inc(struct mpsc_pbuf_buffer *buffer,  in idx_inc()  argument
133 	if (buffer->flags & MPSC_PBUF_SIZE_POW2) {  in idx_inc()
134 		return i & (buffer->size - 1);  in idx_inc()
137 	return (i >= buffer->size) ? i - buffer->size : i;  in idx_inc()
150 static ALWAYS_INLINE void tmp_wr_idx_inc(struct mpsc_pbuf_buffer *buffer, int32_t wlen)  in tmp_wr_idx_inc()  argument
152 	buffer->tmp_wr_idx = idx_inc(buffer, buffer->tmp_wr_idx, wlen);  in tmp_wr_idx_inc()
153 	if (buffer->tmp_wr_idx == buffer->rd_idx) {  in tmp_wr_idx_inc()
154 		buffer->flags |= MPSC_PBUF_FULL;  in tmp_wr_idx_inc()
158 static void rd_idx_inc(struct mpsc_pbuf_buffer *buffer, int32_t wlen)  in rd_idx_inc()  argument
160 	buffer->rd_idx = idx_inc(buffer, buffer->rd_idx, wlen);  in rd_idx_inc()
161 	buffer->flags &= ~MPSC_PBUF_FULL;  in rd_idx_inc()
164 static void add_skip_item(struct mpsc_pbuf_buffer *buffer, uint32_t wlen)  in add_skip_item()  argument
170 	buffer->buf[buffer->tmp_wr_idx] = skip.raw;  in add_skip_item()
171 	tmp_wr_idx_inc(buffer, wlen);  in add_skip_item()
172 	buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, wlen);  in add_skip_item()
175 static bool drop_item_locked(struct mpsc_pbuf_buffer *buffer,  in drop_item_locked()  argument
183 	item = (union mpsc_pbuf_generic *)&buffer->buf[buffer->rd_idx];  in drop_item_locked()
190 		MPSC_PBUF_DBG(buffer, "no space: Found skip packet %d len", skip_wlen);  in drop_item_locked()
192 		rd_idx_inc(buffer, skip_wlen);  in drop_item_locked()
193 		buffer->tmp_rd_idx = buffer->rd_idx;  in drop_item_locked()
198 	if (!(buffer->flags & MPSC_PBUF_MODE_OVERWRITE)) {  in drop_item_locked()
202 	uint32_t rd_wlen = buffer->get_wlen(item);  in drop_item_locked()
208 		MPSC_PBUF_DBG(buffer, "no space: Found busy packet %p (len:%d)", item, rd_wlen);  in drop_item_locked()
211 			add_skip_item(buffer, free_wlen);  in drop_item_locked()
212 			MPSC_PBUF_DBG(buffer, "no space: Added skip packet (len:%d)", free_wlen);  in drop_item_locked()
215 		buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, rd_wlen);  in drop_item_locked()
220 		if (buffer->rd_idx == buffer->tmp_rd_idx) {  in drop_item_locked()
221 			buffer->tmp_rd_idx = idx_inc(buffer, buffer->tmp_rd_idx, rd_wlen);  in drop_item_locked()
224 		buffer->tmp_wr_idx = buffer->tmp_rd_idx;  in drop_item_locked()
225 		buffer->rd_idx = buffer->tmp_rd_idx;  in drop_item_locked()
226 		buffer->flags |= MPSC_PBUF_FULL;  in drop_item_locked()
229 		rd_idx_inc(buffer, rd_wlen);  in drop_item_locked()
230 		buffer->tmp_rd_idx = buffer->rd_idx;  in drop_item_locked()
246 			buffer->buf[buffer->tmp_wr_idx] = invalid.raw;  in drop_item_locked()
250 		buffer->tmp_wr_idx = idx_inc(buffer, buffer->tmp_wr_idx, *tmp_wr_idx_shift);  in drop_item_locked()
251 		buffer->flags |= MPSC_PBUF_FULL;  in drop_item_locked()
254 		MPSC_PBUF_DBG(buffer, "no space: dropping packet %p (len: %d)",  in drop_item_locked()
261 static void post_drop_action(struct mpsc_pbuf_buffer *buffer,  in post_drop_action()  argument
265 	uint32_t cmp_tmp_wr_idx = idx_inc(buffer, prev_tmp_wr_idx, tmp_wr_idx_shift);  in post_drop_action()
267 	if (cmp_tmp_wr_idx == buffer->tmp_wr_idx) {  in post_drop_action()
269 		buffer->tmp_wr_idx = prev_tmp_wr_idx;  in post_drop_action()
270 		buffer->flags &= ~MPSC_PBUF_FULL;  in post_drop_action()
283 	buffer->buf[prev_tmp_wr_idx] = skip.raw;  in post_drop_action()
284 	buffer->wr_idx = idx_inc(buffer,  in post_drop_action()
285 				 buffer->wr_idx,  in post_drop_action()
290 void mpsc_pbuf_put_word(struct mpsc_pbuf_buffer *buffer,  in mpsc_pbuf_put_word()  argument
301 		key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_put_word()
304 			post_drop_action(buffer, tmp_wr_idx_val, tmp_wr_idx_shift);  in mpsc_pbuf_put_word()
308 		(void)free_space(buffer, &free_wlen);  in mpsc_pbuf_put_word()
310 		MPSC_PBUF_DBG(buffer, "put_word (%d free space)", (int)free_wlen);  in mpsc_pbuf_put_word()
313 			buffer->buf[buffer->tmp_wr_idx] = item.raw;  in mpsc_pbuf_put_word()
314 			tmp_wr_idx_inc(buffer, 1);  in mpsc_pbuf_put_word()
316 			buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, 1);  in mpsc_pbuf_put_word()
317 			max_utilization_update(buffer);  in mpsc_pbuf_put_word()
319 			tmp_wr_idx_val = buffer->tmp_wr_idx;  in mpsc_pbuf_put_word()
320 			cont = drop_item_locked(buffer, free_wlen,  in mpsc_pbuf_put_word()
324 		k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_put_word()
328 			if (buffer->notify_drop) {  in mpsc_pbuf_put_word()
329 				buffer->notify_drop(buffer, dropped_item);  in mpsc_pbuf_put_word()
336 union mpsc_pbuf_generic *mpsc_pbuf_alloc(struct mpsc_pbuf_buffer *buffer,  in mpsc_pbuf_alloc()  argument
346 	MPSC_PBUF_DBG(buffer, "alloc %d words", (int)wlen);  in mpsc_pbuf_alloc()
348 	if (wlen > (buffer->size)) {  in mpsc_pbuf_alloc()
349 		MPSC_PBUF_DBG(buffer, "Failed to alloc");  in mpsc_pbuf_alloc()
357 		key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_alloc()
359 			post_drop_action(buffer, tmp_wr_idx_val, tmp_wr_idx_shift);  in mpsc_pbuf_alloc()
363 		wrap = free_space(buffer, &free_wlen);  in mpsc_pbuf_alloc()
367 			    (union mpsc_pbuf_generic *)&buffer->buf[buffer->tmp_wr_idx];  in mpsc_pbuf_alloc()
370 			tmp_wr_idx_inc(buffer, wlen);  in mpsc_pbuf_alloc()
373 			add_skip_item(buffer, free_wlen);  in mpsc_pbuf_alloc()
379 			k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_alloc()
380 			err = k_sem_take(&buffer->sem, timeout);  in mpsc_pbuf_alloc()
381 			key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_alloc()
384 			tmp_wr_idx_val = buffer->tmp_wr_idx;  in mpsc_pbuf_alloc()
385 			cont = drop_item_locked(buffer, free_wlen,  in mpsc_pbuf_alloc()
388 		k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_alloc()
392 			if (buffer->notify_drop) {  in mpsc_pbuf_alloc()
393 				buffer->notify_drop(buffer, dropped_item);  in mpsc_pbuf_alloc()
400 	MPSC_PBUF_DBG(buffer, "allocated %p", item);  in mpsc_pbuf_alloc()
410 void mpsc_pbuf_commit(struct mpsc_pbuf_buffer *buffer,  in mpsc_pbuf_commit()  argument
413 	uint32_t wlen = buffer->get_wlen(item);  in mpsc_pbuf_commit()
415 	k_spinlock_key_t key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_commit()
418 	buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, wlen);  in mpsc_pbuf_commit()
419 	max_utilization_update(buffer);  in mpsc_pbuf_commit()
420 	k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_commit()
421 	MPSC_PBUF_DBG(buffer, "committed %p", item);  in mpsc_pbuf_commit()
424 void mpsc_pbuf_put_word_ext(struct mpsc_pbuf_buffer *buffer,  in mpsc_pbuf_put_word_ext()  argument
440 		key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_put_word_ext()
443 			post_drop_action(buffer, tmp_wr_idx_val, tmp_wr_idx_shift);  in mpsc_pbuf_put_word_ext()
447 		wrap = free_space(buffer, &free_wlen);  in mpsc_pbuf_put_word_ext()
450 			buffer->buf[buffer->tmp_wr_idx] = item.raw;  in mpsc_pbuf_put_word_ext()
452 				(void **)&buffer->buf[buffer->tmp_wr_idx + 1];  in mpsc_pbuf_put_word_ext()
455 			tmp_wr_idx_inc(buffer, l);  in mpsc_pbuf_put_word_ext()
456 			buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, l);  in mpsc_pbuf_put_word_ext()
458 			max_utilization_update(buffer);  in mpsc_pbuf_put_word_ext()
460 			add_skip_item(buffer, free_wlen);  in mpsc_pbuf_put_word_ext()
463 			tmp_wr_idx_val = buffer->tmp_wr_idx;  in mpsc_pbuf_put_word_ext()
464 			cont = drop_item_locked(buffer, free_wlen,  in mpsc_pbuf_put_word_ext()
468 		k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_put_word_ext()
472 			if (buffer->notify_drop) {  in mpsc_pbuf_put_word_ext()
473 				buffer->notify_drop(buffer, dropped_item);  in mpsc_pbuf_put_word_ext()
480 void mpsc_pbuf_put_data(struct mpsc_pbuf_buffer *buffer, const uint32_t *data,  in mpsc_pbuf_put_data()  argument
493 		key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_put_data()
496 			post_drop_action(buffer, tmp_wr_idx_val, tmp_wr_idx_shift);  in mpsc_pbuf_put_data()
500 		wrap = free_space(buffer, &free_wlen);  in mpsc_pbuf_put_data()
503 			memcpy(&buffer->buf[buffer->tmp_wr_idx], data,  in mpsc_pbuf_put_data()
505 			buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, wlen);  in mpsc_pbuf_put_data()
506 			tmp_wr_idx_inc(buffer, wlen);  in mpsc_pbuf_put_data()
508 			max_utilization_update(buffer);  in mpsc_pbuf_put_data()
510 			add_skip_item(buffer, free_wlen);  in mpsc_pbuf_put_data()
513 			tmp_wr_idx_val = buffer->tmp_wr_idx;  in mpsc_pbuf_put_data()
514 			cont = drop_item_locked(buffer, free_wlen,  in mpsc_pbuf_put_data()
518 		k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_put_data()
523 			if (buffer->notify_drop) {  in mpsc_pbuf_put_data()
524 				buffer->notify_drop(buffer, dropped_item);  in mpsc_pbuf_put_data()
531 const union mpsc_pbuf_generic *mpsc_pbuf_claim(struct mpsc_pbuf_buffer *buffer)  in mpsc_pbuf_claim()  argument
541 		key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_claim()
542 		(void)available(buffer, &a);  in mpsc_pbuf_claim()
544 			&buffer->buf[buffer->tmp_rd_idx];  in mpsc_pbuf_claim()
547 			MPSC_PBUF_DBG(buffer, "invalid claim %d: %p", a, item);  in mpsc_pbuf_claim()
554 					skip ? skip : buffer->get_wlen(item);  in mpsc_pbuf_claim()
556 				buffer->tmp_rd_idx =  in mpsc_pbuf_claim()
557 				      idx_inc(buffer, buffer->tmp_rd_idx, inc);  in mpsc_pbuf_claim()
558 				rd_idx_inc(buffer, inc);  in mpsc_pbuf_claim()
562 				buffer->tmp_rd_idx =  in mpsc_pbuf_claim()
563 					idx_inc(buffer, buffer->tmp_rd_idx,  in mpsc_pbuf_claim()
564 						buffer->get_wlen(item));  in mpsc_pbuf_claim()
569 			MPSC_PBUF_DBG(buffer, ">>claimed %d: %p", a, item);  in mpsc_pbuf_claim()
571 		k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_claim()
577 void mpsc_pbuf_free(struct mpsc_pbuf_buffer *buffer,  in mpsc_pbuf_free()  argument
580 	uint32_t wlen = buffer->get_wlen(item);  in mpsc_pbuf_free()
581 	k_spinlock_key_t key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_free()
585 	if (!(buffer->flags & MPSC_PBUF_MODE_OVERWRITE) ||  in mpsc_pbuf_free()
586 		 ((uint32_t *)item == &buffer->buf[buffer->rd_idx])) {  in mpsc_pbuf_free()
588 		if (buffer->rd_idx == buffer->tmp_rd_idx) {  in mpsc_pbuf_free()
596 			buffer->tmp_rd_idx = idx_inc(buffer, buffer->tmp_rd_idx, wlen);  in mpsc_pbuf_free()
598 		rd_idx_inc(buffer, wlen);  in mpsc_pbuf_free()
600 		MPSC_PBUF_DBG(buffer, "Allocation occurred during claim");  in mpsc_pbuf_free()
603 	MPSC_PBUF_DBG(buffer, "<<freed: %p", item);  in mpsc_pbuf_free()
605 	k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_free()
607 		k_sem_give(&buffer->sem);  in mpsc_pbuf_free()
611 bool mpsc_pbuf_is_pending(struct mpsc_pbuf_buffer *buffer)  in mpsc_pbuf_is_pending()  argument
614 	k_spinlock_key_t key = k_spin_lock(&buffer->lock);  in mpsc_pbuf_is_pending()
616 	(void)available(buffer, &a);  in mpsc_pbuf_is_pending()
617 	k_spin_unlock(&buffer->lock, key);  in mpsc_pbuf_is_pending()
622 void mpsc_pbuf_get_utilization(struct mpsc_pbuf_buffer *buffer,  in mpsc_pbuf_get_utilization()  argument
626 	*size = (buffer->size - 1) * sizeof(int);  in mpsc_pbuf_get_utilization()
627 	*now = get_usage(buffer) * sizeof(int);  in mpsc_pbuf_get_utilization()
630 int mpsc_pbuf_get_max_utilization(struct mpsc_pbuf_buffer *buffer, uint32_t *max)  in mpsc_pbuf_get_max_utilization()  argument
633 	if (!(buffer->flags & MPSC_PBUF_MAX_UTILIZATION)) {  in mpsc_pbuf_get_max_utilization()
637 	*max = buffer->max_usage * sizeof(int);  in mpsc_pbuf_get_max_utilization()