buffer pool, somewhat broken after a few runs..hoernchen/wip_bufpool

Change-Id: I78065c71fe57b85888ea1511665cf95f777b924b

4 files changed, 56 insertions, 37 deletions

diff --git a/ccid_common/ccid_device.c b/ccid_common/ccid_device.c
index 7e0e5b1..7c1950c 100644
--- a/ccid_common/ccid_device.c
+++ b/ccid_common/ccid_device.c
@@ -159,13 +159,19 @@ static uint8_t ccid_pc_to_rdr_get_seq(const struct ccid_pc_to_rdr *u)
  * Message generation / sending
  ***********************************************************************/
 
-static struct msgb *ccid_msgb_alloc(void)
+struct msgb * __attribute__((weak)) ccid_msgb_alloc(void)
 {
-	struct msgb *msg = msgb_alloc(512, "ccid");
+	struct msgb *msg = msgb_alloc(300, "ccid");
 	OSMO_ASSERT(msg);
 	return msg;
 }
 
+void __attribute__((weak)) ccid_msgb_free(struct msgb *msg)
+{
+	OSMO_ASSERT(msg);
+	msgb_free(msg);
+}
+
 /* Send given CCID message */
 static int ccid_send(struct ccid_instance *ci, struct msgb *msg)
 {
@@ -768,18 +774,18 @@ int ccid_handle_out(struct ccid_instance *ci, struct msgb *msg)
 		LOGP(DCCID, LOGL_NOTICE, "Unknown CCID Message received: 0x%02x\n", ch->bMessageType);
 		resp = gen_err_resp(ch->bMessageType, ch->bSlot, CCID_ICC_STATUS_NO_ICC, ch->bSeq,
 				    CCID_ERR_CMD_NOT_SUPPORTED);
-		msgb_free(msg);
+		ccid_msgb_free(msg);
 		return ccid_slot_send_unbusy(cs, resp);
 	}
 	/* the various ccid_handle_* functions can return '1' to tell us that they took ownership
 	 * of the msgb */
 	if (rc != 1)
-		msgb_free(msg);
+		ccid_msgb_free(msg);
 	return 0;
 
 short_msg:
 	LOGP(DCCID, LOGL_ERROR, "Short CCID message received: %s; ignoring\n", msgb_hexdump(msg));
-	msgb_free(msg);
+	ccid_msgb_free(msg);
 	return -1;
 }
 
diff --git a/ccid_common/ccid_device.h b/ccid_common/ccid_device.h
index 38a71db..841eb91 100644
--- a/ccid_common/ccid_device.h
+++ b/ccid_common/ccid_device.h
@@ -125,6 +125,8 @@ struct msgb *ccid_gen_parameters_t0(struct ccid_slot *cs, uint8_t seq, uint8_t c
 					   enum ccid_error_code err);
 struct msgb *ccid_gen_parameters_t1(struct ccid_slot *cs, uint8_t seq, uint8_t cmd_sts,
 					   enum ccid_error_code err);
+struct msgb * ccid_msgb_alloc(void);
+void ccid_msgb_free(struct msgb *msg);
 
 /* Invalid request received: Please return STALL */
 #define CCID_CTRL_RET_INVALID	-1
diff --git a/ccid_common/ccid_slot_fsm.c b/ccid_common/ccid_slot_fsm.c
index 8d38f29..1919642 100644
--- a/ccid_common/ccid_slot_fsm.c
+++ b/ccid_common/ccid_slot_fsm.c
@@ -103,7 +103,7 @@ static void iso_fsm_slot_icc_power_on_async(struct ccid_slot *cs, struct msgb *m
 	osmo_fsm_inst_dispatch(ss->fi, ISO7816_E_RESET_REL_IND, NULL);
 	card_uart_ctrl(ss->cuart, CUART_CTL_RST, false);
 
-	msgb_free(msg);
+	ccid_msgb_free(msg);
 	/* continues in iso_fsm_clot_user_cb once ATR is received */
 }
 static void iso_fsm_clot_user_cb(struct osmo_fsm_inst *fi, int event, int cause, void *data)
