Free device after removal

data_bittiming moved into fd

intrepid.c

@@ -66,6 +66,15 @@
 #define VER_MIN_FROM_INT(VERINT) ((VERINT >> 8) & 0xFF)
 #define VER_PATCH_FROM_INT(VERINT) (VERINT & 0xFF)
 
+#define RX_BOX_SIZE (SHARED_MEM_SIZE / (MAX_NET_DEVICES * 2))
+#define TX_BOX_SIZE (SHARED_MEM_SIZE / 4)
+#define GET_RX_BOX(DEVICE_INDEX) \
+	(shared_mem + (RX_BOX_SIZE * DEVICE_INDEX))
+#define GET_TX_BOX(BOX_INDEX) \
+	(shared_mem + (SHARED_MEM_SIZE / 2) + (BOX_INDEX * TX_BOX_SIZE))
+#define MAX_TX (0x100)
+#define DESC_OFFSET (2)
+
 MODULE_DESCRIPTION(KO_DESC);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Paul Hollinsky <phollinsky@intrepidcs.com>");
@@ -99,6 +108,7 @@ MODULE_VERSION(KO_VERSION);
 #define KERNEL_SUPPORTS_ALIASES         (LINUX_VERSION_CODE >= KERNEL_VERSION(4,15,0))
 #define KERNEL_DEFINES_VM_FAULT_T       (LINUX_VERSION_CODE >= KERNEL_VERSION(4,17,0))
 #define KERNEL_CAN_ECHO_TRACKS_LEN      (LINUX_VERSION_CODE >= KERNEL_VERSION(5,12,0))
+#define KERNEL_CAN_PRIV_FD              (LINUX_VERSION_CODE >= KERNEL_VERSION(6,16,0))
 
 #if KERNEL_DEFINES_VM_FAULT_T == 0
 typedef int vm_fault_t;
@@ -118,6 +128,7 @@ struct intrepid_netdevice {
 	unsigned char           *from_user;
 	uint8_t                 tx_idx;
 	int			bitrate_changed;
+	struct sk_buff          *tx_skbs[MAX_TX];
 };
 
 static int                      is_open;
@@ -136,15 +147,6 @@ static int                      tx_box_count[2];
 static size_t                   tx_box_bytes[2];
 static spinlock_t               tx_box_lock;
 
-#define RX_BOX_SIZE (SHARED_MEM_SIZE / (MAX_NET_DEVICES * 2))
-#define TX_BOX_SIZE (SHARED_MEM_SIZE / 4)
-#define GET_RX_BOX(DEVICE_INDEX) \
-	(shared_mem + (RX_BOX_SIZE * DEVICE_INDEX))
-#define GET_TX_BOX(BOX_INDEX) \
-	(shared_mem + (SHARED_MEM_SIZE / 2) + (BOX_INDEX * TX_BOX_SIZE))
-#define MAX_TX (0x100)
-#define DESC_OFFSET (2)
-
 static uint16_t intrepid_next_tx_description(
 	struct intrepid_netdevice* ics,
 	int* idx_out)
