From fe63b9899ecf07c6b4815d20c0a874cfbf38abc4 Mon Sep 17 00:00:00 2001 From: Richard Hirst Date: Fri, 23 Nov 2012 22:31:42 +0000 Subject: [PATCH] Rework to process commands as they are issued, rather than on close() --- ServoBlaster/Makefile | 8 +- ServoBlaster/servoblaster.c | 211 +++++++++++++++++------------------ ServoBlaster/servoblaster.ko | Bin 10451 -> 10747 bytes 3 files changed, 108 insertions(+), 111 deletions(-) diff --git a/ServoBlaster/Makefile b/ServoBlaster/Makefile index 5f9a943..18fd28e 100644 --- a/ServoBlaster/Makefile +++ b/ServoBlaster/Makefile @@ -1,5 +1,7 @@ -KERNEL_TREE := /home/richard/github/linux +KERNEL_TREE := /home/richard/raspberrypi/linux INSTALL_PATH := /lib/modules/$(shell /bin/uname -r)/kernel/drivers/misc/servoblaster +#CROSS_OPTS := CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- ARCH=arm +CROSS_OPTS := .PHONY: all install install_autostart all: servoblaster.ko @@ -7,7 +9,7 @@ all: servoblaster.ko servoblaster.ko: servoblaster.c servoblaster.h @[ -d ${KERNEL_TREE} ] || { echo "Edit Makefile to set KERNEL_TREE to point at your kernel"; exit 1; } @[ -e ${KERNEL_TREE}/Module.symvers ] || { echo "KERNEL_TREE/Module.symvers does not exist, you need to configure and compile your kernel"; exit 1; } - make -C ${KERNEL_TREE} ARCH=arm M=$(PWD) modules + make -C ${KERNEL_TREE} ${CROSS_OPTS} M=$(PWD) modules servodemo: servodemo.c servoblaster.h gcc -Wall -g -O2 -o servodemo servodemo.c -Wl,--export-dynamic `pkg-config --cflags gtk+-3.0 gmodule-export-2.0` `pkg-config --libs gtk+-3.0 gmodule-export-2.0` @@ -30,6 +32,6 @@ install_autostart: install @echo " modprobe -r servoblaster" clean: - make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- M=$(PWD) clean + make -C ${KERNEL_TREE} ${CROSS_OPTS} M=$(PWD) clean rm -f servodemo diff --git a/ServoBlaster/servoblaster.c b/ServoBlaster/servoblaster.c index bfc9223..9fcd86a 100644 --- a/ServoBlaster/servoblaster.c +++ b/ServoBlaster/servoblaster.c @@ -122,11 +122,30 @@ static uint8_t servo2gpio[] = { }; #define NUM_SERVOS (sizeof(servo2gpio)/sizeof(servo2gpio[0])) +// This struct is used to store all temporary data associated with a given +// open() of /dev/servoblaster +struct private_data +{ + // Stores the return string for a read of /dev/servoblaster + // Allowing 10 chars per line. + int rd_len; + char rd_data[NUM_SERVOS * 10]; + + // Stores partial command strings between calls to write(), in case + // someone uses multiple write() calls to issue a single command. + int partial_len; + char partial_cmd[10]; + + // If we get bad data on a write() we reject all subsequent writes + // until the process closes and reopens the device. + int reject_writes; +}; + // Structure of our control data, stored in a 4K page, and accessed by dma controller struct ctldata_s { struct bcm2708_dma_cb cb[NUM_SERVOS * 4]; // gpio-hi, delay, gpio-lo, delay, for each servo output - uint32_t gpiodata[NUM_SERVOS]; // set-pin, clear-pin values, per servo output - uint32_t pwmdata; // the word we write to the pwm fifo + uint32_t gpiodata[NUM_SERVOS]; // set-pin, clear-pin values, per servo output + uint32_t pwmdata; // the word we write to the pwm fifo }; static struct ctldata_s *ctl; @@ -142,6 +161,11 @@ static int my_major; static int cycle_ticks = 2000; static int tick_scale = 6; +// This records the written count values so we can display them on a read() +// call. Cannot derive data directly from DMA control blocks as current +// algorithm has a special case for a count of zero. +static int servo_pos[NUM_SERVOS] = { 0 }; + // Wait until we're not processing the given servo (actually wait until // we are not processing the low period of the previous servo, or the // high period of this one). @@ -315,105 +339,54 @@ void cleanup_module(void) unregister_chrdev_region(devno, 1); } -// This struct is used to store all temporary data required per process -// which reads/writes the device file. -struct process_data -{ - // Stores the /dev/servoblaster content for a given user process. - // Allowing 10 chars per line. - int ret_idx; - char ret_data[NUM_SERVOS * 10]; - - // Stores up to NUM_SERVOS user commands (of up to 10 chars) per user process. - // e.g. "1=60\n2=45\n3=180\n" - int cmd_idx; - char cmd_str[NUM_SERVOS * 10]; -}; - // kmalloc the temporary data required for each user: -static int dev_open(struct inode *inod,struct file *fil) +static int dev_open(struct inode *inod, struct file *fil) { - fil->private_data = kmalloc( sizeof(struct process_data), GFP_KERNEL ); + fil->private_data = kmalloc(sizeof(struct private_data), GFP_KERNEL); if (0 == fil->private_data) { printk(KERN_WARNING "ServoBlaster: Failed to allocate user data\n"); return -ENOMEM; } - memset(fil->private_data, 0, sizeof(struct process_data)); + memset(fil->private_data, 0, sizeof(struct private_data)); + return 0; } -// This records the written count values. Cannot derive data directly from DMA -// control blocks as current algorithm has a special case for a count of zero. -static int written_data[NUM_SERVOS] = { 0 }; - -static ssize_t dev_read(struct file *filp,char *buf,size_t count,loff_t *f_pos) +static ssize_t dev_read(struct file *filp, char *buf, size_t count, loff_t *f_pos) { - ssize_t bytesPrinted = 0; - struct process_data* const pdata = filp->private_data; + ssize_t ret = 0; + struct private_data* const pdata = filp->private_data; // Only proceed if we have private data, else return EOF. - if (0 != pdata) - { - int servo; - int* const idx = &(pdata->ret_idx); - char* const returnedData = pdata->ret_data; + if (pdata) { + if (0 == *f_pos) { + int servo; + char *p = pdata->rd_data, *end = p + sizeof(pdata->rd_data); - if (0 == *f_pos) - { // Get fresh data - for (servo=0, *idx=0; servo < NUM_SERVOS; ++servo) - { - *idx += snprintf(returnedData+*idx, sizeof(pdata->ret_data)-*idx, - "%i=%i\n", - servo, - written_data[servo] - ); + for (servo = 0; servo < NUM_SERVOS; ++servo) { + p += snprintf(p, end - p, "%i=%i\n", servo, + servo_pos[servo]); } + pdata->rd_len = end - p; } - if (*f_pos >= *idx) - { - //EOF - bytesPrinted=0; - } - else if ( (*f_pos + count) < *idx ) - { - // Sufficient data to fulfil request - if (copy_to_user(buf,returnedData+*f_pos,count)) { + if (*f_pos < pdata->rd_len) { + if (count > pdata->rd_len - *f_pos) + count = pdata->rd_len - *f_pos; + if (copy_to_user(buf, pdata->rd_data + *f_pos, count)) return -EFAULT; - } - *f_pos+=count; - bytesPrinted=count; - } - else - { - // Return all the data we have - const int nBytes = *idx-*f_pos; - if (copy_to_user(buf,returnedData+*f_pos, nBytes)) { - return -EFAULT; - } - *f_pos+=nBytes; - bytesPrinted=nBytes; + *f_pos += count; + ret = count; } } - return bytesPrinted; + + return ret; } -// This function takes a single null terminated command string of this -// sort of format; -// 2=130 -// where in this case, servo 2 is given a high duration of 130. -ssize_t process_command_string(const char str[]) +static int set_servo(int servo, int cnt) { - int servo; - int cnt; - int n; - n = sscanf(str, "%d=%d", &servo, &cnt); - if (n != 2) { - printk(KERN_WARNING "ServoBlaster: Failed to parse command (n=%d)\n", n); - return -EINVAL; - } if (servo < 0 || servo >= NUM_SERVOS) { printk(KERN_WARNING "ServoBlaster: Bad servo number %d\n", servo); return -EINVAL; @@ -435,57 +408,79 @@ ssize_t process_command_string(const char str[]) ctl->cb[servo*4+1].length = cnt * sizeof(uint32_t); ctl->cb[servo*4+3].length = (cycle_ticks / 8 - cnt) * sizeof(uint32_t); } - written_data[servo] = cnt; // Record data for use by dev_read + servo_pos[servo] = cnt; // Record data for use by dev_read local_irq_enable(); + return 0; } -// Read user data into buffer until full. -static ssize_t dev_write(struct file *filp,const char *buf,size_t count,loff_t *f_pos) +static ssize_t dev_write(struct file *filp,const char *user_buf,size_t count,loff_t *f_pos) { - struct process_data* const pdata = filp->private_data; - if (0 == pdata) return 0; + struct private_data* const pdata = filp->private_data; + char buf[128], *p = buf, nl; + int len = pdata->partial_len; - { - static const int max_idx = sizeof(pdata->cmd_str) - 1; - int* const idx = &(pdata->cmd_idx); + if (0 == pdata) + return -EFAULT; + if (pdata->reject_writes) + return -EINVAL; + memcpy(buf, pdata->partial_cmd, len); + pdata->partial_len = 0; + if (count > sizeof(buf) - len - 1) + count = sizeof(buf) - len - 1; + if (copy_from_user(buf+len, user_buf, count)) + return -EFAULT; + len += count; + buf[len] = '\0'; + while (p < buf+len) { + int servo, cnt, res; - if ((*idx+count) > max_idx) count = max_idx-*idx; - if (copy_from_user(pdata->cmd_str + *idx, buf, count)) { - return -EFAULT; + if (strchr(p, '\n')) { + if (sscanf(p, "%d=%d%c", &servo, &cnt, &nl) != 3 || + nl != '\n') { + printk(KERN_WARNING "ServoBlaster: Bad data format\n"); + pdata->reject_writes = 1; + return -EINVAL; + } + res = set_servo(servo, cnt); + if (res < 0) { + pdata->reject_writes = 1; + return res; + } + p = strchr(p, '\n') + 1; + } + else if (buf+len - p > 10) { + printk(KERN_WARNING "ServoBlaster: Bad data format\n"); + pdata->reject_writes = 1; + return -EINVAL; + } + else { + // assume more data is coming... + break; } - *idx+=count; } + pdata->partial_len = buf+len - p; + memcpy(pdata->partial_cmd, p, pdata->partial_len); return count; } -// dev_close separates the user input (delimited by \n) into strings for passing -// to process_command_string() then frees up all process data static int dev_close(struct inode *inod,struct file *fil) { - struct process_data* const pdata = fil->private_data; - if (0 != pdata) - { - static const int max_idx = sizeof(pdata->cmd_str) - 1; - char* cmd_str = pdata->cmd_str; - char* command; - cmd_str[max_idx] = 0; // Ensure command string is null terminated. + struct private_data* const pdata = fil->private_data; + int ret = 0; - // Execute all commands. - command = strsep(&cmd_str, "\n"); - while( NULL != command ) { - if (*command != 0) { - printk(KERN_DEBUG "ServoBlaster: Command %s\n", command); - (void)process_command_string(command); - } - command = strsep(&cmd_str, "\n"); + if (pdata) { + if (pdata->partial_len) { + printk(KERN_WARNING "ServoBlaster: partial command " + "pending on close()\n"); + ret = -EIO; } - // Free process data. kfree(pdata); } - return 0; + + return ret; } static long dev_ioctl(struct file *file, unsigned int cmd, unsigned long arg) diff --git a/ServoBlaster/servoblaster.ko b/ServoBlaster/servoblaster.ko index b3d47d8380642ba35a2a63f5516f539e1d416394..3def24aedbdd75ec385a0071a90b3025cc45b31e 100644 GIT binary patch delta 3734 zcmZWr4Qx}_6+X}ZF(!2&6ek$+@=Ml+?a>5ADp=hF80vm%AZt z8xq{6m5FlrDrWo$uJvJttPg2%z1$XJPrZKUd7 z-@SX=4&0D@XWB(6gSjoJW&U&b?&pLy^Z1&-OF>m1Taq$5)Dh#{)KAi+@Saa+EJ;;A zqYN8OZKsTvv3DeT3i78i%Sc=LJoayvhZ1RjU^s2@s=7xWHrSv;&Kg||Sz^^W?Qq)?aof%hh*l^H*)&WB<`78|W$aKSqOF3(( z-r=;PtzUB}2Q^3AfJU;KvC)(IfShPFo+dh_ZBH56^XLatMn2K!Hsz%8Q9;T$QroY+ zTbeRl-hN}zT2;3yhcwqnzmX)@7j7?al66)JB-0LR(tk+w0LyreBSjRf>C zd=Ptj<>7RpGMuK#VdLurl&nzt5j(LI?ot{~SAIik4{(oz^{ zl7|croyCUP)4MP}<4qZ7IPA#1m3HW5Fn(@oI6ZUhooOp+8A(wy5oO4jHTU`l(C$?e zx=YS(!l612PZvY^HYT|yIE2hU18+j{<(V=RQ08;UHk$=Gzd6fqL9Rf)Xo2o0kY&@s zC7W%2k`Gi zL^KKv8dG##w;>wW9#M4*1$FD>pEV-&RUgqCB=689<*Js~rs@<`GY*83eQQ9;pR+aGE2GXfDMyFP@UG>yYca^5kD?q`aD*fxu(ti@bxH z&3iD-#gqpF9J+rZgjh)D(Kjln;sEY_K-K*ujHw0H-a!M%Ke;=^8f*EM3uZhC8dHra z!;g4Y(?D9n1-w?9eNEV{LK-$cL3vjz$BprwDMNvdt?h8S{9!x@V$@n8wTX+?GKnVs zYW++CT(QTb{75LHGtRYoeq^sV&`p@CYL#n>^auj8jFX*dt1XDdt3Ls5bBGF(fkJs-~0DhGYe;ShdR44 zQ6si3O$yh20r%lcXX;|{zHmn$#OvMRle^FEYu^`+zQ|N;TM`h1`2q3n(n&E>XcMu* zj)~dA7O60G$bB&HH!;swhn>A8_L_%|($*>Q(*0!5uxYvIDXwDp6<}<+_$^e%pz@%&QoK}r36-nMMgEE^`#=fNl&D`(Zf~k0dQF5@ z_!9V^P?0qY{CgMZe~RcM#x^1@M2iC#16AjLl4zO{e?(--Vg+L>^cXQ;jgs?s;yjEY z{QWXwpNNS`6OS@JLo@?e`%SE2grONHFpTcX;zkpLCPqx$Z{pjo1QcQ58VjDo2^iU+iJvld!>~C|h!UUF z#t5S>MsyG3<3wIlUc-pf*O}OC%3DlqGx6LL82G#303$q~Vr)hYFg6isCQh3;W8!}q z;g}1xz7mdmfb5uW4#lOFB~rV%x$@y2LSs+xBg-BrGGejUloy(ET&wI!7;%;6=2xK3 znJ+#Mq@#AcS0L@R-xqGy{lx9{{k#e6-+ z%x|EG=k>WTvn{L7Z|OX5#DKZX_RRgc{^sY|Z@js!-w1x36%gb60xraC%NpRvd7iVw zoaSuH&hw)^&w1X1w@7}!fXzyX=!5Wu#~tD?r6&{n!8lsk*#bHMUbBF|49-#9h%Qk9 z9Ru%PFn=06x`3Ypk1yco!S^rV7r^J6jlK}K%L+>W09hS+IVL1>(oKw;(fJ(AZ;O@X zCGIA+Iyeo&t!8x*T!`2d#64alg zFqFeba`qEaIlL~1 zKbpgv@8vsa&%FYAA&37WhY#m)e#_?@C-K)}O=W|9?gK$|Rjw4Hm5(Lr>g%bsbzg6! zHQtGT_gFa8-5!52+Fn<`!IXzz40pHB&Bp5L*MD1>)m~>#ba#f^dtz8#YYIXy#5;QX z>gpddd89oS?(2-kJ9~TT>eqT{!S=MfH!8ZS0(mk1*lCUS#>B;{ko0}wTm5vw!mo_% m_s8JsM;zw2#P3(H&Hw-B$=pZC?CNJlb9K;Retf)EUGqP4%E)d2 delta 3566 zcmZWr4{#J`6@PoVOD-`H{T$3e#j!?*SYc3F&081D!PHDL`0RfR*$Q>j#BxmkK zI-++(LOU&SpkJ}H1+x`rn!yfDr87l_I;W*hMVLujMj3^Ae^^ZsHFO-Uip1aB+e0|N zVe{MHdvD)+-+S--xPNzi)P6{|yfx==_1UmR)#AOLLpsrhq>VJGIr5IZQ$C^>kQTQn z`(x{=BN<*EO|DelO|DnB4{xI3gk2H3HF`Lf6}_sEqBRn1h(=;IqCBF`B>q%b<>6ST zJfd6S2R^5)vKGHHGxH}RPZIuZf1jDjE1+07X($~jB?Jba{zAhi(xFshj>*RooSa3| z;#m1$Kgw4D_Y!2 zk%TgDOsD9m&dEsL@PUM#HYQ2kA0u@{|8!=iV$%D5oU=W@5*470@vP`?Vno`B3Zjz~ z)sp%6*shK0SLjkan-0Zt9IwVKv0~U1U^C(6KQk)EmCBP}ieXU@@F_jnTv8v zr$weuE~u(Y{y6}m*tLq3Et!eF2k)1MIIC^|25>?lB+-~rB;hFraM%$ zN7drBB=kv#mQ44xm-g#fNMc+x*lJeU@xk=^hEE6<7gvwzPBgNf_9rA+OGX>Vj0W&D zl+dXDG11w21uFxiI{Y#M<1o z)qC*ThG!a20O^rOOCD9n^rPM}9TO9;xg$>H{vbRJRX*~Ni^lWzKY(ZnX@K*Flj>3l3-Q+}aah;C53a@ihBu7_LjI&ms1av4D;CdMN&hH&DI?1&d9Vq)TC zqzt1t@gqy57jY*}N~d4R^Gb8vt&Pf>)y>Vy61RKxs#U9-)-)|{ZfX>>@>_pgP$n&{ z^DVFQVdE|C6&?Pe-GibjO>WMRj zS)#c_6=#Z~;+v<^2o7h}lYkiQ&EB2Mw*a8sA;*s)V zajx7U9TJz=ET1SU{#jmQnJN&K6)uajQmn0TW!nphBH3c7qT2RM37T(w_IicGvaVA6 zw8CZk48k_?0esztaBrn3oa?fwnEYD9a@|~q#aAk}&2=#x0F0N4Q!p8a$wKkw++uMP zCLfiGI}jeMM7zWSXN|3;j_5_v?ko#$M$=JN3-T*L*PT0w6vi$d2A~Hw0;a&}Cpu^1 zMIhTxGG-G^vHZ(Kw;AzWKBmRFji?N0>A^cA>{?jLk{PR%_goh(Q9JJ#J3pXKg#GOx|mJjR6tV^V|)^U zO@rG&-YH@3c?VWT%)Fg(J$7o!>lu-hVq&K$-(=!e6E&c-7YQ&T;fpz8ynKo1G7CD1 zu9|q=#G5AmjS-1iaqu2Q=60abG7}d8onJvEkR0!Dk%=uPwgGtq>lpEP7_qUfYfOdL zM4yS{Kxa2v!nhH=X5`i|wiDt1Jj3C21I5|OD#<5)Te&Q(qFN7OaCmV^@Q*AzP%1_k zJ4`v|+K@XPz7CkS=P6FDls(^tY0mz5M$()&=glFErm<21 zxmRh<27Gy@IXmFqq&e&PQcd$mO&$TJ6N=@E8}mLa>V-%`S!=O3ez%HC)h~v>30BX{ z&=%1?a4ub%KMSsax0w3p!Tayge;+(>4}T3jbPs<6oPQdm{k;vIURJs#k~Ky1E zn-%`_K2hpIHEM)EJQ=g^j$VrM#hPBMUzow0Gx+)pzU_YgdFsDkK;O#Xd}F5<2mG#B z<7yR$U1hmG|L(4y{=uMMjJqBUHz{uF>e@Bv>k9Ste6=gs)7|e6Jr(dbDUGJQ=c%54 zzo88_DUbZWFlDWnivGSH|3J|1UfOI5y1yRk9UO8mU1D;dKiD(W7YOwY4!D;tcF>N& zK#=|yLk!PX^S?3F7Yg|Yx_sTCZt<)6-I7~0)Nd&b3=Q`9gTXHIS4S6qGV~4X6i4e< aijV6RQMF)8Hvfw72gIHQ?P7Al!v6x#c!*>G