@@ -128,7 +128,7 @@ static void iso_fsm_clot_user_cb(struct osmo_fsm_inst *fi, int event, int cause,
 			msgb_hexdump(tpdu));
 		resp = ccid_gen_data_block(cs, ss->seq, CCID_CMD_STATUS_OK, 0, msgb_l2(tpdu), msgb_l2len(tpdu));
 		ccid_slot_send_unbusy(cs, resp);
-		msgb_free(tpdu);
+		ccid_msgb_free(tpdu);
 		break;
 	case ISO7816_E_TPDU_FAILED_IND:
 		tpdu = data;
@@ -137,7 +137,7 @@ static void iso_fsm_clot_user_cb(struct osmo_fsm_inst *fi, int event, int cause,
 		/* FIXME: other error causes than card removal?*/
 		resp = ccid_gen_data_block(cs, ss->seq, CCID_CMD_STATUS_FAILED, CCID_ERR_ICC_MUTE, msgb_l2(tpdu), 0);
 		ccid_slot_send_unbusy(cs, resp);
-		msgb_free(tpdu);
+		ccid_msgb_free(tpdu);
 		break;
 	case ISO7816_E_PPS_DONE_IND:
 		tpdu = data;
@@ -156,7 +156,7 @@ static void iso_fsm_clot_user_cb(struct osmo_fsm_inst *fi, int event, int cause,
 		ccid_slot_send_unbusy(cs, resp);
 
 		/* this frees the pps req from the host, pps resp buffer stays with the pps fsm */
-		msgb_free(tpdu);
+		ccid_msgb_free(tpdu);
 		break;
 	case ISO7816_E_PPS_FAILED_IND:
 		tpdu = data;
@@ -164,7 +164,7 @@ static void iso_fsm_clot_user_cb(struct osmo_fsm_inst *fi, int event, int cause,
 		resp = ccid_gen_parameters_t0(cs, ss->seq, CCID_CMD_STATUS_FAILED, 10);
 		ccid_slot_send_unbusy(cs, resp);
 		/* this frees the pps req from the host, pps resp buffer stays with the pps fsm */
-		msgb_free(tpdu);
+		ccid_msgb_free(tpdu);
 		break;
 	default:
 		LOGPCS(cs, LOGL_NOTICE, "%s(event=%d, cause=%d, data=%p) unhandled\n",
@@ -188,17 +188,11 @@ static int iso_fsm_slot_xfr_block_async(struct ccid_slot *cs, struct msgb *msg,
 	OSMO_ASSERT(xfb->wLevelParameter == 0x0000);
 	OSMO_ASSERT(msgb_length(msg) > xfb->hdr.dwLength);
 
-	/* 'msg' contains the raw CCID message as received from USB. We could create
-	 * a new message buffer for the ISO7816 side here or we could 'strip the CCID
-	 * header off the start of the message. Let's KISS and do a copy here */
-	tpdu = msgb_alloc(512, "TPDU");
-	OSMO_ASSERT(tpdu);
-	memcpy(msgb_data(tpdu), xfb->abData, xfb->hdr.dwLength);
-	msgb_put(tpdu, xfb->hdr.dwLength);
-	msgb_free(msg);
+	/* remove ccid header */
+	msgb_pull(msg, 10);
 
-	LOGPCS(cs, LOGL_DEBUG, "scheduling TPDU transfer: %s\n", msgb_hexdump(tpdu));
-	osmo_fsm_inst_dispatch(ss->fi, ISO7816_E_XCEIVE_TPDU_CMD, tpdu);
+	LOGPCS(cs, LOGL_DEBUG, "scheduling TPDU transfer: %s\n", msgb_hexdump(msg));
+	osmo_fsm_inst_dispatch(ss->fi, ISO7816_E_XCEIVE_TPDU_CMD, msg);
 	/* continues in iso_fsm_clot_user_cb once response/error/timeout is received */
 	return 0;
 }
@@ -252,7 +246,7 @@ static int iso_fsm_slot_set_params(struct ccid_slot *cs, uint8_t seq, enum ccid_
 	ss->seq = seq;
 
 	/* Hardware does not support SPU, so no PPS2, and PPS3 is reserved anyway */
-	tpdu = msgb_alloc(6, "PPSRQ");
+	tpdu = ccid_msgb_alloc();
 	OSMO_ASSERT(tpdu);
 	msgb_put_u8(tpdu, 0xff);
 	msgb_put_u8(tpdu, (1 << 4)); /* only PPS1, T=0 */
diff --git a/sysmoOCTSIM/main.c b/sysmoOCTSIM/main.c
index b44e362..6973cf2 100644
--- a/sysmoOCTSIM/main.c
+++ b/sysmoOCTSIM/main.c
@@ -196,6 +196,23 @@ void msgb_enqueue_irqsafe(struct llist_head *q, struct msgb *msg)
 	CRITICAL_SECTION_LEAVE()
 }
 
+struct msgb *ccid_msgb_alloc(void)
+{
+	struct msgb *msg;
+	msg = msgb_dequeue_irqsafe(&g_ccid_s.free_q);
+	OSMO_ASSERT(msg);
+	msgb_reset(msg);
+
+	msg->data[msg->data_len - 4] = __builtin_return_address(0);
+	return msg;
+}
+
+void ccid_msgb_free(struct msgb *msg)
+{
+	OSMO_ASSERT(msg);
+	msgb_enqueue_irqsafe(&g_ccid_s.free_q, msg);
+}
+
 /* submit the next pending (if any) message for the IN EP */
 static int submit_next_in(void)
 {
@@ -316,7 +333,7 @@ static struct msgb *ccid_gen_notify_slot_status(uint8_t old_bm, uint8_t new_bm)
 {
 	uint8_t statusbytes[2] = {0};
 	//struct msgb *msg = ccid_msgb_alloc();
-	struct msgb *msg = msgb_alloc(64, "IRQ");
+	struct msgb *msg = ccid_msgb_alloc();//msgb_alloc(64, "IRQ");
 	struct ccid_rdr_to_pc_notify_slot_change *nsc = msgb_put(msg, sizeof(*nsc) + sizeof(statusbytes));
 	nsc->bMessageType = RDR_to_PC_NotifySlotChange;
 
@@ -1123,7 +1140,7 @@ static const struct ccid_ops c_ops = {
 
 //#######################
 
-#define NUM_OUT_BUF 7
+#define NUM_OUT_BUF 16
 
 int main(void)
 {
@@ -1193,20 +1210,20 @@ int main(void)
 		submit_next_irq();
 		feed_ccid();
 		osmo_timers_update();
-		int qs = llist_count_at(&g_ccid_s.free_q);
-		if(qs > NUM_OUT_BUF)
-			for (int i= 0; i < qs-NUM_OUT_BUF; i++){
-				struct msgb *msg = msgb_dequeue_irqsafe(&g_ccid_s.free_q);
-				if (msg)
-				msgb_free(msg);
-			}
-		if(qs < NUM_OUT_BUF)
-			for (int i= 0; i < qs-NUM_OUT_BUF; i++){
-				struct msgb *msg = msgb_alloc(300,"ccid");
-				OSMO_ASSERT(msg);
-				/* return the message back to the queue of free message buffers */
-				llist_add_tail_at(&msg->list, &g_ccid_s.free_q);
-			}
+//		int qs = llist_count_at(&g_ccid_s.free_q);
+//		if(qs > NUM_OUT_BUF)
+//			for (int i= 0; i < qs-NUM_OUT_BUF; i++){
+//				struct msgb *msg = msgb_dequeue_irqsafe(&g_ccid_s.free_q);
+//				if (msg)
+//				msgb_free(msg);
+//			}
+//		if(qs < NUM_OUT_BUF)
+//			for (int i= 0; i < NUM_OUT_BUF-qs; i++){
+//				struct msgb *msg = msgb_alloc(300,"ccid");
+//				OSMO_ASSERT(msg);
+//				/* return the message back to the queue of free message buffers */
+//				llist_add_tail_at(&msg->list, &g_ccid_s.free_q);
+//			}
 
 
 	}