@@ -215,7 +217,6 @@ static netdev_tx_t intrepid_CAN_netdevice_xmit(struct sk_buff *skb, struct net_d
 	struct canfd_frame *cf = (struct canfd_frame*)skb->data;
 	bool fd = can_is_canfd_skb(skb);
 	bool needs_unlock = false;
-	bool consumed = false;
 	int tx_idx;
 	neomessage_can_t msg = {0};
 
@@ -256,6 +257,7 @@ static netdev_tx_t intrepid_CAN_netdevice_xmit(struct sk_buff *skb, struct net_d
 	if (cf->can_id & CAN_RTR_FLAG) {
 		if (unlikely(fd)) {
 			pr_info("intrepid: tried to send RTR frame on CANFD %s\n", dev->name);
+			kfree_skb(skb);
 			goto exit;
 		}
 		msg.status.remoteFrame = true;
@@ -279,7 +281,6 @@ static netdev_tx_t intrepid_CAN_netdevice_xmit(struct sk_buff *skb, struct net_d
 		, msg.length
 #endif
 	);
-	consumed = true;
 
 	/* Copy the message into the usermode box */
 	memcpy(tx_boxes[current_tx_box] + tx_box_bytes[current_tx_box], &msg, sizeof(neomessage_can_t));
@@ -292,8 +293,6 @@ static netdev_tx_t intrepid_CAN_netdevice_xmit(struct sk_buff *skb, struct net_d
 	if (intrepid_tx_box_no_space_for(sizeof(neomessage_can_t) + CANFD_MTU))
 		intrepid_pause_all_queues();
 exit:
-	if (ret == NETDEV_TX_OK && !consumed)
-		consume_skb(skb);
 	wake_up_interruptible(&tx_wait);
 	if (needs_unlock)
 		spin_unlock_bh(&tx_box_lock);
@@ -304,7 +303,6 @@ static netdev_tx_t intrepid_ETH_netdevice_xmit(struct sk_buff *skb, struct net_d
 	int ret = NETDEV_TX_OK;
 	struct intrepid_netdevice *ics = netdev_priv(dev);
 	bool needs_unlock = false;
-	bool consumed = false;
 	int tx_idx;
 	neomessage_eth_t msg = {0};
 
@@ -334,7 +332,11 @@ static netdev_tx_t intrepid_ETH_netdevice_xmit(struct sk_buff *skb, struct net_d
 		goto exit;
 	}
 	msg.description = intrepid_next_tx_description(ics, &tx_idx);
-	consumed = true;
+	if (ics->tx_skbs[tx_idx]) {
+		kfree_skb(ics->tx_skbs[tx_idx]);
+		++dev->stats.tx_dropped;
+	}
+	ics->tx_skbs[tx_idx] = skb;
 
 	/* Copy the message into the usermode box */
 	memcpy(tx_boxes[current_tx_box] + tx_box_bytes[current_tx_box], &msg, sizeof(neomessage_eth_t));
@@ -347,8 +349,6 @@ static netdev_tx_t intrepid_ETH_netdevice_xmit(struct sk_buff *skb, struct net_d
 	if (intrepid_tx_box_no_space_for(sizeof(neomessage_eth_t) + ETH_DATA_LEN))
 		intrepid_pause_all_queues();
 exit:
-	if (ret == NETDEV_TX_OK && !consumed)
-		consume_skb(skb);
 	wake_up_interruptible(&tx_wait);
 	if (needs_unlock)
 		spin_unlock_bh(&tx_box_lock);
@@ -416,6 +416,8 @@ static int intrepid_remove_can_if(int index)
 
 	unregister_candev(device);
 
+	free_candev(device);
+
 	net_devices[index] = NULL;
 
 	pr_info("intrepid: Removed device %d\n", index);
@@ -439,7 +441,11 @@ static int intrepid_set_bittiming(struct net_device *netdev)
 static int intrepid_set_data_bittiming(struct net_device *netdev)
 {
 	struct intrepid_netdevice *dev = netdev_priv(netdev);
+#if KERNEL_CAN_PRIV_FD
+	struct can_bittiming *bt = &dev->can.fd.data_bittiming;
+#else
 	struct can_bittiming *bt = &dev->can.data_bittiming;
+#endif
 
 	dev_dbg(&netdev->dev, "bitrate %d sample_point %d tq %d sjw %d phase1 %d phase2 %d prop %d brp %d",
 		bt->bitrate, bt->sample_point, bt->tq, bt->sjw, bt->phase_seg1, bt->phase_seg2, bt->prop_seg, bt->brp);
@@ -543,10 +549,16 @@ static int intrepid_add_can_if(struct intrepid_netdevice **result, const char *r
 	if (VER_MIN_FROM_INT(client_version) > 1) {
 		ics->can.bitrate_const = intrepid_bitrates;
 		ics->can.bitrate_const_cnt = ARRAY_SIZE(intrepid_bitrates);
+		ics->can.do_set_bittiming = intrepid_set_bittiming;
+#if KERNEL_CAN_PRIV_FD
+		ics->can.fd.data_bitrate_const = intrepid_data_bitrates;
+		ics->can.fd.data_bitrate_const_cnt = ARRAY_SIZE(intrepid_data_bitrates);
+		ics->can.fd.do_set_data_bittiming = intrepid_set_data_bittiming;
+#else
 		ics->can.data_bitrate_const = intrepid_data_bitrates;
 		ics->can.data_bitrate_const_cnt = ARRAY_SIZE(intrepid_data_bitrates);
-		ics->can.do_set_bittiming = intrepid_set_bittiming;
 		ics->can.do_set_data_bittiming = intrepid_set_data_bittiming;
+#endif
 	}
 	ics->can.state = CAN_STATE_ERROR_ACTIVE;
 	ics->can.ctrlmode_supported = CAN_CTRLMODE_FD;
@@ -698,10 +710,12 @@ static bool handle_CAN_transmit_receipt(
 }
 
 static bool handle_ETH_transmit_receipt(
+	struct net_device *device,
 	const neomessage_eth_t *msg,
 	const uint8_t *data,
 	struct net_device_stats *stats)
 {
+	struct intrepid_netdevice *ics = netdev_priv(device);
 	int tx_idx;
 
 	if (!msg->status.transmitMessage) {
@@ -717,11 +731,14 @@ static bool handle_ETH_transmit_receipt(
 
 	/* unsuccessful transmits */
 	if (msg->status.globalError) {
-		struct sk_buff *skb;
+		kfree_skb(ics->tx_skbs[tx_idx]);
-		kfree_skb(&skb[tx_idx]);
+		ics->tx_skbs[tx_idx] = NULL;
 		return false;
 	}
 
+	dev_kfree_skb(ics->tx_skbs[tx_idx]);
+	ics->tx_skbs[tx_idx] = NULL;
+
 	stats->tx_packets++;
 	stats->tx_bytes += msg->length;
 
@@ -739,8 +756,18 @@ static int intrepid_remove_eth_if(int index)
 
 	pr_info("intrepid: Removing device %d %s 0x%p\n", index, device->name, device);
 
+	struct intrepid_netdevice *ics = netdev_priv(device);
+
+	for (size_t i = 0; i < MAX_TX; ++i) {
+		if (ics->tx_skbs[i]) {
+			kfree_skb(ics->tx_skbs[i]);
+		}
+	}
+
 	unregister_netdev(device);
 
+	free_netdev(device);
+
 	net_devices[index] = NULL;
 
 	pr_info("intrepid: Removed device %d\n", index);
@@ -916,7 +943,7 @@ static struct sk_buff *intrepid_skb_from_neomessage(
 		case ICSNEO_NETWORK_TYPE_ETHERNET:
 			{
 				const neomessage_eth_t *msg = (const neomessage_eth_t*)msg_generic;
-				if (handle_ETH_transmit_receipt(msg, data, stats))
+				if (handle_ETH_transmit_receipt(device, msg, data, stats))
 					goto out;
 				skb = netdev_alloc_skb_ip_align(device, msg->length);
 				if (unlikely(skb == NULL)) {
@@ -1028,7 +1055,11 @@ static long intrepid_dev_ioctl(struct file *fp, unsigned int cmd, unsigned long
 				break;
 			struct intrepid_netdevice *ics = netdev_priv(device);
 			ics->can.bittiming.bitrate = info.baudrates[0];
+#if KERNEL_CAN_PRIV_FD
+			ics->can.fd.data_bittiming.bitrate = info.baudrates[1];
+#else
 			ics->can.data_bittiming.bitrate = info.baudrates[1];
+#endif
 			break;
 		}
 		case SIOCSADDETHIF: {
@@ -1158,7 +1189,11 @@ static int check_bitrate_change(struct intrepid_pending_tx_info *info)
 		if (ics->bitrate_changed) {
 			info->tx_box_index = -(i + 1);
 			info->count = ics->can.bittiming.bitrate;
+#if KERNEL_CAN_PRIV_FD
+			info->bytes = ics->can.fd.data_bittiming.bitrate;
+#else
 			info->bytes = ics->can.data_bittiming.bitrate;
+#endif
 			ics->bitrate_changed = 0;
 			return 1;
 		}