--- /dev/null
+/* can_quekern.c - CAN message queues functions for the Linux kernel
+ * Linux CAN-bus device driver.
+ * New CAN queues by Pavel Pisa - OCERA team member
+ * email:pisa@cmp.felk.cvut.cz
+ * This software is released under the GPL-License.
+ * Version lincan-0.2 9 Jul 2003
+ */
+
+#include "../include/can.h"
+#include "../include/can_sysdep.h"
+#include "../include/can_queue.h"
+
+//#define CAN_DEBUG
+
+extern atomic_t edge_num_cnt;
+
+#ifdef CAN_DEBUG
+ #define DEBUGQUE(fmt,args...) printk(KERN_ERR "can_queue (debug): " fmt,\
+ ##args)
+
+#else
+ #define DEBUGQUE(fmt,args...)
+#endif
+
+#define ERRMSGQUE(fmt,args...) printk(KERN_ERR "can_queue: " fmt,\
+ ##args)
+
+
+/*
+ * Modifies Tx message processing
+ * 0 .. local message processing disabled
+ * 1 .. local messages disabled by default but can be enabled by canque_set_filt
+ * 2 .. local messages enabled by default, can be disabled by canque_set_filt
+ */
+extern int processlocal;
+
+void canque_dead_func(unsigned long data);
+
+/* Support for dead ends structures left after client close */
+spinlock_t canque_dead_func_lock;
+LIST_HEAD(canque_dead_ends);
+/* retrieved by list_entry(canque_dead_ends.next,struct canque_ends_t,dead_peers) */
+LIST_HEAD(canque_dead_edges);
+/* retrieved by list_entry(canque_dead_edges.next,struct canque_edge_t,inpeers) */
+DECLARE_TASKLET(canque_dead_tl, canque_dead_func, 0);
+/* activated by tasklet_schedule(&canque_dead_tl) */
+
+
+static inline
+struct canque_edge_t *canque_dead_edges_cut_first(void)
+{
+ unsigned long flags;
+ struct canque_edge_t *edge;
+ spin_lock_irqsave(&canque_dead_func_lock, flags);
+ if(list_empty(&canque_dead_edges))
+ edge=NULL;
+ else{
+ edge=list_entry(canque_dead_edges.next,struct canque_edge_t,inpeers);
+ list_del(&edge->inpeers);
+ }
+ spin_unlock_irqrestore(&canque_dead_func_lock, flags);
+ return edge;
+}
+
+void canque_dead_func(unsigned long data)
+{
+ unsigned long flags;
+ struct canque_edge_t *qedge;
+ struct canque_ends_t *qends;
+ struct list_head *entry;
+ int i;
+
+ while((qedge=canque_dead_edges_cut_first())){
+ DEBUGQUE("edge %d disposed\n",qedge->edge_num);
+ kfree(qedge);
+ }
+
+ spin_lock_irqsave(&canque_dead_func_lock, flags);
+ entry=canque_dead_ends.next;
+ spin_unlock_irqrestore(&canque_dead_func_lock,flags);
+ while(entry!=&canque_dead_ends){
+ qends=list_entry(canque_dead_ends.next,struct canque_ends_t,dead_peers);
+ entry=entry->next;
+ if(!list_empty(&qends->inlist))
+ continue;
+ if(!list_empty(&qends->idle))
+ continue;
+ for(i=CANQUEUE_PRIO_NR;i--;)
+ if(!list_empty(&qends->active[i]))
+ continue;
+ spin_lock_irqsave(&canque_dead_func_lock, flags);
+ list_del(&qends->dead_peers);
+ spin_unlock_irqrestore(&canque_dead_func_lock,flags);
+ DEBUGQUE("ends structure disposed\n");
+ kfree(qends);
+ }
+
+}
+
+
+void canque_edge_do_dead(struct canque_edge_t *edge, int dead_fl)
+{
+ unsigned long flags;
+
+ if(dead_fl) return;
+
+ if(canqueue_disconnect_edge(edge)<0){
+ ERRMSGQUE("canque_edge_do_dead: canqueue_disconnect_edge failed !!!\n");
+ return;
+ }
+
+ spin_lock_irqsave(&canque_dead_func_lock, flags);
+ list_add(&edge->inpeers,&canque_dead_edges);
+ spin_unlock_irqrestore(&canque_dead_func_lock, flags);
+ tasklet_schedule(&canque_dead_tl);
+}
+
+
+
+/*if(qends->ends_flags & CAN_ENDSF_DEAD){
+ spin_lock_irqsave(&canque_dead_func_lock, flags);
+ list_del(&qends->dead_peers);
+ list_add(&qends->dead_peers,&canque_dead_ends);
+ spin_unlock_irqrestore(&canque_dead_func_lock, flags);
+ tasklet_schedule(&canque_dead_tl);
+}*/
+
+
+/**
+ * canqueue_notify_kern - notification callback handler for Linux userspace clients
+ * @qends: pointer to the callback side ends structure
+ * @qedge: edge which invoked notification
+ * @what: notification type
+ */
+void canqueue_notify_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge, int what)
+{
+ DEBUGQUE("canqueue_notify_kern for edge %d, use %d and event %d\n",
+ qedge->edge_num,(int)atomic_read(&qedge->edge_used),what);
+ switch(what){
+ case CANQUEUE_NOTIFY_EMPTY:
+ wake_up(&qends->endinfo.fileinfo.emptyq);
+ if(canque_fifo_test_and_clear_fl(&qedge->fifo, FREEONEMPTY))
+ canque_edge_decref(qedge);
+ break;
+ case CANQUEUE_NOTIFY_SPACE:
+ wake_up(&qends->endinfo.fileinfo.writeq);
+ break;
+ case CANQUEUE_NOTIFY_PROC:
+ wake_up(&qends->endinfo.fileinfo.readq);
+ break;
+ case CANQUEUE_NOTIFY_NOUSR:
+ wake_up(&qends->endinfo.fileinfo.readq);
+ wake_up(&qends->endinfo.fileinfo.writeq);
+ wake_up(&qends->endinfo.fileinfo.emptyq);
+ break;
+ case CANQUEUE_NOTIFY_DEAD_WANTED:
+ case CANQUEUE_NOTIFY_DEAD:
+ if(canque_fifo_test_and_clear_fl(&qedge->fifo, READY))
+ canque_edge_decref(qedge);
+ break;
+ case CANQUEUE_NOTIFY_ATTACH:
+ break;
+ }
+}
+
+/**
+ * canqueue_ends_init_kern - Linux userspace clients specific ends initialization
+ * @qends: pointer to the callback side ends structure
+ */
+int canqueue_ends_init_kern(struct canque_ends_t *qends)
+{
+ canqueue_ends_init_gen(qends);
+ qends->context=NULL;
+ init_waitqueue_head(&qends->endinfo.fileinfo.readq);
+ init_waitqueue_head(&qends->endinfo.fileinfo.writeq);
+ init_waitqueue_head(&qends->endinfo.fileinfo.emptyq);
+ qends->notify=canqueue_notify_kern;
+ DEBUGQUE("canqueue_ends_init_kern\n");
+ return 0;
+}
+
+
+/**
+ * canque_get_inslot4id_wait_kern - find or wait for best outgoing edge and slot for given ID
+ * @qends: ends structure belonging to calling communication object
+ * @qedgep: place to store pointer to found edge
+ * @slotp: place to store pointer to allocated slot
+ * @cmd: command type for slot
+ * @id: communication ID of message to send into edge
+ * @prio: optional priority of message
+ *
+ * Same as canque_get_inslot4id(), except, that it waits for free slot
+ * in case, that queue is full. Function is specific for Linux userspace clients.
+ * Return Value: If there is no usable edge negative value is returned.
+ */
+int canque_get_inslot4id_wait_kern(struct canque_ends_t *qends,
+ struct canque_edge_t **qedgep, struct canque_slot_t **slotp,
+ int cmd, unsigned long id, int prio)
+{
+ int ret=-1;
+ DEBUGQUE("canque_get_inslot4id_wait_kern for cmd %d, id %ld, prio %d\n",cmd,id,prio);
+ wait_event_interruptible((qends->endinfo.fileinfo.writeq),
+ (ret=canque_get_inslot4id(qends,qedgep,slotp,cmd,id,prio))!=-1);
+ return ret;
+}
+
+/**
+ * canque_get_outslot_wait_kern - receive or wait for ready slot for given ends
+ * @qends: ends structure belonging to calling communication object
+ * @qedgep: place to store pointer to found edge
+ * @slotp: place to store pointer to received slot
+ *
+ * The same as canque_test_outslot(), except it waits in the case, that there is
+ * no ready slot for given ends. Function is specific for Linux userspace clients.
+ * Return Value: Negative value informs, that there is no ready output
+ * slot for given ends. Positive value is equal to the command
+ * slot has been allocated by the input side.
+ */
+int canque_get_outslot_wait_kern(struct canque_ends_t *qends,
+ struct canque_edge_t **qedgep, struct canque_slot_t **slotp)
+{
+ int ret=-1;
+ DEBUGQUE("canque_get_outslot_wait_kern\n");
+ wait_event_interruptible((qends->endinfo.fileinfo.readq),
+ (ret=canque_test_outslot(qends,qedgep,slotp))!=-1);
+ return ret;
+}
+
+/**
+ * canque_sync_wait_kern - wait for all slots processing
+ * @qends: ends structure belonging to calling communication object
+ * @qedge: pointer to edge
+ *
+ * Functions waits for ends transition into empty state.
+ * Return Value: Positive value indicates, that edge empty state has been reached.
+ * Negative or zero value informs about interrupted wait or other problem.
+ */
+int canque_sync_wait_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge)
+{
+ int ret=-1;
+ DEBUGQUE("canque_sync_wait_kern\n");
+ wait_event_interruptible((qends->endinfo.fileinfo.emptyq),
+ (ret=canque_fifo_test_fl(&qedge->fifo,EMPTY)?1:0));
+ return ret;
+}
+
+
+/**
+ * canque_new_edge_kern - allocate new edge structure in the Linux kernel context
+ * @slotsnr: required number of slots in the newly allocated edge structure
+ *
+ * Return Value: Returns pointer to allocated slot structure or %NULL if
+ * there is not enough memory to process operation.
+ */
+struct canque_edge_t *canque_new_edge_kern(int slotsnr)
+{
+ struct canque_edge_t *qedge;
+ qedge = (struct canque_edge_t *)kmalloc(sizeof(struct canque_edge_t), GFP_KERNEL);
+ if(qedge == NULL) return NULL;
+
+ memset(qedge,0,sizeof(struct canque_edge_t));
+ spin_lock_init(&qedge->fifo.fifo_lock);
+ if(canque_fifo_init_slots(&qedge->fifo, slotsnr)<0){
+ kfree(qedge);
+ DEBUGQUE("canque_new_edge_kern failed\n");
+ return NULL;
+ }
+ atomic_set(&qedge->edge_used,1);
+ qedge->filtid = 0;
+ qedge->filtmask = canque_filtid2internal(0l, (processlocal<2)? MSG_LOCAL:0);
+ qedge->edge_prio = 0;
+ #ifdef CAN_DEBUG
+ /* not exactly clean, but enough for debugging */
+ atomic_inc(&edge_num_cnt);
+ qedge->edge_num=atomic_read(&edge_num_cnt);
+ #endif /* CAN_DEBUG */
+ DEBUGQUE("canque_new_edge_kern %d\n",qedge->edge_num);
+ return qedge;
+}
+
+/**
+ * canqueue_disconnect_edge_kern - disconnect edge from communicating entities with wait
+ * @qends: ends structure belonging to calling communication object
+ * @qedge: pointer to edge
+ *
+ * Same as canqueue_disconnect_edge(), but tries to wait for state with zero
+ * use counter.
+ * Return Value: Negative value means, that edge is used and cannot
+ * be disconnected yet. Operation has to be delayed.
+ */
+int canqueue_disconnect_edge_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge)
+{
+ canque_fifo_set_fl(&qedge->fifo,BLOCK);
+ DEBUGQUE("canqueue_disconnect_edge_kern %d called\n",qedge->edge_num);
+ if(!canque_fifo_test_and_set_fl(&qedge->fifo,DEAD)){
+ canque_notify_bothends(qedge, CANQUEUE_NOTIFY_DEAD);
+
+ if(atomic_read(&qedge->edge_used)>0)
+ atomic_dec(&qedge->edge_used);
+
+ DEBUGQUE("canqueue_disconnect_edge_kern %d waiting\n",qedge->edge_num);
+ wait_event((qends->endinfo.fileinfo.emptyq),
+ (canqueue_disconnect_edge(qedge)>=0));
+
+ /*set_current_state(TASK_UNINTERRUPTIBLE);*/
+ /*schedule_timeout(HZ);*/
+ return 0;
+ } else {
+ DEBUGQUE("canqueue_disconnect_edge_kern cannot set DEAD\n");
+ return -1;
+ }
+}
+
+
+int canqueue_disconnect_list_kern(struct canque_ends_t *qends, struct list_head *list)
+{
+ struct canque_edge_t *edge;
+ unsigned long flags;
+ for(;;){
+ spin_lock_irqsave(&qends->ends_lock,flags);
+ if(list_empty(list)){
+ spin_unlock_irqrestore(&qends->ends_lock,flags);
+ return 0;
+ }
+ if(list == &qends->inlist)
+ edge=list_entry(list->next,struct canque_edge_t,inpeers);
+ else
+ edge=list_entry(list->next,struct canque_edge_t,outpeers);
+ atomic_inc(&edge->edge_used);
+ spin_unlock_irqrestore(&qends->ends_lock,flags);
+ if(canqueue_disconnect_edge_kern(qends, edge)>=0) {
+ /* Free edge memory */
+ canque_fifo_done(&edge->fifo);
+ kfree(edge);
+ }else{
+ canque_notify_bothends(edge, CANQUEUE_NOTIFY_DEAD_WANTED);
+ canque_edge_decref(edge);
+ DEBUGQUE("canqueue_disconnect_list_kern in troubles\n");
+ DEBUGQUE("the edge %d has usage count %d and flags %ld\n",edge->edge_num,atomic_read(&edge->edge_used),edge->fifo.fifo_flags);
+ return -1;
+ }
+ }
+}
+
+void canqueue_block_list(struct canque_ends_t *qends, struct list_head *list)
+{
+ struct canque_edge_t *edge;
+ struct list_head *entry;
+
+ /* has to be called with qends->ends_lock already locked */
+ list_for_each(entry,&qends->inlist){
+ if(list == &qends->inlist)
+ edge=list_entry(list->next,struct canque_edge_t,inpeers);
+ else
+ edge=list_entry(list->next,struct canque_edge_t,outpeers);
+ canque_fifo_set_fl(&edge->fifo,BLOCK);
+ }
+}
+
+int canqueue_ends_sync_all_kern(struct canque_ends_t *qends)
+{
+ struct canque_edge_t *qedge;
+
+ canque_for_each_inedge(qends, qedge){
+ DEBUGQUE("canque_sync_wait_kern called for edge %d\n",qedge->edge_num);
+ canque_sync_wait_kern(qends, qedge);
+ }
+ return 0;
+}
+
+int canqueue_ends_done_inends(struct canque_ends_t *qends, int send_rest)
+{
+ struct canque_edge_t *edge;
+
+ canque_for_each_inedge(qends, edge){
+ canque_notify_bothends(edge, CANQUEUE_NOTIFY_DEAD_WANTED);
+ if(send_rest){
+ canque_edge_incref(edge);
+ if(!canque_fifo_test_and_set_fl(&edge->fifo, FREEONEMPTY)){
+ if(!canque_fifo_test_fl(&edge->fifo, EMPTY))
+ continue;
+ if(!canque_fifo_test_and_clear_fl(&edge->fifo, FREEONEMPTY))
+ continue;
+ }
+ canque_edge_decref(edge);
+ }
+ }
+ return list_empty(&qends->inlist)?0:1;
+}
+
+
+/**
+ * canqueue_ends_dispose_kern - finalizing of the ends structure for Linux kernel clients
+ * @qends: pointer to ends structure
+ * @sync: flag indicating, that user wants to wait for processing of all remaining
+ * messages
+ *
+ * Return Value: Function should be designed such way to not fail.
+ */
+int canqueue_ends_dispose_kern(struct canque_ends_t *qends, int sync)
+{
+ unsigned long flags;
+ int i;
+ int delayed;
+
+ DEBUGQUE("canqueue_ends_dispose_kern\n");
+ spin_lock_irqsave(&qends->ends_lock,flags);
+ canqueue_block_list(qends, &qends->idle);
+ for(i=CANQUEUE_PRIO_NR;--i>=0;){
+ canqueue_block_list(qends, &qends->active[i]);
+ }
+ canqueue_block_list(qends, &qends->idle);
+ canqueue_block_list(qends, &qends->inlist);
+ spin_unlock_irqrestore(&qends->ends_lock,flags);
+
+ /*Wait for sending of all pending messages in the output FIFOs*/
+ if(sync)
+ canqueue_ends_sync_all_kern(qends);
+
+ /* Finish all outgoing edges listed in inends */
+ delayed=canqueue_ends_done_inends(qends, 1);
+
+ delayed|=canqueue_disconnect_list_kern(qends, &qends->idle);
+ for(i=CANQUEUE_PRIO_NR;--i>=0;){
+ delayed|=canqueue_disconnect_list_kern(qends, &qends->active[i]);
+ }
+
+ wake_up(&qends->endinfo.fileinfo.readq);
+ wake_up(&qends->endinfo.fileinfo.writeq);
+ wake_up(&qends->endinfo.fileinfo.emptyq);
+
+ if(delayed){
+ spin_lock_irqsave(&canque_dead_func_lock, flags);
+ qends->ends_flags |= CAN_ENDSF_DEAD;
+ list_add(&qends->dead_peers,&canque_dead_ends);
+ spin_unlock_irqrestore(&canque_dead_func_lock, flags);
+ tasklet_schedule(&canque_dead_tl);
+
+ DEBUGQUE("canqueue_ends_dispose_kern delayed\n");
+ return 1;
+ }
+
+ kfree(qends);
+ DEBUGQUE("canqueue_ends_dispose_kern finished\n");
+ return 0;
+}
+
+void canqueue_kern_initialize()
+{
+
+
+}
projects / lincan.git / blobdiff